diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000000..d0c41ad78d --- /dev/null +++ b/.clang-format @@ -0,0 +1,151 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 120 # changed +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 2 # changed +Cpp11BracedListStyle: true +DerivePointerAlignment: false # changed +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^' + Priority: 2 + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - 'c++' + - 'C++' + CanonicalDelimiter: '' + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + CanonicalDelimiter: '' + BasedOnStyle: google +ReflowComments: true +SortIncludes: false # changed +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseTab: Never +... + diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000000..f613777ff1 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,2 @@ +FROM ghcr.io/nasa/astrobee:latest-ubuntu20.04 +ENV DEBIAN_FRONTEND=dialog diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000000..3f67b120c4 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,44 @@ +{ + "dockerFile": "Dockerfile", + "build": { + "args": { + "WORKSPACE": "${containerWorkspaceFolder}" + } + }, + "remoteUser": "root", + "runArgs": [ + "--network=host", + "--cap-add=SYS_PTRACE", + "--security-opt=seccomp:unconfined", + "--security-opt=apparmor:unconfined", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix" + ], + "containerEnv": { + "DISPLAY": "${localEnv:DISPLAY}", + "LIBGL_ALWAYS_SOFTWARE": "1" // Needed for software rendering of opengl + }, + // Set *default* container specific settings.json values on container create. + "settings": { + "terminal.integrated.profiles.linux": { + "zsh": { + "path": "/bin/zsh" + } + }, + "terminal.integrated.defaultProfile.linux": "zsh" + }, + "extensions": [ + // "althack.ament-task-provider", + "DotJoshJohnson.xml", + "ms-azuretools.vscode-docker", + "ms-python.python", + "ms-vscode.cpptools", + "redhat.vscode-yaml", + "smilerobotics.urdf", + "streetsidesoftware.code-spell-checker", + "twxs.cmake", + "yzhang.markdown-all-in-one", + "zachflower.uncrustify" + ], + "workspaceMount": "source=${localWorkspaceFolder},target=/src/astrobee/src,type=bind", + "workspaceFolder": "/src/astrobee/src" +} \ No newline at end of file diff --git a/.dockerignore b/.dockerignore index 7f2316acc0..231d1ed0cc 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,2 +1,6 @@ -.git/ -scripts/setup/Dockerfile +# .git/ This entry was unignored for scripts/docker/astrobee_quick.Dockerfile to retain git history +scripts/docker/ +submodules/android/ +submodules/avionics/ +submodules/common/ +submodules/platform/ diff --git a/.github/workflows/ci_pr.yml b/.github/workflows/ci_pr.yml new file mode 100644 index 0000000000..c8f5361792 --- /dev/null +++ b/.github/workflows/ci_pr.yml @@ -0,0 +1,24 @@ +name: Build and Test CI for Pull Requests + +on: + pull_request: + branches: [ 'master', 'develop' ] + workflow_dispatch: + +jobs: + + build-focal: + + runs-on: ubuntu-20.04 + + steps: + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + + - name: Checkout submodule + run: git submodule update --init --depth 1 description/media + + - name: Build, test + run: ./scripts/docker/build.sh --focal --remote + astrobee test_astrobee diff --git a/.github/workflows/ci_push.yml b/.github/workflows/ci_push.yml new file mode 100644 index 0000000000..f6ed14c486 --- /dev/null +++ b/.github/workflows/ci_push.yml @@ -0,0 +1,45 @@ +name: Build, test and push packages CI + +on: + push: + branches-ignore: + - gh-pages + - ros2 + +jobs: + + build-focal: + + runs-on: ubuntu-20.04 + + steps: + - uses: actions/checkout@v3 + + - name: Checkout submodule + run: git submodule update --init --depth 1 description/media + + - name: Log in to registry + run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin + + - name: Build, test, push docker + run: > + revision="latest"; + build_arg="astrobee"; + push_arg=""; + branch=${GITHUB_HEAD_REF:-${GITHUB_REF#refs/heads/}}; + if [ "${{ github.repository_owner }}" == "nasa" ]; then + if [ $branch == "develop" ]; then + push_arg="push_astrobee"; + fi; + if [ $branch == "master" ]; then + export VERSION=`grep -w -m 1 "Release" RELEASE.md | awk '{print $3}'`; + revision="v${VERSION}"; + build_arg="astrobee_base astrobee"; + push_arg="push_astrobee_base push_astrobee"; + fi; + fi; + command="./scripts/docker/build.sh --focal --remote --revision ${revision} \ + --owner ${{ github.repository_owner }} \ + ${build_arg} test_astrobee ${push_arg}"; + echo "$command"; + eval "$command"; \ No newline at end of file diff --git a/.github/workflows/docker_push_latest_base.yml b/.github/workflows/docker_push_latest_base.yml new file mode 100644 index 0000000000..a178bfa702 --- /dev/null +++ b/.github/workflows/docker_push_latest_base.yml @@ -0,0 +1,27 @@ +# Push the latest-base Docker images without doing a release. Called +# manually only when needed (basically, when we push a change that +# affects the base image on the develop branch). + +name: Docker push latest base + +on: ['workflow_dispatch'] + +jobs: + + build-focal: + + runs-on: ubuntu-20.04 + + steps: + - uses: actions/checkout@v3 + + - name: Checkout submodule + run: git submodule update --init --depth 1 description/media + + - name: Log in to registry + run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin + + - name: Build base, build, test, push base docker + run: ./scripts/docker/build.sh --focal + --owner ${{ github.repository_owner }} + astrobee_base astrobee test_astrobee push_astrobee_base diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml new file mode 100644 index 0000000000..7235e180aa --- /dev/null +++ b/.github/workflows/docs.yaml @@ -0,0 +1,96 @@ +# Build documentation and commit to gh-pages branch. + +name: Build and Push Documentation to gh-pages Branch + +on: + push: + branches: ['develop', 'master', 'ros2'] + workflow_dispatch: + +jobs: + build_and_push_docs: + runs-on: ubuntu-20.04 + + steps: + - name: Checkout + uses: actions/checkout@v3 + with: + path: repo/ + + - name: Build docker image with dependencies + build docs + run: docker build repo -f repo/scripts/docker/documentation.Dockerfile + -t astrobee:documentation + + - name: Checkout gh-pages + uses: actions/checkout@v3 + with: + path: gh-pages/ + ref: gh-pages + + - name: Create/replace documentation for the current branch + run: | + set -x + export BRANCH=$(cd repo && git branch --show-current) + + # Make gh-pages checkout an orphan commit so we don't keep useless history. + (cd gh-pages && git checkout --orphan fresh) + + # Install generated docs to a version folder based on branch name. + (cd gh-pages && git rm -rf --ignore-unmatch "v/$BRANCH") + mkdir -p gh-pages/v/ + docker cp $(docker create --rm astrobee:documentation):/repo/doc/html "gh-pages/v/$BRANCH" + (cd gh-pages && git add --all "v/$BRANCH") + + if [ "$BRANCH" == "develop" ]; then + # Update the few files at root level (mostly redirects) + (cd gh-pages && git rm -f --ignore-unmatch index.html documentation.html README.md 404.html .nojekyll) + cp repo/doc/documentation.html gh-pages/index.html + cp repo/doc/documentation.html gh-pages/documentation.html + cp repo/doc/README.md gh-pages/ + cp repo/doc/404.html gh-pages/ + touch gh-pages/.nojekyll + cp repo/doc/style/doc_version_select.js gh-pages/ + (cd gh-pages && git add index.html documentation.html README.md 404.html .nojekyll doc_version_select.js) + + # Set up HTML redirect to generated docs in the legacy path to + # avoid broken URL references + (cd gh-pages && git rm -rf --ignore-unmatch html) + python repo/doc/scripts/copy_html_link.py -v gh-pages/v/develop gh-pages/html + (cd gh-pages && git add --all html) + fi + + # If the commit is tagged, copy generated docs to a version + # folder based on the tag. Copy rather than symlink so the tag + # folder will remain valid later when the branch folder is + # updated. Note: If you want to manually remove an obsolete docs + # version, use git to check out the gh-pages branch, remove the + # relevant folder, and push back to origin. Also, this action + # only triggers when the branch is pushed, and it detects only + # the tags that are present at that time. So you should either + # (1) push the branch and its tag in the same push call + # (easiest), or (2) manually trigger the CI workflow to run on + # the relevant branch again, after it has been tagged. + (cd repo && git fetch origin --tags) + for tag in $(cd repo && git tag --points-at HEAD | xargs echo); do + (cd gh-pages && git rm -rf --ignore-unmatch "v/$tag") + cp -r "gh-pages/v/$BRANCH" "gh-pages/v/$tag" + (cd gh-pages && git add --all "v/$tag") + done + + # Auto-detect which docs versions are available. The script here + # has additional logic to ensure that develop, master, and ros2 + # are at the beginning of the list if they are present. + all_versions=$(ls "gh-pages/v/" | sort | perl -e '@dirs = ; chomp @dirs; %dirs_hash = map { $_ => 1 } @dirs; @head = grep { exists($dirs_hash{$_}) } ("develop", "master", "ros2"); %head_hash = map { $_ => 1 } @head; @tail = grep { !exists($head_hash{$_}) } @dirs; @versions = (@head, @tail); print "var allVersions = [\"", join("\", \"",@versions), "\"];\n";') + # Replace versions line specified in doc_version_select.js. + perl -i -ple "\$_ = '$all_versions' if /^var allVersions/;" gh-pages/doc_version_select.js + (cd gh-pages && git add doc_version_select.js) + + - name: Commit and Push + run: | + cd gh-pages + EMAIL=`git show -s --format='%ae' gh-pages` + NAME=`git show -s --format='%an' gh-pages` + git config user.email "$EMAIL" + git config user.name "$NAME" + { git commit -m "Automatic update for $GITHUB_SHA." || true; } + git push -f origin HEAD:gh-pages diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml new file mode 100644 index 0000000000..d779f6d155 --- /dev/null +++ b/.github/workflows/lint.yaml @@ -0,0 +1,30 @@ +# Check for lint error and auto correct them + +name: Check for lint errors + +on: ['push', 'pull_request', 'workflow_dispatch'] + +jobs: + lint_check_cpp: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Check repo for lint errors + run: ./scripts/git/cpplint_repo.py . + + lint_check_python: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Install linters + run: | + pip install click==8.0.1 black==22.1.0 isort==5.10.1 + + - name: Run black + run: | + black . --diff --extend-exclude cmake --check + - name: Run isort + run: | + isort . --diff --extend-skip cmake --profile black --check-only diff --git a/.github/workflows/msgs_jar.yaml b/.github/workflows/msgs_jar.yaml new file mode 100644 index 0000000000..ba9cdc8694 --- /dev/null +++ b/.github/workflows/msgs_jar.yaml @@ -0,0 +1,50 @@ +# Check for lint error and auto correct them + +name: Build jar messages + +on: ['push', 'pull_request'] + +jobs: + build-jar-msgs: + runs-on: ubuntu-20.04 + + steps: + - uses: actions/checkout@v3 + + - name: Build image astrobee/astrobee:msgs-ubuntu20.04 + run: docker build . -f ./scripts/docker/astrobee_msgs.Dockerfile + -t astrobee/astrobee:msgs-ubuntu20.04 + + - name: Build image astrobee/astrobee:latest-msgs-jar-ubuntu20.04 + run: docker build . -f ./scripts/docker/build_msgs_jar.Dockerfile + -t ghcr.io/${{ github.repository_owner }}/astrobee:latest-msgs-jar + + - name: Copy jar files + run: | + docker cp $(docker create --rm ghcr.io/${{ github.repository_owner }}/astrobee:latest-msgs-jar):/src/msgs/devel/share/maven/ . + find maven -name *.jar + + - name: Upload .jar messages artifact + if: ${{ github.event_name == 'push' }} + uses: actions/upload-artifact@v3 + with: + name: jar-msgs + path: maven/**/*.jar + + - name: Log in to registry + run: | + echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin + + - name: Push Docker image if master or develop + run: > + branch=${GITHUB_HEAD_REF:-${GITHUB_REF#refs/heads/}}; + if [[ "${{ github.repository_owner }}" == "nasa" ]]; then + if [[ $branch == "develop" ]]; then + docker push ghcr.io/${{ github.repository_owner }}/astrobee:latest-msgs-jar; + fi; + if [[ $branch == "master" ]]; then + export VERSION=`grep -w -m 1 "Release" RELEASE.md | awk '{print $3}'`; + docker tag ghcr.io/${{ github.repository_owner }}/astrobee:latest-msgs-jar ghcr.io/${{ github.repository_owner }}/astrobee:v${VERSION}-msgs-jar + docker push ghcr.io/${{ github.repository_owner }}/astrobee:v${VERSION}-msgs-jar; + fi; + fi; diff --git a/.gitignore b/.gitignore index ea62c86af6..82f212e91b 100644 --- a/.gitignore +++ b/.gitignore @@ -70,7 +70,6 @@ arm_build_release arm_build_debug armhf build_native -data doc/html doc/latex @@ -80,7 +79,6 @@ astrobee/config/dds #zones bin astrobee/zones/*.bin /description/*.tar.gz -/description/media /localization/vive_localization/src/ukf # catkin diff --git a/.gitmodules b/.gitmodules index da6034d3d6..5efb9fea8c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,12 +1,13 @@ -[submodule "submodules/common"] - path = submodules/common - url = https://babelfish.arc.nasa.gov/bitbucket/scm/astrobee/astrobee_common.git [submodule "submodules/android"] path = submodules/android - url = https://babelfish.arc.nasa.gov/bitbucket/scm/astrobee/astrobee_android.git + url = https://github.com/nasa/astrobee_android.git [submodule "submodules/avionics"] path = submodules/avionics - url = https://babelfish.arc.nasa.gov/bitbucket/scm/astrobee/astrobee_avionics.git + url = ssh://git@babelfish.arc.nasa.gov:7999/astrobee/astrobee_avionics.git [submodule "submodules/platform"] path = submodules/platform - url = https://babelfish.arc.nasa.gov/bitbucket/scm/astrobee/astrobee_platform.git + url = ssh://git@babelfish.arc.nasa.gov:7999/astrobee/astrobee_platform.git +[submodule "description/media"] + path = description/media + url = https://github.com/nasa/astrobee_media.git + shallow = true \ No newline at end of file diff --git a/.isort.cfg b/.isort.cfg new file mode 100644 index 0000000000..36c65ca36e --- /dev/null +++ b/.isort.cfg @@ -0,0 +1,18 @@ +[settings] + +# The src_paths setting should be a comma-separated list of all the +# folders in the astrobee repo that contain *.py files. Python files +# found in these folders count as "first party" so they should be in a +# separate group from the "third party" imports. If src_paths is not +# specified, isort will treat only the files it's asked to check as +# first party. Therefore, if we specify src_paths broadly in this way, +# isort behavior should be more repeatable between (1) manually +# running isort on a single file in your local dev machine, vs. (2) +# running the git pre-commit hook locally on your dev machine, which +# historically ran isort only on the files that changed since the last +# commit, or (3) running the CI workflow, which always runs isort on +# all files. If src_paths needs to be updated, like if *.py files are +# added to a new folder, you can auto-update it by running +# scripts/git/configure_isort_paths.sh. + +src_paths = doc/scripts,hardware/eps_driver/tools,hardware/pico_driver/scripts,hardware/pmc_actuator/tools,localization/camera/test,localization/localization_common,localization/localization_common/scripts/localization_common,localization/marker_tracking/tools/marker_tracking_node,localization/sparse_mapping/scripts,localization/sparse_mapping/tools,scripts/build,scripts/calibrate,scripts/debug,scripts/git,scripts/postprocessing/coverage_analysis,tools/bag_processing,tools/bag_processing/scripts,tools/bag_processing/scripts/utilities,tools/bag_processing/src/bag_processing,tools/bag_processing/src/bag_processing/bak,tools/bag_processing/test,tools/calibration/scripts,tools/gds_helper/src,tools/gnc_visualizer/dds,tools/gnc_visualizer/scripts,tools/gnc_visualizer/scripts/communications,tools/localization_analysis/scripts,tools/performance_tester/scripts diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 6d8e49e906..0000000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,391 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -cmake_minimum_required(VERSION 3.0) -project(Astrobee) - -set(ASTROBEE_VERSION 0.12.0) - -if ( "${CMAKE_VERSION}" VERSION_GREATER 3.0.0 ) - cmake_policy(SET CMP0045 OLD) - cmake_policy(SET CMP0046 OLD) -endif() - -set(ROS_FOUND FALSE) -if(DEFINED ENV{ROS_DISTRO}) - set(ROS_DISTRO $ENV{ROS_DISTRO}) - set(ROS_FOUND TRUE) -else() - message("ROS distro variable not set. Trying to figure it out...") - set(AVAILABLE_ROS_VERSIONS "melodic;lunar;kinetic;jade;indigo") - set(ROS_FOUND FALSE) - foreach(version ${AVAILABLE_ROS_VERSIONS}) - if(NOT ROS_FOUND) - find_path(ROS_H ros.h PATHS /opt/ros/${version}/include/ros) - if(ROS_H) - set(ROS_DISTRO ${version}) - message("Found ros version ${ROS_DISTRO}") - set(ROS_FOUND TRUE) - endif() - endif() - endforeach() -endif() - -# Verify the user has the pre-commit hook -execute_process( - COMMAND cp scripts/git/pre-commit .git/hooks - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} - ) - -# Let cmake know of our additional scripts -list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") -list(APPEND CMAKE_PREFIX_PATH "${CMAKE_SOURCE_DIR}/cmake") - -if (USE_CTC) - if("${ROS_DISTRO}" STREQUAL "kinetic") - list(APPEND CMAKE_PREFIX_PATH "${ARM_CHROOT_DIR}/opt/ros/kinetic") - elseif("${ROS_DISTRO}" STREQUAL "melodic") - list(APPEND CMAKE_PREFIX_PATH "${ARM_CHROOT_DIR}/opt/ros/melodic") - endif() -else (USE_CTC) - if("${ROS_DISTRO}" STREQUAL "kinetic") - list(APPEND CMAKE_PREFIX_PATH "/opt/ros/kinetic") - elseif("${ROS_DISTRO}" STREQUAL "melodic") - list(APPEND CMAKE_PREFIX_PATH "/opt/ros/melodic") - endif() -endif (USE_CTC) - -# Enable RPATHs -set(CMAKE_SKIP_BUILD_RPATH FALSE) -set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) - -# The following works for then the binaries are not installed, but are -# ran from the build directory. -list(APPEND CMAKE_INSTALL_RPATH "$ORIGIN/../lib") -if("${ROS_DISTRO}" STREQUAL "kinetic") - list(APPEND CMAKE_INSTALL_RPATH "/opt/ros/kinetic/lib") -elseif("${ROS_DISTRO}" STREQUAL "melodic") - list(APPEND CMAKE_INSTALL_RPATH "/opt/ros/melodic/lib") -endif() - -# The following RPATHs work for when the binaries are installed -if (USE_CTC) - list(APPEND CMAKE_INSTALL_RPATH "/opt/rti/ndds/lib/$ENV{NDDSARCH}") - set(CMAKE_INSTALL_RPATH_USE_LINK_PATH FALSE) -else (USE_CTC) - set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - list(APPEND CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") -endif (USE_CTC) - -# Define our options -option(USE_CCACHE - "Use ccache to speed up compiling, at the cost of optimization" - ON) -option(USE_CTC - "Use cross compile toolchain for making ARM binaries" - OFF) -option(USE_ROS - "Build the ROS-dependent functionality." - ON) -option(USE_DDS - "Build the DDS-dependent functionality." - ON) -option(USE_STATIC_LIBS - "Build using static libraries. Will use lots of drive space." - OFF) -option(TEST_COVERAGE - "Build the code with code coverage options. Not compatible with USE_CTC." - OFF) -option(USE_DRIVERS - "Build the tools in the drivers directory." - ON) -option(IS_BAMBOO_BUILD - "The code is being built under the bamboo CI system" - OFF) -option(ENABLE_GPROF - "Enable compling with support for profiling wih gprof (the GNU Profiler)." - OFF) -option(ENABLE_GOOGLE_PROF - "Enable support for profiling wih pprof (the Google Profiler)." - OFF) -option(ENABLE_QP - "Enable support for the QP planner." - ON) -option(ENABLE_PICOFLEXX - "Enable support for building the PicoFlexx driver" - ON) -option(ENABLE_GAZEBO - "Enable support for building the Gazebo simulator" - ON) -option(ENABLE_VIVE - "Enable support for building the Vive drivers" - ON) -option(ENABLE_VIVE_SOLVER - "Enable support for building the Vive offline solver" - ON) - -# If the user selected static libs .. set BUILD_SHARED_LIBS accordingly -if(USE_STATIC_LIBS) - set(BUILD_SHARED_LIBS OFF) -else(USE_STATIC_LIBS) - set(BUILD_SHARED_LIBS ON) -endif(USE_STATIC_LIBS) - -# Check that the compiler supports C++11 -include(CheckCXXCompilerFlag) -include(CheckCXXSourceCompiles) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif(COMPILER_SUPPORTS_CXX0X) - SET(LAMBDA_SRC "int main(int, char*[]) {int r = [] (int x) {return x;}(0); return r;}") - CHECK_CXX_SOURCE_COMPILES("${LAMBDA_SRC}" COMPILER_SUPPORTS_LAMBDAS) - if (NOT COMPILER_SUPPORTS_LAMBDAS) - message(SEND_ERROR "C++0x compiler doesn't support lambda functions.") - else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") - endif() -else() - message(SEND_ERROR "Couldn't find compiler which supports C++11 or C++0X.") -endif() - -if (TEST_COVERAGE) - find_program(GCOV_PATH gcov) - if (NOT GCOV_PATH) - message(SEND_ERROR "Couldn't find gcov, but test coverage was opted for.") - endif() -endif() - -if (ENABLE_GPROF) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pg") -endif() - -# if we're compiling for native use ccache to speed things up -if (NOT USE_CTC) - if (USE_CCACHE) - find_program(CCACHE_EXECUTABLE ccache) - mark_as_advanced(CCACHE_EXECUTABLE) - if(CCACHE_EXECUTABLE) - foreach(LANG C CXX) - message(STATUS "Enabling ccache for ${LANG}") - set(CMAKE_${LANG}_COMPILER_LAUNCHER ${CCACHE_EXECUTABLE} CACHE STRING "") - endforeach() - endif() - endif (USE_CCACHE) -endif (NOT USE_CTC) - -# Enable OpenMP if we have it. (Ceres will use it .. maybe more of our -# products will use it later). -find_package(OpenMP) -if (OPENMP_FOUND) - set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -endif(OPENMP_FOUND) - -find_package(Glog REQUIRED) -find_package(Gflags REQUIRED) -ADD_DEFINITIONS(-DFREEFLYER_GFLAGS_NAMESPACE=${GFLAGS_NAMESPACE}) -enable_testing() - -find_package(Protobuf REQUIRED) -if (NOT PROTOBUF_PROTOC_EXECUTABLE) - message(FATAL_ERROR "Could not find system's protoc execuable") -endif (NOT PROTOBUF_PROTOC_EXECUTABLE) - -find_package(Ceres REQUIRED) - -find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options) -find_package(Eigen3 REQUIRED) -find_package(ZLIB) -find_package(Luajit20 REQUIRED) - -# Find OpenCV and fix a 3.3.1 bug -find_package(OpenCV 3 REQUIRED) -if (${OpenCV_VERSION} MATCHES "3.3.1") - foreach(__cvcomponent ${OpenCV_LIB_COMPONENTS}) - set (__original_cvcomponent ${__cvcomponent}) - if(NOT __cvcomponent MATCHES "^opencv_") - set(__cvcomponent opencv_${__cvcomponent}) - endif() - if (TARGET ${__cvcomponent}) - set_target_properties(${__cvcomponent} PROPERTIES - MAP_IMPORTED_CONFIG_DEBUG "" - MAP_IMPORTED_CONFIG_RELEASE "" - MAP_IMPORTED_CONFIG_RELWITHDEBINFO "" - MAP_IMPORTED_CONFIG_MINSIZEREL "" - ) - endif() - endforeach(__cvcomponent) -endif() -set(OpenCV_LIBRARIES ${OpenCV_LIBS}) - -find_package(dbow2 REQUIRED) -find_package(Alvar REQUIRED) -find_package(OpenMVG QUIET REQUIRED) -find_package(JsonCpp REQUIRED) -find_package(FFMPEG QUIET) # Optional -if (ENABLE_PICOFLEXX) - find_package(royale REQUIRED) -endif (ENABLE_PICOFLEXX) -# Find pkg-config supported libraries -if (ENABLE_VIVE) - find_package(USB REQUIRED) - find_package(JSONC REQUIRED) - find_package(ARGTABLE2 REQUIRED) - find_package(ZLIB REQUIRED) -endif (ENABLE_VIVE) - - -if (NOT USE_CTC) # pkg-config needs extra work to be used for cross compile - pkg_check_modules(YAMLCPP yaml-cpp) - if (YAMLCPP_FOUND) - set (YAMLCPP_INCLUDE_DIRS ${YAMLCPP_INCLUDEDIR}) - endif (YAMLCPP_FOUND) -endif (NOT USE_CTC) - -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-unused-local-typedefs -Wno-packed-bitfield-compat -std=c++11") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-unused-local-typedefs -Wno-packed-bitfield-compat -std=c++11") -set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-undefined") -set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--no-undefined") - -# Set Architecture Specific Instructions -if (USE_CTC) - # Cross Compiling, armv7-a == Cortex A5, A7, A8 (Beagle Bone), A9 - # (Odroid), A12, A15 (Snapdragon-ish), A17 - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=armv7-a -mtune=cortex-a9 -mfpu=neon -I${ARM_CHROOT_DIR}/usr/include/arm-linux-gnueabihf") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv7-a -mtune=cortex-a9 -mfpu=neon -I${ARM_CHROOT_DIR}/usr/include/arm-linux-gnueabihf") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations -Wno-unused-variable") -else (USE_CTC) - if (NOT IS_BAMBOO_BUILD) - if ((NOT USE_CCACHE) OR (NOT CCACHE_EXECUTABLE)) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native -mtune=native") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native -mtune=native") - endif () - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mno-avx") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mno-avx") - endif (NOT IS_BAMBOO_BUILD) -endif (USE_CTC) - -# Bring in catkin -set(CATKIN_SKIP_TESTING ON CACHE BOOL "NO") -if (USE_CTC) - # Catkin brings in GTest as a dependency. We don't want it because - # we have our own ... on top of that it doesn't build correctly for - # ARM. Our own unit tests however will continue to - # work. (non-catkin) - set(CATKIN_ENABLE_TESTING OFF) - set(CATKIN_SKIP_TESTING OFF) -endif (USE_CTC) -if (USE_ROS) - set(ENV{ROS_LANG_DISABLE} "genlisp:gennodejs:geneus:$ENV{ROS_LANG_DISABLE}") - find_package(catkin2 COMPONENTS roscpp message_generation std_msgs geometry_msgs sensor_msgs cv_bridge image_transport tf tf2 tf2_ros rosbag nodelet) - - if (IS_BAMBOO_BUILD) - include(BambooFix) - endif() - - find_package(PCL REQUIRED COMPONENTS common) - find_package(Octomap MODULE REQUIRED) - #pkg_check_modules(OROCOS_KDL REQUIRED orocos_kdl) - if (USE_CTC) - if("${ROS_DISTRO}" STREQUAL "kinetic") - set(OROCOS_KDL_LIBRARIES ${ARM_CHROOT_DIR}/opt/ros/kinetic/lib/liborocos-kdl.so) - elseif("${ROS_DISTRO}" STREQUAL "melodic") - set(OROCOS_KDL_LIBRARIES ${ARM_CHROOT_DIR}/opt/ros/melodic/lib/liborocos-kdl.so) - endif() - endif (USE_CTC) - set(catkin_FOUND ${catkin2_FOUND}) -endif(USE_ROS) - -if (USE_DDS) - list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/soracore") - - if (USE_CTC) - set(SORACORE_ROOT_DIR ${ARM_CHROOT_DIR}/usr) - else (USE_CTC) - set(SORACORE_ROOT_DIR /usr) - endif (USE_CTC) - - set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) - - find_package(QtXml REQUIRED) - find_package(Miro REQUIRED) - find_package(RtiDds REQUIRED) - find_package(Soracore REQUIRED) -endif(USE_DDS) - -# Include Our CMake Functions -include(CreateLibrary) -include(CreateMsgTargets) -include(CreateRosTestTargets) -include(CreateTestTargets) -include(CreateToolTargets) -include(ExternalProjects) -include(FetchContent) -include(InstallLaunchFiles) - -add_subdirectory(external) - -if (TEST_COVERAGE) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g --coverage") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g --coverage") - set(CMAKE_LIBS_COVERAGE "gcov") -endif() - -set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) -set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) - -# install the version.txt file when install target is run -add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/version.txt - COMMAND ${CMAKE_COMMAND} - -D OUTPUT_FILE=${CMAKE_CURRENT_BINARY_DIR}/version.txt - -D ASTROBEE_VERSION=${ASTROBEE_VERSION} - -D GIT_MODULE_FILE="${CMAKE_SOURCE_DIR}/cmake/GetGitRevisionDescription.cmake" - -P ${CMAKE_SOURCE_DIR}/cmake/SetVersion.cmake) -add_custom_target(version DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/version.txt) -install(CODE "execute_process(COMMAND \"${CMAKE_COMMAND}\" --build \"${CMAKE_CURRENT_BINARY_DIR}\" --target version)") -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/version.txt DESTINATION ${CMAKE_INSTALL_PREFIX} ) - -# install the env_wrapper.sh for running on astrobee -install(PROGRAMS scripts/deploy/env_wrapper.sh DESTINATION ${CMAKE_INSTALL_PREFIX} ) - -if (USE_ROS) - add_subdirectory(tools) - add_subdirectory(astrobee) - add_subdirectory(communications) - add_subdirectory(gnc) - add_subdirectory(mobility) - add_subdirectory(behaviors) - add_subdirectory(management) - add_subdirectory(description) - if (NOT USE_CTC AND ENABLE_GAZEBO) - add_subdirectory(simulation) - endif (NOT USE_CTC AND ENABLE_GAZEBO) -endif (USE_ROS) - -if (USE_DRIVERS) - add_subdirectory(hardware) -endif (USE_DRIVERS) - -add_subdirectory(shared) -add_subdirectory(localization) - -if (USE_DDS) - add_subdirectory(wdock) -endif (USE_DDS) diff --git a/INSTALL.md b/INSTALL.md index 2b766d9da5..fed41de0f5 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -1,132 +1,86 @@ -# Usage instructions for non-NASA users +# Install -Install the 64-bit version of [Ubuntu 16.04](http://releases.ubuntu.com/16.04) -on a host machine, and make sure that you can checkout and build code. -If you are using a virtual machine, please use VMware. Virtualbox doesn't -support some of our Gazebo plugins. +## System requirements - sudo apt-get install build-essential git +Ubuntu 20.04 is currently the only supported platform. -*Note: You will need 4 GBs of RAM to compile the software. If you don't have -that much RAM available, please use swap space.* - -*Note: Please ensure you install Ubuntu 16.04. Running the simulation in Ubuntu 18.04 is possible, but not officially supported, instructions in README_Ubuntu18.md. At this time we do not support -any other operating system or Ubuntu version.* - -*Note: Please ensure you install the 64-bit version of Ubuntu. We do not -support running Astrobee Robot Software on 32-bit systems.* - -## Machine setup - -### Checkout the project source code - -At this point you need to decide where you'd like to put the source code -(`SOURCE_PATH`) on your machine: +Here are the currently available host OS options with development roadmap details (use 64-bit PC (AMD64) desktop image): +- [Ubuntu 20.04](http://releases.ubuntu.com/20.04): This is currently the only supported platform. The Astrobee hardware on ISS has been running Ubuntu 20.04 since it was upgraded during the "Crew-Minimal S14" activity on December 19, 2023. - export SOURCE_PATH=$HOME/astrobee +Specifically not supported: +- ~~[Ubuntu 16.04](http://releases.ubuntu.com/16.04)~~: No longer supported. The Astrobee robot hardware ran Ubuntu 16.04 from its launch in 2019 until it was upgraded to run Ubuntu 20.04. Ubuntu 16.04 support was discontinued in February 2024. Ending Ubuntu 16.04 support removed important limitations. For example, going forward, Astrobee's software will no longer need to be backward-compatible with Python 2 and OpenCV 3. +- ~~[Ubuntu 18.04](http://releases.ubuntu.com/18.04)~~: Ubuntu 18.04 support as a software development platform was discontinued as of November 2023. (It was never supported on the robot hardware.) +- ~~[Ubuntu 22.04](http://releases.ubuntu.com/22.04)~~: There is currently no plan for Ubuntu 22.04 support on the Astrobee roadmap. However, note that Astrobee ROS2 support, when it eventually becomes available, is currently expected to use the ROS2 Humble Hawksbill distribution that normally runs on 22.04, but backported to run on 20.04 (the last Ubuntu version supported by ROS1). This will facilitate migrating from ROS1 to ROS2 without requiring a simultaneous Ubuntu distribution upgrade. -First, clone the flight software repository: +Graphical interfaces will perform best if your host OS is running natively (not in a virtual machine). - git clone https://github.com/nasa/astrobee.git $SOURCE_PATH +Your host OS must have an X11 server installed if you want to use graphical applications, even if you are developing inside a Docker container (the X11 application running inside the container will forward its interface to the host's X11 server). X11 comes with Ubuntu Desktop by default. +If you plan to develop inside Docker, see [this page on using ROS with Docker](http://wiki.ros.org/docker/Tutorials#Tooling_with_Docker) for more details. -If you are planning to work on guest science stuff, you will also need the -`astrobee_android` repository. You should checkout the repository in the same -directory you checked out the source code in: +For users installing Astrobee on a Virtual Machine with the intent on running simulations: +VMWare and VirtualBox have been both tested to work well; Allocate an appropriate amount of RAM, number +of processors and video memory given your total computer capabilities; If graphics acceleration is +available on the settings, turn it on. +For reference (not required), an example of a setup capable of running the +simulation smoothly has 8GB RAM, 4 Processors and 128MB Video memory. - export ANDROID_PATH="${SOURCE_PATH}_android" - - -Clone the android repository: - - git clone https://github.com/nasa/astrobee_android.git $ANDROID_PATH - - -### Dependencies +*Note: You will need 4 GBs of RAM to compile the software. If you don't have +that much RAM available, please use swap space.* -Next, install all required dependencies: -*Note: `root` access is necessary to install the compiled debian packages below* -*Note: Before running this please ensure that your system is completely updated - by running 'sudo apt-get update' and then 'sudo apt-get upgrade'* +*Note: Please ensure you install the 64-bit PC (AMD64) version of Ubuntu (desktop for simulation and +development). We do not support running Astrobee Robot Software on 32-bit systems.* - pushd $SOURCE_PATH - cd scripts/setup - ./add_ros_repository.sh - sudo apt-get update - cd debians - ./build_install_debians.sh - cd ../ - ./install_desktop_16_04_packages.sh - sudo rosdep init - rosdep update - popd +## Option 1: Install inside a Docker container -**Important**: you can safely ignore the following error messages, as they are simply letting you know that certain libraries cannot be found. These libraries are for internal NASA use only, and are not required by public users provided that software is launched with DDS disabled. +1. Make sure you have Docker installed in your system by following: + - [Docker installation instructions](https://docs.docker.com/engine/install/ubuntu/) + - [Docker post-installation configuration for Linux](https://docs.docker.com/engine/install/linux-postinstall/) + - If your system has a discrete graphics card, you may need to install [additional software for hardware acceleration](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration). - E: Unable to locate package libroyale1 - E: Unable to locate package rti - E: Unable to locate package libmiro0 - E: Unable to locate package libsoracore1 - E: Unable to locate package libroyale-dev - E: Unable to locate package rti-dev - E: Unable to locate package libsoracore-dev - E: Unable to locate package libmiro-dev +2. Check out the Astrobee Robot Software with: + ```bash + export ASTROBEE_WS=$HOME/astrobee # your choice where + git clone https://github.com/nasa/astrobee.git $ASTROBEE_WS/src + cd $ASTROBEE_WS/src + git submodule update --init --depth 1 description/media + ``` -## Configuring the build +3. Here is a quick-start command to install the Astrobee Robot Software inside a Docker container and start a simulation run: + ```bash + ./scripts/docker/run.sh --remote + ``` -### Note for the build setup -By default, the configure script uses the following paths: - - native build path: `$HOME/astrobee_build/native` - - native install path: `$HOME/astrobee_install/native` +There is also experimental support for using the Visual Studio Code Dev Containers plugin to access an integrated development environment running inside the Docker container! -If you are satisfied with these paths, you can invoke the `configure.sh` without -the `-p` and `-b` options. For the simplicity of the instructions below, -we assume that `$BUILD_PATH` and `$INSTALL_PATH` contain the location of the -build and install path. For example: +For much more discussion, see: \subpage install-docker. - export BUILD_PATH=$HOME/astrobee_build/native - export INSTALL_PATH=$HOME/astrobee_install/native +## Option 2: Install in your native OS / Virtual Machine -### Native build +The native installation instructions below walk you through manually running the same steps that are fully automated in a Docker installation. -The configure script prepares your build directory for compiling the code. Note -that `configure.sh` is simply a wrapper around CMake that provides an easy way -of turning on and off options. To see which options are supported, simply run -`configure.sh -h`. +- If you are an external developer, see: \subpage install-nonNASA - pushd $SOURCE_PATH - ./scripts/configure.sh -l -F -D - popd +- If you have NASA internal access and need to cross-compile for the robot hardware, see: \subpage install-NASA -If you want to explicitly specify the build and install directories, use -instead: +### Important Notice: OpenCV 4 Upgrade - ./scripts/configure.sh -l -F -D -p $INSTALL_PATH -b $BUILD_PATH +Starting from version 0.18.0, our project uses exclusively OpenCV 4. If you have an earlier version of Astrobee natively build, please follow the steps below to uninstall the previous debians: -*Note: Make sure you use the -F and -D flags. If these flags are not used, the -code will not compile. The -F flag is used to turn off building the Picoflex. -This is only needed for running on the robot and is not needed for the -simulator. The -D is used to turn off building the dds bridge. The bridge is -used to communicate with our ground data system and is also not needed for the -simulator.* + dpkg -r astrobee0 + dpkg -r libalvar2 + dpkg -r libopenmvg1 + dpkg -r libdbow21 + dpkg -r libdbowdlib1 + dpkg -r libopencv3.3.1 -## Building the code -To build, run `make` in the `$BUILD_PATH`. Note that depending on your host -machine, this might take in the order of tens of minutes to complete the first -time round. Future builds will be faster, as only changes to the code are -rebuilt, and not the entire code base. +once this is done, please follow the external developer install instructions \ref install-nonNASA: - pushd $BUILD_PATH - make -j2 + pushd $ASTROBEE_WS + cd src/scripts/setup/debians + sudo apt-get update + ./build_install_debians.sh + cd ../ + ./install_desktop_packages.sh popd - -If you configured your virtual machine with more than the baseline resources, -you can adjust the number of threads (eg. -j4) to speed up the build. - -For more information on running the simulator and moving the robot, please see -the [simulation instructions](simulation/sim_overview.md). - -## Ubuntu 18.04 Install - -Ubuntu 18.04 is currently not officially supported diff --git a/INSTALL_Ubuntu18.md b/INSTALL_Ubuntu18.md deleted file mode 100644 index 276a27960f..0000000000 --- a/INSTALL_Ubuntu18.md +++ /dev/null @@ -1,137 +0,0 @@ -# Usage instructions for non-NASA users - -Install the 64-bit version of [Ubuntu 18.04](http://releases.ubuntu.com/18.04) -on a host machine, and make sure that you can checkout and build code. -If you are using a virtual machine, please use VMware. Virtualbox doesn't -support some of our Gazebo plugins. - - sudo apt-get install build-essential git - -*Note: You will need 4 GBs of RAM to compile the software. If you don't have -that much RAM available, please use swap space.* - -*Note: This version of Ubuntu is only supported for native install and not for cross-compile.* - -*Note: Please ensure you install the 64-bit version of Ubuntu. We do not -support running Astrobee Robot Software on 32-bit systems.* - -## Machine setup - -### Checkout the project source code - -At this point you need to decide where you'd like to put the source code -(`SOURCE_PATH`) on your machine: - - export SOURCE_PATH=$HOME/astrobee - -First, clone the flight software repository: - - git clone https://github.com/nasa/astrobee.git $SOURCE_PATH - - -If you are planning to work on guest science stuff, you will also need the -`astrobee_android` repository. You should checkout the repository in the same -directory you checked out the source code in: - - export ANDROID_PATH="${SOURCE_PATH}_android" - - -Clone the android repository: - - git clone https://github.com/nasa/astrobee_android.git $ANDROID_PATH - - -### Dependencies - -Next, install all required dependencies: -*Note: `root` access is necessary to install the compiled debian packages below* -*Note: You should instal ROS melodic beforehand following the [Installation Instructions](http://wiki.ros.org/melodic/Installation/Ubuntu)* -*Note: Before running this please ensure that your system is completely updated - by running 'sudo apt-get update' and then 'sudo apt-get upgrade'* - - pushd $SOURCE_PATH - cd scripts/setup - ./add_ros_repository.sh - sudo apt-get update - -Install OpenCV 3.3.1, you can parse the argument -p to change install directory. By default, install directory is in /usr/local. If you do so, make sure CMake can find it. - - ./install_opencv.sh - -Install Luajit, it will be installed in /usr/local - - ./install_luajit.sh - - cd debians - ./build_install_debians_18_04.sh - cd ../ - ./install_desktop_18_04_packages.sh - sudo rosdep init - rosdep update - popd - -**Important**: you can safely ignore the following error messages, as they are simply letting you know that certain libraries cannot be found. These libraries are for internal NASA use only, and are not required by public users provided that software is launched with DDS disabled. - - E: Unable to locate package libroyale1 - E: Unable to locate package rti - E: Unable to locate package libmiro0 - E: Unable to locate package libsoracore1 - E: Unable to locate package libroyale-dev - E: Unable to locate package rti-dev - E: Unable to locate package libsoracore-dev - E: Unable to locate package libmiro-dev - -## Configuring the build - -### Note for the build setup -By default, the configure script uses the following paths: - - native build path: `$HOME/astrobee_build/native` - - native install path: `$HOME/astrobee_install/native` - -If you are satisfied with these paths, you can invoke the `configure.sh` without -the `-p` and `-b` options. For the simplicity of the instructions below, -we assume that `$BUILD_PATH` and `$INSTALL_PATH` contain the location of the -build and install path. For example: - - export BUILD_PATH=$HOME/astrobee_build/native - export INSTALL_PATH=$HOME/astrobee_install/native - -### Native build - -The configure script prepares your build directory for compiling the code. Note -that `configure.sh` is simply a wrapper around CMake that provides an easy way -of turning on and off options. To see which options are supported, simply run -`configure.sh -h`. - - pushd $SOURCE_PATH - ./scripts/configure.sh -l -F -D - popd - -If you want to explicitly specify the build and install directories, use -instead: - - ./scripts/configure.sh -l -F -D -p $INSTALL_PATH -b $BUILD_PATH - -*Note: Make sure you use the -F and -D flags. If these flags are not used, the -code will not compile. The -F flag is used to turn off building the Picoflex. -This is only needed for running on the robot and is not needed for the -simulator. The -D is used to turn off building the dds bridge. The bridge is -used to communicate with our ground data system and is also not needed for the -simulator.* - -## Building the code - -To build, run `make` in the `$BUILD_PATH`. Note that depending on your host -machine, this might take in the order of tens of minutes to complete the first -time round. Future builds will be faster, as only changes to the code are -rebuilt, and not the entire code base. - - pushd $BUILD_PATH - make -j2 - popd - -If you configured your virtual machine with more than the baseline resources, -you can adjust the number of threads (eg. -j4) to speed up the build. - -For more information on running the simulator and moving the robot, please see -the [simulation instructions](simulation/sim_overview.md). diff --git a/NASA_INSTALL.md b/NASA_INSTALL.md deleted file mode 100644 index f7df9aed2a..0000000000 --- a/NASA_INSTALL.md +++ /dev/null @@ -1,258 +0,0 @@ -# Usage instructions for NASA users - -Install the 64-bit version of -[Ubuntu16.04](http://releases.ubuntu.com/16.04) on a host machine, and -make sure that you can checkout and build code. If you are using a -virtual machine, please use VMware. Virtualbox doesn't support some of -our Gazebo plugins. - - sudo apt-get install build-essential git - -*Note: Please ensure you install the 64-bit version of Ubuntu. We do not -support running Astrobee Robot Software on 32-bit systems.* - -## Computer setup - -### Username - -If you are using a VM with a username that does not match your NDC username, -please configure the following variable: - - export NDC_USERNAME=your_ndc_username - - -### Access to the Astrobee Debian server - -The custom debian packages are currently distributed by the -`astrobee.ndc.nasa.gov server`. This server is currently on the ARC TI -private network. It is *critical* to be able to reach this server to -install the pre-built custom debian. If none of the solutions below -allow you to reach `astrobee.ndc.nasa.gov`, you can use the -instructions on how to build the Debian dependencies manually -following the INSTALL.md instructions (Dependencies section). - -#### If on the ARC TI private network - -This is the typical case for all wired computers in ARC TI, and simplifies -your life greatly. - -Verify that you are in this situation with the command below should succeed -(remove the Release.gpg file after being fetched). - - wget -v http://astrobee.ndc.nasa.gov/software/dists/xenial/Release.gpg - -Before running the scripts in `scripts/setup` below, set this variable: - - export NO_TUNNEL=1 - -#### If not on the ARC TI private network - -If you are outside the NASA ARC private network, there are two options to -reach `astrobee.ndc.nasa.gov`: - - 1. Use VPN to act like if you were inside the ARC TI private network and - obtain the correct kerberos credentials inside the VM with the following - command (note the capitalization): -``` -kinit $NDC_USERNAME@NDC.NASA.GOV` -``` - 2. setup your `.ssh/config` to do ssh forwarding. A tutorial on this method - is available at: https://babelfish.arc.nasa.gov/trac/freeflyer/wiki/SSHSetup - -For either solution, please verify that you can SSH to `m.ndc.nasa.gov` without -entering your password (`m` is used to tunnel to `astrobee.ndc.nasa.gov`): - - ssh $NDC_USERNAME@m.ndc.nasa.gov - -The command should succeed without entering your password. Once this is verified, -exit this session on `m` with +D. - -- These notes apply to `install_desktop_16.04_packages.sh` and `make_xenial.sh` - - -### Checkout the project source code - -At this point you need to decide where you'd like to put the source code -(`SOURCE_PATH`) on your machine: - - export SOURCE_PATH=$HOME/astrobee - -First, clone the flight software repository: - - git clone --recursive https://$NDC_USERNAME@babelfish.arc.nasa.gov/bitbucket/scm/astrobee/astrobee.git \ - --branch develop $SOURCE_PATH - -(Note: re-enter your username and password for every submodules that are cloned) - -### Dependencies - -Next, install all required dependencies: - - pushd $SOURCE_PATH - cd scripts/setup - ./add_local_repository.sh - ./add_ros_repository.sh - ./install_desktop_16_04_packages.sh - popd - -#### Extra options to install the dependencies - -- If you do not want to configure your `.ssh/config` to just get the -dependencies, you can use the `NDC_USERNAME` variable. -- By default, the custom debians are installed in `$SOURCE_PATH/.astrobee_deb`. -If you prefer to install them at a different location, you can use the -`ARS_DEB_DIR` variable. - -``` -export NDC_USERNAME=jdoe -export ARS_DEB_DIR=$HOME/astrobee_debs -./add_local_repository.sh -``` - -### Cross-compile setup - -If you are planning to compile code to run on the robot hardware, you will need -to install a cross-compile chroot and toolchain. Select two directories for -these: - - export ARMHF_CHROOT_DIR=$HOME/arm_cross/rootfs - export ARMHF_TOOLCHAIN=$HOME/arm_cross/toolchain/gcc - -Append these lines to your .bashrc file, as you will need these two variables -every time you cross compile. - -Next, download the cross toolchain and install the chroot: - - mkdir -p $ARMHF_TOOLCHAIN - cd $HOME/arm_cross - $SOURCE_PATH/submodules/platform/fetch_toolchain.sh - $SOURCE_PATH/submodules/platform/rootfs/make_xenial.sh dev $ARMHF_CHROOT_DIR - -## Configuring the build - -At this point you need to decide whether you'd like to compile natively -[`native`] (run code against a simulator) or for an ARM target [`armhf`] (run -the code on the robot itself). Please skip to the relevant subsection. - -### Note for both builds setup - -By default, the configure script uses the following paths: - - - native build path (BUILD_PATH): `$HOME/astrobee_build/native` - - native install path (INSTALL_PATH): `$HOME/astrobee_install/native` - - armhf build path (BUILD_PATH): `$HOME/astrobee_build/armhf` - - armhf install path (INSTALL_PATH): `$HOME/astrobee_install/armhf` - -You should set these values in your shell. - -If you are satisfied with these paths, you can invoke the `configure.sh` without -the `-p` and `-b` options. For the simplicity of the instructions below, -we assume that `$BUILD_PATH` and `$INSTALL_PATH` contain the location of the -build and install path for either `native` or `armhf` platforms. - -### Native build - -The configure script prepares your build directory for compiling the code. Note -that `configure.sh` is simply a wrapper around CMake that provides an easy way -of turning on and off options. To see which options are supported, simply run -`configure.sh -h`. - - pushd $SOURCE_PATH - ./scripts/configure.sh -l - popd - -If you want to explicitly specify the build and install directories, use -instead: - - ./scripts/configure.sh -l -p $INSTALL_PATH -b $BUILD_PATH - -### Cross-compile build - -Cross compiling for the robot follows the same process, except the configure -script takes a `-a` flag instead of `-l`. - - pushd $SOURCE_PATH - ./scripts/configure.sh -a - popd - -Or with explicit build and install paths: - - ./scripts/configure.sh -a -p $INSTALL_PATH -b $BUILD_PATH - -*Warning: `$INSTALL_PATH` and `$BUILD_PATH` used for cross compiling HAVE to be -different than the paths for native build! See above for the default values -for these.* - -## Building the code - -To build, run `make` in the `$BUILD_PATH`. Note that depending on your host -machine, this might take in the order of tens of minutes to complete the first -time round. Future builds will be faster, as only changes to the code are -rebuilt, and not the entire code base. - - pushd $BUILD_PATH - make -j6 - popd - -*Note: `$BUILD_PATH` above is either the path for native build or armhf build, -whatever you currently are doing.* - -## Running a simulation - -In order to run a simulation you must have build natively. You will need to -first setup your environment, so that ROS knows about the new packages provided -by Astrobee flight software: - - pushd $BUILD_PATH - source devel/setup.bash - popd - -After this command has completed, you should be able to run a simulator from any -directory in your Linux filesystem. So, for example, to start a simulation of a -single Astrobee in the Granite Lab, run the following: - - roslaunch astrobee sim.launch - -This command tells ROS to look for the `sim.launch` file provided by the -`astrobee` package, and use roslaunch to run it. Internally, ROS maintains a -cache of information about package locations, libraries and executables. If you -find that the above command doesn't work, try rebuilding the cache: - - rospack profile - -A simulator readme was created for guest science users. However this readme may -be beneficial to interns and/or new members. If you fall into one of these -categories, please see the [simulation instructions](simulation/sim_overview.md). - -## Running the code on a real robot - -In order to do this, you will need to have followed the cross-compile build -instructions. Once the code has been built, you also need to install the code to -a singular location. CMake remembers what `$INSTALL_PATH` you specified, and -will copy all products into this directory. - - pushd $BUILD_PATH - make install - popd - -Once the installation has completed, copy the install directory to the robot. -This script assumes that you are connected to the Astrobee network, as it uses -rsync to copy the install directory to `~/armhf` on the two processors. It -takes the robot name as an argument. Here we use `p4d'. - - pushd $SOURCE_PATH - ./scripts/install_to_astrobee.sh $INSTALL_PATH p4d - popd - -Here, p4d is the name of the robot, which may be different in your case. - -You are now ready to run the code. This code launches a visualization tool, -which starts the flight software as a background process. - - pushd $SOURCE_PATH - python ./tools/gnc_visualizer/scripts/visualizer --proto4 - popd - -# Further information - -Please refer to the [wiki](https://babelfish.arc.nasa.gov/trac/freeflyer). diff --git a/README.md b/README.md index 049143a317..0fb8c70fa0 100644 --- a/README.md +++ b/README.md @@ -1,98 +1,62 @@ -# Astrobee Robot Software - Flight Software repository +# Astrobee Robot Software -## About +### About -

- -Astrobee is a free-flying robot designed to operate as a payload inside -the International Space Station (ISS). The Astrobee Robot Software consists of -embedded (on-board) software, supporting tools and a simulator. The Astrobee -Robot Software operates on Astrobee's three internal single board computers and -uses the open-source Robot Operating System (ROS) framework as message-passing -middleware. The Astrobee Robot Software performs vision-based localization, +Three NASA [Astrobee free-flying robots](https://nasa.gov/astrobee/) have been operating inside +the International Space Station (ISS) since 2019. This [`astrobee`](https://github.com/nasa/astrobee) repository contains source code for the Astrobee Robot Software, consisting of +the flight software that runs onboard the Astrobee robots, a software simulator, and supporting tools, primarily written in C++. + +Astrobee's flight software performs vision-based localization, provides autonomous navigation, docking and perching, manages various sensors -and actuators, and supports user interaction via screen-based displays, light -signaling, and sound. The Astrobee Robot Software enables Astrobee to be -operated in multiple modes: plan-based task execution (command sequencing), -teleoperation, or autonomously through execution of hosted code uploaded by -project partners (guest science). The software simulator enables Astrobee Robot -Software to be evaluated without the need for robot hardware. -

+and actuators, and supports human-robot interaction with co-located astronauts via screen-based displays, light +signaling, and sound. The flight software is hosted on each Astrobee's three internal single board computers and +uses the open-source [Robot Operating System (ROS)](https://www.ros.org/) framework as message-passing +middleware. It provides a high-level [Astrobee Command API](https://nasa.github.io/astrobee/html/command_dictionary.html) for controlling the robot and has multiple operating modes. It can execute a plan (command sequence), individual operator commands (teleoperation), or commands from guest science code running onboard Astrobee. + +The Astrobee Robot Software simulator, built using [ROS](https://www.ros.org/) and [Gazebo](http://gazebosim.org/), enables the flight software to be evaluated without the need for robot hardware. The supporting tools include a tool that processes ISS imagery to [build maps for Astrobee localization](https://nasa.github.io/astrobee/html/sparsemapping.html), along with many others. -This repository provides flight software and a simulator, both primarily written -in C++. The repository also provides several other utilities, including a tool -for creating maps for localization. A separate repository, -[`astrobee_android`](https://github.com/nasa/astrobee_android), contains the -Java API, which uses the ROS messaging system to communicate with flight -software. +Released separately, the Astrobee ground data system (GDS) includes Astrobee control station software that communicates with Astrobee flight software via the Data Distribution Service (DDS) network protocol over the ISS Ku-IP space-to-ground link. The control station is written primarily in Java using the Eclipse RCP framework. Source code is in the [`astrobee_gds`](https://github.com/nasa/astrobee_gds) repository, or you can download the [binary release](https://software.nasa.gov/software/ARC-17994-1B). -The Astrobee Robot Software is in a beta stage. This means that some -features are incomplete, and extensive changes can be expected. Please consult -[RELEASE](RELEASE.md) for the current list of features and limitations. +Together, the Astrobee Robot and Ground Software were the NASA Software of the Year Award Runner-Up in 2020! -## Usage instructions +The Astrobee Robot Software remains a work in progress. Please consult the +[release notes](https://nasa.github.io/astrobee/html/md_RELEASE.html) for the current list of features and limitations. -If you don't have access to NASA's internal network, please see the -[usage instructions](INSTALL.md). +### Usage -If you do, please see the [NASA usage instructions](NASA_INSTALL.md). +[Instructions on installing and using the Astrobee Robot Software](https://nasa.github.io/astrobee/html/md_INSTALL.html). -## Contributors +### Contributing The Astrobee Robot Software is open source, and we welcome contributions -from the public. However, for us to merge any pull requests, we must request -that contributors sign and submit a +from the public. Please submit pull requests to the [`develop`](https://github.com/nasa/astrobee/tree/develop) branch. The code must follow the [Astrobee code style](https://nasa.github.io/astrobee/html/astrobee-code-style.html). +For us to merge any pull requests, we must request that contributors sign and submit a [Contributor License Agreement](https://www.nasa.gov/sites/default/files/atoms/files/astrobee_individual_contributor_license_agreement.pdf) due to NASA legal requirements. Thank you for your understanding. -## Documentation - -To generate the Astrobee documentation, run the following command after checking -out the code (the latest version of doxygen should be [installed](https://www.doxygen.nl/download.html) for correct -markdown support) : - - doxygen doc/astrobee.doxyfile - -The documentation will be generated in the doc/html folder. Open the doc/documentation.html -to get to the main page of the documentation. - -For familiarization with the Astrobee project, it is recommended to read some -background papers: +### Documentation -#### Astrobee Software Concepts -[Lorenzo Fluckiger, Kathryn Browne, Brian Coltin, Jesse Fusco, Theodore Morse, -and Andrew Symington. “Astrobee Robot Software: Enabling Mobile Autonomy on the -ISS”, *In Proc. of the Int. Symposium on Artificial Intelligence, Robotics and -Automation in Space (i-SAIRAS)*, 2018.]( -https://www.nasa.gov/sites/default/files/atoms/files/fluckiger2018astrobee.pdf) +[Extensive documentation is auto-generated from the contents of this repository.](https://nasa.github.io/astrobee/documentation.html) -#### Localization method -[Brian Coltin et al. “Localization from visual landmarks on a free-flying -robot”. *In: Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International -Conference on. IEEE.* 2016, pp. 4377–4382.]( -https://www.nasa.gov/sites/default/files/atoms/files/coltin2016localization.pdf) +["A Brief Guide to Astrobee’s Flight Software"](https://github.com/albee/a-brief-guide-to-astrobee/raw/master/a_brief_guide_to_astrobee_latest.pdf) is a good tutorial, with a particular emphasis on the advanced topic of modifying Astrobee's flight software to enable Guidance, Navigation, & Control (GN&C) research. (Note that most guest science can be implemented as an app that uses the [Astrobee Command API](https://nasa.github.io/astrobee/html/command_dictionary.html) without modifying the flight software.) -#### Astrobee platform -[Maria Bualat et al. "Astrobee: A New Tool for ISS Operations." *2018 -SpaceOps Conference*. 2018.]( -https://www.nasa.gov/sites/default/files/atoms/files/bualat_spaceops_2018_paper.pdf) +The ["Astrobee ROS Demo"](https://github.com/Pedro-Roque/astrobee_ros_demo) package provides a C++/Python interface to implement custom control methods in ROS. -## Guest Science +For more information, read [Astrobee-related publications](https://www.nasa.gov/content/research-publications-0). +Learning about the Astrobee [platform](https://www.nasa.gov/sites/default/files/atoms/files/bualat_spaceops_2018_paper.pdf), +[software](https://www.nasa.gov/sites/default/files/atoms/files/fluckiger2018astrobee.pdf), +and [localization](https://www.nasa.gov/sites/default/files/atoms/files/coltin2016localization.pdf) +are good starting points. -If you are interested in guest science, please do one of the following for more -information. +### Guest Science -If you are a non-NASA user, please checkout the astrobee_android nasa github -project (if you followed the usage instructions, you should have checked this -out already). Once that is checked out, please see -[`astrobee_android/README.md`](https://github.com/nasa/astrobee_android/blob/master/README.md) -located in the `astrobee_android/` folder. +The ISS Astrobee Facility maintains a collection of [resources for guest scientists](https://www.nasa.gov/content/guest-science-resources) interested in conducting research with Astrobee, including the [Astrobee Guest Science Guide](https://www.nasa.gov/sites/default/files/atoms/files/irg-ff029-astrobee-guest-science-guide.pdf) overview. -If you are a NASA user, please make sure the `android` submodule was checked out -and then visit `submodules/android/guest_science_readme.md`. +Guest science code that runs onboard Astrobee is usually hosted as an Android Java app running on Astrobee's high-level processor (HLP). Guest science apps can use the [Astrobee Command API](https://nasa.github.io/astrobee/html/command_dictionary.html) through its Java bindings. The [`astrobee_android`](https://github.com/nasa/astrobee_android) repository contains source code for Astrobee services that run on the HLP, example guest science apps, as well as a [Guest Science Readme](https://github.com/nasa/astrobee_android/blob/master/guest_science_readme.md) and [Guest Science Developer Guide](https://github.com/nasa/astrobee_android/blob/master/gs_developer_guide.md) focused on HLP guest science apps. -## License +### License Copyright (c) 2017, United States Government, as represented by the Administrator of the National Aeronautics and Space Administration. diff --git a/RELEASE.md b/RELEASE.md index 4652e6c0be..28addb61ad 100644 --- a/RELEASE.md +++ b/RELEASE.md @@ -1,4 +1,141 @@ -# Astrobee Robot Software v1 +# Releases + +## Release 0.19.1 + + * Updated Map Matching with TEBLID and CLAHE + +## Release 0.19.0 + + * New split localizer + +## Release 0.18.0 + + * Astrobee now using OpenCV 4! Please see install updates in INSTALL.md + +## Release 0.17.3 + + * Comms bridge added rate feature + * Bug fixes + * User feedback improvements + +## Release 0.17.2 + + * Adding Generic comms bridge + * Multiple bug fixes + * Ubuntu 20 only supported now + +## Release 0.17.1 + + * Multiple bug fixes + +## Release 0.17.0 + + * Full compatibility and debians built for Ubuntu 20 + * Deleted all simulink autocode for ctl and fam + +## Release 0.16.7 + + * Remove simulink auto-generated code + * improved documentation, other tools and fixes + +## Release 0.16.6 + + * switch map online possibility + * other fixes + +## Release 0.16.5 + + * Fix multiple bugs + +## Release 0.16.4 + + * Auto-exposure + * Add splice tool + * Add arm deploy to bridge + * Various bug fixes + +## Release 0.16.3 + + * Added bag tools + * Disabled mapper (can be enabled through service) + * Add debayer live + * Multiple other fixes and improvements + +## Release 0.16.2 + + * Performance improvements + * Small bug fixes + +## Release 0.16.1 + + * depth odometry + * better performance evaluation + * multiple other fixes and improvements + +## Release 0.16.0 + + * project compiles with catkin + * mapper performance improvement + * imu_augmentor performance improvement + * calibration improvements + * multiple other fixes + +## Release 0.15.2 + + * added python linters black and isort in the CI pipeline + * added perching arm reconnect service + * added nav and dock cam bayer image output option + * added code to report time for llp, mlp, hlp + * added end of motion check + +## Release 0.15.1 + + * Separated config files for DDS communications to avoid conflict + * Increased plan name length + * Pico proxy was added to the MLP launch file + * System monitor now attempts to reload nodelets that die on startup + * Other fixes/software and documentation improvements + +## Release 0.15.0 + + * Perching functional + * Ubuntu 20 compatibility + * Added astrobee to astrobee coms + * Various other fixes/software improvements + +## Release 0.14.3 + + * Rotation fallback fix + * Unified install instructions and Ubuntu 18 nasa install + * Various other minor fixes + * Note, Perching does not work in this release, use another version for this. + +## Release 0.14.1 + + * Dynamic IMU Filtering + * Added measurement spacing for visual odometry + * Tuning configurations + * Bug fixes + Note: Perching does not work in this release, use another version for this. + +## Release 0.14.0 + + * Added graph_localizer package + * Added supporting packages for graph localizer including imu_augmetor, imu_integation, localization_common, localization_measurements + * Added tools for graph localizer including graph_bag and imu_bias_tester + * Added rviz plugins for graph localizer and associated packages in localization_rviz_plugins + * Added gtsam debian + Note: Perching does not work in this release, use another version for this. + +## Release 0.13.0 + + * Multiple updates to documentation, doxygen is exported to github pages + * Adding Github actions to build and test the code and generate documentation + * astrobee_media now a submodule + * SciCam now generates compressed images + * Added memory monitor + * Cross compiling with docker + * Other fixes ## Release 0.12.0 diff --git a/doc/astrobee.doxyfile b/astrobee.doxyfile similarity index 98% rename from doc/astrobee.doxyfile rename to astrobee.doxyfile index 9a06dd4fcc..f7c5a449a2 100644 --- a/doc/astrobee.doxyfile +++ b/astrobee.doxyfile @@ -21,7 +21,7 @@ # that follow. The default is UTF-8 which is also the encoding used for all text # before the first occurrence of this tag. Doxygen uses libiconv (or the iconv # built into libc) for the transcoding. See http://www.gnu.org/software/libiconv -# for the list of possible encodings. +# for the list of possible encodings.F # The default value is: UTF-8. DOXYFILE_ENCODING = UTF-8 @@ -38,13 +38,14 @@ PROJECT_NAME = "NASA Astrobee Robot Software" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 0.12.0 + +PROJECT_NUMBER = 0.19.1 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a # quick idea about the purpose of the project. Keep the description short. -PROJECT_BRIEF = "Flight software for the Astrobee robot operating inside the International Space Station." +PROJECT_BRIEF = "Flight software for the Astrobee robots operating inside the International Space Station." # With the PROJECT_LOGO tag one can specify a logo or an icon that is included # in the documentation. The maximum height of the logo should not exceed 55 @@ -604,7 +605,7 @@ STRICT_PROTO_MATCHING = NO # list. This list is created by putting \todo commands in the documentation. # The default value is: YES. -GENERATE_TODOLIST = YES +GENERATE_TODOLIST = NO # The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test # list. This list is created by putting \test commands in the documentation. @@ -684,7 +685,7 @@ FILE_VERSION_FILTER = # DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE # tag is left empty. -LAYOUT_FILE = doc/DoxygenLayout.xml +LAYOUT_FILE = doc/style/freeflyer_layout.xml # The CITE_BIB_FILES tag can be used to specify one or more bib files containing # the reference definitions. This must be a list of .bib files. The .bib @@ -771,7 +772,11 @@ WARN_LOGFILE = # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING # Note: If this tag is empty the current directory is searched. -INPUT = doc \ +INPUT = README.md \ + INSTALL.md \ + RELEASE.md \ + doc \ + scripts/docker \ shared \ hardware \ wdock \ @@ -782,10 +787,9 @@ INPUT = doc \ management \ behaviors \ astrobee \ - description \ simulation \ tools \ - README.md \ + description/description \ licenses.csv @@ -844,7 +848,6 @@ EXCLUDE = # build \ # armhf \ # cmake \ -# gnc/matlab/code_generation \ # submodules # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or @@ -861,16 +864,13 @@ EXCLUDE_SYMLINKS = YES # Note that the wildcards are matched against the file with absolute path, so to # exclude all test directories for example use the pattern */test/* -EXCLUDE_PATTERNS = */external/*/* \ - */scripts/*/* \ - */tools/visualeyez/matlab/* \ - */gnc/matlab/code_generation/* \ - */hardware/pmc_actuator/tools/*.csv \ +EXCLUDE_PATTERNS = */hardware/pmc_actuator/tools/*.csv \ */diagrams/nodes_helper.puml \ */diagrams/nodes.puml \ */diagrams/colors.puml \ */diagrams/color_palettes.puml \ - */diagrams/common.puml + */diagrams/common.puml \ + */test/* # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names # (namespaces, classes, functions, etc.) that should be excluded from the @@ -887,7 +887,7 @@ EXCLUDE_SYMBOLS = # that contain example code fragments that are included (see the \include # command). -EXAMPLE_PATH = +EXAMPLE_PATH = doc/html # If the value of the EXAMPLE_PATH tag contains directories, you can use the # EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and @@ -908,7 +908,8 @@ EXAMPLE_RECURSIVE = NO # \image command). IMAGE_PATH = doc/images \ - doc/gen/png + doc/gen/png \ + localization/sparse_mapping # The INPUT_FILTER tag can be used to specify a program that doxygen should # invoke to filter for each input file. Doxygen will invoke the filter program @@ -942,11 +943,11 @@ INPUT_FILTER = # need to set EXTENSION_MAPPING for the extension otherwise the files are not # properly processed by doxygen. -FILTER_PATTERNS = "*.msg=$(SOURCE_PATH)/doc/scripts/msg2dox.py" \ - "*.srv=$(SOURCE_PATH)/doc/scripts/srv2dox.py" \ - "*.action=$(SOURCE_PATH)/doc/scripts/action2dox.py" \ - "*.csv=.$(SOURCE_PATH)/doc/scripts/csv2dox.py" \ - "*.puml=$(SOURCE_PATH)/doc/scripts/puml2dox.py" +FILTER_PATTERNS = "*.msg=doc/scripts/msg2dox.py" \ + "*.srv=doc/scripts/srv2dox.py" \ + "*.action=doc/scripts/action2dox.py" \ + "*.csv=doc/scripts/csv2dox.py" \ + "*.puml=doc/scripts/puml2dox.py" # If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using # INPUT_FILTER) will also be used to filter the input files that are used for @@ -1091,7 +1092,7 @@ ALPHABETICAL_INDEX = YES # Minimum value: 1, maximum value: 20, default value: 5. # This tag requires that the tag ALPHABETICAL_INDEX is set to YES. -COLS_IN_ALPHA_INDEX = 5 +COLS_IN_ALPHA_INDEX = 2 # In case all classes in a project start with a common prefix, all classes will # be put under the same header in the alphabetical index. The IGNORE_PREFIX tag @@ -1143,8 +1144,7 @@ HTML_FILE_EXTENSION = .html # of the possible markers and block names see the documentation. # This tag requires that the tag GENERATE_HTML is set to YES. -#HTML_HEADER = doc/style/freeflyer_header.html -HTML_HEADER = +HTML_HEADER = doc/style/freeflyer_header.html # The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each # generated HTML page. If the tag is left blank doxygen will generate a standard @@ -1154,8 +1154,7 @@ HTML_HEADER = # that doxygen normally uses. # This tag requires that the tag GENERATE_HTML is set to YES. -#HTML_FOOTER = doc/style/freeflyer_footer.html -HTML_FOOTER = +HTML_FOOTER = doc/style/freeflyer_footer.html # The HTML_STYLESHEET tag can be used to specify a user-defined cascading style # sheet that is used by each HTML page. It can be used to fine-tune the look of @@ -1180,8 +1179,7 @@ HTML_STYLESHEET = # list). For an example see the documentation. # This tag requires that the tag GENERATE_HTML is set to YES. -#HTML_EXTRA_STYLESHEET = doc/style/freeflyer.css -HTML_EXTRA_STYLESHEET = +HTML_EXTRA_STYLESHEET = doc/style/freeflyer.css # The HTML_EXTRA_FILES tag can be used to specify one or more extra images or # other source files which should be copied to the HTML output directory. Note @@ -1191,8 +1189,7 @@ HTML_EXTRA_STYLESHEET = # files will be copied as-is; there are no commands or markers available. # This tag requires that the tag GENERATE_HTML is set to YES. -#HTML_EXTRA_FILES = doc/style/freeflyer.js -HTML_EXTRA_FILES = +HTML_EXTRA_FILES = doc/images/favicon.png # The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen # will adjust the colors in the style sheet and background images according to @@ -2391,7 +2388,7 @@ DOT_PATH = # command). # This tag requires that the tag HAVE_DOT is set to YES. -DOTFILE_DIRS = behaviors mobility +DOTFILE_DIRS = behaviors mobility gnc # The MSCFILE_DIRS tag can be used to specify one or more directories that # contain msc files that are included in the documentation (see the \mscfile @@ -2411,7 +2408,7 @@ DIAFILE_DIRS = # generate a warning when it encounters a \startuml command in this case and # will not generate output for the diagram. -PLANTUML_JAR_PATH = $(SOURCE_PATH)/doc/diagrams/plantuml +PLANTUML_JAR_PATH = doc/diagrams/plantuml # When using plantuml, the specified paths are searched for files specified by # the !include statement in a plantuml block. diff --git a/astrobee/CMakeLists.txt b/astrobee/CMakeLists.txt index a254356d7a..6247b2329b 100644 --- a/astrobee/CMakeLists.txt +++ b/astrobee/CMakeLists.txt @@ -15,17 +15,57 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(astrobee) +set(ASTROBEE_VERSION 0.19.1) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +# Verify the user has the pre-commit hook +execute_process( + COMMAND cp ../scripts/git/pre-commit ../.git/hooks + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + ) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp +) + catkin_package( CATKIN_DEPENDS roscpp ) +############# +## Install ## +############# + install(DIRECTORY config/ DESTINATION config) # resources will be installed separately # install(DIRECTORY resources/ DESTINATION resources) -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) -install(PROGRAMS scripts/check_env.sh +install(PROGRAMS scripts/apk_print_version.sh + scripts/check_env.sh + scripts/print_version.sh + scripts/cpu_print_version.sh DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +# install the version.txt file when install target is run +add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/version.txt + COMMAND ${CMAKE_COMMAND} + -D OUTPUT_FILE=${CMAKE_CURRENT_BINARY_DIR}/version.txt + -D ASTROBEE_VERSION=${ASTROBEE_VERSION} + -D GIT_MODULE_FILE="${CMAKE_SOURCE_DIR}/../cmake/GetGitRevisionDescription.cmake" + -P ${CMAKE_SOURCE_DIR}/../cmake/SetVersion.cmake) +add_custom_target(version DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/version.txt) +install(CODE "execute_process(COMMAND \"${CMAKE_COMMAND}\" --build \"${CMAKE_CURRENT_BINARY_DIR}\" --target version)") +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/version.txt DESTINATION ${CMAKE_INSTALL_PREFIX} ) + +install(PROGRAMS scripts/env_wrapper.sh DESTINATION ${CMAKE_INSTALL_PREFIX} ) \ No newline at end of file diff --git a/astrobee/commands/.gitignore b/astrobee/commands/.gitignore new file mode 100644 index 0000000000..22c38c3960 --- /dev/null +++ b/astrobee/commands/.gitignore @@ -0,0 +1,5 @@ +##----------------------------------------------------------------------------- +## Commannds .gitignore. Ignores the xgds repo if the user has checked it out. +##----------------------------------------------------------------------------- + +xgds_planner2 diff --git a/astrobee/commands/freeFlyerPlanSchema.json b/astrobee/commands/freeFlyerPlanSchema.json new file mode 100644 index 0000000000..94338c1056 --- /dev/null +++ b/astrobee/commands/freeFlyerPlanSchema.json @@ -0,0 +1,1331 @@ +{ + "xpjson": "0.2", + "type": "PlanSchema", + "name": "Astrobee Free Flyer Plan Schema", + "notes": "Define FreeFlyer plans", + "id": "http://example.com/freeFlyerPlanSchema.json", + "paramSpecs": [ + + { + "type": "ParamSpec", + "id": "Settings.CameraName", + "valueType": "string", + "notes": "Which camera", + "choices": [ + ["Science", "SCI"], + ["Navigation", "NAV"], + ["Hazard", "HAZ"], + ["Dock", "DOCK"], + ["Perch", "PERCH"] + ] + }, + + { + "type": "ParamSpec", + "id": "Settings.CameraResolution", + "valueType": "string", + "notes": "The resolution of the images to produce, in pixels W x H. Specifying resolution smaller than the actual sensor resolution causes binning or downsampling. Only selected downsampling ratios are supported. Available resolutions are:
- SciCam: 1920x1080, 1280x720, 960x540, 480x270.
- NavCam, DockCam: 1280x960, 1024x768, 640x480, 320x240.
- HazCam, PerchCam: 224x171 only", + "choices": [ + ["224x171", "224x171"], + ["320x240", "320x240"], + ["480x270", "480x270"], + ["640x480", "640x480"], + ["960x540", "960x540"], + ["1024x768", "1024x768"], + ["1280x720", "1280x720"], + ["1280x960", "1280x960"], + ["1920x1080", "1920x1080"] + ] + }, + + { + "type": "ParamSpec", + "id": "Settings.CameraMode", + "valueType": "string", + "notes": "Cameras can both stream and record, potentially with different parameters (typically, you might downlink at lower frame rate and lower resolution). Use this field to specify which mode you want to change parameters for.", + "choices": [ + ["Both", "Both"], + ["Recording", "Recording"], + ["Streaming", "Streaming"] + ] + }, + + { + "type": "ParamSpec", + "id": "Settings.FlightMode", + "valueType": "string", + "notes": "Setting the flight mode updates the GN&C gains, hard limits, tolerances, etc., as specified in the world config file. See e.g. astrobee/astrobee/config/worlds/iss.config. Note that executing certain operations, such as docking, automatically switches the flight mode as needed. Values:
  • Off: Turns off propulsion.
  • Quiet: Sets propulsion impeller speed to QUIET (low speed) with gains and limits tuned to safely fly within the reduced performance.
  • Nominal: Sets propulsion impeller speed to NOMINAL (medium speed), with gains and limits tuned for typical utility flying.
  • Difficult: Sets propulsion impeller speed to AGGRESSIVE (high speed), with gains and limits tuned for maximum performance.
  • Precision: Sets propulsion impeller speed to AGGRESSIVE, with gains and limits tuned for precise motion (slow speed, tight position and attitude tolerances).
", + "choices": [ + ["off", "Off"], + ["quiet", "Quiet"], + ["nominal", "Nominal"], + ["difficult", "Difficult"], + ["precision", "Precision"] + ] + }, + + { + "type": "ParamSpec", + "id": "Power.PoweredComponent", + "valueType": "string", + "notes": "Which component. Note: 'Front' means 'Forward'.", + "choices": [ + ["Laser Pointer", "Laser Pointer"], + ["Payload Top Aft", "Payload Top Aft"], + ["Payload Bottom Aft", "Payload Bottom Aft"], + ["Payload Bottom Front", "Payload Bottom Front"], + ["PMC", "PMCs and Signal Lights"] + ] + }, + + { + "type": "ParamSpec", + "id": "Arm.ActionType", + "valueType": "string", + "notes": "Specifies whether the arm needs to pan, tilt, or both.", + "choices": [ + ["Pan", "Pan"], + ["Tilt", "Tilt"], + ["Both", "Both"] + ] + }, + + { + "type": "ParamSpec", + "id": "Settings.TelemetryType", + "valueType": "string", + "choices": [ + ["CommStatus", "CommStatus"], + ["CpuState", "CpuState"], + ["DiskState", "DiskState"], + ["EkfState", "EkfState"], + ["GncState", "GncState"], + ["PmcCmdState", "PmcCmdState"], + ["Position", "Position"], + ["SparseMappingPose", "SparseMappingPose"] + ] + }, + + { + "type": "ParamSpec", + "id": "Settings.FlashlightLocation", + "valueType" : "string", + "notes": "Which flashlight. Note: 'Back' means 'Aft' and 'Front' means 'Forward'.", + "choices": [ + ["Back", "Back"], + ["Front", "Front"] + ] + }, + + { + "type": "ParamSpec", + "id": "Settings.PlannerType", + "valueType": "string", + "notes": "Which planner to use:
  • Trapezoidal planner: Generates a straight-line trajectory from the start position to the target position, using trapezoidal velocity profiles for translation and rotation. If holonomic mode is disabled (usual case), the robot will (1) rotate to face the target position, (2) translate to the target position while facing forward, (3) rotate to the target attitude. If holonomic mode is enabled, the robot will simultaneously translate along a straight line to the target position while rotating to the target attitude.
  • QP planner: An experimental planner that generates a curved trajectory from start to target, avoiding intervening obstacles. As of 10/2020, using the QP planner is not recommended, because it has not yet been tested on the ISS.
", + "choices": [ + ["trapezoidal", "Trapezoidal"], + ["qp", "QuadraticProgram"] + ] + }, + + { + "type": "ParamSpec", + "id": "Admin.LocalizationMode", + "valueType": "string", + "notes": "Which pipeline to use:
  • MappedLandmarks: General-purpose localization mode; Astrobee uses its NavCam to recognize landmark features on the ISS interior, as well as for visual odometry.
  • ARTags: During docking, Astrobee uses its DockCam to view AR tags located on the dock and obtain more robust and accurate localization.
  • Handrail: During perching approach, Astrobee uses its PerchCam LIDAR to recognize the geometry of a handrail in front of a wall.
  • Perch: Not implemented. While perched, Astrobee could use a simplified localization approach based on remembering its initial perched pose and updating its pose during pan/tilt motions using a kinematic model of the arm.
  • Truth: During lab testing, Astrobee can optionally subscribe to pose telemetry from an external ground-truth localization system and use it for motion control.
", + "choices": [ + ["None", "None"], + ["MappedLandmarks", "MappedLandmarks"], + ["ARTags", "ARTags"], + ["Handrail", "Handrail"], + ["Perch", "Perch"], + ["Truth", "Truth"] + ] + } + ], + "commandSpecs": [ + + { + "type": "CommandSpec", + "id": "Admin.wake", + "notes": "Wakes an Astrobee from a hibernating state (only EPS powered) by powering up the main processors. This command is intended to also automatically start the flight software stack such that Astrobee is ready to receive Astrobee API commands, but that feature is not implemented.

The Astrobee in question must be docked. Because each wake command is routed through a dock berth, which Astrobee to wake is specified using the berth number.

See also Admin.wakeSafe. The difference between Admin.wake and Admin.wakeSafe is that Admin.wake is intended to automatically start the flight software stack. Since that feature is not implemented, at present, the two commands are equivalent.

Note: For context about how to wake an Astrobee on the ISS, see the procedure IRG-FFTEST207a Astrobee Quick Wakeup and Checkout.", + "javaNotes": "Do not use this command. It is a dock command, not an Astrobee command.", + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "berthNumber", + "valueType": "long", + "notes": "Which berth the Astrobee is using. 1=left, 2=right." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Admin.wakeSafe", + "notes": "Wakes Astrobee from a hibernating state (only EPS powered) into a safe state (main processors powered, available to log in for debugging, but flight software stack not started).

The Astrobee in question must be docked. Because each wake command is routed through a dock berth, which Astrobee to wake is specified using the berth number. See also Admin.wake.", + "javaNotes": "Do not use this command. It is a dock command, not an Astrobee command.", + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "berthNumber", + "valueType": "long", + "notes": "Which berth the Astrobee is using. 1=left, 2=right." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Mobility.simpleMove6DOF", + "isRapidNative": true, + "notes": "Moves the robot to the specified pose.

Using the default trapezoidal planner, the move works as follows:
- If holonomic mode is disabled (usual case), the robot will sequentially (1) rotate to face the target position, (2) translate to the target position while facing forward, (3) rotate to the target attitude.
- If holonomic mode is enabled, the robot will simultaneously translate along a straight line to the target position while rotating to the target attitude.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "referenceFrame", + "valueType": "string", + "default": "ISS", + "notes": "The reference frame for the target pose.
- ISS: The target is expressed in the fixed coordinate frame for the current environment (which might be the actual ISS, or a lab).
- body: The target is expressed relative to the robot; for example, setting xyz to (0.5, 0, 0) commands 0.5 meters forward motion.", + "choices": [ + ["ISS", "ISS"], + ["body", "body"] + ] + }, + { + "type": "ParamSpec", + "id": "xyz", + "valueType": "Point", + "notes": "Target position" + }, + { + "type": "ParamSpec", + "id": "xyzTolerance", + "valueType": "array[3].double", + "default": [0, 0, 0], + "notes": "Not used! Tolerance is dictated by the flight mode. This legacy parameter was inherited from the RAPID specification." + }, + { + "type": "ParamSpec", + "id": "rot", + "valueType": "quaternion", + "notes": "Target attitude" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setCamera", + "notes": "Sets camera parameters.

The Astrobee camera control life cycle is as follows:

  • When the Astrobee flight software stack is started, recording and streaming are initially disabled for all cameras, and the default camera parameters are as specified in the astrobee/config/cameras.config file.
  • For each camera, while recording and streaming are disabled, you may use this Settings.setCamera command to adjust its parameters.
  • For each camera, you may enable/disable recording imagery to onboard storage using Settings.setCameraRecording.
  • For each camera, you may enable/disable live imagery downlink using Settings.setCameraStreaming.
", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "cameraName", + "parent": "Settings.CameraName" + }, + { + "type": "ParamSpec", + "id": "cameraMode", + "parent": "Settings.CameraMode" + }, + { + "type": "ParamSpec", + "id": "resolution", + "parent": "Settings.CameraResolution" + }, + { + "type": "ParamSpec", + "id": "frameRate", + "valueType": "float", + "minimum": 1, + "maximum": 30, + "unit": "Hz", + "notes": "Frame rate to send. Maximum frame rate varies by camera:
- SciCam: 30 Hz
- NavCam, DockCam: 15 Hz
- HazCam, PerchCam: 5 Hz" + }, + { + "type": "ParamSpec", + "id": "bandwidth", + "valueType": "float", + "unit": "kbps", + "notes": "Only used for SciCam. Sets target bitrate used in streaming video encoder. Lower bitrate may reduce video quality.", + "minimum": 0 + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setCameraStreaming", + "notes": "Sets camera streaming live video to the ground on/off.

The SciCam streams H.264 compressed video via RTSP. All other cameras publish independent image frames via DDS.

See Settings.setCamera for context.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "cameraName", + "parent": "Settings.CameraName" + }, + { + "type": "ParamSpec", + "id": "stream", + "valueType": "boolean", + "notes": "Set to true/false to enable/disable streaming live video to the ground." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setCameraRecording", + "notes": "Sets camera video onboard recording on/off.

Note: For the SciCam, this command actually enables independent recording of H.264 compressed video. For other cameras, this command enables publishing image frames to a topic designated for onboard recording together with other telemetry, but you also need to double-check that the Settings.setDataToDisk configuration will actually log that telemetry topic.

See Settings.setCamera for context.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "cameraName", + "parent": "Settings.CameraName" + }, + { + "type": "ParamSpec", + "id": "record", + "valueType": "boolean", + "notes": "Set to true/false to enable/disable camera video onboard recording." + } + ] + }, + + { + "type": "CommandSpec", + "id": "AccessControl.grabControl", + "isRapidNative": false, + "notes": "Grabs control of an Astrobee.

The Astrobee control station implements the RAPID access control protocol, which includes the following steps:

  1. Use Admin.requestCookie to get a unique cookie.
  2. Use this Admin.grabControl command to grab control. Include the cookie received in step 1.
The RAPID access control protocol exists to prevent accidental contention between different operators. Each robot can be under the control of at most one control station at a time. The robot will reject most commands from other control stations, but a few exceptions are made for commands that might be urgently needed to safe the system: any control station can send the Mobility.stopAllMotion and Mobility.idlePropulsion commands. A single control station can grab control of multiple robots, for example, to manage multi-robot guest science. Note that guest science apps running onboard the robot do not need to grab control of the robot to send commands; there is no mechanism to prevent contention between a guest science app and a control station.", + "javaNotes": "Do not use this command. Guest science apks don't have to grab control of the robot. Only ground operators need to use this command.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "cookie", + "valueType": "string" + } + ] + }, + + { + "type": "CommandSpec", + "id": "AccessControl.requestControl", + "isRapidNative": true, + "notes": "Requests a cookie that can be used to grab control of an Astrobee via the AccessControl.grabControl command.", + "javaNotes": "Do not use this command. Guest science APKs don't have to request/grab control of the robot. Only ground operators need to use this command.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Mobility.idlePropulsion", + "isRapidNative": false, + "notes": "Idles the propulsion impeller motors.

Astrobee will lose control authority. If it is not grounded in some way (docked or perched), it will drift uncontrolled. Note that the impellers have significant inertia, and may take several seconds to spin down.

This command is usually invoked automatically (e.g., at the end of successful docking or perching). It can also be used as a fault response (e.g., responding to an overspeed fault).", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Mobility.dock", + "isRapidNative": false, + "notes": "Docks Astrobee at the specified berth. Preconditions: Astrobee must be near the dock approach point for the specified berth, the berth approach path must not be blocked, and the dock AR tags must not be occluded.

The nominal docking sequence is as follows:

  1. Switch to the \"MappedLandmarks\" localization pipeline (the default for general-purpose navigation).
  2. Coarsely correct positioning at dock approach point, ensuring dock AR tags are in view.
  3. Switch to \"ARTags\" localization pipeline (uses AR tags on dock for more accurate dock-relative localization).
  4. Fine-tune positioning at dock approach point.
  5. Complete the docking approach, which should end with Astrobee magnetically retained on the berth, with power and data pins in contact.
  6. Check magnetic retention by attempting to fly away from the dock. Lack of motion indicates success.
  7. Idle propulsion.
  8. Disable localization system.
", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "berthNumber", + "valueType": "long", + "notes": "Which berth the Astrobee is using. 1=left, 2=right." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Mobility.undock", + "isRapidNative": false, + "notes": "Undocks Astrobee.

The nominal undocking sequence is as follows:

  1. Switch to the \"MappedLandmarks\" localization pipeline (the default for general-purpose navigation).
  2. Spin up impellers.
  3. Command the dock berth retention magnets to retract, releasing Astrobee. (They will automatically re-extend after a fixed time delay.)
  4. Move to the dock approach point. (Note: Astrobee attempts to move immediately, without waiting for the berth magnets to fully retract.)
", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Plan.runPlan", + "isRapidNative": false, + "notes": "Runs the currently loaded plan. If the plan was previously paused with Plan.pausePlan, this command will resume execution where it previously left off. See plan.setPlan.", + "javaNotes": "Do not use this command! This is mainly for ground operators.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Plan.pausePlan", + "isRapidNative": false, + "notes": "Pauses the running plan. The currently executing command will be aborted (equivalent to Mobility.stopAllMotion), and execution of subsequent commands will be delayed until the plan is resumed with a Plan.runPlan command. See plan.setPlan.", + "javaNotes": "Do not use this command! This is mainly for ground operators.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Plan.skipPlanStep", + "isRapidNative": false, + "notes": "Skips the next step of the currently loaded plan (the step could be a trajectory segment or a command at a station). The plan must be paused. See plan.setPlan.", + "javaNotes": "Do not use this command! This is mainly for ground operators.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Power.powerOnItem", + "isRapidNative": false, + "notes": "Powers on a component within Astrobee. Note: to turn flashlights on or off, see Settings.setFlashlightBrightness.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "which", + "parent": "Power.PoweredComponent" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Power.powerOffItem", + "isRapidNative": false, + "notes": "Powers off a component within Astrobee. Note: to turn flashlights on or off, see Settings.setFlashlightBrightness.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "which", + "parent": "Power.PoweredComponent" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setInertia", + "isRapidNative": false, + "notes": "Sets mass and inertia parameters for Astrobee control.

The default inertial parameters for each robot are stored in an onboard configuration file, and should not need to be changed as long as the Astrobee remains in its baseline configuration (four batteries and no payloads installed). However, the inertial parameters may vary when the robot configuration changes. Examples are when a payload is installed or reconfigured (like deploying the arm). When configuration changes for a planned activity are known in advance, the standard management approach is to specify them in the inertiaConfig field of the relevant fplan. This command provides the same parameter update capability, but with more flexibility as to when it is applied (for example, you can change in the middle of a plan, during teleoperation, or when commanded by a guest science app).", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "name_", + "valueType": "string", + "default": "", + "notes": "An optional profile name for this inertia profile. The name has no functional effect, but may be reported in status telemetry and operator displays." + }, + { + "type": "ParamSpec", + "id": "mass", + "valueType": "float", + "unit": "kg", + "minimum": 0, + "notes": "Mass of the Astrobee assembly" + }, + { + "type": "ParamSpec", + "id": "centerOfMass", + "valueType": "array[3].double", + "unit": "meters", + "notes": "Center of mass of the Astrobee assembly. Specified relative to Astrobee's body frame." + }, + { + "type": "ParamSpec", + "id": "matrix", + "valueType": "array[9].float", + "unit": "kg m^2", + "notes": "The moment of inertia tensor. Must be a symmetric matrix. Specified relative to the center of mass." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Mobility.perch", + "isRapidNative": false, + "notes": "Perches Astrobee on a handrail. The robot must already be near the appropriate perch approach point for the desired perch location, with the handrail in view of the PerchCam.

The nominal perching sequence is as follows:

  1. Switch to the \"Handrail\" (handrail-relative) localization pipeline.
  2. Fine-tune positioning at perch approach point.
  3. Deploy arm.
  4. Open gripper.
  5. Move until handrail is in contact, within gripper capture box.
  6. Close gripper.
  7. Confirm grasp by attempting to fly away from the handrail. Lack of motion indicates success.
  8. Idle propulsion.
", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Mobility.unperch", + "isRapidNative": false, + "notes": "Unperches Astrobee from a handrail.

The nominal unperching sequence is as follows:

  1. Switch to \"MappedLandmarks\" localization pipeline (the default for general-purpose navigation).
  2. Spin up impellers.
  3. Open gripper, releasing the handrail.
  4. Move to perch approach point.
  5. Stow arm, as in Arm.stowArm.
", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Arm.gripperControl", + "isRapidNative": false, + "notes": "Opens or closes gripper.

Astrobee's arm has a passively under-actuated gripper with a single motor. Opening the gripper commands the motor to pull on a pair of tendons that open the fingers. Closing the gripper cuts power to the motor; the fingers are spring-loaded to close. The first time the gripper opens, it calibrates the motor encoder position by fully opening until the fingers contact a hard stop, then relaxes slightly to its nominal open position. On later open cycles, it opens directly to the nominal open position. Note that the motor consumes significant power holding the gripper open, so it should be closed when not actively in use. Holding it open for extended periods (minutes) runs the risk of triggering a motor overtemperature fault, which disables the motor.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "open", + "valueType": "boolean", + "notes": "Set to true/false to open/close gripper." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Arm.armPanAndTilt", + "oldFswAlias": "panAndTilt", + "isRapidNative": false, + "notes": "Moves Astrobee's arm.

The arm has two joints. The tilt joint is used to deploy/stow the arm and adjust the SciCam tilt angle while perched. The pitch joint is used to adjust the SciCam pan angle while perched.

The (pan, tilt) = (0, 0) reference position is defined to have the arm fully deployed and aligned with the robot's -X axis. If perched on a handrail on an ISS wall, this position should nominally make the SciCam camera axis point directly toward the opposite wall. Increasing the tilt angle tilts the SciCam up, and increasing the pan angle pans the SciCam to the right. The arm's stowed position is (pan, tilt) = (0, 180).

The arm joints will be moved sequentially:

  1. Pan: If which is \"Pan\" or \"Both\", pan to the specified pan angle.
  2. Tilt: If which is \"Tilt\" or \"Both\", tilt to the specified tilt angle.

Naturally, if you prefer to tilt first, you can issue a tilt-only move, followed by a pan-only move.

Some mistakes to avoid include:

  • While the arm is partially within its payload bay (tilt > 90), the pan angle must be 0 to avoid the arm colliding with its payload bay.
  • While the arm is close to its fully stowed state (tilt > 160), the gripper must be closed to avoid colliding with the arm's payload bay.
  • While perched, the nominal tilt range of motion is restricted to -20 .. +90.
  • While perched, the arm may not be able to realize its full nominal range of motion without causing Astrobee's body to collide with the wall. The actual range of motion depends on clutter and how far the handrail projects from the wall.
", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "pan", + "valueType": "float", + "unit": "degrees", + "minimum": -90, + "maximum": 90, + "notes": "The target pan angle. Ignored if which is \"Tilt\"." + }, + { + "type": "ParamSpec", + "id": "tilt", + "valueType": "float", + "unit": "degrees", + "minimum": -20, + "maximum": 180, + "notes": "The target tilt angle. Ignored if which is \"Pan\"." + }, + { + "type": "ParamSpec", + "id": "which", + "parent": "Arm.ActionType" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Arm.stopArm", + "isRapidNative": false, + "notes": "Stops arm motion by commanding it to hold its current joint positions. Does not affect the gripper.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Arm.deployArm", + "isRapidNative": false, + "notes": "Deploys arm. The motion sequence is:
  1. Pan to 0
  2. Tilt to 0

See also Arm.armPanAndTilt for more discussion of the arm.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params":[] + }, + + { + "type": "CommandSpec", + "id": "Arm.stowArm", + "isRapidNative": false, + "notes": "Stows arm. The motion sequence is:

  1. Close gripper
  2. Pan to 0
  3. Tilt to 180

See also Arm.armPanAndTilt for more discussion of the arm.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Settings.setTelemetryRate", + "isRapidNative": false, + "notes": "Changes the frequency at which one DDS telemetry topic is published.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "telemetryName", + "parent": "Settings.TelemetryType", + "notes": "The DDS telemetry topic to manage. Note: As of Oct 2020, the \"CommStatus\" topic is not implemented." + }, + { + "type": "ParamSpec", + "id": "rate", + "valueType": "float", + "unit": "Hz", + "notes": "The frequency for publishing the topic." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setOperatingLimits", + "isRapidNative": false, + "notes": "Changes the value of Astrobee operating limits", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "profileName", + "valueType": "string", + "default": "", + "notes": "An optional profile name for this set of operating limits. The name has no functional effect, but may be reported in status telemetry and operator displays." + }, + { + "type": "ParamSpec", + "id": "flightMode", + "parent": "Settings.FlightMode" + }, + { + "type": "ParamSpec", + "id": "targetLinearVelocity", + "valueType": "float", + "unit": "m/s", + "minimum": 0, + "notes": "The maximum linear velocity to target while translating" + }, + { + "type": "ParamSpec", + "id": "targetLinearAcceleration", + "valueType": "float", + "unit": "m/s^2", + "minimum": 0, + "notes": "The maximum linear acceleration to target while translating" + }, + { + "type": "ParamSpec", + "id": "targetAngularVelocity", + "valueType": "float", + "unit": "rad/s", + "minimum": 0, + "notes": "The maximum angular velocity to target while rotating" + }, + { + "type": "ParamSpec", + "id": "targetAngularAcceleration", + "valueType": "float", + "unit": "rad/s^2", + "minimum": 0, + "notes": "The maximum angular acceleration to target while rotating" + }, + { + "type": "ParamSpec", + "id": "collisionDistance", + "valueType": "float", + "unit": "meters", + "minimum": 0, + "notes": "Minimum distance margin to maintain away from obstacles" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setHolonomicMode", + "isRapidNative": false, + "notes": "Enables/disables holonomic mode.

Holonomic mode is sometimes called 'blind flying' because it relaxes the constraint to always point the HazCam in the direction of motion while translating in order to enable obstacle detection. When holonomic mode is enabled, the default trapezoidal planner will simultaneously translate to the target position and rotate to the target attitude.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "enableHolonomic", + "valueType": "boolean", + "notes": "Set to true/false to enable/disable holonomic mode." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setCheckObstacles", + "isRapidNative": false, + "notes": "Enables/disables obstacle checking", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "checkObstacles", + "valueType": "boolean", + "notes": "Set to true/false to enable/disable obstacle checking." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setCheckZones", + "isRapidNative": false, + "notes": "Enables/disables keepout zone checking. See Settings.setZones.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "checkZones", + "valueType": "boolean", + "notes": "Set to true/false to enable/disable keepout zone checking." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setEnableAutoReturn", + "isRapidNative": false, + "notes": "Not implemented. This commmand is intended to enable/disable auto-return to the dock, under the assumption that auto-return will eventually be configured in Astrobee's fault table as the automatic fault response for a low-battery fault. The flag is currently ignored because auto-return has not been sufficiently tested and is not used in the fault table. See Mobility.autoReturn.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "enableAutoReturn", + "valueType": "boolean", + "notes": "Set to true/false to enable/disable auto-return." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setExposure", + "isRapidNative": false, + "notes": "Set the exposure value for either the nav or dock camera.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "cameraName", + "parent": "Settings.CameraName" + }, + { + "type": "ParamSpec", + "id": "exposure", + "valueType": "float", + "notes": "The value to set the exposurec to." + } + ] + }, + + { + "type": "CommandSpec", + "id": "GuestScience.startGuestScience", + "isRapidNative": false, + "notes": "Starts a guest science app (APK) running on the Astrobee HLP. This command is part of the guest science app life cycle, implemented by the guest science manager node on the HLP. Steps of the life cycle include:

  • startGuestScience: Starts a guest science app, making it available to receive commands.
  • customGuestScience: Routes a command to a running guest science app.
  • stopGuestScience: Terminates a running guest science app.
", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "apkName", + "valueType": "string", + "notes": "Which guest science APK to start" + } + ] + }, + + { + "type": "CommandSpec", + "id": "GuestScience.stopGuestScience", + "isRapidNative": false, + "notes": "Terminates a guest science app (APK) running on the Astrobee HLP. See GuestScience.startGuestScience.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "apkName", + "valueType": "string", + "notes": "Which guest science APK to terminate" + } + ] + }, + + { + "type": "CommandSpec", + "id": "GuestScience.restartGuestScience", + "isRapidNative": false, + "notes": "This command is the equivalent to issuing the stop guest science command and then the start guest science command. Sometimes guest science APKs become unresponsive and this is a quick way to try to get it work again.

See GuestScience.startGuestScience for more on the guest science life cycle.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "apkName", + "valueType": "string", + "notes": "Which guest science APK to restart" + }, + { + "type": "ParamSpec", + "id": "wait", + "valueType": "long", + "notes": "The time in seconds the guest science manager waits in between sending the stop and start commands to the guest science apk. Different apks will need different wait times to allow a complete shutdown before starting again. A 2 second wait time is recommended for simple guest science apks. For the sci_cam_image apk which has to release the science camera hardware resources, we empirically found a conservative wait time of 10 seconds worked reliably." + } + ] + }, + + { + "type": "CommandSpec", + "id": "GuestScience.customGuestScience", + "oldFswAlias": "guestScience", + "isRapidNative": false, + "notes": "Routes a custom command to a guest science app (APK) running on the Astrobee HLP.

For the convenience of the control station operator, each guest science app can define what custom commands it supports in a configuration file read by the control station. See astrobee_ops/gds/ControlStation/PlanEditorGuestScience.config.

See GuestScience.startGuestScience for more on the guest science life cycle.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "apkName", + "valueType": "string", + "notes": "Which guest science APK to send the command to" + }, + { + "type": "ParamSpec", + "id": "command", + "valueType": "string", + "notes": "The command to send (usually formatted as a JSON dictionary, enabling an arbitrary number and types of parameters)." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setMap", + "isRapidNative": false, + "notes": "Sets the map used for localization.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "mapName", + "valueType" : "string", + "notes": "Full path to the map file to use. 'default' uses the default map loaded on startup." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Plan.setPlan", + "isRapidNative": false, + "notes": "Loads the most recently uplinked plan.

An Astrobee plan is specified as a JSON-formatted fplan file that contains a list of stations (locations to visit), segments (trajectories for flying between stations), and commands to execute at stations. Plans are generated using the control station plan editor. Sample fplan files can be found in the astrobee/astrobee/plans folder.

The Astrobee control station implements the protocol for managing plans:

  1. Uplink a new fplan file using the RAPID compressed file protocol over DDS. (File uplink is not considered a command, so it does not appear in this command dictionary).
  2. Send this Plan.setPlan command to load the uplinked fplan file. Any errors in the fplan will be reported at this time. Once the plan is loaded, it is initially in the 'paused' state.
  3. Use the Plan.runPlan / Plan.pausePlan commands to start / stop plan execution. Note that after a plan is paused using Plan.pausePlan, Plan.runPlan will resume plan execution at the beginning of the first plan step that was not fully completed. In the case that a trajectory segment is paused partway through, resuming the plan will cause the robot to fly back to the beginning of the segment and execute the whole segment again. If this behavior is not desired, you can use Plan.skipNextStep.
Other related commands include:
  • Plan.skipNextStep: Skips the next step of the currently loaded (and paused) plan.
  • Plan.wait: Same as Plan.pausePlan, but execution will resume automatically after the specified wait duration.
", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Settings.setZones", + "isRapidNative": false, + "notes": "Loads the most recently uplinked keepout zones file.

The Astrobee control station implements the protocol for managing keepout zones:

  1. Uplink a new keepout zone file using the RAPID compressed file protocol over DDS. (File uplink is not considered a command, so it does not appear in this command dictionary).
  2. Send this Settings.setZones command to load the uplinked zones file. Any errors in the file will be reported at this time.
  3. Use the Settings.setCheckZones command to enable/disable keepout zone checking.
", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Plan.wait", + "isRapidNative": false, + "notes": "Temporarily pauses the running plan. This command only affects plan execution, not individual teleoperated commands. Plan execution will resume automatically after the specified duration. See plan.setPlan.", + "availableContexts": [ + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "duration", + "valueType": "float", + "unit": "seconds", + "minimum": 0, + "notes": "Seconds to pause" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Mobility.stopAllMotion", + "isRapidNative": true, + "notes": "Stops motion of the mobility system and the arm.

Stopping the mobility system means transitioning to active station keeping (the propulsion system is activated if needed). The arm is stopped as in Arm.stopArm (the joints are commanded to maintain their current position, and the gripper is not affected).

Note that there is special behavior when this command is issued by the system monitor as a fault response: in that case, idled propulsion will not be activated. The special behavior ensures that when responding to multiple faults with different responses configured (stopAllMotion vs. idlePropulsion), the idlePropulsion response takes priority.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Data.setDataToDisk", + "isRapidNative": false, + "notes": "Sets the data-to-disk configuration, which specifies how to log ROS telemetry topics to the robot's onboard storage.

The configuration is specified in a JSON-formatted file that contains a list of topic entries. For each logged topic, one specifies:

  • frequency: Throttles the maximum rate at which to record messages on the topic. Specify -1 to disable throttling. Note: Currently, throttling is not implemented. You must specify -1.
  • downlinkOption: Which log to store messages in, which must be \"Immediate\" or \"Delayed\". The intent is that the \"Immediate\" log is for high-priority data, which can then be prioritized for downlink after the activity. Note: Currently, you must specify \"Delayed\" (\"Immediate\" downlink is currently reserved for internal use by the flight software stack).
Sample data-to-disk configuration files can be found in astrobee_ops/gds/ControlStationConfig.

The Astrobee control station implements the protocol for managing onboard telemetry recording:

  1. Uplink a new data-to-disk file using the RAPID compressed file protocol over DDS. (File uplink is not considered a command, so it does not appear in this command dictionary).
  2. Send this Data.setDataToDisk command to load the uplinked file. Any errors in the file will be reported at this time.
  3. Use the Data.startRecording / Data.stopRecording commands to start / stop onboard telemetry logging.
", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Data.startRecording", + "isRapidNative": false, + "notes": "Starts logging ROS telemetry to onboard storage. See Data.setDataToDisk.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "description", + "valueType": "string", + "default": "", + "notes": "An optional short description string to include in the filename of the stored telemetry file (ROS bag). Note that the filename always includes a timestamp, which ensures uniqueness." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Data.stopRecording", + "isRapidNative": false, + "notes": "Stops logging ROS telemetry to onboard storage, as initiated by Data.startRecording. See also Data.setDataToDisk.", + "availableContexts": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Mobility.autoReturn", + "isRapidNative": false, + "notes": "For expert use only. Returns Astrobee to its dock.

As originally envisioned, this operation could be commanded from anywhere on the ISS, and Astrobee would plan a collision-free path back to the dock. Auto-return could also be invoked as an automatic response to a low-battery fault. To avoid contention for dock berths when multiple Astrobees are in use, each Astrobee should return to the same berth it originally departed from.

However, using this command is not recommended at this time, because (1) it has not been sufficiently tested (including testing of the QP trajectory planner), (2) at present, the proper berth selection logic is not implemented; instead, Astrobee will always return to berth 1 (= left).", + "javaNotes": "Only issue this command if you are sure berth 1 is unoccupied.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "berthNumber", + "valueType": "long", + "notes": "Which berth Astrobee is using. 1=left, 2=right." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Admin.noOp", + "isRapidNative": true, + "notes": "Does nothing. Returns a 'success' response if you have control of the robot. This command may be useful for debugging communication or access control issues.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Admin.fault", + "isRapidNative": false, + "notes": "For internal use only. Places the robot in a fault state. This command can only be sent by Astrobee's onboard executive or system monitor nodes. The executive will reject the command if it comes from any other source.", + "availableContexts": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Settings.setFlashlightBrightness", + "isRapidNative": false, + "notes": "Sets flashlight brightness.", + "availableContexts" : [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "which", + "parent": "Settings.FlashlightLocation" + }, + { + "type": "ParamSpec", + "id": "brightness", + "valueType": "float", + "minimum": 0, + "maximum": 1, + "notes": "Brightness value between 0 (off) and 1 (full brightness). Note that full brightness of an Astrobee flashlight is similar to that of an ordinary pocket flashlight, and may be uncomfortably bright if pointed toward crew eyes. When working with crew, it is advisable to use lower brightness values and/or take steps to avoid pointing the flashlight toward crew." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Admin.reacquirePosition", + "isRapidNative": false, + "notes": "Switches Astrobee's localization to use the general-purpose pipeline.

This command is equivalent to calling Admin.switchLocalization with the \"MappedLandmarks\" pipeline and then calling Admin.resetEKF. It could be useful as a way to manually recover localization if Astrobee is lost because it is using the wrong localization pipeline (e.g., if docking or perching approach got interrupted, and targets are no longer in view).", + "availableContexts" : [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Admin.loadNodelet", + "isRapidNative": false, + "notes": "This command is intended to load a ROS nodelet in the Astrobee flight software. Please note, the underlying system monitor service can't load nodelets that run on the HLP.", + "availableContext": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id" : "nodeletName", + "valueType": "string", + "notes": "Name of nodelet" + }, + { + "type": "ParamSpec", + "id": "managerName", + "valueType": "string", + "default": "", + "notes": "Which nodelet manager should load the nodelet. If left blank, the system monitor will default to using the managerName specified in the last heartbeat it received from the requested nodelet. (That should normally work fine, assuming the nodelet sent out at least one valid heartbeat.)" + }, + { + "type": "ParamSpec", + "id": "type", + "valueType": "string", + "default": "", + "notes": "Type of nodelet (namespace/classname). If left blank, the system monitor will default to the type specified in its config file, which should normally work fine." + }, + { + "type": "ParamSpec", + "id": "bondId", + "valueType": "string", + "default": "", + "notes": "With the current implementation, you should leave this field blank. Specifying a non-empty string (unique id) will activate bond notifications.

Background: ROS nodelets provide a 'bond' mechanism that enables the process requesting a nodelet load (in our case the system monitor) to receive automatic notification if that nodelet crashes. However, our system monitor instead relies on a custom heartbeat message to detect liveness, and it is not currently configured to do anything useful with bond notifications, even if they are activated." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Admin.unloadNodelet", + "isRapidNative": false, + "notes": "This command is intended to unload a ROS nodelet in Astrobee's flight software. Its primary use is in the system monitor's fault response table, where it is the usual response if initialization of a nodelet fails. Please note, the internal service can't unload nodes running on the HLP.", + "availableContext": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "nodeletName", + "valueType": "string", + "notes": "Name of nodelet" + }, + { + "type": "ParamSpec", + "id": "managerName", + "valueType": "string", + "default": "", + "notes": "Which nodelet manager should unload the nodelet. If left blank, the system monitor will default to using the managerName specified in the last heartbeat it received from the nodelet. (That should normally work fine, assuming the nodelet sent out at least one valid heartbeat.)" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Mobility.prepare", + "isRapidNative": false, + "notes": "Manually spins up the propulsion modules, so that a future motion command can execute without a spin-up delay. This command is rarely used. Normally, you would just command the desired motion and the spin-up would be triggered automatically if needed. This command is intended to help with synchronizing motion of multiple Astrobees, but that scenario hasn't really been tested.", + "availableContext": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Settings.setEnableImmediate", + "isRapidNative": false, + "notes": "For expert use only. Changes the semantics of how Astrobee executes fplan trajectory segments.

A segment specifies the motion trajectory between stations in the plan. Within the segment, desired pose and velocity are smoothly interpolated functions of time over a time interval [t0, t1]. Plans created by the Astrobee control station plan editor always specify each segment's time values relative to the start of that segment's execution (i.e. t0 = 0). These plans must be executed in immediate mode, so called because when execution reaches a new segment, the executive and choreographer immediately 'start the clock' on executing the timed trajectory. In principle, if you want to synchronize motion of multiple robots, it could be useful to have the interval [t0, t1] specified using absolute timestamps. That is the behavior when immediate mode is disabled. The timestamp t0 is interpreted as absolute time using ROS conventions (usually UNIX epoch when running on real hardware, but could be any arbitrary time scale when running in simulation), and the start of motion on the segment would be delayed until the current time t = t0.

At this time, we cannot recommend disabling immediate mode to achieve synchronization, due to the following concerns: (1) execution with absolute timestamps has never really been tested, and may be buggy, (2) the control station plan editor doesn't provide any way to generate segments with absolute timestamps, (3) since it is seldom possible to predict exactly when ISS conditions will be right to begin a multi-robot activity, it would be awkward in practice to have the exact absolute timing of segments hard-coded into the plans.

As an alternative way to synchronize motion, you can execute with immediate mode enabled as usual, but take special care to minimize any skew in start time. Upload the plans in advance, use the Mobility.prepare command to get the robots ready to move, and then run the plans simultaneously. Astrobee to Astrobee communication is in the works, and may eventually enable synchronizing Astrobees in a better way.", + "availableContext": [ + "teleop" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "enableImmediate", + "valueType": "boolean", + "notes": "Set to true/false to enable/disable immediate mode motion control." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setPlanner", + "isRapidNative": false, + "notes": "Switches which trajectory planner is used when Astrobee needs to generate a trajectory to reach a target pose. Astrobee uses its trajectory planner when it receives a teleoperation motion command. However, when an fplan is generated by the Astrobee control station plan editor, it normally contains pre-computed trajectories for its motion segments; when Astrobee executes these fplans, the onboard trajectory planner is not used.", + "availableContext": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "planner", + "parent": "Settings.PlannerType" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Settings.setEnableReplan", + "isRapidNative": false, + "notes": "Allows Astrobee to re-plan if it detects an obstacle too close to its forward trajectory. Enabling replanning only makes sense when you are using a planner that is able to plan around obstacles. (See Setting.setPlanner. As of 5/2021, only the QP planner can plan around obstacles).", + "availableContext": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "enableReplan", + "valueType": "boolean", + "notes": "If true, when Astrobee detects an obstacle too close to its forward trajectory, after the robot comes to a stop, the choreographer will automatically request a new trajectory from the current configured planner. If false, the robot will stop and wait for operator assistance." + } + ] + }, + + { + "type": "CommandSpec", + "id": "Admin.initializeBias", + "isRapidNative": false, + "notes": "Calibrates the bias parameters of Astrobee's IMU.

Astrobee's IMU biases slowly drift over time. While Astrobee is actively running its localization system and maintaining a position fix, its EKF automatically corrects for bias drift under the assumption that the local environment is inertially fixed. If the localization system is turned off or loses its fix for an extended period, this command should be used to explicitly calibrate the biases prior to restarting motion. It works by integrating the robot's linear acceleration and angular velocity for about five seconds, and taking the resulting average values as the new zero. During the calibration period, the robot must remain stationary (e.g. docked or perched on a handrail, not held by an astronaut).", + "availableContext": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Admin.resetEkf", + "isRapidNative": false, + "notes": "Resets the EKF component of Astrobee's localization.

This command discards the EKF state and forces the localization system to acquire a new fix. It should not be needed unless the localization system has failure modes where it gets persistently stuck with invalid state information. (Unfortunately, as of 10/2020, that is a frequent occurrence.)", + "availableContext": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Admin.switchLocalization", + "isRapidNative": false, + "notes": "This rarely used command manually switches between localization pipelines. Normally, Astrobee switches its localization pipeline automatically based on what activity it is conducting (normal free flight, docking, perching, etc.).

Note that, although Astrobee could logically integrate information from multiple pipelines at the same time (e.g., NavCam sparse map features as well as DockCam AR targets), at present, it only uses data from one of these pipelines at a time. IMU information is always integrated, regardless of which pipeline is used. While switching pipelines from A to B, updates from A stop, then there may be a dropout period of a few seconds before B acquires a fix. During the dropout period, localization relies solely on the IMU.", + "availableContext": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [ + { + "type": "ParamSpec", + "id": "mode", + "parent": "Admin.LocalizationMode" + } + ] + }, + + { + "type": "CommandSpec", + "id": "Admin.unterminate", + "isRapidNative": false, + "notes": "Clears the terminate flag on the robot. This functionality is tied to Astrobee's physical terminate button and terminate indicator LED. The terminate life cycle is as follows:

  • Nominal ops: When Astrobee is active, the amber terminate LED is turned on and the robot functions normally.
  • Terminate: A crew member may press the terminate button as a way to quickly power down the propulsion modules and payloads, while leaving the main processors running and able to communicate with the ground operator. This is a gentler way to intervene than turning off the physical power switch. While terminated, the terminate LED is also turned off.
  • Unterminate: The ground operator can send this command to clear the terminate flag and turn the terminate LED back on. This does not automatically restore power to the hardware.
  • Configure power: Send Power.powerOnItem commands as needed to restore power to individual hardware components.
", + "availableContext": [ + "teleop" + ], + "parent": "Command", + "params": [] + }, + + { + "type": "CommandSpec", + "id": "Settings.enableAstrobeeIntercomms", + "isRapidNative": false, + "notes": "Starts the Astrobee intercommunication software. This enables the robot to send and receive a subset of data to and from other Astrobees in the network. Please note, this command must be executed on each robot needed in the communication.", + "availableContext": [ + "teleop", + "planner" + ], + "parent": "Command", + "params": [] + } + ] +} diff --git a/astrobee/config/behaviors/dock.config b/astrobee/config/behaviors/dock.config index b8060b054b..b7f2a5e0ef 100644 --- a/astrobee/config/behaviors/dock.config +++ b/astrobee/config/behaviors/dock.config @@ -66,6 +66,10 @@ parameters = { id = "detection_tolerance", reconfigurable = false, type = "double", default = 0.30, min = 0.01, max = 0.2, unit = "meters", description = "Distance used to detect closest berth to current position" + },{ + id = "detection_timeout", reconfigurable = false, type = "double", + default = 30, min = 1, max = 100, unit = "seconds", + description = "Time we allow the berth to be detected in" },{ id = "initial_tolerance", reconfigurable = false, type = "double", default = 1.0, min = 0.01, max = 0.2, unit = "meters", @@ -91,7 +95,7 @@ parameters = { id = "enable_validation", reconfigurable = false, type = "boolean", - default = true, + default = false, unit = "boolean", description = "Should we validate segments before executing them?" },{ @@ -130,7 +134,7 @@ parameters = { id = "enable_faceforward", reconfigurable = false, type = "boolean", - default = true, + default = false, unit = "boolean", description = "Should planning be face-forward only?" },{ @@ -170,4 +174,4 @@ parameters = { unit = "m/s", description = "Soft planning limit on net angular acceleration" } -} \ No newline at end of file +} diff --git a/astrobee/config/cameras.config b/astrobee/config/cameras.config index 3e360272f8..5ace8cd909 100644 --- a/astrobee/config/cameras.config +++ b/astrobee/config/cameras.config @@ -17,6 +17,13 @@ require "context" +-- bayer_enable: Set to true to enable publishing raw Bayer images (can +-- be converted to RGB). May incur significant I/O overhead. + +-- bayer_throttle_ratio: Set to n to publish 1 raw Bayer image for +-- every n images grabbed. With n = 1, every image is +-- published. Larger n reduces I/O overhead. + nav_cam = { width = 1280, height = 960, @@ -27,31 +34,35 @@ nav_cam = { device = "/dev/nav_cam", gain = robot_camera_calibrations.nav_cam.gain, exposure = robot_camera_calibrations.nav_cam.exposure, + auto_exposure = false, calibration_gain = 105, - calibration_exposure = 30 + calibration_exposure = 30, + bayer_enable = false, + bayer_throttle_ratio = 3 }; dock_cam = { - width=1280, - height=960, - undistorted_width=3000, - undistorted_height=1800, - distortion_coeff=robot_camera_calibrations.dock_cam.distortion_coeff, - intrinsic_matrix=robot_camera_calibrations.dock_cam.intrinsic_matrix, - device="/dev/dock_cam", - gain= robot_camera_calibrations.dock_cam.gain, - exposure= robot_camera_calibrations.dock_cam.exposure, + width = 1280, + height = 960, + undistorted_width = 3000, + undistorted_height = 1800, + distortion_coeff = robot_camera_calibrations.dock_cam.distortion_coeff, + intrinsic_matrix = robot_camera_calibrations.dock_cam.intrinsic_matrix, + device = "/dev/dock_cam", + gain = robot_camera_calibrations.dock_cam.gain, + exposure = robot_camera_calibrations.dock_cam.exposure, + auto_exposure = false, calibration_gain = 105, - calibration_exposure = 30 + calibration_exposure = 30, + bayer_enable = false, + bayer_throttle_ratio = 3 }; --- The haz and perch cam params below are ignored for the time being. --- The distortion_coeff, etc, must be set in each bot's config file. haz_cam = { width = 224, height = 171, - undistorted_width = 600, - undistorted_height = 500, + undistorted_width = 250, + undistorted_height = 200, distortion_coeff = robot_camera_calibrations.haz_cam.distortion_coeff, intrinsic_matrix = robot_camera_calibrations.haz_cam.intrinsic_matrix, device = "/dev/haz_cam", @@ -62,15 +73,15 @@ haz_cam = { }; perch_cam = { - width=224, - height=171, - undistorted_width=600, - undistorted_height=500, - distortion_coeff=robot_camera_calibrations.perch_cam.distortion_coeff, - intrinsic_matrix=robot_camera_calibrations.perch_cam.intrinsic_matrix, - device="/dev/perch_cam", - gain= robot_camera_calibrations.perch_cam.gain, - exposure= robot_camera_calibrations.perch_cam.exposure, + width = 224, + height = 171, + undistorted_width = 250, + undistorted_height = 200, + distortion_coeff = robot_camera_calibrations.perch_cam.distortion_coeff, + intrinsic_matrix = robot_camera_calibrations.perch_cam.intrinsic_matrix, + device = "/dev/perch_cam", + gain = robot_camera_calibrations.perch_cam.gain, + exposure = robot_camera_calibrations.perch_cam.exposure, calibration_gain = 105, calibration_exposure = 30 }; @@ -89,6 +100,16 @@ sci_cam = { calibration_exposure = 30 }; +nav_cam_to_haz_cam_timestamp_offset = robot_camera_calibrations.nav_cam_to_haz_cam_timestamp_offset; +nav_cam_to_sci_cam_timestamp_offset = robot_camera_calibrations.nav_cam_to_sci_cam_timestamp_offset; + +-- Auto-exposure parameters +desired_msv = 2.5; -- should be 2.5 as histogram size is 5 +k_p = 25.0; -- proportional gain +k_i = 8.0; -- integral gain +max_i = 100.0; -- maximum i +tolerance = 0.2; -- msv tolerance to minimize exposure reconfigure + ----------------------------------------------------- -- PicoFlexx cameras support the following modes ----------------------------------------------------- @@ -109,21 +130,24 @@ picoflexx = { device = robot_perch_cam_device, -- camera uuid ("" : automatic) exposure = 0, -- exposure time (0: automatic) mode = "MODE_9_5FPS_2000", -- use case - required = true -- is camera required + required = true, -- is camera required + amplitude_factor = 100.0 },{ name = "haz_cam", -- frame topic = "haz", -- frame device = robot_haz_cam_device, -- camera uuid ("" : automatic) exposure = 0, -- exposure time (0: automatic) mode = "MODE_9_5FPS_2000", -- use case - required = true -- is camera required + required = true, -- is camera required + amplitude_factor = 100.0 },{ name = "test_cam", -- special name topic = "test", -- frame device = "0005-4805-0050-1520", -- camera uuid ("" : automatic) exposure = 0, -- exposure time (0: automatic) mode = "MODE_5_45FPS_500", -- use case - required = false -- is camera required + required = false, -- is camera required + amplitude_factor = 100.0 } } } diff --git a/astrobee/config/commands.config b/astrobee/config/commands.config index 06b8302128..3e570d16fd 100644 --- a/astrobee/config/commands.config +++ b/astrobee/config/commands.config @@ -41,11 +41,11 @@ commandConfig = { type="RAPID_STRING" }, { - key="type", + key="managerName", type="RAPID_STRING" }, { - key="managerName", + key="type", type="RAPID_STRING" }, { @@ -66,10 +66,6 @@ commandConfig = { name="resetEkf", parameters={} }, - { - name="shutdown", - parameters={} - }, { name="switchLocalization", parameters={ @@ -113,10 +109,6 @@ commandConfig = { type="RAPID_INT" } } - }, - { - name="wipeHlp", - parameters={} } }, name="AdminType" @@ -140,6 +132,10 @@ commandConfig = { } } }, + { + name="deployArm", + parameters={} + }, { name="gripperControl", parameters={ @@ -162,24 +158,6 @@ commandConfig = { }, { commands={ - { - name="clearData", - parameters={ - { - key="dataMethod", - type="RAPID_STRING" - } - } - }, - { - name="downloadData", - parameters={ - { - key="dataMethod", - type="RAPID_STRING" - } - } - }, { name="setDataToDisk", parameters={} @@ -193,15 +171,6 @@ commandConfig = { } } }, - { - name="stopDownload", - parameters={ - { - key="dataMethod", - type="RAPID_STRING" - } - } - }, { name="stopRecording", parameters={} @@ -224,6 +193,19 @@ commandConfig = { } } }, + { + name="restartGuestScience", + parameters={ + { + key="apkName", + type="RAPID_STRING" + }, + { + key="wait", + type="RAPID_INT" + } + } + }, { name="startGuestScience", parameters={ @@ -249,7 +231,12 @@ commandConfig = { commands={ { name="autoReturn", - parameters={} + parameters={ + { + key="berthNumber", + type="RAPID_INT" + } + } }, { name="dock", @@ -364,17 +351,8 @@ commandConfig = { { commands={ { - name="genericCommand", - parameters={ - { - key="commandName", - type="RAPID_STRING" - }, - { - key="param", - type="RAPID_STRING" - } - } + name="enableAstrobeeIntercomms", + parameters={} }, { name="setCamera", @@ -463,6 +441,28 @@ commandConfig = { } } }, + { + name="setEnableReplan", + parameters={ + { + key="enableReplan", + type="RAPID_BOOL" + } + } + }, + { + name="setExposure", + parameters={ + { + key="cameraName", + type="RAPID_STRING" + }, + { + key="exposure", + type="RAPID_FLOAT" + } + } + }, { name="setFlashlightBrightness", parameters={ @@ -506,6 +506,15 @@ commandConfig = { } } }, + { + name="setMap", + parameters={ + { + key="mapName", + type="RAPID_STRING" + } + } + }, { name="setOperatingLimits", parameters={ @@ -561,15 +570,6 @@ commandConfig = { } } }, - { - name="setTimeSync", - parameters={ - { - key="setTimeSync", - type="RAPID_BOOL" - } - } - }, { name="setZones", parameters={} diff --git a/astrobee/config/communications/astrobee_astrobee_bridge.config b/astrobee/config/communications/astrobee_astrobee_bridge.config new file mode 100644 index 0000000000..d97d6eb848 --- /dev/null +++ b/astrobee/config/communications/astrobee_astrobee_bridge.config @@ -0,0 +1,99 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +-- SHARE TOPICS lists all local topics to be published to DDS. +-- RAPID SUBSCRIBERS lists all rapid subscribers that will be created to listen +-- external data. Must be a subscriber for every agent enabled or a failure +-- will occur. +-- AGENTS section allows to enable/disable and add more robots. It also allows to listen +-- more topics once they are implemented. +-- +-- DO NOT change structure or topic names. + +require "context" + +-- Enable DDS communications on start -- +-- If false, a trigger call will be needed to start communications -- +started = false; + +-- Shared individual topics from local instance -- +share_topics = { + { + name = "TOPIC_GNC_EKF", + enable = true, + rate = 2 + } +} + +-- Shared topic groups from local instance (for compatibility with DDS bridge) -- +share_topic_groups = { + -- Share TOPIC_GUEST_SCIENCE_DATA -- + -- TOPIC_GUEST_SCIENCE_MANAGER_STATE -- + -- TOPIC_GUEST_SCIENCE_MANAGER_CONFIG -- + { + name = "TOPIC_GROUP_GS", + enable = true, + rate = -1 + } +} + +-- Rapid subscribers -- +rapid_sub_names = {"Bumble", "Honey", "Queen", "Bsharp", "Wannabee"} + +-- External data management +agents = { + { + name = "Bumble", + enable = true, + topics = { + { name = "TOPIC_GNC_EKF", enable = true }, + { name = "TOPIC_GUEST_SCIENCE_DATA", enable = true } + } + }, + { + name = "Honey", + enable = true, + topics = { + { name = "TOPIC_GNC_EKF", enable = true }, + { name = "TOPIC_GUEST_SCIENCE_DATA", enable = true } + } + }, + { + name = "Queen", + enable = true, + topics = { + { name = "TOPIC_GNC_EKF", enable = true }, + { name = "TOPIC_GUEST_SCIENCE_DATA", enable = true } + } + }, + { + name = "Bsharp", + enable = true, + topics = { + { name = "TOPIC_GNC_EKF", enable = true }, + { name = "TOPIC_GUEST_SCIENCE_DATA", enable = true } + } + }, + { + name = "Wannabee", + enable = true, + topics = { + { name = "TOPIC_GNC_EKF", enable = true }, + { name = "TOPIC_GUEST_SCIENCE_DATA", enable = true } + } + } +} diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config new file mode 100644 index 0000000000..6912211951 --- /dev/null +++ b/astrobee/config/communications/comms_bridge.config @@ -0,0 +1,110 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +require "context" + +-- Enable DDS communications on start -- +-- If false, a trigger call will be needed to start communications -- +initialize_dds_on_start = false + +-- Request an advertisement info message if a content message was received +-- before the advertisement info message in case the advertisement message +-- never comes. +-- DDS has been setup such that this shouldn't be needed and it can cause more +-- information to be sent then needed. By default, it will be set to false +-- but if communications on the ISS are not working as expected, it can be +-- enabled. +enable_advertisement_info_request = false + +-- The verbosity is used to control the output of the underlying ros +-- publisher/subscriber. Level 2 is the most verbose and will output all of the +-- output including debug statements. Level 1 will output all output except +-- debug statements and level 0 will only output warnings and errors. Since +-- all output statements go directly to the ros log, it is safest to keep it +-- set to 2. +verbose = 2 + +-- ad2pub_delay is a time amount in seconds. The bridge publisher must receive +-- the adverttisement info message for a topic before it can publish a content +-- message for that topic. If the bridge publisher receives the content message +-- before the advertisement info, it will save the content message if the +-- ad2pub_delay is greater than 0. If the bridge publisher receives the +-- advertisement info message within the ad2pub_delay time, it will process +-- and publish the content message. If the comms bridge seems to be dropping +-- the first few messages, try increasing this value. +ad2pub_delay = 3.0 + +links = { + -- Logically, there could be up to three two-way links between the three robots. In practice, we + -- will probably only have one link. + { + -- A single link entry has required fields "from" and "to" that specify the robot roles involved + -- in the link. + -- This link will only work on the ISS since it uses the names of the + -- robots on the ISS. Please add more links if Queen goes back to the ISS. + from = "Bumble", -- manager + to = "Honey", -- actor + + -- The link entry has three optional fields: relay_forward (messages to be relayed only in the + -- `from` to `to` direction), relay_backward (to be relayed only in the `to` to `from` direction), + -- and relay_both (to be relayed in both directions). Providing all three fields gives the user + -- full directional control while minimizing repetition and copy/paste errors. + + -- Each topic entry can contain an input topic, output topic, and a rate. + -- In topic: Required. This is the topic being published on the robot + -- sending the data. + -- Out topic: Optional. This is the name of the topic a user wants the data + -- published on in the receiving robot. If the out topic is not specified, + -- the comms bridge will set it to be the name of the robot sending the data + -- combined with the in topic name. For instance, if the from robot was + -- Bumble and the to robot was Honey and one of the in topics in the relay + -- forward list was "mgt/ack", then it would be published on Honey on topic + -- "bumble/mgt/ack". + -- Rate: Optional. The rate in seconds at which a user wants the topic + -- published to the receiving robot. Please omit the rate, if the topic + -- should be published at the same rate it is published on the robot. + + -- Please note that only one unique in topic can exist in the relay forward + -- and relay both lists and the relay backward and relay both lists. It is + -- fine to have the same in topic in the relay forward and relay backward + -- lists. + + relay_forward = { + }, + relay_backward = { + }, + relay_both = { + {in_topic = "gnc/ekf"}, + {in_topic = "gs/data"}, + }, + }, + { + -- This link will only work in the granite lab since it uses the names of + -- the robots in the granite lab. + from = "Bsharp", -- manager + to = "Wannabee", -- actor + + relay_forward = { + }, + relay_backward = { + }, + relay_both = { + {in_topic = "gnc/ekf"}, + {in_topic = "gs/data"}, + } + } +} diff --git a/astrobee/config/communications/dds/.gitignore b/astrobee/config/communications/dds/.gitignore deleted file mode 100644 index fde27000a4..0000000000 --- a/astrobee/config/communications/dds/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/NDDS_DISCOVERY_PEERS -/RAPID_QOS_PROFILES.xml diff --git a/astrobee/config/communications/dds/NDDS_DISCOVERY_PEERS b/astrobee/config/communications/dds/NDDS_DISCOVERY_PEERS new file mode 100644 index 0000000000..35ef791374 --- /dev/null +++ b/astrobee/config/communications/dds/NDDS_DISCOVERY_PEERS @@ -0,0 +1,17 @@ +;; NDDS_DISCOVERY_PEERS - Default Discovery Configuration File +;;Multicast builtin.udpv4://239.255.0.1 ; default discovery multicast addr +; EVERY LINE IN THIS FILE MUST END IN A COMMENT. Even blank lines. +; +;; Shared Memory +;shmem:// ; All 'shmem' transport plugin(s) +;builtin.shmem:// ; The builtin builtin 'shmem' transport plugin + ; at network address FCC0::0 +; +;; Unicast +; +;; Localhost +127.0.0.1 ; A comma can be used a separator +; +;; Granite Lab - P4 +; +;; Custom entries below this line (Vagrant, etc) diff --git a/astrobee/config/communications/dds/RAPID_QOS_PROFILES.xml b/astrobee/config/communications/dds/RAPID_QOS_PROFILES.xml new file mode 100644 index 0000000000..e4123aee6c --- /dev/null +++ b/astrobee/config/communications/dds/RAPID_QOS_PROFILES.xml @@ -0,0 +1,989 @@ + + + + + + + + + + + + + + + 50, 48, 49, 49, 48, 54,0x32,0x39,0x2e,0x30,0x39,0x34,0x33,0x30,0x30,0x2e,0x48,0x55,0x2d,0x41,0x52,0x43 + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + MASK_DEFAULT + + + UDPv4 + + + + + + true + + + + 4096 + + + 0 + 0 + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController.scheduling_policy + DDS_EDF_FLOW_CONTROLLER_SCHED_POLICY + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.max_tokens + 12 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_added_per_period + 4 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_leaked_per_period + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.bytes_per_token + 1250 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.sec + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.nanosec + 5000000 + + + + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 1250 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65535 + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 65535 + + + + + dds.transport.shmem.builtin.received_message_count_max + 512 + + + dds.transport.shmem.builtin.receive_buffer_size + 1048576 + + + + + dds.transport.UDPv4.builtin.parent.deny_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + dds.transport.UDPv4.builtin.parent.deny_multicast_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + + + + + + + + + + 0 + + + + + + ALIVE_THEN_DISPOSED_INSTANCE_REPLACEMENT + + + ASYNCHRONOUS_PUBLISH_MODE_QOS + + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + KEEP_ALL_HISTORY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_ALL_HISTORY_QOS + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + KEEP_LAST_HISTORY_QOS + 1 + + + 512 + 4 + 128 + 128 + 512 + 380 + + + + 32 + + 0 + 500000000 + + 8 + true + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + KEEP_LAST_HISTORY_QOS + 100 + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 1000 + + + + + KEEP_LAST_HISTORY_QOS + 200 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + 100 + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + + + + + + + 50 + + + + + + + + + + + + + + + + 1000 + + + + + + + + 1000 + + + + + + + + + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController + + + KEEP_LAST_HISTORY_QOS + 1 + + + 32 + 32 + 1 + 1 + 32 + + + + + 4096 + + + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + + + + + + + + + + + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + + + + 30000 + 64 + 2048 + + + + UDPv4 | SHMEM + + + Monitoring UI Application + + + 65530 + + + + + 100 + 0 + + + 10 + 0 + + LIVELINESS_BASED_REMOTE_PARTICIPANT_PURGE + + 10 + 0 + + 1 + + 2 + 0 + + + 2 + 0 + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + + + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 1048576 + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 65530 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65530 + + + + + dds.transport.shmem.builtin.parent.message_size_max + 65530 + + + dds.transport.shmem.builtin.receive_buffer_size + 65530 + + + dds.transport.shmem.builtin.received_message_count_max + 32 + + + + + + + + + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 2 + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + diff --git a/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS new file mode 100644 index 0000000000..dc71e38323 --- /dev/null +++ b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS @@ -0,0 +1,6 @@ +;; NDDS_DISCOVERY_PEERS - Discovery Configuration File +;; This file is used to configure the dds portion of the comms bridge +; EVERY LINE IN THIS FILE MUST END IN A COMMENT. Even blank lines. +; +;; Add the IP addresses of the robots to communicate with +127.0.0.1 ; Local host is added by default diff --git a/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml b/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml new file mode 100644 index 0000000000..27bb1ff25f --- /dev/null +++ b/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml @@ -0,0 +1,994 @@ + + + + + + + + + + + + + + + 50, 48, 49, 49, 48, 54,0x32,0x39,0x2e,0x30,0x39,0x34,0x33,0x30,0x30,0x2e,0x48,0x55,0x2d,0x41,0x52,0x43 + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + MASK_DEFAULT + + + UDPv4 + + + + + + true + + + + 4096 + + + 0 + 0 + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController.scheduling_policy + DDS_EDF_FLOW_CONTROLLER_SCHED_POLICY + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.max_tokens + 12 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_added_per_period + 4 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_leaked_per_period + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.bytes_per_token + 1250 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.sec + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.nanosec + 5000000 + + + + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 1250 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65535 + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 65535 + + + + + dds.transport.shmem.builtin.received_message_count_max + 512 + + + dds.transport.shmem.builtin.receive_buffer_size + 1048576 + + + + + dds.transport.UDPv4.builtin.parent.deny_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + dds.transport.UDPv4.builtin.parent.deny_multicast_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + + + + + + + + + + 0 + + + + + + ALIVE_THEN_DISPOSED_INSTANCE_REPLACEMENT + + + ASYNCHRONOUS_PUBLISH_MODE_QOS + + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + KEEP_ALL_HISTORY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_ALL_HISTORY_QOS + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 20 + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + KEEP_LAST_HISTORY_QOS + 1 + + + 512 + 4 + 128 + 128 + 512 + 380 + + + + 32 + + 0 + 500000000 + + 8 + true + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + KEEP_LAST_HISTORY_QOS + 100 + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 1000 + + + + + KEEP_LAST_HISTORY_QOS + 200 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + 100 + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + + + + + + + 50 + + + + + + + + + + + + + + + + 1000 + + + + + + + + 1000 + + + + + + + + + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController + + + KEEP_LAST_HISTORY_QOS + 1 + + + 32 + 32 + 1 + 1 + 32 + + + + + 4096 + + + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + + + + + + + + + + + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + + + + 30000 + 64 + 2048 + + + + UDPv4 | SHMEM + + + Monitoring UI Application + + + 65530 + + + + + 100 + 0 + + + 10 + 0 + + LIVELINESS_BASED_REMOTE_PARTICIPANT_PURGE + + 10 + 0 + + 1 + + 2 + 0 + + + 2 + 0 + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + + + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 1048576 + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 65530 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65530 + + + + + dds.transport.shmem.builtin.parent.message_size_max + 65530 + + + dds.transport.shmem.builtin.receive_buffer_size + 65530 + + + dds.transport.shmem.builtin.received_message_count_max + 32 + + + + + + + + + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 2 + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + diff --git a/astrobee/config/communications/dds_intercomms/NDDS_DISCOVERY_PEERS b/astrobee/config/communications/dds_intercomms/NDDS_DISCOVERY_PEERS new file mode 100644 index 0000000000..35ef791374 --- /dev/null +++ b/astrobee/config/communications/dds_intercomms/NDDS_DISCOVERY_PEERS @@ -0,0 +1,17 @@ +;; NDDS_DISCOVERY_PEERS - Default Discovery Configuration File +;;Multicast builtin.udpv4://239.255.0.1 ; default discovery multicast addr +; EVERY LINE IN THIS FILE MUST END IN A COMMENT. Even blank lines. +; +;; Shared Memory +;shmem:// ; All 'shmem' transport plugin(s) +;builtin.shmem:// ; The builtin builtin 'shmem' transport plugin + ; at network address FCC0::0 +; +;; Unicast +; +;; Localhost +127.0.0.1 ; A comma can be used a separator +; +;; Granite Lab - P4 +; +;; Custom entries below this line (Vagrant, etc) diff --git a/astrobee/config/communications/dds_intercomms/RAPID_QOS_PROFILES.xml b/astrobee/config/communications/dds_intercomms/RAPID_QOS_PROFILES.xml new file mode 100644 index 0000000000..e4123aee6c --- /dev/null +++ b/astrobee/config/communications/dds_intercomms/RAPID_QOS_PROFILES.xml @@ -0,0 +1,989 @@ + + + + + + + + + + + + + + + 50, 48, 49, 49, 48, 54,0x32,0x39,0x2e,0x30,0x39,0x34,0x33,0x30,0x30,0x2e,0x48,0x55,0x2d,0x41,0x52,0x43 + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + MASK_DEFAULT + + + UDPv4 + + + + + + true + + + + 4096 + + + 0 + 0 + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController.scheduling_policy + DDS_EDF_FLOW_CONTROLLER_SCHED_POLICY + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.max_tokens + 12 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_added_per_period + 4 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_leaked_per_period + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.bytes_per_token + 1250 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.sec + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.nanosec + 5000000 + + + + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 1250 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65535 + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 65535 + + + + + dds.transport.shmem.builtin.received_message_count_max + 512 + + + dds.transport.shmem.builtin.receive_buffer_size + 1048576 + + + + + dds.transport.UDPv4.builtin.parent.deny_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + dds.transport.UDPv4.builtin.parent.deny_multicast_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + + + + + + + + + + 0 + + + + + + ALIVE_THEN_DISPOSED_INSTANCE_REPLACEMENT + + + ASYNCHRONOUS_PUBLISH_MODE_QOS + + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + KEEP_ALL_HISTORY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_ALL_HISTORY_QOS + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + KEEP_LAST_HISTORY_QOS + 1 + + + 512 + 4 + 128 + 128 + 512 + 380 + + + + 32 + + 0 + 500000000 + + 8 + true + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + KEEP_LAST_HISTORY_QOS + 100 + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 1000 + + + + + KEEP_LAST_HISTORY_QOS + 200 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + 100 + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + + + + + + + 50 + + + + + + + + + + + + + + + + 1000 + + + + + + + + 1000 + + + + + + + + + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController + + + KEEP_LAST_HISTORY_QOS + 1 + + + 32 + 32 + 1 + 1 + 32 + + + + + 4096 + + + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + + + + + + + + + + + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + + + + 30000 + 64 + 2048 + + + + UDPv4 | SHMEM + + + Monitoring UI Application + + + 65530 + + + + + 100 + 0 + + + 10 + 0 + + LIVELINESS_BASED_REMOTE_PARTICIPANT_PURGE + + 10 + 0 + + 1 + + 2 + 0 + + + 2 + 0 + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + + + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 1048576 + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 65530 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65530 + + + + + dds.transport.shmem.builtin.parent.message_size_max + 65530 + + + dds.transport.shmem.builtin.receive_buffer_size + 65530 + + + dds.transport.shmem.builtin.received_message_count_max + 32 + + + + + + + + + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 2 + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + diff --git a/astrobee/config/communications/dds_ros_bridge.config b/astrobee/config/communications/dds_ros_bridge.config index 82af273bdf..69b0f69c61 100644 --- a/astrobee/config/communications/dds_ros_bridge.config +++ b/astrobee/config/communications/dds_ros_bridge.config @@ -96,6 +96,10 @@ pub_topic_RCNCIRI = "-Navigation" use_RCPCIRI = true pub_topic_RCPCIRI = "-Perch" +-- ros_compressed_sci_cam_image_rapid_image => RCSCIRI -- +use_RCSCIRI = true +pub_topic_RCSCIRI = "-Science" + -- ros_cpu_state_rapid_cpu_state => RCSRCS -- use_RCSRCS = true pub_topic_RCSRCS = "" @@ -144,6 +148,20 @@ pub_topic_RLRLS = "" use_RORP = true pub_topic_RORP = "" +-- ros_sparse_mapping_pose_rapid_position => RSMPRP -- +use_RSMPRP = true +pub_topic_RSMPRP = "-sparse" +use_rate_RSMPRP = true + +-- ros_vive_pose_rapid_position => RVPRP -- +use_RVPRP = false +pub_topic_RVPRP = "-vive" +use_rate_RVPRP = false + +-- ros_payload_power_state_rapid_payload_power_state => RPPSRPPS -- +use_RPPSRPPS = true +pub_topic_RPPSRPPS = "" + -- ros_plan_status_rapid_plan_status => RPSRPS -- use_RPSRPS = true pub_topic_RPSRPS = "" @@ -160,6 +178,10 @@ use_RTRT = true pub_topic_RTRT = "" +-- ros_zones_rapid_compressed_file => RZRCF -- +use_RZRCF = true +pub_topic_RZRCF = "-current_zones" + comm_status_rate = 1 cpu_state_rate = 0 disk_state_rate = 1 @@ -167,6 +189,7 @@ ekf_state_rate = 0 gnc_state_rate = 0 pmc_command_rate = 0 position_rate = 30 +sparse_mapping_pose_rate = 0 battery_time_round_to_multiple = 10; diff --git a/astrobee/config/communications/ground_dds_ros_bridge.config b/astrobee/config/communications/ground_dds_ros_bridge.config new file mode 100644 index 0000000000..ba95d421ad --- /dev/null +++ b/astrobee/config/communications/ground_dds_ros_bridge.config @@ -0,0 +1,61 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +-- param for each bridge component use the following params -- +-- ABREV = First Letters of Component Ex: ros_string_rapid_text_msg = RSRTM -- +-- use_ABREV bool: turn this component on? -- +-- sub_topic_ABREV string: [RAPID: subscribe topic suffix] -- +-- pub_topic_ABREV string: [RAPID: publish topic suffix] -- +-- queue_size_ABREV int: queue size -- + +-- Robot we are receiving data from +connecting_robot = "Bumble" + +-- Domain id, needs to be 23 for space +domain_id = 37 + +-- If the namespace needs to be the connecting robot name, please set to true +use_namespace = false + +-- rapid_access_control_state_ros_access_control_state => RACSRACS +use_RACSRACS = true +sub_topic_RACSRACS = "" + +-- rapid_guest_science_data_ros_guest_science_data => RGSDRGSD +use_RGSDRGSD = true +sub_topic_RGSDRGSD = "" + +-- rapid_image_ros_compressed_dock_cam_image => RIRCDCI +use_RIRCDCI = true +sub_topic_RIRCDCI = "-Dock" + +-- rapid_image_ros_compressed_nav_cam_image => RIRCNCI +use_RIRCNCI = true +sub_topic_RIRCNCI = "-Navigation" + +-- rapid_image_ros_compressed_sci_cam_image => RIRCSCI +use_RIRCSCI = true +sub_topic_RIRCSCI = "-Science" + +-- rapid_position_ros_odom => RPRO -- +use_RPRO = true +sub_topic_RPRO = "" + +-- ros_command_rapid_command => RCRC +use_RCRC = true +pub_topic_RCRC = "" + diff --git a/astrobee/config/faults.config b/astrobee/config/faults.config index e9d8b01056..559760f7d3 100644 --- a/astrobee/config/faults.config +++ b/astrobee/config/faults.config @@ -17,60 +17,69 @@ -- Autogenerated from FMECA! DO NOT CHANGE! +states = { +} + +light_flow = { +} + arm = { - {id=61, key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, + {id=68, key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, +} + +perch = { } all = { - {id=85, key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, + {id=92, key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, } access_control = { - {id=70, key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, + {id=77, key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, } executive = { - {id=68, key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, + {id=75, key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, } data_bagger = { - {id=83, key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, + {id=90, key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, } -llp_cpu_monitor = { - {id=72, key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"}, - {id=86, key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, - {id=97, key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, - {id=100, key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, +llp_cpu_mem_monitor = { + {id=79, key="INITIALIZATION_FAILED", description="LLP CPU Memory Monitor Initialization Failed"}, + {id=93, key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, + {id=104, key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, + {id=107, key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, } llp_disk_monitor = { - {id=75, key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, - {id=89, key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, - {id=91, key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, - {id=94, key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, + {id=82, key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, + {id=96, key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, + {id=98, key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, + {id=101, key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, } -image_sampler = { - {id=81, key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, +mlp_cpu_mem_monitor = { + {id=80, key="INITIALIZATION_FAILED", description="MLP CPU Memory Monitor Initialization Failed"}, + {id=94, key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, + {id=105, key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, + {id=108, key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, } -mlp_disk_monitor = { - {id=76, key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, - {id=90, key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, - {id=92, key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, - {id=95, key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, +image_sampler = { + {id=88, key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, } -mlp_cpu_monitor = { - {id=73, key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"}, - {id=87, key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, - {id=98, key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, - {id=101, key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, +mlp_disk_monitor = { + {id=83, key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, + {id=97, key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, + {id=99, key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, + {id=102, key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, } sys_monitor = { - {id=105, key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, + {id=112, key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, } optical_flow_nodelet = { @@ -86,43 +95,49 @@ handrail_detect = { } localization_manager = { - {id=62, key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, - {id=117, key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, - {id=119, key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, + {id=69, key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, + {id=124, key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, + {id=126, key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, +} + +astrobee_astrobee_bridge = { } dds_ros_bridge = { - {id=71, key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, + {id=78, key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, } mapper = { - {id=80, key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, + {id=87, key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, } planner_qp = { - {id=78, key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, + {id=85, key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, } planner_trapezoidal = { - {id=79, key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, + {id=86, key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, } choreographer = { - {id=67, key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, + {id=74, key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, } laser = { } +signal_lights = { +} + eps_driver = { } perching_arm = { - {id=60, key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, + {id=67, key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, } framestore = { - {id=82, key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, + {id=89, key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, } pmc_actuator = { @@ -138,45 +153,48 @@ flashlight_aft = { } pico_driver = { - {id=59, key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, - {id=124, key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, + {id=66, key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, + {id=131, key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, } dock_cam = { } ctl = { - {id=53, key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, - {id=133, key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, + {id=59, key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, + {id=140, key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, +} + +imu_aug = { + {id=61, key="INITIALIZATION_FAILED", description="IMU Augmentor Initialization Failed"}, } -ekf = { - {id=54, key="INITIALIZATION_FAILED", description="EKF Initialization Failed"}, - {id=134, key="CONTROL_CYCLE_OVERRUN", description="Control Cycle Overrun"}, - {id=136, key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, +graph_loc = { + {id=60, key="INITIALIZATION_FAILED", description="Graph Localizer Initialization Failed"}, + {id=142, key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, } fam = { - {id=55, key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, + {id=62, key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, } speed_cam = { {id=10, key="VELOCITY_TOO_HIGH", description="Velocity too high"}, - {id=56, key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, - {id=126, key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, + {id=63, key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, + {id=133, key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, } epson_imu = { - {id=112, key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, - {id=145, key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, - {id=146, key="IMU_OVERTEMP", description="IMU overtemp"}, - {id=147, key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, - {id=148, key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, - {id=150, key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, - {id=151, key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, + {id=119, key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, + {id=151, key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, + {id=152, key="IMU_OVERTEMP", description="IMU overtemp"}, + {id=153, key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, + {id=154, key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, + {id=156, key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, + {id=157, key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, } dock = { - {id=84, key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, + {id=91, key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, } diff --git a/astrobee/config/geometry.config b/astrobee/config/geometry.config index 93c04673bb..21e5a88350 100644 --- a/astrobee/config/geometry.config +++ b/astrobee/config/geometry.config @@ -32,12 +32,12 @@ perch_cam_transform = robot_geometry.perch_cam_transform; haz_cam_transform = robot_geometry.haz_cam_transform; nav_cam_transform = robot_geometry.nav_cam_transform; dock_cam_transform = robot_geometry.dock_cam_transform; +sci_cam_transform = robot_geometry.sci_cam_transform; imu_transform = robot_geometry.imu_transform; -hazcam_to_navcam_transform = robot_geometry.hazcam_to_navcam_transform; -scicam_to_hazcam_transform = robot_geometry.scicam_to_hazcam_transform; -navcam_to_hazcam_timestamp_offset = robot_geometry.navcam_to_hazcam_timestamp_offset; -scicam_to_hazcam_timestamp_offset = robot_geometry.scicam_to_hazcam_timestamp_offset; -hazcam_depth_to_image_transform = robot_geometry.hazcam_depth_to_image_transform; + +nav_cam_to_haz_cam_transform = robot_geometry.nav_cam_to_haz_cam_transform; +nav_cam_to_sci_cam_transform = robot_geometry.nav_cam_to_sci_cam_transform; +haz_cam_depth_to_image_transform = robot_geometry.haz_cam_depth_to_image_transform; -- These will maybe be robot-specific post-calibration flashlight_front_transform = transform(vec3(0.1345, 0, .0369), quat4(0.0, 0.0, 0.0, 1.0)); @@ -75,7 +75,7 @@ function berth_transform(sign, tilt) end -- Various constants for docking -freeflyer_length = 0.163 +freeflyer_length = 0.200 freeflyer_reach = 0.155 approach_dist = 0.500 ensure_dist = 0.500 diff --git a/astrobee/config/gnc.config b/astrobee/config/gnc.config index d0753618ad..408f444bdd 100644 --- a/astrobee/config/gnc.config +++ b/astrobee/config/gnc.config @@ -16,68 +16,14 @@ -- under the License. require "common_lua" -require "cameras" require "geometry" -require "matrix" - -local matrix = require 'matrix' - --- Load prototype specific geometry (this will ) - --------------------- Test Parameters --------------------- -tun_ase_gravity_accel = world_gravity_vector; -tun_ase_gravity_removal = false; --- tun_ase_gravity_accel = vec3(0.0, 0.0, 0.0); --- tun_ase_gravity_removal = false; - --- Attitude Parameters -ctl_att_kp = vec3(2.0, 2.0, 2.0); -- .4 -ctl_att_ki = vec3(0.001, 0.001, 0.001); -ctl_omega_kd = matrix{{1.6, 1.6, 1.6}}; -- 1.1 - --- Position Loop -ctl_pos_kp = vec3(.6, .6, .6); --.1 -ctl_pos_ki = vec3(0.0001, 0.0001, 0.0001); -ctl_vel_kd = vec3(1.2, 1.2, 1.2); - --- Docking Parameters --- Position loop gains -ctl_pos_kp_dock = vec3(.6, .6, 0.0); --.1 -ctl_pos_ki_dock = vec3(0.0001, 0.0001, 0.0); -ctl_vel_kd_dock = vec3(1.2, 1.2, 1.2); - --- Attitude loop gains -ctl_att_kp_dock = vec3(0.0, 0.0, 1); -- 4 -ctl_att_ki_dock = vec3(0.0, 0.0, 0.001); -ctl_omega_kd_dock = matrix{{1.1, 1.1, 1.4}}; -- 3 +-- Velocity Feedforward Gain +tun_vel_gain = 0.5; -- Accel Feedforward Gains tun_accel_gain = vec3(1.0, 1.0, 1.0); tun_alpha_gain = vec3(1.0, 1.0, 1.0); --- Simulation Parameters -tun_bpm_enable = true; -- Set model to use bpm instead of vpp -tun_env_accel_dof_gain = vec3(1.0, 1.0, 1.0); -- Constrain Z axis movement in simulation (granite table) -tun_env_alpha_dof_gain = vec3(1.0, 1.0, 1.0); -- Constrain X and Y axis rotation in simulation (granite table) -tun_iss_omega_ISS_ECI_ISS = vec3(-8.2650e-07, 3.0746e-08, 7.2916e-05); -- Earth spin rate in granite table frame, See derivation in env_environment_model_prep.m - --- Debug Truth Data Parameters --- Filter applied to quaternion derivative -tun_truth_q_omega_filt_cutoff_freq = 15*pi/180; -tun_truth_q_omega_filt_enable = 0.0; -- Disabled by default -tun_truth_velocity_filt_cutoff_freq = 15*pi/180; -tun_truth_velocity_filt_enable = 0.0; -- Disabled by default - ------------------------------------------------------------------ - --- Initial Conditions - -tun_ini_P_B_ISS_ISS = vec3(0.0, 0.0, -0.7); -- sim position i.c. - -tun_ini_V_B_ISS_ISS = vec3(0.0, 0.0, 0.0); -- sim velocity i.c. -tun_ini_omega_B_ISS_B = vec3(0.0, 0.0, 0.0); -- sim rotational velocity i.c. -tun_ini_Q_ISS2B = {0.0, 0.0, 0.0, 1.0}; -- sim initial attitude quat i.c. - -- CTL parameters tun_ctl_linear_force_limit = 100.0; tun_ctl_att_sat_lower = -0.5; @@ -86,220 +32,10 @@ tun_ctl_att_sat_upper = 0.5; tun_ctl_pos_sat_lower = -0.1; tun_ctl_pos_sat_upper = 0.1; --------- Default Gain Values -------- --- Actual gains are flight mode specific and set in %%%% -tun_default_att_kp = vec3(2.0, 2.0, 2.0); --.4 -tun_default_att_ki = vec3(0.001, 0.001, 0.001); -tun_default_omega_kd = vec3(1.6, 1.6, 1.6); -- 1.1 -tun_default_pos_kp = vec3(.6, .6, 0.6); --.1 -tun_default_pos_ki = vec3(0.0001, 0.0001, 0.0001); -tun_default_vel_kd = vec3(1.2, 1.2, 1.2); -tun_default_center_of_mass = vec3(0.0, 0.0, 0.00); -tun_default_inertia_matrix = matrix{{0.1083, 0, 0}, {0, 0.1083, 0}, {0, 0, 0.1083}}; -tun_default_mass = 7.0; -tun_default_speed_gain_cmd = 3; -tun_default_localization_mode_cmd = 0; ---tun_ffp_veh_inertia_matrix = ---tun_ffp_veh_inertia_matrix_inv = matrix.invert(tun_ffp_veh_inertia_matrix); - - --------------------------- Command Shaper ---------------------------- -tun_ctl_bypass_cmd_shaper = true; - --- PCH Gains -tun_cmd_pch_pos_k = 0*2*8; -tun_cmd_pch_att_k = 0*20*3; --- nozzle_areas = matrix{{0.0027, 0.0027, 0.0015, 0.0015, 0.0015, 0.0015, 0.0027, 0.0027, 0.0015, 0.0015, 0.0015, 0.0015}}; - -tun_cmd_pch_max_area = {0.0027, 0.0027, 0.0015, 0.0015, 0.0015, 0.0015, 0.0027, - 0.0027, 0.0015, 0.0015, 0.0015, 0.0015}; - --- Reset Parameters -tun_cmd_pos_error_reset_thresh = 0.25; -- Magnitude of position error (m) before the command shaper recomputes a new trajectory -tun_cmd_att_error_reset_thresh = 30 * math.pi / 180; -- Magnitude of attitude error (rad) before the command shaper recomputes a new trajectory - - -- Stopped Mode Parameters tun_ctl_stopped_pos_thresh = 0.1; tun_ctl_stopped_quat_thresh = 10*pi/180; tun_ctl_stopping_vel_thresh = .02^2; -- Linear velocity threshold squared below which the vehicle is considered to be stopped, (m/s)^2 tun_ctl_stopping_omega_thresh = .02^2; -- Angular velocity threshold squared below which the vehicle is considered to be stopped, (rad/s)^2 - - --- Command Shaper Gains -tun_cmd_pos_gain_mtrx = vec3(4/2, 4/2, 4/2); -tun_cmd_vel_gain_mtrx = vec3(67.5/4, 67.5/4, 67.5/4); -tun_cmd_acc_gain_mtrx = vec3(10.0, 10.0, 10.0); -tun_cmd_quat_gain_mtrx = vec3(1.0*10, 1.0*10, 1.0*10); -tun_cmd_omega_gain_mtrx = vec3(4.0*10, 4.0*10, 4.0*10); -tun_cmd_alpha_gain_mtrx = vec3(8.0*10, 8.0*10, 8.0*10); --2 - - -tun_cmd_hard_vel_limit = 0.5; -tun_cmd_hard_accel_limit = 0.1; -tun_cmd_hard_omega_limit= 45 * math.pi / 180; -- [rad/s] Hard limit for vehicle angular velocity, commands will never exceed -tun_cmd_hard_alpha_limit= 50 * math.pi / 180; -- [rad/s/s] Hard limit for vehicle angular acceleration, commands will never exceed - - - --------------------------- EKF parameters ---------------------------- - --- IMU parameters ---q_gyro = 3.24E-5 ^ 2; ---q_gyro_bias = 2e-6 ^ 2; ---q_accel = (10*5.8e-4) ^ 2; ---q_accel_bias = 8e-5 ^ 2; -- 8e-5, 300e-5 ---q_gyro = 1000 * 1.3539e-11; ---q_gyro_bias = 1000 * 3.4320e-14; ---q_accel = 20 * 4.5511e-09; ---q_accel_bias = 10 * 1.3678e-11; - ---q_gyro = 0.0002 ^ 2 ---q_gyro = (0.14 * math.pi / 180) ^ 2 ---q_gyro_bias = (1e-6 * math.pi / 180) ^ 2 ---q_accel = 0.001 ^ 2 ---q_accel_bias = 1e-5 ^ 2 - -q_gyro = (0.274 * math.pi / 180) ^ 2 -q_gyro_bias = (1e-7 * math.pi / 180) ^ 2 -q_accel = 1e-7 -q_accel_bias = 5e-9 - -tun_ase_Q_imu = {q_gyro, q_gyro, q_gyro, q_gyro_bias, q_gyro_bias, q_gyro_bias, - q_accel, q_accel, q_accel, q_accel_bias, q_accel_bias, q_accel_bias}; -tun_ase_max_gyro = (150 * math.pi / 180); -tun_ase_max_accel = 3 * 9.81; -tun_ase_q_saturated_gyro = (2 * math.pi / 180) ^ 2; -tun_ase_q_saturated_accel = 1; - ---------------------- Vision parameters ---------------------- -tun_ase_enable_of = true; -tun_ase_mahal_distance_max = 18; -- Max distance to accept a measurment -tun_max_mahal_reject_frames = 200000; -- Max number of consecutive measurements to reject via the mahalanobis check -tun_ase_ml_forward_projection_time = 0.5 * 1.0 / 15; -tun_ase_map_error = 0.02 - --- Mapped Landmark Navigation -tun_ase_navcam_inv_focal_length = 1.0 / nav_cam.intrinsic_matrix[1]; -tun_ase_navcam_distortion = nav_cam.distortion_coeff; -tun_ase_vis_r_mag = 4.5; -tun_ase_min_ml_meas = 10; --Minumum number of valid features to trigger a ML update - --- Docking -tun_ase_dockcam_inv_focal_length = 1.0 / dock_cam.intrinsic_matrix[1]; -tun_ase_dockcam_distortion = nav_cam.distortion_coeff; -tun_ase_dock_r_mag = 2.0; -tun_ase_min_ar_meas = 4; -- Min number of features in AR mode to trigger an update - --- VizOdometry -tun_ase_vocam_inv_focal_length = tun_ase_navcam_inv_focal_length; -tun_ase_of_r_mag = 1.22; -tun_ase_min_of_meas = 10; --Minumum number of valid track that triggers an OF update - --- Perching -tun_ase_min_hr_meas = 10; --Minumum number of depth sensor points that triggers an HR update -tun_ase_hr_r_mag = 0.01; -- Perching measurment noise -tun_ase_hr_distance_r = 0.06; -- meters - --- State changes --- in units of variance of position -tun_ase_converged_thresh = 0.1 ^ 2 -tun_ase_diverged_thresh = 0.15 ^ 2 -tun_ase_acquired_ticks = 62.5 * 100; -- Num of est. calls before drops to acquired without ML update - --- Initial state -tun_ase_state_ic_P_B_ISS_ISS = tun_ini_P_B_ISS_ISS; -tun_ase_state_ic_quat_ISS2B = tun_ini_Q_ISS2B; -tun_ase_state_ic_omega_B_ISS_B = tun_ini_omega_B_ISS_B; -tun_ase_state_ic_V_B_ISS_ISS = tun_ini_V_B_ISS_ISS; - --- Calculate initial Estimator Position -q1 = tun_ini_Q_ISS2B[1]; -q2 = tun_ini_Q_ISS2B[2]; -q3 = tun_ini_Q_ISS2B[3]; -q4 = tun_ini_Q_ISS2B[4]; -temp_xi = matrix{{q4, -q3, q2}, {q3, q4, -q1}, {-q2, q1, q4}, {-q1, -q2, -q3}}; -temp_xi = temp_xi; -temp_psi = matrix{{q4, q3, -q2}, {-q3, q4, q1}, {q2, -q1, q4}, {-q1, -q2, -q3}}; -xi_trans = matrix.transpose(temp_xi); -temp_A = xi_trans * temp_psi; -imu_P_IMU_B_ISS = matrix.transpose(temp_A) * matrix{imu_transform.trans[1], imu_transform.trans[2], imu_transform.trans[3]}; -imu_P_IMU_B_ISS = vec3(matrix.getelement(imu_P_IMU_B_ISS,1,1), matrix.getelement(imu_P_IMU_B_ISS,2,1), matrix.getelement(imu_P_IMU_B_ISS,3,1)); -tun_ase_state_ic_P_EST_ISS_ISS = tun_ini_P_B_ISS_ISS + imu_P_IMU_B_ISS; -tun_ic_cov_quat = 1e-4; -ic_cov_g_bias = 2e-11; -ic_cov_vel = 1e-4; -ic_cov_a_bias = 1e-9; -tun_ic_cov_pos = 1e-3; -tun_aug_ic_cov = 1e-5; -tun_ase_state_ic_cov_diag = {tun_ic_cov_quat, tun_ic_cov_quat, tun_ic_cov_quat, - ic_cov_g_bias, ic_cov_g_bias, ic_cov_g_bias, - ic_cov_vel, ic_cov_vel, ic_cov_vel, - ic_cov_a_bias, ic_cov_a_bias, ic_cov_a_bias, - tun_ic_cov_pos, tun_ic_cov_pos, tun_ic_cov_pos}; - - --- Sensor Position and Orientation -tun_abp_p_navcam_imu_est = nav_cam_transform.trans - imu_transform.trans; -tun_abp_p_navcam_body_body_sim = nav_cam_transform.trans; -tun_abp_q_body2navcam = {nav_cam_transform.rot.x, nav_cam_transform.rot.y, nav_cam_transform.rot.z, nav_cam_transform.rot.w}; -tun_abp_p_dockcam_imu_est = dock_cam_transform.trans - imu_transform.trans; -tun_abp_p_dockcam_body_body_sim = dock_cam_transform.trans; -tun_abp_q_body2dockcam = {dock_cam_transform.rot.x, dock_cam_transform.rot.y, dock_cam_transform.rot.z, dock_cam_transform.rot.w}; -tun_abp_p_perchcam_imu_est = perch_cam_transform.trans - imu_transform.trans; -tun_abp_p_perchcam_body_body_sim = perch_cam_transform.trans; -tun_abp_q_body2perchcam = {perch_cam_transform.rot.x, perch_cam_transform.rot.y, perch_cam_transform.rot.z, perch_cam_transform.rot.w}; - -tun_abp_p_imu_body_body = imu_transform.trans; -tun_abp_quat_body2imu = {imu_transform.rot.x, imu_transform.rot.y, imu_transform.rot.z, imu_transform.rot.w}; - --- Accel High Pass Filter (For ground testing in the presence of gravity) -tun_grav_hp_enable_f = false; -hp_omega_c = 10; -hp_Ts = 1/62.5; -tun_grav_hp_num = {2, -2}; -tun_grav_hp_den = {2+hp_Ts*hp_omega_c, hp_Ts*hp_omega_c - 2}; - --------------------------- SIM parameters ---------------------------- - tun_debug_ctl_use_truth = false; -- Controller will use ground truth instead of KFL estimate, only works in sim - --- Mass properties -tun_mass_error = 0; -- Computed as true_mass = knowledge_mass + tun_mass_error -tun_inertia_error_mat = matrix{{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; -- true_inertia = known_inertia + tun_inertia_error_mat -tun_sim_cg_error = vec3(0.0, 0.0, 0.0); -- true_cg = tun_default_center_of_mass + tun_cg_error - --- CVS Model -tun_cvs_navcam_focal_length_X = nav_cam.intrinsic_matrix[1]; -tun_cvs_navcam_focal_length_Y = nav_cam.intrinsic_matrix[5]; -tun_cvs_navcam_num_pixels_X = nav_cam.undistorted_width; -tun_cvs_navcam_num_pixels_Y = nav_cam.undistorted_height; -tun_cvs_dockcam_focal_length_X = dock_cam.intrinsic_matrix[1]; -tun_cvs_dockcam_focal_length_Y = dock_cam.intrinsic_matrix[5]; -tun_cvs_dockcam_num_pixels_X = dock_cam.undistorted_width; -tun_cvs_dockcam_num_pixels_Y = dock_cam.undistorted_height; -tun_cvs_perchcam_focal_length_X = 113.638; -tun_cvs_perchcam_focal_length_Y = 86.1833; -tun_cvs_perchcam_num_pixels_X = 224; -tun_cvs_perchcam_num_pixels_Y = 171; - - --- Noise Enables --- tun_epson_report_truth = true; --- tun_cvs_noise_on = false; --- tun_bpm_noise_on_flag = 0.0; -- Needs to be a single for some reason, 1.0 = noise on, 0.0 noise off -tun_env_drag_disturb_on = false; -- Air drag - -tun_epson_report_truth = false; -tun_cvs_noise_on = true; -tun_bpm_noise_on_flag = 0.0; - --- Sim Thrust Scale Factor, scale factor to apply to simulated thrust -tun_bpm_PM1_thrust_error_sf = 1.25; -tun_bpm_PM2_thrust_error_sf = 1.25; - --- Non-GNC parameters -min_of_observations = 15; -bias_required_observations = 62 * 5; -imu_bias_file = resolve_resource("imu_bias.config"); diff --git a/astrobee/config/hw/epson_imu.config b/astrobee/config/hw/epson_imu.config index 56bbad4c90..339adf603e 100644 --- a/astrobee/config/hw/epson_imu.config +++ b/astrobee/config/hw/epson_imu.config @@ -25,7 +25,7 @@ epson_imu = { gpio_num_data_ready = robot_imu_drdy_pin, gpio_num_reset = 24, gpio_num_cs = 62, - sampling_rate = 0x06, + sampling_rate = 0x05, filter = 0x06, calibration_sampling_rate = 0x04, calibration_filter = 0x04, diff --git a/astrobee/config/hw/pmc_actuator.config b/astrobee/config/hw/pmc_actuator.config index dd64e97042..482b2fef27 100644 --- a/astrobee/config/hw/pmc_actuator.config +++ b/astrobee/config/hw/pmc_actuator.config @@ -42,6 +42,9 @@ i2c_retries = 3 -- Control rate in Hz control_rate_hz = 62.5 +-- Maximum PMC time without control input +max_timeout = 2.0 + -- The null speed and nozzle positions null_speed = 0 null_nozzle_positions = {128, 128, 128, 128, 128, 128} diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 16e538c03d..7bfe466c07 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -15,18 +15,108 @@ -- License for the specific language governing permissions and limitations -- under the License. -num_similar = 20 -ransac_inlier_tolerance = 5 -ransac_iterations = 100 -early_break_landmarks = 100 -histogram_equalization = 1 -min_features = 200 -max_features = 800 -detection_retries = 1 -num_threads = 2 -min_brisk_threshold = 20.0 -default_brisk_threshold = 90.0 -max_brisk_threshold = 110.0 matched_features_on = false all_features_on = false -map_cloud_on = false +num_threads = 1 +verbose_localization = false +visualize_localization_matches = false + +-- BRISK +brisk_num_similar = 20 +brisk_min_query_score_ratio = 0 +brisk_ransac_inlier_tolerance = 5 +brisk_num_ransac_iterations = 100 +brisk_early_break_landmarks = 100 +brisk_histogram_equalization = 1 +brisk_check_essential_matrix = false +brisk_essential_ransac_iterations = 0 +brisk_add_similar_images = false +brisk_add_best_previous_image = false +brisk_hamming_distance = 90 +brisk_goodness_ratio = 10000 +brisk_use_clahe = false +brisk_num_extra_localization_db_images = 0 +-- Detection Params +brisk_min_threshold = 20.0 +brisk_default_threshold = 90.0 +brisk_max_threshold = 110.0 +brisk_min_features = 200 +brisk_max_features = 800 +brisk_too_many_ratio = 1.25 +brisk_too_few_ratio = 0.8 +brisk_detection_retries = 1 +-- Localizer Threshold Params +brisk_success_history_size = 0 +brisk_min_success_rate = 0 +brisk_max_success_rate = 1 +brisk_adjust_num_similar = false +brisk_min_num_similar = 20 +brisk_max_num_similar = 20 + +-- TEBLID512 +teblid512_num_similar = 20 +teblid512_min_query_score_ratio = 0.45 +teblid512_ransac_inlier_tolerance = 3 +teblid512_num_ransac_iterations = 1000 +teblid512_early_break_landmarks = 100 +teblid512_histogram_equalization = 3 +teblid512_check_essential_matrix = true +teblid512_essential_ransac_iterations = 2000 +teblid512_add_similar_images = true +teblid512_add_best_previous_image = true +teblid512_hamming_distance = 85 +teblid512_goodness_ratio = 0.8 +teblid512_use_clahe = true +teblid512_num_extra_localization_db_images = 0 + +-- Detection Params +teblid512_min_threshold = 20.0 +teblid512_default_threshold = 72.0 +teblid512_max_threshold = 110.0 +teblid512_min_features = 1000 +teblid512_max_features = 3000 +teblid512_too_many_ratio = 1.05 +teblid512_too_few_ratio = 0.95 +teblid512_detection_retries = 1 + +-- Localizer Threshold Params +teblid512_success_history_size = 10 +teblid512_min_success_rate = 0.5 +teblid512_max_success_rate = 0.9 +teblid512_adjust_num_similar = true +teblid512_min_num_similar = 15 +teblid512_max_num_similar = 20 + +-- TEBLID256 +teblid256_num_similar = 20 +teblid256_min_query_score_ratio = 0.45 +teblid256_ransac_inlier_tolerance = 3 +teblid256_num_ransac_iterations = 1000 +teblid256_early_break_landmarks = 100 +teblid256_histogram_equalization = 3 +teblid256_check_essential_matrix = true +teblid256_essential_ransac_iterations = 2000 +teblid256_add_similar_images = true +teblid256_add_best_previous_image = true +teblid256_hamming_distance = 85 +teblid256_goodness_ratio = 0.8 +teblid256_use_clahe = true +teblid256_num_extra_localization_db_images = 0 + +-- Detection Params +teblid256_min_threshold = 20.0 +teblid256_default_threshold = 72.0 +teblid256_max_threshold = 110.0 +teblid256_min_features = 1000 +teblid256_max_features = 3000 +teblid256_too_many_ratio = 1.05 +teblid256_too_few_ratio = 0.95 +teblid256_detection_retries = 1 + +-- Localizer Threshold Params +teblid256_success_history_size = 10 +teblid256_min_success_rate = 0.5 +teblid256_max_success_rate = 0.9 +teblid256_adjust_num_similar = true +teblid256_min_num_similar = 15 +teblid256_max_num_similar = 20 diff --git a/astrobee/config/localization/depth_odometry.config b/astrobee/config/localization/depth_odometry.config new file mode 100644 index 0000000000..68fa53037f --- /dev/null +++ b/astrobee/config/localization/depth_odometry.config @@ -0,0 +1,156 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +require "context" + +-- Max time diff between successive depth measurements +max_time_diff = 0.7 + +-- Max time diff for correlating image and point cloud measurements to create a depth measurement +-- This ideally have the same timestamp but sometimes vary slightly even though they are from +-- the same measurement +max_image_and_point_cloud_time_diff = 0.05 + +-- Covariance thresholds for rejecting relative odometry result +position_covariance_threshold = 100 +orientation_covariance_threshold = 100 + +-- icp or image_feature +depth_odometry_method = "image_feature" + +-- Buffer size for images and point clouds +max_buffer_size = 5 + +-- Max number of depth images to create in a single iteration +-- Limits depth odometry runtime +max_depth_images = 2 + +-- ICP options + +-- Search radius for each point in ICP +search_radius = 0.04 + +symmetric_objective = true +-- Only applicable for symmetric objective +enforce_same_direction_normals = true + +-- ICP fitness score threshold for rejecting ICP result +use_fitness_threshold_rejection = false +fitness_threshold = 1 +max_iterations = 1 + +-- Use RANSAC AI algorithm to compute initial estimate +inital_estimate_with_ransac_ia = false + +correspondence_rejector_surface_normal = true +-- Threshold for cos(theta) where theta is the angle between two normals. Range from [0,1] +-- The closer to 1, the more aligned the normals are +correspondence_rejector_surface_normal_threshold = 0.75 + +correspondence_rejector_median_distance = false +-- Correspondences with distance > median_distance*factor are rejected +correspondence_rejector_median_distance_factor = 1.0 + +-- Organized normal estimation +use_organized_normal_estimation = true +-- avg_3d_gradient, covariance, avg_depth_change +organized_normal_method = "avg_3d_gradient" +use_depth_dependent_smoothing = false +max_depth_change_factor = 0.02 +normal_smoothing_size = 10.0 + +-- Normal space sampling +use_normal_space_sampling = true +bins_per_axis = 12 +num_samples = 3500 + +-- Downsample options +downsample = false +downsample_leaf_size = 0.02 + +-- coarse to fine options for ICP +coarse_to_fine = false +num_coarse_to_fine_levels = 1 +coarse_to_fine_downsample_ratio = 0.5 +-- Final (smallest) leaf size +coarse_to_fine_final_leaf_size = 0.02 +downsample_last_coarse_to_fine_iteration = true + +-- Depth image aligner options + +-- brisk, surf, lk_optical_flow +detector = "lk_optical_flow" +-- Brisk detector options +brisk_threshold = 120 +brisk_octaves = 4 +brisk_float_pattern_scale = 1 +-- Brisk matching options +brisk_max_match_hamming_distance = 100 +brisk_flann_table_number = 3 +brisk_flann_key_size = 18 +brisk_flann_multi_probe_level = 2 +-- Surf options +surf_threshold = 1000 +surf_max_match_distance = 0.25 +-- LK optical flow options +lk_max_iterations = 10 +lk_termination_epsilon = 0.03 +lk_window_length = 10 +lk_max_level = 3 +lk_min_eigen_threshold = 0.2 +lk_max_flow_distance = 50 +lk_max_backward_match_distance = 0.1 +-- Good features to track options +lk_max_corners = 100 +lk_quality_level = 0.01 +lk_min_distance = 10 +lk_block_size = 5 +lk_use_harris_detector = false +lk_k = 0.04 + +-- CLAHE histogram equalization options +use_clahe = true +clahe_grid_length = 8 +clahe_clip_limit = 40 +-- Other +min_x_distance_to_border = 10 +min_y_distance_to_border = 10 +min_num_correspondences = 5 +-- Don't compute relative pose and just return 3D point correspondences +-- Only applicable for image_feature approach +only_correspondences = true +-- Refine estimate with PointToPlaneICP +refine_estimate = true +-- Filter outliers using essential matrix estimation +filter_outliers = false +-- ransac, lmeds, rho (rho is actual prosac) +filter_method = "rho" +inlier_threshold = 1.0 +inlier_probability = 0.999 + +-- Point cloud with known correspondences aligner +pcwkca_max_num_iterations = 100 +pcwkca_function_tolerance = 1e-6 +pcwkca_max_num_match_sets = 10000000 +pcwkca_normal_search_radius = 0.03 +pcwkca_use_umeyama_initial_guess = false +pcwkca_use_single_iteration_umeyama = false +pcwkca_use_point_to_plane_cost = false +pcwkca_use_symmetric_point_to_plane_cost = false +pcwkca_verbose_optimization = false + +publish_point_clouds = false diff --git a/astrobee/config/localization/graph_localizer.config b/astrobee/config/localization/graph_localizer.config new file mode 100644 index 0000000000..e2fea3bc80 --- /dev/null +++ b/astrobee/config/localization/graph_localizer.config @@ -0,0 +1,82 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Graph Localizer (gl) +-- Factor Adders (fa) +--- Loc +---- Sparse Map (sm) +gl_fa_loc_sm_enabled = true +gl_fa_loc_sm_huber_k = world_huber_k +gl_fa_loc_sm_add_pose_priors = false +gl_fa_loc_sm_add_projection_factors = true +gl_fa_loc_sm_add_prior_if_projection_factors_fail = true +gl_fa_loc_sm_prior_translation_stddev = 0.06 +gl_fa_loc_sm_prior_quaternion_stddev = 0.06 +gl_fa_loc_sm_scale_pose_noise_with_num_landmarks = false +gl_fa_loc_sm_scale_projection_noise_with_num_landmarks = false +gl_fa_loc_sm_scale_projection_noise_with_landmark_distance = false +--- change to <=1 w/ new vio cov +gl_fa_loc_sm_pose_noise_scale = 0.001 +gl_fa_loc_sm_projection_noise_scale = 1.5 +gl_fa_loc_sm_max_num_projection_factors = 25 +gl_fa_loc_sm_min_num_matches_per_measurement = 5 +gl_fa_loc_sm_max_valid_projection_error = 30 +gl_fa_loc_sm_nav_cam_noise_stddev = 0.1 +---- AR Tag (ar) +gl_fa_loc_ar_enabled = true +gl_fa_loc_ar_huber_k = world_huber_k +gl_fa_loc_ar_add_pose_priors = false +gl_fa_loc_ar_add_projection_factors = true +gl_fa_loc_ar_add_prior_if_projection_factors_fail = true +gl_fa_loc_ar_prior_translation_stddev = 0.06 +gl_fa_loc_ar_prior_quaternion_stddev = 0.06 +gl_fa_loc_ar_scale_pose_noise_with_num_landmarks = false +gl_fa_loc_ar_scale_projection_noise_with_num_landmarks = false +gl_fa_loc_ar_scale_projection_noise_with_landmark_distance = false +gl_fa_loc_ar_pose_noise_scale = 0.01 +-- Change to 9 if scale with inverse distance +gl_fa_loc_ar_projection_noise_scale = 10 +gl_fa_loc_ar_max_num_projection_factors = 25 +gl_fa_loc_ar_min_num_matches_per_measurement = 5 +gl_fa_loc_ar_max_valid_projection_error = 30 +gl_fa_loc_ar_dock_cam_noise_stddev = 0.1 +-- Node Adders (na) +--- Pose +gl_na_pose_starting_prior_translation_stddev = 0.02 +gl_na_pose_starting_prior_quaternion_stddev = 0.01 +gl_na_pose_huber_k = world_huber_k +gl_na_pose_add_priors = true +gl_na_pose_ideal_duration = 15 +gl_na_pose_min_num_states = 3 +gl_na_pose_max_num_states = 5 +gl_na_pose_model_huber_k = world_huber_k +-- Optimizer (op) +--- Nonlinear +gl_op_nl_marginals_factorization = "qr" +gl_op_nl_max_iterations = 10 +gl_op_nl_verbose = false +gl_op_nl_use_ceres_params = false +-- Graph Optimizer (go) +--- Sliding Window +gl_go_sw_huber_k = world_huber_k +gl_go_sw_log_stats_on_destruction = false +gl_go_sw_print_after_optimization = false +gl_go_sw_add_marginal_factors = false +gl_go_sw_slide_window_before_optimization = true +-- Other +gl_max_vio_measurement_gap = 3 diff --git a/astrobee/config/localization/graph_vio.config b/astrobee/config/localization/graph_vio.config new file mode 100644 index 0000000000..6f347bb858 --- /dev/null +++ b/astrobee/config/localization/graph_vio.config @@ -0,0 +1,90 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Graph VIO (gv) +-- Factor Adders (fa) +--- Standstill +gv_fa_standstill_enabled = true +gv_fa_standstill_huber_k = world_huber_k +gv_fa_standstill_add_velocity_prior = true +gv_fa_standstill_add_pose_between_factor = true +gv_fa_standstill_prior_velocity_stddev = 0.01 +gv_fa_standstill_pose_between_factor_translation_stddev = 0.01 +gv_fa_standstill_pose_between_factor_rotation_stddev = 0.01 +--- VO +gv_fa_vo_enabled = true +gv_fa_vo_huber_k = world_huber_k +gv_fa_vo_max_num_factors = 8 +gv_fa_vo_min_num_points_per_factor = 2 +gv_fa_vo_max_num_points_per_factor = 6 +gv_fa_vo_min_avg_distance_from_mean = 0.075 +gv_fa_vo_robust = true +gv_fa_vo_rotation_only_fallback = true +gv_fa_vo_fix_invalid_factors = true +gv_fa_vo_scale_noise_with_num_points = true +gv_fa_vo_noise_scale = 2 +gv_fa_vo_nav_cam_noise_stddev = 0.1 +---- Smart Factor +gv_fa_vo_rotation_only_fallback = true +gv_fa_vo_enable_EPI = false +gv_fa_vo_landmark_distance_threshold = 500 +gv_fa_vo_dynamic_outlier_rejection_threshold = 50 +gv_fa_vo_retriangulation_threshold = 1e-5 +gv_fa_vo_verbose_cheirality = false +---- Spaced Feature Tracker +gv_fa_vo_remove_undetected_feature_tracks = true +gv_fa_vo_measurement_spacing = 2 +---- Depth Odometry +gv_fa_do_enabled = true +gv_fa_do_huber_k = world_huber_k +gv_fa_do_use_points_between_factor = true +gv_fa_do_pose_covariance_scale = 1 +gv_fa_do_point_noise_scale = 0.01 +gv_fa_do_scale_point_between_factors_with_inverse_distance = true +gv_fa_do_scale_point_between_factors_with_estimate_error = false +gv_fa_do_reject_large_point_to_point_error = false +gv_fa_do_point_to_point_error_threshold = 10.0 +gv_fa_do_reject_large_translation_norm = false +gv_fa_do_pose_translation_norm_threshold = 3.0 +gv_fa_do_max_num_points_between_factors = 30 +-- Node Adders (na) +--- Combined Nav State +gv_na_cns_huber_k = world_huber_k +gv_na_cns_add_priors = true +gv_na_cns_ideal_duration = 3.25 +gv_na_cns_min_num_states = 3 +gv_na_cns_max_num_states = 8 +--- Combined Nav State Model +gv_na_cns_model_huber_k = world_huber_k +-- Optimizer (op) +--- Nonlinear +gv_op_nl_marginals_factorization = "qr" +gv_op_nl_max_iterations = 6 +gv_op_nl_verbose = false +gv_op_nl_use_ceres_params = false +-- Graph Optimizer (go) +--- Sliding Window +gv_go_sw_huber_k = world_huber_k +gv_go_sw_log_stats_on_destruction = false +gv_go_sw_print_after_optimization = false +gv_go_sw_add_marginal_factors = false +gv_go_sw_slide_window_before_optimization = true +-- Standstill +gv_standstill_min_num_points_per_track = 4 +gv_standstill_duration = 1 +gv_standstill_max_avg_distance_from_mean = 0.075 diff --git a/astrobee/config/localization/handrail_detect.config b/astrobee/config/localization/handrail_detect.config index a8943a3393..71bfe38b44 100644 --- a/astrobee/config/localization/handrail_detect.config +++ b/astrobee/config/localization/handrail_detect.config @@ -38,6 +38,8 @@ arm_length = 0.15 min_measure_gap = 0.004 min_depth_dist = 0.1 max_depth_dist = 1.5 +max_num_line_points = 40 +max_num_plane_points = 20 -- RANSAC related parameters RANSAC_line_iteration = 30 diff --git a/astrobee/config/localization/imu_bias_initializer.config b/astrobee/config/localization/imu_bias_initializer.config new file mode 100644 index 0000000000..8e42670469 --- /dev/null +++ b/astrobee/config/localization/imu_bias_initializer.config @@ -0,0 +1,21 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- IMU bias initializer (ibi) +ibi_imu_bias_filename = resolve_resource("imu_bias.config"); +ibi_num_bias_estimation_measurements = 100 diff --git a/astrobee/config/localization/imu_filter.config b/astrobee/config/localization/imu_filter.config new file mode 100644 index 0000000000..6d6fbe86b6 --- /dev/null +++ b/astrobee/config/localization/imu_filter.config @@ -0,0 +1,27 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- IMU Filter (if) +-- none, ButterOxSxLpxNx, where O is order, S is sample rate, Lp is low pass cutoff, N is notch +-- Note that notch depends on the sample rate and aliasing +if_quiet_accel = "ButterO3S125Lp3N33_33" +if_quiet_ang_vel = "ButterO1S125Lp3N33_33" +if_nominal_accel = "ButterO3S125Lp3N41_66" +if_nominal_ang_vel = "ButterO1S125Lp3N41_66" +if_aggressive_accel = "ButterO3S125Lp3N46_66" +if_aggressive_ang_vel = "ButterO1S125Lp3N46_66" diff --git a/astrobee/config/localization/imu_integrator.config b/astrobee/config/localization/imu_integrator.config new file mode 100644 index 0000000000..782056284c --- /dev/null +++ b/astrobee/config/localization/imu_integrator.config @@ -0,0 +1,27 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- IMU Integrator (ii) +ii_gyro_sigma = world_gyro_sigma +ii_accel_sigma = world_accel_sigma +ii_accel_bias_sigma = world_accel_bias_sigma +ii_gyro_bias_sigma = world_gyro_bias_sigma +ii_integration_variance = world_integration_variance +ii_bias_acc_omega_int = world_bias_acc_omega_int +ii_gravity = world_gravity_vector +ii_ignore_gravity = true diff --git a/astrobee/config/localization/localization_manager.config b/astrobee/config/localization/localization_manager.config index 3abc728f55..f909904f06 100644 --- a/astrobee/config/localization/localization_manager.config +++ b/astrobee/config/localization/localization_manager.config @@ -108,13 +108,13 @@ pipelines = { { id = "ml", name = "Sparse map", ekf_input = 0, needs_filter = true, optical_flow = true, timeout = 1.0, max_confidence = 0, enable_topic = "loc/ml/enable", enable_timeout = 20.0, - reg_topic = "loc/ml/registration", reg_timeout = 2.0, + reg_topic = "loc/ml/registration", reg_timeout = 30.0, feat_topic = "loc/ml/features", feat_timeout = 600.0, feat_threshold = 3 }, -- AR Tags { id = "ar", name = "AR Tags", ekf_input = 1, needs_filter = true, optical_flow = true, timeout = 1.0, max_confidence = 0, enable_topic = "loc/ar/enable", enable_timeout = 10.0, - reg_topic = "loc/ar/registration", reg_timeout = 1.0, + reg_topic = "loc/ar/registration", reg_timeout = 3.0, feat_topic = "loc/ar/features", feat_timeout = 1.0, feat_threshold = 3 }, -- Handrail { id = "hr", name = "Handrail", ekf_input = 2, diff --git a/astrobee/config/localization/ros_graph_localizer.config b/astrobee/config/localization/ros_graph_localizer.config new file mode 100644 index 0000000000..a9cbdae498 --- /dev/null +++ b/astrobee/config/localization/ros_graph_localizer.config @@ -0,0 +1,44 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Ros graph localizer (rgl) +-- Measurement buffer size in between loc messages +rgl_max_relative_vio_buffer_size = 2000 +-- Max duration (s) between vl msgs, if surpassed localizer is reset to avoid VIO msg overflow issues +rgl_max_duration_between_vl_msgs = 1000 +-- Buffer max should be at least 10 and a max of 2 seconds of data +--- VIO ~15 Hz +rgl_max_graph_vio_state_buffer_size = 10 +--- Vl ~1 Hz +rgl_max_vl_matched_projections_buffer_size = 10 +rgl_max_ar_tag_matched_projections_buffer_size = 10 +--- Imu ~62.5 Hz +rgl_max_imu_buffer_size = 125 +--- Feature point ~10 Hz +rgl_max_feature_point_buffer_size = 20 +--- Depth Odometry ~5 Hz +rgl_max_depth_odom_buffer_size = 5 +-- Set depth cloud and image buffer sizes to 1 so only the latest +-- data is used for inline depth odometry +rgl_max_depth_image_buffer_size = 1 +rgl_max_depth_cloud_buffer_size = 1 +-- Others +rgl_run_depth_odometry = true +rgl_publish_depth_odometry = true +rgl_subscribe_to_depth_odometry = false +rgl_extrapolate_dock_pose_with_imu = true diff --git a/astrobee/config/localization/ros_graph_vio.config b/astrobee/config/localization/ros_graph_vio.config new file mode 100644 index 0000000000..b12360c1a4 --- /dev/null +++ b/astrobee/config/localization/ros_graph_vio.config @@ -0,0 +1,25 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Ros Graph VIO (rgv) +--- Starting state noise +rgv_starting_pose_translation_stddev = 0.00000001 +rgv_starting_pose_quaternion_stddev = 0.00000001 +rgv_starting_velocity_stddev_scale = 200 +rgv_starting_accel_bias_stddev_scale = 200 +rgv_starting_gyro_bias_stddev_scale = 200 diff --git a/astrobee/config/localization/ros_pose_extrapolator.config b/astrobee/config/localization/ros_pose_extrapolator.config new file mode 100644 index 0000000000..e91c7e1173 --- /dev/null +++ b/astrobee/config/localization/ros_pose_extrapolator.config @@ -0,0 +1,21 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Ros Pose Extrapolator +rpe_standstill_enabled = true; +rpe_max_relative_vio_buffer_size = 2000; diff --git a/astrobee/config/management/cpu_mem_monitor.config b/astrobee/config/management/cpu_mem_monitor.config new file mode 100644 index 0000000000..5004f88b14 --- /dev/null +++ b/astrobee/config/management/cpu_mem_monitor.config @@ -0,0 +1,69 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +-- frequency to check and publish cpu stats + +llp = { + update_freq_hz = 1, + temperature_scale = 0.001, + cpu_avg_load_limit = 100, + cpu_temp_limit = 100, + mem_load_limit = 100, + -- If the cpu load is high for this long, the load too high fault will be + --trigger + assert_load_high_fault_timeout_sec = 5, + clear_load_high_fault_timeout_sec = 5, + nodes = {{name="llp_cpu_mem_monitor"}, + {name="llp_disk_monitor"}, + {name="llp_gnc"}, + {name="llp_i2c"}, + {name="llp_imu"}, + {name="llp_pose_extr"}, + {name="llp_lights"}, + {name="llp_pmc"}, + {name="llp_serial"}}, +} + +mlp = { + update_freq_hz = 1, + temperature_scale = 1.0, + cpu_avg_load_limit = 100, + cpu_temp_limit = 100, + mem_load_limit = 100, + -- If the cpu load is high for this long, the load too high fault will be + --trigger + assert_load_high_fault_timeout_sec = 10, + clear_load_high_fault_timeout_sec = 10, + nodes = {{name="mlp_arm"}, + {name="mlp_communications"}, + {name="mlp_cpu_mem_monitor"}, + {name="mlp_depth_cam"}, + {name="mlp_disk_monitor"}, + {name="mlp_dock"}, + {name="mlp_graph_localization"}, + {name="mlp_localization"}, + {name="mlp_management"}, + {name="mlp_mapper"}, + {name="mlp_mobility"}, + {name="mlp_monitors"}, + {name="mlp_multibridge"}, + {name="mlp_perch"}, + {name="mlp_recording"}, + {name="mlp_serial"}, + {name="mlp_states"}, + {name="mlp_vision"}}, +} diff --git a/astrobee/config/management/cpu_monitor.config b/astrobee/config/management/cpu_monitor.config deleted file mode 100644 index cbc8231562..0000000000 --- a/astrobee/config/management/cpu_monitor.config +++ /dev/null @@ -1,40 +0,0 @@ --- Copyright (c) 2017, United States Government, as represented by the --- Administrator of the National Aeronautics and Space Administration. --- --- All rights reserved. --- --- The Astrobee platform is licensed under the Apache License, Version 2.0 --- (the "License"); you may not use this file except in compliance with the --- License. You may obtain a copy of the License at --- --- http://www.apache.org/licenses/LICENSE-2.0 --- --- Unless required by applicable law or agreed to in writing, software --- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT --- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the --- License for the specific language governing permissions and limitations --- under the License. - --- frequency to check and publish cpu stats - -llp = { - update_freq_hz = 1, - temperature_scale = 0.001, - cpu_avg_load_limit = 100, - cpu_temp_limit = 100, - -- If the cpu load is high for this long, the load too high fault will be - --trigger - assert_load_high_fault_timeout_sec = 5, - clear_load_high_fault_timeout_sec = 5 -} - -mlp = { - update_freq_hz = 1, - temperature_scale = 1.0, - cpu_avg_load_limit = 100, - cpu_temp_limit = 100, - -- If the cpu load is high for this long, the load too high fault will be - --trigger - assert_load_high_fault_timeout_sec = 10, - clear_load_high_fault_timeout_sec = 10 -} diff --git a/astrobee/config/management/data_bagger.config b/astrobee/config/management/data_bagger.config index d8319293dd..bed1f15835 100644 --- a/astrobee/config/management/data_bagger.config +++ b/astrobee/config/management/data_bagger.config @@ -26,10 +26,12 @@ startup_time_secs = 20 bags_save_directory = "/data/bags/" -bag_size_bytes = 96*1024*1024 +bag_size_bytes = 5*96*1024*1024 default_topics = {{topic="gnc/ctl/traj", downlink="immediate", frequency=-1}, {topic="gnc/ekf", downlink="immediate", frequency=-1}, + {topic="sparse_mapping/pose", downlink="immediate", frequency=-1}, + {topic="graph_loc/state", downlink="immediate", frequency=-1}, {topic="hw/eps/battery/bottom_left/state", downlink="immediate", frequency=-1}, {topic="hw/eps/battery/bottom_left/temp", downlink="immediate", frequency=-1}, {topic="hw/eps/battery/bottom_right/state", downlink="immediate", frequency=-1}, @@ -48,7 +50,11 @@ default_topics = {{topic="gnc/ctl/traj", downlink="immediate", frequency=-1}, {topic="mob/state", downlink="immediate", frequency=-1}, {topic="command", downlink="immediate", frequency=-1}, {topic="mgt/ack", downlink="immediate", frequency=-1}, - {topic="mgt/sys_monitor/time_diff", downlink="immediate", frequency=-1}, + {topic="mgt/sys_monitor/time_sync", downlink="immediate", frequency=-1}, + {topic="gs/gs_manager/ack", downlink="immediate", frequency=-1}, + {topic="gs/gs_manager/config", downlink="immediate", frequency=-1}, + {topic="gs/gs_manager/state", downlink="immediate", frequency=-1}, + {topic="rosout", downlink="immediate", frequency=-1}, {topic="robot_name", downlink="immediate", frequency=-1}} require "context" diff --git a/astrobee/config/management/executive.config b/astrobee/config/management/executive.config index 6147c6ea62..637de42b32 100644 --- a/astrobee/config/management/executive.config +++ b/astrobee/config/management/executive.config @@ -16,16 +16,18 @@ -- under the License. require "context" +require "mobility/choreographer" -- Declare action timeouts. Some are feedback and some are results. action_active_timeout = 1 led_service_available_timeout = 10 -gs_command_timeout = 4 +-- Declare guest science command timeout +gs_command_timeout = 6 -motion_feedback_timeout = 1 -arm_feedback_timeout = 4 +motion_feedback_timeout = max_replanning_attempts * replanning_wait +arm_feedback_timeout = 10 dock_result_timeout = 360 perch_result_timeout = 360 diff --git a/astrobee/config/management/fault_table.config b/astrobee/config/management/fault_table.config index bee16f88b0..3e5d19ff0e 100644 --- a/astrobee/config/management/fault_table.config +++ b/astrobee/config/management/fault_table.config @@ -21,186 +21,204 @@ require "management/fault_functions" subsystems={ {name="behaviors", nodes={ + {name="states", faults={ + {id=50, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from States", heartbeat={timeout_sec=1.1, misses=1.0}}, + }}, + {name="light_flow", faults={ + {id=53, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Light Flow", heartbeat={timeout_sec=1.1, misses=1.0}}, + }}, {name="arm", faults={ - {id=22, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Arm", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=61, warning=false, blocking=false, response=command("unloadNodelet", "arm", ""), key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, + {id=23, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Arm", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=68, warning=false, blocking=false, response=command("unloadNodelet", "arm", ""), key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, + }}, + {name="perch", faults={ + {id=54, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Perch", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, }}, {name="management", nodes={ {name="all", faults={ - {id=85, warning=false, blocking=false, response=command("noOp"), key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, + {id=92, warning=false, blocking=false, response=command("noOp"), key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, }}, {name="access_control", faults={ - {id=31, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from Access Control", heartbeat={timeout_sec=1.1, misses=3.0}}, - {id=70, warning=false, blocking=true, response=command("unloadNodelet", "access_control", ""), key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, + {id=32, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from Access Control", heartbeat={timeout_sec=1.1, misses=3.0}}, + {id=77, warning=false, blocking=true, response=command("unloadNodelet", "access_control", ""), key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, }}, {name="executive", faults={ - {id=29, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Executive", heartbeat={timeout_sec=2.1, misses=2.0}}, - {id=68, warning=false, blocking=false, response=command("unloadNodelet", "executive", ""), key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, + {id=30, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Executive", heartbeat={timeout_sec=2.1, misses=2.0}}, + {id=75, warning=false, blocking=false, response=command("unloadNodelet", "executive", ""), key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, }}, {name="data_bagger", faults={ - {id=47, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Data Bagger", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=83, warning=false, blocking=false, response=command("unloadNodelet", "data_bagger", ""), key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, + {id=48, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Data Bagger", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=90, warning=false, blocking=false, response=command("unloadNodelet", "data_bagger", ""), key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, }}, - {name="llp_cpu_monitor", faults={ - {id=33, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from LLP CPU Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=72, warning=false, blocking=true, response=command("unloadNodelet", "llp_cpu_monitor", ""), key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"}, - {id=86, warning=true, blocking=true, response=command("stopAllMotion"), key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, - {id=97, warning=true, blocking=false, response=command("noOp"), key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, - {id=100, warning=true, blocking=true, response=command("stopAllMotion"), key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, + {name="llp_cpu_mem_monitor", faults={ + {id=34, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from LLP CPU Memory Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=79, warning=false, blocking=true, response=command("unloadNodelet", "llp_cpu_mem_monitor", ""), key="INITIALIZATION_FAILED", description="LLP CPU Memory Monitor Initialization Failed"}, + {id=93, warning=true, blocking=true, response=command("stopAllMotion"), key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, + {id=104, warning=true, blocking=false, response=command("noOp"), key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, + {id=107, warning=true, blocking=true, response=command("stopAllMotion"), key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, }}, {name="llp_disk_monitor", faults={ - {id=36, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from LLP Disk Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=75, warning=false, blocking=true, response=command("unloadNodelet", "llp_disk_monitor", ""), key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, - {id=89, warning=true, blocking=false, response=command("noOp"), key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, - {id=91, warning=true, blocking=false, response=command("noOp"), key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, - {id=94, warning=false, blocking=true, response=command("fault"), key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, + {id=37, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from LLP Disk Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=82, warning=false, blocking=true, response=command("unloadNodelet", "llp_disk_monitor", ""), key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, + {id=96, warning=true, blocking=false, response=command("noOp"), key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, + {id=98, warning=true, blocking=false, response=command("noOp"), key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, + {id=101, warning=false, blocking=true, response=command("fault"), key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, + }}, + {name="mlp_cpu_mem_monitor", faults={ + {id=35, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from MLP CPU Memory Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=80, warning=false, blocking=true, response=command("unloadNodelet", "mlp_cpu_mem_monitor", ""), key="INITIALIZATION_FAILED", description="MLP CPU Memory Monitor Initialization Failed"}, + {id=94, warning=true, blocking=true, response=command("stopAllMotion"), key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, + {id=105, warning=true, blocking=false, response=command("noOp"), key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, + {id=108, warning=true, blocking=true, response=command("stopAllMotion"), key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, }}, {name="image_sampler", faults={ - {id=42, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Image Sampler", heartbeat={timeout_sec=1.1, misses=5.0}}, - {id=81, warning=false, blocking=false, response=command("unloadNodelet", "image_sampler", ""), key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, + {id=43, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Image Sampler", heartbeat={timeout_sec=1.1, misses=5.0}}, + {id=88, warning=false, blocking=false, response=command("unloadNodelet", "image_sampler", ""), key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, }}, {name="mlp_disk_monitor", faults={ - {id=37, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from MLP Disk Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=76, warning=false, blocking=true, response=command("unloadNodelet", "mlp_disk_monitor", ""), key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, - {id=90, warning=true, blocking=false, response=command("noOp"), key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, - {id=92, warning=true, blocking=false, response=command("noOp"), key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, - {id=95, warning=false, blocking=true, response=command("fault"), key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, - }}, - {name="mlp_cpu_monitor", faults={ - {id=34, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from MLP CPU Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=73, warning=false, blocking=true, response=command("unloadNodelet", "mlp_cpu_monitor", ""), key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"}, - {id=87, warning=true, blocking=true, response=command("stopAllMotion"), key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, - {id=98, warning=true, blocking=false, response=command("noOp"), key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, - {id=101, warning=true, blocking=true, response=command("stopAllMotion"), key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, + {id=38, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from MLP Disk Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=83, warning=false, blocking=true, response=command("unloadNodelet", "mlp_disk_monitor", ""), key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, + {id=97, warning=true, blocking=false, response=command("noOp"), key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, + {id=99, warning=true, blocking=false, response=command("noOp"), key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, + {id=102, warning=false, blocking=true, response=command("fault"), key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, }}, }}, {name="avionics", nodes={ {name="sys_monitor", faults={ - {id=105, warning=true, blocking=false, response=command("noOp"), key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, + {id=112, warning=true, blocking=false, response=command("noOp"), key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, }}, }}, {name="localization", nodes={ {name="optical_flow_nodelet", faults={ - {id=27, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from VisualOdometer", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=28, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from VisualOdometer", heartbeat={timeout_sec=1.1, misses=2.0}}, }}, {name="marker_tracking", faults={ - {id=25, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from DockLoc (ar tags)", heartbeat={timeout_sec=1.1, misses=4.0}}, + {id=26, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from DockLoc (ar tags)", heartbeat={timeout_sec=1.1, misses=4.0}}, }}, {name="localization_node", faults={ - {id=24, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from NavLoc (sparse mapping)", heartbeat={timeout_sec=1.1, misses=4.0}}, + {id=25, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from NavLoc (sparse mapping)", heartbeat={timeout_sec=1.1, misses=4.0}}, }}, {name="handrail_detect", faults={ - {id=26, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from PerchLoc (handrail detect)", heartbeat={timeout_sec=1.1, misses=4.0}}, + {id=27, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from PerchLoc (handrail detect)", heartbeat={timeout_sec=1.1, misses=4.0}}, }}, {name="localization_manager", faults={ - {id=23, warning=false, blocking=true, response=command("idlePropulsion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Localization Manager", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=62, warning=false, blocking=true, response=command("unloadNodelet", "localization_manager", ""), key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, - {id=117, warning=false, blocking=false, response=command("stopAllMotion"), key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, - {id=119, warning=false, blocking=true, response=command("idlePropulsion"), key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, + {id=24, warning=false, blocking=true, response=command("idlePropulsion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Localization Manager", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=69, warning=false, blocking=true, response=command("unloadNodelet", "localization_manager", ""), key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, + {id=124, warning=false, blocking=false, response=command("stopAllMotion"), key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, + {id=126, warning=false, blocking=true, response=command("idlePropulsion"), key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, }}, }}, {name="communication", nodes={ + {name="comms_bridge", faults={ + {id=51, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Comms Bridge", heartbeat={timeout_sec=1.1, misses=1.0}}, + }}, {name="dds_ros_bridge", faults={ - {id=32, warning=false, blocking=false, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat form DDS ROS Bridge", heartbeat={timeout_sec=1.1, misses=5.0}}, - {id=71, warning=false, blocking=false, response=command("unloadNodelet", "dds_ros_bridge", ""), key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, + {id=33, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat form DDS ROS Bridge", heartbeat={timeout_sec=1.1, misses=5.0}}, + {id=78, warning=false, blocking=true, response=command("unloadNodelet", "dds_ros_bridge", ""), key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, }}, }}, {name="mobility", nodes={ - --{name="mapper", faults={ - --{id=41, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Mapper", heartbeat={timeout_sec=1.1, misses=2.0}}, - --{id=80, warning=false, blocking=false, response=command("unloadNodelet", "mapper", ""), key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, - --}}, + {name="mapper", faults={ + {id=42, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Mapper", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=87, warning=false, blocking=false, response=command("unloadNodelet", "mapper", ""), key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, + }}, {name="planner_qp", faults={ - {id=39, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from QP Planner", heartbeat={timeout_sec=1.1, misses=5.0}}, - {id=78, warning=false, blocking=false, response=command("unloadNodelet", "planner_qp", ""), key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, + {id=40, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from QP Planner", heartbeat={timeout_sec=1.1, misses=5.0}}, + {id=85, warning=false, blocking=false, response=command("unloadNodelet", "planner_qp", ""), key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, }}, {name="planner_trapezoidal", faults={ - {id=40, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Trapezoidal Planner", heartbeat={timeout_sec=1.1, misses=5.0}}, - {id=79, warning=false, blocking=false, response=command("unloadNodelet", "planner_trapezoidal", ""), key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, + {id=41, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Trapezoidal Planner", heartbeat={timeout_sec=1.1, misses=5.0}}, + {id=86, warning=false, blocking=false, response=command("unloadNodelet", "planner_trapezoidal", ""), key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, }}, {name="choreographer", faults={ - {id=28, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Choreographer", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=67, warning=false, blocking=false, response=command("unloadNodelet", "choreographer", ""), key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, + {id=29, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Choreographer", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=74, warning=false, blocking=false, response=command("unloadNodelet", "choreographer", ""), key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, }}, }}, {name="hardware", nodes={ {name="laser", faults={ - {id=44, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Laser", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=45, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Laser", heartbeat={timeout_sec=1.1, misses=1.0}}, + }}, + {name="signal_lights", faults={ + {id=52, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Signal Lights", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="eps_driver", faults={ {id=11, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from EPS", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="perching_arm", faults={ - {id=21, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Perching Arm", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=60, warning=false, blocking=false, response=command("unloadNodelet", "perching_arm", ""), key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, + {id=22, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Perching Arm", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=67, warning=false, blocking=false, response=command("unloadNodelet", "perching_arm", ""), key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, }}, {name="framestore", faults={ - {id=43, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from FrameStore", heartbeat={timeout_sec=1.1, misses=3.0}}, - {id=82, warning=false, blocking=false, response=command("unloadNodelet", "framestore", ""), key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, + {id=44, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from FrameStore", heartbeat={timeout_sec=1.1, misses=3.0}}, + {id=89, warning=false, blocking=false, response=command("unloadNodelet", "framestore", ""), key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, }}, {name="pmc_actuator", faults={ {id=13, warning=false, blocking=false, response=command("idlePropulsion"), key="HEARTBEAT_MISSING", description="No Heartbeat from PMC Actuator", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="flashlight_front", faults={ - {id=45, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Front Flashlight", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=46, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Front Flashlight", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="nav_cam", faults={ - {id=18, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Navcam", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=19, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Navcam", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="flashlight_aft", faults={ - {id=46, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Aft Flashlight", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=47, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Aft Flashlight", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="pico_driver", faults={ - {id=20, warning=false, blocking=false, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from PicoFlexx", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=59, warning=false, blocking=false, response=command("unloadNodelet", "pico_driver", ""), key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, - {id=124, warning=false, blocking=false, response=command("stop"), key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, + {id=21, warning=false, blocking=false, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from PicoFlexx", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=66, warning=false, blocking=false, response=command("unloadNodelet", "pico_driver", ""), key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, + {id=131, warning=false, blocking=false, response=command("stop"), key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, }}, {name="dock_cam", faults={ - {id=19, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Dockcam", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=20, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Dockcam", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, }}, {name="gnc", nodes={ {name="ctl", faults={ {id=14, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Control", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=53, warning=false, blocking=false, response=command("unloadNodelet", "ctl", ""), key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, - {id=133, warning=false, blocking=false, response=command("noOp"), key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, + {id=59, warning=false, blocking=false, response=command("unloadNodelet", "ctl", ""), key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, + {id=140, warning=false, blocking=false, response=command("noOp"), key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, + }}, + {name="pose_extr", faults={ + {id=16, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from IMU Augmentor", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=61, warning=false, blocking=true, response=command("unloadNodelet", "pose_extr", ""), key="INITIALIZATION_FAILED", description="Pose Extrapolator Initialization Failed"}, }}, - {name="ekf", faults={ - {id=15, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from EKF", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=54, warning=false, blocking=false, response=command("unloadNodelet", "ekf", ""), key="INITIALIZATION_FAILED", description="EKF Initialization Failed"}, - {id=134, warning=true, blocking=false, response=command("noOp"), key="CONTROL_CYCLE_OVERRUN", description="Control Cycle Overrun"}, - {id=136, warning=false, blocking=false, response=command("noOp"), key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, + {name="graph_loc", faults={ + {id=15, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Graph Localizer", heartbeat={timeout_sec=3.1, misses=1.0}}, + {id=60, warning=false, blocking=true, response=command("unloadNodelet", "graph_loc", ""), key="INITIALIZATION_FAILED", description="Graph Localizer Initialization Failed"}, + {id=142, warning=false, blocking=false, response=command("noOp"), key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, }}, {name="fam", faults={ - {id=16, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from FAM", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=55, warning=false, blocking=false, response=command("unloadNodelet", "fam", ""), key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, + {id=17, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from FAM", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=62, warning=false, blocking=false, response=command("unloadNodelet", "fam", ""), key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, }}, }}, {name="speed camera", nodes={ {name="speed_cam", faults={ {id=10, warning=false, blocking=false, response=command("idlePropulsion"), key="VELOCITY_TOO_HIGH", description="Velocity too high"}, - {id=17, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Speedcam", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=56, warning=false, blocking=true, response=command("unloadNodelet", "speed_cam", ""), key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, - {id=126, warning=false, blocking=true, response=command("idlePropulsion"), key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, + {id=18, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Speedcam", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=63, warning=false, blocking=true, response=command("unloadNodelet", "speed_cam", ""), key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, + {id=133, warning=false, blocking=true, response=command("idlePropulsion"), key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, }}, }}, {name="imu", nodes={ {name="epson_imu", faults={ {id=12, warning=false, blocking=false, response=command("idlePropulsion"), key="HEARTBEAT_MISSING", description="No Heartbeat from IMU", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=112, warning=false, blocking=true, response=command("idlePropulsion"), key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, - {id=145, warning=true, blocking=false, response=command("noOp"), key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, - {id=146, warning=false, blocking=true, response=command("idlePropulsion"), key="IMU_OVERTEMP", description="IMU overtemp"}, - {id=147, warning=false, blocking=true, response=command("fault"), key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, - {id=148, warning=false, blocking=true, response=command("fault"), key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, - {id=150, warning=true, blocking=false, response=command("noOp"), key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, - {id=151, warning=false, blocking=true, response=command("idlePropulsion"), key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, + {id=119, warning=false, blocking=true, response=command("idlePropulsion"), key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, + {id=151, warning=true, blocking=false, response=command("noOp"), key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, + {id=152, warning=false, blocking=true, response=command("idlePropulsion"), key="IMU_OVERTEMP", description="IMU overtemp"}, + {id=153, warning=false, blocking=true, response=command("fault"), key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, + {id=154, warning=false, blocking=true, response=command("fault"), key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, + {id=156, warning=true, blocking=false, response=command("noOp"), key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, + {id=157, warning=false, blocking=true, response=command("idlePropulsion"), key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, }}, }}, {name="procedures", nodes={ {name="dock", faults={ - {id=48, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Docker", heartbeat={timeout_sec=1.1, misses=4.0}}, - {id=84, warning=false, blocking=false, response=command("unloadNodelet", "dock", ""), key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, + {id=49, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Docker", heartbeat={timeout_sec=1.1, misses=4.0}}, + {id=91, warning=false, blocking=false, response=command("unloadNodelet", "dock", ""), key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, }}, }}, } diff --git a/astrobee/config/management/sys_monitor.config b/astrobee/config/management/sys_monitor.config index 116582c91c..581c44c26a 100644 --- a/astrobee/config/management/sys_monitor.config +++ b/astrobee/config/management/sys_monitor.config @@ -24,52 +24,62 @@ require "context" startup_time_sec = 30 - +reload_nodelet_timeout_sec = 8 heartbeat_pub_rate_sec = 5 -- Name of node used to calculate time difference between the mlp and the llp. -- The node specified should be running on the llp as the system monitor runs -- on the mlp -time_diff_node = "ekf" +time_diff_node = "pose_extr" -- Threshold used to trigger time drift fault time_drift_thres_sec = 0.25 -- Types of nodelets. Used for loading nodelets. -nodelet_types={ - {name="access_control", type="access_control/AccessControl"}, - {name="executive", type="executive/Executive"}, - {name="llp_cpu_monitor", type="cpu_monitor/CpuMonitor"}, - {name="mlp_cpu_monitor", type="cpu_monitor/CpuMonitor"}, - {name="llp_disk_monitor", type="disk_monitor/DiskMonitor"}, - {name="mlp_disk_monitor", type="disk_monitor/DiskMonitor"}, - {name="image_sampler", type="image_sampler/ImageSampler"}, - {name="mapper", type="mapper/MapperNodelet"}, - {name="planner_qp", type="planner_qp/Planner"}, - {name="choreographer", type="choreographer/ChoreographerNodelet"}, - {name="planner_trapezoidal", type="planner_trapezoidal/PlannerTrapezoidalNodelet"}, - {name="dds_ros_bridge", type="dds_ros_bridge/DdsRosBridge"}, - {name="optical_flow_nodelet", type="lk_optical_flow_node/LKOpticalFlowNodelet"}, - {name="localization_node", type="localization_node/LocalizationNodelet"}, - {name="marker_tracking", type="marker_tracking_node/MarkerTrackingNodelet"}, - {name="handrail_detect", type="handrail_detect/HandrailDetect"}, - {name="localization_manager", type="localization_manager/LocalizationManagerNodelet"}, - {name="laser", type="laser/laserNodelet"}, - {name="epson_imu", type="epson_imu/EpsonImuNodelet"}, - {name="eps_driver", type="eps_driver/EpsDriverNode"}, - {name="framestore", type="description/FrameStore"}, - {name="pmc_actuator", type="pmc_actuator/PmcActuatorNodelet"}, - {name="flashlight_aft", type="flashlight/FlashlightNodelet"}, - {name="flashlight_front", type="flashlight/FlashlightNodelet"}, - {name="dock_cam", type="is_camera/CameraNodelet"}, - {name="nav_cam", type="is_camera/CameraNodelet"}, - {name="speed_cam", type="speed_cam/SpeedCamNode"}, - {name="pico_driver", type="pico_driver/PicoDriverNodelet"}, - {name="ctl", type="ctl/CtlNodelet"}, - {name="ekf", type="ekf/EkfNodelet"}, - {name="fam", type="fam/FamNodelet"}, - {name="arm", type="arm/ArmNodelet"}, - {name="dock", type="dock/DockNodelet"}} +-- Not listed nodes are calibration_dock_cam, calibration_imu, +-- calibration_nav_cam, ground_truth_localizer, and vive +nodelet_info={ + {name="access_control", manager="mlp_management", type="access_control/AccessControl"}, + {name="arm", manager="mlp_arm", type="arm/ArmNodelet"}, + {name="choreographer", manager="mlp_mobility", type="choreographer/ChoreographerNodelet"}, + {name="comms_bridge", manager="mlp_multibridge", type="comms_bridge/CommsBridgeNodelet"}, + {name="ctl", manager="llp_gnc", type="ctl/ctl"}, + {name="data_bagger", manager="mlp_recording", type="data_bagger/DataBagger"}, + {name="dds_ros_bridge", manager="mlp_communications", type="dds_ros_bridge/DdsRosBridge"}, + {name="dock", manager="mlp_dock", type="dock/DockNodelet"}, + {name="dock_cam", manager="mlp_vision", type="is_camera/camera"}, + {name="eps_driver", manager="llp_i2c", type="eps_driver/EpsDriverNode"}, + {name="epson_imu", manager="llp_imu", type="epson_imu/EpsonImuNodelet"}, + {name="executive", manager="mlp_management", type="executive/Executive"}, + {name="fam", manager="llp_gnc", type="fam/fam"}, + {name="flashlight_aft", manager="llp_i2c", type="flashlight/FlashlightNodelet"}, + {name="flashlight_front", manager="llp_i2c", type="flashlight/FlashlightNodelet"}, + {name="framestore", manager="mlp_mobility", type="mobility/FrameStore"}, + {name="graph_loc", manager="mlp_graph_localization", type="ros_graph_localizer/RosGraphLocalizerNodelet"}, + {name="handrail_detect", manager="mlp_depth_cam", type="handrail_detect/HandrailDetect"}, + {name="image_sampler", manager="mlp_localization", type="image_sampler/ImageSampler"}, + {name="pose_extr", manager="llp_pose_extr", type="ros_pose_extrapolator/RosPoseExtrapolatorNodelet"}, + {name="laser", manager="llp_i2c", type="laser/LaserNodelet"}, + {name="light_flow", manager="llp_lights", type="light_flow/LightFlowNodelet"}, + {name="llp_cpu_mem_monitor", manager="llp_monitors", type="cpu_mem_monitor/CpuMemMonitor"}, + {name="llp_disk_monitor", manager="llp_monitors", type="disk_monitor/DiskMonitor"}, + {name="localization_manager", manager="mlp_localization", type="localization_manager/LocalizationManagerNodelet"}, + {name="localization_node", manager="mlp_vision", type="localization_node/LocalizationNodelet"}, + {name="mapper", manager="mlp_mapper", type="mapper/MapperNodelet"}, + {name="marker_tracking", manager="mlp_vision", type="marker_tracking_node/MarkerTrackingNodelet"}, + {name="mlp_cpu_mem_monitor", manager="mlp_monitors", type="cpu_mem_monitor/CpuMemMonitor"}, + {name="mlp_disk_monitor", manager="mlp_monitors", type="disk_monitor/DiskMonitor"}, + {name="nav_cam", manager="mlp_vision", type="is_camera/camera"}, + {name="optical_flow_nodelet", manager="mlp_vision", type="lk_optical_flow_node/LKOpticalFlowNodelet"}, + {name="perch", manager="mlp_perch", type="perch/PerchNodelet"}, + {name="perching_arm", manager="mlp_serial", type="perching_arm/PerchingArmNode"}, + {name="pico_driver", manager="mlp_depth_cam", type="pico_driver/PicoDriverNodelet"}, + {name="planner_qp", manager="mlp_depth_cam", type="planner_qp/Planner"}, + {name="planner_trapezoidal", manager="mlp_mobility", type="planner_trapezoidal/PlannerTrapezoidalNodelet"}, + {name="pmc_actuator", manager="llp_pmc", type="pmc_actuator/PmcActuatorNodelet"}, + {name="signal_lights", manager="llp_lights", type="signal_lights/SignalLightsNodelet"}, + {name="speed_cam", manager="llp_serial", type="speed_cam/SpeedCamNode"}, + {name="states", manager="mlp_states", type="states/StatesNodelet"}} -- Keep constant with Command Args message file diff --git a/astrobee/config/mobility/choreographer.config b/astrobee/config/mobility/choreographer.config index e5aabaa369..c83d965af5 100644 --- a/astrobee/config/mobility/choreographer.config +++ b/astrobee/config/mobility/choreographer.config @@ -17,6 +17,9 @@ require "context" +max_replanning_attempts = 10 +replanning_wait = 1.0 + parameters = { -- PLANNING { @@ -88,8 +91,12 @@ parameters = { },{ -- See above. id = "max_replanning_attempts", reconfigurable = true, type = "integer", - default = 10, min = 1, max = 999, unit = "unitless", + default = max_replanning_attempts, min = 1, max = 999, unit = "unitless", description = "The maximum amount of replanning attempts" + },{ + id = "replanning_wait", reconfigurable = true, type = "double", + default = replanning_wait, min = 0.0, max = 3.0, unit = "seconds", + description = "Wait time in between replanning attempts" }, -- DISCRETE TIME UNIT { @@ -223,7 +230,7 @@ parameters = { description = "Allow new zones to overwrite the zone file" },{ id = "tolerance_max_time", reconfigurable = false, type = "double", - default = 1.0, unit = "seconds", + default = 3.0, unit = "seconds", description = "Maximum time a tolerance is acceptable" } } diff --git a/astrobee/config/mobility/mapper.config b/astrobee/config/mobility/mapper.config index c2e13cc38b..7732d1ebcb 100644 --- a/astrobee/config/mobility/mapper.config +++ b/astrobee/config/mobility/mapper.config @@ -18,6 +18,13 @@ -- Declare the variables above as reconfigurable parameters = { { + id = "disable_mapper", + reconfigurable = true, + type = "boolean", + default = true, + unit = "boolean", + description = "Should we disable the mapper?" + },{ id = "enable_obstacles", reconfigurable = true, type = "boolean", @@ -75,14 +82,23 @@ parameters = { unit = "seconds", description = "How long the octomap remembers the fading memory map. It remembers forever when this variable is <= 0." },{ - id = "inflate_radius", + id = "robot_radius", reconfigurable = true, type = "double", - default = 0.25, + default = 0.28, + min = 0.0, + max = 1.0, + description = "Radius of the robot considered in the planner (default is 0.16 * sqrt(3))", + unit = "m" + },{ + id = "collision_distance", + reconfigurable = true, + type = "double", + default = 0.02, min = 0.01, max = 1, unit = "meters", - description = "How much the octomap is inflated." + description = "Minimum distance margin to maintain away from obstacles." },{ id = "cam_fov", reconfigurable = true, @@ -174,12 +190,12 @@ parameters = { unit = "hertz", description = "Frequency at which the collision checker runs." },{ - id = "tf_update_rate", + id = "octomap_update_rate", reconfigurable = true, type = "double", - default = 10, - min = 1, - max = 100, + default = 1, + min = 0.2, + max = 5, unit = "hertz", description = "Frequency at which the tf listeners update tf data." },{ diff --git a/astrobee/config/mobility/planner_qp.config b/astrobee/config/mobility/planner_qp.config index 31e93a38d4..6899861617 100644 --- a/astrobee/config/mobility/planner_qp.config +++ b/astrobee/config/mobility/planner_qp.config @@ -35,15 +35,6 @@ parameters = { max = 100, description = "Rate at which control state commands are created in plan", unit = "Hz" - }, { - id = "robot_radius", - reconfigurable = true, - type = "double", - default = 0.16, - min = 0.0, - max = 1.0, - description = "Radius of the robot considered in the planner (default is 0.15 * 2* sqrt(3))", - unit = "m" }, { id = "two_d", reconfigurable = true, @@ -92,5 +83,14 @@ parameters = { max = 10.0, description = "loop time for trajectory optimization replay", unit = "seconds" + }, { + id = "planner_distance", + reconfigurable = true, + type = "double", + default = 0.08, + min = 0.0, + max = 10.0, + description = "extra planner keepout distance, min is map tolerance", + unit = "seconds" } } diff --git a/astrobee/config/robots/bsharp.config b/astrobee/config/robots/bsharp.config index 2a83789f68..191cfbd2bb 100644 --- a/astrobee/config/robots/bsharp.config +++ b/astrobee/config/robots/bsharp.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Calibrated transforms -- nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(-0.46938154, -0.52978318, -0.5317378, -0.46504373)), @@ -37,7 +42,9 @@ robot_geometry = { haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ), dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), - imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) + imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -91,7 +98,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/bsharp2.config b/astrobee/config/robots/bsharp2.config index cda2bdee55..2d04ee189b 100644 --- a/astrobee/config/robots/bsharp2.config +++ b/astrobee/config/robots/bsharp2.config @@ -34,30 +34,30 @@ robot_geometry = { -- dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.54029868, -0.45297017, -0.47464308, 0.52688644)), -- imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)), - hazcam_to_navcam_transform = transform(vec3(0.08017845188659993, 0.0015130070850712233, -0.0037397072731650606), quat4(-0.010795173083773568, -0.020686270289023867, 0.99972637753705862, -0.0016469718428887884)), - scicam_to_hazcam_transform = transform(vec3(0.039812511450892832, 0.017762378646844504, -0.0039477526753304197), quat4(-0.0017211102261497006, -0.023078433050076921, 0.99972939958575946, -0.0023561221189618764)), - navcam_to_hazcam_timestamp_offset = -0.02, - scicam_to_hazcam_timestamp_offset = -0.31, - hazcam_depth_to_image_transform = { - 0.9672, 0.0017, 0.0111, -0.0060, - -0.0016, 0.9672, -0.0065, 0.0040, - -0.0112, 0.0065, 0.9672, -0.0133, - 0.0, 0.0, 0.0, 1.0}, + nav_cam_to_haz_cam_transform = transform(vec3(0.079350378, 0.0020932693, -0.025811147), quat4(0.0045827434, -0.00067115432, 0.99998583, 0.0026256072)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.041219515, 0.019857798, 0.015677018), quat4(0.0019155044, 0.00092629811, -0.0019321095, 0.99999587)), + haz_cam_depth_to_image_transform = { + 0.94713911, 0.00063710851, 0.0043044568, 0.0084247659, + -0.00057512557, 0.94311399, 0.032836596, 0.00075736623, + -0.016608396, -0.036455481, 0.95435862, -0.026870438, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)), -- placeholder, not valid! haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ), dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), - imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) + imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { nav_cam = { - distortion_coeff = 0.982597, + distortion_coeff = 1.0092039, intrinsic_matrix = { - 607.592, 0.0, 589.65385, - 0.0, 607.72719, 498.22211, + 621.04422, 0.0, 580.56427, + 0.0, 621.04422, 495.51236, 0.0, 0.0, 1.0 }, gain=100, @@ -84,25 +84,27 @@ nav_cam = { exposure=150 }, haz_cam = { - distortion_coeff = {-0.266289, -0.0723862, 0.0026379, -0.0037575}, + distortion_coeff = {-0.34478639, 0.029176425, 0.0061109035, 0.0011360463}, intrinsic_matrix = { - 214.5571, 0.0, 110.90497, - 0.0, 213.63798, 90.388202, + 215.92492, 0.0, 108.00909, + 0.0, 215.92492, 82.795963, 0.0, 0.0, 1.0 }, gain=100, exposure=150 }, sci_cam = { - distortion_coeff = {0.0582312, -0.0757094, 0.00166962, -0.0025029}, + distortion_coeff = {-0.011275619, 0.049569258, -0.00085412642, 0.0016215327}, intrinsic_matrix = { - 1021.0608, 0.0, 661.64049, - 0.0, 1020.7, 520.01707, + 1056.7224, 0.0, 668.60496, + 0.0, 1056.7224, 507.31654, 0.0, 0.0, 1.0 }, gain=100, exposure=150 }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = -0.01 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/bumble.config b/astrobee/config/robots/bumble.config index 7f6784240b..6d88929620 100644 --- a/astrobee/config/robots/bumble.config +++ b/astrobee/config/robots/bumble.config @@ -15,7 +15,7 @@ -- License for the specific language governing permissions and limitations -- under the License. --- bumblebee config +-- bumble config robot_llp_address = "10.42.0.8" robot_mlp_address = "10.42.0.9" @@ -25,24 +25,23 @@ robot_i2c_bus = "/dev/i2c-1" robot_imu_drdy_pin = 4 robot_geometry = { --- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.08017845188659993, 0.0015130070850712233, -0.0037397072731650606), quat4(-0.010795173083773568, -0.020686270289023867, 0.99972637753705862, -0.0016469718428887884)), - scicam_to_hazcam_transform = transform(vec3(0.039812511450892832, 0.017762378646844504, -0.0039477526753304197), quat4(-0.0017211102261497006, -0.023078433050076921, 0.99972939958575946, -0.0023561221189618764)), - navcam_to_hazcam_timestamp_offset = -0.02, - scicam_to_hazcam_timestamp_offset = -0.31, - hazcam_depth_to_image_transform = { - 0.9672, 0.0017, 0.0111, -0.0060, - -0.0016, 0.9672, -0.0065, 0.0040, - -0.0112, 0.0065, 0.9672, -0.0133, - 0.0, 0.0, 0.0, 1.0}, + nav_cam_to_haz_cam_transform = transform(vec3(0.072789474, 0.0010512418, -0.048805003), quat4(-0.00014444836, 0.020741299, 0.99976044, 0.006988339)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.035350038, 0.019131216, 0.0083921009), quat4(0.0007718104, -0.0043221056, -0.0069533111, 0.99996619)), + haz_cam_depth_to_image_transform = { + 0.91037857, 8.4134405e-05, 0.0050848035, 0.0020508297, + -0.0003594422, 0.9113776, 0.023508033, -0.0011003268, + -0.0044235394, -0.018620972, 0.88773717, -0.028333293, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ), dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), - --imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) - imu_transform = transform(vec3(0.0386, 0.0247, -0.01016), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) + --imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + imu_transform = transform(vec3(0.0386, 0.0247, -0.01016), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + -- Not accurate, only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -77,26 +76,27 @@ robot_camera_calibrations = { exposure=150 }, haz_cam = { - distortion_coeff = {-0.411106, 0.375288, -0.00080993, -0.00817328}, + distortion_coeff = {-0.259498, -0.08484934, 0.0032980311, -0.00024045673}, intrinsic_matrix = { - 188.27394, 0.0, 117.94881, - 0.0, 186.18359, 87.740547, + 206.19095, 0.0, 112.48999, + 0.0, 206.19095, 81.216598, 0.0, 0.0, 1.0 }, gain=50, exposure=150 }, --- Placeholder for sci_cam, not accurate! sci_cam = { - distortion_coeff = {0.128628, -0.167456, 0.00213421, -0.00262211}, + distortion_coeff = {-0.025598438, 0.048258987, -0.00041380657, 0.0056673533}, intrinsic_matrix = { - 859.44015, 0.0, 754.24485, - 0.0, 849.35466, 487.7349, + 1023.6054, 0.0, 683.97547, + 0.0, 1023.6054, 511.2185, 0.0, 0.0, 1.0 }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.08 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/flatsat.config b/astrobee/config/robots/flatsat.config index eac5e2b7f1..6be2ca7292 100644 --- a/astrobee/config/robots/flatsat.config +++ b/astrobee/config/robots/flatsat.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.1375, 0.055, -0.03), quat4(0, -0.70711, 0, 0.70711)), haz_cam_transform = transform(vec3(0, 0, 0), quat4(0, 0, 0, 1)), -- placeholder, not valid! @@ -38,7 +43,9 @@ robot_geometry = { -- body to nav cam nav_cam_transform = transform(vec3(0.153069, -0.0534689, -0.0516773), quat4(0.471123, 0.530379, 0.528796, 0.465956)), dock_cam_transform = transform(vec3(-0.127825, -0.0993965, 0.0249159), quat4(0.530088, -0.467269, -0.469412, 0.529451)), - imu_transform = transform(vec3(0.014, 0.016, -0.0319625), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)) + imu_transform = transform(vec3(0.014, 0.016, -0.0319625), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -94,7 +101,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- robot_perch_cam_device = "0004-3608-0024-1107" diff --git a/astrobee/config/robots/honey.config b/astrobee/config/robots/honey.config index ec63b5ab5c..5c22798c94 100644 --- a/astrobee/config/robots/honey.config +++ b/astrobee/config/robots/honey.config @@ -26,27 +26,34 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ), dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), - imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) + imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { nav_cam = { - distortion_coeff = 0.99872, + distortion_coeff = 0.998757, intrinsic_matrix = { - 607.64218, 0.0, 624.85552, - 0.0, 606.53899, 514.57737, + 610.12594, 0.0, 628.35064, + 0.0, 610.12594, 514.89592, 0.0, 0.0, 1.0 }, - gain=100, - exposure=150 + gain=50, + exposure=175 }, dock_cam = { distortion_coeff = 1.00882, @@ -90,7 +97,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/killer.config b/astrobee/config/robots/killer.config index 3299f697ac..b983162f8a 100644 --- a/astrobee/config/robots/killer.config +++ b/astrobee/config/robots/killer.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.1375, 0.055, -0.03), quat4(0, -0.70711, 0, 0.70711)), haz_cam_transform = transform(vec3(0, 0, 0), quat4(0, 0, 0, 1)), -- placeholder, not valid! @@ -38,7 +43,9 @@ robot_geometry = { -- body to nav cam nav_cam_transform = transform(vec3(0.153069, -0.0534689, -0.0516773), quat4(0.471123, 0.530379, 0.528796, 0.465956)), dock_cam_transform = transform(vec3(-0.127825, -0.0993965, 0.0249159), quat4(0.530088, -0.467269, -0.469412, 0.529451)), - imu_transform = transform(vec3(0.014, 0.016, -0.0319625), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)) + imu_transform = transform(vec3(0.014, 0.016, -0.0319625), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -94,7 +101,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } robot_perch_cam_device = "0005-4805-0050-1520" diff --git a/astrobee/config/robots/melissa.config b/astrobee/config/robots/melissa.config index 6a74e08e18..04d4dd5ce1 100644 --- a/astrobee/config/robots/melissa.config +++ b/astrobee/config/robots/melissa.config @@ -26,15 +26,22 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ), dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), - imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) + imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -88,7 +95,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/p4c.config b/astrobee/config/robots/p4c.config index 9fabfab7a5..ba6adeb600 100644 --- a/astrobee/config/robots/p4c.config +++ b/astrobee/config/robots/p4c.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 101 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.1375, 0.055, -0.03), quat4(0, -0.70711, 0, 0.70711)), haz_cam_transform = transform(vec3(0, 0, 0), quat4(0, 0, 0, 1)), -- placeholder, not valid! @@ -34,7 +39,9 @@ robot_geometry = { nav_cam_transform = transform(vec3(0.1145, 0.0536, 0.1016), quat4(0.4831, 0.5302, 0.5334, 0.4484)), dock_cam_transform = transform(vec3(-0.12094, -0.05715, -0.01905), quat4(-0.5, -0.5, 0.5, 0.5)), -- translation: [0.0100375 -0.0114875 -0.0367625] euler angles: [7.318125 -0.243125 0.879375] degrees - imu_transform = transform(vec3(0.0177375, 0.0219125, -0.0319625), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)) + imu_transform = transform(vec3(0.0177375, 0.0219125, -0.0319625), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -90,7 +97,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/p4d.config b/astrobee/config/robots/p4d.config index 25439572f1..127d96dc9d 100644 --- a/astrobee/config/robots/p4d.config +++ b/astrobee/config/robots/p4d.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 101 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.17520466, 0.040140345, -0.049281679), quat4(0.027207383, -0.71130843, 0.013260087, 0.70222806)), haz_cam_transform = transform(vec3(0.092136082, 0.041911327, -0.11237338), quat4(-0.51485943, 0.49197658, -0.47825571, 0.51395553)), -- placeholder, not valid! @@ -38,7 +43,9 @@ robot_geometry = { -- body to nav cam nav_cam_transform = transform(vec3(0.10092553, -0.035829837, -0.11337286), quat4(-0.48308168, -0.51481273, -0.51707214, -0.48397969)), dock_cam_transform = transform(vec3(-0.15848363, -0.064524236, -0.039838535), quat4(0.51481214, -0.48239012, -0.48683339, 0.51503542)), - imu_transform = transform(vec3(0.014, 0.016, -0.0319625), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)) + imu_transform = transform(vec3(0.014, 0.016, -0.0319625), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -92,7 +99,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/queen.config b/astrobee/config/robots/queen.config index 079740aa8b..9f988c40a3 100644 --- a/astrobee/config/robots/queen.config +++ b/astrobee/config/robots/queen.config @@ -25,16 +25,22 @@ robot_i2c_bus = "/dev/i2c-1" robot_imu_drdy_pin = 4 robot_geometry = { --- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.04048657, -0.0091504194, -0.028062211), quat4(-0.012882005, 0.0027199368, 0.99990853, 0.00309779)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.046790197, 0.02346969, 0.0046008775), quat4(-0.00066136779, -0.0063282848, -0.0026048588, 0.99997636)), + haz_cam_depth_to_image_transform = { + 0.99295903, -0.00015649313, -0.037771467, -0.034439502, + -0.00010991374, 0.99230798, -0.00024249114, -0.0088065933, + 0.031393807, 0.012642902, 0.96478105, -0.012399336, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ), dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), - imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) + imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -46,8 +52,8 @@ robot_camera_calibrations = { 0.0, 602.67924, 509.73835, 0.0, 0.0, 1.0 }, - gain=100, - exposure=150 + gain=50, + exposure=175 }, dock_cam = { distortion_coeff = 1.01007, @@ -70,26 +76,27 @@ robot_camera_calibrations = { exposure=150 }, haz_cam = { - distortion_coeff = {-0.411106, 0.375288, -0.00080993, -0.00817328}, + distortion_coeff = {-0.37295935, -0.01115315, 0.0029100743, -0.013234186}, intrinsic_matrix = { - 188.27394, 0.0, 117.94881, - 0.0, 186.18359, 87.740547, + 210.7242, 0.0, 124.59857, + 0.0, 210.7242, 87.888263, 0.0, 0.0, 1.0 }, gain=50, exposure=150 }, --- Placeholder for sci_cam, not accurate! sci_cam = { - distortion_coeff = {0.128628, -0.167456, 0.00213421, -0.00262211}, + distortion_coeff = {-0.019654579, 0.024057067, -0.00060629998, 0.0027509131}, intrinsic_matrix = { - 859.44015, 0.0, 754.24485, - 0.0, 849.35466, 487.7349, + 1016.3726, 0.0, 689.17409, + 0.0, 1016.3726, 501.88817, 0.0, 0.0, 1.0 }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/sim.config b/astrobee/config/robots/sim.config index f24c961758..2510117a97 100644 --- a/astrobee/config/robots/sim.config +++ b/astrobee/config/robots/sim.config @@ -25,14 +25,21 @@ robot_imu_drdy_pin = 4 -- Engineering positions and idealized orientations robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ), dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), - imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) + imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -87,7 +94,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } robot_perch_cam_device = "" @@ -99,7 +108,7 @@ agent_name = "sim" heartbeat_queue_size = 20 -nodes_not_running = {} +nodes_not_running = {"light_flow"} bags_save_directory = "/tmp/bags" diff --git a/astrobee/config/robots/sim_pub.config b/astrobee/config/robots/sim_pub.config index adfb18b253..e47e2f5f6e 100644 --- a/astrobee/config/robots/sim_pub.config +++ b/astrobee/config/robots/sim_pub.config @@ -16,4 +16,4 @@ -- under the License. require "robots/sim" -nodes_not_running = {"dds_ros_bridge"} +nodes_not_running = {"dds_ros_bridge", "comms_bridge", "light_flow"} diff --git a/astrobee/config/robots/wannabee.config b/astrobee/config/robots/wannabee.config index 09362fbef6..c78ebfcc4e 100644 --- a/astrobee/config/robots/wannabee.config +++ b/astrobee/config/robots/wannabee.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Calibrated transforms -- nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(-0.46938154, -0.52978318, -0.5317378, -0.46504373)), @@ -37,7 +42,9 @@ robot_geometry = { haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ), dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), - imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ) + imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), + -- Not accurate only for sim purposes + sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; robot_camera_calibrations = { @@ -91,7 +98,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/simulation/simulation.config b/astrobee/config/simulation/simulation.config index c48a650575..e881a56ad2 100644 --- a/astrobee/config/simulation/simulation.config +++ b/astrobee/config/simulation/simulation.config @@ -30,9 +30,5 @@ disable_cameras_on_speedup = true; nav_cam_rate = 0.0; dock_cam_rate = 0.0; -perch_cam_rate = 5.0; -haz_cam_rate = 1.0; -sci_cam_rate = 1.0; - --- Set to true to enable sci cam continous picture taking at startup -sci_cam_continuous_picture_taking = false; +perch_cam_rate = 0.0; +haz_cam_rate = 0.0; diff --git a/astrobee/config/tests/faults.config b/astrobee/config/tests/faults.config deleted file mode 100644 index e9d8b01056..0000000000 --- a/astrobee/config/tests/faults.config +++ /dev/null @@ -1,182 +0,0 @@ --- Copyright (c) 2017, United States Government, as represented by the --- Administrator of the National Aeronautics and Space Administration. --- --- All rights reserved. --- --- The Astrobee platform is licensed under the Apache License, Version 2.0 --- (the "License"); you may not use this file except in compliance with the --- License. You may obtain a copy of the License at --- --- http://www.apache.org/licenses/LICENSE-2.0 --- --- Unless required by applicable law or agreed to in writing, software --- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT --- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the --- License for the specific language governing permissions and limitations --- under the License. - --- Autogenerated from FMECA! DO NOT CHANGE! - -arm = { - {id=61, key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, -} - -all = { - {id=85, key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, -} - -access_control = { - {id=70, key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, -} - -executive = { - {id=68, key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, -} - -data_bagger = { - {id=83, key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, -} - -llp_cpu_monitor = { - {id=72, key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"}, - {id=86, key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, - {id=97, key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, - {id=100, key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, -} - -llp_disk_monitor = { - {id=75, key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, - {id=89, key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, - {id=91, key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, - {id=94, key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, -} - -image_sampler = { - {id=81, key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, -} - -mlp_disk_monitor = { - {id=76, key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, - {id=90, key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, - {id=92, key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, - {id=95, key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, -} - -mlp_cpu_monitor = { - {id=73, key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"}, - {id=87, key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, - {id=98, key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, - {id=101, key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, -} - -sys_monitor = { - {id=105, key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, -} - -optical_flow_nodelet = { -} - -marker_tracking = { -} - -localization_node = { -} - -handrail_detect = { -} - -localization_manager = { - {id=62, key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, - {id=117, key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, - {id=119, key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, -} - -dds_ros_bridge = { - {id=71, key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, -} - -mapper = { - {id=80, key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, -} - -planner_qp = { - {id=78, key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, -} - -planner_trapezoidal = { - {id=79, key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, -} - -choreographer = { - {id=67, key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, -} - -laser = { -} - -eps_driver = { -} - -perching_arm = { - {id=60, key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, -} - -framestore = { - {id=82, key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, -} - -pmc_actuator = { -} - -flashlight_front = { -} - -nav_cam = { -} - -flashlight_aft = { -} - -pico_driver = { - {id=59, key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, - {id=124, key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, -} - -dock_cam = { -} - -ctl = { - {id=53, key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, - {id=133, key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, -} - -ekf = { - {id=54, key="INITIALIZATION_FAILED", description="EKF Initialization Failed"}, - {id=134, key="CONTROL_CYCLE_OVERRUN", description="Control Cycle Overrun"}, - {id=136, key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, -} - -fam = { - {id=55, key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, -} - -speed_cam = { - {id=10, key="VELOCITY_TOO_HIGH", description="Velocity too high"}, - {id=56, key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, - {id=126, key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, -} - -epson_imu = { - {id=112, key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, - {id=145, key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, - {id=146, key="IMU_OVERTEMP", description="IMU overtemp"}, - {id=147, key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, - {id=148, key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, - {id=150, key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, - {id=151, key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, -} - -dock = { - {id=84, key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, -} - diff --git a/astrobee/config/tests/faults.config b/astrobee/config/tests/faults.config new file mode 120000 index 0000000000..0cc25021b8 --- /dev/null +++ b/astrobee/config/tests/faults.config @@ -0,0 +1 @@ +../faults.config \ No newline at end of file diff --git a/astrobee/config/tools/camera_target_based_intrinsics_calibrator.config b/astrobee/config/tools/camera_target_based_intrinsics_calibrator.config new file mode 100644 index 0000000000..0bed02b753 --- /dev/null +++ b/astrobee/config/tools/camera_target_based_intrinsics_calibrator.config @@ -0,0 +1,70 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +require "context" + +camera = "nav_cam" + +calibrate_focal_lengths = true +calibrate_principal_points = true +calibrate_distortion = true +calibrate_target_poses = false + +-- fov, rad, or radtan +distortion_type = "fov" + +-- Calibration optimizer params +calibrator_max_num_iterations = 100 +calibrator_function_tolerance = 1e-8 +calibrator_parameter_tolerance = 1e-8 +calibrator_verbose_optimization = true +calibrator_huber_loss = 1.345 +-- dense_qr, dense_schur, sparse_normal_cholesky, sparse_schur, iterative_schur +-- Favor dense_qr or dense_schur for smaller/medium sized problems +-- Favor iterative schur for large problems +calibrator_linear_solver = "dense_qr" +calibrator_use_explicit_schur_complement = false +scale_loss_radially = false +radial_scale_power = 1 +-- ReprojectionPoseEstimate optimizer params +reprojection_optimize_estimate = true +reprojection_max_num_iterations = 100 +reprojection_function_tolerance = 1e-8 +reprojection_parameter_tolerance = 1e-8 +reprojection_verbose_optimization = true +reprojection_huber_loss = 1.345 +reprojection_linear_solver = "dense_qr" +reprojection_use_explicit_schur_complement = false +reprojection_max_inlier_threshold = 3.0 +-- RansacPnP params +ransac_max_inlier_threshold = 7.0 +ransac_num_iterations = 100 +ransac_min_num_inliers = 4 +-- p3p, epnp, ap3p, or it +ransac_pnp_method = "p3p" +-- Data Loading Params +max_num_match_sets = 10000 +min_num_target_inliers = 4 +only_use_inliers = false + +-- Other +image_width = 1280 +image_height = 960 +max_visualization_error_norm = 50 +save_individual_initial_reprojection_images = false +save_final_reprojection_image = true +individual_max_visualization_error_norm = 50 diff --git a/astrobee/config/tools/offline_replay.config b/astrobee/config/tools/offline_replay.config new file mode 100644 index 0000000000..da59096a2e --- /dev/null +++ b/astrobee/config/tools/offline_replay.config @@ -0,0 +1,58 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +-- LiveMeasurementSimulator +--- imu +or_imu_msg_delay = 0 +or_imu_min_msg_spacing = 0 +--- flight_mode +or_flight_mode_msg_delay = 0 +or_flight_mode_min_msg_spacing = 0 +--- depth odometry +or_depth_odometry_msg_delay = 0 +or_depth_odometry_min_msg_spacing = 0 +--- depth image +or_depth_image_msg_delay = 0 +or_depth_image_min_msg_spacing = 0 +--- depth cloud +or_depth_cloud_msg_delay = 0 +or_depth_cloud_min_msg_spacing = 0 +--- optical flow +or_of_msg_delay = 0 +or_of_min_msg_spacing = 0 +--- sparse mapping +or_vl_msg_delay = 0 +or_vl_min_msg_spacing = 0 +--- april tag +or_ar_msg_delay = 0 +or_ar_min_msg_spacing = 0 +--- vio +or_vio_msg_delay = 0 +or_vio_min_msg_spacing = 0 + +-- graph_localizer_simulator +or_loc_optimization_time = 0.05 + +-- graph_vio_simulator +or_vio_optimization_time = 0.30 + +-- Other +or_save_optical_flow_images = false +or_use_bag_depth_odom_msgs = false +or_log_relative_time = false +or_ar_min_num_landmarks = 5 +or_sparse_mapping_min_num_landmarks = 5 diff --git a/astrobee/config/transforms.config b/astrobee/config/transforms.config index f3b7c55db9..01750605d4 100644 --- a/astrobee/config/transforms.config +++ b/astrobee/config/transforms.config @@ -39,14 +39,7 @@ transforms = { { global = false, parent = "body", child = "dock_cam", transform = dock_cam_transform }, { global = false, parent = "body", child = "sci_cam", - transform = nav_cam_transform }, - - -- TRANSFORMS BETWEEN CAMERAS - -- For some reason this interfers with placing the bot in the right location - -- { global = false, parent = "haz_cam", child = "nav_cam", - -- transform = hazcam_to_navcam_transform }, - -- { global = false, parent = "sci_cam", child = "haz_cam", - -- transform = scicam_to_hazcam_transform }, + transform = sci_cam_transform }, -- IMU @@ -81,6 +74,16 @@ transforms = { { global = false, parent = "body", child = "payload/bottom_front", transform = payload_bottom_front_transform }, + -- HANDRAIL + -- Even though it is not relative to the robot, we want one per robot + + { global = false, parent = "handrail/body", child = "handrail/complete", + transform = handrail_to_complete_transform }, + { global = false, parent = "handrail/body", child = "handrail/approach", + transform = handrail_to_approach_transform }, + -- handrail_to_ensure_transform doesn't exist so this isn't being used + --{ global = false, parent = "handrail/body", child = "handrail/ensure", + --transform = handrail_to_ensure_transform }, ------------- -- GLOBALS -- @@ -128,15 +131,6 @@ transforms = { { global = true, parent = "dock/berth2", child = "dock/berth2/complete", transform = berth_to_complete_transform }, { global = true, parent = "dock/berth2", child = "dock/berth2/approach", - transform = berth_to_approach_transform }, - - -- HANDRAIL + transform = berth_to_approach_transform } - { global = true, parent = "handrail/body", child = "handrail/complete", - transform = handrail_to_complete_transform }, - { global = true, parent = "handrail/body", child = "handrail/approach", - transform = handrail_to_approach_transform }, - -- handrail_to_ensure_transform doesn't exist so this isn't being used - --{ global = true, parent = "handrail/body", child = "handrail/ensure", - --transform = handrail_to_ensure_transform } } diff --git a/astrobee/config/worlds/granite.config b/astrobee/config/worlds/granite.config index 6107b92f3e..620b3e5309 100755 --- a/astrobee/config/worlds/granite.config +++ b/astrobee/config/worlds/granite.config @@ -46,6 +46,14 @@ world_vision_map_filename = resolve_resource("maps/granite.map") world_vision_ar_tag_filename = "dock_markers_world.config" -- Distance of handrail from wall plane world_handrail_wall_min_gap = 0.11 +-- Huber loss function cutoff point to switch from quadratic to linear +world_huber_k = 1.345 +world_gyro_sigma = 0.00001 +world_accel_sigma = 0.0005 +world_accel_bias_sigma = 0.0005 +world_gyro_bias_sigma = 0.0000035 +world_integration_variance = 0.0001 +world_bias_acc_omega_int = 0.0001 -------------------------- -- PHYSICS -- @@ -85,9 +93,6 @@ world_flight_modes = { -- Control enabled control_enabled = false; - -- Collision radius - collision_radius = 0; - -- Attitude Parameters att_kp = vec3(0.0, 0.0, 4.0); att_ki = vec3(0.0, 0.0, 0.002); @@ -102,6 +107,7 @@ world_flight_modes = { speed = 0; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -125,9 +131,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(0.0, 0.0, 4.0); att_ki = vec3(0.0, 0.0, 0.002); @@ -143,9 +146,10 @@ world_flight_modes = { speed = 2; -- Tolerances - tolerance_pos = 0.15; -- Position (10 cm) + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) + tolerance_pos = 0.2; -- Position (20 cm) tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) + tolerance_att = 0.3491; -- Attitude (20 degrees) tolerance_omega = 0; -- Omega (disabled) tolerance_time = 1.0; -- Time (for sync check) @@ -166,9 +170,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(0.0, 0.0, 4.0); att_ki = vec3(0.0, 0.0, 0.002); @@ -184,6 +185,7 @@ world_flight_modes = { speed = 3; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) tolerance_pos = 0.2; -- Position (20 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.3490; -- Attitude (20 degrees) @@ -207,9 +209,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(0.0, 0.0, 4.0); att_ki = vec3(0.0, 0.0, 0.002); @@ -225,6 +224,7 @@ world_flight_modes = { speed = 1; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -238,100 +238,16 @@ world_flight_modes = { hard_limit_omega = 0.0174; hard_limit_alpha = 0.0174; }, - ------------- - -- DOCKING -- - ------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "docking"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(0.0, 0.0, 4.0); - att_ki = vec3(0.0, 0.0, 0.002); - omega_kd = vec3(0.001, 0.001, 3.2); - - -- Position Loop - pos_kp = vec3(0.6, 0.6, 0.0); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0); - vel_kd = vec3(1.2, 1.2, .001); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.0200; - hard_limit_accel = 0.0100; - hard_limit_omega = 0.1745; - hard_limit_alpha = 0.1745; - }, --------------- - -- UNDOCKING -- + -- PRECISION -- --------------- { -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "undocking"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(0.0, 0.0, 4.0); - att_ki = vec3(0.0, 0.0, 0.002); - omega_kd = vec3(0.001, 0.001, 3.2); - - -- Position Loop - pos_kp = vec3(0.6, 0.6, 0.0); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0); - vel_kd = vec3(1.2, 1.2, .001); - - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.0200; - hard_limit_accel = 0.0200; - hard_limit_omega = 0.1745; - hard_limit_alpha = 0.1745; - }, - -------------- - -- PERCHING -- - -------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "perching"; + name = "precision"; -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(0.0, 0.0, 4.0); att_ki = vec3(0.0, 0.0, 0.002); @@ -346,6 +262,7 @@ world_flight_modes = { speed = 3; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -358,47 +275,6 @@ world_flight_modes = { hard_limit_accel = 0.0100; hard_limit_omega = 0.1745; hard_limit_alpha = 0.1745; - }, - ---------------- - -- UNPERCHING -- - ---------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "perching"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(0.0, 0.0, 4.0); - att_ki = vec3(0.0, 0.0, 0.002); - omega_kd = vec3(0.001, 0.001, 3.2); - - -- Position Loop - pos_kp = vec3(0.6, 0.6, 0.0); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0); - vel_kd = vec3(1.2, 1.2, .001); - - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.0200; - hard_limit_accel = 0.0200; - hard_limit_omega = 0.1745; - hard_limit_alpha = 0.1745; } } diff --git a/astrobee/config/worlds/iss.config b/astrobee/config/worlds/iss.config index 2bd18be186..5d21853b3f 100755 --- a/astrobee/config/worlds/iss.config +++ b/astrobee/config/worlds/iss.config @@ -32,7 +32,7 @@ world_dock_berth2_tilt = math.rad(0) -- Tilt in body-frame Y -- 1, 0.00336, 0.00202) -- --> back transformed to body + berth world_dock_transform = transform( - vec3(9.82, -9.50, 4.41), quat4(0.0, 0.0, 0.0, 1.0)); + vec3(9.5868938, -10.0604698, 4.2955872), quat4(0.0, 0.0, 0.0, 1.0)); -------------------------- -- MOBILITY -- @@ -52,6 +52,15 @@ world_vision_map_filename = resolve_resource("maps/iss.map") world_vision_ar_tag_filename = "dock_markers_world.config" -- Distance of handrail from wall plane world_handrail_wall_min_gap = 0.06 +-- Huber loss function cutoff point to switch from quadratic to linear +world_huber_k = 1.345 +-- From gtsam: Angular and velocity random walk expressed in degrees respectively m/s per sqrt(hr) +world_gyro_sigma = 0.00001; +world_accel_sigma = 0.0005 +world_accel_bias_sigma = 0.0005 +world_gyro_bias_sigma = 0.0000035 +world_integration_variance = 0.0001 +world_bias_acc_omega_int = 0.00015 -------------------------- -- PHYSICS -- @@ -92,9 +101,6 @@ world_flight_modes = { -- Control enabled control_enabled = false; - -- Collision radius - collision_radius = 0; - -- Attitude Parameters att_kp = vec3(4.0, 4.0, 4.0); -- .4 att_ki = vec3(0.002, 0.002, 0.002); @@ -109,6 +115,7 @@ world_flight_modes = { speed = 0; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -132,9 +139,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(4.0, 4.0, 4.0); -- .4 att_ki = vec3(0.002, 0.002, 0.002); @@ -146,12 +150,13 @@ world_flight_modes = { vel_kd = vec3(1.2, 1.2, 1.2); -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; + speed = 2; -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) + tolerance_pos = 0.25; -- Position (25 cm) tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) + tolerance_att = 0.3491; -- Attitude (20 degrees) tolerance_omega = 0; -- Omega (disabled) tolerance_time = 1.0; -- Time (for sync check) @@ -172,9 +177,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(4.0, 4.0, 4.0); -- .4 att_ki = vec3(0.002, 0.002, 0.002); @@ -189,6 +191,7 @@ world_flight_modes = { speed = 3; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) tolerance_pos = 0.2; -- Position (20 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.3490; -- Attitude (20 degrees) @@ -212,9 +215,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(4.0, 4.0, 4.0); -- .4 att_ki = vec3(0.002, 0.002, 0.002); @@ -229,6 +229,7 @@ world_flight_modes = { speed = 1; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -242,59 +243,16 @@ world_flight_modes = { hard_limit_omega = 0.0174; hard_limit_alpha = 0.0174; }, - ------------- - -- DOCKING -- - ------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "docking"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.0200; - hard_limit_accel = 0.0100; - hard_limit_omega = 0.1745; - hard_limit_alpha = 0.1745; - }, --------------- - -- UNDOCKING -- + -- PRECISION -- --------------- { -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "undocking"; + name = "precision"; -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(4.0, 4.0, 4.0); -- .4 att_ki = vec3(0.002, 0.002, 0.002); @@ -309,6 +267,7 @@ world_flight_modes = { speed = 3; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (10 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -318,89 +277,9 @@ world_flight_modes = { -- The values below instruct the planner to generate segments that abide -- by kinematic constraints on linear and angular positions and velocities. hard_limit_vel = 0.0200; - hard_limit_accel = 0.0200; + hard_limit_accel = 0.0100; hard_limit_omega = 0.1745; hard_limit_alpha = 0.1745; - }, - -------------- - -- PERCHING -- - -------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "perching"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.0200; - hard_limit_accel = 0.0200; - hard_limit_omega = 0.1745; - hard_limit_alpha = 0.1745; - }, - ---------------- - -- UNPERCHING -- - ---------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "unperching"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.0200; - hard_limit_accel = 0.0200; - hard_limit_omega = 0.1745; - hard_limit_alpha = 0.1745; } } diff --git a/astrobee/config/worlds/mgtf.config b/astrobee/config/worlds/mgtf.config old mode 100644 new mode 100755 index 42328492f6..5d5198141d --- a/astrobee/config/worlds/mgtf.config +++ b/astrobee/config/worlds/mgtf.config @@ -25,8 +25,8 @@ require "rotations" -- Location of the dock and berth tilt values (in degrees) world_dock_berth1_tilt = math.rad(0) -- Tilt in body-frame Y world_dock_berth2_tilt = math.rad(0) -- Tilt in body-frame Y -world_dock_transform = transform(0.0, 0.0, 0.0), - quat4(0.0, 0.0, 1.0, 1.0)); +world_dock_transform = transform( + vec3(0.0, 0.0, 0.0), quat4(0.0, 0.0, 0.0, 1.0)); -------------------------- -- MOBILITY -- @@ -58,12 +58,13 @@ world_gravity_vector = vec3(0.0, 0.0, 9.80665); -------------------------- -- Inertial Parameters +-- Default assumes freeflyer, no payloads, 4 batteries, measured EEl 2/26/2019 world_inertia_name = "default"; -world_inertia_com = vec3(0.0, 0.0, 0.0); -- Center of mass -world_inertia_mass = 7.0; -- Platform mass -world_inertia_matrix = matrix{{.1083, 0, 0}, - {0, .1083, 0}, - {0, 0, .1083}}; -- Inertia matrix +world_inertia_com = vec3(0.003713818, -0.000326347, -0.002532192); -- Center of mass +world_inertia_mass = 9.583788668; -- Platform + 4 batteries +world_inertia_matrix = matrix{{0.153427995, 0, 0}, + {0, 0.14271405, 0}, + {0, 0, 0.162302759}}; -- Inertia matrix -------------------------- -- FLIGHT MODES -- @@ -85,9 +86,6 @@ world_flight_modes = { -- Control enabled control_enabled = false; - -- Collision radius - collision_radius = 0; - -- Attitude Parameters att_kp = vec3(2.0, 2.0, 2.0); -- .4 att_ki = vec3(0.001, 0.001, 0.001); @@ -102,6 +100,7 @@ world_flight_modes = { speed_gain = 0; -- Tolerances + tolerance_pos_endpoint = 0.05; -- Endpoint position (5 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -125,9 +124,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(2.0, 2.0, 2.0); -- .4 att_ki = vec3(0.001, 0.001, 0.001); @@ -139,9 +135,10 @@ world_flight_modes = { vel_kd = vec3(1.2, 1.2, 1.2); -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; + speed = 2; -- Tolerances + tolerance_pos_endpoint = 0.05; -- Endpoint position (5 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -165,9 +162,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(2.0, 2.0, 2.0); -- .4 att_ki = vec3(0.001, 0.001, 0.001); @@ -182,6 +176,7 @@ world_flight_modes = { speed = 3; -- Tolerances + tolerance_pos_endpoint = 0.1; -- Endpoint position (5 cm) tolerance_pos = 0.2; -- Position (20 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.3490; -- Attitude (20 degrees) @@ -205,9 +200,6 @@ world_flight_modes = { -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(2.0, 2.0, 2.0); -- .4 att_ki = vec3(0.001, 0.001, 0.001); @@ -222,6 +214,7 @@ world_flight_modes = { speed = 1; -- Tolerances + tolerance_pos_endpoint = 0.05; -- Endpoint position (5 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -235,139 +228,16 @@ world_flight_modes = { hard_limit_omega = 0.0174; -- 1 deg/s (in rads/s) hard_limit_alpha = 0.0174; -- 1 deg/s^2 (in rads/s^2) }, - ------------- - -- DOCKING -- - ------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "docking"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(2.0, 2.0, 2.0); -- .4 - att_ki = vec3(0.001, 0.001, 0.001); - omega_kd = vec3(1.6, 1.6, 1.6); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.2000; -- 20 cm/s - hard_limit_accel = 0.0200; -- 5 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - }, --------------- - -- UNDOCKING -- + -- PRECISION -- --------------- { -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "undocking"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(2.0, 2.0, 2.0); -- .4 - att_ki = vec3(0.001, 0.001, 0.001); - omega_kd = vec3(1.6, 1.6, 1.6); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.2000; -- 20 cm/s - hard_limit_accel = 0.0200; -- 5 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - }, - -------------- - -- PERCHING -- - -------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "perching"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(2.0, 2.0, 2.0); -- .4 - att_ki = vec3(0.001, 0.001, 0.001); - omega_kd = vec3(1.6, 1.6, 1.6); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.2000; -- 20 cm/s - hard_limit_accel = 0.0200; -- 5 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - }, - ---------------- - -- UNPERCHING -- - ---------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "unperching"; + name = "precision"; -- Control enabled control_enabled = true; - -- Collision radius - collision_radius = 0.25; - -- Attitude Parameters att_kp = vec3(2.0, 2.0, 2.0); -- .4 att_ki = vec3(0.001, 0.001, 0.001); @@ -379,9 +249,10 @@ world_flight_modes = { vel_kd = vec3(1.2, 1.2, 1.2); -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; + speed = 2; -- Tolerances + tolerance_pos_endpoint = 0.05; -- Endpoint position (5 cm) tolerance_pos = 0.1; -- Position (10 cm) tolerance_vel = 0; -- Velocity (disabled) tolerance_att = 0.1745; -- Attitude (10 degrees) @@ -407,7 +278,7 @@ world_visualeyez_grounding = { { tcm=1, led=2, x=0.1835635587737848, y=-3.219100563007645, z=-0.2319006270806013 }, { tcm=1, led=3, x=1.043904870248541, y=-3.219100563007645, z= 0.6628505519526655 }, { tcm=1, led=4, x=1.110810070079649, y=-2.050167430479354, z=-0.2681018912672751 } -}; +} -- Visualeyez target matrix world_visualeyez_targets = { diff --git a/astrobee/config/worlds/perch.config b/astrobee/config/worlds/perch.config deleted file mode 100644 index 334da7ab3d..0000000000 --- a/astrobee/config/worlds/perch.config +++ /dev/null @@ -1,416 +0,0 @@ --- Copyright (c) 2017, United States Government, as represented by the --- Administrator of the National Aeronautics and Space Administration. --- --- All rights reserved. --- --- The Astrobee platform is licensed under the Apache License, Version 2.0 --- (the "License"); you may not use this file except in compliance with the --- License. You may obtain a copy of the License at --- --- http://www.apache.org/licenses/LICENSE-2.0 --- --- Unless required by applicable law or agreed to in writing, software --- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT --- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the --- License for the specific language governing permissions and limitations --- under the License. - -local matrix = require 'matrix' -require "rotations" - --------------------------- --- DOCKING -- --------------------------- - --- Location of the dock and berth tilt values (in degrees) -world_dock_berth1_tilt = math.rad(0) -- Tilt in body-frame Y -world_dock_berth2_tilt = math.rad(0) -- Tilt in body-frame Y -world_dock_transform = transform( - vec3(9.7, -9.79, 4.5), quat4(0.0, 0.0, 0.0, 1.0)); - --------------------------- --- MOBILITY -- --------------------------- - --- Path to the file containing zones -world_zone_file = resolve_resource("zones/iss.bin") - --------------------------- --- LOCALIZATION -- --------------------------- - --- Sparse map -world_vision_map_filename = resolve_resource("maps/p4d.map") - --- AR Tag marker file -world_vision_ar_tag_filename = resolve_resource("granite_lab_markers.config") --- Distance of handrail from wall plane -world_handrail_wall_min_gap = 0.06 - --------------------------- --- PHYSICS -- --------------------------- - -world_gravity_vector = vec3(0.0, 0.0, 0.0); - --------------------------- --- INERTIA -- --------------------------- - --- Inertial Parameters --- Default assumes freeflyer, no payloads, 4 batteries, measured EEl 2/26/2019 -world_inertia_name = "default"; -world_inertia_com = vec3(0.003713818, -0.000326347, -0.002532192); -- Center of mass -world_inertia_mass = 9.583788668; -- Platform + 4 batteries -world_inertia_matrix = matrix{{0.153427995, 0, 0}, - {0, 0.14271405, 0}, - {0, 0, 0.162302759}}; -- Inertia matrix - --------------------------- --- FLIGHT MODES -- --------------------------- - --- Initial and default flight mode -world_initial_flight_mode = "off"; -world_default_flight_mode = "nominal"; - --- All possible flight modes -world_flight_modes = { - --------- - -- OFF -- - --------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "off"; - - -- Control enabled - control_enabled = false; - - -- Collision radius - collision_radius = 0; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 0; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.2000; -- 20 cm/s - hard_limit_accel = 0.0200; -- 5 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - }, - ------------- - -- NOMINAL -- - ------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "nominal"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.2000; -- 20 cm/s - hard_limit_accel = 0.0200; -- 5 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - }, - --------------- - -- DIFFICULT -- - --------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "difficult"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.2; -- Position (20 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.3490; -- Attitude (20 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.4000; -- 40 cm/s - hard_limit_accel = 0.0400; -- 8 cm/s^2 - hard_limit_omega = 0.5236; -- 30 deg/s (in rads/s) - hard_limit_alpha = 0.5236; -- 30 deg/s^2 (in rads/s^2) - }, - ----------- - -- QUIET -- - ----------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "quiet"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 1; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.0200; -- 2 mm/s - hard_limit_accel = 0.0020; -- 2 mm/s^2 - hard_limit_omega = 0.0174; -- 1 deg/s (in rads/s) - hard_limit_alpha = 0.0174; -- 1 deg/s^2 (in rads/s^2) - }, - ------------- - -- DOCKING -- - ------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "docking"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.0200; -- 2 cm/s - hard_limit_accel = 0.0100; -- 1 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - }, - --------------- - -- UNDOCKING -- - --------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "undocking"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.2000; -- 20 cm/s - hard_limit_accel = 0.0200; -- 5 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - }, - -------------- - -- PERCHING -- - -------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "perching"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.2000; -- 20 cm/s - hard_limit_accel = 0.0200; -- 5 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - }, - ---------------- - -- UNPERCHING -- - ---------------- - { - -- This is the key we use to access the mode through FlightModeUtils::Load("") - name = "unperching"; - - -- Control enabled - control_enabled = true; - - -- Collision radius - collision_radius = 0.25; - - -- Attitude Parameters - att_kp = vec3(4.0, 4.0, 4.0); -- .4 - att_ki = vec3(0.002, 0.002, 0.002); - omega_kd = vec3(3.2, 3.2, 3.2); -- 1.1 - - -- Position Loop - pos_kp = vec3(.6, .6, .6); --.1 - pos_ki = vec3(0.0001, 0.0001, 0.0001); - vel_kd = vec3(1.2, 1.2, 1.2); - - -- Impeller speed (0 => OFF, 1 = QUIET, 2 = NOMINAL, 3 = AGGRESSIVE) - speed = 3; - - -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) - tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) - tolerance_omega = 0; -- Omega (disabled) - tolerance_time = 1.0; -- Time (for sync check) - - -- The values below instruct the planner to generate segments that abide - -- by kinematic constraints on linear and angular positions and velocities. - hard_limit_vel = 0.2000; -- 20 cm/s - hard_limit_accel = 0.0200; -- 5 cm/s^2 - hard_limit_omega = 0.1745; -- 10 deg/s (in rads/s) - hard_limit_alpha = 0.1745; -- 10 deg/s^2 (in rads/s^2) - } -} - --------------------------- --- VISUALEYEZ -- --------------------------- - --- Visualeyez not available -world_visualeyez_grounding = {} -world_visualeyez_targets = {} -world_visualeyez_calibration = "" - - --------------------------- --- VIVE -- --------------------------- - -world_vive_force2d = false -world_vive_lighthouses = {} diff --git a/astrobee/gds_configs/IssWorld/keepins/keepin.json b/astrobee/gds_configs/IssWorld/keepins/keepin.json new file mode 100644 index 0000000000..84ff0c75df --- /dev/null +++ b/astrobee/gds_configs/IssWorld/keepins/keepin.json @@ -0,0 +1,34 @@ +{ + "sequence" : [ [ -2.3920228000000012, -0.638923, 4.190492, -0.9758328000000001, 0.681877, 5.426472 ], + [ -0.9758328000000001, -1.044933, 3.7924819999999997, 5.892357199999999, 1.085177, 5.915652 ], + [ 5.892357199999999, -0.653923, 4.190492, 6.8085472, 0.6668769999999999, 5.426472 ], + [ 10.272166400000001, 1.8509731999999999, 4.189467199999999, 11.5929664, 2.7654632, 5.425437199999999 ], + [ 9.8588664, 2.7654632, 3.7867971999999996, 11.9889764, 7.3350832, 5.9163172 ], + [ 10.275493800000001, -2.7531998, 4.1909372, 11.5962938, -1.8364698000000002, 5.511727199999999 ], + [ 9.8594838, -9.2364698, 3.7831271999999996, 11.995003800000001, -2.7531998, 5.9180472 ], + [ 9.5358938, -11.6364698, 3.7505871999999996, 12.335893800000001, -9.2364698, 5.950587199999999 ], + [ -3.1292748, -0.6631431999999999, 4.1904666, -2.2130848, 0.6576568, 5.4264466 ], + [ -3.8182448, -1.0684032, 3.8745865999999998, -3.1292748, 1.0651967999999998, 5.8081866 ], + [ -5.1551447999999995, -1.5891532000000002, 3.5433065999999998, -3.8182448, 1.5130367999999998, 6.101866599999999 ], + [ -6.991184799999999, -1.1094732, 3.7634065999999997, -5.1551447999999995, 1.0460368, 5.9184266 ], + [ -7.4073747999999995, -0.7070831999999999, 4.156856599999999, -6.991184799999999, 0.6609368000000001, 5.4331366 ], + [ -5.1216948, -2.1876132, 4.2614866, -3.8453448, -1.5891532000000002, 5.4575866 ], + [ 6.734420599999999, -0.6496558, 4.1954958, 7.650610599999999, 0.6711442, 5.431475799999999 ], + [ 7.650610599999999, -1.0583658, 3.7911358, 10.557180599999999, 1.0844442, 5.9206558 ], + [ 10.292640599999999, -1.2239658, 3.6150057999999996, 11.6015606, 1.2186642, 6.0576358 ], + [ 10.292640599999999, 1.2186642, 4.1298958, 11.6015606, 2.6927242, 5.4979157999999995 ], + [ 10.292640599999999, -2.7574958, 4.2238158, 11.6015606, -1.2239658, 5.4595658 ], + [ 11.6015606, -0.7892558000000001, 4.1558958, 12.0844206, 0.8107442, 5.5558958 ], + [ -5.1190144, -2.8618892000000002, 4.1830244, -3.7982144, -1.9456992, 5.4190044 ], + [ -5.5223144, -6.0224592, 3.7659643999999997, -3.3922044, -2.8618892000000002, 5.9081844 ], + [ -5.700624400000001, -7.292459200000001, 3.6025343999999997, -3.2579944000000003, -6.0224592, 6.0451644 ], + [ -5.0560944, -7.244599200000001, 4.8434244, -3.9035044, -6.0703192, 7.1207644000000005 ], + [ -5.1023244000000005, -7.4794592, 6.7986144, -3.8149044, -6.191939199999999, 7.5009444 ], + [ -4.8312544, -7.2083692, 7.3275144, -4.0859744000000005, -6.463029199999999, 8.0728244 ] ], + "dateCreated" : "1530672755", + "notes" : "Go here.", + "author" : "Astrobee FSW", + "safe" : true, + "name" : "KeepinSet", + "dateModified" : "1530672755" +} \ No newline at end of file diff --git a/astrobee/gds_configs/IssWorld/keepouts/keepouts.json b/astrobee/gds_configs/IssWorld/keepouts/keepouts.json index 8157b98474..70269a3927 100644 --- a/astrobee/gds_configs/IssWorld/keepouts/keepouts.json +++ b/astrobee/gds_configs/IssWorld/keepouts/keepouts.json @@ -1,15 +1,11 @@ { - "sequence" : [ [ 10.1594, -10.0326, 4.64553, 11.5899, -11.3668, 6.03042 ], - [ 10.3207, -9.98851, 3.59318, 11.5511, -11.2598, 4.13493], - [ 11.8145, -8.94329, 4.00119, 11.9457, -3.09453, 5.66031], - [ 9.93057, -6.10715, 5.57685, 11.9412, -3.04415, 5.89445], - [ 9.62497, -9.81079, 4.57952, 10.0356, -9.03889, 5.63321], - [ 9.94108, -9.19364, 4.22575, 10.3345, -8.03365, 5.89156], - [ 9.90532, -4.92513, 4.67524, 10.2401, -4.08059, 5.88617], - [ 9.90532, -7.99054, 3.88574, 10.0281, -3.04415, 5.81595]], - "dateCreated" : "1456251730865", - "notes" : "Don't go here.", - "author" : "Marina", + "sequence" : [ [ 12.3539, -10.5727, 4.4233, 11.8722, -9.6330, 5.6942], + [ 10.2721, -11.9284, 4.0808, 11.5956, -10.5859, 3.6015], + [ 9.5142, -10.5559, 5.8497, 9.82404, -9.6458, 4.5099], + [ 10.1819, -5.3350, 4.6383, 9.8428, -5.7243, 4.0600] ], + "dateCreated" : "2020-09-04", + "notes" : "These keepouts reflect the occupation in the JEM module.", + "author" : "Astrobee FSW", "name" : "KeepoutSet", "safe" : false -} +} \ No newline at end of file diff --git a/astrobee/launch/astrobee.launch b/astrobee/launch/astrobee.launch index a32646da18..1add8f8366 100644 --- a/astrobee/launch/astrobee.launch +++ b/astrobee/launch/astrobee.launch @@ -29,9 +29,8 @@ - - - + + @@ -63,31 +62,25 @@ - - - - + command='$(find xacro)/xacro --inorder $(find description)/urdf/model.urdf.xacro world:="$(arg world)" top_aft:="$(arg top_aft)" bot_aft:="$(arg bot_aft)" bot_front:="$(arg bot_front)" ns:="_$(arg ns)" prefix:="$(arg ns)/"'/> - - - - - - - - - + + + + + + + @@ -120,7 +113,7 @@ - + @@ -140,9 +133,9 @@ name="ROS_HOSTNAME" value="$(arg mlp)" /> - - @@ -160,7 +153,7 @@ - + @@ -170,11 +163,4 @@ - - - > - - - - diff --git a/astrobee/launch/controller/ocam.launch b/astrobee/launch/controller/ocam.launch deleted file mode 100644 index 3306512a3c..0000000000 --- a/astrobee/launch/controller/ocam.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/astrobee/launch/controller/sim_start.launch b/astrobee/launch/controller/sim_start.launch index 69624e21bb..e1607eaa00 100644 --- a/astrobee/launch/controller/sim_start.launch +++ b/astrobee/launch/controller/sim_start.launch @@ -21,6 +21,7 @@ + @@ -29,5 +30,6 @@ + diff --git a/astrobee/launch/granite.launch b/astrobee/launch/granite.launch index 6717911bc1..73aec9e67f 100644 --- a/astrobee/launch/granite.launch +++ b/astrobee/launch/granite.launch @@ -34,7 +34,6 @@ - @@ -72,7 +71,6 @@ - diff --git a/astrobee/launch/offline_localization/enable_localization b/astrobee/launch/offline_localization/enable_localization new file mode 100755 index 0000000000..5fe9311f96 --- /dev/null +++ b/astrobee/launch/offline_localization/enable_localization @@ -0,0 +1,15 @@ +#!/bin/bash + +# Run the command to enable localization. If it fails, try again after a short time. + +while [ 1 ]; do + rosservice call --wait /loc/ml/enable true + ans=$? + if [ "$ans" -eq 0 ]; then + break + fi + sleep 0.25 +done + + + diff --git a/astrobee/launch/offline_localization/loc_only.launch b/astrobee/launch/offline_localization/loc_only.launch new file mode 100644 index 0000000000..0b6c8f51ad --- /dev/null +++ b/astrobee/launch/offline_localization/loc_only.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/loc_rviz.launch b/astrobee/launch/offline_localization/loc_rviz.launch new file mode 100644 index 0000000000..b0317858a9 --- /dev/null +++ b/astrobee/launch/offline_localization/loc_rviz.launch @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/localization_from_bag.launch b/astrobee/launch/offline_localization/localization_from_bag.launch new file mode 100644 index 0000000000..eea8a3e9ea --- /dev/null +++ b/astrobee/launch/offline_localization/localization_from_bag.launch @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/record_localization.launch b/astrobee/launch/offline_localization/record_localization.launch new file mode 100644 index 0000000000..19c7730113 --- /dev/null +++ b/astrobee/launch/offline_localization/record_localization.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/replay_localization.launch b/astrobee/launch/offline_localization/replay_localization.launch new file mode 100644 index 0000000000..d1dab21f1b --- /dev/null +++ b/astrobee/launch/offline_localization/replay_localization.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/sparse_mapping_matching_from_bag.launch b/astrobee/launch/offline_localization/sparse_mapping_matching_from_bag.launch new file mode 100644 index 0000000000..b4b9d841f3 --- /dev/null +++ b/astrobee/launch/offline_localization/sparse_mapping_matching_from_bag.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/pico.launch b/astrobee/launch/pico.launch index e3db9edf39..066c07268d 100644 --- a/astrobee/launch/pico.launch +++ b/astrobee/launch/pico.launch @@ -31,7 +31,6 @@ - @@ -55,7 +54,6 @@ - diff --git a/astrobee/launch/replay.launch b/astrobee/launch/replay.launch deleted file mode 100644 index 0427f93167..0000000000 --- a/astrobee/launch/replay.launch +++ /dev/null @@ -1,99 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/astrobee/launch/robot/LLP.launch b/astrobee/launch/robot/LLP.launch index cdc39b8546..ceda9ce578 100644 --- a/astrobee/launch/robot/LLP.launch +++ b/astrobee/launch/robot/LLP.launch @@ -22,57 +22,36 @@ - + - - + - - - - - - - - + + + + + + + - - - - - - - - - - - + - - - - - - - - - - - - - + + + @@ -80,6 +59,26 @@ + + + + + + + + + + + + + + + + + + + + @@ -187,8 +186,8 @@ - - + + diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 8a07a65d1f..4d6f954072 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -25,38 +25,44 @@ - + + - - + - + - - + - - - - - - - @@ -80,341 +86,383 @@ + + + + + + + + + + - - + + - - - + + + + + - + - - - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/astrobee/launch/sim.launch b/astrobee/launch/sim.launch index dc61930a04..414f0be244 100644 --- a/astrobee/launch/sim.launch +++ b/astrobee/launch/sim.launch @@ -28,11 +28,14 @@ + + + @@ -40,7 +43,7 @@ - + @@ -49,10 +52,11 @@ - + + default="9.816 -9.806 4.293 0 0 0 1" /> + @@ -63,14 +67,16 @@ name="pose" default="0 0 -0.7 0 0 0 1" /> - - - + + + + + @@ -89,9 +95,6 @@ - - - @@ -110,8 +113,7 @@ - - + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - + @@ -162,7 +163,11 @@ + + + + @@ -172,7 +177,10 @@ - + + @@ -181,7 +189,11 @@ + + + + @@ -192,7 +204,7 @@ + name="pose" value="11 -4 4.8 0 0 0 1" /> @@ -203,7 +215,11 @@ + + + + @@ -213,7 +229,10 @@ - + + @@ -222,7 +241,12 @@ + + + + + diff --git a/astrobee/launch/spawn.launch b/astrobee/launch/spawn.launch index 94253eec17..acc931274d 100644 --- a/astrobee/launch/spawn.launch +++ b/astrobee/launch/spawn.launch @@ -31,6 +31,7 @@ + @@ -85,6 +86,7 @@ + diff --git a/astrobee/package.xml b/astrobee/package.xml index d4f7e90b5c..5549996fc5 100644 --- a/astrobee/package.xml +++ b/astrobee/package.xml @@ -14,9 +14,7 @@ Astrobee Flight Software catkin - ff_msgs roscpp - ff_msgs roscpp diff --git a/astrobee/readme.md b/astrobee/readme.md index 1aaff437bb..56ff7d8f69 100644 --- a/astrobee/readme.md +++ b/astrobee/readme.md @@ -1,4 +1,4 @@ -\page astrobee Astrobee +\page astrobee General Considerations # Folder description @@ -17,6 +17,8 @@ to examine the 'launch' directory in this folder for a XML file called the system. Additional files might be needed depending on the context. 6. `scripts` - A simple bash script to print out the environment variables, which can be used to check the context at any point in the launch sequence. + A second script to print out the relevant software versions installed on the + MLP, LLP, and HLP. # Environment variables @@ -129,6 +131,9 @@ connection to the LLP (in case it is absent from the test rig): `roslaunch astrobee spawn.launch world:=iss ns:=bumble` +See more details in \ref running-the-sim for how to run the robot in +simulation. + 4. Do a processor-in-the-loop simulation `roslaunch astrobee sim.launch llp:=10.42.0.10 mlp:=10.42.0.11` diff --git a/astrobee/resources/debug.config b/astrobee/resources/debug.config index 76ba0e9101..bc7b20fdea 100644 --- a/astrobee/resources/debug.config +++ b/astrobee/resources/debug.config @@ -36,8 +36,8 @@ log4j.logger.ros.Astrobee./nav_cam = DEBUG log4j.logger.ros.Astrobee./dock_cam = DEBUG log4j.logger.ros.Astrobee./marker_tracking = DEBUG log4j.logger.ros.Astrobee./localization_node = DEBUG -log4j.logger.ros.Astrobee./ctl_node = DEBUG -log4j.logger.ros.Astrobee./fam_node = DEBUG +log4j.logger.ros.Astrobee./ctl = DEBUG +log4j.logger.ros.Astrobee./fam = DEBUG log4j.logger.ros.Astrobee./executive = DEBUG log4j.logger.ros.Astrobee./dds_ros_bridge = DEBUG log4j.logger.ros.Astrobee./framestore = DEBUG diff --git a/astrobee/resources/rviz/granite.rviz b/astrobee/resources/rviz/granite.rviz index 349d2c871d..885c95029e 100644 --- a/astrobee/resources/rviz/granite.rviz +++ b/astrobee/resources/rviz/granite.rviz @@ -7,8 +7,8 @@ Panels: - /Debug1 - /Debug1/Sensors1 - "/Debug1/Sensors1/DEBUG: NavCam1" - Splitter Ratio: 0.579250693 - Tree Height: 216 + Splitter Ratio: 0.579250693321228 + Tree Height: 221 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -17,27 +17,30 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 - SyncSource: Features + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - - Alpha: 0.100000001 + - Alpha: 0.10000000149011612 Cell Size: 1 Class: rviz/Grid Color: 255; 255; 127 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Billboards Name: Grid Normal Cell Count: 0 @@ -50,41 +53,39 @@ Visualization Manager: Reference Frame: world Value: true - Alpha: 0.5 - Cell Size: 0.100000001 + Cell Size: 0.10000000149011612 Class: rviz/Grid Color: 255; 255; 0 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 - Z: 0.00999999978 + Z: 0.009999999776482582 Plane: XY Plane Cell Count: 20 Reference Frame: Value: true - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true Length: 3 Name: Axes - Radius: 0.00999999978 + Radius: 0.009999999776482582 Reference Frame: world + Show Trail: false Value: true - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: false - ar_tag: - Value: true - body: - Value: true - carriage: - Value: true dock/berth1: Value: true dock/berth1/approach: @@ -99,133 +100,152 @@ Visualization Manager: Value: true dock/body: Value: true - dock_cam: + granite/body: Value: true - flashlight_aft: + iss/body: Value: true - flashlight_front: + perch/body: Value: true - granite/body: + queen/ar_tag: Value: true - handrail/approach: + queen/body: Value: true - handrail/body: + queen/carriage: Value: true - handrail/complete: + queen/dock/body: Value: true - haz_cam: + queen/dock_cam: Value: true - imu: + queen/flashlight_aft: Value: true - inertial_link: + queen/flashlight_front: Value: true - iss/body: + queen/handrail/approach: Value: true - laser: + queen/handrail/body: Value: true - nav_cam: + queen/handrail/complete: Value: true - payload/bottom_aft: + queen/haz_cam: Value: true - payload/bottom_front: + queen/imu: Value: true - payload/top_aft: + queen/inertial_link: Value: true - payload/top_front: + queen/laser: Value: true - perch_cam: + queen/nav_cam: Value: true - rviz: + queen/payload/bottom_aft: + Value: true + queen/payload/bottom_front: + Value: true + queen/payload/top_aft: + Value: true + queen/payload/top_front: + Value: true + queen/perch_cam: + Value: true + queen/sci_cam: Value: true - top_aft: + queen/top_aft: Value: true - top_aft_arm_distal_link: + queen/top_aft_arm_distal_link: Value: true - top_aft_arm_proximal_link: + queen/top_aft_arm_proximal_link: Value: true - top_aft_gripper_left_distal_link: + queen/top_aft_gripper_left_distal_link: Value: true - top_aft_gripper_left_proximal_link: + queen/top_aft_gripper_left_proximal_link: Value: true - top_aft_gripper_right_distal_link: + queen/top_aft_gripper_right_distal_link: Value: true - top_aft_gripper_right_proximal_link: + queen/top_aft_gripper_right_proximal_link: Value: true - truth: + queen/truth: + Value: true + rviz: Value: true world: Value: true - Marker Scale: 0.300000012 + Marker Alpha: 1 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: false Show Axes: true Show Names: true Tree: + queen/handrail/body: + queen/handrail/approach: + {} + queen/handrail/complete: + {} world: - body: - ar_tag: + dock/body: + dock/berth1: + dock/berth1/approach: + {} + dock/berth1/complete: + {} + dock/berth2: + dock/berth2/approach: + {} + dock/berth2/complete: + {} + granite/body: + {} + iss/body: + {} + perch/body: + {} + queen/body: + queen/ar_tag: {} - carriage: + queen/carriage: {} - dock_cam: + queen/dock_cam: {} - flashlight_aft: + queen/flashlight_aft: {} - flashlight_front: + queen/flashlight_front: {} - haz_cam: + queen/haz_cam: {} - imu: + queen/imu: {} - inertial_link: + queen/inertial_link: {} - laser: + queen/laser: {} - nav_cam: + queen/nav_cam: {} - payload/bottom_aft: + queen/payload/bottom_aft: {} - payload/bottom_front: + queen/payload/bottom_front: {} - payload/top_aft: + queen/payload/top_aft: {} - payload/top_front: + queen/payload/top_front: {} - perch_cam: + queen/perch_cam: {} - top_aft: - top_aft_arm_proximal_link: - top_aft_arm_distal_link: - top_aft_gripper_left_proximal_link: - top_aft_gripper_left_distal_link: + queen/sci_cam: + {} + queen/top_aft: + queen/top_aft_arm_proximal_link: + queen/top_aft_arm_distal_link: + queen/top_aft_gripper_left_proximal_link: + queen/top_aft_gripper_left_distal_link: {} - top_aft_gripper_right_proximal_link: - top_aft_gripper_right_distal_link: + queen/top_aft_gripper_right_proximal_link: + queen/top_aft_gripper_right_distal_link: {} - dock/body: - dock/berth1: - dock/berth1/approach: - {} - dock/berth1/complete: - {} - dock/berth2: - dock/berth2/approach: - {} - dock/berth2/complete: - {} - granite/body: + queen/dock/body: {} - handrail/body: - handrail/approach: - {} - handrail/complete: - {} - iss/body: + queen/truth: {} rviz: {} - truth: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -303,7 +323,7 @@ Visualization Manager: Link Tree Style: Links in Alphabetic Order Name: Honey Robot Description: /honey/robot_description - TF Prefix: honey + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -319,7 +339,7 @@ Visualization Manager: Link Tree Style: Links in Alphabetic Order Name: Bumble Robot Description: /bumble/robot_description - TF Prefix: bumble + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -333,9 +353,58 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + queen/body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/carriage: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/inertial_link: + Alpha: 1 + Show Axes: false + Show Trail: false + queen/top_aft: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_arm_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_arm_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_gripper_left_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_gripper_left_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_gripper_right_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_gripper_right_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: Queen Robot Description: /queen/robot_description - TF Prefix: queen + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -351,55 +420,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - carriage: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - inertial_link: - Alpha: 1 - Show Axes: false - Show Trail: false - top_aft: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Name: / Robot Description: /robot_description TF Prefix: "" @@ -455,8 +475,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.541443229 - Min Value: -0.164811939 + Max Value: 0.5414432287216187 + Min Value: -0.16481193900108337 Value: true Axis: Y Channel Name: intensity @@ -467,15 +487,13 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: HazCam Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Points Topic: /hw/depth_haz/points Unreliable: false @@ -497,15 +515,13 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: PerchCam Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Points Topic: /hw/depth_perch/points Unreliable: false @@ -531,15 +547,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: Features Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.0500000007 + Size (m): 0.05000000074505806 Style: Spheres Topic: /gnc/ekf/features Unreliable: false @@ -548,15 +562,16 @@ Visualization Manager: Value: true - Alpha: 1 Axes Length: 1 - Axes Radius: 0.100000001 + Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 255; 0 Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 Name: Ground Truth + Queue Size: 10 Shaft Length: 0.5 - Shaft Radius: 0.00999999978 + Shaft Radius: 0.009999999776482582 Shape: Arrow Topic: /loc/truth Unreliable: false @@ -578,11 +593,11 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: CurrentPlan Offset: X: 0 @@ -590,9 +605,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /mob/choreographer/segment Unreliable: false Value: true @@ -602,10 +618,11 @@ Visualization Manager: Color: 204; 51; 204 Enabled: true History Length: 1 - Line Thickness: 0.100000001 + Line Thickness: 0.10000000149011612 Name: Trajectory Plot Acceleration: false Plot Velocity: true + Queue Size: 10 Style: Mike Tangent Samples: 25 Topic: /planner_qp_manager/trajectory @@ -618,7 +635,7 @@ Visualization Manager: Marker Topic: /mob/mapper/zones Name: Zones Namespaces: - zone_visualization: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -626,7 +643,7 @@ Visualization Manager: Marker Topic: /mob/mapper/obstacle_markers Name: Map Namespaces: - obstacleMap: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -656,7 +673,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -666,25 +686,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.06954694 + Distance: 1.0695469379425049 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: -0.0357269906 - Y: -2.39005232 - Z: 1.45076084 + X: -0.035726990550756454 + Y: -2.390052318572998 + Z: 1.450760841369629 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.234798893 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.23479889333248138 Target Frame: world - Value: Orbit (rviz) - Yaw: 4.71003866 + Yaw: 4.710038661956787 Saved: ~ Window Geometry: "DEBUG: DockCam": @@ -696,7 +716,7 @@ Window Geometry: Height: 884 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -706,5 +726,5 @@ Window Geometry: Views: collapsed: false Width: 1081 - X: 0 - Y: 0 + X: 46 + Y: 27 diff --git a/astrobee/resources/rviz/iss.rviz b/astrobee/resources/rviz/iss.rviz index 5a2851dd70..60279c7f72 100644 --- a/astrobee/resources/rviz/iss.rviz +++ b/astrobee/resources/rviz/iss.rviz @@ -4,12 +4,11 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Handrail1 - /Debug1 - /Debug1/Sensors1 - /Debug1/Mobility1/Mapper1 - Splitter Ratio: 0.579250693 - Tree Height: 187 + Splitter Ratio: 0.579250693321228 + Tree Height: 202 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -18,7 +17,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -28,7 +27,11 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Features + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -37,10 +40,6 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - ar_tag: - Value: true - body: - Value: true dock/berth1: Value: true dock/berth1/approach: @@ -55,12 +54,6 @@ Visualization Manager: Value: true dock/body: Value: true - dock_cam: - Value: true - flashlight_aft: - Value: true - flashlight_front: - Value: true granite/body: Value: true handrail/approach: @@ -69,160 +62,179 @@ Visualization Manager: Value: true handrail/complete: Value: true - haz_cam: + honey/ar_tag: Value: true - imu: + honey/body: Value: true - inertial_link: + honey/dock/body: Value: true - iss/body: + honey/dock_cam: Value: true - laser: + honey/flashlight_aft: Value: true - nav_cam: + honey/flashlight_front: Value: true - payload/bottom_aft: + honey/haz_cam: Value: true - payload/bottom_front: + honey/imu: Value: true - payload/top_aft: + honey/inertial_link: Value: true - payload/top_front: + honey/laser: Value: true - perch_cam: + honey/nav_cam: Value: true - rviz: + honey/payload/bottom_aft: + Value: true + honey/payload/bottom_front: Value: true - top_aft: + honey/payload/top_aft: Value: true - top_aft_arm_distal_link: + honey/payload/top_front: Value: true - top_aft_arm_proximal_link: + honey/perch_cam: Value: true - top_aft_gripper_left_distal_link: + honey/sci_cam: Value: true - top_aft_gripper_left_proximal_link: + honey/top_aft: Value: true - top_aft_gripper_right_distal_link: + honey/top_aft_arm_distal_link: + Value: true + honey/top_aft_arm_proximal_link: + Value: true + honey/top_aft_gripper_left_distal_link: + Value: true + honey/top_aft_gripper_left_proximal_link: + Value: true + honey/top_aft_gripper_right_distal_link: + Value: true + honey/top_aft_gripper_right_proximal_link: + Value: true + honey/truth: + Value: true + iss/body: Value: true - top_aft_gripper_right_proximal_link: + perch/body: Value: true - truth: + rviz: Value: true world: Value: true - Marker Scale: 0.300000012 + Marker Alpha: 1 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: false Show Axes: true Show Names: true Tree: world: - body: - ar_tag: + dock/body: + dock/berth1: + dock/berth1/approach: + {} + dock/berth1/complete: + {} + dock/berth2: + dock/berth2/approach: + {} + dock/berth2/complete: + {} + granite/body: + {} + honey/body: + honey/ar_tag: + {} + honey/dock_cam: {} - dock_cam: + honey/flashlight_aft: {} - flashlight_aft: + honey/flashlight_front: {} - flashlight_front: + honey/haz_cam: {} - haz_cam: + honey/imu: {} - imu: + honey/inertial_link: {} - inertial_link: + honey/laser: {} - laser: + honey/nav_cam: {} - nav_cam: + honey/payload/bottom_aft: {} - payload/bottom_aft: + honey/payload/bottom_front: {} - payload/bottom_front: + honey/payload/top_aft: {} - payload/top_aft: + honey/payload/top_front: {} - payload/top_front: + honey/perch_cam: {} - perch_cam: + honey/sci_cam: {} - top_aft: - top_aft_arm_proximal_link: - top_aft_arm_distal_link: - top_aft_gripper_left_proximal_link: - top_aft_gripper_left_distal_link: + honey/top_aft: + honey/top_aft_arm_proximal_link: + honey/top_aft_arm_distal_link: + honey/top_aft_gripper_left_proximal_link: + honey/top_aft_gripper_left_distal_link: {} - top_aft_gripper_right_proximal_link: - top_aft_gripper_right_distal_link: + honey/top_aft_gripper_right_proximal_link: + honey/top_aft_gripper_right_distal_link: {} - dock/body: - dock/berth1: - dock/berth1/approach: - {} - dock/berth1/complete: - {} - dock/berth2: - dock/berth2/approach: - {} - dock/berth2/complete: - {} - granite/body: + honey/dock/body: + {} + honey/truth: {} - handrail/body: - handrail/approach: - {} - handrail/complete: - {} iss/body: {} - rviz: + perch/body: {} - truth: + rviz: {} Update Interval: 0 Value: true - Class: rviz/Group Displays: - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true Length: 3 Name: Axes - Radius: 0.00999999978 + Radius: 0.009999999776482582 Reference Frame: world Value: true - - Alpha: 0.100000001 + - Alpha: 0.10000000149011612 Cell Size: 1 Class: rviz/Grid Color: 255; 255; 127 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Billboards Name: Grid 1m Normal Cell Count: 0 Offset: X: 0 Y: 0 - Z: 4.8499999 + Z: 4.849999904632568 Plane: XY Plane Cell Count: 100 Reference Frame: world Value: true - - Alpha: 0.100000001 - Cell Size: 0.100000001 + - Alpha: 0.10000000149011612 + Cell Size: 0.10000000149011612 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid 0.1m Normal Cell Count: 0 Offset: X: 0 Y: 0 - Z: 4.8499999 + Z: 4.849999904632568 Plane: XY Plane Cell Count: 1000 Reference Frame: world @@ -233,14 +245,14 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid 10m Normal Cell Count: 0 Offset: X: 0 Y: 0 - Z: 4.8499999 + Z: 4.849999904632568 Plane: XY Plane Cell Count: 10 Reference Frame: world @@ -320,9 +332,53 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + honey/body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + honey/inertial_link: + Alpha: 1 + Show Axes: false + Show Trail: false + honey/top_aft: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + honey/top_aft_arm_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + honey/top_aft_arm_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + honey/top_aft_gripper_left_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + honey/top_aft_gripper_left_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + honey/top_aft_gripper_right_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + honey/top_aft_gripper_right_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: Honey Robot Description: /honey/robot_description - TF Prefix: honey + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -338,7 +394,7 @@ Visualization Manager: Link Tree Style: Links in Alphabetic Order Name: Bumble Robot Description: /bumble/robot_description - TF Prefix: bumble + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -352,9 +408,53 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + queen/body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/inertial_link: + Alpha: 1 + Show Axes: false + Show Trail: false + queen/top_aft: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_arm_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_arm_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_gripper_left_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_gripper_left_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_gripper_right_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + queen/top_aft_gripper_right_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: Queen Robot Description: /queen/robot_description - TF Prefix: queen + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -370,50 +470,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - inertial_link: - Alpha: 1 - Show Axes: false - Show Trail: false - top_aft: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Name: / Robot Description: /robot_description TF Prefix: "" @@ -475,7 +531,7 @@ Visualization Manager: Name: "DEBUG: SciCam" Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed Unreliable: false Value: false - Class: rviz/Image @@ -493,8 +549,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.541443229 - Min Value: -0.164811939 + Max Value: 0.5414432287216187 + Min Value: -0.16481193900108337 Value: true Axis: Y Channel Name: intensity @@ -505,15 +561,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: HazCam Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Points Topic: /hw/depth_haz/points Unreliable: false @@ -535,15 +589,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: PerchCam Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Points Topic: /hw/depth_perch/points Unreliable: false @@ -569,15 +621,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: Features Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.0500000007 + Size (m): 0.05000000074505806 Style: Spheres Topic: /gnc/ekf/features Unreliable: false @@ -586,15 +636,16 @@ Visualization Manager: Value: true - Alpha: 1 Axes Length: 1 - Axes Radius: 0.100000001 + Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 255; 0 Enabled: true - Head Length: 0.300000012 - Head Radius: 0.100000001 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 Name: Ground Truth + Queue Size: 10 Shaft Length: 0.5 - Shaft Radius: 0.00999999978 + Shaft Radius: 0.009999999776482582 Shape: Arrow Topic: /loc/truth Unreliable: false @@ -612,7 +663,7 @@ Visualization Manager: Marker Topic: /loc/registration Name: Registration Namespaces: - thick_traj: true + {} Queue Size: 100 Value: true Enabled: true @@ -624,11 +675,11 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: CurrentPlan Offset: X: 0 @@ -636,9 +687,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /mob/choreographer/segment Unreliable: false Value: true @@ -658,10 +710,11 @@ Visualization Manager: Color: 204; 51; 204 Enabled: true History Length: 1 - Line Thickness: 0.100000001 + Line Thickness: 0.10000000149011612 Name: Trajectory Plot Acceleration: false Plot Velocity: false + Queue Size: 10 Style: Mike Tangent Samples: 25 Topic: /mob/planner_qp/trajectory @@ -675,10 +728,11 @@ Visualization Manager: Color: 255; 0; 0 Enabled: true History Length: 1 - Line Thickness: 0.100000001 + Line Thickness: 0.10000000149011612 Name: TrajectoryDebug Plot Acceleration: false Plot Velocity: false + Queue Size: 10 Style: Mike Tangent Samples: 10 Topic: /mob/planner_qp/trajectory_debug @@ -695,7 +749,7 @@ Visualization Manager: Marker Topic: /mob/mapper/zones Name: Zones Namespaces: - zone_visualization: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -714,7 +768,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Alpha: 0.100000001 + - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -729,15 +783,13 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: FreeSpace Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.100000001 + Size (m): 0.10000000149011612 Style: Boxes Topic: /mob/mapper/free_space_cloud Unreliable: false @@ -747,8 +799,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -3.6500001 - Min Value: -5.3499999 + Max Value: -3.6500000953674316 + Min Value: -5.349999904632568 Value: true Axis: Z Channel Name: intensity @@ -759,15 +811,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: Map Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.100000001 + Size (m): 0.10000000149011612 Style: Boxes Topic: /mob/mapper/obstacle_cloud Unreliable: false @@ -795,7 +845,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -806,21 +859,21 @@ Visualization Manager: Current: Class: rviz/FPS Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.959796906 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8697966933250427 Position: - X: 8.61772633 - Y: 8.51602364 - Z: -1.08066916 + X: 7.894586086273193 + Y: 8.00870418548584 + Z: -1.0062958002090454 + Roll: 0 Target Frame: rviz - Value: FPS (rviz) - Yaw: 0.420246959 + Yaw: 0.5702470541000366 Saved: ~ Window Geometry: "DEBUG: DockCam": @@ -836,7 +889,7 @@ Window Geometry: Height: 848 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -846,5 +899,5 @@ Window Geometry: Views: collapsed: false Width: 1076 - X: 0 - Y: 6 + X: 72 + Y: 27 diff --git a/astrobee/resources/rviz/iss_viz.rviz b/astrobee/resources/rviz/iss_viz.rviz index e1481638f6..b35796e36c 100644 --- a/astrobee/resources/rviz/iss_viz.rviz +++ b/astrobee/resources/rviz/iss_viz.rviz @@ -475,7 +475,7 @@ Visualization Manager: Name: "DEBUG: SciCam" Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed Unreliable: false Value: false - Class: rviz/Image diff --git a/astrobee/resources/rviz/loc_granite.rviz b/astrobee/resources/rviz/loc_granite.rviz new file mode 100644 index 0000000000..5317085a22 --- /dev/null +++ b/astrobee/resources/rviz/loc_granite.rviz @@ -0,0 +1,275 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Graph1 + - /Image1 + - /PoseDisplay1 + - /Granite1/Links1 + - /Granite1/Links1/body1 + Splitter Ratio: 0.5 + Tree Height: 178 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: localization_rviz_plugins/GraphPanel + Name: GraphPanel +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: localization_rviz_plugins/Graph + Enabled: true + Imu Factor Arrows Diameter: 0.00999999978 + Name: Graph + Pose Axes Size: 0.100000001 + Show Imu Factor Arrows: true + Show Pose Axes: true + Topic: /graph_loc/graph + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /graph_localizer/optical_flow_feature_tracks + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: localization_rviz_plugins/PoseDisplay + Enabled: true + Name: PoseDisplay + Number of Poses: 10 + Pose Axes Size: 0.100000001 + Topic: /sparse_mapping/pose + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: false + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + carriage: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + inertial_link: + Alpha: 1 + Show Axes: false + Show Trail: false + top_aft: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_arm_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_arm_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_gripper_left_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_gripper_left_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_gripper_right_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_gripper_right_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + Name: Robot + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: Granite + Robot Description: /granite/robot_description + TF Prefix: granite + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: localization_rviz_plugins/SparseMappingDisplay + Enabled: true + Name: SparseMappingDisplay + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /sparse_mapping/map_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: rviz + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.82393694 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.106598854 + Y: 0.172128871 + Z: -0.198201209 + Focal Shape Fixed Size: false + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.445203364 + Target Frame: body + Value: Orbit (rviz) + Yaw: 3.74267197 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + GraphPanel: + collapsed: false + Height: 1041 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000020c00000372fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000000f3000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000013a000001550000001600fffffffb000000140047007200610070006800500061006e0065006c01000002950000011e0000011e00ffffff000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004100000372000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077c0000003efc0100000002fb0000000800540069006d006501000000000000077c0000027600fffffffb0000000800540069006d006501000000000000045000000000000000000000056a0000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1916 + X: 0 + Y: 18 diff --git a/astrobee/resources/rviz/loc_iss.rviz b/astrobee/resources/rviz/loc_iss.rviz new file mode 100644 index 0000000000..d4355a340e --- /dev/null +++ b/astrobee/resources/rviz/loc_iss.rviz @@ -0,0 +1,273 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Graph1 + - /Image1 + - /PoseDisplay1 + Splitter Ratio: 0.5 + Tree Height: 178 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: localization_rviz_plugins/GraphPanel + Name: GraphPanel +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: localization_rviz_plugins/Graph + Enabled: true + Imu Factor Arrows Diameter: 0.00999999978 + Name: Graph + Pose Axes Size: 0.100000001 + Show Imu Factor Arrows: true + Show Pose Axes: true + Topic: /graph_loc/graph + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /graph_localizer/optical_flow_feature_tracks + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: localization_rviz_plugins/PoseDisplay + Enabled: true + Name: PoseDisplay + Number of Poses: 10 + Pose Axes Size: 0.100000001 + Topic: /sparse_mapping/pose + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + carriage: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + inertial_link: + Alpha: 1 + Show Axes: false + Show Trail: false + top_aft: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_arm_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_arm_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_gripper_left_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_gripper_left_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_gripper_right_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_gripper_right_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: Robot + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: ISS + Robot Description: /iss/robot_description + TF Prefix: iss + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: localization_rviz_plugins/SparseMappingDisplay + Enabled: true + Name: SparseMappingDisplay + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /sparse_mapping/map_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: rviz + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.08937788 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.240398735 + Target Frame: body + Value: Orbit (rviz) + Yaw: 1.66722131 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + GraphPanel: + collapsed: false + Height: 1041 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000020c00000372fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000000f3000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000013a000001550000001600fffffffb000000140047007200610070006800500061006e0065006c01000002950000011e0000011e00ffffff000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004100000372000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bc0000003efc0100000002fb0000000800540069006d00650100000000000003bc0000027600fffffffb0000000800540069006d00650100000000000004500000000000000000000001aa0000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 956 + X: 960 + Y: 18 diff --git a/astrobee/resources/zones/iss.bin b/astrobee/resources/zones/iss.bin index 764c196a4c..bd34b32f09 100644 Binary files a/astrobee/resources/zones/iss.bin and b/astrobee/resources/zones/iss.bin differ diff --git a/astrobee/scripts/apk_print_version.sh b/astrobee/scripts/apk_print_version.sh new file mode 100755 index 0000000000..05964d414b --- /dev/null +++ b/astrobee/scripts/apk_print_version.sh @@ -0,0 +1,23 @@ +#!/bin/bash + +function apk_version { + VERSION=$(adb shell dumpsys package $1 | grep versionName) + if [ $? -eq 0 ]; then + echo $VERSION + else + echo "None Installed" + fi +} + +adb connect hlp + +echo "APKs:" +echo " cpu monitor: $(apk_version gov.nasa.arc.astrobee.cpu_monitor)" +echo " disk monitor: $(apk_version gov.nasa.arc.astrobee.disk_monitor)" +echo " guest science manager: $(apk_version gov.nasa.arc.astrobee.android.gs.manager)" +echo " mic test: $(apk_version gov.nasa.arc.irg.astrobee.mictest)" +echo " signal intention state: $(apk_version gov.nasa.arc.astrobee.signal_intention_state)" +echo " sci cam: $(apk_version gov.nasa.arc.astrobee.android.scicam)" +echo " set wallpaper: $(apk_version gov.nasa.arc.astrobee.set_wallpaper)" +echo " wifi setup helper: $(apk_version gov.nasa.arc.irg.astrobee.wifisetup)" +echo " battery monitor: $(apk_version gov.nasa.arc.irg.astrobee.battery_monitor)" diff --git a/astrobee/scripts/cpu_print_version.sh b/astrobee/scripts/cpu_print_version.sh new file mode 100755 index 0000000000..ed0a17a6a8 --- /dev/null +++ b/astrobee/scripts/cpu_print_version.sh @@ -0,0 +1,28 @@ +#!/bin/bash + +function deb_version { + VERSION=$(dpkg -s $1 2>/dev/null | grep -oP 'Version: \K.+') + if [ $? -eq 0 ]; then + echo $VERSION + else + echo "None Installed" + fi +} + +source /opt/ros/noetic/setup.bash +date +echo "Kernel: $(uname -r)" +lsb_release -r +echo "Ros $(rosversion -d) $(rosversion roscpp)" +echo "Debians:" +echo " astrobee-comms: $(deb_version astrobee-comms)" +echo " astrobee0: $(deb_version astrobee0)" +echo " astrobee-config: $(deb_version astrobee-config)" +echo " astrobee-avionics: $(deb_version astrobee-avionics)" +echo "FSW Version:" +cat /opt/astrobee/version.txt | sed 's/^/ /' +echo "Modified Files:" +dpkg -V astrobee-comms | sed 's/^/ /' +dpkg -V astrobee0 | sed 's/^/ /' +dpkg -V astrobee-config | sed 's/^/ /' +dpkg -V astrobee-avionics | sed 's/^/ /' diff --git a/scripts/deploy/env_wrapper.sh b/astrobee/scripts/env_wrapper.sh similarity index 79% rename from scripts/deploy/env_wrapper.sh rename to astrobee/scripts/env_wrapper.sh index c372e20e3c..a5edcb76d6 100755 --- a/scripts/deploy/env_wrapper.sh +++ b/astrobee/scripts/env_wrapper.sh @@ -17,7 +17,17 @@ # License for the specific language governing permissions and limitations # under the License. -export ROS_DISTRO=kinetic +ubuntu_version=$(cat /etc/os-release | grep -oP "(?<=VERSION_CODENAME=).*") +if [ "$ubuntu_version" = "xenial" ]; then + export ROS_DISTRO=kinetic + export PYTHON_VERSION=2.7 +elif [ "$ubuntu_version" = "focal" ]; then + export ROS_DISTRO=noetic + export PYTHON_VERSION=3 +else + echo "Unsupported OS release. Found: $ubuntu_version" + exit 1 +fi # this script sets the ROS_IP, without it the other processors send back their hostname which can't be resolved if [ -e /etc/ros.sh ]; then @@ -42,7 +52,7 @@ export ROS_PACKAGE_PATH=${ff_root}/share:${ff_root}/stacks:${ros_root}/share:${r export LD_LIBRARY_PATH=${ff_root}/lib:${dds_root}:${ros_root}/lib:${ros_root}/lib/arm-linux-gnueabihf:/usr/local/lib:/lib:/usr/local/lib:${ff_root}/bin export PATH=${ff_root}/bin:${ros_root}/bin:/usr/local/bin:/usr/bin:/bin:/usr/sbin:/sbin -export PYTHONPATH=${ff_root}/lib/python2.7/dist-packages:${ros_root}/lib/python2.7/dist-packages +export PYTHONPATH=${ff_root}/lib/python${PYTHON_VERSION}/dist-packages:${ros_root}/lib/python${PYTHON_VERSION}/dist-packages export CMAKE_PREFIX_PATH=${ff_root}:${ros_root} unset dds_root diff --git a/astrobee/scripts/print_version.sh b/astrobee/scripts/print_version.sh new file mode 100755 index 0000000000..f07c634e8e --- /dev/null +++ b/astrobee/scripts/print_version.sh @@ -0,0 +1,23 @@ +#!/bin/bash + +# Print the software versions on the MLP, LLP, and HLP. + +if [ `hostname` != mlp ] ; then + echo "Must be run on mlp." + exit 1 +fi + +DIR=$(dirname "$(readlink -f "$0")") + +echo "=================== MLP ===================" +$DIR/cpu_print_version.sh | sed 's/^/ /' + +echo "=================== LLP ===================" +ssh llp /opt/astrobee/lib/astrobee/cpu_print_version.sh | sed 's/^/ /' + +echo "=================== HLP ===================" + +$DIR/apk_print_version.sh | sed 's/versionName=/ /' + +echo "=================== Map ===================" +echo "Map: $(sha256sum /res/maps/iss.map)" diff --git a/behaviors/CMakeLists.txt b/behaviors/CMakeLists.txt deleted file mode 100644 index 41b13472bc..0000000000 --- a/behaviors/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -add_subdirectory(arm) -add_subdirectory(dock) -add_subdirectory(states) -add_subdirectory(light_flow) -add_subdirectory(perch) - - diff --git a/behaviors/arm/CMakeLists.txt b/behaviors/arm/CMakeLists.txt index ea201b11fc..179a5bb305 100644 --- a/behaviors/arm/CMakeLists.txt +++ b/behaviors/arm/CMakeLists.txt @@ -15,8 +15,22 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(arm) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + std_msgs + nodelet + ff_msgs + ff_hw_msgs + ff_util +) + catkin_package( LIBRARIES arm @@ -27,33 +41,59 @@ catkin_package( actionlib sensor_msgs ff_msgs + ff_hw_msgs ff_util ) -create_library( - TARGET - arm - LIBS - ${catkin_LIBRARIES} - ff_nodelet - config_server - INC - ${catkin_INCLUDE_DIRS} - DEPS - ff_msgs +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_tool_targets( - DIR - tools - LIBS - ${catkin_LIBRARIES} - ff_common - ff_nodelet - INC - ${catkin_INCLUDE_DIRS} - DEPS - ff_msgs +# Declare C++ libraries +add_library(arm + src/arm_nodelet.cc ) +add_dependencies(arm ${catkin_EXPORTED_TARGETS}) +target_link_libraries(arm ${catkin_LIBRARIES}) + +## Declare a C++ executable: arm_tool +add_executable(arm_tool tools/arm_tool.cc) +add_dependencies(arm_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(arm_tool + arm gflags ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS arm_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/arm_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/behaviors/arm/package.xml b/behaviors/arm/package.xml index cdb1f13dc2..bbd2b0aade 100644 --- a/behaviors/arm/package.xml +++ b/behaviors/arm/package.xml @@ -20,6 +20,7 @@ pluginlib sensor_msgs ff_msgs + ff_hw_msgs ff_util roscpp nodelet @@ -27,6 +28,7 @@ actionlib sensor_msgs ff_msgs + ff_hw_msgs ff_util diff --git a/behaviors/arm/src/arm_nodelet.cc b/behaviors/arm/src/arm_nodelet.cc index c5e2b28651..0eb679d40a 100644 --- a/behaviors/arm/src/arm_nodelet.cc +++ b/behaviors/arm/src/arm_nodelet.cc @@ -156,9 +156,8 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { // STOWED -> DEPLOYING_PANNING // [label="[5]\nGOAL_DEPLOY\nPan(DEPLOY)"]; - fsm_.Add(STATE::STOWED, - GOAL_DEPLOY, - [this](FSM::Event const& event) -> FSM::State { + fsm_.Add(GOAL_DEPLOY, + [this](FSM::State const& state, FSM::Event const& event) -> FSM::State { if (!Arm(PAN)) return Result(RESPONSE::PAN_FAILED); return STATE::DEPLOYING_PANNING; @@ -197,6 +196,11 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::DEPLOYING_TILTING, TILT_COMPLETE, [this](FSM::Event const& event) -> FSM::State { + if (!calibrated_) { + if (!CalibrateGripper()) + return Result(RESPONSE::CALIBRATE_FAILED); + return STATE::CALIBRATING; + } return Result(RESPONSE::SUCCESS); }); @@ -253,8 +257,8 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { return STATE::STOWING_SETTING; else if (goal_set_) return STATE::SETTING; - else // We don't know where we were, return error - return Result(RESPONSE::NO_GOAL); + else // Finished + return Result(RESPONSE::SUCCESS); } if (!Arm(PAN)) return Result(RESPONSE::PAN_FAILED); @@ -514,7 +518,7 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { sub_joint_states_ = nh->subscribe(TOPIC_JOINT_STATES, 1, &ArmNodelet::JointStateCallback, this); pub_joint_goals_ = nh->advertise( - TOPIC_JOINT_GOALS, 1, true); + TOPIC_JOINT_GOALS, 1); // Subscribe to Proximal Joint Servo Enabling service client_enable_prox_servo_ = nh->serviceClient( @@ -945,9 +949,6 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { // Deploy the arm case ff_msgs::ArmGoal::ARM_DEPLOY: NODELET_DEBUG_STREAM("Received a new ARM_DEPLOY command"); - if (fsm_.GetState() == STATE::DEPLOYED) { - Result(RESPONSE::SUCCESS, true); - } joints_[PAN].goal = K_PAN_DEPLOY; joints_[TILT].goal = K_TILT_DEPLOY; joints_[GRIPPER].goal = K_GRIPPER_DEPLOY; @@ -989,7 +990,10 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { Result(RESPONSE::BAD_PAN_VALUE, true); return; } - if (new_t > K_TILT_SAFE && fabs(new_p - K_PAN_STOW) > joints_[PAN].tol) { + // Check current and goal tilt, and if close to stowed, make + // sure the pan value is zero within tolerance + if ((joints_[TILT].goal > K_TILT_SAFE || new_t > K_TILT_SAFE) + && fabs(new_p - K_PAN_STOW) > joints_[PAN].tol) { Result(RESPONSE::COLLISION_AVOIDED, true); return; } diff --git a/behaviors/arm/tools/arm_tool.cc b/behaviors/arm/tools/arm_tool.cc index 1ff952b689..2b7a55426a 100644 --- a/behaviors/arm/tools/arm_tool.cc +++ b/behaviors/arm/tools/arm_tool.cc @@ -23,7 +23,7 @@ // Include RPOS #include // FSW includes -#include +#include #include #include diff --git a/behaviors/dock/CMakeLists.txt b/behaviors/dock/CMakeLists.txt index 41afe25811..dbb2aa8c96 100644 --- a/behaviors/dock/CMakeLists.txt +++ b/behaviors/dock/CMakeLists.txt @@ -15,8 +15,24 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(dock) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + ff_hw_msgs + ff_msgs + ff_util +) + catkin_package( LIBRARIES dock @@ -31,34 +47,55 @@ catkin_package( ff_util ) -create_library( - TARGET - dock - LIBS - ${catkin_LIBRARIES} - ff_nodelet - config_reader - msg_conversions - config_server - config_client - INC - ${catkin_INCLUDE_DIRS} - DEPS - ff_msgs - ff_hw_msgs +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_tool_targets( - DIR - tools - LIBS - ${catkin_LIBRARIES} - ff_common - ff_nodelet - INC - ${catkin_INCLUDE_DIRS} - DEPS - ff_msgs +# Declare C++ libraries +add_library(dock + src/dock_nodelet.cc ) +add_dependencies(dock ${catkin_EXPORTED_TARGETS}) +target_link_libraries(dock ${catkin_LIBRARIES}) + +## Declare a C++ executable: dock_tool +add_executable(dock_tool tools/dock_tool.cc) +add_dependencies(dock_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(dock_tool + dock gflags ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS dock_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/dock_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/behaviors/dock/src/dock_nodelet.cc b/behaviors/dock/src/dock_nodelet.cc index bbb22fbae6..b1edbe3aa4 100644 --- a/behaviors/dock/src/dock_nodelet.cc +++ b/behaviors/dock/src/dock_nodelet.cc @@ -43,6 +43,7 @@ // Services #include #include +#include // Actions #include @@ -133,6 +134,14 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { // [3] fsm_.Add(STATE::UNDOCKED, GOAL_DOCK, [this](FSM::Event const& event) -> FSM::State { + ff_msgs::GetPipelines srv; + if (client_l_.Call(srv)) { + if (!srv.response.pipelines.empty() && srv.response.pipelines[0].id == "ar") { + // Move to the approach pose using AR localization + Move(APPROACH_POSE, ff_msgs::MotionGoal::NOMINAL); + return STATE::DOCKING_MOVING_TO_APPROACH_POSE; + } + } Switch(LOCALIZATION_MAPPED_LANDMARKS); return STATE::DOCKING_SWITCHING_TO_ML_LOC; }); @@ -146,7 +155,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::DOCKING_SWITCHING_TO_ML_LOC, SWITCH_FAILED, [this](FSM::Event const& event) -> FSM::State { Result(RESPONSE::SWITCH_TO_ML_FAILED, - "Could not switch to mapped landmark localization"); + "Could not switch to mapped landmark localization. " + err_msg_); return STATE::UNDOCKED; }); // [6] @@ -159,27 +168,27 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::DOCKING_MOVING_TO_APPROACH_POSE, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { err_ = RESPONSE::MOTION_APPROACH_FAILED; - err_msg_ = "Failed while moving to the approach pose"; + err_msg_ = "Failed while moving to the approach pose. " + err_msg_; Switch(LOCALIZATION_MAPPED_LANDMARKS); return STATE::RECOVERY_SWITCHING_TO_ML_LOC; }); // [31] fsm_.Add(STATE::DOCKING_SWITCHING_TO_AR_LOC, SWITCH_SUCCESS, [this](FSM::Event const& event) -> FSM::State { - Move(BERTHING_POSE, ff_msgs::MotionGoal::DOCKING); + Move(BERTHING_POSE, ff_msgs::MotionGoal::PRECISION); return STATE::DOCKING_MOVING_TO_COMPLETE_POSE; }); // [32] fsm_.Add(STATE::DOCKING_SWITCHING_TO_AR_LOC, SWITCH_FAILED, [this](FSM::Event const& event) -> FSM::State { Result(RESPONSE::SWITCH_TO_AR_FAILED, - "Could not switch to marker tracking localization"); + "Could not switch to marker tracking localization. " + err_msg_); return STATE::UNDOCKED; }); // [8] fsm_.Add(STATE::DOCKING_MOVING_TO_COMPLETE_POSE, EPS_DOCKED, [this](FSM::Event const& event) -> FSM::State { - Move(APPROACH_POSE, ff_msgs::MotionGoal::DOCKING); + Move(APPROACH_POSE, ff_msgs::MotionGoal::PRECISION); return STATE::DOCKING_CHECKING_ATTACHED; }); // [9] @@ -192,9 +201,9 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { } if (event == MOTION_FAILED) { err_ = RESPONSE::MOTION_COMPLETE_FAILED; - err_msg_ = "Motion failed trying to go to complete pose"; + err_msg_ = "Motion failed trying to go to complete pose. " + err_msg_; } - Move(APPROACH_POSE, ff_msgs::MotionGoal::DOCKING); + Move(APPROACH_POSE, ff_msgs::MotionGoal::PRECISION); return STATE::RECOVERY_MOVING_TO_APPROACH_POSE; }); // [10] @@ -208,7 +217,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { MOTION_SUCCESS, [this](FSM::Event const& event) -> FSM::State { err_ = RESPONSE::MOTION_ATTACHED_FAILED; err_msg_ = "Check attached failed, magnets didn't stop robot's movement"; - Move(APPROACH_POSE, ff_msgs::MotionGoal::DOCKING); + Move(APPROACH_POSE, ff_msgs::MotionGoal::PRECISION); return STATE::RECOVERY_MOVING_TO_APPROACH_POSE; }); // [12] @@ -221,7 +230,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::DOCKING_SWITCHING_TO_NO_LOC, SWITCH_FAILED, [this](FSM::Event const& event) -> FSM::State { Result(RESPONSE::SWITCH_TO_NO_FAILED, - "Could not turn localization off"); + "Could not turn localization off. " + err_msg_); return STATE::DOCKED; }); // [14] @@ -247,7 +256,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::UNDOCKING_SWITCHING_TO_ML_LOC, SWITCH_FAILED, [this](FSM::Event const& event) -> FSM::State { err_ = RESPONSE::SWITCH_TO_ML_FAILED; - err_msg_ = "Switch to Mapped Landmarks Failed"; + err_msg_ = "Switch to Mapped Landmarks Failed. " + err_msg_; Switch(LOCALIZATION_NONE); return STATE::RECOVERY_SWITCHING_TO_NO_LOC; }); @@ -262,7 +271,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::UNDOCKING_MOVING_TO_APPROACH_POSE, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { Result(RESPONSE::MOTION_APPROACH_FAILED, - "Could not move to approach point"); + "Could not move to approach point. " + err_msg_); return STATE::UNDOCKED; }); // [21] @@ -319,7 +328,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { // [28] fsm_.Add(STATE::DOCKING_WAITING_FOR_SPIN_DOWN, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { - Result(RESPONSE::PREP_DISABLE_FAILED, "Could not spin down motors"); + Result(RESPONSE::PREP_DISABLE_FAILED, "Could not spin down motors. " + err_msg_); Switch(LOCALIZATION_NONE); return STATE::DOCKED; }); @@ -327,7 +336,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::UNDOCKING_WAITING_FOR_SPIN_UP, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { err_ = RESPONSE::PREP_ENABLE_FAILED; - err_msg_ = "Spin up was not successful"; + err_msg_ = "Spin up was not successful. " + err_msg_; Prep(ff_msgs::MotionGoal::OFF); return STATE::RECOVERY_WAITING_FOR_SPIN_DOWN; }); @@ -422,6 +431,9 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { server_set_state_ = nh->advertiseService(SERVICE_BEHAVIORS_DOCK_SET_STATE, &DockNodelet::SetStateCallback, this); + // Query the current localization pipeline + client_l_.Create(nh, SERVICE_LOCALIZATION_MANAGER_GET_CURR_PIPELINE); + // Contact EPS service for undocking client_u_.SetConnectedTimeout(cfg_.Get("timeout_undock_connected")); client_u_.SetConnectedCallback(std::bind( @@ -600,11 +612,11 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { } // Check that we are close enough to the approach pose - bool CloseEnoughToApproach(uint32_t berth) { + bool CloseEnoughToApproach(std::string berth) { try { // Look up the body frame in the berth frame geometry_msgs::TransformStamped tf = tf_buffer_.lookupTransform( - berths_[berth] + "/approach", GetTransform(FRAME_NAME_BODY), + berth + "/approach", GetTransform(FRAME_NAME_BODY), ros::Time(0)); // Copy the transform double d = tf.transform.translation.x * tf.transform.translation.x @@ -620,21 +632,30 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { } bool CheckBerth() { - // Look for the berth and confirm localization is working + // Look for the berth and confirm localization is working std::map::iterator it; - for (it = berths_.begin(); it != berths_.end(); it++) { - try { - // Look up the body frame in the berth frame - geometry_msgs::TransformStamped tf = tf_buffer_.lookupTransform( - it->second + "/complete", GetTransform(FRAME_NAME_BODY), ros::Time(0)); - // Copy the transform - double d = tf.transform.translation.x * tf.transform.translation.x - + tf.transform.translation.y * tf.transform.translation.y - + tf.transform.translation.z * tf.transform.translation.z; - // If we are within some delta of the origin, then we are at this berth - if (sqrt(d) < cfg_.Get("detection_tolerance")) - break; - } catch (tf2::TransformException &ex) {} + ros::Time begin = ros::Time::now(); + // Some number bigger than the tolerance + double d = cfg_.Get("detection_tolerance") * 2; + // Give localization time to stabilize if berth not detected within detection_timeout + while (((ros::Time::now() - begin).toSec() < cfg_.Get("detection_timeout")) + && sqrt(d) > cfg_.Get("detection_tolerance")) { + for (it = berths_.begin(); it != berths_.end(); it++) { + try { + // Look up the body frame in the berth frame + geometry_msgs::TransformStamped tf = tf_buffer_.lookupTransform( + it->second + "/complete", GetTransform(FRAME_NAME_BODY), ros::Time(0)); + // Copy the transform + d = tf.transform.translation.x * tf.transform.translation.x + + tf.transform.translation.y * tf.transform.translation.y + + tf.transform.translation.z * tf.transform.translation.z; + // If we are within some delta of the origin, then we are at this berth + if (sqrt(d) < cfg_.Get("detection_tolerance")) + break; + else + ros::Duration(0.5).sleep(); // sleep for half a second + } catch (tf2::TransformException &ex) {} + } } // Let the use know what's happening if (it == berths_.end()) { @@ -713,6 +734,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { case ff_util::FreeFlyerActionState::SUCCESS: return fsm_.Update(SWITCH_SUCCESS); default: + err_msg_ = "Switch Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")"; return fsm_.Update(SWITCH_FAILED); } } @@ -763,8 +785,9 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { msg.header.frame_id = frame_ + "/approach"; goal.states.push_back(msg); - // if docking - if (fsm_.GetState() == STATE::DOCKING_SWITCHING_TO_ML_LOC) { + // if docking and return to dock enabled + if (!CloseEnoughToApproach(frame_) && + fsm_.GetState() == STATE::DOCKING_SWITCHING_TO_ML_LOC) { // Load parameters from config file std::string planner = cfg_.Get("planner"); bool coll_check = cfg_.Get("enable_collision_checking"); @@ -807,10 +830,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { geometry_msgs::TransformStamped tf = tf_buffer_.lookupTransform( "world", pose.header.frame_id, ros::Time(0)); // Copy the transform - pose.pose.position.x = tf.transform.translation.x; - pose.pose.position.y = tf.transform.translation.y; - pose.pose.position.z = tf.transform.translation.z; - pose.pose.orientation = tf.transform.rotation; + pose.pose = msg_conversions::ros_transform_to_ros_pose(tf.transform); } catch (tf2::TransformException &ex) { NODELET_WARN_STREAM("Transform failed" << ex.what()); return false; @@ -835,6 +855,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { case ff_util::FreeFlyerActionState::SUCCESS: return fsm_.Update(MOTION_SUCCESS); default: + err_msg_ = "Move Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")"; return fsm_.Update(MOTION_FAILED); } } @@ -856,7 +877,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { return; } // Check that we are close enough to the approach pose - if (!CloseEnoughToApproach(goal->berth) && cfg_.Get("planner") != "qp") { + if (!CloseEnoughToApproach(berths_[goal->berth]) && !goal->return_dock) { result.fsm_result = "Too far from dock"; result.response = RESPONSE::TOO_FAR_AWAY_FROM_APPROACH; server_.SendResult(ff_util::FreeFlyerActionState::ABORTED, result); @@ -931,6 +952,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet { ff_util::FSM fsm_; ff_util::FreeFlyerActionClient client_m_; ff_util::FreeFlyerActionClient client_s_; + ff_util::FreeFlyerServiceClient client_l_; ff_util::FreeFlyerServiceClient client_u_; ff_util::FreeFlyerActionServer server_; ff_util::ConfigServer cfg_; diff --git a/behaviors/dock/tools/dock_tool.cc b/behaviors/dock/tools/dock_tool.cc index 6c06ac6d1f..8f20348bf8 100644 --- a/behaviors/dock/tools/dock_tool.cc +++ b/behaviors/dock/tools/dock_tool.cc @@ -24,7 +24,7 @@ #include // FSW includes -#include +#include #include // Action @@ -46,6 +46,7 @@ DEFINE_string(ns, "", "Robot namespace"); DEFINE_bool(dock, false, "Send a dock command"); DEFINE_bool(undock, false, "Send an undock command"); DEFINE_int32(berth, 1, "Berth ID (1 = left, 2 = right)"); +DEFINE_bool(return_dock, false, "Return to dock from afar"); // Timeout values DEFINE_double(connect, 10.0, "Action connect timeout"); @@ -113,6 +114,7 @@ void ConnectedCallback( ros::shutdown(); break; } + goal.return_dock = FLAGS_return_dock; } else if (FLAGS_undock) { goal.command = ff_msgs::DockGoal::UNDOCK; } diff --git a/behaviors/light_flow/CMakeLists.txt b/behaviors/light_flow/CMakeLists.txt index 7f4a6b69f5..d5f12052ac 100644 --- a/behaviors/light_flow/CMakeLists.txt +++ b/behaviors/light_flow/CMakeLists.txt @@ -15,8 +15,21 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(light_flow) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + ff_hw_msgs + ff_msgs + nodelet + ff_util +) + catkin_package( LIBRARIES light_flow @@ -29,54 +42,67 @@ catkin_package( pluginlib ) -create_library( - DIR src/light_flow - TARGET light_flow - DEPS - ff_msgs - ff_hw_msgs - LIBS - ${catkin_LIBRARIES} - ff_nodelet - ${JSONCPP_LIBRARIES} - INC - ${catkin_INCLUDE_DIRS} - ${JSONCPP_INCLUDE_DIRS} +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_tool_targets( - DIR - tools - LIBS - ${catkin_LIBRARIES} - ff_nodelet - ${JSONCPP_LIBRARIES} - light_flow - INC - ${catkin_INCLUDE_DIRS} - ${JSONCPP_INCLUDE_DIRS} - DEPS - ff_msgs - ff_hw_msgs - light_flow +# Declare C++ libraries +add_library(light_flow + src/light_flow/light_flow.cc ) +add_dependencies(light_flow ${catkin_EXPORTED_TARGETS}) +target_link_libraries(light_flow ${catkin_LIBRARIES}) -create_library( - DIR src/light_flow_nodelet - TARGET light_flow_nodelet - LIBS - ${catkin_LIBRARIES} - ${JSONCPP_LIBRARIES} - config_reader - ff_nodelet - light_flow - INC - ${catkin_INCLUDE_DIRS} - ${JSONCPP_INCLUDE_DIRS} - DEPS - ff_msgs - ff_hw_msgs - light_flow +# Declare C++ libraries +add_library(light_flow_nodelet + src/light_flow_nodelet/light_flow_nodelet.cc ) +add_dependencies(light_flow_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(light_flow_nodelet light_flow jsoncpp ${catkin_LIBRARIES}) + +## Declare a C++ executable: inspection_tool +add_executable(light_flow_tool tools/light_flow_tool.cc) +add_dependencies(light_flow_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(light_flow_tool + light_flow gflags jsoncpp ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS ${PROJECT_NAME}_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS light_flow_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/light_flow_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/behaviors/light_flow/include/light_flow.h b/behaviors/light_flow/include/light_flow.h index a6699da161..85af7acacf 100644 --- a/behaviors/light_flow/include/light_flow.h +++ b/behaviors/light_flow/include/light_flow.h @@ -20,7 +20,7 @@ #define LIGHT_FLOW_H_ #include -#include +#include #include #include #include diff --git a/behaviors/light_flow/src/light_flow/light_flow.cc b/behaviors/light_flow/src/light_flow/light_flow.cc index 02afd96a30..d0b1e0b1eb 100644 --- a/behaviors/light_flow/src/light_flow/light_flow.cc +++ b/behaviors/light_flow/src/light_flow/light_flow.cc @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc b/behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc index 471c5397d6..5a39722529 100644 --- a/behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc +++ b/behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc @@ -32,7 +32,7 @@ #include // FSW nodelet -#include +#include #include // Services diff --git a/behaviors/light_flow/tools/light_flow_tool.cc b/behaviors/light_flow/tools/light_flow_tool.cc index 6b99f4f8f2..9f7f1903b2 100644 --- a/behaviors/light_flow/tools/light_flow_tool.cc +++ b/behaviors/light_flow/tools/light_flow_tool.cc @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include diff --git a/behaviors/perch/CMakeLists.txt b/behaviors/perch/CMakeLists.txt index 5361a8810a..cb98d02e9a 100644 --- a/behaviors/perch/CMakeLists.txt +++ b/behaviors/perch/CMakeLists.txt @@ -15,8 +15,25 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(perch) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + ff_hw_msgs + ff_msgs + ff_util + localization_manager +) + catkin_package( LIBRARIES perch @@ -32,34 +49,55 @@ catkin_package( localization_manager ) -create_library( - TARGET - perch - LIBS - ${catkin_LIBRARIES} - ff_nodelet - config_reader - msg_conversions - config_server - config_client - INC - ${catkin_INCLUDE_DIRS} - DEPS - ff_msgs - ff_hw_msgs +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_tool_targets( - DIR - tools - LIBS - ${catkin_LIBRARIES} - ff_common - ff_nodelet - INC - ${catkin_INCLUDE_DIRS} - DEPS - ff_msgs +# Declare C++ libraries +add_library(perch + src/perch_nodelet.cc ) +add_dependencies(perch ${catkin_EXPORTED_TARGETS}) +target_link_libraries(perch ${catkin_LIBRARIES}) + +## Declare a C++ executable: inspection_tool +add_executable(perch_tool tools/perch_tool.cc) +add_dependencies(perch_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(perch_tool + perch gflags ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS perch_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/perch_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/behaviors/perch/perch_fsm.dot b/behaviors/perch/perch_fsm.dot index 76fe136315..7d19fd7939 100644 --- a/behaviors/perch/perch_fsm.dot +++ b/behaviors/perch/perch_fsm.dot @@ -68,7 +68,7 @@ digraph G { PERCHING_WAITING_FOR_SPIN_DOWN -> PERCHING_SWITCHING_TO_PL_LOC [label="[11]\nE:PMC_OFF\nSwitch(PL)" ]; PERCHING_SWITCHING_TO_PL_LOC -> PERCHED - [label="[12]\nE:MOTION_SUCCESS", color=darkgreen]; + [label="[12]\nE:SWITCH_SUCCESS", color=darkgreen]; # Nominal unperch PERCHED -> UNPERCHING_SWITCHING_TO_ML_LOC @@ -92,7 +92,7 @@ digraph G { [label="[20]\nE:MOTION_FAILED", color=red]; PERCHING_DEPLOYING_ARM -> RECOVERY_SWITCH_TO_ML_LOC [label="[21]\nE:ARM_FAILED", color=red]; - PERCHING_OPENING_GRIPPER -> RECOVERY_SWITCH_TO_ML_LOC + PERCHING_OPENING_GRIPPER -> RECOVERY_STOWING_ARM [label="[22]\nE:ARM_FAILED", color=red]; PERCHING_MOVING_TO_COMPLETE_POSE -> RECOVERY_OPENING_GRIPPER [label="[23]\nE:MOTION_FAILED", color=red]; @@ -120,10 +120,18 @@ digraph G { # Recovery RECOVERY_OPENING_GRIPPER -> RECOVERY_SWITCH_TO_ML_LOC [label="[33]\nE:ARM_SUCCESS", color=black]; + RECOVERY_OPENING_GRIPPER -> RECOVERY_SWITCH_TO_ML_LOC + [label="[33]\nE:ARM_FAILED", color=red]; RECOVERY_SWITCH_TO_ML_LOC -> RECOVERY_MOVING_TO_RECOVERY_POSE [label="[34]\nE:SWITCH_SUCCESS", color=black]; + RECOVERY_SWITCH_TO_ML_LOC -> UNPERCHED + [label="[34]\nE:SWITCH_FAILED", color=red]; RECOVERY_MOVING_TO_RECOVERY_POSE -> RECOVERY_STOWING_ARM [label="[35]\nE:MOTION_SUCCESS", color=black]; + RECOVERY_MOVING_TO_RECOVERY_POSE -> UNPERCHED + [label="[35]\nE:MOTION_FAILED", color=red]; RECOVERY_STOWING_ARM -> UNPERCHED [label="[36]\nE:ARM_SUCCESS", color=black]; + RECOVERY_STOWING_ARM -> UNPERCHED + [label="[36]\nE:ARM_FAILED", color=red]; } diff --git a/behaviors/perch/src/perch_nodelet.cc b/behaviors/perch/src/perch_nodelet.cc index 5794171634..1571ffbe04 100644 --- a/behaviors/perch/src/perch_nodelet.cc +++ b/behaviors/perch/src/perch_nodelet.cc @@ -156,6 +156,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { // then we can try moving to the approach pose in nominal mode. fsm_.Add(STATE::PERCHING_SWITCHING_TO_HR_LOC, SWITCH_SUCCESS, [this](FSM::Event const& event) -> FSM::State { + SaveApproachPose(); Move(APPROACH_POSE, ff_msgs::MotionGoal::NOMINAL); return STATE::PERCHING_MOVING_TO_APPROACH_POSE; }); @@ -176,7 +177,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { // [7] - If the gripper is opened, we move to complete pose. fsm_.Add(STATE::PERCHING_OPENING_GRIPPER, ARM_SUCCESS, [this](FSM::Event const& event) -> FSM::State { - Move(COMPLETE_POSE, ff_msgs::MotionGoal::NOMINAL); + Move(COMPLETE_POSE, ff_msgs::MotionGoal::PRECISION); return STATE::PERCHING_MOVING_TO_COMPLETE_POSE; }); // [8] - If the complete pose is reached, we close the gripper. @@ -200,7 +201,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { // [11] - If we successfully stopped, we switch to perch localization (future work) fsm_.Add(STATE::PERCHING_WAITING_FOR_SPIN_DOWN, MOTION_SUCCESS, [this](FSM::Event const& event) -> FSM::State { - Switch(LOCALIZATION_PERCH); + Switch(LOCALIZATION_MAPPED_LANDMARKS); return STATE::PERCHING_SWITCHING_TO_PL_LOC; }); // [12] - With all steps done, we conclude we are perched. @@ -264,36 +265,48 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { // [20] fsm_.Add(STATE::PERCHING_MOVING_TO_APPROACH_POSE, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { + err_ = RESPONSE::MOTION_FAILED; + err_msg_ = "Failed while moving to approach pose. " + err_msg_; Switch(LOCALIZATION_MAPPED_LANDMARKS); return STATE::RECOVERY_SWITCHING_TO_ML_LOC; }); // [21] fsm_.Add(STATE::PERCHING_DEPLOYING_ARM, ARM_FAILED, [this](FSM::Event const& event) -> FSM::State { - Arm(ff_msgs::ArmGoal::ARM_STOW); + err_ = RESPONSE::ARM_FAILED; + err_msg_ = "Failed while deploying the arm. " + err_msg_; + Switch(LOCALIZATION_MAPPED_LANDMARKS); return STATE::RECOVERY_SWITCHING_TO_ML_LOC; }); // [22] fsm_.Add(STATE::PERCHING_OPENING_GRIPPER, ARM_FAILED, [this](FSM::Event const& event) -> FSM::State { + err_ = RESPONSE::ARM_FAILED; + err_msg_ = "Failed while opening the gripper. " + err_msg_; Arm(ff_msgs::ArmGoal::ARM_STOW); - return STATE::RECOVERY_SWITCHING_TO_ML_LOC; + return STATE::RECOVERY_STOWING_ARM; }); // [23] fsm_.Add(STATE::PERCHING_MOVING_TO_COMPLETE_POSE, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { + err_ = RESPONSE::MOTION_FAILED; + err_msg_ = "Failed while moving to complete pose. " + err_msg_; Arm(ff_msgs::ArmGoal::GRIPPER_OPEN); return STATE::RECOVERY_OPENING_GRIPPER; }); // [24] fsm_.Add(STATE::PERCHING_CLOSING_GRIPPER, ARM_FAILED, [this](FSM::Event const& event) -> FSM::State { + err_ = RESPONSE::ARM_FAILED; + err_msg_ = "Failed while closing the gripper. " + err_msg_; Arm(ff_msgs::ArmGoal::GRIPPER_OPEN); return STATE::RECOVERY_OPENING_GRIPPER; }); // [25] fsm_.Add(STATE::PERCHING_CHECKING_ATTACHED, MOTION_SUCCESS, [this](FSM::Event const& event) -> FSM::State { + err_ = RESPONSE::MOTION_FAILED; + err_msg_ = "Failed while checking the attachment"; Arm(ff_msgs::ArmGoal::GRIPPER_OPEN); return STATE::RECOVERY_OPENING_GRIPPER; }); @@ -301,7 +314,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::PERCHING_WAITING_FOR_SPIN_DOWN, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { Result(RESPONSE::MOTION_FAILED, - "Spinning down propulsion failed"); + "Spinning down propulsion failed. " + err_msg_); return STATE::PERCHED; }); // [27] @@ -324,25 +337,29 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::UNPERCHING_WAITING_FOR_SPIN_UP, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { Result(RESPONSE::MOTION_FAILED, - "Spinning up propulsion failed"); + "Spinning up propulsion failed. " + err_msg_); return STATE::PERCHED; }); // [30] fsm_.Add(STATE::UNPERCHING_OPENING_GRIPPER, ARM_FAILED, [this](FSM::Event const& event) -> FSM::State { Result(RESPONSE::ARM_FAILED, - "Gripper open failed"); + "Gripper open failed. " + err_msg_); return STATE::PERCHED; }); // [31] fsm_.Add(STATE::UNPERCHING_MOVING_TO_APPROACH_POSE, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { + err_ = RESPONSE::MOTION_FAILED; + err_msg_ = "Failed while moving to approach pose. " + err_msg_; Move(RECOVERY_POSE, ff_msgs::MotionGoal::NOMINAL); return STATE::RECOVERY_MOVING_TO_RECOVERY_POSE; }); // [32] fsm_.Add(STATE::UNPERCHING_STOWING_ARM, ARM_FAILED, [this](FSM::Event const& event) -> FSM::State { + err_ = RESPONSE::ARM_FAILED; + err_msg_ = "Failed while stowing the arm. " + err_msg_; Arm(ff_msgs::ArmGoal::ARM_STOW); return STATE::RECOVERY_STOWING_ARM; }); @@ -352,15 +369,10 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { /////////////////////// // [33] - Opening gripper for recovery attempt: Success and Fail options fsm_.Add(STATE::RECOVERY_OPENING_GRIPPER, - ARM_SUCCESS, [this](FSM::Event const& event) -> FSM::State { + ARM_SUCCESS | ARM_FAILED, [this](FSM::Event const& event) -> FSM::State { Switch(LOCALIZATION_MAPPED_LANDMARKS); return STATE::RECOVERY_SWITCHING_TO_ML_LOC; }); - fsm_.Add(STATE::RECOVERY_OPENING_GRIPPER, - ARM_FAILED, [this](FSM::Event const& event) -> FSM::State { - return Result(RESPONSE::ARM_FAILED, - "Gripper open failed"); - }); // [34] - Switching to ML loc in recovery: Success and Fail options fsm_.Add(STATE::RECOVERY_SWITCHING_TO_ML_LOC, SWITCH_SUCCESS, [this](FSM::Event const& event) -> FSM::State { @@ -369,8 +381,8 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { }); fsm_.Add(STATE::RECOVERY_SWITCHING_TO_ML_LOC, SWITCH_FAILED, [this](FSM::Event const& event) -> FSM::State { - return Result(RESPONSE::SWITCH_FAILED, - "Switch to mapped landmarks localization failed"); + Result(err_, "Recovery switch to mapped landmarks localization failed: " + err_msg_); + return STATE::UNPERCHED; }); // [35] - Motion to recovery pose: Success and Fail options fsm_.Add(STATE::RECOVERY_MOVING_TO_RECOVERY_POSE, @@ -380,19 +392,19 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { }); fsm_.Add(STATE::RECOVERY_MOVING_TO_RECOVERY_POSE, MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State { - return Result(RESPONSE::MOTION_FAILED, - "Recovery motion to recovery pose failed"); + Result(err_, "Recovery motion to recovery pose failed: " + err_msg_); + return STATE::UNPERCHED; }); // [36] - Stowing arm in recovery: Success and Fail options fsm_.Add(STATE::RECOVERY_STOWING_ARM, ARM_SUCCESS, [this](FSM::Event const& event) -> FSM::State { - Result(RESPONSE::UNPERCHED, "Successful Recovery"); + Result(err_, "Successful Recovery: " + err_msg_); return STATE::UNPERCHED; }); fsm_.Add(STATE::RECOVERY_STOWING_ARM, ARM_FAILED, [this](FSM::Event const& event) -> FSM::State { - return Result(RESPONSE::ARM_FAILED, - "Stowing arm failed"); + Result(err_, "Recovery Stowing arm failed: " + err_msg_); + return STATE::UNPERCHED; }); ////////////////////////////////////////////// @@ -462,6 +474,10 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { return AssertFault(ff_util::INITIALIZATION_FAILED, "Could not load config"); + // Setup the platform name + platform_name_ = GetPlatform(); + platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); + // Create a transform buffer to listen for transforms tf_listener_ = std::shared_ptr( new tf2_ros::TransformListener(tf_buffer_)); @@ -734,6 +750,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { case ff_util::FreeFlyerActionState::SUCCESS: return fsm_.Update(ARM_SUCCESS); default: + err_msg_ = "Arm Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")"; return fsm_.Update(ARM_FAILED); } } @@ -747,22 +764,12 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { */ void SaveApproachPose(void) { geometry_msgs::TransformStamped tf = tf_buffer_.lookupTransform( - "world", "body", ros::Time(0)); + "world", platform_name_ + "body", ros::Time(0)); ROS_WARN("[Perch] Saving Approach Pose"); // Save the transform - // Position - Cartesian - approach_position_(0) = tf.transform.translation.x; - approach_position_(1) = tf.transform.translation.y; - approach_position_(2) = tf.transform.translation.z; - - // Orientation - Quaternions - Eigen::Quaterniond quat(tf.transform.rotation.w, - tf.transform.rotation.x, - tf.transform.rotation.y, - tf.transform.rotation.z); - approach_orientation_ = quat; + approach_pose_ = tf.transform; } // Enable or disable optical flow @@ -801,34 +808,34 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { // then to the complete pose. case COMPLETE_POSE: // The movement to approach pose is already done twice - msg.header.frame_id = "handrail/approach"; + msg.header.frame_id = platform_name_ + "handrail/approach"; goal.states.push_back(msg); - msg.header.frame_id = "handrail/complete"; + msg.header.frame_id = platform_name_ + "handrail/complete"; goal.states.push_back(msg); break; // Move to the approach pose. case APPROACH_POSE: - msg.header.frame_id = "handrail/approach"; + msg.header.frame_id = platform_name_ + "handrail/approach"; goal.states.push_back(msg); break; // Move to the recovery pose. This option is currently used to move to the // approach pose, but it is here for potential change in recovery options. case RECOVERY_POSE: - msg.header.frame_id = "body"; + msg.header.frame_id = platform_name_ + "body"; goal.states.push_back(msg); break; // Move to the perched pose. This option is currently used as a placeholder // operation for perched localization. It is implemented for futrure use, // if the user wants the robot to have a pan and tilt position for example. case PERCHED_POSE: - msg.header.frame_id = "body"; + msg.header.frame_id = platform_name_ + "body"; goal.states.push_back(msg); break; // Move to the unperched pose. Placeholder to return to a default pan/tilt // option, to make the aft face parallel to the wall supporting the handrail, // for example. case UNPERCHED_POSE: - msg.header.frame_id = "body"; + msg.header.frame_id = platform_name_ + "body"; goal.states.push_back(msg); break; default: @@ -858,14 +865,8 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { * TL;DR: This is good if we don't lose localization while perched, * otherwise relative movements are needed to securely move away. */ - pose.pose.orientation.w = approach_orientation_.w(); - pose.pose.orientation.x = approach_orientation_.x(); - pose.pose.orientation.y = approach_orientation_.y(); - pose.pose.orientation.z = approach_orientation_.z(); - - pose.pose.position.x = approach_position_(0); - pose.pose.position.y = approach_position_(1); - pose.pose.position.z = approach_position_(2); + pose.pose = msg_conversions::ros_transform_to_ros_pose(approach_pose_); + } else { /* * Any other movement implies a copy of a transform @@ -875,14 +876,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { "world", pose.header.frame_id, ros::Time(0)); // Copy the transform - pose.pose.orientation.w = tf.transform.rotation.w; - pose.pose.orientation.x = tf.transform.rotation.x; - pose.pose.orientation.y = tf.transform.rotation.y; - pose.pose.orientation.z = tf.transform.rotation.z; - - pose.pose.position.x = tf.transform.translation.x; - pose.pose.position.y = tf.transform.translation.y; - pose.pose.position.z = tf.transform.translation.z; + pose.pose = msg_conversions::ros_transform_to_ros_pose(tf.transform); } } catch (tf2::TransformException &ex) { NODELET_WARN_STREAM("Transform failed" << ex.what()); @@ -921,6 +915,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { case ff_util::FreeFlyerActionState::SUCCESS: return fsm_.Update(MOTION_SUCCESS); default: + err_msg_ = "Move Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")"; return fsm_.Update(MOTION_FAILED); } } @@ -1010,8 +1005,15 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet { ros::ServiceServer server_set_state_; ros::ServiceClient client_service_of_enable_; ros::ServiceClient client_service_hr_reset_; - Eigen::Quaterniond approach_orientation_; - Eigen::Vector3d approach_position_; + geometry_msgs::Transform approach_pose_; + int32_t err_; + std::string err_msg_; + std::string platform_name_; + + public: + // This fixes the Eigen aligment issue + // http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; PLUGINLIB_EXPORT_CLASS(perch::PerchNodelet, nodelet::Nodelet); diff --git a/behaviors/perch/tools/perch_tool.cc b/behaviors/perch/tools/perch_tool.cc index 69d0f249da..f6a665ee42 100644 --- a/behaviors/perch/tools/perch_tool.cc +++ b/behaviors/perch/tools/perch_tool.cc @@ -24,7 +24,7 @@ #include // FSW includes -#include +#include #include // Action diff --git a/behaviors/readme.md b/behaviors/readme.md index ced5fadd9c..5d65770f66 100644 --- a/behaviors/readme.md +++ b/behaviors/readme.md @@ -3,9 +3,15 @@ Behaviors are high-level actions that support complex maneuvers, such as control of the arm, docking, and perching. They are built upon the more more generic actions offered by other subsystems, such as the mobility subsystem's "motion" action, or the localization manager's "switch" action. The current behaviors available are: + \subpage arm + \subpage dock + \subpage signal + \subpage perch + \subpage states + \subpage signal \ No newline at end of file diff --git a/behaviors/states/CMakeLists.txt b/behaviors/states/CMakeLists.txt index 528bce0e0b..5ae0152483 100644 --- a/behaviors/states/CMakeLists.txt +++ b/behaviors/states/CMakeLists.txt @@ -15,30 +15,66 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(states) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + ff_hw_msgs + ff_msgs + ff_util +) + catkin_package( LIBRARIES states CATKIN_DEPENDS roscpp + nodelet ff_hw_msgs ff_msgs ff_util ) -create_library( - DIR src/states_nodelet - TARGET states_nodelet - LIBS - ${catkin_LIBRARIES} - config_reader - ff_nodelet - INC - ${catkin_INCLUDE_DIRS} - DEPS - ff_msgs - ff_hw_msgs +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(states + src/states_nodelet/states_nodelet.cc +) +add_dependencies(states ${catkin_EXPORTED_TARGETS}) +target_link_libraries(states ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -install_launch_files() \ No newline at end of file +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/behaviors/states/nodelet_plugins.xml b/behaviors/states/nodelet_plugins.xml index e323994948..b1a0737d23 100644 --- a/behaviors/states/nodelet_plugins.xml +++ b/behaviors/states/nodelet_plugins.xml @@ -1,4 +1,4 @@ - + Nodelet for the automatically switching between Astrobee signal states diff --git a/behaviors/states/src/states_nodelet/states_nodelet.cc b/behaviors/states/src/states_nodelet/states_nodelet.cc index d02d706ff9..d7c1e9594c 100644 --- a/behaviors/states/src/states_nodelet/states_nodelet.cc +++ b/behaviors/states/src/states_nodelet/states_nodelet.cc @@ -25,7 +25,7 @@ #include // FSW nodelet -#include +#include #include // Services diff --git a/cmake/CATKIN_LICENSE b/cmake/CATKIN_LICENSE deleted file mode 100644 index 3f4820a5c2..0000000000 --- a/cmake/CATKIN_LICENSE +++ /dev/null @@ -1,28 +0,0 @@ -CATKIN: - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions -are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above - copyright notice, this list of conditions and the following - disclaimer in the documentation and/or other materials provided - with the distribution. - * Neither the name of 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. diff --git a/cmake/CreateLibrary.cmake b/cmake/CreateLibrary.cmake deleted file mode 100644 index a7cf5983dd..0000000000 --- a/cmake/CreateLibrary.cmake +++ /dev/null @@ -1,83 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -function(create_library) - cmake_parse_arguments( - library # Prefix of output variables - "" # List of Boolean Variables - "TARGET;DIR" # List of mono valued arguments - "LIBS;INC;DEPS;ADD_SRCS;EXCLUDE;DEFINES;OPTIONS" # List of Multi Value Arguments - ${ARGN} - ) - - # If no directory was given, assume all files in the src dir go in our library - if(NOT library_DIR) - SET(library_DIR src) - endif(NOT library_DIR) - - # Search for all the executables in tool dir - file(GLOB SRC_FILES_C "${library_DIR}/*.c") - file(GLOB SRC_FILES_CC "${library_DIR}/*.cc") - set(SRC_FILES ${SRC_FILES_C} ${SRC_FILES_CC}) - - # Sift through SRC_FILES and remove EXCLUDE - foreach(SRC ${SRC_FILES}) - foreach (TEST_SRC ${library_EXCLUDE}) - string(FIND ${SRC} ${TEST_SRC} POSITION) - if (${POSITION} GREATER -1) - list(REMOVE_ITEM SRC_FILES ${SRC}) - endif() - endforeach() - endforeach() - - # Some of our dependencies might be header only, their headers paths are not - # being added automatically like the targets listed in LIB. Here we'll do it - # manually. - if (library_DEPS) - foreach(DEP ${library_DEPS}) - get_target_property(INC ${DEP} INCLUDE_DIRECTORIES) - if (INC) - list(APPEND library_INC ${INC}) - endif (INC) - endforeach() - list(REMOVE_DUPLICATES library_INC) - endif (library_DEPS) - - add_library(${library_TARGET} - ${SRC_FILES} ${library_ADD_SRCS}) - target_include_directories(${library_TARGET} - PUBLIC ${library_INC} ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${library_TARGET} - ${library_LIBS} ${CMAKE_LIBS_COVERAGE}) - if (library_DEFINES) - target_compile_definitions(${library_TARGET} - PUBLIC ${library_DEFINES}) - endif (library_DEFINES) - if (library_OPTIONS) - target_compile_options(${library_TARGET} - PUBLIC ${library_OPTIONS}) - endif (library_OPTIONS) - if (NOT "${library_DEPS}" STREQUAL "") - add_dependencies(${library_TARGET} - ${library_DEPS}) - endif() - - install(TARGETS ${library_TARGET} DESTINATION lib) - install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ - DESTINATION include - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp") -endfunction() diff --git a/cmake/CreateMsgTargets.cmake b/cmake/CreateMsgTargets.cmake deleted file mode 100644 index 4f910d88dc..0000000000 --- a/cmake/CreateMsgTargets.cmake +++ /dev/null @@ -1,83 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -function(create_msg_targets) - cmake_parse_arguments( - msg # Prefix of output variables - "" # List of boolean variables - "DIR;SDIR;ADIR" # List of mono valued arguments - "DEPS" # List of multi value arguments - ${ARGN}) - - if((NOT msg_DIR) AND (NOT msg_SDIR)) - message(WARNING "no msgs or srvs given to create_msg_targets") - return() - endif((NOT msg_DIR) AND (NOT msg_SDIR)) - - # Determine our module name - get_filename_component(MODULE_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME) - - # Search for all the messages to install - if(msg_DIR) - file(GLOB MSG_FILES ${msg_DIR}/*.msg) - - # The msgs to just be their name - foreach(SRC ${MSG_FILES}) - get_filename_component(NAME ${SRC} NAME) - list(APPEND MSG_FILE_NAMES ${NAME}) - endforeach() - - add_message_files(FILES ${MSG_FILE_NAMES}) - endif(msg_DIR) - - # Search for all the services to install - if(msg_SDIR) - file(GLOB SRV_FILES ${msg_SDIR}/*.srv) - - foreach(SRC ${SRV_FILES}) - get_filename_component(NAME ${SRC} NAME) - list(APPEND SRV_FILE_NAMES ${NAME}) - endforeach() - - add_service_files(FILES ${SRV_FILE_NAMES}) - endif(msg_SDIR) - - # Search for all the actions to install - if(msg_ADIR) - file(GLOB ACTION_FILES ${msg_ADIR}/*.action) - - foreach(SRC ${ACTION_FILES}) - get_filename_component(NAME ${SRC} NAME) - list(APPEND ACTION_FILE_NAMES ${NAME}) - endforeach() - - add_action_files(FILES ${ACTION_FILE_NAMES}) - endif(msg_ADIR) - - # Generate them - generate_messages(DEPENDENCIES ${msg_DEPS}) - - # Create a target that people can DEP to and find the headers - add_custom_target(${MODULE_NAME} - DEPENDS ${MODULE_NAME}_gencpp) - get_target_property(MSG_INC ${MODULE_NAME}_gencpp INCLUDE_DIRECTORIES) - set_target_properties(${MODULE_NAME} - PROPERTIES INCLUDE_DIRECTORIES ${MSG_INC}) - - # Make sure we install the package.xml so that it can be found later in the install directory - install(FILES package.xml DESTINATION share/${MODULE_NAME}) -endfunction(create_msg_targets) diff --git a/cmake/CreateToolTargets.cmake b/cmake/CreateToolTargets.cmake deleted file mode 100644 index 252032cea5..0000000000 --- a/cmake/CreateToolTargets.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -function(create_tool_targets) - cmake_parse_arguments( - tool # Prefix of output variables - "" # List of boolean variables - "DIR" # List of mono valued arguments - "LIBS;INC;DEPS;EXCLUDE" # List of multi-value arguments - ${ARGN} - ) - - # Search for all the executables in tool dir - file(GLOB TOOL_SRC_FILES_C "${tool_DIR}/*.c") - file(GLOB TOOL_SRC_FILES_CC "${tool_DIR}/*.cc") - set(TOOL_SRC_FILES ${TOOL_SRC_FILES_C} ${TOOL_SRC_FILES_CC}) - - # Sift through SRC_FILES and remove EXCLUDE - foreach(SRC ${TOOL_SRC_FILES}) - foreach(TEST_SRC ${tool_EXCLUDE}) - string(FIND ${SRC} ${TEST_SRC} POSITION) - if (${POSITION} GREATER -1) - list(REMOVE_ITEM TOOL_SRC_FILES ${SRC}) - endif() - endforeach() - endforeach() - - # Determine our module name - get_filename_component(MODULE_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME) - - # Some of our dependencies might be header only, their headers paths are not - # being added automatically like the targets listed in LIB. Here we'll do it - # manually. - if (tool_DEPS) - foreach(DEP ${tool_DEPS}) - get_target_property(INC ${DEP} INCLUDE_DIRECTORIES) - if (INC) - list(APPEND tool_INC ${INC}) - endif (INC) - endforeach() - list(REMOVE_DUPLICATES tool_INC) - endif (tool_DEPS) - - # Link the executables! - foreach(filename ${TOOL_SRC_FILES}) - string(REGEX REPLACE ".(cc|c)$" "" execname ${filename}) - string(REGEX REPLACE "^[^ ]*/" "" execname ${execname}) - add_executable(${execname} ${filename}) - target_link_libraries(${execname} - LINK_PUBLIC ${tool_LIBS}) - target_include_directories(${execname} PUBLIC ${tool_INC}) - if (tool_DEPS) - add_dependencies(${execname} ${tool_DEPS}) - endif (tool_DEPS) - install(TARGETS ${execname} DESTINATION bin) - - # Create symlinks so our executables can be found with rosrun. This is done - # inside an 'install' so that it happens after the installation. - install(CODE "execute_process( - COMMAND mkdir -p share/${PROJECT_NAME} - COMMAND ln -s ../../bin/${execname} share/${PROJECT_NAME}/${execname} - WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} - OUTPUT_QUIET - ERROR_QUIET - )") - endforeach() -endfunction() diff --git a/cmake/FindLuajit20.cmake b/cmake/FindLuajit20.cmake index 7ea4445f93..faafccfead 100644 --- a/cmake/FindLuajit20.cmake +++ b/cmake/FindLuajit20.cmake @@ -23,7 +23,7 @@ # LUAJIT_VERSION_STRING, the version of Lua found (since CMake 2.8.8) find_path(LUAJIT_INCLUDE_DIR luajit.h - PATH_SUFFIXES include/luajit-2.0 include + PATH_SUFFIXES include/luajit-2.0 include/luajit-2.1 include PATHS ~/Library/Frameworks /Library/Frameworks diff --git a/cmake/soracore/FindMiro.cmake b/cmake/FindMiro.cmake similarity index 100% rename from cmake/soracore/FindMiro.cmake rename to cmake/FindMiro.cmake diff --git a/cmake/FindOpenCV4WithXFeatures.cmake b/cmake/FindOpenCV4WithXFeatures.cmake new file mode 100644 index 0000000000..cc92435d9c --- /dev/null +++ b/cmake/FindOpenCV4WithXFeatures.cmake @@ -0,0 +1,108 @@ + +# Find OpenCV installation +find_package(OpenCV 4.0 REQUIRED) + + +list(APPEND OpenCV_LIB_COMPONENTS "opencv_xfeatures2d") + +if(NOT CMAKE_VERSION VERSION_LESS "2.8.11") + # Target property INTERFACE_INCLUDE_DIRECTORIES available since 2.8.11: + # * http://www.cmake.org/cmake/help/v2.8.11/cmake.html#prop_tgt:INTERFACE_INCLUDE_DIRECTORIES + foreach(__component ${OpenCV_LIB_COMPONENTS}) + if(TARGET ${__component}) + set_target_properties( + ${__component} + PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${OpenCV_INCLUDE_DIRS}" + ) + endif() + endforeach() +endif() + +# ============================================================== +# Form list of modules (components) to find +# ============================================================== +if(NOT OpenCV_FIND_COMPONENTS) + set(OpenCV_FIND_COMPONENTS ${OpenCV_LIB_COMPONENTS}) + list(REMOVE_ITEM OpenCV_FIND_COMPONENTS opencv_java) + if(GTest_FOUND OR GTEST_FOUND) + list(REMOVE_ITEM OpenCV_FIND_COMPONENTS opencv_ts) + endif() +endif() + +set(OpenCV_WORLD_COMPONENTS ) + +# expand short module names and see if requested components exist +foreach(__cvcomponent ${OpenCV_FIND_COMPONENTS}) + # Store the name of the original component so we can set the + # OpenCV__FOUND variable which can be checked by the user. + set (__original_cvcomponent ${__cvcomponent}) + if(NOT __cvcomponent MATCHES "^opencv_") + set(__cvcomponent opencv_${__cvcomponent}) + endif() + list(FIND OpenCV_LIB_COMPONENTS ${__cvcomponent} __cvcomponentIdx) + if(__cvcomponentIdx LESS 0) + if(_OpenCV_HANDLE_COMPONENTS_MANUALLY) + # Either the component is required or the user did not set any components at + # all. In the latter case, the OpenCV_FIND_REQUIRED_ variable + # will not be defined since it is not set by this config. So let's assume + # the implicitly set components are always required. + if(NOT DEFINED OpenCV_FIND_REQUIRED_${__original_cvcomponent} OR + OpenCV_FIND_REQUIRED_${__original_cvcomponent}) + message(FATAL_ERROR "${__cvcomponent} is required but was not found") + elseif(NOT OpenCV_FIND_QUIETLY) + # The component was marked as optional using OPTIONAL_COMPONENTS + message(WARNING "Optional component ${__cvcomponent} was not found") + endif() + endif(_OpenCV_HANDLE_COMPONENTS_MANUALLY) + #indicate that module is NOT found + string(TOUPPER "${__cvcomponent}" __cvcomponentUP) + set(${__cvcomponentUP}_FOUND "${__cvcomponentUP}_FOUND-NOTFOUND") + set(OpenCV_${__original_cvcomponent}_FOUND FALSE) + else() + # Not using list(APPEND) here, because OpenCV_LIBS may not exist yet. + # Also not clearing OpenCV_LIBS anywhere, so that multiple calls + # to find_package(OpenCV) with different component lists add up. + set(OpenCV_LIBS ${OpenCV_LIBS} "${__cvcomponent}") + #indicate that module is found + string(TOUPPER "${__cvcomponent}" __cvcomponentUP) + set(${__cvcomponentUP}_FOUND 1) + set(OpenCV_${__original_cvcomponent}_FOUND TRUE) + endif() + if(OpenCV_SHARED AND ";${OpenCV_WORLD_COMPONENTS};" MATCHES ";${__cvcomponent};" AND NOT TARGET ${__cvcomponent}) + get_target_property(__implib_dbg opencv_world IMPORTED_IMPLIB_DEBUG) + get_target_property(__implib_release opencv_world IMPORTED_IMPLIB_RELEASE) + get_target_property(__location_dbg opencv_world IMPORTED_LOCATION_DEBUG) + get_target_property(__location_release opencv_world IMPORTED_LOCATION_RELEASE) + get_target_property(__include_dir opencv_world INTERFACE_INCLUDE_DIRECTORIES) + add_library(${__cvcomponent} SHARED IMPORTED) + set_target_properties(${__cvcomponent} PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${__include_dir}") + if(__location_dbg) + set_property(TARGET ${__cvcomponent} APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG) + set_target_properties(${__cvcomponent} PROPERTIES + IMPORTED_IMPLIB_DEBUG "${__implib_dbg}" + IMPORTED_LINK_INTERFACE_LIBRARIES_DEBUG "" + IMPORTED_LOCATION_DEBUG "${__location_dbg}" + ) + endif() + if(__location_release) + set_property(TARGET ${__cvcomponent} APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE) + set_target_properties(${__cvcomponent} PROPERTIES + IMPORTED_IMPLIB_RELEASE "${__implib_release}" + IMPORTED_LINK_INTERFACE_LIBRARIES_RELEASE "" + IMPORTED_LOCATION_RELEASE "${__location_release}" + ) + endif() + endif() + if(TARGET ${__cvcomponent}) + ocv_map_imported_config(${__cvcomponent}) + endif() +endforeach() + +if(__remap_warnings AND NOT OpenCV_FIND_QUIETLY) + message("OpenCV: configurations remap warnings:\n${__remap_warnings}OpenCV: Check variable OPENCV_MAP_IMPORTED_CONFIG=${OPENCV_MAP_IMPORTED_CONFIG}") +endif() + +# find_package(OpenCV 4 REQUIRED PATHS ${OpenCV_DIR_LOCAL}) +set(OpenCV_LIBRARIES ${OpenCV_LIBS}) + diff --git a/cmake/soracore/FindRtiDds.cmake b/cmake/FindRtiDds.cmake similarity index 100% rename from cmake/soracore/FindRtiDds.cmake rename to cmake/FindRtiDds.cmake diff --git a/cmake/soracore/FindSoracore.cmake b/cmake/FindSoracore.cmake similarity index 100% rename from cmake/soracore/FindSoracore.cmake rename to cmake/FindSoracore.cmake diff --git a/cmake/soracore/GenerateMiroMakeParams.cmake b/cmake/GenerateMiroMakeParams.cmake similarity index 100% rename from cmake/soracore/GenerateMiroMakeParams.cmake rename to cmake/GenerateMiroMakeParams.cmake diff --git a/cmake/soracore/GenerateRtiDdsIdl.cmake b/cmake/GenerateRtiDdsIdl.cmake similarity index 100% rename from cmake/soracore/GenerateRtiDdsIdl.cmake rename to cmake/GenerateRtiDdsIdl.cmake diff --git a/cmake/soracore/GetLibraryList.cmake b/cmake/GetLibraryList.cmake similarity index 100% rename from cmake/soracore/GetLibraryList.cmake rename to cmake/GetLibraryList.cmake diff --git a/cmake/soracore/GetPackageLibSearchPath.cmake b/cmake/GetPackageLibSearchPath.cmake similarity index 100% rename from cmake/soracore/GetPackageLibSearchPath.cmake rename to cmake/GetPackageLibSearchPath.cmake diff --git a/cmake/InstallLaunchFiles.cmake b/cmake/InstallLaunchFiles.cmake deleted file mode 100644 index a01b35ed95..0000000000 --- a/cmake/InstallLaunchFiles.cmake +++ /dev/null @@ -1,22 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -function(install_launch_files) - file (GLOB LAUNCH_FILES launch/*.launch launch/*.yaml launch/*.xml) - get_filename_component(MODULE_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME) - install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) -endfunction() diff --git a/cmake/catkin/all.cmake b/cmake/catkin/all.cmake deleted file mode 100644 index ac31ea1d2e..0000000000 --- a/cmake/catkin/all.cmake +++ /dev/null @@ -1,206 +0,0 @@ -# prevent multiple inclusion -if(DEFINED _CATKIN_ALL_INCLUDED_) - message(FATAL_ERROR "catkin/cmake/all.cmake included multiple times") -endif() -set(_CATKIN_ALL_INCLUDED_ TRUE) - -if(NOT DEFINED catkin_EXTRAS_DIR) - message(FATAL_ERROR "catkin_EXTRAS_DIR is not set") -endif() - -# define devel space -if(CATKIN_DEVEL_PREFIX) - set(CATKIN_DEVEL_PREFIX ${CATKIN_DEVEL_PREFIX} CACHE PATH "catkin devel space") -else() - set(CATKIN_DEVEL_PREFIX "${CMAKE_BINARY_DIR}/devel") -endif() -#message(STATUS "Using CATKIN_DEVEL_PREFIX: ${CATKIN_DEVEL_PREFIX}") - -# update develspace marker file with a reference to this sourcespace -set(_catkin_marker_file "${CATKIN_DEVEL_PREFIX}/.catkin") - -# check if the develspace marker file exists yet -if(EXISTS ${_catkin_marker_file}) - file(READ ${_catkin_marker_file} _existing_sourcespaces) - if(_existing_sourcespaces STREQUAL "") - # write this sourcespace to the marker file - file(WRITE ${_catkin_marker_file} "${CMAKE_SOURCE_DIR}") - else() - # append to existing list of sourcespaces if it's not in the list - list(FIND _existing_sourcespaces "${CMAKE_SOURCE_DIR}" _existing_sourcespace_index) - if(_existing_sourcespace_index EQUAL -1) - file(APPEND ${_catkin_marker_file} ";${CMAKE_SOURCE_DIR}") - endif() - endif() -else() - # create a new develspace marker file - # NOTE: extra care must be taken when running multiple catkin jobs in parallel - # so that this does not overwrite the result of a similar call in another package - file(WRITE ${_catkin_marker_file} "${CMAKE_SOURCE_DIR}") -endif() - -# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument -# or CMAKE_PREFIX_PATH from the environment -if(NOT DEFINED CMAKE_PREFIX_PATH) - if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") - string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) - endif() -endif() -#message(STATUS "Using CMAKE_PREFIX_PATH: ${CMAKE_PREFIX_PATH}") -# store original CMAKE_PREFIX_PATH -set(CMAKE_PREFIX_PATH_AS_IS ${CMAKE_PREFIX_PATH}) - -# list of unique catkin workspaces based on CMAKE_PREFIX_PATH -set(CATKIN_WORKSPACES "") -foreach(path ${CMAKE_PREFIX_PATH}) - if(EXISTS "${path}/.catkin") - list(FIND CATKIN_WORKSPACES ${path} _index) - if(_index EQUAL -1) - list(APPEND CATKIN_WORKSPACES ${path}) - endif() - endif() -endforeach() -if(CATKIN_WORKSPACES) - #message(STATUS "This workspace overlays: ${CATKIN_WORKSPACES}") -endif() - -# prepend devel space to CMAKE_PREFIX_PATH -list(FIND CMAKE_PREFIX_PATH ${CATKIN_DEVEL_PREFIX} _index) -if(_index EQUAL -1) - list(INSERT CMAKE_PREFIX_PATH 0 ${CATKIN_DEVEL_PREFIX}) -endif() - - -# enable all new policies (if available) -macro(_set_cmake_policy_to_new_if_available policy) - if(POLICY ${policy}) - cmake_policy(SET ${policy} NEW) - endif() -endmacro() -_set_cmake_policy_to_new_if_available(CMP0000) -_set_cmake_policy_to_new_if_available(CMP0001) -_set_cmake_policy_to_new_if_available(CMP0002) -_set_cmake_policy_to_new_if_available(CMP0003) -_set_cmake_policy_to_new_if_available(CMP0004) -_set_cmake_policy_to_new_if_available(CMP0005) -_set_cmake_policy_to_new_if_available(CMP0006) -_set_cmake_policy_to_new_if_available(CMP0007) -_set_cmake_policy_to_new_if_available(CMP0008) -_set_cmake_policy_to_new_if_available(CMP0009) -_set_cmake_policy_to_new_if_available(CMP0010) -_set_cmake_policy_to_new_if_available(CMP0011) -_set_cmake_policy_to_new_if_available(CMP0012) -_set_cmake_policy_to_new_if_available(CMP0013) -_set_cmake_policy_to_new_if_available(CMP0014) -_set_cmake_policy_to_new_if_available(CMP0015) -_set_cmake_policy_to_new_if_available(CMP0016) -_set_cmake_policy_to_new_if_available(CMP0017) - -# the following operations must be performed inside a project context -if(NOT PROJECT_NAME) - project(catkin_internal) -endif() - -# include CMake functions -include(CMakeParseArguments) - -# functions/macros: list_append_unique, safe_execute_process -# python-integration: catkin_python_setup.cmake, interrogate_setup_dot_py.py, templates/__init__.py.in, templates/script.py.in, templates/python_distutils_install.bat.in, templates/python_distutils_install.sh.in, templates/safe_execute_install.cmake.in -foreach(filename - assert - atomic_configure_file - catkin_add_env_hooks - catkin_destinations - catkin_generate_environment - catkin_install_python - catkin_libraries - catkin_metapackage - catkin_package - catkin_package_xml - catkin_workspace - debug_message - em_expand - python # defines PYTHON_EXECUTABLE, required by empy - empy - find_program_required - legacy - list_append_deduplicate - list_append_unique - list_insert_in_workspace_order - safe_execute_process - stamp - string_starts_with - platform/lsb - platform/ubuntu - platform/windows - test/tests # defines CATKIN_ENABLE_TESTING, required by other test functions - test/catkin_download_test_data - test/gtest - test/nosetests - tools/doxygen - tools/libraries - tools/rt - -# tools/threads - ) - include(${catkin_EXTRAS_DIR}/${filename}.cmake) -endforeach() - -# output catkin version for debugging -_catkin_package_xml(${CMAKE_BINARY_DIR}/catkin/catkin_generated/version DIRECTORY ${catkin_EXTRAS_DIR}/..) -#message(STATUS "catkin ${catkin_VERSION}") -# ensure that no current package name is set -unset(_CATKIN_CURRENT_PACKAGE) - -# set global install destinations -set(CATKIN_GLOBAL_BIN_DESTINATION bin) -set(CATKIN_GLOBAL_ETC_DESTINATION etc) -set(CATKIN_GLOBAL_INCLUDE_DESTINATION include) -set(CATKIN_GLOBAL_LIB_DESTINATION lib) -set(CATKIN_GLOBAL_LIBEXEC_DESTINATION lib) -set(CATKIN_GLOBAL_PYTHON_DESTINATION ${PYTHON_INSTALL_DIR}) -set(CATKIN_GLOBAL_SHARE_DESTINATION share) - -# undefine CATKIN_ENV since it might be set in the cache from a previous build -set(CATKIN_ENV "" CACHE INTERNAL "catkin environment" FORCE) - -# generate environment files like env.* and setup.* -# uses em_expand without CATKIN_ENV being set yet -catkin_generate_environment() - -# file extension of env script -if(CMAKE_HOST_UNIX) # true for linux, apple, mingw-cross and cygwin - set(script_ext sh) -else() - set(script_ext bat) -endif() -# take snapshot of the modifications the setup script causes -# to reproduce the same changes with a static script in a fraction of the time -set(SETUP_DIR ${CMAKE_BINARY_DIR}/catkin_generated) -set(SETUP_FILENAME "setup_cached") -configure_file(${catkin_EXTRAS_DIR}/templates/generate_cached_setup.py.in - ${CMAKE_BINARY_DIR}/catkin_generated/generate_cached_setup.py) -set(GENERATE_ENVIRONMENT_CACHE_COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_BINARY_DIR}/catkin_generated/generate_cached_setup.py) -# the script is generated once here and refreshed by every call to catkin_add_env_hooks() -safe_execute_process(COMMAND ${GENERATE_ENVIRONMENT_CACHE_COMMAND}) -# generate env_cached which just relays to the setup_cached -configure_file(${catkin_EXTRAS_DIR}/templates/env.${script_ext}.in - ${SETUP_DIR}/env_cached.${script_ext} - @ONLY) -# environment to call external processes -set(CATKIN_ENV ${SETUP_DIR}/env_cached.${script_ext} CACHE INTERNAL "catkin environment") - -# add additional environment hooks -if(CATKIN_BUILD_BINARY_PACKAGE) - set(catkin_skip_install_env_hooks "SKIP_INSTALL") -endif() -if(CMAKE_HOST_UNIX) - catkin_add_env_hooks(05.catkin_make SHELLS bash DIRECTORY ${catkin_EXTRAS_DIR}/env-hooks ${catkin_skip_install_env_hooks}) - catkin_add_env_hooks(05.catkin_make_isolated SHELLS bash DIRECTORY ${catkin_EXTRAS_DIR}/env-hooks ${catkin_skip_install_env_hooks}) - catkin_add_env_hooks(05.catkin-test-results SHELLS sh DIRECTORY ${catkin_EXTRAS_DIR}/env-hooks ${catkin_skip_install_env_hooks}) -else() - catkin_add_env_hooks(05.catkin-test-results SHELLS bat DIRECTORY ${catkin_EXTRAS_DIR}/env-hooks ${catkin_skip_install_env_hooks}) -endif() - -# requires stamp and environment files -include(${catkin_EXTRAS_DIR}/catkin_python_setup.cmake) diff --git a/cmake/catkin/assert.cmake b/cmake/catkin/assert.cmake deleted file mode 100644 index 1a3f2c194b..0000000000 --- a/cmake/catkin/assert.cmake +++ /dev/null @@ -1,22 +0,0 @@ -function(assert VAR) - if(NOT ${VAR}) - message(FATAL_ERROR "\nAssertion failed: ${VAR} (value is '${${VAR}}')\n") - endif() - debug_message(3 "assert(${VAR}) passed (${VAR} = ${${VAR}})") -endfunction() - -function(assert_unset VAR) - if(${VAR}) - message(FATAL_ERROR "\nAssertion failed: '${VAR}' is set but should not be (value is '${${VAR}}')\n") - endif() - debug_message(3 "assert_unset(${VAR}) passed") -endfunction() - -function(assert_file_exists FILENAME MESSAGE) - if(NOT FILENAME) - message(FATAL_ERROR "\nAssertion failed: check for file existence, but filename (${FILENAME}) unset. Message: ${MESSAGE}\n") - endif() - if(NOT EXISTS ${FILENAME}) - message(FATAL_ERROR "\nAssertion failed: file '${FILENAME}' does not exist. Message: ${MESSAGE}\n") - endif() -endfunction() diff --git a/cmake/catkin/atomic_configure_file.cmake b/cmake/catkin/atomic_configure_file.cmake deleted file mode 100644 index b5a20a37bb..0000000000 --- a/cmake/catkin/atomic_configure_file.cmake +++ /dev/null @@ -1,9 +0,0 @@ -function(atomic_configure_file input output) - set(atomic_file "${CMAKE_BINARY_DIR}/atomic_configure_file") - configure_file("${input}" "${atomic_file}" ${ARGN}) - get_filename_component(output_path ${output} PATH) - if(NOT EXISTS ${output_path}) - file(MAKE_DIRECTORY ${output_path}) - endif() - file(RENAME "${atomic_file}" "${output}") -endfunction() diff --git a/cmake/catkin/catkinConfig.cmake.in b/cmake/catkin/catkinConfig.cmake.in deleted file mode 100644 index 2183ce8c6a..0000000000 --- a/cmake/catkin/catkinConfig.cmake.in +++ /dev/null @@ -1,117 +0,0 @@ -# generated from catkin/cmake/catkinConfig.cmake.in -# which overlays the default template catkin/cmake/template/pkgConfig.cmake.in -# -# :outvar catkin_INCLUDE_DIRS: contains the include dirs of all searched components. -# For use with CMake ``include_directories(${catkin_INCLUDE_DIRS})``. -# :outvar catkin_LIBRARY_DIRS: contains the library dirs of all searched components. -# For use with CMake ``link_directories(${catkin_INCLUDE_DIRS})``. -# :outvar catkin_LIBRARIES: contains the include dirs of all searched components. -# For use with CMake ``include_directories(${catkin_INCLUDE_DIRS})``. -# :outvar _INCLUDE_DIRS/_LIBRARY_DIRS/_LIBRARY: -# contains the include dirs / library dirs / libraries of the searched component . - -if(CATKIN_TOPLEVEL_FIND_PACKAGE OR NOT CATKIN_TOPLEVEL) - set(catkin_EXTRAS_DIR "@PKG_CMAKE_DIR@") - - # prevent multiple inclusion from repeated find_package() calls in non-workspace context - # as long as this variable is in the scope the variables from all.cmake are also, so no need to be evaluated again - if(NOT DEFINED _CATKIN_CONFIG_ALL_INCLUDED_) - set(_CATKIN_CONFIG_ALL_INCLUDED_ TRUE) - include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) - endif() -endif() - -# skip setting find_package() variables when discovered from toplevel.cmake -if(CATKIN_TOPLEVEL_FIND_PACKAGE) - return() -endif() - - -# prevent usage with wrong case -if(CATKIN_FIND_COMPONENTS OR CATKIN_FIND_REQUIRED OR CATKIN_FIND_QUIETLY) - message(FATAL_ERROR "find_package() only supports lower-case package name 'catkin'") -endif() - -# mark as found -if(NOT catkin_FOUND) - set(catkin_FOUND) - set(CATKIN_PACKAGE_PREFIX "" CACHE STRING "Prefix to apply to package generated via gendebian") -endif() - -# flag project as catkin-based to distinguish if a find_package()-ed project is a catkin project -set(catkin_FOUND_CATKIN_PROJECT TRUE) - -# XXXX don't overwrite catkin_* variables when being called recursively -if(NOT _CATKIN_FIND_ OR _CATKIN_FIND_ EQUAL 0) - set(_CATKIN_FIND_ 0) - if(catkin_FIND_COMPONENTS) - set(catkin_INCLUDE_DIRS "") - set(catkin_LIBRARIES "") - set(catkin_LIBRARY_DIRS "") - set(catkin_EXPORTED_TARGETS "") - endif() -endif() - -# increment recursion counter -math(EXPR _CATKIN_FIND_ "${_CATKIN_FIND_} + 1") - -# find all components -if(catkin_FIND_COMPONENTS) - foreach(component ${catkin_FIND_COMPONENTS}) - string(TOLOWER "${component}" component_lower) - # skip catkin since it does not make sense as a component - if(NOT ${component_lower} STREQUAL "catkin") - - # get search paths from CMAKE_PREFIX_PATH (which includes devel space) - set(paths "") - foreach(path ${CMAKE_PREFIX_PATH}) - if(IS_DIRECTORY ${path}/share/${component}/cmake) - list(APPEND paths ${path}/share/${component}/cmake) - endif() - endforeach() - - # find package component - if(catkin_FIND_REQUIRED) - find_package(${component} REQUIRED NO_MODULE PATHS ${paths} - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) - elseif(catkin_FIND_QUIETLY) - find_package(${component} QUIET NO_MODULE PATHS ${paths} - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) - else() - find_package(${component} NO_MODULE PATHS ${paths} - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) - endif() - - # append component-specific variables to catkin_* variables - list_append_unique(catkin_INCLUDE_DIRS ${${component}_INCLUDE_DIRS}) - - # merge build configuration keywords with library names to correctly deduplicate - catkin_pack_libraries_with_build_configuration(catkin_LIBRARIES ${catkin_LIBRARIES}) - catkin_pack_libraries_with_build_configuration(_libraries ${${component}_LIBRARIES}) - list_append_deduplicate(catkin_LIBRARIES ${_libraries}) - # undo build configuration keyword merging after deduplication - catkin_unpack_libraries_with_build_configuration(catkin_LIBRARIES ${catkin_LIBRARIES}) - - list_append_unique(catkin_LIBRARY_DIRS ${${component}_LIBRARY_DIRS}) - list(APPEND catkin_EXPORTED_TARGETS ${${component}_EXPORTED_TARGETS}) - endif() - endforeach() - list_insert_in_workspace_order(catkin_INCLUDE_DIRS ${catkin_INCLUDE_DIRS}) - list_insert_in_workspace_order(catkin_LIBRARY_DIRS ${catkin_LIBRARY_DIRS}) -endif() - -# add dummy target to catkin_EXPORTED_TARGETS if empty -if(NOT catkin_EXPORTED_TARGETS) - if(NOT TARGET _catkin_empty_exported_target) - add_custom_target(_catkin_empty_exported_target) - endif() - list(APPEND catkin_EXPORTED_TARGETS _catkin_empty_exported_target) -endif() - -# decrement recursion counter -math(EXPR _CATKIN_FIND_ "${_CATKIN_FIND_} - 1") - -if(_CATKIN_FIND_ EQUAL 0) - # store found components (from the fist level only) for validation in catkin_package() that they are build dependencies - list(APPEND catkin_ALL_FOUND_COMPONENTS ${catkin_FIND_COMPONENTS}) -endif() diff --git a/cmake/catkin/catkin_add_env_hooks.cmake b/cmake/catkin/catkin_add_env_hooks.cmake deleted file mode 100644 index dc063bd803..0000000000 --- a/cmake/catkin/catkin_add_env_hooks.cmake +++ /dev/null @@ -1,139 +0,0 @@ -# -# Register environment hooks which are executed by the setup script. -# -# For each shell in ``SHELLS``, the macro searches for one of the -# following files in the directory ``DIRECTORY``: -# ``.``, -# ``...em``, -# ``..em``, -# ``...in`` or -# ``..in``. -# -# Plain shells, will be copied to, templates are expanded to -# ``etc/catkin/profile.d/``, where it will be read by global generated -# ``setup.``. -# -# The templates can also distinguish between devel- and installspace -# using the boolean variables ``DEVELSPACE`` and ``INSTALLSPACE`` -# which are either ``true`` or ``false``. -# E.g. @[if DEVELSPACE]@ ... @[end if]@ for .em -# -# .. note:: Note that the extra extensions must appear in the filename -# but must not appear in the argument. -# -# .. note:: These files will share a single directory with other -# packages that choose to install env hooks. Be careful to give -# the file a unique name. Typically ``NN.name.`` is used, -# where NN can define when something should be run (the files are -# read in alphanumeric order) and the name serves to disambiguate -# in the event of collisions. -# -# Example:: -# -# catkin_add_env_hooks(my_prefix SHELLS bash tcsh zsh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) -# -# looks for files env-hooks/my_prefix.[bash|tcsh|zsh]((.(devel|install)space)?.[em|in])? -# -# :param file_prefix: the filename prefix -# :type file_prefix: string -# :param SHELLS: the shell extensions (e.g.: sh bat bash zsh tcsh) -# :type SHELLS: list of strings -# :param DIRECTORY: the directory (default: ${CMAKE_CURRENT_SOURCE_DIR}) -# :type DIRECTORY: string -# :param SKIP_INSTALL: if specified the env hooks are only generated -# in the devel space but not installed -# :type SKIP_INSTALL: option -# -# @public -# -function(catkin_add_env_hooks file_prefix) - cmake_parse_arguments(ARG "SKIP_INSTALL" "DIRECTORY" "SHELLS" ${ARGN}) - - # create directory if necessary - if(NOT IS_DIRECTORY ${CATKIN_DEVEL_PREFIX}/etc/catkin/profile.d) - file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/etc/catkin/profile.d) - endif() - - if(NOT ARG_DIRECTORY) - set(ARG_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) - endif() - - foreach(shell ${ARG_SHELLS}) - set(ENV_HOOK ${file_prefix}.${shell}) - set(base ${ARG_DIRECTORY}/${ENV_HOOK}) - - # generate environment hook for devel space - set(DEVELSPACE True) - set(INSTALLSPACE False) - if(EXISTS ${base}.em OR EXISTS ${base}.develspace.em) - # evaluate em template - if(EXISTS ${base}.develspace.em) - set(em_template ${base}.develspace.em) - else() - set(em_template ${base}.em) - endif() - em_expand(${catkin_EXTRAS_DIR}/templates/env-hook.context.py.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/${ENV_HOOK}.develspace.context.py - ${em_template} - ${CATKIN_DEVEL_PREFIX}/etc/catkin/profile.d/${ENV_HOOK}) - elseif(EXISTS ${base}.in OR EXISTS ${base}.develspace.in) - # evaluate in template - if(EXISTS ${base}.develspace.in) - set(in_template ${base}.develspace.in) - else() - set(in_template ${base}.in) - endif() - atomic_configure_file(${in_template} - ${CATKIN_DEVEL_PREFIX}/etc/catkin/profile.d/${ENV_HOOK} - @ONLY) - elseif (EXISTS ${base}) - # copy plain file - file(COPY ${base} DESTINATION ${CATKIN_DEVEL_PREFIX}/etc/catkin/profile.d) - else() - message(FATAL_ERROR "catkin_add_env_hooks() could not find environment hook. Either '${base}', '${base}.em', '${base}.develspace.em' or '${base}.in' must exist.") - endif() - - # generate and install environment hook for installspace - set(DEVELSPACE False) - set(INSTALLSPACE True) - if(EXISTS ${base}.em OR EXISTS ${base}.installspace.em) - # evaluate em template and install - if(EXISTS ${base}.installspace.em) - set(em_template ${base}.installspace.em) - else() - set(em_template ${base}.em) - endif() - em_expand(${catkin_EXTRAS_DIR}/templates/env-hook.context.py.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/${ENV_HOOK}.installspace.context.py - ${em_template} - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${ENV_HOOK}) - if(NOT ${ARG_SKIP_INSTALL}) - install(FILES ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${ENV_HOOK} - DESTINATION etc/catkin/profile.d) - endif() - elseif(EXISTS ${base}.in OR EXISTS ${base}.installspace.in) - # evaluate in template and install - if(EXISTS ${base}.installspace.in) - set(in_template ${base}.installspace.in) - else() - set(in_template ${base}.in) - endif() - configure_file(${in_template} - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${ENV_HOOK} - @ONLY) - if(NOT ${ARG_SKIP_INSTALL}) - install(FILES ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${ENV_HOOK} - DESTINATION etc/catkin/profile.d) - endif() - elseif (EXISTS ${base}) - # install plain file - if(NOT ${ARG_SKIP_INSTALL}) - install(FILES ${base} - DESTINATION etc/catkin/profile.d) - endif() - endif() - endforeach() - - # refresh environment cache - safe_execute_process(COMMAND ${GENERATE_ENVIRONMENT_CACHE_COMMAND}) -endfunction() diff --git a/cmake/catkin/catkin_destinations.cmake b/cmake/catkin/catkin_destinations.cmake deleted file mode 100644 index c4327e1223..0000000000 --- a/cmake/catkin/catkin_destinations.cmake +++ /dev/null @@ -1,59 +0,0 @@ -# -# Set several path suffixes for install destinations. -# -# :outvar CATKIN_PACKAGE_BIN_DESTINATION: -# See :cmake:data:`CATKIN_PACKAGE_BIN_DESTINATION`. -# :outvar CATKIN_PACKAGE_ETC_DESTINATION: -# See :cmake:data:`CATKIN_PACKAGE_ETC_DESTINATION`. -# :outvar CATKIN_PACKAGE_INCLUDE_DESTINATION: -# See :cmake:data:`CATKIN_PACKAGE_INCLUDE_DESTINATION`. -# :outvar CATKIN_PACKAGE_LIB_DESTINATION: -# See :cmake:data:`CATKIN_PACKAGE_LIB_DESTINATION`. -# :outvar CATKIN_PACKAGE_PYTHON_DESTINATION: -# See :cmake:data:`CATKIN_PACKAGE_PYTHON_DESTINATION`. -# :outvar CATKIN_PACKAGE_SHARE_DESTINATION: -# See :cmake:data:`CATKIN_PACKAGE_SHARE_DESTINATION`. -# -# :outvar CATKIN_GLOBAL_BIN_DESTINATION: -# See :cmake:data:`CATKIN_GLOBAL_BIN_DESTINATION`. -# :outvar CATKIN_GLOBAL_ETC_DESTINATION: -# See :cmake:data:`CATKIN_GLOBAL_ETC_DESTINATION`. -# :outvar CATKIN_GLOBAL_INCLUDE_DESTINATION: -# See :cmake:data:`CATKIN_GLOBAL_INCLUDE_DESTINATION`. -# :outvar CATKIN_GLOBAL_LIB_DESTINATION: -# See :cmake:data:`CATKIN_GLOBAL_LIB_DESTINATION`. -# :outvar CATKIN_GLOBAL_LIBEXEC_DESTINATION: -# See :cmake:data:`CATKIN_GLOBAL_LIBEXEC_DESTINATION`. -# :outvar CATKIN_GLOBAL_PYTHON_DESTINATION: -# See :cmake:data:`CATKIN_GLOBAL_PYTHON_DESTINATION`. -# :outvar CATKIN_GLOBAL_SHARE_DESTINATION: -# See :cmake:data:`CATKIN_GLOBAL_SHARE_DESTINATION`. -# -macro(catkin_destinations) - # verify that project() has been called before - if(NOT PROJECT_NAME) - message(FATAL_ERROR "catkin_destinations() PROJECT_NAME is not set. You must call project() before you can call catkin_destinations().") - endif() - - # execute catkin_destinations() only once, skip repeated invocations - if(NOT DEFINED _${PROJECT_NAME}_CATKIN_DESTINATIONS) - debug_message(10 "catkin_destinations()") - - # mark that catkin_destinations() was called - set(_${PROJECT_NAME}_CATKIN_DESTINATIONS TRUE) - - # set project specific install destinations - set(CATKIN_PACKAGE_BIN_DESTINATION ${CATKIN_GLOBAL_LIBEXEC_DESTINATION}/${PROJECT_NAME}) - set(CATKIN_PACKAGE_ETC_DESTINATION ${CATKIN_GLOBAL_ETC_DESTINATION}/${PROJECT_NAME}) - set(CATKIN_PACKAGE_INCLUDE_DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/${PROJECT_NAME}) - set(CATKIN_PACKAGE_LIB_DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) - set(CATKIN_PACKAGE_PYTHON_DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION}/${PROJECT_NAME}) - set(CATKIN_PACKAGE_SHARE_DESTINATION ${CATKIN_GLOBAL_SHARE_DESTINATION}/${PROJECT_NAME}) - - # set project specific output directory for libraries - set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) - set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) - # set project specific output directory for binaries - set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}) - endif() -endmacro() diff --git a/cmake/catkin/catkin_generate_environment.cmake b/cmake/catkin/catkin_generate_environment.cmake deleted file mode 100644 index 912078b6e6..0000000000 --- a/cmake/catkin/catkin_generate_environment.cmake +++ /dev/null @@ -1,146 +0,0 @@ -function(catkin_generate_environment) - set(SETUP_FILENAME "setup") - - # devel space - set(SETUP_DIR ${CATKIN_DEVEL_PREFIX}) - - # generate empty file to prevent searching for packages in binary dir - # except if source space and build space are identical (which is the case for dry eclipse projects) - if(NOT "${CMAKE_BINARY_DIR}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}") - file(WRITE "${CMAKE_BINARY_DIR}/CATKIN_IGNORE" "") - endif() - - # get multiarch name - set(CATKIN_LIB_ENVIRONMENT_PATHS "'${CATKIN_GLOBAL_LIB_DESTINATION}'") - set(CATKIN_PKGCONFIG_ENVIRONMENT_PATHS "os.path.join('${CATKIN_GLOBAL_LIB_DESTINATION}', 'pkgconfig')") - if (UNIX AND NOT APPLE) - # Two step looking for multiarch support: check for gcc -print-multiarch - # and, if failed, try to run dpkg-architecture - execute_process(COMMAND gcc -print-multiarch - OUTPUT_VARIABLE CATKIN_MULTIARCH - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_QUIET - ) - if ("${CATKIN_MULTIARCH}" STREQUAL "") - execute_process(COMMAND dpkg-architecture -qDEB_HOST_MULTIARCH - OUTPUT_VARIABLE CATKIN_MULTIARCH - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_QUIET - ) - endif() - if (NOT "${CATKIN_MULTIARCH}" STREQUAL "") - set(CATKIN_LIB_ENVIRONMENT_PATHS - "[${CATKIN_LIB_ENVIRONMENT_PATHS}, os.path.join('${CATKIN_GLOBAL_LIB_DESTINATION}', '${CATKIN_MULTIARCH}')]") - set(CATKIN_PKGCONFIG_ENVIRONMENT_PATHS - "[${CATKIN_PKGCONFIG_ENVIRONMENT_PATHS}, os.path.join('${CATKIN_GLOBAL_LIB_DESTINATION}', '${CATKIN_MULTIARCH}', 'pkgconfig')]") - endif() - endif() - - # generate Python setup util - atomic_configure_file(${catkin_EXTRAS_DIR}/templates/_setup_util.py.in - ${CATKIN_DEVEL_PREFIX}/_setup_util.py - @ONLY) - - if(NOT WIN32) - # non-windows - # generate env - atomic_configure_file(${catkin_EXTRAS_DIR}/templates/env.sh.in - ${CATKIN_DEVEL_PREFIX}/env.sh - @ONLY) - # generate setup for various shells - foreach(shell bash sh zsh) - atomic_configure_file(${catkin_EXTRAS_DIR}/templates/setup.${shell}.in - ${CATKIN_DEVEL_PREFIX}/setup.${shell} - @ONLY) - endforeach() - - else() - # windows - # generate env - atomic_configure_file(${catkin_EXTRAS_DIR}/templates/env.bat.in - ${CATKIN_DEVEL_PREFIX}/env.bat - @ONLY) - # generate setup - atomic_configure_file(${catkin_EXTRAS_DIR}/templates/setup.bat.in - ${CATKIN_DEVEL_PREFIX}/setup.bat - @ONLY) - endif() - - # generate rosinstall file referencing setup.sh - atomic_configure_file(${catkin_EXTRAS_DIR}/templates/rosinstall.in - ${CATKIN_DEVEL_PREFIX}/.rosinstall - @ONLY) - - # installspace - set(SETUP_DIR ${CMAKE_INSTALL_PREFIX}) - - if(NOT CATKIN_BUILD_BINARY_PACKAGE) - # install empty workspace marker if it doesn't already exist - install(CODE " - if (NOT EXISTS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}\") - file(MAKE_DIRECTORY \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}\") - endif() - if (NOT EXISTS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/.catkin\") - file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/.catkin\" \"\") - endif()") - - # generate and install Python setup util - configure_file(${catkin_EXTRAS_DIR}/templates/_setup_util.py.in - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/_setup_util.py - @ONLY) - catkin_install_python(PROGRAMS - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/_setup_util.py - DESTINATION ${CMAKE_INSTALL_PREFIX}) - endif() - - if(NOT WIN32) - # non-windows - # generate and install env - configure_file(${catkin_EXTRAS_DIR}/templates/env.sh.in - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/env.sh - @ONLY) - if(NOT CATKIN_BUILD_BINARY_PACKAGE) - install(PROGRAMS - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/env.sh - DESTINATION ${CMAKE_INSTALL_PREFIX}) - endif() - # generate and install setup for various shells - foreach(shell bash sh zsh) - configure_file(${catkin_EXTRAS_DIR}/templates/setup.${shell}.in - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/setup.${shell} - @ONLY) - if(NOT CATKIN_BUILD_BINARY_PACKAGE) - install(FILES - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/setup.${shell} - DESTINATION ${CMAKE_INSTALL_PREFIX}) - endif() - endforeach() - - else() - # windows - # generate and install env - configure_file(${catkin_EXTRAS_DIR}/templates/env.bat.in - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/env.bat - @ONLY) - install(PROGRAMS - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/env.bat - DESTINATION ${CMAKE_INSTALL_PREFIX}) - # generate and install setup - configure_file(${catkin_EXTRAS_DIR}/templates/setup.bat.in - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/setup.bat - @ONLY) - install(FILES - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/setup.bat - DESTINATION ${CMAKE_INSTALL_PREFIX}) - endif() - - # generate rosinstall file referencing setup.sh - configure_file(${catkin_EXTRAS_DIR}/templates/rosinstall.in - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/.rosinstall - @ONLY) - if(NOT CATKIN_BUILD_BINARY_PACKAGE) - install(FILES - ${CMAKE_BINARY_DIR}/catkin_generated/installspace/.rosinstall - DESTINATION ${CMAKE_INSTALL_PREFIX}) - endif() -endfunction() diff --git a/cmake/catkin/catkin_install_python.cmake b/cmake/catkin/catkin_install_python.cmake deleted file mode 100644 index 45f7a3c745..0000000000 --- a/cmake/catkin/catkin_install_python.cmake +++ /dev/null @@ -1,55 +0,0 @@ -# -# Install Python files and update their shebang lines -# to use a different Python executable. -# -# The signature: -# -# catkin_install_python(PROGRAMS files... DESTINATION -# [OPTIONAL] -# ) -# -# See the documentation for CMake install() function for more information. -# -# @public -# -function(catkin_install_python signature) - string(TOUPPER "${signature}" signature) - if(NOT "${signature}" STREQUAL "PROGRAMS") - message(FATAL_ERROR "catkin_install_python() only supports the PROGRAMS signature (not '${signature}').") - endif() - cmake_parse_arguments(ARG "OPTIONAL" "DESTINATION" "" ${ARGN}) - if(NOT ARG_DESTINATION) - message(FATAL_ERROR "catkin_install_python() called without required DESTINATION argument.") - endif() - foreach(file ${ARG_UNPARSED_ARGUMENTS}) - if(NOT IS_ABSOLUTE ${file}) - set(file "${CMAKE_CURRENT_SOURCE_DIR}/${file}") - endif() - if(EXISTS ${file}) - # read file and check shebang line - file(READ ${file} data) - set(regex "^#!/([^\r\n]+)/env python([\r\n])") - string(REGEX MATCH "${regex}" shebang_line "${data}") - string(LENGTH "${shebang_line}" length) - string(SUBSTRING "${data}" 0 ${length} prefix) - if("${shebang_line}" STREQUAL "${prefix}") - # write modified file with modified shebang line - get_filename_component(python_name ${PYTHON_EXECUTABLE} NAME) - string(REGEX REPLACE "${regex}" "#!/\\1/env ${python_name}\\2" data "${data}") - get_filename_component(filename ${file} NAME) - set(file "${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace") - file(MAKE_DIRECTORY ${file}) - set(file "${file}/${filename}") - file(WRITE ${file} "${data}") - endif() - # install (modified) file to destination - set(optional_flag "") - if(ARG_OPTIONAL) - set(optional_flag "OPTIONAL") - endif() - install(PROGRAMS "${file}" DESTINATION "${ARG_DESTINATION}" ${optional_flag}) - elseif(NOT ARG_OPTIONAL) - message(FATAL_ERROR "catkin_install_python() called with non-existing file '${file}'.") - endif() - endforeach() -endfunction() diff --git a/cmake/catkin/catkin_libraries.cmake b/cmake/catkin/catkin_libraries.cmake deleted file mode 100644 index 723d40abd1..0000000000 --- a/cmake/catkin/catkin_libraries.cmake +++ /dev/null @@ -1,159 +0,0 @@ -set(CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR ":") - -# -# Filter libraries based on optional build configuration keywords. -# -# :param VAR: the output variable name -# :type VAR: string -# :param ARGN: a list of libraries -# :type ARGN: list of strings -# :param BUILD_TYPE: a keyword for the build type (default: -# ``CMAKE_BUILD_TYPE``) -# :type BUILD_TYPE: list of strings -# -# @public -# -macro(catkin_filter_libraries_for_build_configuration VAR) - cmake_parse_arguments(ARG "" "BUILD_TYPE" "" ${ARGN}) - if(NOT ARG_BUILD_TYPE) - set(ARG_BUILD_TYPE ${CMAKE_BUILD_TYPE}) - endif() - if(NOT ARG_BUILD_TYPE) - set(ARG_BUILD_TYPE "Debug") - endif() - set(${VAR} "") - list(LENGTH ARG_UNPARSED_ARGUMENTS _count) - set(_index 0) - while(${_index} LESS ${_count}) - list(GET ARG_UNPARSED_ARGUMENTS ${_index} lib) - - if("${lib}" STREQUAL "debug") - if(NOT "${ARG_BUILD_TYPE}" STREQUAL "Debug") - # skip keyword and debug library for non-debug builds - math(EXPR _index "${_index} + 1") - if(${_index} EQUAL ${_count}) - message(FATAL_ERROR "catkin_filter_libraries_for_build_configuration() the list of libraries '${ARG_UNPARSED_ARGUMENTS}' ends with '${lib}' which is a build configuration keyword and must be followed by a library") - endif() - endif() - elseif("${lib}" STREQUAL "optimized") - if("${ARG_BUILD_TYPE}" STREQUAL "Debug") - # skip keyword and non-debug library for debug builds - math(EXPR _index "${_index} + 1") - if(${_index} EQUAL ${_count}) - message(FATAL_ERROR "catkin_filter_libraries_for_build_configuration() the list of libraries '${ARG_UNPARSED_ARGUMENTS}' ends with '${lib}' which is a build configuration keyword and must be followed by a library") - endif() - endif() - elseif("${lib}" STREQUAL "general") - # just consume the keyword - if(${_index} EQUAL ${_count}) - message(FATAL_ERROR "catkin_package() the list of libraries '${ARG_UNPARSED_ARGUMENTS}' ends with '${lib}' which is a build configuration keyword and must be followed by a library") - endif() - else() - list(APPEND ${VAR} "${lib}") - endif() - math(EXPR _index "${_index} + 1") - endwhile() - debug_message(10 "catkin_filter_libraries_for_build_configuration(${VAR} ${ARG_UNPARSED_ARGUMENTS} BUILD_TYPE ${ARG_BUILD_TYPE}) ${${VAR}}") -endmacro() - -# -# Pack a list of libraries with optional build configuration keywords. -# Each keyword is joined with its library using a separator. -# A packed library list can be deduplicated correctly. -# -# :param VAR: the output variable name -# :type VAR: string -# :param ARGN: a list of libraries -# :type ARGN: list of strings -# -# @public -# -macro(catkin_pack_libraries_with_build_configuration VAR) - set(${VAR} "") - set(_argn ${ARGN}) - list(LENGTH _argn _count) - set(_index 0) - while(${_index} LESS ${_count}) - list(GET _argn ${_index} lib) - if("${lib}" MATCHES "^debug|optimized|general$") - math(EXPR _index "${_index} + 1") - if(${_index} EQUAL ${_count}) - message(FATAL_ERROR "catkin_pack_libraries_with_build_configuration() the list of libraries '${_argn}' ends with '${lib}' which is a build configuration keyword and must be followed by a library") - endif() - list(GET _argn ${_index} library) - list(APPEND ${VAR} "${lib}${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}${library}") - else() - list(APPEND ${VAR} "${lib}") - endif() - math(EXPR _index "${_index} + 1") - endwhile() - #debug_message(10 "catkin_pack_libraries_with_build_configuration(${VAR} ${_argn}) ${${VAR}}") -endmacro() - -# -# Unpack a list of libraries with optional build configuration keyword prefixes. -# Libraries prefixed with a keyword are split into the keyword and the library. -# -# :param VAR: the output variable name -# :type VAR: string -# :param ARGN: a list of libraries -# :type ARGN: list of strings -# -# @public -# -macro(catkin_unpack_libraries_with_build_configuration VAR) - set(${VAR} "") - foreach(lib ${ARGN}) - string(REGEX REPLACE "^(debug|optimized|general)${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}(.+)$" "\\1;\\2" lib "${lib}") - list(APPEND ${VAR} "${lib}") - endforeach() - #set(_argn ${ARGN}) - #debug_message(10 "catkin_unpack_libraries_with_build_configuration(${VAR} ${_argn}) ${${VAR}}") -endmacro() - -# -# Replace imported library target names with the library name. -# -# :param VAR: the output variable name -# :type VAR: string -# :param ARGN: a list of libraries -# :type ARGN: list of strings -# -# @public -# -macro(catkin_replace_imported_library_targets VAR) - set(${VAR} "") - foreach(lib ${ARGN}) - if((NOT "${lib}" MATCHES "^debug|optimized|general$") AND TARGET ${lib}) - # sometimes cmake dependencies define imported targets, in which - # case the imported library information is not the target name, but - # the information embedded in cmake properties inside the imported library - get_target_property(${lib}_imported ${lib} IMPORTED) - if(${${lib}_imported}) - set(imported_libraries) # empty list - get_target_property(${lib}_imported_location ${lib} IMPORTED_LOCATION) - if(${lib}_imported_location) - list(APPEND imported_libraries ${${lib}_imported_location}) - else() - get_target_property(${lib}_imported_configurations ${lib} IMPORTED_CONFIGURATIONS) - foreach(cfg ${${lib}_imported_configurations}) - get_target_property(${lib}_imported_location_${cfg} ${lib} IMPORTED_LOCATION_${cfg}) - if(${lib}_imported_location_${cfg}) - list(APPEND imported_libraries ${${lib}_imported_location_${cfg}}) - endif() - endforeach() - endif() - foreach(imp_lib ${imported_libraries}) - list(APPEND ${VAR} "${imp_lib}") - endforeach() - else() - # not an imported library target - list(APPEND ${VAR} "${lib}") - endif() - else() - list(APPEND ${VAR} "${lib}") - endif() - endforeach() - #set(_argn ${ARGN}) - #debug_message(10 "catkin_replace_imported_library_targets(${VAR} ${_argn}) ${${VAR}}") -endmacro() diff --git a/cmake/catkin/catkin_metapackage.cmake b/cmake/catkin/catkin_metapackage.cmake deleted file mode 100644 index e111338a90..0000000000 --- a/cmake/catkin/catkin_metapackage.cmake +++ /dev/null @@ -1,45 +0,0 @@ -# -# It installs the package.xml file of a metapackage. -# -# .. note:: It must be called once for each metapackage. Best -# practice is to call this macro early in your root CMakeLists.txt, -# immediately after calling ``project()`` and -# ``find_package(catkin REQUIRED)``. -# -# :param DIRECTORY: the path to the package.xml file if not in the same -# location as the CMakeLists.txt file -# :type DIRECTORY: string -# -# @public -# -function(catkin_metapackage) - cmake_parse_arguments(ARG "" "DIRECTORY" "" ${ARGN}) - if(ARG_UNPARSED_ARGUMENTS) - message(FATAL_ERROR "catkin_metapackage() called with unused arguments: ${ARG_UNPARSED_ARGUMENTS}") - endif() - - # verify that project() has been called before - if(NOT PROJECT_NAME) - message(FATAL_ERROR "catkin_metapackage() PROJECT_NAME is not set. You must call project() before calling catkin_metapackage().") - endif() - if(PROJECT_NAME STREQUAL "Project") - message(FATAL_ERROR "catkin_metapackage() PROJECT_NAME is set to 'Project', which is not a valid project name. You must call project() before calling catkin_metapackage().") - endif() - - debug_message(10 "catkin_metapackage() called in file ${CMAKE_CURRENT_LIST_FILE}") - - if(NOT ARG_DIRECTORY) - if(${CMAKE_CURRENT_LIST_FILE} STREQUAL ${CMAKE_BINARY_DIR}/catkin_generated/metapackages/${PROJECT_NAME}/CMakeLists.txt) - set(ARG_DIRECTORY ${CMAKE_SOURCE_DIR}/${path}) - else() - set(ARG_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) - endif() - endif() - - catkin_package_xml(DIRECTORY ${ARG_DIRECTORY}) - - # install package.xml - install(FILES ${ARG_DIRECTORY}/package.xml - DESTINATION share/${PROJECT_NAME} - ) -endfunction() diff --git a/cmake/catkin/catkin_package.cmake b/cmake/catkin/catkin_package.cmake deleted file mode 100644 index 5a6aee1952..0000000000 --- a/cmake/catkin/catkin_package.cmake +++ /dev/null @@ -1,528 +0,0 @@ -# -# It installs the package.xml file, and it generates code for -# ``find_package`` and ``pkg-config`` so that other packages can get -# information about this package. For this purpose the information -# about include directories, libraries, further dependencies and -# CMake variables are used. -# -# .. note:: It must be called once for each package. It is indirectly -# calling``catkin_destinations()`` which will provide additional -# output variables. Please make sure to call ``catkin_package()`` -# before using those variables. -# -# :param INCLUDE_DIRS: ``CMAKE_CURRENT_SOURCE_DIR``-relative paths to -# C/C++ includes -# :type INCLUDE_DIRS: list of strings -# :param LIBRARIES: names of library targets that will appear in the -# ``catkin_LIBRARIES`` and ``${PROJECT_NAME}_LIBRARIES`` of other -# projects that search for you via ``find_package``. Currently -# this will break if the logical target names are not the same as -# the installed names. -# :type LIBRARIES: list of strings -# :param CATKIN_DEPENDS: a list of catkin projects which this project -# depends on. It is used when client code finds this project via -# ``find_package()`` or ``pkg-config``. Each project listed will in -# turn be ``find_package``\ -ed or is states as ``Requires`` in the -# .pc file. Therefore their ``INCLUDE_DIRS`` and ``LIBRARIES`` will -# be appended to ours. Only catkin projects should be used where it -# be guarantee that they are *find_packagable* and have pkg-config -# files. -# :type CATKIN_DEPENDS: list of strings -# :param DEPENDS: a list of CMake projects which this project depends -# on. Since they might not be *find_packagable* or lack a pkg-config -# file their ``INCLUDE_DIRS`` and ``LIBRARIES`` are passed directly. -# This requires that it has been ``find_package``\ -ed before. -# :type DEPENDS: list of strings -# :param CFG_EXTRAS: a CMake file containing extra stuff that should -# be accessible to users of this package after -# ``find_package``\ -ing it. This file must live in the -# subdirectory ``cmake`` or be an absolute path. Various additional -# file extension are possible: -# for a plain cmake file just ``.cmake``, for files expanded using -# CMake's ``configure_file()`` use ``.cmake.in`` or for files expanded -# by empy use ``.cmake.em``. The templates can distinguish between -# devel- and installspace using the boolean variables ``DEVELSPACE`` -# and ``INSTALLSPACE``. For templated files it is also possible to -# use the extensions ``.cmake.develspace.(in|em)`` or -# ``.cmake.installspace.(em|in)`` to generate the files only for a -# specific case. -# If the global variable ${PROJECT_NAME}_CFG_EXTRAS is set it will be -# prepended to the explicitly passed argument. -# :type CFG_EXTRAS: string -# :param EXPORTED_TARGETS: a list of target names which usually generate -# code. Downstream packages can depend on these targets to ensure that -# code is generated before it is being used. The generated CMake config -# file will ensure that the targets exists. -# If the global variable ${PROJECT_NAME}_EXPORTED_TARGETS is -# set it will be prepended to the explicitly passed argument. -# :type EXPORTED_TARGETS: list of strings -# :param SKIP_CMAKE_CONFIG_GENERATION: the option to skip the generation -# of the CMake config files for the package -# :type SKIP_CMAKE_CONFIG_GENERATION: bool -# :param SKIP_PKG_CONFIG_GENERATION: the option to skip the generation of -# the pkg-config file for the package -# :type SKIP_PKG_CONFIG_GENERATION: bool -# -# Example: -# :: -# -# catkin_package( -# INCLUDE_DIRS include -# LIBRARIES projlib1 projlib2 -# CATKIN_DEPENDS roscpp -# DEPENDS Eigen -# CFG_EXTRAS proj-extras[.cmake|.cmake.in|.cmake(.develspace|.installspace)?.em] -# ) -# -# @public -# -macro(catkin_package) - debug_message(10 "catkin_package() called in file ${CMAKE_CURRENT_LIST_FILE}") - - # verify that project() has been called before - if(NOT PROJECT_NAME) - message(FATAL_ERROR "catkin_package() PROJECT_NAME is not set. You must call project() before calling catkin_package().") - endif() - if(PROJECT_NAME STREQUAL "Project") - message(FATAL_ERROR "catkin_package() PROJECT_NAME is set to 'Project', which is not a valid project name. You must call project() before calling catkin_package().") - endif() - - # mark that catkin_package() was called in order to detect wrong order of calling with generate_messages() - set(${PROJECT_NAME}_CATKIN_PACKAGE TRUE) - - # call catkin_package_xml() if it has not been called before - if(NOT _CATKIN_CURRENT_PACKAGE) - catkin_package_xml() - endif() - - _catkin_package(${ARGN}) -endmacro() - -function(_catkin_package) - cmake_parse_arguments(PROJECT "SKIP_CMAKE_CONFIG_GENERATION;SKIP_PKG_CONFIG_GENERATION" "" "INCLUDE_DIRS;LIBRARIES;CATKIN_DEPENDS;DEPENDS;CFG_EXTRAS;EXPORTED_TARGETS" ${ARGN}) - if(PROJECT_UNPARSED_ARGUMENTS) - message(FATAL_ERROR "catkin_package() called with unused arguments: ${PROJECT_UNPARSED_ARGUMENTS}") - endif() - - if(NOT ${PROJECT_NAME} STREQUAL "catkin") - list(FIND ${PROJECT_NAME}_BUILDTOOL_DEPENDS "catkin" _index) - if(_index EQUAL -1) - list(FIND ${PROJECT_NAME}_BUILD_DEPENDS "catkin" _index) - if(_index EQUAL -1) - message(FATAL_ERROR "catkin_package() 'catkin' must be listed as a buildtool dependency in the package.xml") - endif() - message("WARNING: 'catkin' should be listed as a buildtool dependency in the package.xml (instead of build dependency)") - endif() - endif() - - # prepend INCLUDE_DIRS and LIBRARIES passed using a variable - if(${PROJECT_NAME}_INCLUDE_DIRS) - list(INSERT PROJECT_INCLUDE_DIRS 0 ${${PROJECT_NAME}_INCLUDE_DIRS}) - endif() - if(${PROJECT_NAME}_LIBRARIES) - list(INSERT PROJECT_LIBRARIES 0 ${${PROJECT_NAME}_LIBRARIES}) - endif() - - # unset previously found directory of this package, so that this package overlays the other cleanly - if(${PROJECT_NAME}_DIR) - set(${PROJECT_NAME}_DIR "" CACHE PATH "" FORCE) - endif() - - set(_PROJECT_CATKIN_DEPENDS ${PROJECT_CATKIN_DEPENDS}) - - set(PROJECT_DEPENDENCIES_INCLUDE_DIRS "") - set(PROJECT_DEPENDENCIES_LIBRARIES "") - foreach(depend ${PROJECT_DEPENDS}) - string(REPLACE " " ";" depend_list ${depend}) - # check if the second argument is the COMPONENTS keyword - list(LENGTH depend_list count) - set(second_item "") - if(${count} GREATER 1) - list(GET depend_list 1 second_item) - endif() - if("${second_item}" STREQUAL "COMPONENTS") - list(GET depend_list 0 depend_name) - if(NOT ${${depend_name}_FOUND}) - message(FATAL_ERROR "catkin_package() DEPENDS on '${depend}' which must be find_package()-ed before") - endif() - message(WARNING "catkin_package() DEPENDS on '${depend}' which is deprecated. find_package() it before and only DEPENDS on '${depend_name}' instead") - list(APPEND PROJECT_DEPENDENCIES_INCLUDE_DIRS ${${depend_name}_INCLUDE_DIRS}) - list(APPEND PROJECT_DEPENDENCIES_LIBRARIES ${${depend_name}_LIBRARIES}) - else() - # split multiple names (without COMPONENTS) into separate dependencies - foreach(depend_name ${depend_list}) - if(${depend_name}_FOUND_CATKIN_PROJECT) - #message(WARNING "catkin_package() DEPENDS on catkin package '${depend_name}' which is deprecated. Use CATKIN_DEPENDS for catkin packages instead.") - list(APPEND _PROJECT_CATKIN_DEPENDS ${depend_name}) - else() - if(NOT ${${depend_name}_FOUND}) - message(FATAL_ERROR "catkin_package() DEPENDS on '${depend_name}' which must be find_package()-ed before. If it is a catkin package it can be declared as CATKIN_DEPENDS instead without find_package()-ing it.") - endif() - list(APPEND PROJECT_DEPENDENCIES_INCLUDE_DIRS ${${depend_name}_INCLUDE_DIRS}) - list(APPEND PROJECT_DEPENDENCIES_LIBRARIES ${${depend_name}_LIBRARIES}) - endif() - endforeach() - endif() - endforeach() - - # for catkin packages it can be guaranteed that they are find_package()-able and have pkg-config files - set(PROJECT_DEPENDENCIES "") - foreach(depend_name ${_PROJECT_CATKIN_DEPENDS}) - # verify that all catkin packages which have been find_package()-ed are listed as build dependencies - if(${depend_name}_FOUND) - # verify that these packages are really catkin packages - if(NOT ${depend_name}_FOUND_CATKIN_PROJECT) - if(DEFINED ${depend_name}_CONFIG) - message(FATAL_ERROR "catkin_package() CATKIN_DEPENDS on '${depend_name}', which has been found in '${${depend_name}_CONFIG}', but it is not a catkin package") - else() - message(FATAL_ERROR "catkin_package() CATKIN_DEPENDS on '${depend_name}', but it is not a catkin package") - endif() - endif() - if(catkin_ALL_FOUND_COMPONENTS) - list(FIND catkin_ALL_FOUND_COMPONENTS ${depend_name} _index) - else() - set(_index -1) - endif() - if(NOT _index EQUAL -1) - list(FIND ${PROJECT_NAME}_BUILD_DEPENDS ${depend_name} _index) - if(_index EQUAL -1) - message(FATAL_ERROR "catkin_package() the catkin package '${depend_name}' has been find_package()-ed but is not listed as a build dependency in the package.xml") - endif() - # verify versioned dependency constraints - if(DEFINED ${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_LT AND - NOT "${${depend_name}_VERSION}" VERSION_LESS "${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_LT}") - message(WARNING "catkin_package() version mismatch: the package.xml of '${PROJECT_NAME}' build_depends on '${depend_name} < ${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_LT}', but '${depend_name} ${${depend_name}_VERSION}' found") - endif() - if(DEFINED ${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_LTE AND - "${${depend_name}_VERSION}" VERSION_GREATER "${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_LTE}") - message(WARNING "catkin_package() version mismatch: the package.xml of '${PROJECT_NAME}' build_depends on '${depend_name} <= ${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_LTE}', but '${depend_name} ${${depend_name}_VERSION}' found") - endif() - if(DEFINED ${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_EQ AND - NOT "${${depend_name}_VERSION}" VERSION_EQUAL "${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_EQ}") - message(WARNING "catkin_package() version mismatch: the package.xml of '${PROJECT_NAME}' build_depends on '${depend_name} = ${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_EQ}', but '${depend_name} ${${depend_name}_VERSION}' found") - endif() - if(DEFINED ${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_GTE AND - "${${depend_name}_VERSION}" VERSION_LESS "${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_GTE}") - message(WARNING "catkin_package() version mismatch: the package.xml of '${PROJECT_NAME}' build_depends on '${depend_name} >= ${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_GTE}', but '${depend_name} ${${depend_name}_VERSION}' found") - endif() - if(DEFINED ${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_GT AND - NOT "${${depend_name}_VERSION}" VERSION_GREATER "${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_GT}") - message(WARNING "catkin_package() version mismatch: the package.xml of '${PROJECT_NAME}' build_depends on '${depend_name} > ${${PROJECT_NAME}_BUILD_DEPENDS_${depend_name}_VERSION_GT}', but '${depend_name} ${${depend_name}_VERSION}' found") - endif() - endif() - endif() - # verify that all catkin packages are listed as run dependencies - list(FIND ${PROJECT_NAME}_RUN_DEPENDS ${depend_name} _index) - if(_index EQUAL -1) - message(FATAL_ERROR "catkin_package() DEPENDS on the catkin package '${depend_name}' which must therefore be listed as a run dependency in the package.xml") - endif() - list(APPEND PROJECT_DEPENDENCIES ${depend_name}) - endforeach() - - # package version provided by package.cmake/xml - set(PROJECT_VERSION ${${PROJECT_NAME}_VERSION}) - - # flag if package is deprecated provided by package.cmake/xml - set(PROJECT_DEPRECATED ${${PROJECT_NAME}_DEPRECATED}) - - # package maintainer provided by package.cmake/xml - set(PROJECT_MAINTAINER ${${PROJECT_NAME}_MAINTAINER}) - - # get library paths from all workspaces - set(lib_paths "") - foreach(workspace ${CATKIN_WORKSPACES}) - list_append_unique(lib_paths ${workspace}/lib) - endforeach() - - # merge explicitly listed libraries and libraries from non-catkin but find_package()-ed packages - set(_PKG_CONFIG_LIBRARIES "") - if(PROJECT_LIBRARIES) - list(APPEND _PKG_CONFIG_LIBRARIES ${PROJECT_LIBRARIES}) - endif() - if(PROJECT_DEPENDENCIES_LIBRARIES) - list(APPEND _PKG_CONFIG_LIBRARIES ${PROJECT_DEPENDENCIES_LIBRARIES}) - endif() - - # resolve imported library targets - catkin_replace_imported_library_targets(_PKG_CONFIG_LIBRARIES ${_PKG_CONFIG_LIBRARIES}) - - # deduplicate libraries while maintaining build configuration keywords - catkin_pack_libraries_with_build_configuration(_PKG_CONFIG_LIBRARIES ${_PKG_CONFIG_LIBRARIES}) - set(PKG_CONFIG_LIBRARIES "") - foreach(library ${_PKG_CONFIG_LIBRARIES}) - list_append_deduplicate(PKG_CONFIG_LIBRARIES ${library}) - endforeach() - catkin_unpack_libraries_with_build_configuration(PKG_CONFIG_LIBRARIES ${PKG_CONFIG_LIBRARIES}) - - # .pc files can not handle build configuration keywords therefore filter them out based on the current build type - set(PKG_CONFIG_LIBRARIES_WITH_PREFIX "") - catkin_filter_libraries_for_build_configuration(libraries ${PKG_CONFIG_LIBRARIES}) - foreach(library ${libraries}) - if(IS_ABSOLUTE ${library}) - get_filename_component(suffix ${library} EXT) - if(NOT "${suffix}" STREQUAL "${CMAKE_STATIC_LIBRARY_SUFFIX}") - set(library "-l:${library}") - endif() - else() - set(library "-l${library}") - endif() - list_append_deduplicate(PKG_CONFIG_LIBRARIES_WITH_PREFIX ${library}) - endforeach() - - # - # DEVEL SPACE - # - - # used in the cmake extra files - set(DEVELSPACE TRUE) - set(INSTALLSPACE FALSE) - - set(PROJECT_SPACE_DIR ${CATKIN_DEVEL_PREFIX}) - set(PKG_INCLUDE_PREFIX ${CMAKE_CURRENT_SOURCE_DIR}) - - # absolute path to include dirs and validate that they are existing either absolute or relative to packages source - set(PROJECT_CMAKE_CONFIG_INCLUDE_DIRS "") - set(PROJECT_PKG_CONFIG_INCLUDE_DIRS "") - foreach(idir ${PROJECT_INCLUDE_DIRS}) - if(IS_ABSOLUTE ${idir}) - if(IS_DIRECTORY ${idir}) - set(include ${idir}) - else() - message(FATAL_ERROR "catkin_package() absolute include dir '${idir}' does not exist") - endif() - elseif(IS_DIRECTORY ${PKG_INCLUDE_PREFIX}/${idir}) - set(include ${PKG_INCLUDE_PREFIX}/${idir}) - else() - message(FATAL_ERROR "catkin_package() include dir '${idir}' does not exist relative to '${PKG_INCLUDE_PREFIX}'") - endif() - list_append_unique(PROJECT_CMAKE_CONFIG_INCLUDE_DIRS ${include}) - list_append_unique(PROJECT_PKG_CONFIG_INCLUDE_DIRS ${include}) - endforeach() - if(PROJECT_DEPENDENCIES_INCLUDE_DIRS) - list_append_unique(PROJECT_CMAKE_CONFIG_INCLUDE_DIRS ${PROJECT_DEPENDENCIES_INCLUDE_DIRS}) - list_append_unique(PROJECT_PKG_CONFIG_INCLUDE_DIRS ${PROJECT_DEPENDENCIES_INCLUDE_DIRS}) - endif() - - # prepend library path of this workspace - set(PKG_CONFIG_LIB_PATHS ${lib_paths}) - list(INSERT PKG_CONFIG_LIB_PATHS 0 ${PROJECT_SPACE_DIR}/lib) - set(PKG_CMAKE_DIR ${PROJECT_SPACE_DIR}/share/${PROJECT_NAME}/cmake) - if("${PROJECT_NAME}" STREQUAL "catkin") - set(PKG_CMAKE_DIR "${catkin_EXTRAS_DIR}") - endif() - - if(NOT PROJECT_SKIP_PKG_CONFIG_GENERATION) - # ensure that output folder exists - file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/pkgconfig) - # generate devel space pc for project - em_expand(${catkin_EXTRAS_DIR}/templates/pkg.context.pc.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/pkg.develspace.context.pc.py - ${catkin_EXTRAS_DIR}/em/pkg.pc.em - ${CATKIN_DEVEL_PREFIX}/lib/pkgconfig/${PROJECT_NAME}.pc) - endif() - - # generate devel space cfg-extras for project - set(PKG_CFG_EXTRAS "") - foreach(extra ${${PROJECT_NAME}_CFG_EXTRAS} ${PROJECT_CFG_EXTRAS}) - if(IS_ABSOLUTE ${extra}) - set(base ${extra}) - get_filename_component(extra ${extra} NAME) - else() - set(base ${CMAKE_CURRENT_SOURCE_DIR}/cmake/${extra}) - endif() - if(EXISTS ${base}.em OR EXISTS ${base}.develspace.em) - if(EXISTS ${base}.develspace.em) - set(em_template ${base}.develspace.em) - else() - set(em_template ${base}.em) - endif() - em_expand(${catkin_EXTRAS_DIR}/templates/cfg-extras.context.py.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/${extra}.develspace.context.cmake.py - ${em_template} - ${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/cmake/${extra}) - list(APPEND PKG_CFG_EXTRAS ${extra}) - elseif(EXISTS ${base}.in OR EXISTS ${base}.develspace.in) - if(EXISTS ${base}.develspace.in) - set(in_template ${base}.develspace.in) - else() - set(in_template ${base}.in) - endif() - configure_file(${in_template} - ${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/cmake/${extra} - @ONLY - ) - list(APPEND PKG_CFG_EXTRAS ${extra}) - elseif(EXISTS ${base}) - list(APPEND PKG_CFG_EXTRAS ${base}) - elseif(NOT EXISTS ${base}.installspace.em AND NOT EXISTS ${base}.installspace.in) - message(FATAL_ERROR "catkin_package() could not find CFG_EXTRAS file. Either 'cmake/${extra}.develspace.em', 'cmake/${extra}.em', 'cmake/${extra}.develspace.in', 'cmake/${extra}.in', 'cmake/${extra}' or a variant specific to the installspace must exist.") - endif() - endforeach() - - if(NOT PROJECT_SKIP_CMAKE_CONFIG_GENERATION) - set(PKG_EXPORTED_TARGETS ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_EXPORTED_TARGETS}) - foreach(t ${PKG_EXPORTED_TARGETS}) - if(NOT TARGET ${t}) - message(FATAL_ERROR "catkin_package() could not find target '${t}' for code generation.") - endif() - endforeach() - - # generate devel space config for project - set(infile ${${PROJECT_NAME}_EXTRAS_DIR}/${PROJECT_NAME}Config.cmake.in) - if(NOT EXISTS ${infile}) - set(infile ${catkin_EXTRAS_DIR}/templates/pkgConfig.cmake.in) - endif() - configure_file(${infile} - ${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/cmake/${PROJECT_NAME}Config.cmake - @ONLY - ) - # generate devel space config-version for project - configure_file(${catkin_EXTRAS_DIR}/templates/pkgConfig-version.cmake.in - ${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/cmake/${PROJECT_NAME}Config-version.cmake - @ONLY - ) - endif() - - # - # INSTALLSPACE - # - - # used in the cmake extra files - set(DEVELSPACE FALSE) - set(INSTALLSPACE TRUE) - - set(PROJECT_SPACE_DIR ${CMAKE_INSTALL_PREFIX}) - set(PKG_INCLUDE_PREFIX ${PROJECT_SPACE_DIR}) - - # absolute path to include dir under install prefix if any include dir is set - set(PROJECT_CMAKE_CONFIG_INCLUDE_DIRS "") - set(PROJECT_PKG_CONFIG_INCLUDE_DIRS "") - foreach(idir ${PROJECT_INCLUDE_DIRS}) - # include dirs in source / build / devel space are handled like relative ones - # since these files are supposed to be installed to the include folder in install space - string_starts_with("${idir}/" "${CMAKE_CURRENT_SOURCE_DIR}/" _is_source_prefix) - string_starts_with("${idir}/" "${CMAKE_CURRENT_BINARY_DIR}/" _is_build_prefix) - string_starts_with("${idir}/" "${CATKIN_DEVEL_PREFIX}/" _is_devel_prefix) - if(_is_source_prefix OR _is_build_prefix OR _is_devel_prefix) - # generated header files should be places in the devel space rather then in the build space - if(_is_build_prefix) - message(WARNING "catkin_package() include dir '${idir}' should be placed in the devel space instead of the build space") - endif() - # the value doesn't matter as long as it doesn't match IS_ABSOLUTE - set(idir "${CATKIN_GLOBAL_INCLUDE_DESTINATION}") - endif() - if(IS_ABSOLUTE ${idir}) - list_append_unique(PROJECT_CMAKE_CONFIG_INCLUDE_DIRS "${idir}") - list_append_unique(PROJECT_PKG_CONFIG_INCLUDE_DIRS "${idir}") - else() - list_append_unique(PROJECT_CMAKE_CONFIG_INCLUDE_DIRS "${CATKIN_GLOBAL_INCLUDE_DESTINATION}") - list_append_unique(PROJECT_PKG_CONFIG_INCLUDE_DIRS "${PKG_INCLUDE_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}") - endif() - endforeach() - if(PROJECT_DEPENDENCIES_INCLUDE_DIRS) - list_append_unique(PROJECT_CMAKE_CONFIG_INCLUDE_DIRS ${PROJECT_DEPENDENCIES_INCLUDE_DIRS}) - list_append_unique(PROJECT_PKG_CONFIG_INCLUDE_DIRS ${PROJECT_DEPENDENCIES_INCLUDE_DIRS}) - endif() - - # prepend library path of this workspace - set(PKG_CONFIG_LIB_PATHS ${lib_paths}) - list(INSERT PKG_CONFIG_LIB_PATHS 0 ${PROJECT_SPACE_DIR}/lib) - # package cmake dir is the folder where the generated pkgConfig.cmake is located - set(PKG_CMAKE_DIR "\${${PROJECT_NAME}_DIR}") - - if(NOT PROJECT_SKIP_PKG_CONFIG_GENERATION) - # ensure that output folder exists - file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace) - # generate and install pc for project - em_expand(${catkin_EXTRAS_DIR}/templates/pkg.context.pc.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/pkg.installspace.context.pc.py - ${catkin_EXTRAS_DIR}/em/pkg.pc.em - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${PROJECT_NAME}.pc) - install(FILES ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${PROJECT_NAME}.pc - DESTINATION lib/pkgconfig - ) - endif() - - # generate and install cfg-extras for project - set(PKG_CFG_EXTRAS "") - set(installable_cfg_extras "") - foreach(extra ${${PROJECT_NAME}_CFG_EXTRAS} ${PROJECT_CFG_EXTRAS}) - if(IS_ABSOLUTE ${extra}) - set(base ${extra}) - get_filename_component(extra ${extra} NAME) - else() - set(base ${CMAKE_CURRENT_SOURCE_DIR}/cmake/${extra}) - endif() - if(EXISTS ${base}.em OR EXISTS ${base}.installspace.em) - if(EXISTS ${base}.installspace.em) - set(em_template ${base}.installspace.em) - else() - set(em_template ${base}.em) - endif() - em_expand(${catkin_EXTRAS_DIR}/templates/cfg-extras.context.py.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/${extra}.installspace.context.cmake.py - ${em_template} - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${extra}) - list(APPEND installable_cfg_extras ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${extra}) - list(APPEND PKG_CFG_EXTRAS ${extra}) - elseif(EXISTS ${base}.in OR EXISTS ${base}.installspace.in) - if(EXISTS ${base}.installspace.in) - set(in_template ${base}.installspace.in) - else() - set(in_template ${base}.in) - endif() - configure_file(${in_template} - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${extra} - @ONLY - ) - list(APPEND installable_cfg_extras ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${extra}) - list(APPEND PKG_CFG_EXTRAS ${extra}) - elseif(EXISTS ${base}) - list(APPEND installable_cfg_extras ${base}) - list(APPEND PKG_CFG_EXTRAS ${extra}) - elseif(NOT EXISTS ${base}.develspace.em AND NOT EXISTS ${base}.develspace.in) - message(FATAL_ERROR "catkin_package() could not find CFG_EXTRAS file. Either 'cmake/${extra}.installspace.em', 'cmake/${extra}.em', 'cmake/${extra}.installspace.in', 'cmake/${extra}.in', 'cmake/${extra}'or a variant specific to the develspace must exist.") - endif() - endforeach() - install(FILES - ${installable_cfg_extras} - DESTINATION share/${PROJECT_NAME}/cmake - ) - - if(NOT PROJECT_SKIP_CMAKE_CONFIG_GENERATION) - # generate config for project - set(infile ${${PROJECT_NAME}_EXTRAS_DIR}/${PROJECT_NAME}Config.cmake.in) - if(NOT EXISTS ${infile}) - set(infile ${catkin_EXTRAS_DIR}/templates/pkgConfig.cmake.in) - endif() - configure_file(${infile} - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${PROJECT_NAME}Config.cmake - @ONLY - ) - # generate config-version for project - set(infile ${${PROJECT_NAME}_EXTRAS_DIR}/${PROJECT_NAME}Config-version.cmake.in) - if(NOT EXISTS ${infile}) - set(infile ${catkin_EXTRAS_DIR}/templates/pkgConfig-version.cmake.in) - endif() - configure_file(${infile} - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${PROJECT_NAME}Config-version.cmake - @ONLY - ) - # install config, config-version and cfg-extras for project - install(FILES - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${PROJECT_NAME}Config.cmake - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/installspace/${PROJECT_NAME}Config-version.cmake - DESTINATION share/${PROJECT_NAME}/cmake - ) - endif() - - # install package.xml - install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/package.xml - DESTINATION share/${PROJECT_NAME} - ) - - if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/nodelet_plugins.xml) - install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/nodelet_plugins.xml - DESTINATION share/${PROJECT_NAME} ) - endif () -endfunction() diff --git a/cmake/catkin/catkin_package_xml.cmake b/cmake/catkin/catkin_package_xml.cmake deleted file mode 100644 index 99dc212add..0000000000 --- a/cmake/catkin/catkin_package_xml.cmake +++ /dev/null @@ -1,69 +0,0 @@ -# -# Parse package.xml from ``CMAKE_CURRENT_SOURCE_DIR`` and -# make several information available to CMake. -# -# .. note:: It is called automatically by ``catkin_package()`` if not -# called manually before. It must be called once in each package, -# after calling ``project()`` where the project name must match the -# package name. The macro should only be called manually if the -# variables are use to parameterize ``catkin_package()``. -# -# :param DIRECTORY: the directory of the package.xml (default -# ``${CMAKE_CURRENT_SOURCE_DIR}``). -# :type DIRECTORY: string -# -# :outvar _VERSION: the version number -# :outvar _MAINTAINER: the name and email of the -# maintainer(s) -# :outvar _CATKIN_CURRENT_PACKAGE: the name of the package from the -# manifest -# -# .. note:: It is calling ``catkin_destinations()`` which will provide -# additional output variables. -# -# @public -# -macro(catkin_package_xml) - debug_message(10 "catkin_package_xml()") - - # verify that project() has been called before - if(NOT PROJECT_NAME) - message(FATAL_ERROR "catkin_package_xml() PROJECT_NAME is not set. You must call project() before you can call catkin_package_xml().") - endif() - - # ensure that function is not called multiple times per package - if(DEFINED _CATKIN_CURRENT_PACKAGE) - message(FATAL_ERROR "catkin_package_xml(): in '${CMAKE_CURRENT_LIST_FILE}', _CATKIN_CURRENT_PACKAGE is already set (to: ${_CATKIN_CURRENT_PACKAGE}). Did you called catkin_package_xml() multiple times?") - endif() - - _catkin_package_xml(${CMAKE_CURRENT_BINARY_DIR}/catkin_generated ${ARGN}) - - # verify that the package name from package.xml equals the project() name - if(NOT _CATKIN_CURRENT_PACKAGE STREQUAL PROJECT_NAME) - message(FATAL_ERROR "catkin_package_xml() package name '${_CATKIN_CURRENT_PACKAGE}' in '${_PACKAGE_XML_DIRECTORY}/package.xml' does not match current PROJECT_NAME '${PROJECT_NAME}'. You must call project() with the same package name before.") - endif() - - catkin_destinations() -endmacro() - -macro(_catkin_package_xml dest_dir) - cmake_parse_arguments(_PACKAGE_XML "" "DIRECTORY" "" ${ARGN}) - if(_PACKAGE_XML_UNPARSED_ARGUMENTS) - message(FATAL_ERROR "catkin_package_xml() called with unused arguments: ${_PACKAGE_XML_UNPARSED_ARGUMENTS}") - endif() - - # set default directory - if(NOT _PACKAGE_XML_DIRECTORY) - set(_PACKAGE_XML_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) - endif() - - # stamp and parse package.xml - stamp(${_PACKAGE_XML_DIRECTORY}/package.xml) - file(MAKE_DIRECTORY ${dest_dir}) - safe_execute_process(COMMAND ${PYTHON_EXECUTABLE} - ${catkin_EXTRAS_DIR}/parse_package_xml.py - ${_PACKAGE_XML_DIRECTORY}/package.xml - ${dest_dir}/package.cmake) - # load extracted variable into cmake - include(${dest_dir}/package.cmake) -endmacro() diff --git a/cmake/catkin/catkin_python_setup.cmake b/cmake/catkin/catkin_python_setup.cmake deleted file mode 100644 index 2a8a0f7558..0000000000 --- a/cmake/catkin/catkin_python_setup.cmake +++ /dev/null @@ -1,150 +0,0 @@ -# This macro will interrogate the Python setup.py file in -# ``${${PROJECT_NAME}_SOURCE_DIR}``, and then creates forwarding -# Python :term:`pkgutil` infrastructure in devel space -# accordingly for the scripts and packages declared in setup.py. -# -# Doing so enables mixing :term:`generated code` in -# devel space with :term:`static code` from sourcespace within a -# single Python package. -# -# In addition, it adds the install command of -# distutils/setuputils to the install target. -# -# .. note:: If the project also uses genmsg message generation via -# ``generate_messages()`` this function must be called before. -# -# @public -# -function(catkin_python_setup) - if(ARGN) - message(FATAL_ERROR "catkin_python_setup() called with unused arguments: ${ARGN}") - endif() - - if(${PROJECT_NAME}_GENERATE_MESSAGES) - message(FATAL_ERROR "generate_messages() must be called after catkin_python_setup() in project '${PROJECT_NAME}'") - endif() - if(${PROJECT_NAME}_GENERATE_DYNAMIC_RECONFIGURE) - message(FATAL_ERROR "generate_dynamic_reconfigure_options() must be called after catkin_python_setup() in project '${PROJECT_NAME}'") - endif() - - if(NOT EXISTS ${${PROJECT_NAME}_SOURCE_DIR}/setup.py) - message(FATAL_ERROR "catkin_python_setup() called without 'setup.py' in project folder ' ${${PROJECT_NAME}_SOURCE_DIR}'") - endif() - - assert(PYTHON_INSTALL_DIR) - set(INSTALL_CMD_WORKING_DIRECTORY ${${PROJECT_NAME}_SOURCE_DIR}) - if(NOT WIN32) - set(INSTALL_SCRIPT - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/python_distutils_install.sh) - configure_file(${catkin_EXTRAS_DIR}/templates/python_distutils_install.sh.in - ${INSTALL_SCRIPT} - @ONLY) - else() - # need to convert install prefix to native path for python setuptools --prefix (its fussy about \'s) - file(TO_NATIVE_PATH ${CMAKE_INSTALL_PREFIX} PYTHON_INSTALL_PREFIX) - set(INSTALL_SCRIPT - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/python_distutils_install.bat) - configure_file(${catkin_EXTRAS_DIR}/templates/python_distutils_install.bat.in - ${INSTALL_SCRIPT} - @ONLY) - endif() - - # generate python script which gets executed at install time - configure_file(${catkin_EXTRAS_DIR}/templates/safe_execute_install.cmake.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/safe_execute_install.cmake) - install(SCRIPT ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/safe_execute_install.cmake) - - # interrogate setup.py - stamp(${${PROJECT_NAME}_SOURCE_DIR}/setup.py) - assert(CATKIN_ENV) - assert(PYTHON_EXECUTABLE) - set(cmd - ${CATKIN_ENV} ${PYTHON_EXECUTABLE} - ${catkin_EXTRAS_DIR}/interrogate_setup_dot_py.py - ${PROJECT_NAME} - ${${PROJECT_NAME}_SOURCE_DIR}/setup.py - ${${PROJECT_NAME}_BINARY_DIR}/catkin_generated/setup_py_interrogation.cmake - ) - debug_message(10 "catkin_python_setup() in project '{PROJECT_NAME}' executes: ${cmd}") - safe_execute_process(COMMAND ${cmd}) - include(${${PROJECT_NAME}_BINARY_DIR}/catkin_generated/setup_py_interrogation.cmake) - - # call catkin_package_xml() if it has not been called before - if(NOT _CATKIN_CURRENT_PACKAGE) - catkin_package_xml() - endif() - assert(${PROJECT_NAME}_VERSION) - # verify that version from setup.py is equal to version from package.xml - if(NOT "${${PROJECT_NAME}_SETUP_PY_VERSION}" STREQUAL "${${PROJECT_NAME}_VERSION}") - message(FATAL_ERROR "catkin_python_setup() version in setup.py (${${PROJECT_NAME}_SETUP_PY_VERSION}) differs from version in package.xml (${${PROJECT_NAME}_VERSION})") - endif() - - # generate relaying __init__.py for each python package - if(${PROJECT_NAME}_SETUP_PY_PACKAGES) - list(LENGTH ${PROJECT_NAME}_SETUP_PY_PACKAGES pkgs_count) - math(EXPR pkgs_range "${pkgs_count} - 1") - foreach(index RANGE ${pkgs_range}) - list(GET ${PROJECT_NAME}_SETUP_PY_PACKAGES ${index} pkg) - if("${pkg}" STREQUAL "${PROJECT_NAME}") - # mark that catkin_python_setup() was called and the setup.py file contains a package with the same name as the current project - # in order to disable installation of generated __init__.py files in generate_messages() and generate_dynamic_reconfigure_options() - set(${PROJECT_NAME}_CATKIN_PYTHON_SETUP_HAS_PACKAGE_INIT TRUE PARENT_SCOPE) - endif() - list(GET ${PROJECT_NAME}_SETUP_PY_PACKAGE_DIRS ${index} pkg_dir) - get_filename_component(name ${pkg_dir} NAME) - if(NOT ("${pkg}" STREQUAL "${name}")) - message(FATAL_ERROR "The package name '${pkg}' differs from the basename of the path '${pkg_dir}' in project '${PROJECT_NAME}'") - endif() - get_filename_component(path ${pkg_dir} PATH) - set(PACKAGE_PYTHONPATH ${CMAKE_CURRENT_SOURCE_DIR}/${path}) - configure_file(${catkin_EXTRAS_DIR}/templates/__init__.py.in - ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/${pkg}/__init__.py - @ONLY) - endforeach() - endif() - - # generate relay-script for each python module (and __init__.py files) if available - if(${PROJECT_NAME}_SETUP_PY_MODULES) - list(LENGTH ${PROJECT_NAME}_SETUP_PY_MODULES modules_count) - math(EXPR modules_range "${modules_count} - 1") - foreach(index RANGE ${modules_range}) - list(GET ${PROJECT_NAME}_SETUP_PY_MODULES ${index} module) - list(GET ${PROJECT_NAME}_SETUP_PY_MODULE_DIRS ${index} module_dir) - set(PYTHON_SCRIPT ${CMAKE_CURRENT_SOURCE_DIR}/${module_dir}/${module}) - if(EXISTS ${PYTHON_SCRIPT}) - get_filename_component(path ${module} PATH) - file(MAKE_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${path}") - configure_file(${catkin_EXTRAS_DIR}/templates/relay.py.in - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${module} - @ONLY) - # relay parent __init__.py files if they exist - while(NOT "${path}" STREQUAL "") - set(PYTHON_SCRIPT ${CMAKE_CURRENT_SOURCE_DIR}/${module_dir}/${path}/__init__.py) - if(EXISTS ${PYTHON_SCRIPT}) - file(MAKE_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${path}") - configure_file(${catkin_EXTRAS_DIR}/templates/relay.py.in - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${path}/__init__.py - @ONLY) - else() - message(WARNING "The module '${module_dir}/${module}' lacks an '__init__.py' file in the parent folder '${module_dir}/${path}' in project '${PROJECT_NAME}'") - endif() - get_filename_component(path ${path} PATH) - endwhile() - endif() - endforeach() - endif() - - # generate relay-script for each python script - foreach(script ${${PROJECT_NAME}_SETUP_PY_SCRIPTS}) - get_filename_component(name ${script} NAME) - if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${script}) - message(FATAL_ERROR "The script '${name}' as listed in 'setup.py' of '${PROJECT_NAME}' doesn't exist") - endif() - set(PYTHON_SCRIPT ${CMAKE_CURRENT_SOURCE_DIR}/${script}) - atomic_configure_file(${catkin_EXTRAS_DIR}/templates/script.py.in - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_BIN_DESTINATION}/${name} - @ONLY) - endforeach() -endfunction() - -stamp(${catkin_EXTRAS_DIR}/interrogate_setup_dot_py.py) diff --git a/cmake/catkin/catkin_workspace.cmake b/cmake/catkin/catkin_workspace.cmake deleted file mode 100644 index d346ff0fde..0000000000 --- a/cmake/catkin/catkin_workspace.cmake +++ /dev/null @@ -1,122 +0,0 @@ -# -# Search all subfolders in the workspace for ``package.xml`` files. -# Based on the dependencies specified in the ``build_depends``, -# ``buildtool_depends`` and (as of package format version 2) -# ``test_depends`` tags it performs a topological sort and calls -# ``add_subdirectory()`` for each directory. -# -# The functions is only called in catkin's ``toplevel.cmake``, which -# is usually symlinked to the workspace root directory (which -# contains multiple packages). -# -function(catkin_workspace) - debug_message(10 "catkin_workspace() called in file '${CMAKE_CURRENT_LIST_FILE}'") - - # set global output directories for artifacts and create them if necessary - set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib) - set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib) - if(NOT IS_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}) - file(MAKE_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}) - endif() - - # tools/libraries.cmake - configure_shared_library_build_settings() - - set(CATKIN_WHITELIST_PACKAGES "" CACHE STRING "List of ';' separated packages to build") - set(CATKIN_BLACKLIST_PACKAGES "" CACHE STRING "List of ';' separated packages to exclude") - if(NOT "${CATKIN_WHITELIST_PACKAGES}" STREQUAL "") - message(STATUS "Using CATKIN_WHITELIST_PACKAGES: ${CATKIN_WHITELIST_PACKAGES}") - endif() - if(NOT "${CATKIN_BLACKLIST_PACKAGES}" STREQUAL "") - message(STATUS "Using CATKIN_BLACKLIST_PACKAGES: ${CATKIN_BLACKLIST_PACKAGES}") - endif() - - assert(catkin_EXTRAS_DIR) - em_expand( - ${catkin_EXTRAS_DIR}/templates/order_packages.context.py.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/order_packages.py - ${catkin_EXTRAS_DIR}/em/order_packages.cmake.em - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/order_packages.cmake - ) - debug_message(10 "catkin_workspace() including order_packages.cmake") - include(${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/order_packages.cmake) - - if(CATKIN_ORDERED_PACKAGES) - set(CATKIN_NONCONFORMANT_METAPACKAGE FALSE) - set(CATKIN_NONHOMOGENEOUS_WORKSPACE FALSE) - list(LENGTH CATKIN_ORDERED_PACKAGES count) - message(STATUS "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~") - message(STATUS "~~ traversing ${count} packages in topological order:") - math(EXPR range "${count} - 1") - foreach(index RANGE ${range}) - list(GET CATKIN_ORDERED_PACKAGES ${index} name) - list(GET CATKIN_ORDERED_PACKAGE_PATHS ${index} path) - list(GET CATKIN_ORDERED_PACKAGES_IS_META ${index} is_meta) - list(GET CATKIN_ORDERED_PACKAGES_BUILD_TYPE ${index} build_type) - if(${is_meta}) - message(STATUS "~~ - ${name} (metapackage)") - # verify that CMakeLists.txt of metapackage conforms to standard - set(metapackage_arguments "") - assert(CATKIN_METAPACKAGE_CMAKE_TEMPLATE) - configure_file(${CATKIN_METAPACKAGE_CMAKE_TEMPLATE} - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/metapackages/${name}/CMakeLists.txt - @ONLY) - if(EXISTS ${CMAKE_SOURCE_DIR}/${path}/CMakeLists.txt) - # compare CMakeLists.txt with standard content - file(STRINGS ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/metapackages/${name}/CMakeLists.txt generated_cmakelists) - file(STRINGS ${path}/CMakeLists.txt existing_cmakelists) - if(NOT "${generated_cmakelists}" STREQUAL "${existing_cmakelists}") - set(CATKIN_NONHOMOGENEOUS_WORKSPACE TRUE) - message("WARNING: The CMakeLists.txt of the metapackage '${name}' contains non standard content. Use the content of the following file instead: ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/metapackages/${name}/CMakeLists.txt") - endif() - else() - message("WARNING: The metapackage '${name}' has no CMakeLists.txt. Please add one to the package source. You can use the following file: ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/metapackages/${name}/CMakeLists.txt") - endif() - else() - if(${build_type} MATCHES catkin) - message(STATUS "~~ - ${name}") - else() - set(CATKIN_NONHOMOGENEOUS_WORKSPACE TRUE) - if(${build_type} MATCHES cmake) - message(STATUS "~~ - ${name} (plain cmake)") - else() - message(STATUS "~~ - ${name} (unknown)") - message(WARNING "Unknown build type '${build_type}' for package '${name}'") - endif() - endif() - endif() - endforeach() - message(STATUS "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~") - - if(${CATKIN_NONCONFORMANT_METAPACKAGE}) - message(FATAL_ERROR "This workspace contains metapackages with a non-standard CMakeLists.txt.") - endif() - if(${CATKIN_NONHOMOGENEOUS_WORKSPACE}) - message(FATAL_ERROR "This workspace contains non-catkin packages in it, and catkin cannot build a non-homogeneous workspace without isolation. Try the 'catkin_make_isolated' command instead.") - endif() - - foreach(index RANGE ${range}) - list(GET CATKIN_ORDERED_PACKAGES ${index} name) - list(GET CATKIN_ORDERED_PACKAGE_PATHS ${index} path) - list(GET CATKIN_ORDERED_PACKAGES_IS_META ${index} is_meta) - list(GET CATKIN_ORDERED_PACKAGES_BUILD_TYPE ${index} build_type) - if(${is_meta}) - message(STATUS "+++ processing catkin metapackage: '${name}'") - if(EXISTS ${CMAKE_SOURCE_DIR}/${path}/CMakeLists.txt) - message(STATUS "==> add_subdirectory(${path})") - add_subdirectory(${path}) - else() - message(STATUS "==> add_subdirectory(${path}) (using generated file from /catkin_generated/metapackages/${name})") - message("WARNING: Add a CMakeLists.txt file to the metapackage '${name}'") - add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/metapackages/${name} ${CMAKE_BINARY_DIR}/${path}) - endif() - elseif(${build_type} MATCHES catkin) - message(STATUS "+++ processing catkin package: '${name}'") - message(STATUS "==> add_subdirectory(${path})") - add_subdirectory(${path}) - else() - message(FATAL_ERROR "Non-catkin package found, non-homogeneous workspaces are not supported.") - endif() - endforeach() - endif() -endfunction() diff --git a/cmake/catkin/debug_message.cmake b/cmake/catkin/debug_message.cmake deleted file mode 100644 index bdb94e5a11..0000000000 --- a/cmake/catkin/debug_message.cmake +++ /dev/null @@ -1,16 +0,0 @@ -# Log levels -# 0 Normal use -# 1 Catkin developer use (Stuff being developed) -# 2 Catkin developer use (Stuff working) -# 3 Also Print True Assert Statements - -macro(debug_message level) - set(loglevel ${CATKIN_LOG}) - if(NOT loglevel) - set(loglevel 0) - endif() - - if(NOT ${level} GREATER ${loglevel}) - message(STATUS " ${ARGN}") - endif() -endmacro() diff --git a/cmake/catkin/em/order_packages.cmake.em b/cmake/catkin/em/order_packages.cmake.em deleted file mode 100644 index 087d4d802e..0000000000 --- a/cmake/catkin/em/order_packages.cmake.em +++ /dev/null @@ -1,56 +0,0 @@ -# generated from catkin/cmake/em/order_packages.cmake.em -@{ -import os -try: - from catkin_pkg.cmake import get_metapackage_cmake_template_path -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.cmake import get_metapackage_cmake_template_path" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -try: - from catkin_pkg.topological_order import topological_order -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.topological_order import topological_order" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -try: - from catkin_pkg.package import InvalidPackage -except ImportError as e: - raise RuntimeError('ImportError: "from catkin_pkg.package import InvalidPackage" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) -# vars defined in order_packages.context.py.in -try: - ordered_packages = topological_order(os.path.normpath(source_root_dir), whitelisted=whitelisted_packages, blacklisted=blacklisted_packages, underlay_workspaces=underlay_workspaces) -except InvalidPackage as e: - print('message(FATAL_ERROR "%s")' % ('%s' % e).replace('"', '\\"')) - ordered_packages = [] -fatal_error = False -}@ - -set(CATKIN_ORDERED_PACKAGES "") -set(CATKIN_ORDERED_PACKAGE_PATHS "") -set(CATKIN_ORDERED_PACKAGES_IS_META "") -set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "") -@[for path, package in ordered_packages]@ -@[if path is None]@ -message(FATAL_ERROR "Circular dependency in subset of packages:\n@package") -@{ -fatal_error = True -}@ -@[elif package.name != 'catkin']@ -list(APPEND CATKIN_ORDERED_PACKAGES "@(package.name)") -list(APPEND CATKIN_ORDERED_PACKAGE_PATHS "@(path.replace('\\','/'))") -list(APPEND CATKIN_ORDERED_PACKAGES_IS_META "@(str('metapackage' in [e.tagname for e in package.exports]))") -list(APPEND CATKIN_ORDERED_PACKAGES_BUILD_TYPE "@(str([e.content for e in package.exports if e.tagname == 'build_type'][0]) if 'build_type' in [e.tagname for e in package.exports] else 'catkin')") -@{ -deprecated = [e for e in package.exports if e.tagname == 'deprecated'] -}@ -@[if deprecated]@ -message("WARNING: Package '@(package.name)' is deprecated@(' (%s)' % deprecated[0].content if deprecated[0].content else '')") -@[end if]@ -@[end if]@ -@[end for]@ - -@[if not fatal_error]@ -@{ -message_generators = [package.name for (_, package) in ordered_packages if 'message_generator' in [e.tagname for e in package.exports]] -}@ -set(CATKIN_MESSAGE_GENERATORS @(' '.join(message_generators))) -@[end if]@ - -set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "@(get_metapackage_cmake_template_path().replace('\\','/'))") diff --git a/cmake/catkin/em/pkg.pc.em b/cmake/catkin/em/pkg.pc.em deleted file mode 100644 index a4f72576cf..0000000000 --- a/cmake/catkin/em/pkg.pc.em +++ /dev/null @@ -1,8 +0,0 @@ -prefix=@PROJECT_SPACE_DIR - -Name: @(CATKIN_PACKAGE_PREFIX + PROJECT_NAME) -Description: Description of @PROJECT_NAME -Version: @PROJECT_VERSION -Cflags: @(' '.join(['-I%s' % include for include in PROJECT_PKG_CONFIG_INCLUDE_DIRS])) -Libs: -L@PROJECT_SPACE_DIR/lib @(' '.join(PKG_CONFIG_LIBRARIES_WITH_PREFIX)) -Requires: @(PROJECT_CATKIN_DEPENDS) diff --git a/cmake/catkin/em_expand.cmake b/cmake/catkin/em_expand.cmake deleted file mode 100644 index 0cca5f4667..0000000000 --- a/cmake/catkin/em_expand.cmake +++ /dev/null @@ -1,31 +0,0 @@ -macro(em_expand context_in context_out em_file_in file_out) - assert_file_exists("${context_in}" "input file for context missing") - assert_file_exists("${em_file_in}" "template file missing") - debug_message(2 "configure_file(${context_in}, ${context_out})") - configure_file(${context_in} ${context_out} @ONLY) - assert_file_exists("${context_out}" "context file was not generated correctly") - - stamp(${em_file_in}) - - # create directory if necessary - get_filename_component(_folder_out ${file_out} PATH) - if(NOT IS_DIRECTORY ${_folder_out}) - file(MAKE_DIRECTORY ${_folder_out}) - endif() - - debug_message(2 "Evaluate template '${em_file_in}' to '${file_out}' (with context from '${context_out}')") - assert(EMPY_SCRIPT) - # since empy contains a specific python version in its shebang line - # override the used python version by invoking it explicitly - set(command "${PYTHON_EXECUTABLE};${EMPY_SCRIPT}") - # prepend environment if set - if(CATKIN_ENV) - set(command ${CATKIN_ENV} ${command}) - endif() - safe_execute_process(COMMAND - ${command} - --raw-errors - -F ${context_out} - -o ${file_out} - ${em_file_in}) -endmacro() diff --git a/cmake/catkin/empy.cmake b/cmake/catkin/empy.cmake deleted file mode 100644 index 3328ce513d..0000000000 --- a/cmake/catkin/empy.cmake +++ /dev/null @@ -1,39 +0,0 @@ -function(find_python_module module) - # cribbed from http://www.cmake.org/pipermail/cmake/2011-January/041666.html - string(TOUPPER ${module} module_upper) - if(NOT PY_${module_upper}) - if(ARGC GREATER 1 AND ARGV1 STREQUAL "REQUIRED") - set(${module}_FIND_REQUIRED TRUE) - endif() - # A module's location is usually a directory, but for - # binary modules - # it's a .so file. - execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" "import re, ${module}; print(re.compile('/__init__.py.*').sub('',${module}.__file__))" - RESULT_VARIABLE _${module}_status - OUTPUT_VARIABLE _${module}_location - ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE) - if(NOT _${module}_status) - set(PY_${module_upper} ${_${module}_location} CACHE STRING "Location of Python module ${module}") - endif(NOT _${module}_status) - endif(NOT PY_${module_upper}) - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(PY_${module} DEFAULT_MSG PY_${module_upper}) -endfunction(find_python_module) - -if(NOT EMPY_SCRIPT) - find_program(EMPY_EXECUTABLE empy) - if(NOT EMPY_EXECUTABLE) - # On OSX, there's an em.py, but not an executable empy script - find_python_module(em) - if(NOT PY_EM) - message(FATAL_ERROR "Unable to find either executable 'empy' or Python module 'em'... try installing the package 'python-empy'") - endif() - # ensure to use cmake-style path separators on Windows - file(TO_CMAKE_PATH "${PY_EM}" EMPY_SCRIPT) - else() - # ensure to use cmake-style path separators on Windows - file(TO_CMAKE_PATH "${EMPY_EXECUTABLE}" EMPY_SCRIPT) - endif() - set(EMPY_SCRIPT "${EMPY_SCRIPT}" CACHE STRING "Empy script" FORCE) -endif() -#message(STATUS "Using empy: ${EMPY_SCRIPT}") diff --git a/cmake/catkin/env-hooks/05.catkin-test-results.bat.develspace.in b/cmake/catkin/env-hooks/05.catkin-test-results.bat.develspace.in deleted file mode 100644 index e2eb02379b..0000000000 --- a/cmake/catkin/env-hooks/05.catkin-test-results.bat.develspace.in +++ /dev/null @@ -1,4 +0,0 @@ -REM generated from catkin/cmake/env-hooks/05.catkin-test-results.bat.develspace.in - -set CATKIN_TEST_RESULTS_DIR="@CATKIN_TEST_RESULTS_DIR@" -set ROS_TEST_RESULTS_DIR=%CATKIN_TEST_RESULTS_DIR% diff --git a/cmake/catkin/env-hooks/05.catkin-test-results.sh.develspace.in b/cmake/catkin/env-hooks/05.catkin-test-results.sh.develspace.in deleted file mode 100644 index dcbf018650..0000000000 --- a/cmake/catkin/env-hooks/05.catkin-test-results.sh.develspace.in +++ /dev/null @@ -1,4 +0,0 @@ -# generated from catkin/cmake/env-hooks/05.catkin-test-results.sh.develspace.in - -export CATKIN_TEST_RESULTS_DIR="@CATKIN_TEST_RESULTS_DIR@" -export ROS_TEST_RESULTS_DIR="$CATKIN_TEST_RESULTS_DIR" diff --git a/cmake/catkin/env-hooks/05.catkin_make.bash b/cmake/catkin/env-hooks/05.catkin_make.bash deleted file mode 100644 index 621d668a46..0000000000 --- a/cmake/catkin/env-hooks/05.catkin_make.bash +++ /dev/null @@ -1,67 +0,0 @@ -function _catkin_make() -{ - local cur prev - cur=${COMP_WORDS[COMP_CWORD]} - prev=${COMP_WORDS[COMP_CWORD-1]} - - # autocomplete path arguments for -C, --directory, --source, --build - case $prev in - -C|--directory|--source|--build) - _filedir -d - return 0 - ;; - esac - - if [[ "$cur" == -DCMAKE_BUILD_TYPE=* ]]; then - # autocomplete CMake argument CMAKE_BUILD_TYPE with its options - COMPREPLY=( $( compgen -P "-DCMAKE_BUILD_TYPE=" -W "None Debug Release RelWithDebInfo MinSizeRel" -- "${cur:19}" ) ) - elif [[ "$cur" == -DCATKIN_ENABLE_TESTING=* ]]; then - # autocomplete catkin argument CATKIN_ENABLE_TESTING with its options - COMPREPLY=( $( compgen -P "-DCATKIN_ENABLE_TESTING=" -W "0 1" -- "${cur:24}" ) ) - elif [[ "$cur" == -DCATKIN_DEVEL_PREFIX=* || "$cur" == -DCMAKE_INSTALL_PREFIX=* ]]; then - COMPREPLY=() - elif [[ "$cur" == -* ]]; then - local opts="$( _parse_help "$1" )" - [[ $opts ]] || opts="$( _parse_usage "$1" )" - if [[ "$cur" == -* ]]; then - # suggest some common CMake arguments - opts="$opts -DCATKIN_DEVEL_PREFIX= -DCATKIN_ENABLE_TESTING= -DCMAKE_INSTALL_PREFIX= -DCMAKE_BUILD_TYPE=" - fi - COMPREPLY=( $( compgen -W "$opts" -- "$cur" ) ) - [[ $COMPREPLY == *= ]] && compopt -o nospace - else - # check if custom workspace root has been specified on the command line - local workspace_dir="." - for (( i=0; i < ${#COMP_WORDS[@]}; i++ )); do - if [[ ${COMP_WORDS[i]} == -C || ${COMP_WORDS[i]} == --directory ]]; then - # eval to expand tilde - eval workspace_dir=${COMP_WORDS[i+1]} - fi - done - # check if custom build folder has been specified on the command line - local build_dir="build" - for (( i=0; i < ${#COMP_WORDS[@]}; i++ )); do - if [[ ${COMP_WORDS[i]} == --build ]]; then - # eval to expand tilde - eval build_dir=${COMP_WORDS[i+1]} - fi - done - - # determine location of Makefile - local makefile_dir - if [[ "$build_dir" = /* ]]; then - makefile_dir="$build_dir" - else - makefile_dir="$workspace_dir/$build_dir" - fi - COMPREPLY=() - if [ -f "$makefile_dir/Makefile" ]; then - cur=${COMP_WORDS[COMP_CWORD]} - COMPREPLY=( $( compgen -W "`make -C $makefile_dir -qp 2>/dev/null | awk -F':' '/^[a-zA-Z0-9][a-zA-Z0-9_\.]*:/ { print $1 }'`" -- $cur )) - elif [ -f "$makefile_dir/build.ninja" ]; then - cur=${COMP_WORDS[COMP_CWORD]} - COMPREPLY=( $( compgen -W "`ninja -C $makefile_dir -t targets 2>/dev/null | awk -F':' '/^[a-zA-Z0-9][a-zA-Z0-9_\.]*:/ { print $1 }'`" -- $cur )) - fi - fi -} && -complete -F _catkin_make catkin_make diff --git a/cmake/catkin/env-hooks/05.catkin_make_isolated.bash b/cmake/catkin/env-hooks/05.catkin_make_isolated.bash deleted file mode 100644 index 99e5d30164..0000000000 --- a/cmake/catkin/env-hooks/05.catkin_make_isolated.bash +++ /dev/null @@ -1,65 +0,0 @@ -function _catkin_make_isolated() -{ - local cur prev - cur=${COMP_WORDS[COMP_CWORD]} - prev=${COMP_WORDS[COMP_CWORD-1]} - - # autocomplete path arguments for -C, --directory, --source, --build, --devel, --install - case $prev in - -C|--directory|--source|--build|--devel|--install) - _filedir -d - return 0 - ;; - esac - - if [[ "$cur" == -DCMAKE_BUILD_TYPE=* ]]; then - # autocomplete CMake argument CMAKE_BUILD_TYPE with its options - COMPREPLY=( $( compgen -P "-DCMAKE_BUILD_TYPE=" -W "None Debug Release RelWithDebInfo MinSizeRel" -- "${cur:19}" ) ) - elif [[ "$cur" == -DCATKIN_ENABLE_TESTING=* ]]; then - # autocomplete catkin argument CATKIN_ENABLE_TESTING with its options - COMPREPLY=( $( compgen -P "-DCATKIN_ENABLE_TESTING=" -W "0 1" -- "${cur:24}" ) ) - elif [[ "$cur" == -* ]]; then - local opts="$( _parse_help "$1" )" - [[ $opts ]] || opts="$( _parse_usage "$1" )" - if [[ "$cur" == -* ]]; then - # suggest some common CMake arguments - opts="$opts -DCATKIN_ENABLE_TESTING= -DCMAKE_BUILD_TYPE=" - fi - COMPREPLY=( $( compgen -W "$opts" -- "$cur" ) ) - [[ $COMPREPLY == *= ]] && compopt -o nospace - else - # check if custom workspace root has been specified on the command line - local workspace_dir="." - for (( i=0; i < ${#COMP_WORDS[@]}; i++ )); do - if [[ ${COMP_WORDS[i]} == -C || ${COMP_WORDS[i]} == --directory ]]; then - # eval to expand tilde - eval workspace_dir=${COMP_WORDS[i+1]} - fi - done - # check if custom build folder has been specified on the command line - local build_dir="build_isolated" - for (( i=0; i < ${#COMP_WORDS[@]}; i++ )); do - if [[ ${COMP_WORDS[i]} == --build ]]; then - # eval to expand tilde - eval build_dir=${COMP_WORDS[i+1]} - fi - done - - # determine location of Makefile - local makefile_dir - if [[ "$build_dir" = /* ]]; then - makefile_dir="$build_dir" - else - makefile_dir="$workspace_dir/$build_dir" - fi - COMPREPLY=() - if [ -f "$makefile_dir/Makefile" ]; then - cur=${COMP_WORDS[COMP_CWORD]} - COMPREPLY=( $( compgen -W "`make -C $makefile_dir -qp 2>/dev/null | awk -F':' '/^[a-zA-Z0-9][a-zA-Z0-9_\.]*:/ { print $1 }'`" -- $cur )) - elif [ -f "$makefile_dir/build.ninja" ]; then - cur=${COMP_WORDS[COMP_CWORD]} - COMPREPLY=( $( compgen -W "`ninja -C $makefile_dir -t targets 2>/dev/null | awk -F':' '/^[a-zA-Z0-9][a-zA-Z0-9_\.]*:/ { print $1 }'`" -- $cur )) - fi - fi -} && -complete -F _catkin_make_isolated catkin_make_isolated diff --git a/cmake/catkin/find_program_required.cmake b/cmake/catkin/find_program_required.cmake deleted file mode 100644 index 4cd3ab0c79..0000000000 --- a/cmake/catkin/find_program_required.cmake +++ /dev/null @@ -1,9 +0,0 @@ -function(find_program_required ARG_VAR ARG_PROGRAM_NAME) - cmake_parse_arguments(ARG "NO_DEFAULT_PATH;NO_CMAKE_FIND_ROOT_PATH" "" "PATHS" ${ARGN}) - find_program(${ARG_VAR} ${ARG_PROGRAM_NAME} PATHS ${ARG_PATHS} ${ARG_NO_DEFAULT_PATH} ${ARG_NO_CMAKE_FIND_ROOT_PATH}) - if(NOT ${ARG_VAR}) - message(FATAL_ERROR "${ARG_PROGRAM_NAME} not found") - else() - debug_message(1 "${ARG_PROGRAM_NAME} was found at ${${ARG_VAR}}") - endif() -endfunction() diff --git a/cmake/catkin/interrogate_setup_dot_py.py b/cmake/catkin/interrogate_setup_dot_py.py deleted file mode 100644 index 26882f02aa..0000000000 --- a/cmake/catkin/interrogate_setup_dot_py.py +++ /dev/null @@ -1,250 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, 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: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import print_function -import os -import sys - -import distutils.core -try: - import setuptools -except ImportError: - pass - -from argparse import ArgumentParser - - -def _get_locations(pkgs, package_dir): - """ - based on setuptools logic and the package_dir dict, builds a dict - of location roots for each pkg in pkgs. - See http://docs.python.org/distutils/setupscript.html - - :returns: a dict {pkgname: root} for each pkgname in pkgs (and each of their parents) - """ - # package_dir contains a dict {package_name: relativepath} - # Example {'': 'src', 'foo': 'lib', 'bar': 'lib2'} - # - # '' means where to look for any package unless a parent package - # is listed so package bar.pot is expected at lib2/bar/pot, - # whereas package sup.dee is expected at src/sup/dee - # - # if package_dir does not state anything about a package, - # setuptool expects the package folder to be in the root of the - # project - locations = {} - allprefix = package_dir.get('', '') - for pkg in pkgs: - parent_location = None - splits = pkg.split('.') - # we iterate over compound name from parent to child - # so once we found parent, children just append to their parent - for key_len in range(len(splits)): - key = '.'.join(splits[:key_len + 1]) - if key not in locations: - if key in package_dir: - locations[key] = package_dir[key] - elif parent_location is not None: - locations[key] = parent_location - else: - locations[key] = allprefix - parent_location = locations[key] - return locations - - -def generate_cmake_file(package_name, version, scripts, package_dir, pkgs, modules): - """ - Generates lines to add to a cmake file which will set variables - - :param version: str, format 'int.int.int' - :param scripts: [list of str]: relative paths to scripts - :param package_dir: {modulename: path} - :pkgs: [list of str] python_packages declared in catkin package - :modules: [list of str] python modules - """ - prefix = '%s_SETUP_PY' % package_name - result = [] - result.append(r'set(%s_VERSION "%s")' % (prefix, version)) - result.append(r'set(%s_SCRIPTS "%s")' % (prefix, ';'.join(scripts))) - - # Remove packages with '.' separators. - # - # setuptools allows specifying submodules in other folders than - # their parent - # - # The symlink approach of catkin does not work with such submodules. - # In the common case, this does not matter as the submodule is - # within the containing module. We verify this assumption, and if - # it passes, we remove submodule packages. - locations = _get_locations(pkgs, package_dir) - for pkgname, location in locations.items(): - if not '.' in pkgname: - continue - splits = pkgname.split('.') - # hack: ignore write-combining setup.py files for msg and srv files - if splits[1] in ['msg', 'srv']: - continue - # check every child has the same root folder as its parent - parent_name = '.'.join(splits[:1]) - if location != locations[parent_name]: - raise RuntimeError( - "catkin_export_python does not support setup.py files that combine across multiple directories: %s in %s, %s in %s" % (pkgname, location, parent_name, locations[parent_name])) - - # If checks pass, remove all submodules - pkgs = [p for p in pkgs if '.' not in p] - - resolved_pkgs = [] - for pkg in pkgs: - resolved_pkgs += [os.path.join(locations[pkg], pkg)] - - result.append(r'set(%s_PACKAGES "%s")' % (prefix, ';'.join(pkgs))) - result.append(r'set(%s_PACKAGE_DIRS "%s")' % (prefix, ';'.join(resolved_pkgs).replace("\\", "/"))) - - # skip modules which collide with package names - filtered_modules = [] - for modname in modules: - splits = modname.split('.') - # check all parents too - equals_package = [('.'.join(splits[:-i]) in locations) for i in range(len(splits))] - if any(equals_package): - continue - filtered_modules.append(modname) - module_locations = _get_locations(filtered_modules, package_dir) - - result.append(r'set(%s_MODULES "%s")' % (prefix, ';'.join(['%s.py' % m.replace('.', '/') for m in filtered_modules]))) - result.append(r'set(%s_MODULE_DIRS "%s")' % (prefix, ';'.join([module_locations[m] for m in filtered_modules]).replace("\\", "/"))) - - return result - - -def _create_mock_setup_function(package_name, outfile): - """ - Creates a function to call instead of distutils.core.setup or - setuptools.setup, which just captures some args and writes them - into a file that can be used from cmake - - :param package_name: name of the package - :param outfile: filename that cmake will use afterwards - :returns: a function to replace disutils.core.setup and setuptools.setup - """ - - def setup(*args, **kwargs): - ''' - Checks kwargs and writes a scriptfile - ''' - if 'version' not in kwargs: - sys.stderr.write("\n*** Unable to find 'version' in setup.py of %s\n" % package_name) - raise RuntimeError("version not found in setup.py") - version = kwargs['version'] - package_dir = kwargs.get('package_dir', {}) - - pkgs = kwargs.get('packages', []) - scripts = kwargs.get('scripts', []) - modules = kwargs.get('py_modules', []) - - unsupported_args = [ - 'entry_points', - 'exclude_package_data', - 'ext_modules ', - 'ext_package', - 'include_package_data', - 'namespace_packages', - 'setup_requires', - 'use_2to3', - 'zip_safe'] - used_unsupported_args = [arg for arg in unsupported_args if arg in kwargs] - if used_unsupported_args: - sys.stderr.write("*** Arguments %s to setup() not supported in catkin devel space in setup.py of %s\n" % (used_unsupported_args, package_name)) - - result = generate_cmake_file(package_name=package_name, - version=version, - scripts=scripts, - package_dir=package_dir, - pkgs=pkgs, - modules=modules) - with open(outfile, 'w') as out: - out.write('\n'.join(result)) - - return setup - - -def main(): - """ - Script main, parses arguments and invokes Dummy.setup indirectly. - """ - parser = ArgumentParser(description='Utility to read setup.py values from cmake macros. Creates a file with CMake set commands setting variables.') - parser.add_argument('package_name', help='Name of catkin package') - parser.add_argument('setupfile_path', help='Full path to setup.py') - parser.add_argument('outfile', help='Where to write result to') - - args = parser.parse_args() - - # print("%s" % sys.argv) - # PACKAGE_NAME = sys.argv[1] - # OUTFILE = sys.argv[3] - # print("Interrogating setup.py for package %s into %s " % (PACKAGE_NAME, OUTFILE), - # file=sys.stderr) - - # print("executing %s" % args.setupfile_path) - - # be sure you're in the directory containing - # setup.py so the sys.path manipulation works, - # so the import of __version__ works - os.chdir(os.path.dirname(os.path.abspath(args.setupfile_path))) - - # patch setup() function of distutils and setuptools for the - # context of evaluating setup.py - try: - fake_setup = _create_mock_setup_function(package_name=args.package_name, - outfile=args.outfile) - - distutils_backup = distutils.core.setup - distutils.core.setup = fake_setup - try: - setuptools_backup = setuptools.setup - setuptools.setup = fake_setup - except NameError: - pass - - with open(args.setupfile_path, 'r') as fh: - exec(fh.read()) - finally: - distutils.core.setup = distutils_backup - try: - setuptools.setup = setuptools_backup - except NameError: - pass - -if __name__ == '__main__': - main() diff --git a/cmake/catkin/legacy.cmake b/cmake/catkin/legacy.cmake deleted file mode 100644 index a4c65c2146..0000000000 --- a/cmake/catkin/legacy.cmake +++ /dev/null @@ -1,3 +0,0 @@ -function(catkin_stack) - message(FATAL_ERROR "catkin_stack() is not supported by catkin as of groovy. Please update the package to the latest catkin version.") -endfunction() diff --git a/cmake/catkin/list_append_deduplicate.cmake b/cmake/catkin/list_append_deduplicate.cmake deleted file mode 100644 index e59a61bb14..0000000000 --- a/cmake/catkin/list_append_deduplicate.cmake +++ /dev/null @@ -1,16 +0,0 @@ -# -# Append elements to a list and remove existing duplicates from the list. -# -# .. note:: Using CMake's ``list(APPEND ..)`` and -# ``list(REMOVE_DUPLICATES ..)`` is not sufficient since its -# implementation uses a set internally which makes the operation -# unstable. -# -macro(list_append_deduplicate listname) - if(NOT "${ARGN}" STREQUAL "") - if(${listname}) - list(REMOVE_ITEM ${listname} ${ARGN}) - endif() - list(APPEND ${listname} ${ARGN}) - endif() -endmacro() diff --git a/cmake/catkin/list_append_unique.cmake b/cmake/catkin/list_append_unique.cmake deleted file mode 100644 index 4c9bb1df86..0000000000 --- a/cmake/catkin/list_append_unique.cmake +++ /dev/null @@ -1,16 +0,0 @@ -# -# Append elements to a list if they are not already in the list. -# -# .. note:: Using CMake's ``list(APPEND ..)`` and -# ``list(REMOVE_DUPLICATES ..)`` is not sufficient since its -# implementation uses a set internally which makes the operation -# unstable. -# -macro(list_append_unique listname) - foreach(_item ${ARGN}) - list(FIND ${listname} ${_item} _index) - if(_index EQUAL -1) - list(APPEND ${listname} ${_item}) - endif() - endforeach() -endmacro() diff --git a/cmake/catkin/list_insert_in_workspace_order.cmake b/cmake/catkin/list_insert_in_workspace_order.cmake deleted file mode 100644 index 57ade4a493..0000000000 --- a/cmake/catkin/list_insert_in_workspace_order.cmake +++ /dev/null @@ -1,35 +0,0 @@ -# -# Insert elements to a list in the same order as the chained catkin workspaces. -# -set(CATKIN_ORDERED_SPACES "") -foreach(_space ${CATKIN_DEVEL_PREFIX} ${CATKIN_WORKSPACES}) - list(APPEND CATKIN_ORDERED_SPACES ${_space}) - if(NOT EXISTS "${_space}/.catkin") - message(FATAL_ERROR "The path '${_space}' is in CATKIN_WORKSPACES but does not have a .catkin file") - endif() - # prepend to existing list of sourcespaces - file(READ "${_space}/.catkin" _sourcespaces) - list(APPEND CATKIN_ORDERED_SPACES ${_sourcespaces}) -endforeach() - -debug_message(10 "CATKIN_ORDERED_SPACES ${CATKIN_ORDERED_SPACES}") - -macro(list_insert_in_workspace_order listname) - if(NOT "${ARGN}" STREQUAL "") - assert(CATKIN_ENV) - assert(PYTHON_EXECUTABLE) - set(cmd - ${CATKIN_ENV} ${PYTHON_EXECUTABLE} - ${catkin_EXTRAS_DIR}/order_paths.py - ${${PROJECT_NAME}_BINARY_DIR}/catkin_generated/ordered_paths.cmake - --paths-to-order ${ARGN} - --prefixes ${CATKIN_ORDERED_SPACES} - ) - debug_message(10 "list_insert_in_workspace_order() in project '{PROJECT_NAME}' executes: ${cmd}") - safe_execute_process(COMMAND ${cmd}) - include(${${PROJECT_NAME}_BINARY_DIR}/catkin_generated/ordered_paths.cmake) - set(${listname} ${ORDERED_PATHS}) - else() - set(${listname} "") - endif() -endmacro() diff --git a/cmake/catkin/order_paths.py b/cmake/catkin/order_paths.py deleted file mode 100644 index d2a3eca105..0000000000 --- a/cmake/catkin/order_paths.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function - -from argparse import ArgumentParser -import os -import sys - -try: - from catkin_pkg.workspaces import order_paths -except ImportError as e: - sys.exit('ImportError: "from catkin_pkg.package import parse_package" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) - - -def main(): - """ - Order a list of paths according to a list of prefixes which define the order. - """ - parser = ArgumentParser(description='Utility to order a list of paths according to a list of prefixes. Creates a file with CMake set command setting a variable.') - parser.add_argument('outfile', help='The filename of the generated CMake file') - parser.add_argument('--paths-to-order', nargs='*', help='The semicolon-separated paths to order') - parser.add_argument('--prefixes', nargs='*', help='The semicolon-separated prefixes defining the order') - args = parser.parse_args() - - ordered_paths = order_paths(args.paths_to_order, args.prefixes) - - # create directory if necessary - outdir = os.path.dirname(args.outfile) - if not os.path.exists(outdir): - os.makedirs(outdir) - - with open(args.outfile, 'w') as fh: - fh.write('set(ORDERED_PATHS "%s")' % ';'.join(ordered_paths)) - - -if __name__ == '__main__': - main() diff --git a/cmake/catkin/parse_package_xml.py b/cmake/catkin/parse_package_xml.py deleted file mode 100755 index 71e3e91877..0000000000 --- a/cmake/catkin/parse_package_xml.py +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, 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: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import print_function -import sys -import argparse - -try: - from catkin_pkg.package import parse_package -except ImportError as e: - sys.exit('ImportError: "from catkin_pkg.package import parse_package" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e) - - -def _get_output(package): - """ - returns a list of strings with cmake commands to execute to set cmake variables - - :param package: Package object - :returns: list of str, lines to output - """ - values = {} - values['VERSION'] = '"%s"' % package.version - - values['MAINTAINER'] = '"%s"' % (', '.join([str(m) for m in package.maintainers])) - - values.update(_get_dependency_values('BUILD_DEPENDS', package.build_depends)) - values.update(_get_dependency_values('BUILDTOOL_DEPENDS', package.buildtool_depends)) - values.update(_get_dependency_values('RUN_DEPENDS', package.run_depends)) - - deprecated = [e.content for e in package.exports if e.tagname == 'deprecated'] - values['DEPRECATED'] = '"%s"' % ((deprecated[0] if deprecated[0] else 'TRUE') if deprecated else '') - - output = [] - output.append(r'set(_CATKIN_CURRENT_PACKAGE "%s")' % package.name) - for k, v in values.items(): - output.append('set(%s_%s %s)' % (package.name, k, v)) - return output - -def _get_dependency_values(key, depends): - values = {} - values[key] = ' '.join(['"%s"' % str(d) for d in depends]) - for d in depends: - comparisons = ['version_lt', 'version_lte', 'version_eq', 'version_gte', 'version_gt'] - for comp in comparisons: - value = getattr(d, comp, None) - if value is not None: - values['%s_%s_%s' % (key, str(d), comp.upper())] = '"%s"' % value - return values - - -def main(argv=sys.argv[1:]): - """ - Reads given package_xml and writes extracted variables to outfile. - """ - parser = argparse.ArgumentParser(description="Read package.xml and write extracted variables to stdout") - parser.add_argument('package_xml') - parser.add_argument('outfile') - args = parser.parse_args(argv) - package = parse_package(args.package_xml) - - lines = _get_output(package) - with open(args.outfile, 'w') as ofile: - ofile.write('\n'.join(lines)) - - -if __name__ == '__main__': - main() diff --git a/cmake/catkin/platform/lsb.cmake b/cmake/catkin/platform/lsb.cmake deleted file mode 100644 index 7aca48b5f5..0000000000 --- a/cmake/catkin/platform/lsb.cmake +++ /dev/null @@ -1,28 +0,0 @@ -find_program(LSB_RELEASE_EXECUTABLE lsb_release) - -if(LSB_RELEASE_EXECUTABLE) - set(LSB_FOUND TRUE CACHE BOOL "lsb_release executable was found") - execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -si - OUTPUT_VARIABLE LSB_DISTRIB_ID - OUTPUT_STRIP_TRAILING_WHITESPACE) - string(TOUPPER ${LSB_DISTRIB_ID} v) - set(${v} TRUE CACHE BOOL "LSB Distrib tag") - - execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -sd - OUTPUT_VARIABLE LSB_DESCRIPTION - OUTPUT_STRIP_TRAILING_WHITESPACE) - execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -sr - OUTPUT_VARIABLE LSB_RELEASE - OUTPUT_STRIP_TRAILING_WHITESPACE) - execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -sc - OUTPUT_VARIABLE LSB_CODENAME - OUTPUT_STRIP_TRAILING_WHITESPACE) - - string(TOUPPER ${LSB_DISTRIB_ID} v) - set(${v} TRUE CACHE BOOL "LSB Distribution") - #message(STATUS "${v} is on") - - string(TOUPPER ${LSB_DISTRIB_ID}_${LSB_CODENAME} v) - set(${v} TRUE CACHE BOOL "LSB Distrib - codename tag") - #message(STATUS "${v} is on") -endif() diff --git a/cmake/catkin/platform/ubuntu.cmake b/cmake/catkin/platform/ubuntu.cmake deleted file mode 100644 index 3850736270..0000000000 --- a/cmake/catkin/platform/ubuntu.cmake +++ /dev/null @@ -1,4 +0,0 @@ -# requires lsb to be included first -if(UBUNTU) - # ubuntu-specifics go here -endif() diff --git a/cmake/catkin/platform/windows.cmake b/cmake/catkin/platform/windows.cmake deleted file mode 100644 index 35e61be15c..0000000000 --- a/cmake/catkin/platform/windows.cmake +++ /dev/null @@ -1,56 +0,0 @@ -# BUILD_SHARED_LIBS is a global cmake variable (usually defaults to on) -# that determines the build type of libraries: -# http://www.cmake.org/cmake/help/cmake-2-8-docs.html#variable:BUILD_SHARED_LIBS -# It defaults to shared. - -# Make sure this is already defined as a cached variable (@sa tools/libraries.cmake) -if(NOT DEFINED BUILD_SHARED_LIBS) - option(BUILD_SHARED_LIBS "Build dynamically-linked binaries" ON) -endif() - -# Windows/cmake make things difficult if building dll's. -# By default: -# .dll -> CMAKE_RUNTIME_OUTPUT_DIRECTORY -# .exe -> CMAKE_RUNTIME_OUTPUT_DIRECTORY -# .lib -> CMAKE_LIBRARY_OUTPUT_DIRECTORY -# -# Subsequently, .dll's and .exe's use the same variable and by -# default must be installed to the same place. Which is not -# what we want for catkin. We wish: -# -# .dll -> CATKIN_GLOBAL_BIN_DESTINATION -# .exe -> CATKIN_PACKAGE_BIN_DESTINATION -# .lib -> CATKIN_PACKAGE_LIB_DESTINATION -# -# Since we can't put CMAKE_RUNTIME_OUTPUT_DIRECTORY to -# two values at once, we have this ugly workaround here. -# -# Note - we want to move away from redefining -# add_library style calls, but necessary until a better solution -# is available for windows. Alternatives are to set_target_properties -# on every lib (painful) or to make exe's public (name conflicts -# bound to arise). -# -# Another feature that would be desirable, is to install .pdb's for -# debugging along with the library. Can't do that here as we do not -# know for sure if the library target is intended for installation -# or not. Might a good idea to have a script that searches for all -# pdb's under CATKIN_DEVEL_PREFIX and copies them over at the end -# of the cmake build. -if(BUILD_SHARED_LIBS) - if(WIN32) - function(add_library library) - # Check if its an external, imported library (e.g. boost libs via cmake module definition) - list(FIND ARGN "IMPORTED" FIND_POS) - _add_library(${ARGV0} ${ARGN}) - if(${FIND_POS} EQUAL -1) - set_target_properties(${ARGV0} - PROPERTIES - RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_BIN_DESTINATION} - LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION} - ) - endif() - endfunction() - endif() -endif() diff --git a/cmake/catkin/python.cmake b/cmake/catkin/python.cmake deleted file mode 100644 index c82f31b479..0000000000 --- a/cmake/catkin/python.cmake +++ /dev/null @@ -1,41 +0,0 @@ -# the CMake variable PYTHON_INSTALL_DIR has the same value as the Python function catkin.builder.get_python_install_dir() - -set(PYTHON_VERSION "" CACHE STRING "Specify specific Python version to use ('major.minor' or 'major')") -if(PYTHON_VERSION) - set(PythonInterp_FIND_VERSION "${PYTHON_VERSION}") -endif() - -find_package(PythonInterp REQUIRED) -#message(STATUS "Using PYTHON_EXECUTABLE: ${PYTHON_EXECUTABLE}") - -set(_PYTHON_PATH_VERSION_SUFFIX "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}") - -set(enable_setuptools_deb_layout OFF) -if(EXISTS "/etc/debian_version") - set(enable_setuptools_deb_layout ON) -endif() -option(SETUPTOOLS_DEB_LAYOUT "Enable debian style python package layout" ${enable_setuptools_deb_layout}) - -if(SETUPTOOLS_DEB_LAYOUT) - #message(STATUS "Using Debian Python package layout") - set(PYTHON_PACKAGES_DIR dist-packages) - set(SETUPTOOLS_ARG_EXTRA "--install-layout=deb") - # use major version only when installing 3.x with debian layout - if("${PYTHON_VERSION_MAJOR}" STREQUAL "3") - set(_PYTHON_PATH_VERSION_SUFFIX "${PYTHON_VERSION_MAJOR}") - endif() -else() - #message(STATUS "Using default Python package layout") - set(PYTHON_PACKAGES_DIR site-packages) - # setuptools is fussy about windows paths, make sure the install prefix is in native format - file(TO_NATIVE_PATH "${CMAKE_INSTALL_PREFIX}" SETUPTOOLS_INSTALL_PREFIX) -endif() - -if(NOT WIN32) - set(PYTHON_INSTALL_DIR lib/python${_PYTHON_PATH_VERSION_SUFFIX}/${PYTHON_PACKAGES_DIR} - CACHE INTERNAL "This needs to be in PYTHONPATH when 'setup.py install' is called. And it needs to match. But setuptools won't tell us where it will install things.") -else() - # Windows setuptools installs to lib/site-packages not lib/python2.7/site-packages - set(PYTHON_INSTALL_DIR lib/${PYTHON_PACKAGES_DIR} - CACHE INTERNAL "This needs to be in PYTHONPATH when 'setup.py install' is called. And it needs to match. But setuptools won't tell us where it will install things.") -endif() diff --git a/cmake/catkin/safe_execute_process.cmake b/cmake/catkin/safe_execute_process.cmake deleted file mode 100644 index 3f6ea9aad4..0000000000 --- a/cmake/catkin/safe_execute_process.cmake +++ /dev/null @@ -1,13 +0,0 @@ -macro(safe_execute_process cmd_keyword arg1) - set(_cmd ${arg1}) - foreach(_arg ${ARGN}) - set(_cmd "${_cmd} \"${_arg}\"") - endforeach() - - debug_message(2 "execute_process(${_cmd})") - execute_process(${ARGV} RESULT_VARIABLE _res) - - if(NOT _res EQUAL 0) - message(FATAL_ERROR "execute_process(${_cmd}) returned error code ${_res}") - endif() -endmacro() diff --git a/cmake/catkin/shell.cmake b/cmake/catkin/shell.cmake deleted file mode 100644 index 4b150d419e..0000000000 --- a/cmake/catkin/shell.cmake +++ /dev/null @@ -1,17 +0,0 @@ -function(shell arg1) - set(cmd ${arg1}) - foreach(arg ${ARGN}) - set(cmd "${cmd} ${arg}") - endforeach() - - execute_process(COMMAND ${arg1} ${ARGN} - RESULT_VARIABLE res - OUTPUT_VARIABLE out - ERROR_VARIABLE out) - - if(res EQUAL 0) - debug_message(2 "execute_process(${cmd}) succeeded returning: ${out}") - else() - message(FATAL_ERROR "execute_process(${cmd})\n***FAILED with ERROR:***\n${out}") - endif() -endfunction() diff --git a/cmake/catkin/stamp.cmake b/cmake/catkin/stamp.cmake deleted file mode 100644 index 91de185de5..0000000000 --- a/cmake/catkin/stamp.cmake +++ /dev/null @@ -1,13 +0,0 @@ -# -# :param path: file name -# -# Uses ``configure_file`` to generate a file ``filepath.stamp`` hidden -# somewhere in the build tree. This will cause cmake to rebuild its -# cache when ``filepath`` is modified. -# -function(stamp path) - get_filename_component(filename "${path}" NAME) - configure_file(${path} - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/stamps/${PROJECT_NAME}/${filename}.stamp - COPYONLY) -endfunction() diff --git a/cmake/catkin/string_starts_with.cmake b/cmake/catkin/string_starts_with.cmake deleted file mode 100644 index 85483ec9e7..0000000000 --- a/cmake/catkin/string_starts_with.cmake +++ /dev/null @@ -1,22 +0,0 @@ -# -# Check if a string starts with a prefix. -# -# :param str: the string -# :type str: string -# :param prefix: the prefix -# :type prefix: string -# :param var: the output variable name -# :type var: bool -# -function(string_starts_with str prefix var) - string(LENGTH "${str}" str_length) - string(LENGTH "${prefix}" prefix_length) - set(value FALSE) - if(NOT ${str_length} LESS ${prefix_length}) - string(SUBSTRING "${str}" 0 ${prefix_length} str_prefix) - if("${str_prefix}" STREQUAL "${prefix}") - set(value TRUE) - endif() - endif() - set(${var} ${value} PARENT_SCOPE) -endfunction() diff --git a/cmake/catkin/templates/Doxyfile.in b/cmake/catkin/templates/Doxyfile.in deleted file mode 100644 index f64c9bdff0..0000000000 --- a/cmake/catkin/templates/Doxyfile.in +++ /dev/null @@ -1,1630 +0,0 @@ -# Doxyfile 1.7.1 -# generated from catkin/cmake/template/Doxyfile.in -# This file describes the settings to be used by the documentation system -# doxygen (www.doxygen.org) for a project -# -# All text after a hash (#) is considered a comment and will be ignored -# The format is: -# TAG = value [value, ...] -# For lists items can also be appended using: -# TAG += value [value, ...] -# Values that contain spaces should be placed between quotes (" ") - -#--------------------------------------------------------------------------- -# Project related configuration options -#--------------------------------------------------------------------------- - -# This tag specifies the encoding used for all characters in the config file -# that follow. The default is UTF-8 which is also the encoding used for all -# text before the first occurrence of this tag. Doxygen uses libiconv (or the -# iconv built into libc) for the transcoding. See -# http://www.gnu.org/software/libiconv for the list of possible encodings. - -DOXYFILE_ENCODING = UTF-8 - -# The PROJECT_NAME tag is a single word (or a sequence of words surrounded -# by quotes) that should identify the project. - -PROJECT_NAME = @PROJECT_NAME@ - -# The PROJECT_NUMBER tag can be used to enter a project or revision number. -# This could be handy for archiving the generated documentation or -# if some version control system is used. - -PROJECT_NUMBER = @PACKAGE_VERSION@ - -# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) -# base path where the generated documentation will be put. -# If a relative path is entered, it will be relative to the location -# where doxygen was started. If left blank the current directory will be used. - -OUTPUT_DIRECTORY = api - -# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create -# 4096 sub-directories (in 2 levels) under the output directory of each output -# format and will distribute the generated files over these directories. -# Enabling this option can be useful when feeding doxygen a huge amount of -# source files, where putting all generated files in the same directory would -# otherwise cause performance problems for the file system. - -CREATE_SUBDIRS = NO - -# The OUTPUT_LANGUAGE tag is used to specify the language in which all -# documentation generated by doxygen is written. Doxygen will use this -# information to generate all constant output in the proper language. -# The default language is English, other supported languages are: -# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, -# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, -# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English -# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, -# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrilic, Slovak, -# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. - -OUTPUT_LANGUAGE = English - -# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will -# include brief member descriptions after the members that are listed in -# the file and class documentation (similar to JavaDoc). -# Set to NO to disable this. - -BRIEF_MEMBER_DESC = YES - -# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend -# the brief description of a member or function before the detailed description. -# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the -# brief descriptions will be completely suppressed. - -REPEAT_BRIEF = YES - -# This tag implements a quasi-intelligent brief description abbreviator -# that is used to form the text in various listings. Each string -# in this list, if found as the leading text of the brief description, will be -# stripped from the text and the result after processing the whole list, is -# used as the annotated text. Otherwise, the brief description is used as-is. -# If left blank, the following values are used ("$name" is automatically -# replaced with the name of the entity): "The $name class" "The $name widget" -# "The $name file" "is" "provides" "specifies" "contains" -# "represents" "a" "an" "the" - -ABBREVIATE_BRIEF = - -# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then -# Doxygen will generate a detailed section even if there is only a brief -# description. - -ALWAYS_DETAILED_SEC = NO - -# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all -# inherited members of a class in the documentation of that class as if those -# members were ordinary class members. Constructors, destructors and assignment -# operators of the base classes will not be shown. - -INLINE_INHERITED_MEMB = NO - -# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full -# path before files name in the file list and in the header files. If set -# to NO the shortest path that makes the file name unique will be used. - -FULL_PATH_NAMES = YES - -# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag -# can be used to strip a user-defined part of the path. Stripping is -# only done if one of the specified strings matches the left-hand part of -# the path. The tag can be used to show relative paths in the file list. -# If left blank the directory from which doxygen is run is used as the -# path to strip. - -STRIP_FROM_PATH = - -# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of -# the path mentioned in the documentation of a class, which tells -# the reader which header file to include in order to use a class. -# If left blank only the name of the header file containing the class -# definition is used. Otherwise one should specify the include paths that -# are normally passed to the compiler using the -I flag. - -STRIP_FROM_INC_PATH = - -# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter -# (but less readable) file names. This can be useful is your file systems -# doesn't support long names like on DOS, Mac, or CD-ROM. - -SHORT_NAMES = NO - -# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen -# will interpret the first line (until the first dot) of a JavaDoc-style -# comment as the brief description. If set to NO, the JavaDoc -# comments will behave just like regular Qt-style comments -# (thus requiring an explicit @brief command for a brief description.) - -JAVADOC_AUTOBRIEF = NO - -# If the QT_AUTOBRIEF tag is set to YES then Doxygen will -# interpret the first line (until the first dot) of a Qt-style -# comment as the brief description. If set to NO, the comments -# will behave just like regular Qt-style comments (thus requiring -# an explicit \brief command for a brief description.) - -QT_AUTOBRIEF = NO - -# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen -# treat a multi-line C++ special comment block (i.e. a block of //! or /// -# comments) as a brief description. This used to be the default behaviour. -# The new default is to treat a multi-line C++ comment block as a detailed -# description. Set this tag to YES if you prefer the old behaviour instead. - -MULTILINE_CPP_IS_BRIEF = NO - -# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented -# member inherits the documentation from any documented member that it -# re-implements. - -INHERIT_DOCS = YES - -# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce -# a new page for each member. If set to NO, the documentation of a member will -# be part of the file/class/namespace that contains it. - -SEPARATE_MEMBER_PAGES = NO - -# The TAB_SIZE tag can be used to set the number of spaces in a tab. -# Doxygen uses this value to replace tabs by spaces in code fragments. - -TAB_SIZE = 4 - -# This tag can be used to specify a number of aliases that acts -# as commands in the documentation. An alias has the form "name=value". -# For example adding "sideeffect=\par Side Effects:\n" will allow you to -# put the command \sideeffect (or @sideeffect) in the documentation, which -# will result in a user-defined paragraph with heading "Side Effects:". -# You can put \n's in the value part of an alias to insert newlines. - -ALIASES = - -# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C -# sources only. Doxygen will then generate output that is more tailored for C. -# For instance, some of the names that are used will be different. The list -# of all members will be omitted, etc. - -OPTIMIZE_OUTPUT_FOR_C = NO - -# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java -# sources only. Doxygen will then generate output that is more tailored for -# Java. For instance, namespaces will be presented as packages, qualified -# scopes will look different, etc. - -OPTIMIZE_OUTPUT_JAVA = NO - -# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran -# sources only. Doxygen will then generate output that is more tailored for -# Fortran. - -OPTIMIZE_FOR_FORTRAN = NO - -# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL -# sources. Doxygen will then generate output that is tailored for -# VHDL. - -OPTIMIZE_OUTPUT_VHDL = NO - -# Doxygen selects the parser to use depending on the extension of the files it -# parses. With this tag you can assign which parser to use for a given extension. -# Doxygen has a built-in mapping, but you can override or extend it using this -# tag. The format is ext=language, where ext is a file extension, and language -# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C, -# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make -# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C -# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions -# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. - -EXTENSION_MAPPING = - -# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want -# to include (a tag file for) the STL sources as input, then you should -# set this tag to YES in order to let doxygen match functions declarations and -# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. -# func(std::string) {}). This also make the inheritance and collaboration -# diagrams that involve STL classes more complete and accurate. - -BUILTIN_STL_SUPPORT = NO - -# If you use Microsoft's C++/CLI language, you should set this option to YES to -# enable parsing support. - -CPP_CLI_SUPPORT = NO - -# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. -# Doxygen will parse them like normal C++ but will assume all classes use public -# instead of private inheritance when no explicit protection keyword is present. - -SIP_SUPPORT = NO - -# For Microsoft's IDL there are propget and propput attributes to indicate getter -# and setter methods for a property. Setting this option to YES (the default) -# will make doxygen to replace the get and set methods by a property in the -# documentation. This will only work if the methods are indeed getting or -# setting a simple type. If this is not the case, or you want to show the -# methods anyway, you should set this option to NO. - -IDL_PROPERTY_SUPPORT = YES - -# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC -# tag is set to YES, then doxygen will reuse the documentation of the first -# member in the group (if any) for the other members of the group. By default -# all members of a group must be documented explicitly. - -DISTRIBUTE_GROUP_DOC = NO - -# Set the SUBGROUPING tag to YES (the default) to allow class member groups of -# the same type (for instance a group of public functions) to be put as a -# subgroup of that type (e.g. under the Public Functions section). Set it to -# NO to prevent subgrouping. Alternatively, this can be done per class using -# the \nosubgrouping command. - -SUBGROUPING = YES - -# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum -# is documented as struct, union, or enum with the name of the typedef. So -# typedef struct TypeS {} TypeT, will appear in the documentation as a struct -# with name TypeT. When disabled the typedef will appear as a member of a file, -# namespace, or class. And the struct will be named TypeS. This can typically -# be useful for C code in case the coding convention dictates that all compound -# types are typedef'ed and only the typedef is referenced, never the tag name. - -TYPEDEF_HIDES_STRUCT = NO - -# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to -# determine which symbols to keep in memory and which to flush to disk. -# When the cache is full, less often used symbols will be written to disk. -# For small to medium size projects (<1000 input files) the default value is -# probably good enough. For larger projects a too small cache size can cause -# doxygen to be busy swapping symbols to and from disk most of the time -# causing a significant performance penality. -# If the system has enough physical memory increasing the cache will improve the -# performance by keeping more symbols in memory. Note that the value works on -# a logarithmic scale so increasing the size by one will rougly double the -# memory usage. The cache size is given by this formula: -# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, -# corresponding to a cache size of 2^16 = 65536 symbols - -SYMBOL_CACHE_SIZE = 0 - -#--------------------------------------------------------------------------- -# Build related configuration options -#--------------------------------------------------------------------------- - -# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in -# documentation are documented, even if no documentation was available. -# Private class members and static file members will be hidden unless -# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES - -EXTRACT_ALL = YES - -# If the EXTRACT_PRIVATE tag is set to YES all private members of a class -# will be included in the documentation. - -EXTRACT_PRIVATE = YES - -# If the EXTRACT_STATIC tag is set to YES all static members of a file -# will be included in the documentation. - -EXTRACT_STATIC = YES - -# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) -# defined locally in source files will be included in the documentation. -# If set to NO only classes defined in header files are included. - -EXTRACT_LOCAL_CLASSES = YES - -# This flag is only useful for Objective-C code. When set to YES local -# methods, which are defined in the implementation section but not in -# the interface are included in the documentation. -# If set to NO (the default) only methods in the interface are included. - -EXTRACT_LOCAL_METHODS = NO - -# If this flag is set to YES, the members of anonymous namespaces will be -# extracted and appear in the documentation as a namespace called -# 'anonymous_namespace{file}', where file will be replaced with the base -# name of the file that contains the anonymous namespace. By default -# anonymous namespace are hidden. - -EXTRACT_ANON_NSPACES = NO - -# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all -# undocumented members of documented classes, files or namespaces. -# If set to NO (the default) these members will be included in the -# various overviews, but no documentation section is generated. -# This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_MEMBERS = NO - -# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all -# undocumented classes that are normally visible in the class hierarchy. -# If set to NO (the default) these classes will be included in the various -# overviews. This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_CLASSES = NO - -# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all -# friend (class|struct|union) declarations. -# If set to NO (the default) these declarations will be included in the -# documentation. - -HIDE_FRIEND_COMPOUNDS = NO - -# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any -# documentation blocks found inside the body of a function. -# If set to NO (the default) these blocks will be appended to the -# function's detailed documentation block. - -HIDE_IN_BODY_DOCS = NO - -# The INTERNAL_DOCS tag determines if documentation -# that is typed after a \internal command is included. If the tag is set -# to NO (the default) then the documentation will be excluded. -# Set it to YES to include the internal documentation. - -INTERNAL_DOCS = NO - -# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate -# file names in lower-case letters. If set to YES upper-case letters are also -# allowed. This is useful if you have classes or files whose names only differ -# in case and if your file system supports case sensitive file names. Windows -# and Mac users are advised to set this option to NO. - -CASE_SENSE_NAMES = YES - -# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen -# will show members with their full class and namespace scopes in the -# documentation. If set to YES the scope will be hidden. - -HIDE_SCOPE_NAMES = NO - -# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen -# will put a list of the files that are included by a file in the documentation -# of that file. - -SHOW_INCLUDE_FILES = YES - -# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen -# will list include files with double quotes in the documentation -# rather than with sharp brackets. - -FORCE_LOCAL_INCLUDES = NO - -# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] -# is inserted in the documentation for inline members. - -INLINE_INFO = YES - -# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen -# will sort the (detailed) documentation of file and class members -# alphabetically by member name. If set to NO the members will appear in -# declaration order. - -SORT_MEMBER_DOCS = YES - -# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the -# brief documentation of file, namespace and class members alphabetically -# by member name. If set to NO (the default) the members will appear in -# declaration order. - -SORT_BRIEF_DOCS = NO - -# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen -# will sort the (brief and detailed) documentation of class members so that -# constructors and destructors are listed first. If set to NO (the default) -# the constructors will appear in the respective orders defined by -# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. -# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO -# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. - -SORT_MEMBERS_CTORS_1ST = NO - -# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the -# hierarchy of group names into alphabetical order. If set to NO (the default) -# the group names will appear in their defined order. - -SORT_GROUP_NAMES = NO - -# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be -# sorted by fully-qualified names, including namespaces. If set to -# NO (the default), the class list will be sorted only by class name, -# not including the namespace part. -# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. -# Note: This option applies only to the class list, not to the -# alphabetical list. - -SORT_BY_SCOPE_NAME = NO - -# The GENERATE_TODOLIST tag can be used to enable (YES) or -# disable (NO) the todo list. This list is created by putting \todo -# commands in the documentation. - -GENERATE_TODOLIST = YES - -# The GENERATE_TESTLIST tag can be used to enable (YES) or -# disable (NO) the test list. This list is created by putting \test -# commands in the documentation. - -GENERATE_TESTLIST = YES - -# The GENERATE_BUGLIST tag can be used to enable (YES) or -# disable (NO) the bug list. This list is created by putting \bug -# commands in the documentation. - -GENERATE_BUGLIST = YES - -# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or -# disable (NO) the deprecated list. This list is created by putting -# \deprecated commands in the documentation. - -GENERATE_DEPRECATEDLIST = YES - -# The ENABLED_SECTIONS tag can be used to enable conditional -# documentation sections, marked by \if sectionname ... \endif. - -ENABLED_SECTIONS = - -# The MAX_INITIALIZER_LINES tag determines the maximum number of lines -# the initial value of a variable or define consists of for it to appear in -# the documentation. If the initializer consists of more lines than specified -# here it will be hidden. Use a value of 0 to hide initializers completely. -# The appearance of the initializer of individual variables and defines in the -# documentation can be controlled using \showinitializer or \hideinitializer -# command in the documentation regardless of this setting. - -MAX_INITIALIZER_LINES = 30 - -# Set the SHOW_USED_FILES tag to NO to disable the list of files generated -# at the bottom of the documentation of classes and structs. If set to YES the -# list will mention the files that were used to generate the documentation. - -SHOW_USED_FILES = YES - -# If the sources in your project are distributed over multiple directories -# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy -# in the documentation. The default is NO. - -SHOW_DIRECTORIES = NO - -# Set the SHOW_FILES tag to NO to disable the generation of the Files page. -# This will remove the Files entry from the Quick Index and from the -# Folder Tree View (if specified). The default is YES. - -SHOW_FILES = YES - -# Set the SHOW_NAMESPACES tag to NO to disable the generation of the -# Namespaces page. -# This will remove the Namespaces entry from the Quick Index -# and from the Folder Tree View (if specified). The default is YES. - -SHOW_NAMESPACES = YES - -# The FILE_VERSION_FILTER tag can be used to specify a program or script that -# doxygen should invoke to get the current version for each file (typically from -# the version control system). Doxygen will invoke the program by executing (via -# popen()) the command , where is the value of -# the FILE_VERSION_FILTER tag, and is the name of an input file -# provided by doxygen. Whatever the program writes to standard output -# is used as the file version. See the manual for examples. - -FILE_VERSION_FILTER = - -# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed -# by doxygen. The layout file controls the global structure of the generated -# output files in an output format independent way. The create the layout file -# that represents doxygen's defaults, run doxygen with the -l option. -# You can optionally specify a file name after the option, if omitted -# DoxygenLayout.xml will be used as the name of the layout file. - -LAYOUT_FILE = - -#--------------------------------------------------------------------------- -# configuration options related to warning and progress messages -#--------------------------------------------------------------------------- - -# The QUIET tag can be used to turn on/off the messages that are generated -# by doxygen. Possible values are YES and NO. If left blank NO is used. - -QUIET = NO - -# The WARNINGS tag can be used to turn on/off the warning messages that are -# generated by doxygen. Possible values are YES and NO. If left blank -# NO is used. - -WARNINGS = YES - -# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings -# for undocumented members. If EXTRACT_ALL is set to YES then this flag will -# automatically be disabled. - -WARN_IF_UNDOCUMENTED = YES - -# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for -# potential errors in the documentation, such as not documenting some -# parameters in a documented function, or documenting parameters that -# don't exist or using markup commands wrongly. - -WARN_IF_DOC_ERROR = YES - -# This WARN_NO_PARAMDOC option can be abled to get warnings for -# functions that are documented, but have no documentation for their parameters -# or return value. If set to NO (the default) doxygen will only warn about -# wrong or incomplete parameter documentation, but not about the absence of -# documentation. - -WARN_NO_PARAMDOC = NO - -# The WARN_FORMAT tag determines the format of the warning messages that -# doxygen can produce. The string should contain the $file, $line, and $text -# tags, which will be replaced by the file and line number from which the -# warning originated and the warning text. Optionally the format may contain -# $version, which will be replaced by the version of the file (if it could -# be obtained via FILE_VERSION_FILTER) - -WARN_FORMAT = "$file:$line: $text" - -# The WARN_LOGFILE tag can be used to specify a file to which warning -# and error messages should be written. If left blank the output is written -# to stderr. - -WARN_LOGFILE = - -#--------------------------------------------------------------------------- -# configuration options related to the input files -#--------------------------------------------------------------------------- - -# The INPUT tag can be used to specify the files and/or directories that contain -# documented source files. You may enter file names like "myfile.cpp" or -# directories like "/usr/src/myproject". Separate the files or directories -# with spaces. - -INPUT = @doc_sources@ - -# This tag can be used to specify the character encoding of the source files -# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is -# also the default input encoding. Doxygen uses libiconv (or the iconv built -# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for -# the list of possible encodings. - -INPUT_ENCODING = UTF-8 - -# If the value of the INPUT tag contains directories, you can use the -# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp -# and *.h) to filter out the source-files in the directories. If left -# blank the following patterns are tested: -# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx -# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90 - -FILE_PATTERNS = *.hpp - -# The RECURSIVE tag can be used to turn specify whether or not subdirectories -# should be searched for input files as well. Possible values are YES and NO. -# If left blank NO is used. - -RECURSIVE = YES - -# The EXCLUDE tag can be used to specify files and/or directories that should -# excluded from the INPUT source files. This way you can easily exclude a -# subdirectory from a directory tree whose root is specified with the INPUT tag. - -EXCLUDE = - -# The EXCLUDE_SYMLINKS tag can be used select whether or not files or -# directories that are symbolic links (a Unix filesystem feature) are excluded -# from the input. - -EXCLUDE_SYMLINKS = NO - -# If the value of the INPUT tag contains directories, you can use the -# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude -# certain files from those directories. Note that the wildcards are matched -# against the file with absolute path, so to exclude all test directories -# for example use the pattern */test/* - -EXCLUDE_PATTERNS = - -# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names -# (namespaces, classes, functions, etc.) that should be excluded from the -# output. The symbol name can be a fully qualified name, a word, or if the -# wildcard * is used, a substring. Examples: ANamespace, AClass, -# AClass::ANamespace, ANamespace::*Test - -EXCLUDE_SYMBOLS = - -# The EXAMPLE_PATH tag can be used to specify one or more files or -# directories that contain example code fragments that are included (see -# the \include command). - -EXAMPLE_PATH = - -# If the value of the EXAMPLE_PATH tag contains directories, you can use the -# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp -# and *.h) to filter out the source-files in the directories. If left -# blank all files are included. - -EXAMPLE_PATTERNS = - -# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be -# searched for input files to be used with the \include or \dontinclude -# commands irrespective of the value of the RECURSIVE tag. -# Possible values are YES and NO. If left blank NO is used. - -EXAMPLE_RECURSIVE = NO - -# The IMAGE_PATH tag can be used to specify one or more files or -# directories that contain image that are included in the documentation (see -# the \image command). - -IMAGE_PATH = - -# The INPUT_FILTER tag can be used to specify a program that doxygen should -# invoke to filter for each input file. Doxygen will invoke the filter program -# by executing (via popen()) the command , where -# is the value of the INPUT_FILTER tag, and is the name of an -# input file. Doxygen will then use the output that the filter program writes -# to standard output. -# If FILTER_PATTERNS is specified, this tag will be -# ignored. - -# INPUT_FILTER = *.hpp - -# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern -# basis. -# Doxygen will compare the file name with each pattern and apply the -# filter if there is a match. -# The filters are a list of the form: -# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further -# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER -# is applied to all files. - -# FILTER_PATTERNS = *.py=/usr/bin/doxypy - -# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using -# INPUT_FILTER) will be used to filter the input files when producing source -# files to browse (i.e. when SOURCE_BROWSER is set to YES). - -FILTER_SOURCE_FILES = YES - -#--------------------------------------------------------------------------- -# configuration options related to source browsing -#--------------------------------------------------------------------------- - -# If the SOURCE_BROWSER tag is set to YES then a list of source files will -# be generated. Documented entities will be cross-referenced with these sources. -# Note: To get rid of all source code in the generated output, make sure also -# VERBATIM_HEADERS is set to NO. - -SOURCE_BROWSER = NO - -# Setting the INLINE_SOURCES tag to YES will include the body -# of functions and classes directly in the documentation. - -INLINE_SOURCES = NO - -# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct -# doxygen to hide any special comment blocks from generated source code -# fragments. Normal C and C++ comments will always remain visible. - -STRIP_CODE_COMMENTS = YES - -# If the REFERENCED_BY_RELATION tag is set to YES -# then for each documented function all documented -# functions referencing it will be listed. - -REFERENCED_BY_RELATION = NO - -# If the REFERENCES_RELATION tag is set to YES -# then for each documented function all documented entities -# called/used by that function will be listed. - -REFERENCES_RELATION = NO - -# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) -# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from -# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will -# link to the source code. -# Otherwise they will link to the documentation. - -REFERENCES_LINK_SOURCE = YES - -# If the USE_HTAGS tag is set to YES then the references to source code -# will point to the HTML generated by the htags(1) tool instead of doxygen -# built-in source browser. The htags tool is part of GNU's global source -# tagging system (see http://www.gnu.org/software/global/global.html). You -# will need version 4.8.6 or higher. - -USE_HTAGS = NO - -# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen -# will generate a verbatim copy of the header file for each class for -# which an include is specified. Set to NO to disable this. - -VERBATIM_HEADERS = YES - -#--------------------------------------------------------------------------- -# configuration options related to the alphabetical class index -#--------------------------------------------------------------------------- - -# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index -# of all compounds will be generated. Enable this if the project -# contains a lot of classes, structs, unions or interfaces. - -ALPHABETICAL_INDEX = YES - -# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then -# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns -# in which this list will be split (can be a number in the range [1..20]) - -COLS_IN_ALPHA_INDEX = 5 - -# In case all classes in a project start with a common prefix, all -# classes will be put under the same header in the alphabetical index. -# The IGNORE_PREFIX tag can be used to specify one or more prefixes that -# should be ignored while generating the index headers. - -IGNORE_PREFIX = - -#--------------------------------------------------------------------------- -# configuration options related to the HTML output -#--------------------------------------------------------------------------- - -# If the GENERATE_HTML tag is set to YES (the default) Doxygen will -# generate HTML output. - -GENERATE_HTML = YES - -# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `html' will be used as the default path. - -HTML_OUTPUT = html - -# The HTML_FILE_EXTENSION tag can be used to specify the file extension for -# each generated HTML page (for example: .htm,.php,.asp). If it is left blank -# doxygen will generate files with .html extension. - -HTML_FILE_EXTENSION = .html - -# The HTML_HEADER tag can be used to specify a personal HTML header for -# each generated HTML page. If it is left blank doxygen will generate a -# standard header. - -HTML_HEADER = - -# The HTML_FOOTER tag can be used to specify a personal HTML footer for -# each generated HTML page. If it is left blank doxygen will generate a -# standard footer. - -HTML_FOOTER = - -# The HTML_STYLESHEET tag can be used to specify a user-defined cascading -# style sheet that is used by each HTML page. It can be used to -# fine-tune the look of the HTML output. If the tag is left blank doxygen -# will generate a default style sheet. Note that doxygen will try to copy -# the style sheet file to the HTML output directory, so don't put your own -# stylesheet in the HTML output directory as well, or it will be erased! - -HTML_STYLESHEET = - -# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. -# Doxygen will adjust the colors in the stylesheet and background images -# according to this color. Hue is specified as an angle on a colorwheel, -# see http://en.wikipedia.org/wiki/Hue for more information. -# For instance the value 0 represents red, 60 is yellow, 120 is green, -# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. -# The allowed range is 0 to 359. - -HTML_COLORSTYLE_HUE = 220 - -# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of -# the colors in the HTML output. For a value of 0 the output will use -# grayscales only. A value of 255 will produce the most vivid colors. - -HTML_COLORSTYLE_SAT = 100 - -# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to -# the luminance component of the colors in the HTML output. Values below -# 100 gradually make the output lighter, whereas values above 100 make -# the output darker. The value divided by 100 is the actual gamma applied, -# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, -# and 100 does not change the gamma. - -HTML_COLORSTYLE_GAMMA = 80 - -# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML -# page will contain the date and time when the page was generated. Setting -# this to NO can help when comparing the output of multiple runs. - -HTML_TIMESTAMP = YES - -# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, -# files or namespaces will be aligned in HTML using tables. If set to -# NO a bullet list will be used. - -HTML_ALIGN_MEMBERS = YES - -# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML -# documentation will contain sections that can be hidden and shown after the -# page has loaded. For this to work a browser that supports -# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox -# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). - -HTML_DYNAMIC_SECTIONS = NO - -# If the GENERATE_DOCSET tag is set to YES, additional index files -# will be generated that can be used as input for Apple's Xcode 3 -# integrated development environment, introduced with OSX 10.5 (Leopard). -# To create a documentation set, doxygen will generate a Makefile in the -# HTML output directory. Running make will produce the docset in that -# directory and running "make install" will install the docset in -# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find -# it at startup. -# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html -# for more information. - -GENERATE_DOCSET = NO - -# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the -# feed. A documentation feed provides an umbrella under which multiple -# documentation sets from a single provider (such as a company or product suite) -# can be grouped. - -DOCSET_FEEDNAME = "Doxygen generated docs" - -# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that -# should uniquely identify the documentation set bundle. This should be a -# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen -# will append .docset to the name. - -DOCSET_BUNDLE_ID = org.doxygen.Project - -# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify -# the documentation publisher. This should be a reverse domain-name style -# string, e.g. com.mycompany.MyDocSet.documentation. - -DOCSET_PUBLISHER_ID = org.doxygen.Publisher - -# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. - -DOCSET_PUBLISHER_NAME = Publisher - -# If the GENERATE_HTMLHELP tag is set to YES, additional index files -# will be generated that can be used as input for tools like the -# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) -# of the generated HTML documentation. - -GENERATE_HTMLHELP = NO - -# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can -# be used to specify the file name of the resulting .chm file. You -# can add a path in front of the file if the result should not be -# written to the html output directory. - -CHM_FILE = - -# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can -# be used to specify the location (absolute path including file name) of -# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run -# the HTML help compiler on the generated index.hhp. - -HHC_LOCATION = - -# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag -# controls if a separate .chi index file is generated (YES) or that -# it should be included in the master .chm file (NO). - -GENERATE_CHI = NO - -# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING -# is used to encode HtmlHelp index (hhk), content (hhc) and project file -# content. - -CHM_INDEX_ENCODING = - -# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag -# controls whether a binary table of contents is generated (YES) or a -# normal table of contents (NO) in the .chm file. - -BINARY_TOC = NO - -# The TOC_EXPAND flag can be set to YES to add extra items for group members -# to the contents of the HTML help documentation and to the tree view. - -TOC_EXPAND = NO - -# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and -# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated -# that can be used as input for Qt's qhelpgenerator to generate a -# Qt Compressed Help (.qch) of the generated HTML documentation. - -GENERATE_QHP = NO - -# If the QHG_LOCATION tag is specified, the QCH_FILE tag can -# be used to specify the file name of the resulting .qch file. -# The path specified is relative to the HTML output folder. - -QCH_FILE = - -# The QHP_NAMESPACE tag specifies the namespace to use when generating -# Qt Help Project output. For more information please see -# http://doc.trolltech.com/qthelpproject.html#namespace - -QHP_NAMESPACE = org.doxygen.Project - -# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating -# Qt Help Project output. For more information please see -# http://doc.trolltech.com/qthelpproject.html#virtual-folders - -QHP_VIRTUAL_FOLDER = doc - -# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to -# add. For more information please see -# http://doc.trolltech.com/qthelpproject.html#custom-filters - -QHP_CUST_FILTER_NAME = - -# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the -# custom filter to add. For more information please see -# -# Qt Help Project / Custom Filters. - -QHP_CUST_FILTER_ATTRS = - -# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this -# project's -# filter section matches. -# -# Qt Help Project / Filter Attributes. - -QHP_SECT_FILTER_ATTRS = - -# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can -# be used to specify the location of Qt's qhelpgenerator. -# If non-empty doxygen will try to run qhelpgenerator on the generated -# .qhp file. - -QHG_LOCATION = - -# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files -# will be generated, which together with the HTML files, form an Eclipse help -# plugin. To install this plugin and make it available under the help contents -# menu in Eclipse, the contents of the directory containing the HTML and XML -# files needs to be copied into the plugins directory of eclipse. The name of -# the directory within the plugins directory should be the same as -# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before -# the help appears. - -GENERATE_ECLIPSEHELP = NO - -# A unique identifier for the eclipse help plugin. When installing the plugin -# the directory name containing the HTML and XML files should also have -# this name. - -ECLIPSE_DOC_ID = org.doxygen.Project - -# The DISABLE_INDEX tag can be used to turn on/off the condensed index at -# top of each HTML page. The value NO (the default) enables the index and -# the value YES disables it. - -DISABLE_INDEX = NO - -# This tag can be used to set the number of enum values (range [1..20]) -# that doxygen will group on one line in the generated HTML documentation. - -ENUM_VALUES_PER_LINE = 4 - -# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index -# structure should be generated to display hierarchical information. -# If the tag value is set to YES, a side panel will be generated -# containing a tree-like index structure (just like the one that -# is generated for HTML Help). For this to work a browser that supports -# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). -# Windows users are probably better off using the HTML help feature. - -GENERATE_TREEVIEW = NO - -# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, -# and Class Hierarchy pages using a tree view instead of an ordered list. - -USE_INLINE_TREES = NO - -# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be -# used to set the initial width (in pixels) of the frame in which the tree -# is shown. - -TREEVIEW_WIDTH = 250 - -# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open -# links to external symbols imported via tag files in a separate window. - -EXT_LINKS_IN_WINDOW = NO - -# Use this tag to change the font size of Latex formulas included -# as images in the HTML documentation. The default is 10. Note that -# when you change the font size after a successful doxygen run you need -# to manually remove any form_*.png images from the HTML output directory -# to force them to be regenerated. - -FORMULA_FONTSIZE = 10 - -# Use the FORMULA_TRANPARENT tag to determine whether or not the images -# generated for formulas are transparent PNGs. Transparent PNGs are -# not supported properly for IE 6.0, but are supported on all modern browsers. -# Note that when changing this option you need to delete any form_*.png files -# in the HTML output before the changes have effect. - -FORMULA_TRANSPARENT = YES - -# When the SEARCHENGINE tag is enabled doxygen will generate a search box -# for the HTML output. The underlying search engine uses javascript -# and DHTML and should work on any modern browser. Note that when using -# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets -# (GENERATE_DOCSET) there is already a search function so this one should -# typically be disabled. For large projects the javascript based search engine -# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. - -SEARCHENGINE = YES - -# When the SERVER_BASED_SEARCH tag is enabled the search engine will be -# implemented using a PHP enabled web server instead of at the web client -# using Javascript. Doxygen will generate the search PHP script and index -# file to put on the web server. The advantage of the server -# based approach is that it scales better to large projects and allows -# full text search. The disadvances is that it is more difficult to setup -# and does not have live searching capabilities. - -SERVER_BASED_SEARCH = NO - -#--------------------------------------------------------------------------- -# configuration options related to the LaTeX output -#--------------------------------------------------------------------------- - -# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will -# generate Latex output. - -GENERATE_LATEX = YES - -# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `latex' will be used as the default path. - -LATEX_OUTPUT = latex - -# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be -# invoked. If left blank `latex' will be used as the default command name. -# Note that when enabling USE_PDFLATEX this option is only used for -# generating bitmaps for formulas in the HTML output, but not in the -# Makefile that is written to the output directory. - -LATEX_CMD_NAME = latex - -# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to -# generate index for LaTeX. If left blank `makeindex' will be used as the -# default command name. - -MAKEINDEX_CMD_NAME = makeindex - -# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact -# LaTeX documents. This may be useful for small projects and may help to -# save some trees in general. - -COMPACT_LATEX = NO - -# The PAPER_TYPE tag can be used to set the paper type that is used -# by the printer. Possible values are: a4, a4wide, letter, legal and -# executive. If left blank a4wide will be used. - -PAPER_TYPE = a4wide - -# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX -# packages that should be included in the LaTeX output. - -EXTRA_PACKAGES = - -# The LATEX_HEADER tag can be used to specify a personal LaTeX header for -# the generated latex document. The header should contain everything until -# the first chapter. If it is left blank doxygen will generate a -# standard header. Notice: only use this tag if you know what you are doing! - -LATEX_HEADER = - -# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated -# is prepared for conversion to pdf (using ps2pdf). The pdf file will -# contain links (just like the HTML output) instead of page references -# This makes the output suitable for online browsing using a pdf viewer. - -PDF_HYPERLINKS = YES - -# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of -# plain latex in the generated Makefile. Set this option to YES to get a -# higher quality PDF documentation. - -USE_PDFLATEX = YES - -# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. -# command to the generated LaTeX files. This will instruct LaTeX to keep -# running if errors occur, instead of asking the user for help. -# This option is also used when generating formulas in HTML. - -LATEX_BATCHMODE = NO - -# If LATEX_HIDE_INDICES is set to YES then doxygen will not -# include the index chapters (such as File Index, Compound Index, etc.) -# in the output. - -LATEX_HIDE_INDICES = NO - -# If LATEX_SOURCE_CODE is set to YES then doxygen will include -# source code with syntax highlighting in the LaTeX output. -# Note that which sources are shown also depends on other settings -# such as SOURCE_BROWSER. - -LATEX_SOURCE_CODE = NO - -#--------------------------------------------------------------------------- -# configuration options related to the RTF output -#--------------------------------------------------------------------------- - -# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output -# The RTF output is optimized for Word 97 and may not look very pretty with -# other RTF readers or editors. - -GENERATE_RTF = NO - -# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `rtf' will be used as the default path. - -RTF_OUTPUT = rtf - -# If the COMPACT_RTF tag is set to YES Doxygen generates more compact -# RTF documents. This may be useful for small projects and may help to -# save some trees in general. - -COMPACT_RTF = NO - -# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated -# will contain hyperlink fields. The RTF file will -# contain links (just like the HTML output) instead of page references. -# This makes the output suitable for online browsing using WORD or other -# programs which support those fields. -# Note: wordpad (write) and others do not support links. - -RTF_HYPERLINKS = NO - -# Load stylesheet definitions from file. Syntax is similar to doxygen's -# config file, i.e. a series of assignments. You only have to provide -# replacements, missing definitions are set to their default value. - -RTF_STYLESHEET_FILE = - -# Set optional variables used in the generation of an rtf document. -# Syntax is similar to doxygen's config file. - -RTF_EXTENSIONS_FILE = - -#--------------------------------------------------------------------------- -# configuration options related to the man page output -#--------------------------------------------------------------------------- - -# If the GENERATE_MAN tag is set to YES (the default) Doxygen will -# generate man pages - -GENERATE_MAN = NO - -# The MAN_OUTPUT tag is used to specify where the man pages will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `man' will be used as the default path. - -MAN_OUTPUT = man - -# The MAN_EXTENSION tag determines the extension that is added to -# the generated man pages (default is the subroutine's section .3) - -MAN_EXTENSION = .3 - -# If the MAN_LINKS tag is set to YES and Doxygen generates man output, -# then it will generate one additional man file for each entity -# documented in the real man page(s). These additional files -# only source the real man page, but without them the man command -# would be unable to find the correct page. The default is NO. - -MAN_LINKS = NO - -#--------------------------------------------------------------------------- -# configuration options related to the XML output -#--------------------------------------------------------------------------- - -# If the GENERATE_XML tag is set to YES Doxygen will -# generate an XML file that captures the structure of -# the code including all documentation. - -GENERATE_XML = YES - -# The XML_OUTPUT tag is used to specify where the XML pages will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `xml' will be used as the default path. - -XML_OUTPUT = xml - -# The XML_SCHEMA tag can be used to specify an XML schema, -# which can be used by a validating XML parser to check the -# syntax of the XML files. - -XML_SCHEMA = - -# The XML_DTD tag can be used to specify an XML DTD, -# which can be used by a validating XML parser to check the -# syntax of the XML files. - -XML_DTD = - -# If the XML_PROGRAMLISTING tag is set to YES Doxygen will -# dump the program listings (including syntax highlighting -# and cross-referencing information) to the XML output. Note that -# enabling this will significantly increase the size of the XML output. - -XML_PROGRAMLISTING = YES - -#--------------------------------------------------------------------------- -# configuration options for the AutoGen Definitions output -#--------------------------------------------------------------------------- - -# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will -# generate an AutoGen Definitions (see autogen.sf.net) file -# that captures the structure of the code including all -# documentation. Note that this feature is still experimental -# and incomplete at the moment. - -GENERATE_AUTOGEN_DEF = NO - -#--------------------------------------------------------------------------- -# configuration options related to the Perl module output -#--------------------------------------------------------------------------- - -# If the GENERATE_PERLMOD tag is set to YES Doxygen will -# generate a Perl module file that captures the structure of -# the code including all documentation. Note that this -# feature is still experimental and incomplete at the -# moment. - -GENERATE_PERLMOD = NO - -# If the PERLMOD_LATEX tag is set to YES Doxygen will generate -# the necessary Makefile rules, Perl scripts and LaTeX code to be able -# to generate PDF and DVI output from the Perl module output. - -PERLMOD_LATEX = NO - -# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be -# nicely formatted so it can be parsed by a human reader. -# This is useful -# if you want to understand what is going on. -# On the other hand, if this -# tag is set to NO the size of the Perl module output will be much smaller -# and Perl will parse it just the same. - -PERLMOD_PRETTY = YES - -# The names of the make variables in the generated doxyrules.make file -# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. -# This is useful so different doxyrules.make files included by the same -# Makefile don't overwrite each other's variables. - -PERLMOD_MAKEVAR_PREFIX = - -#--------------------------------------------------------------------------- -# Configuration options related to the preprocessor -#--------------------------------------------------------------------------- - -# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will -# evaluate all C-preprocessor directives found in the sources and include -# files. - -ENABLE_PREPROCESSING = YES - -# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro -# names in the source code. If set to NO (the default) only conditional -# compilation will be performed. Macro expansion can be done in a controlled -# way by setting EXPAND_ONLY_PREDEF to YES. - -MACRO_EXPANSION = NO - -# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES -# then the macro expansion is limited to the macros specified with the -# PREDEFINED and EXPAND_AS_DEFINED tags. - -EXPAND_ONLY_PREDEF = NO - -# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files -# in the INCLUDE_PATH (see below) will be search if a #include is found. - -SEARCH_INCLUDES = YES - -# The INCLUDE_PATH tag can be used to specify one or more directories that -# contain include files that are not input files but should be processed by -# the preprocessor. - -# INCLUDE_PATH = ../include - -# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard -# patterns (like *.h and *.hpp) to filter out the header-files in the -# directories. If left blank, the patterns specified with FILE_PATTERNS will -# be used. - -# INCLUDE_FILE_PATTERNS = *.hpp - -# The PREDEFINED tag can be used to specify one or more macro names that -# are defined before the preprocessor is started (similar to the -D option of -# gcc). The argument of the tag is a list of macros of the form: name -# or name=definition (no spaces). If the definition and the = are -# omitted =1 is assumed. To prevent a macro definition from being -# undefined via #undef or recursively expanded use the := operator -# instead of the = operator. - -PREDEFINED = - -# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then -# this tag can be used to specify a list of macro names that should be expanded. -# The macro definition that is found in the sources will be used. -# Use the PREDEFINED tag if you want to use a different macro definition. - -EXPAND_AS_DEFINED = - -# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then -# doxygen's preprocessor will remove all function-like macros that are alone -# on a line, have an all uppercase name, and do not end with a semicolon. Such -# function macros are typically used for boiler-plate code, and will confuse -# the parser if not removed. - -SKIP_FUNCTION_MACROS = YES - -#--------------------------------------------------------------------------- -# Configuration::additions related to external references -#--------------------------------------------------------------------------- - -# The TAGFILES option can be used to specify one or more tagfiles. -# Optionally an initial location of the external documentation -# can be added for each tagfile. The format of a tag file without -# this location is as follows: -# -# TAGFILES = file1 file2 ... -# Adding location for the tag files is done as follows: -# -# TAGFILES = file1=loc1 "file2 = loc2" ... -# where "loc1" and "loc2" can be relative or absolute paths or -# URLs. If a location is present for each tag, the installdox tool -# does not have to be run to correct the links. -# Note that each tag file must have a unique name -# (where the name does NOT include the path) -# If a tag file is not located in the directory in which doxygen -# is run, you must also specify the path to the tagfile here. - -TAGFILES = - -# When a file name is specified after GENERATE_TAGFILE, doxygen will create -# a tag file that is based on the input files it reads. - -GENERATE_TAGFILE = - -# If the ALLEXTERNALS tag is set to YES all external classes will be listed -# in the class index. If set to NO only the inherited external classes -# will be listed. - -ALLEXTERNALS = NO - -# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed -# in the modules index. If set to NO, only the current project's groups will -# be listed. - -EXTERNAL_GROUPS = YES - -# The PERL_PATH should be the absolute path and name of the perl script -# interpreter (i.e. the result of `which perl'). - -PERL_PATH = /usr/bin/perl - -#--------------------------------------------------------------------------- -# Configuration options related to the dot tool -#--------------------------------------------------------------------------- - -# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will -# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base -# or super classes. Setting the tag to NO turns the diagrams off. Note that -# this option is superseded by the HAVE_DOT option below. This is only a -# fallback. It is recommended to install and use dot, since it yields more -# powerful graphs. - -CLASS_DIAGRAMS = NO - -# You can define message sequence charts within doxygen comments using the \msc -# command. Doxygen will then run the mscgen tool (see -# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the -# documentation. The MSCGEN_PATH tag allows you to specify the directory where -# the mscgen tool resides. If left empty the tool is assumed to be found in the -# default search path. - -MSCGEN_PATH = - -# If set to YES, the inheritance and collaboration graphs will hide -# inheritance and usage relations if the target is undocumented -# or is not a class. - -HIDE_UNDOC_RELATIONS = YES - -# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is -# available from the path. This tool is part of Graphviz, a graph visualization -# toolkit from AT&T and Lucent Bell Labs. The other options in this section -# have no effect if this option is set to NO (the default) - -HAVE_DOT = YES - -# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is -# allowed to run in parallel. When set to 0 (the default) doxygen will -# base this on the number of processors available in the system. You can set it -# explicitly to a value larger than 0 to get control over the balance -# between CPU load and processing speed. - -DOT_NUM_THREADS = 0 - -# By default doxygen will write a font called FreeSans.ttf to the output -# directory and reference it in all dot files that doxygen generates. This -# font does not include all possible unicode characters however, so when you need -# these (or just want a differently looking font) you can specify the font name -# using DOT_FONTNAME. You need need to make sure dot is able to find the font, -# which can be done by putting it in a standard location or by setting the -# DOTFONTPATH environment variable or by setting DOT_FONTPATH to the directory -# containing the font. - -DOT_FONTNAME = FreeSans.ttf - -# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. -# The default size is 10pt. - -DOT_FONTSIZE = 10 - -# By default doxygen will tell dot to use the output directory to look for the -# FreeSans.ttf font (which doxygen will put there itself). If you specify a -# different font using DOT_FONTNAME you can set the path where dot -# can find it using this tag. - -DOT_FONTPATH = - -# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for each documented class showing the direct and -# indirect inheritance relations. Setting this tag to YES will force the -# the CLASS_DIAGRAMS tag to NO. - -CLASS_GRAPH = YES - -# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for each documented class showing the direct and -# indirect implementation dependencies (inheritance, containment, and -# class references variables) of the class with other documented classes. - -COLLABORATION_GRAPH = YES - -# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for groups, showing the direct groups dependencies - -GROUP_GRAPHS = YES - -# If the UML_LOOK tag is set to YES doxygen will generate inheritance and -# collaboration diagrams in a style similar to the OMG's Unified Modeling -# Language. - -UML_LOOK = NO - -# If set to YES, the inheritance and collaboration graphs will show the -# relations between templates and their instances. - -TEMPLATE_RELATIONS = NO - -# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT -# tags are set to YES then doxygen will generate a graph for each documented -# file showing the direct and indirect include dependencies of the file with -# other documented files. - -INCLUDE_GRAPH = YES - -# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and -# HAVE_DOT tags are set to YES then doxygen will generate a graph for each -# documented header file showing the documented files that directly or -# indirectly include this file. - -INCLUDED_BY_GRAPH = YES - -# If the CALL_GRAPH and HAVE_DOT options are set to YES then -# doxygen will generate a call dependency graph for every global function -# or class method. Note that enabling this option will significantly increase -# the time of a run. So in most cases it will be better to enable call graphs -# for selected functions only using the \callgraph command. - -CALL_GRAPH = NO - -# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then -# doxygen will generate a caller dependency graph for every global function -# or class method. Note that enabling this option will significantly increase -# the time of a run. So in most cases it will be better to enable caller -# graphs for selected functions only using the \callergraph command. - -CALLER_GRAPH = NO - -# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen -# will graphical hierarchy of all classes instead of a textual one. - -GRAPHICAL_HIERARCHY = YES - -# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES -# then doxygen will show the dependencies a directory has on other directories -# in a graphical way. The dependency relations are determined by the #include -# relations between the files in the directories. - -DIRECTORY_GRAPH = YES - -# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images -# generated by dot. Possible values are png, jpg, or gif -# If left blank png will be used. - -DOT_IMAGE_FORMAT = png - -# The tag DOT_PATH can be used to specify the path where the dot tool can be -# found. If left blank, it is assumed the dot tool can be found in the path. - -DOT_PATH = - -# The DOTFILE_DIRS tag can be used to specify one or more directories that -# contain dot files that are included in the documentation (see the -# \dotfile command). - -DOTFILE_DIRS = - -# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of -# nodes that will be shown in the graph. If the number of nodes in a graph -# becomes larger than this value, doxygen will truncate the graph, which is -# visualized by representing a node as a red box. Note that doxygen if the -# number of direct children of the root node in a graph is already larger than -# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note -# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. - -DOT_GRAPH_MAX_NODES = 50 - -# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the -# graphs generated by dot. A depth value of 3 means that only nodes reachable -# from the root by following a path via at most 3 edges will be shown. Nodes -# that lay further from the root node will be omitted. Note that setting this -# option to 1 or 2 may greatly reduce the computation time needed for large -# code bases. Also note that the size of a graph can be further restricted by -# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. - -MAX_DOT_GRAPH_DEPTH = 0 - -# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent -# background. This is disabled by default, because dot on Windows does not -# seem to support this out of the box. Warning: Depending on the platform used, -# enabling this option may lead to badly anti-aliased labels on the edges of -# a graph (i.e. they become hard to read). - -DOT_TRANSPARENT = NO - -# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output -# files in one run (i.e. multiple -o and -T options on the command line). This -# makes dot run faster, but since only newer versions of dot (>1.8.10) -# support this, this feature is disabled by default. - -DOT_MULTI_TARGETS = YES - -# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will -# generate a legend page explaining the meaning of the various boxes and -# arrows in the dot generated graphs. - -GENERATE_LEGEND = YES - -# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will -# remove the intermediate dot files that are used to generate -# the various graphs. - -DOT_CLEANUP = YES diff --git a/cmake/catkin/templates/__init__.py.in b/cmake/catkin/templates/__init__.py.in deleted file mode 100644 index 68f2f652ce..0000000000 --- a/cmake/catkin/templates/__init__.py.in +++ /dev/null @@ -1,37 +0,0 @@ -# generated from catkin/cmake/template/__init__.py.in -# keep symbol table as clean as possible by deleting all unnecessary symbols - -from os import path as os_path -from sys import path as sys_path - -from pkgutil import extend_path - -__extended_path = "@PACKAGE_PYTHONPATH@".split(";") -for p in reversed(__extended_path): - sys_path.insert(0, p) - del p -del sys_path - -__path__ = extend_path(__path__, __name__) -del extend_path - -__execfiles = [] -for p in __extended_path: - src_init_file = os_path.join(p, __name__ + '.py') - if os_path.isfile(src_init_file): - __execfiles.append(src_init_file) - else: - src_init_file = os_path.join(p, __name__, '__init__.py') - if os_path.isfile(src_init_file): - __execfiles.append(src_init_file) - del src_init_file - del p -del os_path -del __extended_path - -for __execfile in __execfiles: - with open(__execfile, 'r') as __fh: - exec(__fh.read()) - del __fh - del __execfile -del __execfiles diff --git a/cmake/catkin/templates/_setup_util.py.in b/cmake/catkin/templates/_setup_util.py.in deleted file mode 100755 index 46c8628052..0000000000 --- a/cmake/catkin/templates/_setup_util.py.in +++ /dev/null @@ -1,287 +0,0 @@ -#!@PYTHON_EXECUTABLE@ - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, 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: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -'''This file generates shell code for the setup.SHELL scripts to set environment variables''' - -from __future__ import print_function -import argparse -import copy -import errno -import os -import platform -import sys - -CATKIN_MARKER_FILE = '.catkin' - -system = platform.system() -IS_DARWIN = (system == 'Darwin') -IS_WINDOWS = (system == 'Windows') - -# subfolder of workspace prepended to CMAKE_PREFIX_PATH -ENV_VAR_SUBFOLDERS = { - 'CMAKE_PREFIX_PATH': '', - 'CPATH': 'include', - 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': @CATKIN_LIB_ENVIRONMENT_PATHS@, - 'PATH': '@CATKIN_GLOBAL_BIN_DESTINATION@', - 'PKG_CONFIG_PATH': @CATKIN_PKGCONFIG_ENVIRONMENT_PATHS@, - 'PYTHONPATH': '@PYTHON_INSTALL_DIR@', -} - - -def rollback_env_variables(environ, env_var_subfolders): - ''' - Generate shell code to reset environment variables - by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. - This does not cover modifications performed by environment hooks. - ''' - lines = [] - unmodified_environ = copy.copy(environ) - for key in sorted(env_var_subfolders.keys()): - subfolders = env_var_subfolders[key] - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - value = _rollback_env_variable(unmodified_environ, key, subfolder) - if value is not None: - environ[key] = value - lines.append(assignment(key, value)) - if lines: - lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) - return lines - - -def _rollback_env_variable(environ, name, subfolder): - ''' - For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. - - :param subfolder: str '' or subfoldername that may start with '/' - :returns: the updated value of the environment variable. - ''' - value = environ[name] if name in environ else '' - env_paths = [path for path in value.split(os.pathsep) if path] - value_modified = False - if subfolder: - if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): - subfolder = subfolder[1:] - if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): - subfolder = subfolder[:-1] - for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): - path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path - path_to_remove = None - for env_path in env_paths: - env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path - if env_path_clean == path_to_find: - path_to_remove = env_path - break - if path_to_remove: - env_paths.remove(path_to_remove) - value_modified = True - new_value = os.pathsep.join(env_paths) - return new_value if value_modified else None - - -def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): - ''' - Based on CMAKE_PREFIX_PATH return all catkin workspaces. - - :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` - ''' - # get all cmake prefix paths - env_name = 'CMAKE_PREFIX_PATH' - value = environ[env_name] if env_name in environ else '' - paths = [path for path in value.split(os.pathsep) if path] - # remove non-workspace paths - workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] - return workspaces - - -def prepend_env_variables(environ, env_var_subfolders, workspaces): - ''' - Generate shell code to prepend environment variables - for the all workspaces. - ''' - lines = [] - lines.append(comment('prepend folders of workspaces to environment variables')) - - paths = [path for path in workspaces.split(os.pathsep) if path] - - prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') - lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) - - for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): - subfolder = env_var_subfolders[key] - prefix = _prefix_env_variable(environ, key, paths, subfolder) - lines.append(prepend(environ, key, prefix)) - return lines - - -def _prefix_env_variable(environ, name, paths, subfolders): - ''' - Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. - ''' - value = environ[name] if name in environ else '' - environ_paths = [path for path in value.split(os.pathsep) if path] - checked_paths = [] - for path in paths: - if not isinstance(subfolders, list): - subfolders = [subfolders] - for subfolder in subfolders: - path_tmp = path - if subfolder: - path_tmp = os.path.join(path_tmp, subfolder) - # exclude any path already in env and any path we already added - if path_tmp not in environ_paths and path_tmp not in checked_paths: - checked_paths.append(path_tmp) - prefix_str = os.pathsep.join(checked_paths) - if prefix_str != '' and environ_paths: - prefix_str += os.pathsep - return prefix_str - - -def assignment(key, value): - if not IS_WINDOWS: - return 'export %s="%s"' % (key, value) - else: - return 'set %s=%s' % (key, value) - - -def comment(msg): - if not IS_WINDOWS: - return '# %s' % msg - else: - return 'REM %s' % msg - - -def prepend(environ, key, prefix): - if key not in environ or not environ[key]: - return assignment(key, prefix) - if not IS_WINDOWS: - return 'export %s="%s$%s"' % (key, prefix, key) - else: - return 'set %s=%s%%%s%%' % (key, prefix, key) - - -def find_env_hooks(environ, cmake_prefix_path): - ''' - Generate shell code with found environment hooks - for the all workspaces. - ''' - lines = [] - lines.append(comment('found environment hooks in workspaces')) - - generic_env_hooks = [] - generic_env_hooks_workspace = [] - specific_env_hooks = [] - specific_env_hooks_workspace = [] - generic_env_hooks_by_filename = {} - specific_env_hooks_by_filename = {} - generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' - specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None - # remove non-workspace paths - workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] - for workspace in reversed(workspaces): - env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') - if os.path.isdir(env_hook_dir): - for filename in sorted(os.listdir(env_hook_dir)): - if filename.endswith('.%s' % generic_env_hook_ext): - # remove previous env hook with same name if present - if filename in generic_env_hooks_by_filename: - i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) - generic_env_hooks.pop(i) - generic_env_hooks_workspace.pop(i) - # append env hook - generic_env_hooks.append(os.path.join(env_hook_dir, filename)) - generic_env_hooks_workspace.append(workspace) - generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] - elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): - # remove previous env hook with same name if present - if filename in specific_env_hooks_by_filename: - i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) - specific_env_hooks.pop(i) - specific_env_hooks_workspace.pop(i) - # append env hook - specific_env_hooks.append(os.path.join(env_hook_dir, filename)) - specific_env_hooks_workspace.append(workspace) - specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] - env_hooks = generic_env_hooks + specific_env_hooks - env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace - count = len(env_hooks) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) - for i in range(count): - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) - lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) - return lines - - -def _parse_arguments(args=None): - parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') - parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') - return parser.parse_known_args(args=args)[0] - - -if __name__ == '__main__': - try: - try: - args = _parse_arguments() - except Exception as e: - print(e, file=sys.stderr) - sys.exit(1) - - # environment at generation time - CMAKE_PREFIX_PATH = '@CMAKE_PREFIX_PATH_AS_IS@'.split(';') - # prepend current workspace if not already part of CPP - base_path = os.path.dirname(__file__) - if base_path not in CMAKE_PREFIX_PATH: - CMAKE_PREFIX_PATH.insert(0, base_path) - CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) - - environ = dict(os.environ) - lines = [] - if not args.extend: - lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) - lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) - lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) - print('\n'.join(lines)) - - # need to explicitly flush the output - sys.stdout.flush() - except IOError as e: - # and catch potantial "broken pipe" if stdout is not writable - # which can happen when piping the output to a file but the disk is full - if e.errno == errno.EPIPE: - print(e, file=sys.stderr) - sys.exit(2) - raise - - sys.exit(0) diff --git a/cmake/catkin/templates/cfg-extras.context.py.in b/cmake/catkin/templates/cfg-extras.context.py.in deleted file mode 100644 index 256bb69bd6..0000000000 --- a/cmake/catkin/templates/cfg-extras.context.py.in +++ /dev/null @@ -1,33 +0,0 @@ -# generated from catkin/cmake/template/cfg-extras.context.py.in -DEVELSPACE = '@DEVELSPACE@' == 'TRUE' -INSTALLSPACE = '@INSTALLSPACE@' == 'TRUE' - -CATKIN_DEVEL_PREFIX = '@CATKIN_DEVEL_PREFIX@' - -CATKIN_GLOBAL_BIN_DESTINATION = '@CATKIN_GLOBAL_BIN_DESTINATION@' -CATKIN_GLOBAL_ETC_DESTINATION = '@CATKIN_GLOBAL_ETC_DESTINATION@' -CATKIN_GLOBAL_INCLUDE_DESTINATION = '@CATKIN_GLOBAL_INCLUDE_DESTINATION@' -CATKIN_GLOBAL_LIB_DESTINATION = '@CATKIN_GLOBAL_LIB_DESTINATION@' -CATKIN_GLOBAL_LIBEXEC_DESTINATION = '@CATKIN_GLOBAL_LIBEXEC_DESTINATION@' -CATKIN_GLOBAL_PYTHON_DESTINATION = '@CATKIN_GLOBAL_PYTHON_DESTINATION@' -CATKIN_GLOBAL_SHARE_DESTINATION = '@CATKIN_GLOBAL_SHARE_DESTINATION@' - -CATKIN_PACKAGE_BIN_DESTINATION = '@CATKIN_PACKAGE_BIN_DESTINATION@' -CATKIN_PACKAGE_ETC_DESTINATION = '@CATKIN_PACKAGE_ETC_DESTINATION@' -CATKIN_PACKAGE_INCLUDE_DESTINATION = '@CATKIN_PACKAGE_INCLUDE_DESTINATION@' -CATKIN_PACKAGE_LIB_DESTINATION = '@CATKIN_PACKAGE_LIB_DESTINATION@' -CATKIN_PACKAGE_LIBEXEC_DESTINATION = '@CATKIN_PACKAGE_LIBEXEC_DESTINATION@' -CATKIN_PACKAGE_PYTHON_DESTINATION = '@CATKIN_PACKAGE_PYTHON_DESTINATION@' -CATKIN_PACKAGE_SHARE_DESTINATION = '@CATKIN_PACKAGE_SHARE_DESTINATION@' - -CMAKE_BINARY_DIR = '@CMAKE_BINARY_DIR@' -CMAKE_CURRENT_BINARY_DIR = '@CMAKE_CURRENT_BINARY_DIR@' -CMAKE_CURRENT_SOURCE_DIR = '@CMAKE_CURRENT_SOURCE_DIR@' -CMAKE_INSTALL_PREFIX = '@CMAKE_INSTALL_PREFIX@' -CMAKE_SOURCE_DIR = '@CMAKE_SOURCE_DIR@' - -PKG_CMAKE_DIR = '@PKG_CMAKE_DIR@' - -PROJECT_NAME = '@PROJECT_NAME@' -PROJECT_BINARY_DIR = '@PROJECT_BINARY_DIR@' -PROJECT_SOURCE_DIR = '@PROJECT_SOURCE_DIR@' diff --git a/cmake/catkin/templates/env-hook.context.py.in b/cmake/catkin/templates/env-hook.context.py.in deleted file mode 100644 index 57cb22de6f..0000000000 --- a/cmake/catkin/templates/env-hook.context.py.in +++ /dev/null @@ -1,33 +0,0 @@ -# generated from catkin/cmake/template/env-hook.context.py.in -DEVELSPACE = @DEVELSPACE@ -INSTALLSPACE = @INSTALLSPACE@ - -CATKIN_DEVEL_PREFIX = '@CATKIN_DEVEL_PREFIX@' - -CATKIN_GLOBAL_BIN_DESTINATION = '@CATKIN_GLOBAL_BIN_DESTINATION@' -CATKIN_GLOBAL_ETC_DESTINATION = '@CATKIN_GLOBAL_ETC_DESTINATION@' -CATKIN_GLOBAL_INCLUDE_DESTINATION = '@CATKIN_GLOBAL_INCLUDE_DESTINATION@' -CATKIN_GLOBAL_LIB_DESTINATION = '@CATKIN_GLOBAL_LIB_DESTINATION@' -CATKIN_GLOBAL_LIBEXEC_DESTINATION = '@CATKIN_GLOBAL_LIBEXEC_DESTINATION@' -CATKIN_GLOBAL_PYTHON_DESTINATION = '@CATKIN_GLOBAL_PYTHON_DESTINATION@' -CATKIN_GLOBAL_SHARE_DESTINATION = '@CATKIN_GLOBAL_SHARE_DESTINATION@' - -CATKIN_PACKAGE_BIN_DESTINATION = '@CATKIN_PACKAGE_BIN_DESTINATION@' -CATKIN_PACKAGE_ETC_DESTINATION = '@CATKIN_PACKAGE_ETC_DESTINATION@' -CATKIN_PACKAGE_INCLUDE_DESTINATION = '@CATKIN_PACKAGE_INCLUDE_DESTINATION@' -CATKIN_PACKAGE_LIB_DESTINATION = '@CATKIN_PACKAGE_LIB_DESTINATION@' -CATKIN_PACKAGE_LIBEXEC_DESTINATION = '@CATKIN_PACKAGE_LIBEXEC_DESTINATION@' -CATKIN_PACKAGE_PYTHON_DESTINATION = '@CATKIN_PACKAGE_PYTHON_DESTINATION@' -CATKIN_PACKAGE_SHARE_DESTINATION = '@CATKIN_PACKAGE_SHARE_DESTINATION@' - -CMAKE_BINARY_DIR = '@CMAKE_BINARY_DIR@' -CMAKE_CURRENT_BINARY_DIR = '@CMAKE_CURRENT_BINARY_DIR@' -CMAKE_CURRENT_SOURCE_DIR = '@CMAKE_CURRENT_SOURCE_DIR@' -CMAKE_INSTALL_PREFIX = '@CMAKE_INSTALL_PREFIX@' -CMAKE_SOURCE_DIR = '@CMAKE_SOURCE_DIR@' - -PROJECT_NAME = '@PROJECT_NAME@' -PROJECT_BINARY_DIR = '@PROJECT_BINARY_DIR@' -PROJECT_SOURCE_DIR = '@PROJECT_SOURCE_DIR@' - -PYTHON_EXECUTABLE = '@PYTHON_EXECUTABLE@' diff --git a/cmake/catkin/templates/env.bat.in b/cmake/catkin/templates/env.bat.in deleted file mode 100644 index 55d882ec14..0000000000 --- a/cmake/catkin/templates/env.bat.in +++ /dev/null @@ -1,11 +0,0 @@ -@echo off -REM generated from catkin/cmake/templates/env.bat.in - -if "%1"=="" ( - echo "Usage: env.bat COMMANDS" - echo "Calling env.bat without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." - exit 1 -) else ( - call "@SETUP_DIR@/@SETUP_FILENAME@.bat" - %* -) diff --git a/cmake/catkin/templates/env.sh.in b/cmake/catkin/templates/env.sh.in deleted file mode 100755 index 4ffb7dd4b3..0000000000 --- a/cmake/catkin/templates/env.sh.in +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/templates/env.sh.in - -if [ $# -eq 0 ] ; then - /bin/echo "Usage: env.sh COMMANDS" - /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." - exit 1 -fi - -# ensure to not use different shell type which was set before -CATKIN_SHELL=sh - -# source @SETUP_FILENAME@.sh from same directory as this file -_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/@SETUP_FILENAME@.sh" -exec "$@" diff --git a/cmake/catkin/templates/generate_cached_setup.py.in b/cmake/catkin/templates/generate_cached_setup.py.in deleted file mode 100644 index c2848e04c5..0000000000 --- a/cmake/catkin/templates/generate_cached_setup.py.in +++ /dev/null @@ -1,29 +0,0 @@ -from __future__ import print_function -import argparse -import os -import stat -import sys - -# find the import for catkin's python package - either from source space or from an installed underlay -if os.path.exists(os.path.join('@catkin_EXTRAS_DIR@', 'catkinConfig.cmake.in')): - sys.path.insert(0, os.path.join('@catkin_EXTRAS_DIR@', '..', 'python')) -try: - from catkin.environment_cache import generate_environment_script -except ImportError: - # search for catkin package in all workspaces and prepend to path - for workspace in "@CATKIN_WORKSPACES@".split(';'): - python_path = os.path.join(workspace, '@CATKIN_GLOBAL_PYTHON_DESTINATION@') - if os.path.isdir(os.path.join(python_path, 'catkin')): - sys.path.insert(0, python_path) - break - from catkin.environment_cache import generate_environment_script - -code = generate_environment_script('@CATKIN_DEVEL_PREFIX@/env.@script_ext@') - -output_filename = '@SETUP_DIR@/@SETUP_FILENAME@.@script_ext@' -with open(output_filename, 'w') as f: - #print('Generate script for cached setup "%s"' % output_filename) - f.write('\n'.join(code)) - -mode = os.stat(output_filename).st_mode -os.chmod(output_filename, mode | stat.S_IXUSR) diff --git a/cmake/catkin/templates/order_packages.context.py.in b/cmake/catkin/templates/order_packages.context.py.in deleted file mode 100644 index 3609c0b300..0000000000 --- a/cmake/catkin/templates/order_packages.context.py.in +++ /dev/null @@ -1,5 +0,0 @@ -# generated from catkin/cmake/template/order_packages.context.py.in -source_root_dir = "@CMAKE_CURRENT_SOURCE_DIR@" -whitelisted_packages = "@CATKIN_WHITELIST_PACKAGES@".split(';') if "@CATKIN_WHITELIST_PACKAGES@" != "" else [] -blacklisted_packages = "@CATKIN_BLACKLIST_PACKAGES@".split(';') if "@CATKIN_BLACKLIST_PACKAGES@" != "" else [] -underlay_workspaces = "@CATKIN_WORKSPACES@".split(';') if "@CATKIN_WORKSPACES@" != "" else [] diff --git a/cmake/catkin/templates/pkg.context.pc.in b/cmake/catkin/templates/pkg.context.pc.in deleted file mode 100644 index 8e9cd6bc12..0000000000 --- a/cmake/catkin/templates/pkg.context.pc.in +++ /dev/null @@ -1,8 +0,0 @@ -# generated from catkin/cmake/template/pkg.context.pc.in -CATKIN_PACKAGE_PREFIX = "@CATKIN_PACKAGE_PREFIX@" -PROJECT_PKG_CONFIG_INCLUDE_DIRS = "@PROJECT_PKG_CONFIG_INCLUDE_DIRS@".split(';') if "@PROJECT_PKG_CONFIG_INCLUDE_DIRS@" != "" else [] -PROJECT_CATKIN_DEPENDS = "@_PROJECT_CATKIN_DEPENDS@".replace(';', ' ') -PKG_CONFIG_LIBRARIES_WITH_PREFIX = "@PKG_CONFIG_LIBRARIES_WITH_PREFIX@".split(';') if "@PKG_CONFIG_LIBRARIES_WITH_PREFIX@" != "" else [] -PROJECT_NAME = "@PROJECT_NAME@" -PROJECT_SPACE_DIR = "@PROJECT_SPACE_DIR@" -PROJECT_VERSION = "@PROJECT_VERSION@" diff --git a/cmake/catkin/templates/pkgConfig-version.cmake.in b/cmake/catkin/templates/pkgConfig-version.cmake.in deleted file mode 100644 index f29cb36d47..0000000000 --- a/cmake/catkin/templates/pkgConfig-version.cmake.in +++ /dev/null @@ -1,14 +0,0 @@ -# generated from catkin/cmake/template/pkgConfig-version.cmake.in -set(PACKAGE_VERSION "@PROJECT_VERSION@") - -set(PACKAGE_VERSION_EXACT False) -set(PACKAGE_VERSION_COMPATIBLE False) - -if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}") - set(PACKAGE_VERSION_EXACT True) - set(PACKAGE_VERSION_COMPATIBLE True) -endif() - -if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}") - set(PACKAGE_VERSION_COMPATIBLE True) -endif() diff --git a/cmake/catkin/templates/pkgConfig.cmake.in b/cmake/catkin/templates/pkgConfig.cmake.in deleted file mode 100644 index 91f6d65354..0000000000 --- a/cmake/catkin/templates/pkgConfig.cmake.in +++ /dev/null @@ -1,191 +0,0 @@ -# generated from catkin/cmake/template/pkgConfig.cmake.in - -# append elements to a list and remove existing duplicates from the list -# copied from catkin/cmake/list_append_deduplicate.cmake to keep pkgConfig -# self contained -macro(_list_append_deduplicate listname) - if(NOT "${ARGN}" STREQUAL "") - if(${listname}) - list(REMOVE_ITEM ${listname} ${ARGN}) - endif() - list(APPEND ${listname} ${ARGN}) - endif() -endmacro() - -# append elements to a list if they are not already in the list -# copied from catkin/cmake/list_append_unique.cmake to keep pkgConfig -# self contained -macro(_list_append_unique listname) - foreach(_item ${ARGN}) - list(FIND ${listname} ${_item} _index) - if(_index EQUAL -1) - list(APPEND ${listname} ${_item}) - endif() - endforeach() -endmacro() - -# pack a list of libraries with optional build configuration keywords -# copied from catkin/cmake/catkin_libraries.cmake to keep pkgConfig -# self contained -macro(_pack_libraries_with_build_configuration VAR) - set(${VAR} "") - set(_argn ${ARGN}) - list(LENGTH _argn _count) - set(_index 0) - while(${_index} LESS ${_count}) - list(GET _argn ${_index} lib) - if("${lib}" MATCHES "^debug|optimized|general$") - math(EXPR _index "${_index} + 1") - if(${_index} EQUAL ${_count}) - message(FATAL_ERROR "_pack_libraries_with_build_configuration() the list of libraries '${ARGN}' ends with '${lib}' which is a build configuration keyword and must be followed by a library") - endif() - list(GET _argn ${_index} library) - list(APPEND ${VAR} "${lib}${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}${library}") - else() - list(APPEND ${VAR} "${lib}") - endif() - math(EXPR _index "${_index} + 1") - endwhile() -endmacro() - -# unpack a list of libraries with optional build configuration keyword prefixes -# copied from catkin/cmake/catkin_libraries.cmake to keep pkgConfig -# self contained -macro(_unpack_libraries_with_build_configuration VAR) - set(${VAR} "") - foreach(lib ${ARGN}) - string(REGEX REPLACE "^(debug|optimized|general)${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}(.+)$" "\\1;\\2" lib "${lib}") - list(APPEND ${VAR} "${lib}") - endforeach() -endmacro() - - -if(@PROJECT_NAME@_CONFIG_INCLUDED) - return() -endif() -set(@PROJECT_NAME@_CONFIG_INCLUDED TRUE) - -# set variables for source/devel/install prefixes -if("@DEVELSPACE@" STREQUAL "TRUE") - set(@PROJECT_NAME@_SOURCE_PREFIX @CMAKE_CURRENT_SOURCE_DIR@) - set(@PROJECT_NAME@_DEVEL_PREFIX @CATKIN_DEVEL_PREFIX@) - set(@PROJECT_NAME@_INSTALL_PREFIX "") - set(@PROJECT_NAME@_PREFIX ${@PROJECT_NAME@_DEVEL_PREFIX}) -else() - set(@PROJECT_NAME@_SOURCE_PREFIX "") - set(@PROJECT_NAME@_DEVEL_PREFIX "") - set(@PROJECT_NAME@_INSTALL_PREFIX @CMAKE_INSTALL_PREFIX@) - set(@PROJECT_NAME@_PREFIX ${@PROJECT_NAME@_INSTALL_PREFIX}) -endif() - -# warn when using a deprecated package -if(NOT "@PROJECT_DEPRECATED@" STREQUAL "") - set(_msg "WARNING: package '@PROJECT_NAME@' is deprecated") - # append custom deprecation text if available - if(NOT "@PROJECT_DEPRECATED@" STREQUAL "TRUE") - set(_msg "${_msg} (@PROJECT_DEPRECATED@)") - endif() - message("${_msg}") -endif() - -# flag project as catkin-based to distinguish if a find_package()-ed project is a catkin project -set(@PROJECT_NAME@_FOUND_CATKIN_PROJECT TRUE) - -if(NOT "@PROJECT_CMAKE_CONFIG_INCLUDE_DIRS@" STREQUAL "") - set(@PROJECT_NAME@_INCLUDE_DIRS "") - set(_include_dirs "@PROJECT_CMAKE_CONFIG_INCLUDE_DIRS@") - foreach(idir ${_include_dirs}) - if(IS_ABSOLUTE ${idir} AND IS_DIRECTORY ${idir}) - set(include ${idir}) - elseif("${idir}" STREQUAL "@CATKIN_GLOBAL_INCLUDE_DESTINATION@") - get_filename_component(include "${@PROJECT_NAME@_DIR}/../../../@CATKIN_GLOBAL_INCLUDE_DESTINATION@" ABSOLUTE) - if(NOT IS_DIRECTORY ${include}) - message(FATAL_ERROR "Project '@PROJECT_NAME@' specifies '${idir}' as an include dir, which is not found. It does not exist in '${include}'. Ask the maintainer '@PROJECT_MAINTAINER@' to fix it.") - endif() - else() - message(FATAL_ERROR "Project '@PROJECT_NAME@' specifies '${idir}' as an include dir, which is not found. It does neither exist as an absolute directory nor in '@PKG_INCLUDE_PREFIX@/${idir}'. Ask the maintainer '@PROJECT_MAINTAINER@' to fix it.") - endif() - _list_append_unique(@PROJECT_NAME@_INCLUDE_DIRS ${include}) - endforeach() -endif() - -set(libraries "@PKG_CONFIG_LIBRARIES@") -foreach(library ${libraries}) - # keep build configuration keywords, target names and absolute libraries as-is - if("${library}" MATCHES "^debug|optimized|general$") - list(APPEND @PROJECT_NAME@_LIBRARIES ${library}) - elseif(TARGET ${library}) - list(APPEND @PROJECT_NAME@_LIBRARIES ${library}) - elseif(IS_ABSOLUTE ${library}) - list(APPEND @PROJECT_NAME@_LIBRARIES ${library}) - else() - set(lib_path "") - set(lib "${library}-NOTFOUND") - # since the path where the library is found is returned we have to iterate over the paths manually - foreach(path @PKG_CONFIG_LIB_PATHS@) - find_library(lib ${library} - PATHS ${path} - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) - if(lib) - set(lib_path ${path}) - break() - endif() - endforeach() - if(lib) - _list_append_unique(@PROJECT_NAME@_LIBRARY_DIRS ${lib_path}) - list(APPEND @PROJECT_NAME@_LIBRARIES ${lib}) - else() - # as a fall back for non-catkin libraries try to search globally - find_library(lib ${library}) - if(NOT lib) - message(FATAL_ERROR "Project '${PROJECT_NAME}' tried to find library '${library}'. The library is neither a target nor built/installed properly. Did you compile project '@PROJECT_NAME@'? Did you find_package() it before the subdirectory containing its code is included?") - endif() - list(APPEND @PROJECT_NAME@_LIBRARIES ${lib}) - endif() - endif() -endforeach() - -set(@PROJECT_NAME@_EXPORTED_TARGETS "@PKG_EXPORTED_TARGETS@") -# create dummy targets for exported code generation targets to make life of users easier -foreach(t ${@PROJECT_NAME@_EXPORTED_TARGETS}) - if(NOT TARGET ${t}) - add_custom_target(${t}) - endif() -endforeach() - -set(depends "@PROJECT_DEPENDENCIES@") -foreach(depend ${depends}) - string(REPLACE " " ";" depend_list ${depend}) - # the package name of the dependency must be kept in a unique variable so that it is not overwritten in recursive calls - list(GET depend_list 0 @PROJECT_NAME@_dep) - list(LENGTH depend_list count) - if(${count} EQUAL 1) - # simple dependencies must only be find_package()-ed once - if(NOT ${@PROJECT_NAME@_dep}_FOUND) - find_package(${@PROJECT_NAME@_dep} REQUIRED) - endif() - else() - # dependencies with components must be find_package()-ed again - list(REMOVE_AT depend_list 0) - find_package(${@PROJECT_NAME@_dep} REQUIRED ${depend_list}) - endif() - _list_append_unique(@PROJECT_NAME@_INCLUDE_DIRS ${${@PROJECT_NAME@_dep}_INCLUDE_DIRS}) - - # merge build configuration keywords with library names to correctly deduplicate - _pack_libraries_with_build_configuration(@PROJECT_NAME@_LIBRARIES ${@PROJECT_NAME@_LIBRARIES}) - _pack_libraries_with_build_configuration(_libraries ${${@PROJECT_NAME@_dep}_LIBRARIES}) - _list_append_deduplicate(@PROJECT_NAME@_LIBRARIES ${_libraries}) - # undo build configuration keyword merging after deduplication - _unpack_libraries_with_build_configuration(@PROJECT_NAME@_LIBRARIES ${@PROJECT_NAME@_LIBRARIES}) - - _list_append_unique(@PROJECT_NAME@_LIBRARY_DIRS ${${@PROJECT_NAME@_dep}_LIBRARY_DIRS}) - list(APPEND @PROJECT_NAME@_EXPORTED_TARGETS ${${@PROJECT_NAME@_dep}_EXPORTED_TARGETS}) -endforeach() - -set(pkg_cfg_extras "@PKG_CFG_EXTRAS@") -foreach(extra ${pkg_cfg_extras}) - if(NOT IS_ABSOLUTE ${extra}) - set(extra ${@PROJECT_NAME@_DIR}/${extra}) - endif() - include(${extra}) -endforeach() diff --git a/cmake/catkin/templates/python_distutils_install.bat.in b/cmake/catkin/templates/python_distutils_install.bat.in deleted file mode 100644 index fd9906a687..0000000000 --- a/cmake/catkin/templates/python_distutils_install.bat.in +++ /dev/null @@ -1,17 +0,0 @@ -@echo off - -if DEFINED DESTDIR ( - echo "Destdir.............%DESTDIR%" - set DESTDIR_ARG="--root=%DESTDIR%" -) - -cd "@INSTALL_CMD_WORKING_DIRECTORY@" - -cmd /V:on /C set PYTHONPATH="@CMAKE_INSTALL_PREFIX@/@PYTHON_INSTALL_DIR@;@CMAKE_BINARY_DIR@/@PYTHON_INSTALL_DIR@" ^ - && set CATKIN_BINARY_DIR="@CMAKE_BINARY_DIR@" ^ - && "@PYTHON_EXECUTABLE@" ^ - "@CMAKE_CURRENT_SOURCE_DIR@/setup.py" ^ - build --build-base "@CMAKE_CURRENT_BINARY_DIR@" ^ - install %DESTDIR_ARG% @SETUPTOOLS_ARG_EXTRA@ ^ - --prefix="@SETUPTOOLS_INSTALL_PREFIX@" ^ - --install-scripts="@SETUPTOOLS_INSTALL_PREFIX@\@CATKIN_GLOBAL_BIN_DESTINATION@" diff --git a/cmake/catkin/templates/python_distutils_install.sh.in b/cmake/catkin/templates/python_distutils_install.sh.in deleted file mode 100755 index e7b56c72d0..0000000000 --- a/cmake/catkin/templates/python_distutils_install.sh.in +++ /dev/null @@ -1,28 +0,0 @@ -#!/bin/sh -x - -if [ -n "$DESTDIR" ] ; then - case $DESTDIR in - /*) # ok - ;; - *) - /bin/echo "DESTDIR argument must be absolute... " - /bin/echo "otherwise python's distutils will bork things." - exit 1 - esac - DESTDIR_ARG="--root=$DESTDIR" -fi - -cd "@INSTALL_CMD_WORKING_DIRECTORY@" - -# Note that PYTHONPATH is pulled from the environment to support installing -# into one location when some dependencies were installed in another -# location, #123. -/usr/bin/env \ - PYTHONPATH="@CMAKE_INSTALL_PREFIX@/@PYTHON_INSTALL_DIR@:@CMAKE_BINARY_DIR@/@PYTHON_INSTALL_DIR@:$PYTHONPATH" \ - CATKIN_BINARY_DIR="@CMAKE_BINARY_DIR@" \ - "@PYTHON_EXECUTABLE@" \ - "@CMAKE_CURRENT_SOURCE_DIR@/setup.py" \ - build --build-base "@CMAKE_CURRENT_BINARY_DIR@" \ - install \ - $DESTDIR_ARG \ - @SETUPTOOLS_ARG_EXTRA@ --prefix="@CMAKE_INSTALL_PREFIX@" --install-scripts="@CMAKE_INSTALL_PREFIX@/@CATKIN_GLOBAL_BIN_DESTINATION@" diff --git a/cmake/catkin/templates/relay.py.in b/cmake/catkin/templates/relay.py.in deleted file mode 100644 index 5221f48dc7..0000000000 --- a/cmake/catkin/templates/relay.py.in +++ /dev/null @@ -1,4 +0,0 @@ -# creates a relay to a python script source file, acting as that file. -# The purpose is that of a symlink -with open("@PYTHON_SCRIPT@", 'r') as fh: - exec(fh.read()) diff --git a/cmake/catkin/templates/rosinstall.in b/cmake/catkin/templates/rosinstall.in deleted file mode 100644 index 17f78a2981..0000000000 --- a/cmake/catkin/templates/rosinstall.in +++ /dev/null @@ -1,2 +0,0 @@ -- setup-file: - local-name: @SETUP_DIR@/setup.sh diff --git a/cmake/catkin/templates/safe_execute_install.cmake.in b/cmake/catkin/templates/safe_execute_install.cmake.in deleted file mode 100644 index 643c6eb4c9..0000000000 --- a/cmake/catkin/templates/safe_execute_install.cmake.in +++ /dev/null @@ -1,5 +0,0 @@ -execute_process(COMMAND "@INSTALL_SCRIPT@" RESULT_VARIABLE res) - -if(NOT res EQUAL 0) - message(FATAL_ERROR "execute_process(@INSTALL_SCRIPT@) returned error code ${res}") -endif() diff --git a/cmake/catkin/templates/script.bash.in b/cmake/catkin/templates/script.bash.in deleted file mode 100755 index ab172b97b8..0000000000 --- a/cmake/catkin/templates/script.bash.in +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/env bash -# generated from catkin/cmake/templates/script.bash.in - -"@BASH_SCRIPT@" "$@" diff --git a/cmake/catkin/templates/script.in b/cmake/catkin/templates/script.in deleted file mode 100755 index 6c1be8d232..0000000000 --- a/cmake/catkin/templates/script.in +++ /dev/null @@ -1 +0,0 @@ -@SCRIPT@ diff --git a/cmake/catkin/templates/script.py.in b/cmake/catkin/templates/script.py.in deleted file mode 100755 index dc3a277371..0000000000 --- a/cmake/catkin/templates/script.py.in +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env python -# creates a relay to a python script source file, acting as that file. -# The purpose is that of a symlink -with open("@PYTHON_SCRIPT@", 'r') as fh: - exec(fh.read()) diff --git a/cmake/catkin/templates/script.sh.in b/cmake/catkin/templates/script.sh.in deleted file mode 100755 index 54e4e7a0a6..0000000000 --- a/cmake/catkin/templates/script.sh.in +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/templates/script.sh.in - -"@EXECUTABLE@" "$@" diff --git a/cmake/catkin/templates/setup.bash.in b/cmake/catkin/templates/setup.bash.in deleted file mode 100644 index ff47af8f30..0000000000 --- a/cmake/catkin/templates/setup.bash.in +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env bash -# generated from catkin/cmake/templates/setup.bash.in - -CATKIN_SHELL=bash - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) -. "$_CATKIN_SETUP_DIR/setup.sh" diff --git a/cmake/catkin/templates/setup.bat.in b/cmake/catkin/templates/setup.bat.in deleted file mode 100644 index 1549181597..0000000000 --- a/cmake/catkin/templates/setup.bat.in +++ /dev/null @@ -1,59 +0,0 @@ -@echo off -REM generated from catkin/cmake/template/setup.bat.in - -REM Sets various environment variables and sources additional environment hooks. -REM It tries it's best to undo changes from a previously sourced setup file before. -REM Supported command line options: -REM --extend: skips the undoing of changes from a previously sourced setup file - -set _SETUP_UTIL="@SETUP_DIR@/_setup_util.py" - -if NOT EXIST "%_SETUP_UTIL%" ( - echo "Missing Python script: %_SETUP_UTIL%" - exit 22 -) - -REM set the Python executable -set _PYTHON="@PYTHON_EXECUTABLE@" - -REM generate pseudo random temporary filename -:GenerateTempFilename -REM replace leading space of time with zero -set _SETUP_TMP=%Time: =0% -REM remove time delimiters -set _SETUP_TMP=%_SETUP_TMP::=% -set _SETUP_TMP=%_SETUP_TMP:.=% -set _SETUP_TMP=%_SETUP_TMP:,=% -set _SETUP_TMP=%Temp%\setup.%_SETUP_TMP%.bat -if EXIST %_SETUP_TMP% do goto GenerateTempFilename -type NUL > "%_SETUP_TMP%" -if NOT EXIST %_SETUP_TMP% ( - echo "Could not create temporary file: %_SETUP_TMP%" - exit 1 -) - -REM invoke Python script to generate necessary exports of environment variables -%_PYTHON% %_SETUP_UTIL% %* > %_SETUP_TMP% -if NOT EXIST %_SETUP_TMP% ( - echo "Could not create temporary file: %_SETUP_TMP%" - return 1 -) -call %_SETUP_TMP% -del %_SETUP_TMP% - -REM source all environment hooks -for /F "tokens=* delims=;" %%a in ("%_CATKIN_ENVIRONMENT_HOOKS%") do ( - for %%b in (%%a) do ( - call "%%b" - ) -) - -REM 3rdparty packages often put dll's into lib (convention is bin) and -REM windows finds it's dll's via the PATH variable. Make that happen here! -set PATH=%LD_LIBRARY_PATH%;%PATH% - -REM unset temporary variables -set _SETUP_UTIL= -set _PYTHON= -set _SETUP_TMP= -set _CATKIN_ENVIRONMENT_HOOKS= diff --git a/cmake/catkin/templates/setup.sh.in b/cmake/catkin/templates/setup.sh.in deleted file mode 100644 index a47611bdd0..0000000000 --- a/cmake/catkin/templates/setup.sh.in +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env sh -# generated from catkin/cmake/template/setup.sh.in - -# Sets various environment variables and sources additional environment hooks. -# It tries it's best to undo changes from a previously sourced setup file before. -# Supported command line options: -# --extend: skips the undoing of changes from a previously sourced setup file - -# since this file is sourced either use the provided _CATKIN_SETUP_DIR -# or fall back to the destination set at configure time -: ${_CATKIN_SETUP_DIR:=@SETUP_DIR@} -_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" -unset _CATKIN_SETUP_DIR - -if [ ! -f "$_SETUP_UTIL" ]; then - echo "Missing Python script: $_SETUP_UTIL" - return 22 -fi - -# detect if running on Darwin platform -_UNAME=`uname -s` -_IS_DARWIN=0 -if [ "$_UNAME" = "Darwin" ]; then - _IS_DARWIN=1 -fi -unset _UNAME - -# make sure to export all environment variables -export CMAKE_PREFIX_PATH -export CPATH -if [ $_IS_DARWIN -eq 0 ]; then - export LD_LIBRARY_PATH -else - export DYLD_LIBRARY_PATH -fi -unset _IS_DARWIN -export PATH -export PKG_CONFIG_PATH -export PYTHONPATH - -# remember type of shell if not already set -if [ -z "$CATKIN_SHELL" ]; then - CATKIN_SHELL=sh -fi - -# invoke Python script to generate necessary exports of environment variables -_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX` -if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then - echo "Could not create temporary file: $_SETUP_TMP" - return 1 -fi -CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP -_RC=$? -if [ $_RC -ne 0 ]; then - if [ $_RC -eq 2 ]; then - echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" - else - echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" - fi - unset _RC - unset _SETUP_UTIL - rm -f $_SETUP_TMP - unset _SETUP_TMP - return 1 -fi -unset _RC -unset _SETUP_UTIL -. $_SETUP_TMP -rm -f $_SETUP_TMP -unset _SETUP_TMP - -# source all environment hooks -_i=0 -while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do - eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i - unset _CATKIN_ENVIRONMENT_HOOKS_$_i - eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE - # set workspace for environment hook - CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace - . "$_envfile" - unset CATKIN_ENV_HOOK_WORKSPACE - _i=$((_i + 1)) -done -unset _i - -unset _CATKIN_ENVIRONMENT_HOOKS_COUNT diff --git a/cmake/catkin/templates/setup.zsh.in b/cmake/catkin/templates/setup.zsh.in deleted file mode 100644 index 9f780b7410..0000000000 --- a/cmake/catkin/templates/setup.zsh.in +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env zsh -# generated from catkin/cmake/templates/setup.zsh.in - -CATKIN_SHELL=zsh - -# source setup.sh from same directory as this file -_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) -emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh"' diff --git a/cmake/catkin/test/catkin_download_test_data.cmake b/cmake/catkin/test/catkin_download_test_data.cmake deleted file mode 100644 index 9d39052607..0000000000 --- a/cmake/catkin/test/catkin_download_test_data.cmake +++ /dev/null @@ -1,52 +0,0 @@ -_generate_function_if_testing_is_disabled("catkin_download_test_data") - -# -# Download a file containing test data from a URL. -# -# It is commonly used to download larger data files for unit tests -# which should not be stored in the repository. -# -# .. note:: The target will be registered as a dependency -# of the "tests" target. -# -# .. note:: If the tests should be run on the ROS buildfarm the URL -# must be publically and reliably accessible. -# -# :param target: the target name -# :type target: string -# :param url: the url to download -# :type url: string - -# :param DESTINATION: the directory where the file is downloaded to -# (default: ${PROJECT_BINARY_DIR}) -# :type DESTINATION: string -# :param FILENAME: the filename of the downloaded file -# (default: the basename of the url) -# :type FILENAME: string -# :param MD5: the expected md5 hash to compare against -# (default: empty, skipping the check) -# :type MD5: string -# -# @public -function(catkin_download_test_data target url) - _warn_if_skip_testing("catkin_download_test_data") - - cmake_parse_arguments(ARG "" "DESTINATION;FILENAME;MD5" "" ${ARGN}) - if(ARG_UNPARSED_ARGUMENTS) - message(FATAL_ERROR "catkin_download_test_data() called with unused arguments: ${ARG_UNPARSED_ARGUMENTS}") - endif() - if(NOT ARG_DESTINATION) - set(ARG_DESTINATION ${PROJECT_BINARY_DIR}) - endif() - if(NOT ARG_FILENAME) - get_filename_component(ARG_FILENAME ${url} NAME) - endif() - set(output "${ARG_DESTINATION}/${ARG_FILENAME}") - add_custom_command(OUTPUT ${output} - COMMAND ${PYTHON_EXECUTABLE} ${catkin_EXTRAS_DIR}/test/download_checkmd5.py ${url} ${output} ${ARG_MD5} - VERBATIM) - add_custom_target(${target} DEPENDS ${output}) - if(TARGET tests) - add_dependencies(tests ${target}) - endif() -endfunction() diff --git a/cmake/catkin/test/download_checkmd5.py b/cmake/catkin/test/download_checkmd5.py deleted file mode 100755 index 7394315b80..0000000000 --- a/cmake/catkin/test/download_checkmd5.py +++ /dev/null @@ -1,175 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function -import errno -import os -import sys -try: - from urllib.request import addinfourl, BaseHandler, build_opener, Request, URLError -except ImportError: - from urllib2 import addinfourl, BaseHandler, build_opener, Request, URLError -import hashlib -from argparse import ArgumentParser - -NAME = "download_checkmd5.py" - - -class HTTPRangeHandler(BaseHandler): - - def http_error_206(self, req, fp, code, msg, hdrs): - r = addinfourl(fp, hdrs, req.get_full_url()) - r.code = code - r.msg = msg - return r - - def http_error_416(self, req, fp, code, msg, hdrs): - raise URLError('Requested Range Not Satisfiable') - - -def download_with_resume(uri, dest): - handler = HTTPRangeHandler() - opener = build_opener(handler) - - offset = 0 - content_length = None - accept_ranges = False - while True: - req = Request(uri) - if offset: - req.add_header('Range', 'bytes=%d-' % offset) - src_file = None - try: - src_file = opener.open(req) - headers = src_file.info() - if not offset: - # on first connection check server capabilities - if 'Content-Length' in headers: - content_length = int(headers['Content-Length']) - if 'Accept-Ranges' in headers: - accept_ranges = headers['Accept-Ranges'] != 'none' - else: - # on resume verify that server understood range header and responded accordingly - if 'Content-Range' not in headers: - raise IOError('Download aborted and server does not support resuming download') - if int(headers['Content-Range'][len('bytes '):].split('-')[0]) != offset: - raise IOError('Download aborted because server replied with different content range then requested') - sys.stdout.write(' resume from %d...' % offset) - sys.stdout.flush() - with open(dest, 'ab' if offset else 'wb') as dst_file: - progress = False - while True: - data = src_file.read(8192) - if not data: - break - progress = True - dst_file.write(data) - offset += len(data) - if not progress: - # if no bytes have been received abort download - raise IOError("No progress when trying to download '%s'" % uri) - except: - if src_file: - src_file.close() - raise - - # when content length is unknown it is assumed that the download is complete - if content_length is None: - break - # or when enough data has been downloaded (> is especially a valid case) - if offset >= content_length: - break - if not accept_ranges: - raise IOError('Server does not accept ranges to resume download') - - -def download_md5(uri, dest): - """ - downloads file from uri to file dest - """ - # Create intermediate directories as necessary, #2970 - dirname = os.path.dirname(dest) - if len(dirname): - try: - os.makedirs(dirname) - except OSError as e: - if e.errno != errno.EEXIST: - raise - - sys.stdout.write('Downloading %s to %s...' % (uri, dest)) - sys.stdout.flush() - try: - download_with_resume(uri, dest) - sys.stdout.write(' done.\n') - except Exception as e: - # delete partially downloaded data - if os.path.exists(dest): - os.unlink(dest) - sys.stdout.write(' failed (%s)!\n' % e) - raise - - -def checkmd5(dest, md5sum=None): - """ - checks file at dest against md5. - :returns (boolean, hexdigest): True if dest contents matches md5sum - """ - if not os.path.exists(dest): - return False, 'null' - with open(dest, 'rb') as f: - md5value = hashlib.md5() - while True: - buf = f.read(4096) - if not buf: - break - md5value.update(buf) - hexdigest = md5value.hexdigest() - - print('Checking md5sum on %s' % (dest)) - return hexdigest == md5sum, hexdigest - - -def main(argv=sys.argv[1:]): - """ - Dowloads URI to file dest and checks md5 if given. - """ - parser = ArgumentParser(description='Dowloads URI to file dest. If md5sum is given, checks md5sum. If file existed and mismatch, downloads and checks again') - parser.add_argument('uri') - parser.add_argument('dest') - parser.add_argument('md5sum', nargs='?') - parser.add_argument('--ignore-error', action='store_true', help='Ignore download errors') - args = parser.parse_args(argv) - - uri = args.uri - if '://' not in uri: - uri = 'file://' + uri - - fresh = False - if not os.path.exists(args.dest): - try: - download_md5(uri, args.dest) - except Exception: - if args.ignore_error: - return 0 - raise - fresh = True - - if args.md5sum: - result, hexdigest = checkmd5(args.dest, args.md5sum) - if result is False and fresh is False: - print('WARNING: md5sum mismatch (%s != %s); re-downloading file %s' % (hexdigest, args.md5sum, args.dest)) - os.remove(args.dest) - try: - download_md5(uri, args.dest) - except Exception: - if args.ignore_error: - return 0 - raise - result, hexdigest = checkmd5(args.dest, args.md5sum) - if result is False: - return 'ERROR: md5sum mismatch (%s != %s) on %s; aborting' % (hexdigest, args.md5sum, args.dest) - - return 0 - - -if __name__ == '__main__': - sys.exit(main()) diff --git a/cmake/catkin/test/gtest.cmake b/cmake/catkin/test/gtest.cmake deleted file mode 100644 index 916f98f0c8..0000000000 --- a/cmake/catkin/test/gtest.cmake +++ /dev/null @@ -1,139 +0,0 @@ -_generate_function_if_testing_is_disabled("catkin_add_gtest") - -# -# Add a GTest based test target. -# -# An executable target is created with the source files, it is linked -# against GTest and added to the set of unit tests. -# -# .. note:: The test can be executed by calling the binary directly -# or using: ``make run_tests_${PROJECT_NAME}_gtest_${target}`` -# -# :param target: the target name -# :type target: string -# :param source_files: a list of source files used to build the test -# executable -# :type source_files: list of strings -# :param TIMEOUT: currently not supported -# :type TIMEOUT: integer -# :param WORKING_DIRECTORY: the working directory when executing the -# executable -# :type WORKING_DIRECTORY: string -# -# @public -# -function(catkin_add_gtest target) - _warn_if_skip_testing("catkin_add_gtest") - - if(NOT GTEST_FOUND AND NOT GTEST_FROM_SOURCE_FOUND) - message(WARNING "skipping gtest '${target}' in project '${PROJECT_NAME}'") - return() - endif() - - if(NOT DEFINED CMAKE_RUNTIME_OUTPUT_DIRECTORY) - message(FATAL_ERROR "catkin_add_gtest() must be called after catkin_package() so that default output directories for the test binaries are defined") - endif() - - # XXX look for optional TIMEOUT argument, #2645 - cmake_parse_arguments(_gtest "" "TIMEOUT;WORKING_DIRECTORY" "" ${ARGN}) - if(_gtest_TIMEOUT) - message(WARNING "TIMEOUT argument to catkin_add_gtest() is ignored") - endif() - - # create the executable, with basic + gtest build flags - include_directories(${GTEST_INCLUDE_DIRS}) - link_directories(${GTEST_LIBRARY_DIRS}) - add_executable(${target} EXCLUDE_FROM_ALL ${_gtest_UNPARSED_ARGUMENTS}) - assert(GTEST_LIBRARIES) - target_link_libraries(${target} ${GTEST_LIBRARIES} ${THREADS_LIBRARY}) - - # make sure gtest is built before the test target - add_dependencies(${target} gtest gtest_main) - # make sure the target is built before running tests - add_dependencies(tests ${target}) - - # XXX we DONT use rosunit to call the executable to get process control, #1629, #3112 - get_target_property(_target_path ${target} RUNTIME_OUTPUT_DIRECTORY) - set(cmd "${_target_path}/${target} --gtest_output=xml:${CATKIN_TEST_RESULTS_DIR}/${PROJECT_NAME}/gtest-${target}.xml") - catkin_run_tests_target("gtest" ${target} "gtest-${target}.xml" COMMAND ${cmd} DEPENDENCIES ${target} WORKING_DIRECTORY ${_gtest_WORKING_DIRECTORY}) -endfunction() - -find_package(GTest QUIET) -if(NOT GTEST_FOUND) - # only add gtest directory once per workspace - if(NOT TARGET gtest) - # fall back to system installed path (i.e. on Ubuntu) - set(_paths "/usr/src/gtest/src") - if(CATKIN_TOPLEVEL) - # search in the current workspace before - list(INSERT _paths 0 "${CMAKE_SOURCE_DIR}/gtest/src") - endif() - find_file(_CATKIN_GTEST_SRC "gtest.cc" - PATHS ${_paths} - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) - - # fall back to system installed path (i.e. on Ubuntu) - set(_paths "/usr/include/gtest") - if(CATKIN_TOPLEVEL) - # search in the current workspace before - list(INSERT _paths 0 "${CMAKE_SOURCE_DIR}/gtest/include/gtest") - endif() - find_file(_CATKIN_GTEST_INCLUDE "gtest.h" - PATHS ${_paths} - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) - - if(_CATKIN_GTEST_SRC) - get_filename_component(_CATKIN_GTEST_SOURCE_DIR ${_CATKIN_GTEST_SRC} PATH) - get_filename_component(_CATKIN_GTEST_BASE_DIR ${_CATKIN_GTEST_SOURCE_DIR} PATH) - # add CMakeLists.txt from gtest dir - set(_CATKIN_GTEST_BINARY_DIR ${CMAKE_BINARY_DIR}/gtest) - add_subdirectory(${_CATKIN_GTEST_BASE_DIR} ${_CATKIN_GTEST_BINARY_DIR}) - # mark gtest targets with EXCLUDE_FROM_ALL to only build when tests are built which depend on them - set_target_properties(gtest gtest_main PROPERTIES EXCLUDE_FROM_ALL 1) - get_filename_component(_CATKIN_GTEST_INCLUDE_DIR ${_CATKIN_GTEST_INCLUDE} PATH) - get_filename_component(_CATKIN_GTEST_INCLUDE_DIR ${_CATKIN_GTEST_INCLUDE_DIR} PATH) - # set from-source variables - set(GTEST_FROM_SOURCE_FOUND TRUE CACHE INTERNAL "") - set(GTEST_FROM_SOURCE_INCLUDE_DIRS ${_CATKIN_GTEST_INCLUDE_DIR} CACHE INTERNAL "") - set(GTEST_FROM_SOURCE_LIBRARY_DIRS ${_CATKIN_GTEST_BINARY_DIR} CACHE INTERNAL "") - set(GTEST_FROM_SOURCE_LIBRARIES "gtest" CACHE INTERNAL "") - set(GTEST_FROM_SOURCE_MAIN_LIBRARIES "gtest_main" CACHE INTERNAL "") - #message(STATUS "Found gtest sources under '${_CATKIN_GTEST_BASE_DIR}': gtests will be built") - endif() - if(NOT GTEST_FROM_SOURCE_FOUND) - if(CATKIN_TOPLEVEL) - message(STATUS "gtest not found, C++ tests can not be built. Please install the gtest headers globally in your system or checkout gtest (by running 'svn checkout http://googletest.googlecode.com/svn/tags/release-1.6.0 gtest' in the source space '${CMAKE_SOURCE_DIR}' of your workspace) to enable gtests") - else() - message(STATUS "gtest not found, C++ tests can not be built. Please install the gtest headers globally in your system to enable gtests") - endif() - endif() - endif() - if(GTEST_FROM_SOURCE_FOUND) - # set the same variables as find_package() - # do NOT set GTEST_FOUND in the cache since when using gtest from source - # we must always add the subdirectory to have the gtest targets defined - set(GTEST_FOUND ${GTEST_FROM_SOURCE_FOUND}) - set(GTEST_INCLUDE_DIRS ${GTEST_FROM_SOURCE_INCLUDE_DIRS}) - set(GTEST_LIBRARY_DIRS ${GTEST_FROM_SOURCE_LIBRARY_DIRS}) - set(GTEST_LIBRARIES ${GTEST_FROM_SOURCE_LIBRARIES}) - set(GTEST_MAIN_LIBRARIES ${GTEST_FROM_SOURCE_MAIN_LIBRARIES}) - set(GTEST_BOTH_LIBRARIES ${GTEST_LIBRARIES} ${GTEST_MAIN_LIBRARIES}) - endif() -else() - #message(STATUS "Found gtest: gtests will be built") - add_library(gtest SHARED IMPORTED) - set_target_properties(gtest PROPERTIES IMPORTED_LOCATION "${GTEST_LIBRARIES}") - add_library(gtest_main SHARED IMPORTED) - set_target_properties(gtest_main PROPERTIES IMPORTED_LOCATION "${GTEST_MAIN_LIBRARIES}") - set(GTEST_FOUND ${GTEST_FOUND} CACHE INTERNAL "") - set(GTEST_INCLUDE_DIRS ${GTEST_INCLUDE_DIRS} CACHE INTERNAL "") - set(GTEST_LIBRARIES ${GTEST_LIBRARIES} CACHE INTERNAL "") - set(GTEST_MAIN_LIBRARIES ${GTEST_MAIN_LIBRARIES} CACHE INTERNAL "") - set(GTEST_BOTH_LIBRARIES ${GTEST_BOTH_LIBRARIES} CACHE INTERNAL "") -endif() -# For Visual C++, need to increase variadic template size to build gtest -if(GTEST_FOUND) - if(WIN32) - add_definitions(/D _VARIADIC_MAX=10) - endif() -endif() diff --git a/cmake/catkin/test/nosetests.cmake b/cmake/catkin/test/nosetests.cmake deleted file mode 100644 index 874e46115f..0000000000 --- a/cmake/catkin/test/nosetests.cmake +++ /dev/null @@ -1,117 +0,0 @@ -_generate_function_if_testing_is_disabled("catkin_add_nosetests") - -# -# Add Python nose tests. -# -# Nose collects tests from the directory ``dir`` automatically. -# -# .. note:: The test can be executed by calling ``nosetests`` -# directly or using: -# `` make run_tests_${PROJECT_NAME}_nosetests_${dir}`` -# (where slashes in the ``dir`` are replaced with underscores) -# -# :param path: a relative or absolute directory to search for -# nosetests in or a relative or absolute file containing tests -# :type path: string -# :param DEPENDENCIES: the targets which must be built before executing -# the test -# :type DEPENDENCIES: list of strings -# :param TIMEOUT: the timeout for individual tests in seconds -# (default: 60) -# :type TIMEOUT: integer -# :param WORKING_DIRECTORY: the working directory when executing the -# tests -# :type WORKING_DIRECTORY: string -# -# @public -# -function(catkin_add_nosetests path) - _warn_if_skip_testing("catkin_add_nosetests") - - if(NOT NOSETESTS) - message(STATUS "skipping nosetests(${path}) in project '${PROJECT_NAME}'") - return() - endif() - - cmake_parse_arguments(_nose "" "TIMEOUT;WORKING_DIRECTORY" "DEPENDENCIES" ${ARGN}) - if(NOT _nose_TIMEOUT) - set(_nose_TIMEOUT 60) - endif() - if(NOT _nose_TIMEOUT GREATER 0) - message(FATAL_ERROR "nosetests() TIMEOUT argument must be a valid number of seconds greater than zero") - endif() - - # check that the directory exists - set(_path_name _path_name-NOTFOUND) - if(IS_ABSOLUTE ${path}) - set(_path_name ${path}) - else() - find_file(_path_name ${path} - PATHS ${CMAKE_CURRENT_SOURCE_DIR} - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) - if(NOT _path_name) - message(FATAL_ERROR "Can't find nosetests path '${path}'") - endif() - endif() - - # check if coverage reports are being requested - if("$ENV{CATKIN_TEST_COVERAGE}" STREQUAL "1") - set(_covarg " --with-coverage") - endif() - - # strip PROJECT_SOURCE_DIR and PROJECT_BINARY_DIR prefix from output_file_name - set(output_file_name ${path}) - _strip_path_prefix(output_file_name "${output_file_name}" "${PROJECT_SOURCE_DIR}") - _strip_path_prefix(output_file_name "${output_file_name}" "${PROJECT_BINARY_DIR}") - if("${output_file_name}" STREQUAL "") - set(output_file_name ".") - endif() - string(REPLACE "/" "." output_file_name ${output_file_name}) - string(REPLACE ":" "." output_file_name ${output_file_name}) - - set(output_path ${CATKIN_TEST_RESULTS_DIR}/${PROJECT_NAME}) - # make --xunit-file argument an absolute path (https://github.com/nose-devs/nose/issues/779) - get_filename_component(output_path "${output_path}" ABSOLUTE) - set(cmd "${CMAKE_COMMAND} -E make_directory ${output_path}") - if(IS_DIRECTORY ${_path_name}) - set(tests "--where=${_path_name}") - else() - set(tests "${_path_name}") - endif() - set(cmd ${cmd} "${NOSETESTS} -P --process-timeout=${_nose_TIMEOUT} ${tests} --with-xunit --xunit-file=${output_path}/nosetests-${output_file_name}.xml${_covarg}") - catkin_run_tests_target("nosetests" ${output_file_name} "nosetests-${output_file_name}.xml" COMMAND ${cmd} DEPENDENCIES ${_nose_DEPENDENCIES} WORKING_DIRECTORY ${_nose_WORKING_DIRECTORY}) -endfunction() - -find_program(NOSETESTS NAMES - "nosetests${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" - "nosetests-${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" - "nosetests${PYTHON_VERSION_MAJOR}" - "nosetests-${PYTHON_VERSION_MAJOR}" - "nosetests") -if(NOT NOSETESTS) - if("${PYTHON_VERSION_MAJOR}" STREQUAL "3") - message(WARNING "nosetests not found, Python tests can not be run (try installing package 'python3-nose')") - else() - message(WARNING "nosetests not found, Python tests can not be run (try installing package 'python-nose')") - endif() -endif() - -macro(_strip_path_prefix var value prefix) - if("${value}" STREQUAL "${prefix}" OR "${value}" STREQUAL "${prefix}/") - set(${var} "") - else() - set(${var} "${value}") - string(LENGTH "${prefix}/" prefix_length) - string(LENGTH "${value}" var_length) - if(${var_length} GREATER ${prefix_length}) - string(SUBSTRING "${value}" 0 ${prefix_length} var_prefix) - if("${var_prefix}" STREQUAL "${prefix}/") - # passing length -1 does not work for CMake < 2.8.5 - # http://public.kitware.com/Bug/view.php?id=10740 - string(LENGTH "${value}" _rest) - math(EXPR _rest "${_rest} - ${prefix_length}") - string(SUBSTRING "${value}" ${prefix_length} ${_rest} ${var}) - endif() - endif() - endif() -endmacro() diff --git a/cmake/catkin/test/run_tests.py b/cmake/catkin/test/run_tests.py deleted file mode 100755 index 9b93d55e2b..0000000000 --- a/cmake/catkin/test/run_tests.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function - -import argparse -import os -import sys -import subprocess - -from catkin.test_results import ensure_junit_result_exist, remove_junit_result - - -def main(argv=sys.argv[1:]): - parser = argparse.ArgumentParser(description='Runs the test command passed as an argument and verifies that the expected result file has been generated.') - parser.add_argument('results', help='The path to the xunit result file') - parser.add_argument('command', nargs='+', help='The test command to execute') - parser.add_argument('--working-dir', nargs='?', help='The working directory for the executed command') - parser.add_argument('--return-code', action='store_true', default=False, help='Set the return code based on the success of the test command') - args = parser.parse_args(argv) - - remove_junit_result(args.results) - - work_dir_msg = ' with working directory "%s"' % args.working_dir if args.working_dir is not None else '' - cmds_msg = ''.join(['\n %s' % cmd for cmd in args.command]) - print('-- run_tests.py: execute commands%s%s' % (work_dir_msg, cmds_msg)) - - rc = 0 - for cmd in args.command: - rc = subprocess.call(cmd, cwd=args.working_dir, shell=True) - if rc: - break - - print('-- run_tests.py: verify result "%s"' % args.results) - no_errors = ensure_junit_result_exist(args.results) - if not no_errors: - rc = 1 - - if args.return_code: - return rc - return 0 - - -if __name__ == '__main__': - sys.exit(main()) diff --git a/cmake/catkin/test/tests.cmake b/cmake/catkin/test/tests.cmake deleted file mode 100644 index 53ef20f40e..0000000000 --- a/cmake/catkin/test/tests.cmake +++ /dev/null @@ -1,144 +0,0 @@ -option(CATKIN_ENABLE_TESTING "Catkin enable testing" ON) -option(CATKIN_SKIP_TESTING "Catkin skip testing" OFF) - -# check if testing is explicity skipped -if(CATKIN_SKIP_TESTING) - set(CATKIN_ENABLE_TESTING OFF) - #message(STATUS "Using CATKIN_SKIP_TESTING: ${CATKIN_SKIP_TESTING} (implying CATKIN_ENABLE_TESTING=${CATKIN_ENABLE_TESTING})") -else() - #message(STATUS "Using CATKIN_ENABLE_TESTING: ${CATKIN_ENABLE_TESTING}") -endif() - -# creates a dummy function in case testing has been explicitly disabled (and not only skipping) -# which outputs an error message when being invoked -macro(_generate_function_if_testing_is_disabled funcname) - if(DEFINED CATKIN_ENABLE_TESTING AND NOT CATKIN_ENABLE_TESTING AND NOT CATKIN_SKIP_TESTING) - function(${funcname}) - message(FATAL_ERROR - "${funcname}() is not available when tests are not enabled. The CMake code should only use it inside a conditional block which checks that testing is enabled:\n" - "if(CATKIN_ENABLE_TESTING)\n" - " ${funcname}(...)\n" - "endif()\n") - endfunction() - return() - endif() -endmacro() - -# checks if a function has been called while testing is skipped -# and outputs a warning message -macro(_warn_if_skip_testing funcname) - if(DEFINED CATKIN_ENABLE_TESTING AND NOT CATKIN_ENABLE_TESTING) - message(WARNING - "${funcname}() should only be used inside a conditional block which checks that testing is enabled:\n" - "if(CATKIN_ENABLE_TESTING)\n" - " ${funcname}(...)\n" - "endif()\n") - endif() -endmacro() - -if(DEFINED CATKIN_ENABLE_TESTING AND NOT CATKIN_ENABLE_TESTING AND NOT CATKIN_SKIP_TESTING) - return() -endif() - -# do not enable ctest's on the farm, since they are automatically executed by the current rules files -# and since the tests have not been build rostests would hang forever -if(NOT CATKIN_BUILD_BINARY_PACKAGE) - # do not enable ctest's for dry packages, since they have a custom test target which must not be overwritten - if(NOT ROSBUILD_init_called) - #message(STATUS "Call enable_testing()") - enable_testing() - else() - message(STATUS "Skip enable_testing() for dry packages") - endif() -else() - message(STATUS "Skip enable_testing() when building binary package") -endif() - -# allow overriding CATKIN_TEST_RESULTS_DIR when explicitly passed to CMake as a command line argument -if(DEFINED CATKIN_TEST_RESULTS_DIR) - set(CATKIN_TEST_RESULTS_DIR ${CATKIN_TEST_RESULTS_DIR} CACHE INTERNAL "") -else() - set(CATKIN_TEST_RESULTS_DIR ${CMAKE_BINARY_DIR}/test_results CACHE INTERNAL "") -endif() -#message(STATUS "Using CATKIN_TEST_RESULTS_DIR: ${CATKIN_TEST_RESULTS_DIR}") -file(MAKE_DIRECTORY ${CATKIN_TEST_RESULTS_DIR}) - -# create target to build tests -if(NOT TARGET tests) - add_custom_target(tests) -endif() - -# create target to run all tests -# it uses the dot-prefixed test targets to depend on building all tests and cleaning test results before the tests are executed -if(NOT TARGET run_tests) - add_custom_target(run_tests) -endif() - -# create target to clean test results -if(NOT TARGET clean_test_results) - add_custom_target(clean_test_results - COMMAND ${CMAKE_COMMAND} -E remove_directory ${CATKIN_TEST_RESULTS_DIR}) -endif() - -# -# Create a test target, integrate it with the run_tests infrastructure -# and post-process the junit result. -# -# All test results go under ${CATKIN_TEST_RESULTS_DIR}/${PROJECT_NAME}/.. -# -# This function is only used internally by the various -# catkin_add_*test() functions. -# -function(catkin_run_tests_target type name xunit_filename) - cmake_parse_arguments(_testing "" "WORKING_DIRECTORY" "COMMAND;DEPENDENCIES" ${ARGN}) - if(_testing_UNPARSED_ARGUMENTS) - message(FATAL_ERROR "catkin_run_tests_target() called with unused arguments: ${_testing_UNPARSED_ARGUMENTS}") - endif() - - # create meta target to trigger all tests of a project - if(NOT TARGET run_tests_${PROJECT_NAME}) - add_custom_target(run_tests_${PROJECT_NAME}) - # create hidden meta target which depends on hidden test targets which depend on clean_test_results - add_custom_target(_run_tests_${PROJECT_NAME}) - # run_tests depends on this hidden target hierarchy to clear test results before running all tests - add_dependencies(run_tests _run_tests_${PROJECT_NAME}) - endif() - # create meta target to trigger all tests of a specific type of a project - if(NOT TARGET run_tests_${PROJECT_NAME}_${type}) - add_custom_target(run_tests_${PROJECT_NAME}_${type}) - add_dependencies(run_tests_${PROJECT_NAME} run_tests_${PROJECT_NAME}_${type}) - # hidden meta target which depends on hidden test targets which depend on clean_test_results - add_custom_target(_run_tests_${PROJECT_NAME}_${type}) - add_dependencies(_run_tests_${PROJECT_NAME} _run_tests_${PROJECT_NAME}_${type}) - endif() - if(NOT DEFINED CATKIN_ENABLE_TESTING OR CATKIN_ENABLE_TESTING) - # create target for test execution - set(results ${CATKIN_TEST_RESULTS_DIR}/${PROJECT_NAME}/${xunit_filename}) - if (_testing_WORKING_DIRECTORY) - set(working_dir_arg "--working-dir" ${_testing_WORKING_DIRECTORY}) - endif() - assert(CATKIN_ENV) - set(cmd_wrapper ${CATKIN_ENV} ${PYTHON_EXECUTABLE} - ${catkin_EXTRAS_DIR}/test/run_tests.py ${results} ${working_dir_arg}) - # for ctest the command needs to return non-zero if any test failed - set(cmd ${cmd_wrapper} "--return-code" ${_testing_COMMAND}) - add_test(NAME _ctest_${PROJECT_NAME}_${type}_${name} COMMAND ${cmd}) - # for the run_tests target the command needs to return zero so that testing is not aborted - set(cmd ${cmd_wrapper} ${_testing_COMMAND}) - add_custom_target(run_tests_${PROJECT_NAME}_${type}_${name} - COMMAND ${cmd}) - else() - # create empty dummy target - set(cmd "${CMAKE_COMMAND}" "-E" "echo" "Skipping test target \\'run_tests_${PROJECT_NAME}_${type}_${name}\\'. Enable testing via -DCATKIN_ENABLE_TESTING.") - add_custom_target(run_tests_${PROJECT_NAME}_${type}_${name} ${cmd}) - endif() - add_dependencies(run_tests_${PROJECT_NAME}_${type} run_tests_${PROJECT_NAME}_${type}_${name}) - if(_testing_DEPENDENCIES) - add_dependencies(run_tests_${PROJECT_NAME}_${type}_${name} ${_testing_DEPENDENCIES}) - endif() - # hidden test target which depends on building all tests and cleaning test results - add_custom_target(_run_tests_${PROJECT_NAME}_${type}_${name} - COMMAND ${cmd}) - add_dependencies(_run_tests_${PROJECT_NAME}_${type} _run_tests_${PROJECT_NAME}_${type}_${name}) - add_dependencies(_run_tests_${PROJECT_NAME}_${type}_${name} clean_test_results tests ${_testing_DEPENDENCIES}) -endfunction() diff --git a/cmake/catkin/tools/bz2.cmake b/cmake/catkin/tools/bz2.cmake deleted file mode 100644 index d5f129d2a3..0000000000 --- a/cmake/catkin/tools/bz2.cmake +++ /dev/null @@ -1,31 +0,0 @@ -# -# Copyright (c) 2011, Willow Garage, 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: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# - - -find_package(Bzip2) - diff --git a/cmake/catkin/tools/doxygen.cmake b/cmake/catkin/tools/doxygen.cmake deleted file mode 100644 index e8ed5d721f..0000000000 --- a/cmake/catkin/tools/doxygen.cmake +++ /dev/null @@ -1,64 +0,0 @@ -# -# Copyright (c) 2011, Willow Garage, 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: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# - -## -# doxygen( ) -# TARGET_NAME -> The cmake target to create. -# SEARCH_DIRS -> a CMake List of directories to search for doxygenated files. -# -find_program(DOXYGEN_EXECUTABLE doxygen) - -if (DOXYGEN_EXECUTABLE) - set(DOXYGEN_FOUND TRUE CACHE BOOL "Doxygen found") -endif() - -if(NOT TARGET doxygen) - add_custom_target(doxygen) -endif() - -macro(catkin_doxygen TARGET_NAME SEARCH_DIRS) - foreach(dir ${SEARCH_DIRS}) - file(GLOB_RECURSE _doc_sources ${dir}/*) - list(APPEND doc_sources ${_doc_sources}) - endforeach() - - string(REPLACE ";" " " doc_sources "${doc_sources}") - - configure_file(${catkin_EXTRAS_DIR}/templates/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY) - - add_custom_target(${TARGET_NAME} - COMMENT "Generating API documentation with Doxygen" VERBATIM - ) - - add_custom_command(TARGET ${TARGET_NAME} - COMMAND ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} - ) - add_dependencies(doxygen ${TARGET_NAME}) - -endmacro() diff --git a/cmake/catkin/tools/libraries.cmake b/cmake/catkin/tools/libraries.cmake deleted file mode 100644 index 77e38d680e..0000000000 --- a/cmake/catkin/tools/libraries.cmake +++ /dev/null @@ -1,22 +0,0 @@ -# BUILD_SHARED_LIBS is a global cmake variable (usually defaults to on) -# that determines the build type of libraries: -# http://www.cmake.org/cmake/help/cmake-2-8-docs.html#variable:BUILD_SHARED_LIBS -# It defaults to shared. -# -# Our only current major use case for static libraries is -# via the mingw cross compiler, though embedded builds -# could be feasibly built this way also (largely untested). - -# Make sure this is already defined as a cached variable (@sa platform/windows.cmake) -if(NOT DEFINED BUILD_SHARED_LIBS) - option(BUILD_SHARED_LIBS "Build dynamically-linked binaries" ON) -endif() - -function(configure_shared_library_build_settings) - if(BUILD_SHARED_LIBS) - message(STATUS "BUILD_SHARED_LIBS is on") - add_definitions(-DROS_BUILD_SHARED_LIBS=1) - else() - message(STATUS "BUILD_SHARED_LIBS is off") - endif() -endfunction() diff --git a/cmake/catkin/tools/rt.cmake b/cmake/catkin/tools/rt.cmake deleted file mode 100644 index 3e5b708d8f..0000000000 --- a/cmake/catkin/tools/rt.cmake +++ /dev/null @@ -1,45 +0,0 @@ -# -# Copyright (c) 2011, Willow Garage, 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: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (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("CMAKE_LIBRARY_PATH: ${CMAKE_LIBRARY_PATH}") -# message("CMAKE_LIBRARY_ARCHITECTURE: ${CMAKE_LIBRARY_ARCHITECTURE}") -# message("CMAKE_SYSTEM_LIBRARY_PATH: ${CMAKE_SYSTEM_LIBRARY_PATH}") -# message("CMAKE_VERSION=${CMAKE_VERSION}") - -if(NOT (APPLE OR WIN32 OR MINGW OR ANDROID)) - if (${CMAKE_VERSION} VERSION_LESS 2.8.4) - # cmake later than 2.8.0 appears to have a better find_library - # that knows about the ABI of the compiler. For lucid we just - # depend on the linker to find it for us. - set(RT_LIBRARY rt CACHE FILEPATH "Hacked find of rt for cmake < 2.8.4") - else() - find_library(RT_LIBRARY rt) - assert_file_exists(${RT_LIBRARY} "RT Library") - endif() - #message(STATUS "RT_LIBRARY: ${RT_LIBRARY}") -endif() diff --git a/cmake/catkin/tools/threads.cmake b/cmake/catkin/tools/threads.cmake deleted file mode 100644 index 8087ad6fa1..0000000000 --- a/cmake/catkin/tools/threads.cmake +++ /dev/null @@ -1,31 +0,0 @@ -# -# Copyright (c) 2011, Willow Garage, 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: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# - - -find_package(Threads) -set(THREADS_LIBRARY ${CMAKE_THREAD_LIBS_INIT} CACHE FILEPATH "Threads library") diff --git a/cmake/catkin/toplevel.cmake b/cmake/catkin/toplevel.cmake deleted file mode 100644 index 2978ef0654..0000000000 --- a/cmake/catkin/toplevel.cmake +++ /dev/null @@ -1,63 +0,0 @@ -# toplevel CMakeLists.txt for a catkin workspace -# catkin/cmake/toplevel.cmake - -cmake_minimum_required(VERSION 2.8.3) - -set(CATKIN_TOPLEVEL TRUE) - -# search for catkin within the workspace -set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") -execute_process(COMMAND ${_cmd} - RESULT_VARIABLE _res - OUTPUT_VARIABLE _out - ERROR_VARIABLE _err - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_STRIP_TRAILING_WHITESPACE -) -if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) - # searching fot catkin resulted in an error - string(REPLACE ";" " " _cmd_str "${_cmd}") - message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") -endif() - -# include catkin from workspace or via find_package() -if(_res EQUAL 0) - set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") - # include all.cmake without add_subdirectory to let it operate in same scope - include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) - add_subdirectory("${_out}") - -else() - # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument - # or CMAKE_PREFIX_PATH from the environment - if(NOT DEFINED CMAKE_PREFIX_PATH) - if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") - string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) - endif() - endif() - - # list of catkin workspaces - set(catkin_search_path "") - foreach(path ${CMAKE_PREFIX_PATH}) - if(EXISTS "${path}/.catkin") - list(FIND catkin_search_path ${path} _index) - if(_index EQUAL -1) - list(APPEND catkin_search_path ${path}) - endif() - endif() - endforeach() - - # search for catkin in all workspaces - set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) - find_package(catkin QUIET - NO_POLICY_SCOPE - PATHS ${catkin_search_path} - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) - unset(CATKIN_TOPLEVEL_FIND_PACKAGE) - - if(NOT catkin_FOUND) - message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") - endif() -endif() - -catkin_workspace() diff --git a/cmake/catkin2Config.cmake b/cmake/catkin2Config.cmake index f1212b16f3..fd585f9ebf 100644 --- a/cmake/catkin2Config.cmake +++ b/cmake/catkin2Config.cmake @@ -1,14 +1,14 @@ # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -27,8 +27,9 @@ # :outvar _INCLUDE_DIRS/_LIBRARY_DIRS/_LIBRARY: # contains the include dirs / library dirs / libraries of the searched component . +find_package(catkin REQUIRED) # defines catkin_DIR if(CATKIN_TOPLEVEL_FIND_PACKAGE OR NOT CATKIN_TOPLEVEL) - set(catkin_EXTRAS_DIR ${CMAKE_CURRENT_LIST_DIR}/catkin) + set(catkin_EXTRAS_DIR "${catkin_DIR}") # prevent multiple inclusion from repeated find_package() calls in non-workspace context # as long as this variable is in the scope the variables from all.cmake are also, so no need to be evaluated again @@ -92,15 +93,24 @@ if(catkin2_FIND_COMPONENTS) endforeach() # find package component - if(catkin2_FIND_REQUIRED) - find_package(${component} REQUIRED NO_MODULE PATHS ${paths} - NO_DEFAULT_PATH) - elseif(catkin2_FIND_QUIETLY) + if(catkin_FIND_REQUIRED) + # try without REQUIRED first + find_package(${component} NO_MODULE PATHS ${paths} + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + if(NOT ${component}_FOUND) + # show better message to help users with the CMake error message coming up + message(STATUS "Could not find the required component '${component}'. " + "The following CMake error indicates that you either need to install the package " + "with the same name or change your environment so that it can be found.") + find_package(${component} REQUIRED NO_MODULE PATHS ${paths} + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + endif() + elseif(catkin_FIND_QUIETLY) find_package(${component} QUIET NO_MODULE PATHS ${paths} - NO_DEFAULT_PATH) + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) else() find_package(${component} NO_MODULE PATHS ${paths} - NO_DEFAULT_PATH) + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) endif() # ROS Packages give their library packages via hard coded @@ -112,7 +122,7 @@ if(catkin2_FIND_COMPONENTS) foreach(library ${${component}_LIBRARIES}) string(REGEX REPLACE "^/usr/lib" "${ARM_CHROOT_DIR}/usr/lib" library ${library}) string(REPLACE "i386-linux-gnu" "arm-linux-gnueabihf" library ${library}) - string(REGEX REPLACE "^/opt/ros/kinetic" "${ARM_CHROOT_DIR}/opt/ros/kinetic" library ${library}) + string(REGEX REPLACE "^/opt/ros/${ARM_ROS_DISTRO}" "${ARM_CHROOT_DIR}/opt/ros/${ARM_ROS_DISTRO}" library ${library}) list(APPEND temp_LIBRARIES ${library}) endforeach() set(${component}_LIBRARIES ${temp_LIBRARIES}) diff --git a/cmake/package.xml b/cmake/package.xml deleted file mode 100644 index c0bf80b0d7..0000000000 --- a/cmake/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - catkin - 0.6.11 - Low-level build system macros and infrastructure for ROS. - Dirk Thomas - BSD - - http://www.ros.org/wiki/catkin - https://github.com/ros/catkin/issues - https://github.com/ros/catkin - - Troy Straszheim - Morten Kjaergaard - Brian Gerkey - Dirk Thomas - - cmake - cmake - - python-argparse - python-catkin-pkg - - python-empy - - gtest - python-empy - python-nose - - python-mock - python-nose - - - - - - diff --git a/communications/CMakeLists.txt b/communications/CMakeLists.txt deleted file mode 100644 index d8162a363c..0000000000 --- a/communications/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -add_subdirectory(ff_msgs) -add_subdirectory(ff_hw_msgs) - -if (USE_DDS) - add_subdirectory(dds_msgs) - add_subdirectory(dds_ros_bridge) -endif (USE_DDS) diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt new file mode 100644 index 0000000000..a9f6aaf995 --- /dev/null +++ b/communications/comms_bridge/CMakeLists.txt @@ -0,0 +1,141 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + + +cmake_minimum_required(VERSION 3.0) +project(comms_bridge) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +if (USE_DDS) +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + config_reader + ff_util + ff_msgs + topic_tools + dds_msgs +) + + +# System dependencies are found with CMake's conventions +# find_package(Eigen3 REQUIRED) +find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options timer) + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") + +if (USE_CTC) + set(SORACORE_ROOT_DIR ${ARM_CHROOT_DIR}/usr) +else (USE_CTC) + set(SORACORE_ROOT_DIR /usr) +endif (USE_CTC) + +set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) + +SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# find Qt version according to OS +find_program(LSB_RELEASE_EXEC lsb_release) +execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) +else () + find_package(Qt4 4.6.0 REQUIRED QtXml) +endif () + +find_package(Miro REQUIRED) +find_package(RtiDds REQUIRED) +find_package(Soracore REQUIRED) + +catkin_package( + LIBRARIES comms_bridge + CATKIN_DEPENDS message_runtime std_msgs nodelet config_reader ff_util ff_msgs dds_msgs +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${RTIDDS_INCLUDE_DIR} + ${SORACORE_INCLUDE_DIRS} + ${MIRO_INCLUDE_DIR} + ${QT_INCLUDE_DIR} + ${QT_INCLUDE_DIR}/Qt + ${Boost_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cpp" +) + +# Declare C++ libraries +add_library(comms_bridge + ${cc_files} +) +target_compile_definitions(comms_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) +add_dependencies(comms_bridge ${catkin_EXPORTED_TARGETS}) +target_link_libraries(comms_bridge rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + + +#add_library(comms_bridge_pub +# src/bridge_publisher.cpp +# src/dds_ros_bridge_publisher.cpp +# src/bridge_publisher_nodelet.cpp +#) +#add_dependencies(comms_bridge_pub ${catkin_EXPORTED_TARGETS} ) +#target_link_libraries(comms_bridge_pub +# rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) + + +############# +## Install ## +############# +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + +else (USE_DDS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_DDS) diff --git a/communications/comms_bridge/include/comms_bridge/bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/bridge_publisher.h new file mode 100644 index 0000000000..d8a4202697 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/bridge_publisher.h @@ -0,0 +1,138 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ +#define COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ + +/* + Virtual base class for the subscriber (input side) of a generic ROS bridge + Actual implementation depends on an inheriting class + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef std::shared_ptr SubscriberPtr; +typedef std::shared_ptr PublisherPtr; +typedef std::chrono::time_point timepoint_t; + +class BridgePublisher { + public: + virtual ~BridgePublisher(); + + // 0 => none, > 0 => progressively more + void setVerbosity(unsigned int verbosity); + + protected: + // a yet-unknown message type will have null string values + class AdvertisementInfo { + public: + // whehter republisher should advertise this as a latching ROS topic + bool latching; + + // ROS message data type name, eg "std_msgs/Int32" + std::string data_type; + + // ROS message md5sum of type, eg "da5909fbe378aeaf85e547e830cc1bb7" + std::string md5_sum; + + // ROS message definition, eg as one finds in a .msg definition file + std::string definition; + }; + + class ContentInfo { + public: + // the md5sum from the original advertisement + std::string type_md5_sum; + + // opaque serialized message data + std::vector data; + }; + + class RelayTopicInfo { + public: + RelayTopicInfo() : out_topic(""), advertised(false), relay_seqnum(0) {} + + ros::Publisher publisher; // our subscriber on this input topic + std::string out_topic; // topic name to republish on + bool advertised; // whether we have sent type info to other side + unsigned int relay_seqnum; // counter of messages relayed on this topic + + // Will be filled in only on receipt of advertisement from our + // bridger subscriber counterpart + AdvertisementInfo ad_info; + + std::queue + waiting_msgs; // unpublished messages because type yet unknown or we're still delaying after advertising + timepoint_t delay_until; // time to wait until relaying any data, after having advertised + }; + + /**************************************************************************/ + + // This base class should only be extended, not itself instantiated + explicit BridgePublisher(double ad2pub_delay); + + // Should be called by implementing class when it has received a topic's + // AdvertisementInfo + // Must be called with mutex held + bool advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info); + + // Should be called by an implementing class when it receives a serialized + // message over its particular conduit + // Must be called with mutex held + bool relayMessage(RelayTopicInfo& topic_info, const ContentInfo& content_info); + + // worker thread spawned at construction + void drainThread(); + + // does actual work of publishing a message (eg called by relayMessage) + // Must be called with mutex held + bool relayContent(RelayTopicInfo& topic_info, const ContentInfo& content_info); + + // Publishes messages that were enqueued for a given topic + // Must be called with mutex held + void drainWaitingQueue(RelayTopicInfo& topic_info); + void drainWaitingQueue(std::string const& output_topic); + + unsigned int m_verbose_; + + double m_ad2pub_delay_; // seconds + + std::mutex m_mutex_; // serializes access to below data structures + std::shared_ptr worker_thread_; + std::map m_relay_topics_; // keyed by input topic + std::priority_queue> m_drain_queue_; + std::condition_variable m_drain_cv_; + // stats: + unsigned int m_n_relayed_; +}; + +#endif // COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h new file mode 100644 index 0000000000..158d85376a --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h @@ -0,0 +1,129 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ +#define COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ + +/* + Virtual base class for the subscriber (input side) of a generic ROS bridge + Actual implementation depends on an inheriting class + */ + +#include + +#include +#include + +#include +#include +#include +#include + +typedef std::shared_ptr SubscriberPtr; +typedef std::shared_ptr PublisherPtr; + +class BridgeSubscriber { + public: + BridgeSubscriber(); + virtual ~BridgeSubscriber(); + + // Add a relay between this input topic on the subscriber side and this + // output topic on the republisher side + // All relays must have a unique input topic and unique output topic + // Other topologies (merging topics or duplicating topics) should be + // implemented by some other node on top of this one + bool addTopic(std::string const& in_topic, std::string const& out_topic); + + // 0 => none, > 0 => progressively more + void setVerbosity(unsigned int verbosity); + + protected: + class AdvertisementInfo { + public: + // whehter republisher should advertise this as a latching ROS topic + bool latching; + + // ROS message data type name, eg "std_msgs/Int32" + std::string data_type; + + // ROS message md5sum of type, eg "da5909fbe378aeaf85e547e830cc1bb7" + std::string md5_sum; + + // ROS message definition, eg as one finds in a .msg definition file + std::string definition; + }; + + class RelayTopicInfo { + public: + RelayTopicInfo() : out_topic(""), advertised(false), relay_seqnum(0) {} + + SubscriberPtr sub; // our subscriber on this input topic + std::string out_topic; // topic name to republish on + bool advertised; // whether we have sent type info to other side + unsigned int relay_seqnum; // counter of messages relayed on this topic + + // not filled in until advertiseTopic() is called + AdvertisementInfo ad_info; + }; + + class ContentInfo { + public: + // the md5sum from the original advertisement + std::string type_md5_sum; + + // length in bytes of the serialized data + size_t data_size; + + // opaque serialized message data + const uint8_t* data; + }; + + // Called via addTopic() + // Notifies implementation that a bridge between these two topics is needed + // No information about the message type etc. is yet available + // Called with the mutex held + virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) = 0; + + // Called upon receipt of the first message on a subscribed topic + // Notifies implementation to advertise the topic with this type info + // Called with the mutex held + virtual void advertiseTopic(const RelayTopicInfo& info) = 0; + + // Called on receipt of any message on a subscribed topic + // Notifies implementation to relay this message content to the output topic + // Note any pointers within info struct are valid only during this call + // Called with the mutex held + virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) = 0; + + /**************************************************************************/ + + SubscriberPtr rosSubscribe(std::string const& topic); + + void handleRelayedMessage(const ros::MessageEvent& msg_event, + std::string const& topic, SubscriberPtr sub); + + unsigned int m_verbose_; + + std::mutex m_mutex_; // serializes access to below data structures + uint8_t* m_msgbuffer_; // data serialization scratch + std::map m_relay_topics_; // keyed by input topic + // stats: + unsigned int m_n_relayed_; +}; + +#endif // COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h new file mode 100644 index 0000000000..29df797e86 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ + +#include +#include +#include + +#include +#include +#include + +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +// default time to delay between advertisement and publishing on that topic [sec] +#define DEFAULT_ADVERTISE_TO_PUB_DELAY 3.0 + +namespace ff { + +class GenericRapidMsgRosPub : public BridgePublisher { + public: + explicit GenericRapidMsgRosPub(double ad2pub_delay = DEFAULT_ADVERTISE_TO_PUB_DELAY); + virtual ~GenericRapidMsgRosPub(); + + void InitializeDDS(std::vector const& connections, bool enable_advertisement_info_request); + + void ConvertAdvertisementInfo( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data); + void ConvertContent(rapid::ext::astrobee::GenericCommsContent const* data, + std::string const& connecting_robot); + + void RequestAdvertisementInfo(std::string const& output_topic, + std::string const& connecting_robot); + + private: + bool dds_initialized_, enable_advertisement_info_request_; + + std::map robot_connections_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h new file mode 100644 index 0000000000..4aef42a757 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h @@ -0,0 +1,80 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstantsSupport.h" +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +namespace ff { + +class GenericRapidPub { + public: + explicit GenericRapidPub(std::string const& robot_name); + ~GenericRapidPub(); + + template + void CopyString(const int max_size, + std::string src, + T &dest, + std::string const& data_name, + std::string const& topic); + + void SendAdvertisementInfo(std::string const& output_topic, + bool latching, + std::string const& data_type, + std::string const& md5_sum, + std::string definition); + + void SendContent(std::string const& output_topic, + std::string const& md5_sum, + uint8_t const* data, + const size_t data_size, + const int seq_num); + + private: + using AdvertisementInfoSupplier = + kn::DdsTypedSupplier; + using AdvertisementInfoSupplierPtr = + std::unique_ptr; + AdvertisementInfoSupplierPtr advertisement_info_supplier_; + + using ContentSupplier = + kn::DdsTypedSupplier; + using ContentSupplierPtr = std::unique_ptr; + ContentSupplierPtr content_supplier_; + + unsigned int advertisement_info_seq_; +}; + +typedef std::shared_ptr GenericRapidPubPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h new file mode 100644 index 0000000000..52fa3e5f4e --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ + +#include +#include +#include +#include + +#include "ros/ros.h" + +#include "knDds/DdsEventLoop.h" + +#include "knShare/Time.h" + +namespace ff { + +/** + * @brief base class for rapid subscriber to ros publisher + * @details base class for rapid subscriber to ros publisher. + * A kn::DdsEventLoop is run within its own thread of execution. + * Child classes must connect requested message and callback + * to m_ddsEventLoop and call startThread() + */ +class GenericRapidSub { + protected: + GenericRapidSub(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition); + + ~GenericRapidSub(); + + /** + * Will start thread exection by calling threadExec() + */ + virtual void StartThread(); + + std::string subscribe_topic_; + std::string subscriber_partition_; + + std::atomic alive_; + std::thread thread_; + kn::DdsEventLoop dds_event_loop_; + + private: + /** + * Function to execute within seperate thread + * process DdsEventLoop at 10Hz + */ + virtual void ThreadExec(); +}; + +typedef std::shared_ptr GenericRapidSubPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h new file mode 100644 index 0000000000..4f3282cb0d --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h @@ -0,0 +1,124 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ +#define COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ + +/* This is a specialization of ROSBridgeSubscriber using a DDS conduit +*/ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include "dds_msgs/GenericCommsRequestSupport.h" + +namespace ff { + +class TopicEntry { + public: + TopicEntry() : connecting_robot_(""), + out_topic_(""), + rate_seconds_(0), + last_time_pub_(0), + seq_num_(0), + type_md5_sum_(""), + data_size_(0), + data_(NULL) {} + TopicEntry(std::string robot_in, std::string topic_in, double rate_in) : + connecting_robot_(robot_in), + out_topic_(topic_in), + rate_seconds_(rate_in), + last_time_pub_(0), + seq_num_(0), + type_md5_sum_(""), + data_size_(0), + data_(NULL) {} + + void SetDataToSend(const int seq_num, + std::string const& md5_sum, + const size_t data_size, + uint8_t const* data) { + seq_num_ = seq_num; + type_md5_sum_ = md5_sum; + data_size_ = data_size; + data_ = data; + } + + std::string connecting_robot_; + std::string out_topic_; + double rate_seconds_; + + // Info only needed for rate messages. A little wasted space but quicker + // program execution + double last_time_pub_; + int seq_num_; + std::string type_md5_sum_; + size_t data_size_; + uint8_t const* data_; +}; + +class GenericROSSubRapidPub : public BridgeSubscriber { + public: + explicit GenericROSSubRapidPub(ros::NodeHandle const* nh); + ~GenericROSSubRapidPub(); + + void AddTopics(std::map>> const& link_entries); + + float FloatMod(float x, float y); + + void InitializeDDS(std::vector const& connections); + + // Called with the mutex held + virtual void subscribeTopic(std::string const& in_topic, + const RelayTopicInfo& info); + + // Called with the mutex held + virtual void advertiseTopic(const RelayTopicInfo& info); + + // Called with the mutex held + virtual void relayMessage(const RelayTopicInfo& topic_info, + ContentInfo const& content_info); + + void ConvertRequest(rapid::ext::astrobee::GenericCommsRequest const* data, + std::string const& connecting_robot); + + void CheckPubMsg(const ros::TimerEvent& event); + + private: + bool dds_initialized_, pub_rate_msgs_; + + ros::Timer rate_msg_pub_timer_; + + std::map>> topic_mapping_; + std::map robot_connections_; + std::vector> rate_topic_entries_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_pub_request.h b/communications/comms_bridge/include/comms_bridge/rapid_pub_request.h new file mode 100644 index 0000000000..9a8f841f6c --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_pub_request.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_PUB_REQUEST_H_ +#define COMMS_BRIDGE_RAPID_PUB_REQUEST_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstantsSupport.h" +#include "dds_msgs/GenericCommsRequestSupport.h" + +namespace ff { + +class RapidPubRequest { + public: + explicit RapidPubRequest(std::string const& robot_name); + ~RapidPubRequest(); + + void SendRequest(std::string const& output_topic); + + private: + using RequestSupplier = + kn::DdsTypedSupplier; + using RequestSupplierPtr = std::unique_ptr; + RequestSupplierPtr request_supplier_; + + unsigned int request_seq_; +}; + +typedef std::shared_ptr RapidPubRequestPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_PUB_REQUEST_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_sub_advertisement_info.h b/communications/comms_bridge/include/comms_bridge/rapid_sub_advertisement_info.h new file mode 100644 index 0000000000..a566c7ee4a --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_sub_advertisement_info.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_SUB_ADVERTISEMENT_INFO_H_ +#define COMMS_BRIDGE_RAPID_SUB_ADVERTISEMENT_INFO_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" + +namespace ff { + +class RapidSubAdvertisementInfo : public GenericRapidSub { + public: + RapidSubAdvertisementInfo(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub); + + /** + * Call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::GenericCommsAdvertisementInfo const* + ad_info); + private: + GenericRapidMsgRosPub* ros_pub_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_SUB_ADVERTISEMENT_INFO_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_sub_content.h b/communications/comms_bridge/include/comms_bridge/rapid_sub_content.h new file mode 100644 index 0000000000..5ea483a8ee --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_sub_content.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_SUB_CONTENT_H_ +#define COMMS_BRIDGE_RAPID_SUB_CONTENT_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +namespace ff { + +class RapidSubContent : public GenericRapidSub { + public: + RapidSubContent(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub); + + /** + * Call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::GenericCommsContent const* content); + + private: + GenericRapidMsgRosPub* ros_pub_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_SUB_CONTENT_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_sub_request.h b/communications/comms_bridge/include/comms_bridge/rapid_sub_request.h new file mode 100644 index 0000000000..d15f557ef8 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_sub_request.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_SUB_REQUEST_H_ +#define COMMS_BRIDGE_RAPID_SUB_REQUEST_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GenericCommsRequestSupport.h" + +namespace ff { + +class RapidSubRequest : public GenericRapidSub { + public: + RapidSubRequest(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericROSSubRapidPub* ros_sub_rapid_pub); + + /** + * Call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::GenericCommsRequest const* request); + + private: + GenericROSSubRapidPub* ros_sub_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_SUB_REQUEST_H_ diff --git a/communications/comms_bridge/include/comms_bridge/util.h b/communications/comms_bridge/include/comms_bridge/util.h new file mode 100644 index 0000000000..51c07eda52 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/util.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_UTIL_H_ +#define COMMS_BRIDGE_UTIL_H_ + +#include + +#include +#include + +#include + +#include + +#include + +#include + +namespace comms_util { + + // Time-related helpers + ::ros::Time RapidTime2RosTime(const int64_t dds_time); + + int64_t RosTime2RapidTime(::ros::Time const& ros_time); + + // Header-related helpers + void RosHeader2Rapid(::std_msgs::Header const& ros_hdr, + ::rapid::Header* rapid_hdr, + int status = 0, + int serial = -1); + + void RapidHeader2Ros(::rapid::Header const& rapid_hdr, + ::std_msgs::Header* ros_hdr, + std::string const& frame_id = "world"); + +} // end namespace comms_util + +#endif // COMMS_BRIDGE_UTIL_H_ diff --git a/communications/comms_bridge/launch/comms_bridge.launch b/communications/comms_bridge/launch/comms_bridge.launch new file mode 100644 index 0000000000..07ec93abbf --- /dev/null +++ b/communications/comms_bridge/launch/comms_bridge.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/communications/comms_bridge/nodelet_plugins.xml b/communications/comms_bridge/nodelet_plugins.xml new file mode 100644 index 0000000000..f30810f612 --- /dev/null +++ b/communications/comms_bridge/nodelet_plugins.xml @@ -0,0 +1,8 @@ + + + + Nodelet for the comms bridge + + + diff --git a/communications/comms_bridge/package.xml b/communications/comms_bridge/package.xml new file mode 100644 index 0000000000..df3076380b --- /dev/null +++ b/communications/comms_bridge/package.xml @@ -0,0 +1,45 @@ + + + comms_bridge + 0.0.0 + + The comms_bridge package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + ff_util + ff_msgs + topic_tools + config_reader + dds_msgs + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + ff_util + ff_msgs + topic_tools + config_reader + dds_msgs + + + + diff --git a/communications/comms_bridge/readme.md b/communications/comms_bridge/readme.md new file mode 100644 index 0000000000..9f89f2b31c --- /dev/null +++ b/communications/comms_bridge/readme.md @@ -0,0 +1,52 @@ +\page comms_bridge DDS Generic Comms Bridge + +This is a bidirectional bridge to communicate between different astrobees or ros +clients using DDS to relay the topics. Supporting an arbitrary number of in-out +topics, relaying of ROS messages from one topic to another, without any +specification of the message types, nor even the requirement that the types be +available to the nodes at compile-time. + +## Basic architecture + +- The subscriber class (A) subscribes to a specified list of topics +- Upon receipt of the first message on a given forwarded topic, A sends an advertisement message to a metadata "advertisement" topic +- Receiving the metadata advertisement, the republisher class (B) itself advertises the output (republished) topic +- The first and all subsequent messages A receives are serialized and published to a metadata "content" topic +- A delay is implemented in B between its advertising and subsequent republishing of any messages to practically avoid the potential race condition of a subscriber on that end missing the first message as it connects to B only upon seeing the advertisement +- Otherwise, B deserializes all messages received on the "content" metadata topic, looks up the topic in previously received "advertisement" metadata, and republishes the message on the output topic +- In this implementation, the metadata advertisement info and content are converted to and passed over dds. + +## Enabling DDS Communication + +The generic comms bridge runs by default when the astrobee flight starts up. By +default, dds publishing and subscribing is disabled since astrobee to astrobee +communication is not desired for every activity. There are 2 ways to enable it. +One can either change the 'initialize_dds_on_start' parameter in the comms bridge +config file to true before starting flight software or send an "enable astrobee +intercomms" command to astrobee after the flight software has started. + +## Specifying Topics + +To add topics to be passed between robots, please see the comms bridge config +file. The comments in the config file should explain how to add topics. + +## DDS Specifics + +### Discovery Peers File + +In order to have two or more Astrobees communicate, one must add the ip +addresses to the "NDDS_DISCOVERY_PEERS" file in the "dds_generic_comms" folder. +Be sure to add a semi-colon (;) to the end of every line added. + +### QoS Profile + +By default, DDS properties are used to ensure the advertisement info messages +are reliably deliveried to the republisher class even if the subscriber or +republisher are restarted. Currently, DDS is configured to work if there are +no more than 20 topics being passed between robots. If more topics are required, +please change the message depth in the "RAPID_QOS_PROFILES.xml" file in the +"dds_generic_comms" folder. The depth for the "RapidReliableDurableQos" will +need to be changed for both the "datareader_qos" and the "datawriter_qos". This +should be on or around lines 206 and 218 in the file. You will also need to +change the value check in the "comms_bridge_nodelet.cpp" file on or around +line 307. diff --git a/communications/comms_bridge/src/bridge_publisher.cpp b/communications/comms_bridge/src/bridge_publisher.cpp new file mode 100644 index 0000000000..61c4dfe743 --- /dev/null +++ b/communications/comms_bridge/src/bridge_publisher.cpp @@ -0,0 +1,245 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/bridge_publisher.h" + +#include +#include +#include +#include +#include + +// ROS internal publisher message queue size +#define PUBLISHER_QUEUE_SIZE 10 +// max number of messages to store because we're not ready to republish them +#define MAX_WAITING_QUEUE_SIZE 100 +// default time to delay between advertising and publishing on that topic [sec] +#define DEFAULT_ADVERTISE_DELAY 3.0 + +using ros::Publisher; +using ros::Publisher; +using topic_tools::ShapeShifter; + +BridgePublisher::BridgePublisher(double ad2pub_delay) : m_ad2pub_delay_(ad2pub_delay) { + m_n_relayed_ = 0; + m_verbose_ = 0; + + worker_thread_.reset(new std::thread(&BridgePublisher::drainThread, this)); +} + +BridgePublisher::~BridgePublisher() { + worker_thread_->join(); +} + +void BridgePublisher::setVerbosity(unsigned int verbosity) { m_verbose_ = verbosity; } + +bool BridgePublisher::advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info) { + if (m_verbose_) + ROS_INFO("Advertising for topic %s\n", output_topic.c_str()); + + std::map::iterator iter = m_relay_topics_.find(output_topic); + if (iter == m_relay_topics_.end()) { + if (m_verbose_) + ROS_INFO(" topic is yet-unknown\n"); + RelayTopicInfo topic_info; + topic_info.out_topic = output_topic; + iter = m_relay_topics_.emplace(output_topic, topic_info).first; + } + + RelayTopicInfo &topic_info = iter->second; + + if (topic_info.advertised) { + // we've already advertised, nothing to do + if (topic_info.ad_info.md5_sum != ad_info.md5_sum || + topic_info.ad_info.data_type != ad_info.data_type || + topic_info.ad_info.definition != ad_info.definition) + ROS_ERROR("Received advertisement with differing type info from previous advertisement, messages will be lost"); + return false; + } else { + if (topic_info.ad_info.md5_sum != "" && topic_info.ad_info.md5_sum != ad_info.md5_sum) + ROS_WARN( + "Advertisement received for topic %s with differing md5sum from prior content, prior messages will be lost", + output_topic.c_str()); + + // we haven't seen an ad for this topic yet, populate everything + topic_info.ad_info = ad_info; + + ros::NodeHandle nh; + ShapeShifter shifter; + // will always succeed, even if inconsistent type info + shifter.morph(ad_info.md5_sum, ad_info.data_type, ad_info.definition, ""); + topic_info.publisher = shifter.advertise(nh, output_topic, PUBLISHER_QUEUE_SIZE, ad_info.latching); + + topic_info.advertised = true; + + const timepoint_t now = std::chrono::steady_clock::now(); + topic_info.delay_until = now + std::chrono::microseconds((unsigned)(m_ad2pub_delay_ * 1e6)); + + if (m_ad2pub_delay_ > 0) { + m_drain_queue_.push(std::make_pair(topic_info.delay_until, output_topic)); + m_drain_cv_.notify_all(); + } else { + // no post-advertise delay + + // send out any old messages first + drainWaitingQueue(topic_info); + } + } + + return true; +} + +bool BridgePublisher::relayContent(RelayTopicInfo& topic_info, const ContentInfo& content_info) { + ShapeShifter shifter; + + // IStreams don't really modify their input but the implementation is + // lazily general so we have to be ugly + ros::serialization::IStream stream(const_cast(content_info.data.data()), content_info.data.size()); + try { + stream.next(shifter); + } + catch (ros::Exception &e) { + ROS_ERROR("Deserialization error (%s), losing message", e.what()); + return false; + } + + // Output error if inconsistent type info + if (topic_info.ad_info.md5_sum != content_info.type_md5_sum) { + ROS_ERROR("Content md5 sum doesn't match the advertiment info md5 sum. This could cause issues!"); + } + + // will always succeed, even if inconsistent type info + shifter.morph(topic_info.ad_info.md5_sum, topic_info.ad_info.data_type, topic_info.ad_info.definition, ""); + + try { + topic_info.publisher.publish(shifter); + } + catch (ros::Exception &e) { + ROS_ERROR("Error publishing morphed message (%s), losing message", e.what()); + return false; + } + + topic_info.relay_seqnum++; + m_n_relayed_++; + + if (m_verbose_) + ROS_INFO("Sent message on topic %s\n", topic_info.out_topic.c_str()); + + return true; +} + +bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo& content_info) { + if (m_verbose_) + ROS_INFO("Relaying content message for topic %s\n", topic_info.out_topic.c_str()); + + if (!topic_info.advertised) { + if (m_verbose_) + ROS_INFO(" topic is yet-unknown, enqueuing message for now\n"); + + topic_info.waiting_msgs.emplace(content_info); + + return true; + } + + if (topic_info.ad_info.md5_sum != content_info.type_md5_sum) + ROS_WARN( + "Message received for topic %s with differing md5sum from prior advertiesment, future messages will be lost", + topic_info.out_topic.c_str()); + + const timepoint_t now = std::chrono::steady_clock::now(); + + if (!topic_info.advertised || now < topic_info.delay_until) { + // we still don't have type information for this message, or + // we're still delaying after advertising + // so just enqueue messages + + if (m_verbose_) + ROS_INFO(" topic is not %s, enqueuing message for now\n", + (!topic_info.advertised ? "yet advertised" : "yet ready")); + + // limit the number of waiting messages, tossing the oldest ones + while (topic_info.waiting_msgs.size() >= MAX_WAITING_QUEUE_SIZE) + topic_info.waiting_msgs.pop(); + + topic_info.waiting_msgs.emplace(content_info); + + if (m_verbose_ > 1) + ROS_INFO(" %zu messages now waiting\n", topic_info.waiting_msgs.size()); + + return true; + } + + // send out any old messages first + drainWaitingQueue(topic_info); + + // send out this message + return relayContent(topic_info, content_info); +} + +void BridgePublisher::drainThread() { + std::unique_lock lk(m_mutex_); + + if (m_verbose_) + ROS_INFO("Started drain thread\n"); + + while (1) { + while (m_drain_queue_.empty()) + m_drain_cv_.wait(lk); + + std::pair entry = m_drain_queue_.top(); + m_drain_queue_.pop(); + + lk.unlock(); + + timepoint_t t = entry.first; + + if (m_verbose_ > 1) + ROS_INFO( + "draining %s in %0.3f seconds\n", entry.second.c_str(), + std::chrono::duration((t - std::chrono::steady_clock::now())).count()); + std::this_thread::sleep_until(t); + + if (m_verbose_ > 1) + ROS_INFO("draining %s now\n", entry.second.c_str()); + + lk.lock(); + + drainWaitingQueue(entry.second); + } +} + +// called with mutex held +void BridgePublisher::drainWaitingQueue(RelayTopicInfo& topic_info) { + if (m_verbose_ && topic_info.waiting_msgs.size() > 0) + ROS_INFO("Draining queue of %zu waiting messages\n", topic_info.waiting_msgs.size()); + while (topic_info.waiting_msgs.size() > 0) { + const ContentInfo old_msg = topic_info.waiting_msgs.front(); + relayContent(topic_info, old_msg); + topic_info.waiting_msgs.pop(); + } +} + +// called with mutex held +void BridgePublisher::drainWaitingQueue(std::string const& output_topic) { + std::map::iterator iter = m_relay_topics_.find(output_topic); + assert(iter != m_relay_topics_.end()); + + RelayTopicInfo &topic_info = iter->second; + + drainWaitingQueue(topic_info); +} diff --git a/communications/comms_bridge/src/bridge_subscriber.cpp b/communications/comms_bridge/src/bridge_subscriber.cpp new file mode 100644 index 0000000000..d35987c1eb --- /dev/null +++ b/communications/comms_bridge/src/bridge_subscriber.cpp @@ -0,0 +1,187 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include "comms_bridge/bridge_subscriber.h" + +// ROS internal subscriber message queue size +#define SUBSCRIBER_QUEUE_SIZE 10 +// max serialized message size +#define ROS_BRIDGE_MAX_MSG_SIZE (10*1024*1024) + +using ros::Subscriber; +using ros::Publisher; +using topic_tools::ShapeShifter; + +BridgeSubscriber::BridgeSubscriber() { + m_n_relayed_ = 0; + m_verbose_ = 0; + + m_msgbuffer_ = new uint8_t[ROS_BRIDGE_MAX_MSG_SIZE]; +} + +BridgeSubscriber::~BridgeSubscriber() { delete[] m_msgbuffer_; } + +void BridgeSubscriber::setVerbosity(unsigned int verbosity) { m_verbose_ = verbosity; } + +void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent& msg_event, + std::string const& topic, SubscriberPtr sub) { + ShapeShifter::ConstPtr ptr = msg_event.getConstMessage(); + + boost::shared_ptr const& connection_header = + msg_event.getConnectionHeaderPtr(); + + if (m_verbose_) { + ROS_INFO("got data on %s:\n", topic.c_str()); + if (m_verbose_ > 1) { + ROS_INFO(" datatype: \"%s\"\n", ptr->getDataType().c_str()); + ROS_INFO(" md5: \"%s\"\n", ptr->getMD5Sum().c_str()); + } + if (m_verbose_ > 2) + ROS_INFO(" def: \"%s\"\n", ptr->getMessageDefinition().c_str()); + + if (m_verbose_ > 2) { + ROS_INFO(" conn header:\n"); + for (ros::M_string::const_iterator iter = connection_header->begin(); + iter != connection_header->end(); + iter++) + ROS_INFO(" %s: %s\n", iter->first.c_str(), iter->second.c_str()); + } + } + + std::unique_lock lock(m_mutex_); + + std::map::iterator iter = m_relay_topics_.find(topic); + if (iter == m_relay_topics_.end()) { + ROS_INFO("Received message on non-relayed topic %s\n", topic.c_str()); + return; + } + + RelayTopicInfo &topic_info = iter->second; + + if (!topic_info.advertised) { + if (m_verbose_) + ROS_INFO(" sending advertisement\n"); + + bool latching = false; + if (connection_header) { + ros::M_string::const_iterator iter = connection_header->find("latching"); + if (iter != connection_header->end() && iter->second == "1") + latching = true; + } + + AdvertisementInfo ad_info; + ad_info.latching = latching; + ad_info.data_type = ptr->getDataType(); + ad_info.md5_sum = ptr->getMD5Sum(); + ad_info.definition = ptr->getMessageDefinition(); + topic_info.ad_info = ad_info; + + advertiseTopic(topic_info); + topic_info.advertised = true; + + // ROS "latches" the msg type md5sum for us, only accepting same in future + // so no need for us to save and check that it doesn't change + } + + ros::serialization::OStream stream(m_msgbuffer_, ROS_BRIDGE_MAX_MSG_SIZE); + stream.next(*ptr); // serializes just the message contents + ssize_t serialized_size = ROS_BRIDGE_MAX_MSG_SIZE - stream.getLength(); + if (m_verbose_ > 2) + ROS_INFO(" serialized size = %zd\n", serialized_size); + if (serialized_size <= 0) { + ROS_ERROR("Serialization buffer size deficient, discarding message"); + return; + } + + ContentInfo content_info; + content_info.type_md5_sum = ptr->getMD5Sum(); + content_info.data_size = (size_t)serialized_size; + content_info.data = m_msgbuffer_; + relayMessage(topic_info, content_info); + topic_info.relay_seqnum++; + m_n_relayed_++; + + lock.release()->unlock(); + // now done with any access to topic info + + if (m_verbose_) + fflush(stdout); +} + +// called with mutex held +SubscriberPtr BridgeSubscriber::rosSubscribe(std::string const& topic) { + ros::NodeHandle nh; + + SubscriberPtr ptr = std::make_shared(); + + ros::SubscribeOptions opts; + opts.topic = topic; + opts.queue_size = SUBSCRIBER_QUEUE_SIZE; + opts.md5sum = ros::message_traits::md5sum(); + opts.datatype = ros::message_traits::datatype(); + opts.helper = boost::make_shared &> >( + std::bind(&BridgeSubscriber::handleRelayedMessage, this, std::placeholders::_1, topic, ptr)); + *ptr = nh.subscribe(opts); + + if (m_verbose_) + ROS_INFO("Subscribed to topic %s\n", topic.c_str()); + + return ptr; +} + +bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const& out_topic) { + const std::lock_guard lock(m_mutex_); + + // Enforce that all relays have a unique input topic + if (m_relay_topics_.find(in_topic) != m_relay_topics_.end()) { + if (m_verbose_) + ROS_ERROR("Already subscribed to relay from topic %s\n", in_topic.c_str()); + return false; + } + + // Enforce that all relays have a unique output topic + // The republishing side will already be checking for this but may have no way + // to communicate that to us, so we check too + std::map::iterator iter = m_relay_topics_.begin(); + while (iter != m_relay_topics_.end()) { + if (iter->second.out_topic == out_topic) { + if (m_verbose_) + ROS_ERROR("Already relaying to topic %s\n", out_topic.c_str()); + return false; + } + iter++; + } + + RelayTopicInfo topic_info; + topic_info.out_topic = out_topic; + + topic_info.sub = rosSubscribe(in_topic); + // handleRelayedMessage() may immediately start getting called + + m_relay_topics_[in_topic] = topic_info; + + // let implementation know + subscribeTopic(in_topic, topic_info); + + return true; +} diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp new file mode 100644 index 0000000000..57838aa775 --- /dev/null +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -0,0 +1,417 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Standard ROS includes +#include +#include +#include + +// FSW shared libraries +#include + +// FSW nodelet +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "comms_bridge/generic_rapid_msg_ros_pub.h" +#include "comms_bridge/generic_rapid_sub.h" +#include "comms_bridge/generic_ros_sub_rapid_pub.h" +#include "comms_bridge/rapid_sub_advertisement_info.h" +#include "comms_bridge/rapid_sub_content.h" +#include "comms_bridge/rapid_sub_request.h" + +#include "dds_msgs/AstrobeeConstants.h" + +#include "ff_msgs/ResponseOnly.h" + +// SoraCore +#include "knDds/DdsSupport.h" +#include "knDds/DdsEntitiesFactory.h" +#include "knDds/DdsEntitiesFactorySvc.h" +#include "knDds/DdsTypedSupplier.h" + +// miro +#include "miro/Configuration.h" +#include "miro/Robot.h" +#include "miro/Log.h" + +namespace kn { + class DdsEntitiesFactorySvc; +} // end namespace kn + +namespace comms_bridge { + +class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { + public: + CommsBridgeNodelet() : ff_util::FreeFlyerNodelet("comms_bridge"), + dds_initialized_(false), + initialize_dds_on_start_(false), + enable_advertisement_info_request_(false) {} + + virtual ~CommsBridgeNodelet() {} + + protected: + virtual void Initialize(ros::NodeHandle* nh) { + // Need to get robot name which is in the lua config files, add files, read + // files, and get robot name but don't get the rest of the config parameters + // since these params are used to create the classes that use rapid dds. Need + // to set up Miro/DDs before reading the parameters. + + config_params_.AddFile("communications/comms_bridge.config"); + + if (!config_params_.ReadFiles()) { + ROS_FATAL("BridgeSubscriberNodelet: Error reading config files."); + exit(EXIT_FAILURE); + return; + } + + if (!config_params_.GetStr("agent_name", &agent_name_)) { + ROS_FATAL("BridgeSubscriberNodelet: Could not read robot name."); + exit(EXIT_FAILURE); + return; + } + + // In simulation, the namespace is usually set to the robot name so we need to + // check if we are in simulation and get the right name + if (agent_name_ == "sim" || agent_name_ == "simulator") { + // The platform name should be the simulated robot name + agent_name_ = GetPlatform(); + + // If there is not robot name, set it to a default name so that we can + // connect to the bridge + if (agent_name_ == "") { + agent_name_ = "Bumble"; + } else { + // Make sure that first letter of robot name is capitialized. GDS only + // recognizes capitialized robot names. + agent_name_[0] = toupper(agent_name_[0]); + } + } + ROS_INFO_STREAM("Comms Bridge Nodelet: agent name " << agent_name_); + + ros_sub_ = std::make_shared(nh); + + int fake_argc = 1; + + // Make path to QOS and NDDS files + std::string config_path = ff_common::GetConfigDir(); + config_path += "/communications/dds_generic_comms/"; + + // Create fake argv containing only the participant name + // Participant name needs to be unique so combine robot name with timestamp + ros::Time time = ros::Time::now(); + participant_name_ = agent_name_ + std::to_string(time.sec) + std::string("-comms-bridge"); + char **fake_argv = new char*[1]; + fake_argv[0] = new char[(participant_name_.size() + 1)]; + std::strcpy(fake_argv[0], participant_name_.c_str()); // NOLINT + + /* fake miro log into thinking we have no arguments */ + Miro::Log::init(fake_argc, fake_argv); + Miro::Log::level(9); + + /* fake miro configuration into thinking we have no arguments */ + Miro::Configuration::init(fake_argc, fake_argv); + + Miro::RobotParameters *robot_params = Miro::RobotParameters::instance(); + + kn::DdsEntitiesFactorySvcParameters *dds_params = + kn::DdsEntitiesFactorySvcParameters::instance(); + + /* get the defaults for *all the things!* */ + Miro::ConfigDocument *config = Miro::Configuration::document(); + config->setSection("Robot"); + config->getParameters("Miro::RobotParameters", *robot_params); + config->getParameters("kn::DdsEntitiesFactorySvcParameters", *dds_params); + + robot_params->name = agent_name_; + robot_params->namingContextName = robot_params->name; + + ROS_INFO("Agent name %s and participant name %s\n", agent_name_.c_str(), participant_name_.c_str()); + + // Set values for default punlisher and subscriber + dds_params->publishers[0].name = agent_name_; + dds_params->publishers[0].partition = agent_name_; + dds_params->publishers[0].participant = participant_name_; + dds_params->subscribers[0].participant = participant_name_; + + // Clear config files so that dds only looks for the files we add + dds_params->participants[0].discoveryPeersFiles.clear(); + dds_params->configFiles.clear(); + + dds_params->participants[0].name = participant_name_; + dds_params->participants[0].participantName = participant_name_; + dds_params->participants[0].domainId = 38; + dds_params->participants[0].discoveryPeersFiles.push_back( + (config_path + "NDDS_DISCOVERY_PEERS")); + dds_params->configFiles.push_back((config_path + "RAPID_QOS_PROFILES.xml")); + + std::string local_subscriber = Miro::RobotParameters::instance()->name.c_str(); + + if (!ReadParams()) { + exit(EXIT_FAILURE); + return; + } + + // Register the connections into the parameters so they can be used later + for (int i = 0; i < rapid_connections_.size(); i++) { + // This shouldn't be needed but check just in case + if (local_subscriber != rapid_connections_[i]) { + kn::DdsNodeParameters subscriber; + subscriber.name = rapid_connections_[i]; + subscriber.partition = rapid_connections_[i]; + subscriber.participant = participant_name_; + dds_params->subscribers.push_back(subscriber); + } + } + + /** + * Use DdsEntitiesFactorySvc to create a new DdsEntitiesFactory + * which will create all objects: + * Participants DdsDomainParticipantRepository::instance() + * Publishers DdsPublisherRespository::instance() + * Subscribers DdsSubscriberRepository::instance() + * Topics + * and store in relevant repository + * based on DdsEntitiesFactoryParameters + */ + dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); + dds_entities_factory_->init(dds_params); + + dds_initialize_srv_ = nh->advertiseService( + SERVICE_COMMUNICATIONS_ENABLE_ASTROBEE_INTERCOMMS, + &CommsBridgeNodelet::StartDDS, + this); + + if (initialize_dds_on_start_) { + InitializeDDS(); + } + } + + bool StartDDS(ff_msgs::ResponseOnly::Request& req, + ff_msgs::ResponseOnly::Response& res) { + if (!dds_initialized_) { + InitializeDDS(); + } + res.success = true; + return true; + } + + void InitializeDDS() { + std::string connection, dds_topic_name; + ff::GenericRapidSubPtr rapid_sub; + ros_sub_->InitializeDDS(rapid_connections_); + if (enable_advertisement_info_request_) { + ros_pub_->InitializeDDS(rapid_connections_, + enable_advertisement_info_request_); + } + for (size_t i = 0; i < rapid_connections_.size(); i++) { + // Lower case the external agent name to use it like a namespace + connection = rapid_connections_[i]; + dds_topic_name = agent_name_ + "-" + + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; + ROS_INFO("Comms Bridge: DDS Sub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); + rapid_sub = std::make_shared( + "AstrobeeGenericCommsAdvertisementInfoProfile", + dds_topic_name, + connection, + ros_pub_.get()); + rapid_subs_.push_back(rapid_sub); + + dds_topic_name = agent_name_ + "-" + + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; + ROS_INFO("Comms Bridge: DDS Sub DDS content topic name: %s\n", + dds_topic_name.c_str()); + rapid_sub = std::make_shared( + "AstrobeeGenericCommsContentProfile", + dds_topic_name, + connection, + ros_pub_.get()); + rapid_subs_.push_back(rapid_sub); + + if (enable_advertisement_info_request_) { + dds_topic_name = agent_name_ + "-" + + rapid::ext::astrobee::GENERIC_COMMS_REQUEST_TOPIC; + ROS_INFO("Comms Bridge: DDS Sub DDS request topic name: %s\n", + dds_topic_name.c_str()); + rapid_sub = std::make_shared( + "AstrobeeGenericCommsRequestProfile", + dds_topic_name, + connection, + ros_sub_.get()); + rapid_subs_.push_back(rapid_sub); + } + } + ros_sub_->AddTopics(link_entries_); + dds_initialized_ = true; + } + + bool ReadParams() { + double ad2pub_delay = 0; + if (!config_params_.GetReal("ad2pub_delay", &ad2pub_delay) || + ad2pub_delay <= 0) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read/or invalid ad2pub_delay. Setting to 3."); + ad2pub_delay = 3; + } + ros_pub_ = std::make_shared(ad2pub_delay); + + unsigned int verbose = 2; + if (!config_params_.GetUInt("verbose", &verbose)) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read verbosity level. Setting to 2 (info?)."); + } + ros_sub_->setVerbosity(verbose); + ros_pub_->setVerbosity(verbose); + + initialize_dds_on_start_ = false; + if (!config_params_.GetBool("initialize_dds_on_start", + &initialize_dds_on_start_)) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read initialize dds on start. Setting to false."); + } + + enable_advertisement_info_request_ = false; + if (!config_params_.GetBool("enable_advertisement_info_request", + &enable_advertisement_info_request_)) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read enable advertisement info request. Setting to false."); + } + + // Load shared topic groups + config_reader::ConfigReader::Table links, link; + if (!config_params_.GetTable("links", &links)) { + ROS_FATAL("Comms Bridge Nodelet: Links not specified!"); + return false; + } + + std::string ns = std::string("/") + agent_name_ + "/"; + ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case + int num_topics = 0; + ROS_INFO_STREAM("Read Params numebr of links: " << links.GetSize()); + for (int i = 1; i <= links.GetSize(); i++) { + if (!links.GetTable(i, &link)) { + NODELET_ERROR("Comms Bridge Nodelet: Could read link table row %i", i); + continue; + } + std::string config_agent, connection_agent; + num_topics = 0; + ROS_INFO_STREAM("Link " << i << " from " << link.GetStr("from", &config_agent)); + ROS_INFO_STREAM("Link " << i << " to " << link.GetStr("to", &config_agent)); + if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { + if (AddRapidConnections(link, "to", connection_agent)) { + AddTableToSubs(link, "relay_forward", ns, connection_agent, num_topics); + AddTableToSubs(link, "relay_both", ns, connection_agent, num_topics); + } + } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { + if (AddRapidConnections(link, "from", connection_agent)) { + AddTableToSubs(link, "relay_backward", ns, connection_agent, num_topics); + AddTableToSubs(link, "relay_both", ns, connection_agent, num_topics); + } + } + + // Check to make sure the number of topics added doesn't exceed the + // the number of messages dds reliably delivers + if (num_topics > 20) { + ROS_ERROR("Comms bridge: Num of added topics is greater than the number of topics dds will reliably deliver."); + } + } + return true; + } + + bool AddRapidConnections(config_reader::ConfigReader::Table &link_table, + std::string const& direction, + std::string &connection) { + if (!link_table.GetStr(direction.c_str(), &connection)) { + NODELET_ERROR("Comms Bridge Nodelet: %s not specified for one link", direction.c_str()); + return false; + } + + // This should be very quick since we shouldn't have more than 2 connections + bool found = false; + for (size_t i = 0; i < rapid_connections_.size() && !found; i++) { + if (connection == rapid_connections_[i]) { + found = true; + } + } + + if (!found) { + rapid_connections_.push_back(connection); + } + return true; + } + + void AddTableToSubs(config_reader::ConfigReader::Table &link_table, + std::string table_name, + std::string const& current_robot_ns, + std::string const& connection_robot, + int &num_topics) { + config_reader::ConfigReader::Table relay_table, relay_item; + std::string in_topic, out_topic; + double rate = -1.0; + if (link_table.GetTable(table_name.c_str(), &relay_table)) { + num_topics += relay_table.GetSize(); + for (size_t i = 1; i <= relay_table.GetSize(); i++) { + relay_table.GetTable(i, &relay_item); + if (!relay_item.GetStr("in_topic", &in_topic)) { + NODELET_ERROR("Comms Bridge Nodelet: In topic not specified!"); + continue; + } + + if (!relay_item.GetStr("out_topic", &out_topic)) { + out_topic = current_robot_ns + in_topic; + } + + if (!relay_item.GetReal("rate", &rate)) { + rate = -1.0; + } + + // Save all output topics under the same in topic since we don't want + // to subscribe to the same topic multiple times + link_entries_[in_topic].push_back( + std::make_shared(connection_robot, + out_topic, + rate)); + } + } + } + + private: + bool initialize_dds_on_start_, dds_initialized_; + bool enable_advertisement_info_request_; + config_reader::ConfigReader config_params_; + + std::vector rapid_subs_; + std::vector rapid_connections_; + + std::shared_ptr dds_entities_factory_; + std::shared_ptr ros_pub_; + std::shared_ptr ros_sub_; + + std::string agent_name_, participant_name_; + std::map>> link_entries_; + ros::ServiceServer dds_initialize_srv_; +}; + +PLUGINLIB_EXPORT_CLASS(comms_bridge::CommsBridgeNodelet, nodelet::Nodelet) + +} // namespace comms_bridge diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp new file mode 100644 index 0000000000..9a7b4389c3 --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -0,0 +1,141 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include +#include +#include + +namespace ff { + +GenericRapidMsgRosPub::GenericRapidMsgRosPub(double ad2pub_delay) : + BridgePublisher(ad2pub_delay), + dds_initialized_(false), + enable_advertisement_info_request_(false) { +} + +GenericRapidMsgRosPub::~GenericRapidMsgRosPub() {} + +void GenericRapidMsgRosPub::InitializeDDS(std::vector const& connections, + bool enable_advertisement_info_request) { + std::string robot_name; + for (size_t i = 0; i < connections.size(); ++i) { + robot_name = connections[i]; + RapidPubRequestPtr rapid_pub = std::make_shared(robot_name); + robot_connections_[robot_name] = rapid_pub; + } + + enable_advertisement_info_request_ = enable_advertisement_info_request; + + dds_initialized_ = true; +} + +// Handle Ad Message +void GenericRapidMsgRosPub::ConvertAdvertisementInfo( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) { + const std::lock_guard lock(m_mutex_); + + const std::string output_topic = data->outputTopic; + + ROS_INFO("Comms Bridge Nodelet: Received advertisement message for topic %s\n", + output_topic.c_str()); + + AdvertisementInfo ad_info; + ad_info.latching = data->latching; + ad_info.data_type = data->dataType; + ad_info.md5_sum = data->md5Sum; + ad_info.definition = data->msgDefinition; + ROS_INFO_STREAM("ad_info latching: " << data->latching); + ROS_INFO_STREAM("ad_info dataType: " << data->dataType); + ROS_INFO_STREAM("ad_info md5Sum: " << data->md5Sum); + ROS_INFO_STREAM("ad_info msgDefinition: " << data->msgDefinition); + + if (!advertiseTopic(output_topic, ad_info)) { + ROS_INFO("Comms Bridge Nodelet: Received more than one advertisement info message for topic: %s\n", + output_topic.c_str()); + } +} + +// Handle content message +void GenericRapidMsgRosPub::ConvertContent( + rapid::ext::astrobee::GenericCommsContent const* data, + std::string const& connecting_robot) { + const std::lock_guard lock(m_mutex_); + + const std::string output_topic = data->outputTopic; + + ROS_INFO("Comms Bridge Nodelet: Received content message for topic %s\n", + output_topic.c_str()); + + std::map::iterator iter = m_relay_topics_.find(output_topic); + if (iter == m_relay_topics_.end()) { + ROS_ERROR("Comms Bridge Nodelet: Received content for topic %s but never received advertisement info.\n", + output_topic.c_str()); + + RelayTopicInfo topic_info; + topic_info.out_topic = output_topic; + topic_info.ad_info.md5_sum = data->md5Sum; + iter = m_relay_topics_.emplace(output_topic, topic_info).first; + + RequestAdvertisementInfo(output_topic, connecting_robot); + } + + RelayTopicInfo &topic_info = iter->second; + + ContentInfo content_info; + content_info.type_md5_sum = data->md5Sum; + + unsigned char* buf = data->data.get_contiguous_buffer(); + for (size_t i = 0; i < data->data.length(); i++) { + content_info.data.push_back(buf[i]); + } + + ROS_INFO("Comms Bridge Nodelet: Calling relay message for topic %s\n", + output_topic.c_str()); + + if (!relayMessage(topic_info, content_info)) { + ROS_ERROR("Comms Bridge Nodelet: Error relaying message for topic %s\n", + output_topic.c_str()); + } +} + +void GenericRapidMsgRosPub::RequestAdvertisementInfo( + std::string const& output_topic, + std::string const& connecting_robot) { + if (enable_advertisement_info_request_) { + if (dds_initialized_) { + // Check robot connection exists + if (robot_connections_.find(connecting_robot) != + robot_connections_.end()) { + ROS_INFO("Sending request for advertisement info for topic %s!\n", + output_topic.c_str()); + robot_connections_[connecting_robot]->SendRequest(output_topic); + } else { + ROS_ERROR("Comms Bridge: No connection for %s in rapid msg ros pub.\n", + connecting_robot.c_str()); + } + } else { + ROS_ERROR("Comms Bridge: DDS is not initialized for sending advertisement info request.\n"); + } + } else { + ROS_INFO("Comms Bridge: Advertisement info request not enabled.\n"); + } +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/generic_rapid_pub.cpp b/communications/comms_bridge/src/generic_rapid_pub.cpp new file mode 100644 index 0000000000..036e7e20b8 --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_pub.cpp @@ -0,0 +1,168 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/generic_rapid_pub.h" + +#include + +namespace ff { + +GenericRapidPub::GenericRapidPub(std::string const& robot_name) : + advertisement_info_seq_(0) { + std::string dds_topic_name; + dds_topic_name = robot_name + "-" + + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; + ROS_INFO("Comms Bridge: DDS Pub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); + advertisement_info_supplier_.reset( + new GenericRapidPub::AdvertisementInfoSupplier( + dds_topic_name, "", "AstrobeeGenericCommsAdvertisementInfoProfile", "")); + + dds_topic_name = robot_name + "-" + + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; + ROS_INFO("Comms Bridge: DDS Publisher DDS content topic name: %s\n", + dds_topic_name.c_str()); + content_supplier_.reset(new GenericRapidPub::ContentSupplier( + dds_topic_name, "", "AstrobeeGenericCommsContentProfile", "")); + + rapid::RapidHelper::initHeader(advertisement_info_supplier_->event().hdr); + rapid::RapidHelper::initHeader(content_supplier_->event().hdr); +} + +GenericRapidPub::~GenericRapidPub() {} + +template +void GenericRapidPub::CopyString(const int max_size, + std::string src, + T &dest, + std::string const& data_name, + std::string const& topic) { + unsigned int size = src.size(); + if (size > (max_size - 1)) { + ROS_ERROR("Comms Bridge: %s for topic %s is greater than %i characters. Please change idl.", + data_name.c_str(), topic.c_str(), max_size); + size = (max_size - 1); + } + memset(dest, 0, max_size); + strncpy(dest, src.data(), size); + dest[size] = '\0'; +} + +void GenericRapidPub::SendAdvertisementInfo(std::string const& output_topic, + bool latching, + std::string const& data_type, + std::string const& md5_sum, + std::string definition) { + rapid::ext::astrobee::GenericCommsAdvertisementInfo &msg = + advertisement_info_supplier_->event(); + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = ++advertisement_info_seq_; + + // Currrently the output topic can only be 128 characters long + CopyString(128, + output_topic, + msg.outputTopic, + "Out topic", + output_topic); + + msg.latching = latching; + + // Currently the data type can only be 128 characters long + CopyString(128, + data_type, + msg.dataType, + "Data type", + output_topic); + + // Currently the md5 sum can only be 64 characters long + CopyString(64, + md5_sum, + msg.md5Sum, + "Md5 sum", + output_topic); + + // Remove legal statement(s) + std::string::size_type begin = definition.find("# Copyright (c) 20"); + std::string::size_type end; + while (begin != definition.npos) { + end = definition.find("# under the License."); + end += 20; + definition.erase(begin, (end - begin)); + begin = definition.find("# Copyright (c) 20"); + } + + // Currently the ROS message definition can only by 8192 characters long + ROS_INFO("Definition is %i long\n", definition.size()); + CopyString(8192, + definition, + msg.msgDefinition, + "Message definition", + output_topic); + + // Send message + advertisement_info_supplier_->sendEvent(); +} + +void GenericRapidPub::SendContent(std::string const& output_topic, + std::string const& md5_sum, + uint8_t const* data, + const size_t data_size, + const int seq_num) { + unsigned int size; + rapid::ext::astrobee::GenericCommsContent &msg = content_supplier_->event(); + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = seq_num; + + // Currently the output topic can only be 128 characters long + CopyString(128, + output_topic, + msg.outputTopic, + "Out topic", + output_topic); + + // Currently the md5 sum can only be 64 characters long + CopyString(64, + md5_sum, + msg.md5Sum, + "Md5 sum", + output_topic); + + // Currently the content can only be 128K bytes long + size = data_size; + if (size > 131071) { + ROS_ERROR("Comms Bridge: The message data for topic %s is greater than 131072 . Please change idl.", + output_topic); + size = 131071; + } + + msg.data.ensure_length(size, (size + 1)); + unsigned char *buf = msg.data.get_contiguous_buffer(); + if (buf == NULL) { + ROS_ERROR("DDS: RTI didn't give a contiguous buffer for the content data!"); + return; + } + std::memset(buf, 0, (size + 1)); + std::memmove(buf, data, size); + + // Send message + content_supplier_->sendEvent(); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/generic_rapid_sub.cpp b/communications/comms_bridge/src/generic_rapid_sub.cpp new file mode 100644 index 0000000000..ad41b7d35b --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_sub.cpp @@ -0,0 +1,49 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/generic_rapid_sub.h" + +#include + +namespace ff { + +GenericRapidSub::GenericRapidSub(const std::string& entity_name, const std::string& subscribe_topic, + const std::string& subscriber_partition) + : dds_event_loop_(entity_name), + subscribe_topic_(subscribe_topic), + subscriber_partition_(subscriber_partition), + alive_(true) {} + +GenericRapidSub::~GenericRapidSub() { + alive_ = false; // Notify thread to exit + thread_.join(); +} + +void GenericRapidSub::StartThread() { + // Start joinable thread + thread_ = std::thread(&GenericRapidSub::ThreadExec, this); +} + +void GenericRapidSub::ThreadExec() { + while (alive_) { + // process events at 10hz + dds_event_loop_.processEvents(kn::milliseconds(100)); + } +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp new file mode 100644 index 0000000000..4049d1b837 --- /dev/null +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -0,0 +1,286 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/generic_ros_sub_rapid_pub.h" + +#include +#include +#include +#include + +namespace ff { + +GenericROSSubRapidPub::GenericROSSubRapidPub(ros::NodeHandle const* nh) : + dds_initialized_(false), pub_rate_msgs_(false) { + // Setup timer for checking when to publish messages that need to be published + // at a certain rate. The timer will trigger at 10Hz + rate_msg_pub_timer_ = nh->createTimer(ros::Rate(10.0), + &GenericROSSubRapidPub::CheckPubMsg, + this, + false, + false); +} + +GenericROSSubRapidPub::~GenericROSSubRapidPub() {} + +void GenericROSSubRapidPub::AddTopics(std::map>> const& link_entries) { + std::string in_topic, primary_out_topic; + // Make sure dds is initialized before adding topics + if (dds_initialized_) { + for (auto it = link_entries.begin(); it != link_entries.end(); ++it) { + in_topic = it->first; + // Use the first out_topic we read in as the out topic the base class uses + primary_out_topic = it->second[0]->out_topic_; + // Save all robot/out topic pairs so that the bridge can pass the correct + // advertisement info and content message to each roboot that needs it + topic_mapping_[primary_out_topic] = it->second; + + // Check if there is a valid rate that a message needs to be published at + for (size_t i = 0; i < it->second.size(); i++) { + if (it->second[i]->rate_seconds_ > 0) { + pub_rate_msgs_ = true; + rate_topic_entries_.push_back(it->second[i]); + if (FloatMod(it->second[i]->rate_seconds_, 0.1) != 0) { + ROS_ERROR("Rate for %s is not divisible by 0.1 and will not be published at the desired rate.\n", + primary_out_topic.c_str()); + } + } + } + + // Add topic to base class + ROS_INFO("Adding topic %s to base class.", in_topic.c_str()); + addTopic(in_topic, primary_out_topic); + } + } else { + ROS_ERROR("Comms Bridge: Cannot add topics until dds is initialized.\n"); + } + + // If we have messages that get published at a certain rate, start the timer + // that checks when to publish the rate messages + if (pub_rate_msgs_) { + rate_msg_pub_timer_.start(); + } +} + +float GenericROSSubRapidPub::FloatMod(float x, float y) { + int n = static_cast(x/y); + float result = x - n * y; + return result; +} + +void GenericROSSubRapidPub::InitializeDDS(std::vector const& connections) { + std::string robot_name; + for (size_t i = 0; i < connections.size(); ++i) { + robot_name = connections[i]; + GenericRapidPubPtr rapid_pub = std::make_shared(robot_name); + robot_connections_[robot_name] = rapid_pub; + } + + dds_initialized_ = true; +} + +// Called with the mutex held +void GenericROSSubRapidPub::subscribeTopic(std::string const& in_topic, + const RelayTopicInfo& info) { + // this is just the base subscriber letting us know it's adding a topic + // nothing more we need to do +} + +// Called with the mutex held +void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { + const AdvertisementInfo &info = relay_info.ad_info; + std::string out_topic = relay_info.out_topic, robot_name, robot_out_topic; + + ROS_INFO("Received ros advertise topic for topic %s\n", out_topic.c_str()); + + // Make sure we recognize the topic + if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { + ROS_ERROR("Comms Bridge: Output topic %s unknown in advertise topic.\n", + out_topic.c_str()); + return; + } + + for (size_t i = 0; i < topic_mapping_[out_topic].size(); ++i) { + robot_name = topic_mapping_[out_topic][i]->connecting_robot_; + robot_out_topic = topic_mapping_[out_topic][i]->out_topic_; + + ROS_INFO("Robot name: %s Robot out topic: %s\n", + robot_name.c_str(), + robot_out_topic.c_str()); + + // Check robot connection exists + if (robot_connections_.find(robot_name) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name.c_str()); + continue; + } + + robot_connections_[robot_name]->SendAdvertisementInfo(robot_out_topic, + info.latching, + info.data_type, + info.md5_sum, + info.definition); + } +} + +// Called with the mutex held +void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, + ContentInfo const& content_info) { + std::string out_topic = topic_info.out_topic, robot_name, robot_out_topic; + unsigned int size; + ROS_INFO("Received ros content message for topic %s\n", out_topic.c_str()); + + // Make sure we recognize the topic + if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { + ROS_ERROR("Comms Bridge: Output topic %s unknown in relay message.\n", + out_topic.c_str()); + return; + } + + for (size_t i = 0; i < topic_mapping_[out_topic].size(); ++i) { + robot_name = topic_mapping_[out_topic][i]->connecting_robot_; + robot_out_topic = topic_mapping_[out_topic][i]->out_topic_; + + ROS_INFO("Robot name: %s Robot out topic: %s\n", + robot_name.c_str(), + robot_out_topic.c_str()); + + // Check the message is a rate message. If so, don't publish the message. + // Save the data so it can be sent when it needs to be sent. + if (topic_mapping_[out_topic][i]->rate_seconds_ > 0) { + topic_mapping_[out_topic][i]->SetDataToSend(topic_info.relay_seqnum, + content_info.type_md5_sum, + content_info.data_size, + content_info.data); + } else { + // Check robot connection exists + if (robot_connections_.find(robot_name) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name.c_str()); + continue; + } + + robot_connections_[robot_name]->SendContent(robot_out_topic, + content_info.type_md5_sum, + content_info.data, + content_info.data_size, + topic_info.relay_seqnum); + } + } +} + +void GenericROSSubRapidPub::ConvertRequest( + rapid::ext::astrobee::GenericCommsRequest const* data, + std::string const& connecting_robot) { + const std::lock_guard lock(m_mutex_); + + std::string out_topic, robot_out_topic = data->outputTopic; + bool found = false; + + // This is the output topic on the robot and may not match the keyed output + // topic so we need to find the keyed one + // First check if it is the keyed topic + auto search = topic_mapping_.find(robot_out_topic); + if (search != topic_mapping_.end()) { + out_topic = robot_out_topic; + found = true; + } else { + // If it is not the keyed topic, try to find it. + for (auto it = topic_mapping_.begin(); it != topic_mapping_.end() && !found; it++) { + for (size_t i = 0; it->second.size() && !found; i++) { + if (robot_out_topic == it->second[i]->out_topic_) { + out_topic = it->first; + found = true; + } + } + } + } + + // Make sure we found the keyed topic + if (!found) { + ROS_ERROR("Received request for topic %s but it wasn't added to the ros sub rapid pub.\n", + robot_out_topic.c_str()); + return; + } + + std::map::iterator iter = m_relay_topics_.begin(); + while (iter != m_relay_topics_.end()) { + if (iter->second.out_topic == out_topic) + break; + iter++; + } + + if (iter == m_relay_topics_.end()) { + ROS_ERROR("Received request for topic %s but it wasn't added to the bridge subscriber.\n", + out_topic.c_str()); + return; + } + + ROS_INFO("Received reset for topic %s\n", out_topic.c_str()); + + // Check robot connection exists + if (robot_connections_.find(connecting_robot) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s\n", connecting_robot.c_str()); + return; + } + + const AdvertisementInfo &info = iter->second.ad_info; + robot_connections_[connecting_robot]->SendAdvertisementInfo(robot_out_topic, + info.latching, + info.data_type, + info.md5_sum, + info.definition); +} + +void GenericROSSubRapidPub::CheckPubMsg(const ros::TimerEvent& event) { + double time_now = ros::Time::now().toSec(); + std::string robot_name, robot_out_topic; + std::shared_ptr topic_entry; + ROS_ERROR_STREAM("Time now in seconds is: " << time_now); + // Go through all rate messages and publish them if it is time to + for (size_t i = 0; i < rate_topic_entries_.size(); i++) { + topic_entry = rate_topic_entries_[i]; + double time_diff = time_now - topic_entry->last_time_pub_; + // Check if it is time to publish the rate nmessage. + // Add a little time buffer since this timer isn't tailored specifically to + // the message + if ((time_diff + 0.05) > topic_entry->rate_seconds_) { + robot_name = topic_entry->connecting_robot_; + robot_out_topic = topic_entry->out_topic_; + + ROS_ERROR("Rate message for topic %s is being published.", + robot_out_topic.c_str()); + + // Check robot connection exists + if (robot_connections_.find(robot_name) == robot_connections_.end()) { + ROS_ERROR("Comms bridge: No connection for %s.\n", robot_name.c_str()); + continue; + } + + robot_connections_[robot_name]->SendContent(robot_out_topic, + topic_entry->type_md5_sum_, + topic_entry->data_, + topic_entry->data_size_, + topic_entry->seq_num_); + + // Set time published for message + topic_entry->last_time_pub_ = time_now; + } + } +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_pub_request.cpp b/communications/comms_bridge/src/rapid_pub_request.cpp new file mode 100644 index 0000000000..396acd6b5f --- /dev/null +++ b/communications/comms_bridge/src/rapid_pub_request.cpp @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_pub_request.h" + +#include + +namespace ff { + +RapidPubRequest::RapidPubRequest(std::string const& robot_name) : + request_seq_(0) { + std::string dds_topic_name = robot_name + "-" + + rapid::ext::astrobee::GENERIC_COMMS_REQUEST_TOPIC; + ROS_INFO("Comms Bridge: DDS Publisher DDS request topic name: %s\n", + dds_topic_name.c_str()); + request_supplier_.reset(new RapidPubRequest::RequestSupplier( + dds_topic_name, "", "AstrobeeGenericCommsRequestProfile", "")); + + rapid::RapidHelper::initHeader(request_supplier_->event().hdr); +} + +RapidPubRequest::~RapidPubRequest() {} + +void RapidPubRequest::SendRequest(std::string const& output_topic) { + rapid::ext::astrobee::GenericCommsRequest &msg = request_supplier_->event(); + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = ++request_seq_; + + // Currently the output topic can only be 128 characters long + unsigned int size = output_topic.size(); + if (size > 127) { + ROS_ERROR("Comms Bridge: Out topic for topic %s is greater than 128 characters. Please change idl.", + output_topic.c_str()); + size = 127; + } + memset(msg.outputTopic, 0, 128); + strncpy(msg.outputTopic, output_topic.data(), size); + msg.outputTopic[size] = '\0'; + + // Send message + request_supplier_->sendEvent(); +} + + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_sub_advertisement_info.cpp b/communications/comms_bridge/src/rapid_sub_advertisement_info.cpp new file mode 100644 index 0000000000..9bdf7833a7 --- /dev/null +++ b/communications/comms_bridge/src/rapid_sub_advertisement_info.cpp @@ -0,0 +1,57 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_sub_advertisement_info.h" + +#include + +namespace ff { + +RapidSubAdvertisementInfo::RapidSubAdvertisementInfo( + const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub) + : GenericRapidSub(entity_name, subscribe_topic, subscriber_partition), + ros_pub_(rapid_msg_ros_pub) { + // connect to ddsEventLoop + try { + dds_event_loop_.connect( + this, + subscribe_topic, // topic + subscriber_partition, // name + entity_name, // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidSubAdvertisementInfo exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidSubAdvertisementInfo exception unknown"); + throw; + } + + // Start thread + StartThread(); +} + +void RapidSubAdvertisementInfo::operator() ( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* ad_info) { + ros_pub_->ConvertAdvertisementInfo(ad_info); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_sub_content.cpp b/communications/comms_bridge/src/rapid_sub_content.cpp new file mode 100644 index 0000000000..1b3f255e4d --- /dev/null +++ b/communications/comms_bridge/src/rapid_sub_content.cpp @@ -0,0 +1,55 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_sub_content.h" + +#include + +namespace ff { + +RapidSubContent::RapidSubContent(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub) + : GenericRapidSub(entity_name, subscribe_topic, subscriber_partition), + ros_pub_(rapid_msg_ros_pub) { + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + subscribe_topic, // topic + subscriber_partition, // name + entity_name, // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidSubContent exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidSubContent exception unknown"); + throw; + } + + // Start thread + StartThread(); +} + +void RapidSubContent::operator() ( + rapid::ext::astrobee::GenericCommsContent const* content) { + ros_pub_->ConvertContent(content, subscriber_partition_); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_sub_request.cpp b/communications/comms_bridge/src/rapid_sub_request.cpp new file mode 100644 index 0000000000..8817f2c1d5 --- /dev/null +++ b/communications/comms_bridge/src/rapid_sub_request.cpp @@ -0,0 +1,55 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_sub_request.h" + +#include + +namespace ff { + +RapidSubRequest::RapidSubRequest(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericROSSubRapidPub* ros_sub_rapid_pub) + : GenericRapidSub(entity_name, subscribe_topic, subscriber_partition), + ros_sub_(ros_sub_rapid_pub) { + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + subscribe_topic, // topic + subscriber_partition, // name + entity_name, // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidSubRequest exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidSubRequest exception unknown"); + throw; + } + + // Start thread + StartThread(); +} + +void RapidSubRequest::operator() ( + rapid::ext::astrobee::GenericCommsRequest const* request) { + ros_sub_->ConvertRequest(request, subscriber_partition_); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/util.cpp b/communications/comms_bridge/src/util.cpp new file mode 100644 index 0000000000..d90f9bd01c --- /dev/null +++ b/communications/comms_bridge/src/util.cpp @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +ros::Time comms_util::RapidTime2RosTime(const int64_t dds_time) { + ros::Time t; + + // DDS time is in microseconds, we need nanoseconds + t.fromNSec(static_cast(dds_time) * 1000); + + return t; +} + +int64_t comms_util::RosTime2RapidTime(ros::Time const& ros_time) { + return static_cast(ros_time.toNSec() / 1000ULL); +} + +void comms_util::RosHeader2Rapid(std_msgs::Header const& ros_hdr, + rapid::Header* rapid_hdr, + int status, + int serial) { + rapid::RapidHelper::initHeader(*rapid_hdr); + if (serial >= 0) { + rapid_hdr->serial = serial; + } + rapid_hdr->statusCode = status; + rapid_hdr->timeStamp = comms_util::RosTime2RapidTime(ros_hdr.stamp); +} + +void comms_util::RapidHeader2Ros(rapid::Header const& rapid_hdr, std_msgs::Header* ros_hdr, + std::string const& frame_id) { + ros_hdr->frame_id = frame_id; + ros_hdr->stamp = comms_util::RapidTime2RosTime(rapid_hdr.timeStamp); +} diff --git a/communications/dds_msgs/CMakeLists.txt b/communications/dds_msgs/CMakeLists.txt index 6d9c160e76..025c41089d 100644 --- a/communications/dds_msgs/CMakeLists.txt +++ b/communications/dds_msgs/CMakeLists.txt @@ -15,4 +15,110 @@ # License for the specific language governing permissions and limitations # under the License. -add_subdirectory(idl) +cmake_minimum_required(VERSION 3.0) +project(dds_msgs) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +if (USE_DDS) + ## Find catkin macros and libraries + find_package(catkin2 REQUIRED COMPONENTS + ) + + if (USE_CTC) + set(SORACORE_ROOT_DIR ${ARM_CHROOT_DIR}/usr) + else (USE_CTC) + set(SORACORE_ROOT_DIR /usr) + endif (USE_CTC) + + set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) + + list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") + + # find Qt version according to OS + find_program(LSB_RELEASE_EXEC lsb_release) + execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + + if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) + else () + find_package(Qt4 4.6.0 REQUIRED QtXml) + endif () + + + find_package(Miro REQUIRED) + find_package(RtiDds REQUIRED) + find_package(Soracore REQUIRED) + + catkin_package( + INCLUDE_DIRS ${CMAKE_CURRENT_BINARY_DIR}/.. + LIBRARIES + rapidExtAstrobee + ) + + ########### + ## Build ## + ########### + # Specify additional locations of header files + include_directories( + ${catkin_INCLUDE_DIRS} + ${CMAKE_CURRENT_BINARY_DIR} + ${RTIDDS_INCLUDE_DIR} + ${SORACORE_INCLUDE_DIRS} + ${CMAKE_SOURCE_DIR}/rapidDds + ${SORACORE_IDL_DIR}/rapidDds + ) + + # Copying IDL files + file(GLOB ASTROBEE_IDL_FILES + "idl/*.idl") + file(COPY ${ASTROBEE_IDL_FILES} DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) + + set(LIB_NAME "rapidExtAstrobee") + + include(CreateDdsMsgTargets) + + set(EXTRA_RTIDDS_IDL_ARGS -I${SORACORE_IDL_DIR}/rapidDds) + create_dds_msg_targets( + NAME ${LIB_NAME} + DIR ${CMAKE_CURRENT_BINARY_DIR} + INCLUDES "-I${SORACORE_IDL_DIR}/rapidDds") + + add_library(${LIB_NAME} ${rapidExtAstrobee_GENERATED}) + target_link_libraries(${LIB_NAME} + rapidDds + ) + target_compile_definitions(${LIB_NAME} PUBLIC ${RTIDDS_DEFINE_FLAGS}) + target_compile_options(${LIB_NAME} PUBLIC -Wno-write-strings) + target_compile_options(${LIB_NAME} PRIVATE -Wno-strict-aliasing) + target_include_directories(${LIB_NAME} PUBLIC + ${CMAKE_CURRENT_BINARY_DIR} + ${RTIDDS_INCLUDE_DIR} + ${SORACORE_INCLUDE_DIRS} + ${MIRO_INCLUDE_DIR}) + + ############# + ## Install ## + ############# + + # Mark libraries for installation + install(TARGETS ${LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + install(FILES ${rapidExtAstrobee_GENERATED_HEADERS} + DESTINATION include/${LIB_NAME}) + + # Mark cpp header files for installation + install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}/../ + FILES_MATCHING PATTERN "dds_msgs/*.h" + PATTERN ".svn" EXCLUDE + ) +else (USE_DDS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_DDS) \ No newline at end of file diff --git a/communications/dds_msgs/idl/ArmState.idl b/communications/dds_msgs/idl/ArmState.idl new file mode 100644 index 0000000000..7360ce8112 --- /dev/null +++ b/communications/dds_msgs/idl/ArmState.idl @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All Rights Reserved. + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** + //@copy-declaration * The joint state of Astrobee's arm + //@copy-declaration *
    + //@copy-declaration *
  • ARM_JOINT_STATE_UNKNOWN: The arm doesn't know what state it is in + //@copy-declaration *
  • ARM_JOINT_STATE_STOWED: The arm is not deployed (thus not moving) + //@copy-declaration *
  • ARM_JOINT_STATE_DEPLOYING: The arm is deploying itself + //@copy-declaration *
  • ARM_JOINT_STATE_STOPPED: The arm is deployed, but not moving + //@copy-declaration *
  • ARM_JOINT_STATE_MOVING: The arm is deployed and moving + //@copy-declaration *
  • ARM_JOINT_STATE_STOWING: The arm is stowing itself + //@copy-declaration *
+ //@copy-declaration */ + enum ArmJointState { + ARM_JOINT_STATE_UNKNOWN, + ARM_JOINT_STATE_STOWED, + ARM_JOINT_STATE_DEPLOYING, + ARM_JOINT_STATE_STOPPED, + ARM_JOINT_STATE_MOVING, + ARM_JOINT_STATE_STOWING + }; + + //@copy-declaration /** + //@copy-declaration * The state of Astrobee's arm's gripper + //@copy-declaration *
    + //@copy-declaration *
  • ARM_GRIPPER_STATE_UNKNOWN: Astrobee's gripper doesn't know what state it is in + //@copy-declaration *
  • ARM_GRIPPER_STATE_UNCALIBRATED: Astrobee's gripper isn't calibrated and thus doesn't know what state it is in + //@copy-declaration *
  • ARM_GRIPPER_STATE_CALIBRATING: Astrobee's gripper is going through a calibration sequence. + //@copy-declaration *
  • ARM_GRIPPER_STATE_CLOSED: Astrobee's lil' fingers are clenched tight + //@copy-declaration *
  • ARM_GRIPPER_STATE_OPEN: Astrobee's lil' fingers are wide open + //@copy-declaration *
+ //@copy-declaration */ + enum ArmGripperState { + ARM_GRIPPER_STATE_UNKNOWN, + ARM_GRIPPER_STATE_UNCALIBRATED, + ARM_GRIPPER_STATE_CALIBRATING, + ARM_GRIPPER_STATE_CLOSED, + ARM_GRIPPER_STATE_OPEN + }; + + //@copy-c-declaration class ArmStateTypeSupport; + //@copy-c-declaration class ArmStateDataWriter; + //@copy-c-declaration class ArmStateDataReader; + //@copy-c-declaration struct ArmStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of Astrobee's arm + //@copy-declaration */ + valuetype ArmState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef ArmStateTypeSupport TypeSupport; + //@copy-c-declaration typedef ArmStateDataWriter DataWriter; + //@copy-c-declaration typedef ArmStateDataReader DataReader; + //@copy-c-declaration typedef ArmStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef ArmState Type; + + //@copy-declaration /** @see ArmJointState */ + public ArmJointState jointState; + + //@copy-declaration /** @see ArmGripperState */ + public ArmGripperState gripperState; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/AstrobeeAgentState.idl b/communications/dds_msgs/idl/AstrobeeAgentState.idl new file mode 100644 index 0000000000..43f3c26b7b --- /dev/null +++ b/communications/dds_msgs/idl/AstrobeeAgentState.idl @@ -0,0 +1,162 @@ +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All Rights Reserved. + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** + //@copy-declaration * Operating state of Astrobee + //@copy-declaration *
    + //@copy-declaration *
  • OPERATING_STATE_READY: Robot is ready to take commands. + //@copy-declaration *
  • OPERATING_STATE_FAULT: Robot is executing a fault response + //@copy-declaration *
  • OPERATING_STATE_PLAN_EXECUTION: Robot is executing a loaded plan + //@copy-declaration *
  • OPERATING_STATE_TELEOPERATION: Robot is executing a teleop command + //@copy-declaration *
  • OPERATING_STATE_AUTO_RETURN: Robot returns to dock + //@copy-declaration *
  • OPERATING_STATE_GUEST_SCIENCE: Guest science has control. + //@copy-declaration *
+ //@copy-declaration */ + enum OperatingState { + OPERATING_STATE_READY, + OPERATING_STATE_PLAN_EXECUTION, + OPERATING_STATE_TELEOPERATION, + OPERATING_STATE_AUTO_RETURN, + OPERATING_STATE_FAULT + }; + + //@copy-declaration /** + //@copy-declaration * Executing state of Astrobee + //@copy-declaration *
    + //@copy-declaration *
  • EXECUTION_STATE_IDLE: Robot does not have a plan loaded. + //@copy-declaration *
  • EXECUTION_STATE_EXECUTING: Robot is executing a plan + //@copy-declaration *
  • EXECUTION_STATE_PAUSED: Robot is not executing a plan, but has a plan loaded and ready to resume + //@copy-declaration *
  • EXECUTION_STATE_ERROR: Robot was unable to load the plan + //@copy-declaration *
+ //@copy-declaration */ + enum ExecutionState { + EXECUTION_STATE_IDLE, + EXECUTION_STATE_EXECUTING, + EXECUTION_STATE_PAUSED, + EXECUTION_STATE_ERROR + }; + + //@copy-declaration /** + //@copy-declaration * Mobility state of Astrobee + //@copy-declaration *
    + //@copy-declaration *
  • MOBILITY_STATE_DRIFTING: Robot is floating around in spaaaaace + //@copy-declaration *
  • MOBILITY_STATE_STOPPING: Robot is either stopping or stopped + //@copy-declaration *
  • MOBILITY_STATE_FLYING: Robot is flying around in spaaaaace + //@copy-declaration *
  • MOBILITY_STATE_DOCKING: Robot is either docking, docked, or undocking. + //@copy-declaration *
  • MOBILITY_STATE_PERCHING: Robot is either perching, perched, or unperching via the arm. + //@copy-declaration *
+ //@copy-declaration */ + enum MobilityState { + MOBILITY_STATE_DRIFTING, + MOBILITY_STATE_STOPPING, + MOBILITY_STATE_FLYING, + MOBILITY_STATE_DOCKING, + MOBILITY_STATE_PERCHING + }; + + //@copy-c-declaration class AgentStateTypeSupport; + //@copy-c-declaration class AgentStateDataWriter; + //@copy-c-declaration class AgentStateDataReader; + //@copy-c-declaration struct AgentStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of Astrobee. + //@copy-declaration */ + valuetype AgentState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef AgentStateTypeSupport TypeSupport; + //@copy-c-declaration typedef AgentStateDataWriter DataWriter; + //@copy-c-declaration typedef AgentStateDataReader DataReader; + //@copy-c-declaration typedef AgentStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef AgentState Type; + + //@copy-declaration /** @see OperatingState */ + public OperatingState operatingState; + + //@copy-declaration /** @see ExecutionState */ + public ExecutionState executionState; + + //@copy-declaration /** @see MobilityState */ + public MobilityState mobilityState; + + //@copy-declaration /** For MOBILITY_STATE_PERCHING, + //@copy-declaration * MOBILITY_STATE_STOPPING, and + //@copy-declaration * MOBILITY_STATE_DOCKING, this specifies the + //@copy-declaration * progress of the action. For docking, this value + //@copy-declaration * can be N to -N where N through 1 is docking, + //@copy-declaration * 0 is docked, and -1 through -N is undocking. + //@copy-declaration * For stopping, this value can be 1 or where 1 is + //@copy-declaration * stopping and 0 is stopped. For perching, this + //@copy-declaration * value can be N to -N where N through 1 is + //@copy-declaration * perching, 0 is perched, and -1 through -N is + //@copy-declaration * unperching. In other states this field's value + //@copy-declaration * SHOULD be 0. + //@copy-declaration */ + public long subMobilityState; + + //@copy-declaration /** The Executing State of Guest Science mode of Astrobee, + //@copy-declaration * this is only valid if the Operating State is + //@copy-declaration * OPERATING_STATE_GUEST_SCIENCE + //@copy-declaration */ + public ExecutionState guestScienceState; + + //@copy-declaration /** For MOBILITY_STATE_PERCHING and MOBILITY_STATE_DOCKING, + //@copy-declaration * this specifies the proximity to the target. In other states + //@copy-declaration * this field's value SHOULD be 0. + //@copy-declaration */ + public float proximity; + + //@copy-declaration /** Name of configuration, i.e. Nominal, + //@copy-declaration * IgnoreObstacles, Faceforward, Quiet, etc.*/ + public String32 profileName; + + //@copy-declaration /** Defines OMC gains, hard limits, and tolerances + //@copy-declaration * i.e. Nominal, quiet, aggressive, etc. */ + public String32 flightMode; + + //@copy-declaration /** Target speed in m/s */ + public float targetLinearVelocity; + + //@copy-declaration /** Target acceleration in m/s/s */ + public float targetLinearAccel; + + //@copy-declaration /** Target turning speed in rad/s */ + public float targetAngularVelocity; + + //@copy-declaration /** Target turning acceleration in rad/s/s */ + public float targetAngularAccel; + + //@copy-declaration /** The operational hard limit on the collision distance. */ + public float collisionDistance; + + //@copy-declaration /** Allow Astrobee to fly in a direction it does not have cameras pointing */ + public boolean enableHolonomic; + + //@copy-declaration /** Set to false to disable checking for obstacles */ + public boolean checkObstacles; + + //@copy-declaration /** Set to false to disable keepout checks */ + public boolean checkKeepouts; + + //@copy-declaration /** Setting this to true allows Astrobee to + //@copy-declaration * auto return when battery gets low and there is + //@copy-declaration * a LOS */ + public boolean enableAutoReturn; + + //@copy-declaration /** Number of seconds since the Unix Epoch. */ + public long bootTime; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/AstrobeeCommandConstants.idl b/communications/dds_msgs/idl/AstrobeeCommandConstants.idl new file mode 100644 index 0000000000..3126e7a34a --- /dev/null +++ b/communications/dds_msgs/idl/AstrobeeCommandConstants.idl @@ -0,0 +1,506 @@ + +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All Rights Reserved. + */ + +#include "BaseTypes.idl" + +/** + * \file + * + * \brief AstrobeeCommandConstants.idl extends the RAPID CommandConstants.idl file + * with Astrobee-specific commands. + * + * \details Refer to RAPID CommandConstants.idl for interpretation. + * + * \ingroup idlfiles + */ +module rapid { + module ext { + module astrobee { + const rapid::String32 ACCESSCONTROL = "AccessControl"; + + //@copy-declaration /** Grabs control of an Astrobee.

The Astrobee control station implements the RAPID access control protocol, which includes the following steps:

  1. Use Admin.requestCookie to get a unique cookie.
  2. Use this Admin.grabControl command to grab control. Include the cookie received in step 1.
The RAPID access control protocol exists to prevent accidental contention between different operators. Each robot can be under the control of at most one control station at a time. The robot will reject most commands from other control stations, but a few exceptions are made for commands that might be urgently needed to safe the system: any control station can send the Mobility.stopAllMotion and Mobility.idlePropulsion commands. A single control station can grab control of multiple robots, for example, to manage multi-robot guest science. Note that guest science apps running onboard the robot do not need to grab control of the robot to send commands; there is no mechanism to prevent contention between a guest science app and a control station. */ + const rapid::String32 ACCESSCONTROL_METHOD_GRAB_CONTROL = "grabControl"; + + const rapid::String32 ACCESSCONTROL_METHOD_GRAB_CONTROL_PARAM_COOKIE = "cookie"; + const rapid::DataType ACCESSCONTROL_METHOD_GRAB_CONTROL_DTYPE_COOKIE = rapid::RAPID_STRING; + + const rapid::String32 ADMIN = "Admin"; + + //@copy-declaration /** For internal use only. Places the robot in a fault state. This command can only be sent by Astrobee's onboard executive or system monitor nodes. The executive will reject the command if it comes from any other source. */ + const rapid::String32 ADMIN_METHOD_FAULT = "fault"; + + //@copy-declaration /** Calibrates the bias parameters of Astrobee's IMU.

Astrobee's IMU biases slowly drift over time. While Astrobee is actively running its localization system and maintaining a position fix, its EKF automatically corrects for bias drift under the assumption that the local environment is inertially fixed. If the localization system is turned off or loses its fix for an extended period, this command should be used to explicitly calibrate the biases prior to restarting motion. It works by integrating the robot's linear acceleration and angular velocity for about five seconds, and taking the resulting average values as the new zero. During the calibration period, the robot must remain stationary (e.g. docked or perched on a handrail, not held by an astronaut). */ + const rapid::String32 ADMIN_METHOD_INITIALIZE_BIAS = "initializeBias"; + + //@copy-declaration /** This command is intended to load a ROS nodelet in the Astrobee flight software. Please note, the underlying system monitor service can't load nodelets that run on the HLP. */ + const rapid::String32 ADMIN_METHOD_LOAD_NODELET = "loadNodelet"; + + //@copy-declaration /** Name of nodelet */ + const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_NODELET_NAME = "nodeletName"; + const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_NODELET_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Which nodelet manager should load the nodelet. If left blank, the system monitor will default to using the managerName specified in the last heartbeat it received from the requested nodelet. (That should normally work fine, assuming the nodelet sent out at least one valid heartbeat.) */ + const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_MANAGER_NAME = "managerName"; + const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_MANAGER_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Type of nodelet (namespace/classname). If left blank, the system monitor will default to the type specified in its config file, which should normally work fine. */ + const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_TYPE = "type"; + const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_TYPE = rapid::RAPID_STRING; + + //@copy-declaration /** With the current implementation, you should leave this field blank. Specifying a non-empty string (unique id) will activate bond notifications.

Background: ROS nodelets provide a 'bond' mechanism that enables the process requesting a nodelet load (in our case the system monitor) to receive automatic notification if that nodelet crashes. However, our system monitor instead relies on a custom heartbeat message to detect liveness, and it is not currently configured to do anything useful with bond notifications, even if they are activated. */ + const rapid::String32 ADMIN_METHOD_LOAD_NODELET_PARAM_BOND_ID = "bondId"; + const rapid::DataType ADMIN_METHOD_LOAD_NODELET_DTYPE_BOND_ID = rapid::RAPID_STRING; + + //@copy-declaration /** Switches Astrobee's localization to use the general-purpose pipeline.

This command is equivalent to calling Admin.switchLocalization with the "MappedLandmarks" pipeline and then calling Admin.resetEKF. It could be useful as a way to manually recover localization if Astrobee is lost because it is using the wrong localization pipeline (e.g., if docking or perching approach got interrupted, and targets are no longer in view). */ + const rapid::String32 ADMIN_METHOD_REACQUIRE_POSITION = "reacquirePosition"; + + //@copy-declaration /** Resets the EKF component of Astrobee's localization.

This command discards the EKF state and forces the localization system to acquire a new fix. It should not be needed unless the localization system has failure modes where it gets persistently stuck with invalid state information. (Unfortunately, as of 10/2020, that is a frequent occurrence.) */ + const rapid::String32 ADMIN_METHOD_RESET_EKF = "resetEkf"; + + //@copy-declaration /** This rarely used command manually switches between localization pipelines. Normally, Astrobee switches its localization pipeline automatically based on what activity it is conducting (normal free flight, docking, perching, etc.).

Note that, although Astrobee could logically integrate information from multiple pipelines at the same time (e.g., NavCam sparse map features as well as DockCam AR targets), at present, it only uses data from one of these pipelines at a time. IMU information is always integrated, regardless of which pipeline is used. While switching pipelines from A to B, updates from A stop, then there may be a dropout period of a few seconds before B acquires a fix. During the dropout period, localization relies solely on the IMU. */ + const rapid::String32 ADMIN_METHOD_SWITCH_LOCALIZATION = "switchLocalization"; + + //@copy-declaration /** Which pipeline to use:

  • MappedLandmarks: General-purpose localization mode; Astrobee uses its NavCam to recognize landmark features on the ISS interior, as well as for visual odometry.
  • ARTags: During docking, Astrobee uses its DockCam to view AR tags located on the dock and obtain more robust and accurate localization.
  • Handrail: During perching approach, Astrobee uses its PerchCam LIDAR to recognize the geometry of a handrail in front of a wall.
  • Perch: Not implemented. While perched, Astrobee could use a simplified localization approach based on remembering its initial perched pose and updating its pose during pan/tilt motions using a kinematic model of the arm.
  • Truth: During lab testing, Astrobee can optionally subscribe to pose telemetry from an external ground-truth localization system and use it for motion control.
*/ + const rapid::String32 ADMIN_METHOD_SWITCH_LOCALIZATION_PARAM_MODE = "mode"; + const rapid::DataType ADMIN_METHOD_SWITCH_LOCALIZATION_DTYPE_MODE = rapid::RAPID_STRING; + + //@copy-declaration /** This command is intended to unload a ROS nodelet in Astrobee's flight software. Its primary use is in the system monitor's fault response table, where it is the usual response if initialization of a nodelet fails. Please note, the internal service can't unload nodes running on the HLP. */ + const rapid::String32 ADMIN_METHOD_UNLOAD_NODELET = "unloadNodelet"; + + //@copy-declaration /** Name of nodelet */ + const rapid::String32 ADMIN_METHOD_UNLOAD_NODELET_PARAM_NODELET_NAME = "nodeletName"; + const rapid::DataType ADMIN_METHOD_UNLOAD_NODELET_DTYPE_NODELET_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Which nodelet manager should unload the nodelet. If left blank, the system monitor will default to using the managerName specified in the last heartbeat it received from the nodelet. (That should normally work fine, assuming the nodelet sent out at least one valid heartbeat.) */ + const rapid::String32 ADMIN_METHOD_UNLOAD_NODELET_PARAM_MANAGER_NAME = "managerName"; + const rapid::DataType ADMIN_METHOD_UNLOAD_NODELET_DTYPE_MANAGER_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Clears the terminate flag on the robot. This functionality is tied to Astrobee's physical terminate button and terminate indicator LED. The terminate life cycle is as follows:
  • Nominal ops: When Astrobee is active, the amber terminate LED is turned on and the robot functions normally.
  • Terminate: A crew member may press the terminate button as a way to quickly power down the propulsion modules and payloads, while leaving the main processors running and able to communicate with the ground operator. This is a gentler way to intervene than turning off the physical power switch. While terminated, the terminate LED is also turned off.
  • Unterminate: The ground operator can send this command to clear the terminate flag and turn the terminate LED back on. This does not automatically restore power to the hardware.
  • Configure power: Send Power.powerOnItem commands as needed to restore power to individual hardware components.
*/ + const rapid::String32 ADMIN_METHOD_UNTERMINATE = "unterminate"; + + //@copy-declaration /** Wakes an Astrobee from a hibernating state (only EPS powered) by powering up the main processors. This command is intended to also automatically start the flight software stack such that Astrobee is ready to receive Astrobee API commands, but that feature is not implemented.

The Astrobee in question must be docked. Because each wake command is routed through a dock berth, which Astrobee to wake is specified using the berth number.

See also Admin.wakeSafe. The difference between Admin.wake and Admin.wakeSafe is that Admin.wake is intended to automatically start the flight software stack. Since that feature is not implemented, at present, the two commands are equivalent.

Note: For context about how to wake an Astrobee on the ISS, see the procedure IRG-FFTEST207a Astrobee Quick Wakeup and Checkout. */ + const rapid::String32 ADMIN_METHOD_WAKE = "wake"; + + //@copy-declaration /** Which berth the Astrobee is using. 1=left, 2=right. */ + const rapid::String32 ADMIN_METHOD_WAKE_PARAM_BERTH_NUMBER = "berthNumber"; + const rapid::DataType ADMIN_METHOD_WAKE_DTYPE_BERTH_NUMBER = rapid::RAPID_INT; + + //@copy-declaration /** Wakes Astrobee from a hibernating state (only EPS powered) into a safe state (main processors powered, available to log in for debugging, but flight software stack not started).

The Astrobee in question must be docked. Because each wake command is routed through a dock berth, which Astrobee to wake is specified using the berth number. See also Admin.wake. */ + const rapid::String32 ADMIN_METHOD_WAKE_SAFE = "wakeSafe"; + + //@copy-declaration /** Which berth the Astrobee is using. 1=left, 2=right. */ + const rapid::String32 ADMIN_METHOD_WAKE_SAFE_PARAM_BERTH_NUMBER = "berthNumber"; + const rapid::DataType ADMIN_METHOD_WAKE_SAFE_DTYPE_BERTH_NUMBER = rapid::RAPID_INT; + + const rapid::String32 ARM = "Arm"; + + //@copy-declaration /** Moves Astrobee's arm.

The arm has two joints. The tilt joint is used to deploy/stow the arm and adjust the SciCam tilt angle while perched. The pitch joint is used to adjust the SciCam pan angle while perched.

The (pan, tilt) = (0, 0) reference position is defined to have the arm fully deployed and aligned with the robot's -X axis. If perched on a handrail on an ISS wall, this position should nominally make the SciCam camera axis point directly toward the opposite wall. Increasing the tilt angle tilts the SciCam up, and increasing the pan angle pans the SciCam to the right. The arm's stowed position is (pan, tilt) = (0, 180).

The arm joints will be moved sequentially:

  1. Pan: If which is "Pan" or "Both", pan to the specified pan angle.
  2. Tilt: If which is "Tilt" or "Both", tilt to the specified tilt angle.

Naturally, if you prefer to tilt first, you can issue a tilt-only move, followed by a pan-only move.

Some mistakes to avoid include:

  • While the arm is partially within its payload bay (tilt > 90), the pan angle must be 0 to avoid the arm colliding with its payload bay.
  • While the arm is close to its fully stowed state (tilt > 160), the gripper must be closed to avoid colliding with the arm's payload bay.
  • While perched, the nominal tilt range of motion is restricted to -20 .. +90.
  • While perched, the arm may not be able to realize its full nominal range of motion without causing Astrobee's body to collide with the wall. The actual range of motion depends on clutter and how far the handrail projects from the wall.
*/ + const rapid::String32 ARM_METHOD_ARM_PAN_AND_TILT = "armPanAndTilt"; + + //@copy-declaration /** The target pan angle. Ignored if which is "Tilt". */ + const rapid::String32 ARM_METHOD_ARM_PAN_AND_TILT_PARAM_PAN = "pan"; + const rapid::DataType ARM_METHOD_ARM_PAN_AND_TILT_DTYPE_PAN = rapid::RAPID_FLOAT; + + //@copy-declaration /** The target tilt angle. Ignored if which is "Pan". */ + const rapid::String32 ARM_METHOD_ARM_PAN_AND_TILT_PARAM_TILT = "tilt"; + const rapid::DataType ARM_METHOD_ARM_PAN_AND_TILT_DTYPE_TILT = rapid::RAPID_FLOAT; + + //@copy-declaration /** Specifies whether the arm needs to pan, tilt, or both. */ + const rapid::String32 ARM_METHOD_ARM_PAN_AND_TILT_PARAM_WHICH = "which"; + const rapid::DataType ARM_METHOD_ARM_PAN_AND_TILT_DTYPE_WHICH = rapid::RAPID_STRING; + + //@copy-declaration /** Deploys arm. The motion sequence is:
  1. Pan to 0
  2. Tilt to 0

See also Arm.armPanAndTilt for more discussion of the arm. */ + const rapid::String32 ARM_METHOD_DEPLOY_ARM = "deployArm"; + + //@copy-declaration /** Opens or closes gripper.

Astrobee's arm has a passively under-actuated gripper with a single motor. Opening the gripper commands the motor to pull on a pair of tendons that open the fingers. Closing the gripper cuts power to the motor; the fingers are spring-loaded to close. The first time the gripper opens, it calibrates the motor encoder position by fully opening until the fingers contact a hard stop, then relaxes slightly to its nominal open position. On later open cycles, it opens directly to the nominal open position. Note that the motor consumes significant power holding the gripper open, so it should be closed when not actively in use. Holding it open for extended periods (minutes) runs the risk of triggering a motor overtemperature fault, which disables the motor. */ + const rapid::String32 ARM_METHOD_GRIPPER_CONTROL = "gripperControl"; + + //@copy-declaration /** Set to true/false to open/close gripper. */ + const rapid::String32 ARM_METHOD_GRIPPER_CONTROL_PARAM_OPEN = "open"; + const rapid::DataType ARM_METHOD_GRIPPER_CONTROL_DTYPE_OPEN = rapid::RAPID_BOOL; + + //@copy-declaration /** Stops arm motion by commanding it to hold its current joint positions. Does not affect the gripper. */ + const rapid::String32 ARM_METHOD_STOP_ARM = "stopArm"; + + //@copy-declaration /** Stows arm. The motion sequence is:

  1. Close gripper
  2. Pan to 0
  3. Tilt to 180

See also Arm.armPanAndTilt for more discussion of the arm. */ + const rapid::String32 ARM_METHOD_STOW_ARM = "stowArm"; + + const rapid::String32 DATA = "Data"; + + //@copy-declaration /** Sets the data-to-disk configuration, which specifies how to log ROS telemetry topics to the robot's onboard storage.

The configuration is specified in a JSON-formatted file that contains a list of topic entries. For each logged topic, one specifies:

  • frequency: Throttles the maximum rate at which to record messages on the topic. Specify -1 to disable throttling. Note: Currently, throttling is not implemented. You must specify -1.
  • downlinkOption: Which log to store messages in, which must be "Immediate" or "Delayed". The intent is that the "Immediate" log is for high-priority data, which can then be prioritized for downlink after the activity. Note: Currently, you must specify "Delayed" ("Immediate" downlink is currently reserved for internal use by the flight software stack).
Sample data-to-disk configuration files can be found in astrobee_ops/gds/ControlStationConfig.

The Astrobee control station implements the protocol for managing onboard telemetry recording:

  1. Uplink a new data-to-disk file using the RAPID compressed file protocol over DDS. (File uplink is not considered a command, so it does not appear in this command dictionary).
  2. Send this Data.setDataToDisk command to load the uplinked file. Any errors in the file will be reported at this time.
  3. Use the Data.startRecording / Data.stopRecording commands to start / stop onboard telemetry logging.
*/ + const rapid::String32 DATA_METHOD_SET_DATA_TO_DISK = "setDataToDisk"; + + //@copy-declaration /** Starts logging ROS telemetry to onboard storage. See Data.setDataToDisk. */ + const rapid::String32 DATA_METHOD_START_RECORDING = "startRecording"; + + //@copy-declaration /** An optional short description string to include in the filename of the stored telemetry file (ROS bag). Note that the filename always includes a timestamp, which ensures uniqueness. */ + const rapid::String32 DATA_METHOD_START_RECORDING_PARAM_DESCRIPTION = "description"; + const rapid::DataType DATA_METHOD_START_RECORDING_DTYPE_DESCRIPTION = rapid::RAPID_STRING; + + //@copy-declaration /** Stops logging ROS telemetry to onboard storage, as initiated by Data.startRecording. See also Data.setDataToDisk. */ + const rapid::String32 DATA_METHOD_STOP_RECORDING = "stopRecording"; + + const rapid::String32 GUESTSCIENCE = "GuestScience"; + + //@copy-declaration /** Routes a custom command to a guest science app (APK) running on the Astrobee HLP.

For the convenience of the control station operator, each guest science app can define what custom commands it supports in a configuration file read by the control station. See astrobee_ops/gds/ControlStation/PlanEditorGuestScience.config.

See GuestScience.startGuestScience for more on the guest science life cycle. */ + const rapid::String32 GUESTSCIENCE_METHOD_CUSTOM_GUEST_SCIENCE = "customGuestScience"; + + //@copy-declaration /** Which guest science APK to send the command to */ + const rapid::String32 GUESTSCIENCE_METHOD_CUSTOM_GUEST_SCIENCE_PARAM_APK_NAME = "apkName"; + const rapid::DataType GUESTSCIENCE_METHOD_CUSTOM_GUEST_SCIENCE_DTYPE_APK_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** The command to send (usually formatted as a JSON dictionary, enabling an arbitrary number and types of parameters). */ + const rapid::String32 GUESTSCIENCE_METHOD_CUSTOM_GUEST_SCIENCE_PARAM_COMMAND = "command"; + const rapid::DataType GUESTSCIENCE_METHOD_CUSTOM_GUEST_SCIENCE_DTYPE_COMMAND = rapid::RAPID_STRING; + + //@copy-declaration /** This command is the equivalent to issuing the stop guest science command and then the start guest science command. Sometimes guest science APKs become unresponsive and this is a quick way to try to get it work again.

See GuestScience.startGuestScience for more on the guest science life cycle. */ + const rapid::String32 GUESTSCIENCE_METHOD_RESTART_GUEST_SCIENCE = "restartGuestScience"; + + //@copy-declaration /** Which guest science APK to restart */ + const rapid::String32 GUESTSCIENCE_METHOD_RESTART_GUEST_SCIENCE_PARAM_APK_NAME = "apkName"; + const rapid::DataType GUESTSCIENCE_METHOD_RESTART_GUEST_SCIENCE_DTYPE_APK_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** The time in seconds the guest science manager waits in between sending the stop and start commands to the guest science apk. Different apks will need different wait times to allow a complete shutdown before starting again. A 2 second wait time is recommended for simple guest science apks. For the sci_cam_image apk which has to release the science camera hardware resources, we empirically found a conservative wait time of 10 seconds worked reliably. */ + const rapid::String32 GUESTSCIENCE_METHOD_RESTART_GUEST_SCIENCE_PARAM_WAIT = "wait"; + const rapid::DataType GUESTSCIENCE_METHOD_RESTART_GUEST_SCIENCE_DTYPE_WAIT = rapid::RAPID_INT; + + //@copy-declaration /** Starts a guest science app (APK) running on the Astrobee HLP. This command is part of the guest science app life cycle, implemented by the guest science manager node on the HLP. Steps of the life cycle include:

  • startGuestScience: Starts a guest science app, making it available to receive commands.
  • customGuestScience: Routes a command to a running guest science app.
  • stopGuestScience: Terminates a running guest science app.
*/ + const rapid::String32 GUESTSCIENCE_METHOD_START_GUEST_SCIENCE = "startGuestScience"; + + //@copy-declaration /** Which guest science APK to start */ + const rapid::String32 GUESTSCIENCE_METHOD_START_GUEST_SCIENCE_PARAM_APK_NAME = "apkName"; + const rapid::DataType GUESTSCIENCE_METHOD_START_GUEST_SCIENCE_DTYPE_APK_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Terminates a guest science app (APK) running on the Astrobee HLP. See GuestScience.startGuestScience. */ + const rapid::String32 GUESTSCIENCE_METHOD_STOP_GUEST_SCIENCE = "stopGuestScience"; + + //@copy-declaration /** Which guest science APK to terminate */ + const rapid::String32 GUESTSCIENCE_METHOD_STOP_GUEST_SCIENCE_PARAM_APK_NAME = "apkName"; + const rapid::DataType GUESTSCIENCE_METHOD_STOP_GUEST_SCIENCE_DTYPE_APK_NAME = rapid::RAPID_STRING; + + const rapid::String32 MOBILITY = "Mobility"; + + //@copy-declaration /** For expert use only. Returns Astrobee to its dock.

As originally envisioned, this operation could be commanded from anywhere on the ISS, and Astrobee would plan a collision-free path back to the dock. Auto-return could also be invoked as an automatic response to a low-battery fault. To avoid contention for dock berths when multiple Astrobees are in use, each Astrobee should return to the same berth it originally departed from.

However, using this command is not recommended at this time, because (1) it has not been sufficiently tested (including testing of the QP trajectory planner), (2) at present, the proper berth selection logic is not implemented; instead, Astrobee will always return to berth 1 (= left). */ + const rapid::String32 MOBILITY_METHOD_AUTO_RETURN = "autoReturn"; + + //@copy-declaration /** Which berth Astrobee is using. 1=left, 2=right. */ + const rapid::String32 MOBILITY_METHOD_AUTO_RETURN_PARAM_BERTH_NUMBER = "berthNumber"; + const rapid::DataType MOBILITY_METHOD_AUTO_RETURN_DTYPE_BERTH_NUMBER = rapid::RAPID_INT; + + //@copy-declaration /** Docks Astrobee at the specified berth. Preconditions: Astrobee must be near the dock approach point for the specified berth, the berth approach path must not be blocked, and the dock AR tags must not be occluded.

The nominal docking sequence is as follows:

  1. Switch to the "MappedLandmarks" localization pipeline (the default for general-purpose navigation).
  2. Coarsely correct positioning at dock approach point, ensuring dock AR tags are in view.
  3. Switch to "ARTags" localization pipeline (uses AR tags on dock for more accurate dock-relative localization).
  4. Fine-tune positioning at dock approach point.
  5. Complete the docking approach, which should end with Astrobee magnetically retained on the berth, with power and data pins in contact.
  6. Check magnetic retention by attempting to fly away from the dock. Lack of motion indicates success.
  7. Idle propulsion.
  8. Disable localization system.
*/ + const rapid::String32 MOBILITY_METHOD_DOCK = "dock"; + + //@copy-declaration /** Which berth the Astrobee is using. 1=left, 2=right. */ + const rapid::String32 MOBILITY_METHOD_DOCK_PARAM_BERTH_NUMBER = "berthNumber"; + const rapid::DataType MOBILITY_METHOD_DOCK_DTYPE_BERTH_NUMBER = rapid::RAPID_INT; + + //@copy-declaration /** Idles the propulsion impeller motors.

Astrobee will lose control authority. If it is not grounded in some way (docked or perched), it will drift uncontrolled. Note that the impellers have significant inertia, and may take several seconds to spin down.

This command is usually invoked automatically (e.g., at the end of successful docking or perching). It can also be used as a fault response (e.g., responding to an overspeed fault). */ + const rapid::String32 MOBILITY_METHOD_IDLE_PROPULSION = "idlePropulsion"; + + //@copy-declaration /** Perches Astrobee on a handrail. The robot must already be near the appropriate perch approach point for the desired perch location, with the handrail in view of the PerchCam.

The nominal perching sequence is as follows:

  1. Switch to the "Handrail" (handrail-relative) localization pipeline.
  2. Fine-tune positioning at perch approach point.
  3. Deploy arm.
  4. Open gripper.
  5. Move until handrail is in contact, within gripper capture box.
  6. Close gripper.
  7. Confirm grasp by attempting to fly away from the handrail. Lack of motion indicates success.
  8. Idle propulsion.
*/ + const rapid::String32 MOBILITY_METHOD_PERCH = "perch"; + + //@copy-declaration /** Manually spins up the propulsion modules, so that a future motion command can execute without a spin-up delay. This command is rarely used. Normally, you would just command the desired motion and the spin-up would be triggered automatically if needed. This command is intended to help with synchronizing motion of multiple Astrobees, but that scenario hasn't really been tested. */ + const rapid::String32 MOBILITY_METHOD_PREPARE = "prepare"; + + //@copy-declaration /** Undocks Astrobee.

The nominal undocking sequence is as follows:

  1. Switch to the "MappedLandmarks" localization pipeline (the default for general-purpose navigation).
  2. Spin up impellers.
  3. Command the dock berth retention magnets to retract, releasing Astrobee. (They will automatically re-extend after a fixed time delay.)
  4. Move to the dock approach point. (Note: Astrobee attempts to move immediately, without waiting for the berth magnets to fully retract.)
*/ + const rapid::String32 MOBILITY_METHOD_UNDOCK = "undock"; + + //@copy-declaration /** Unperches Astrobee from a handrail.

The nominal unperching sequence is as follows:

  1. Switch to "MappedLandmarks" localization pipeline (the default for general-purpose navigation).
  2. Spin up impellers.
  3. Open gripper, releasing the handrail.
  4. Move to perch approach point.
  5. Stow arm, as in Arm.stowArm.
*/ + const rapid::String32 MOBILITY_METHOD_UNPERCH = "unperch"; + + const rapid::String32 PLAN = "Plan"; + + //@copy-declaration /** Pauses the running plan. The currently executing command will be aborted (equivalent to Mobility.stopAllMotion), and execution of subsequent commands will be delayed until the plan is resumed with a Plan.runPlan command. See plan.setPlan. */ + const rapid::String32 PLAN_METHOD_PAUSE_PLAN = "pausePlan"; + + //@copy-declaration /** Runs the currently loaded plan. If the plan was previously paused with Plan.pausePlan, this command will resume execution where it previously left off. See plan.setPlan. */ + const rapid::String32 PLAN_METHOD_RUN_PLAN = "runPlan"; + + //@copy-declaration /** Loads the most recently uplinked plan.

An Astrobee plan is specified as a JSON-formatted fplan file that contains a list of stations (locations to visit), segments (trajectories for flying between stations), and commands to execute at stations. Plans are generated using the control station plan editor. Sample fplan files can be found in the astrobee/astrobee/plans folder.

The Astrobee control station implements the protocol for managing plans:

  1. Uplink a new fplan file using the RAPID compressed file protocol over DDS. (File uplink is not considered a command, so it does not appear in this command dictionary).
  2. Send this Plan.setPlan command to load the uplinked fplan file. Any errors in the fplan will be reported at this time. Once the plan is loaded, it is initially in the 'paused' state.
  3. Use the Plan.runPlan / Plan.pausePlan commands to start / stop plan execution. Note that after a plan is paused using Plan.pausePlan, Plan.runPlan will resume plan execution at the beginning of the first plan step that was not fully completed. In the case that a trajectory segment is paused partway through, resuming the plan will cause the robot to fly back to the beginning of the segment and execute the whole segment again. If this behavior is not desired, you can use Plan.skipNextStep.
Other related commands include:
  • Plan.skipNextStep: Skips the next step of the currently loaded (and paused) plan.
  • Plan.wait: Same as Plan.pausePlan, but execution will resume automatically after the specified wait duration.
*/ + const rapid::String32 PLAN_METHOD_SET_PLAN = "setPlan"; + + //@copy-declaration /** Skips the next step of the currently loaded plan (the step could be a trajectory segment or a command at a station). The plan must be paused. See plan.setPlan. */ + const rapid::String32 PLAN_METHOD_SKIP_PLAN_STEP = "skipPlanStep"; + + //@copy-declaration /** Temporarily pauses the running plan. This command only affects plan execution, not individual teleoperated commands. Plan execution will resume automatically after the specified duration. See plan.setPlan. */ + const rapid::String32 PLAN_METHOD_WAIT = "wait"; + + //@copy-declaration /** Seconds to pause */ + const rapid::String32 PLAN_METHOD_WAIT_PARAM_DURATION = "duration"; + const rapid::DataType PLAN_METHOD_WAIT_DTYPE_DURATION = rapid::RAPID_FLOAT; + + const rapid::String32 POWER = "Power"; + + //@copy-declaration /** Powers off a component within Astrobee. Note: to turn flashlights on or off, see Settings.setFlashlightBrightness. */ + const rapid::String32 POWER_METHOD_POWER_OFF_ITEM = "powerOffItem"; + + //@copy-declaration /** Which component. Note: 'Front' means 'Forward'. */ + const rapid::String32 POWER_METHOD_POWER_OFF_ITEM_PARAM_WHICH = "which"; + const rapid::DataType POWER_METHOD_POWER_OFF_ITEM_DTYPE_WHICH = rapid::RAPID_STRING; + + //@copy-declaration /** Powers on a component within Astrobee. Note: to turn flashlights on or off, see Settings.setFlashlightBrightness. */ + const rapid::String32 POWER_METHOD_POWER_ON_ITEM = "powerOnItem"; + + //@copy-declaration /** Which component. Note: 'Front' means 'Forward'. */ + const rapid::String32 POWER_METHOD_POWER_ON_ITEM_PARAM_WHICH = "which"; + const rapid::DataType POWER_METHOD_POWER_ON_ITEM_DTYPE_WHICH = rapid::RAPID_STRING; + + const rapid::String32 SETTINGS = "Settings"; + + //@copy-declaration /** Starts the Astrobee intercommunication software. This enables the robot to send and receive a subset of data to and from other Astrobees in the network. Please note, this command must be executed on each robot needed in the communication. */ + const rapid::String32 SETTINGS_METHOD_ENABLE_ASTROBEE_INTERCOMMS = "enableAstrobeeIntercomms"; + + //@copy-declaration /** Sets camera parameters.

The Astrobee camera control life cycle is as follows:

  • When the Astrobee flight software stack is started, recording and streaming are initially disabled for all cameras, and the default camera parameters are as specified in the astrobee/config/cameras.config file.
  • For each camera, while recording and streaming are disabled, you may use this Settings.setCamera command to adjust its parameters.
  • For each camera, you may enable/disable recording imagery to onboard storage using Settings.setCameraRecording.
  • For each camera, you may enable/disable live imagery downlink using Settings.setCameraStreaming.
*/ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA = "setCamera"; + + //@copy-declaration /** Which camera */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_PARAM_CAMERA_NAME = "cameraName"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_DTYPE_CAMERA_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Cameras can both stream and record, potentially with different parameters (typically, you might downlink at lower frame rate and lower resolution). Use this field to specify which mode you want to change parameters for. */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_PARAM_CAMERA_MODE = "cameraMode"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_DTYPE_CAMERA_MODE = rapid::RAPID_STRING; + + //@copy-declaration /** The resolution of the images to produce, in pixels W x H. Specifying resolution smaller than the actual sensor resolution causes binning or downsampling. Only selected downsampling ratios are supported. Available resolutions are:
- SciCam: 1920x1080, 1280x720, 960x540, 480x270.
- NavCam, DockCam: 1280x960, 1024x768, 640x480, 320x240.
- HazCam, PerchCam: 224x171 only */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_PARAM_RESOLUTION = "resolution"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_DTYPE_RESOLUTION = rapid::RAPID_STRING; + + //@copy-declaration /** Frame rate to send. Maximum frame rate varies by camera:
- SciCam: 30 Hz
- NavCam, DockCam: 15 Hz
- HazCam, PerchCam: 5 Hz */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_PARAM_FRAME_RATE = "frameRate"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_DTYPE_FRAME_RATE = rapid::RAPID_FLOAT; + + //@copy-declaration /** Only used for SciCam. Sets target bitrate used in streaming video encoder. Lower bitrate may reduce video quality. */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_PARAM_BANDWIDTH = "bandwidth"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_DTYPE_BANDWIDTH = rapid::RAPID_FLOAT; + + //@copy-declaration /** Sets camera video onboard recording on/off.

Note: For the SciCam, this command actually enables independent recording of H.264 compressed video. For other cameras, this command enables publishing image frames to a topic designated for onboard recording together with other telemetry, but you also need to double-check that the Settings.setDataToDisk configuration will actually log that telemetry topic.

See Settings.setCamera for context. */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_RECORDING = "setCameraRecording"; + + //@copy-declaration /** Which camera */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_RECORDING_PARAM_CAMERA_NAME = "cameraName"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_RECORDING_DTYPE_CAMERA_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Set to true/false to enable/disable camera video onboard recording. */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_RECORDING_PARAM_RECORD = "record"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_RECORDING_DTYPE_RECORD = rapid::RAPID_BOOL; + + //@copy-declaration /** Sets camera streaming live video to the ground on/off.

The SciCam streams H.264 compressed video via RTSP. All other cameras publish independent image frames via DDS.

See Settings.setCamera for context. */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_STREAMING = "setCameraStreaming"; + + //@copy-declaration /** Which camera */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_STREAMING_PARAM_CAMERA_NAME = "cameraName"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_STREAMING_DTYPE_CAMERA_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Set to true/false to enable/disable streaming live video to the ground. */ + const rapid::String32 SETTINGS_METHOD_SET_CAMERA_STREAMING_PARAM_STREAM = "stream"; + const rapid::DataType SETTINGS_METHOD_SET_CAMERA_STREAMING_DTYPE_STREAM = rapid::RAPID_BOOL; + + //@copy-declaration /** Enables/disables obstacle checking */ + const rapid::String32 SETTINGS_METHOD_SET_CHECK_OBSTACLES = "setCheckObstacles"; + + //@copy-declaration /** Set to true/false to enable/disable obstacle checking. */ + const rapid::String32 SETTINGS_METHOD_SET_CHECK_OBSTACLES_PARAM_CHECK_OBSTACLES = "checkObstacles"; + const rapid::DataType SETTINGS_METHOD_SET_CHECK_OBSTACLES_DTYPE_CHECK_OBSTACLES = rapid::RAPID_BOOL; + + //@copy-declaration /** Enables/disables keepout zone checking. See Settings.setZones. */ + const rapid::String32 SETTINGS_METHOD_SET_CHECK_ZONES = "setCheckZones"; + + //@copy-declaration /** Set to true/false to enable/disable keepout zone checking. */ + const rapid::String32 SETTINGS_METHOD_SET_CHECK_ZONES_PARAM_CHECK_ZONES = "checkZones"; + const rapid::DataType SETTINGS_METHOD_SET_CHECK_ZONES_DTYPE_CHECK_ZONES = rapid::RAPID_BOOL; + + //@copy-declaration /** Not implemented. This commmand is intended to enable/disable auto-return to the dock, under the assumption that auto-return will eventually be configured in Astrobee's fault table as the automatic fault response for a low-battery fault. The flag is currently ignored because auto-return has not been sufficiently tested and is not used in the fault table. See Mobility.autoReturn. */ + const rapid::String32 SETTINGS_METHOD_SET_ENABLE_AUTO_RETURN = "setEnableAutoReturn"; + + //@copy-declaration /** Set to true/false to enable/disable auto-return. */ + const rapid::String32 SETTINGS_METHOD_SET_ENABLE_AUTO_RETURN_PARAM_ENABLE_AUTO_RETURN = "enableAutoReturn"; + const rapid::DataType SETTINGS_METHOD_SET_ENABLE_AUTO_RETURN_DTYPE_ENABLE_AUTO_RETURN = rapid::RAPID_BOOL; + + //@copy-declaration /** For expert use only. Changes the semantics of how Astrobee executes fplan trajectory segments.

A segment specifies the motion trajectory between stations in the plan. Within the segment, desired pose and velocity are smoothly interpolated functions of time over a time interval [t0, t1]. Plans created by the Astrobee control station plan editor always specify each segment's time values relative to the start of that segment's execution (i.e. t0 = 0). These plans must be executed in immediate mode, so called because when execution reaches a new segment, the executive and choreographer immediately 'start the clock' on executing the timed trajectory. In principle, if you want to synchronize motion of multiple robots, it could be useful to have the interval [t0, t1] specified using absolute timestamps. That is the behavior when immediate mode is disabled. The timestamp t0 is interpreted as absolute time using ROS conventions (usually UNIX epoch when running on real hardware, but could be any arbitrary time scale when running in simulation), and the start of motion on the segment would be delayed until the current time t = t0.

At this time, we cannot recommend disabling immediate mode to achieve synchronization, due to the following concerns: (1) execution with absolute timestamps has never really been tested, and may be buggy, (2) the control station plan editor doesn't provide any way to generate segments with absolute timestamps, (3) since it is seldom possible to predict exactly when ISS conditions will be right to begin a multi-robot activity, it would be awkward in practice to have the exact absolute timing of segments hard-coded into the plans.

As an alternative way to synchronize motion, you can execute with immediate mode enabled as usual, but take special care to minimize any skew in start time. Upload the plans in advance, use the Mobility.prepare command to get the robots ready to move, and then run the plans simultaneously. Astrobee to Astrobee communication is in the works, and may eventually enable synchronizing Astrobees in a better way. */ + const rapid::String32 SETTINGS_METHOD_SET_ENABLE_IMMEDIATE = "setEnableImmediate"; + + //@copy-declaration /** Set to true/false to enable/disable immediate mode motion control. */ + const rapid::String32 SETTINGS_METHOD_SET_ENABLE_IMMEDIATE_PARAM_ENABLE_IMMEDIATE = "enableImmediate"; + const rapid::DataType SETTINGS_METHOD_SET_ENABLE_IMMEDIATE_DTYPE_ENABLE_IMMEDIATE = rapid::RAPID_BOOL; + + //@copy-declaration /** Allows Astrobee to re-plan if it detects an obstacle too close to its forward trajectory. Enabling replanning only makes sense when you are using a planner that is able to plan around obstacles. (See Setting.setPlanner. As of 5/2021, only the QP planner can plan around obstacles). */ + const rapid::String32 SETTINGS_METHOD_SET_ENABLE_REPLAN = "setEnableReplan"; + + //@copy-declaration /** If true, when Astrobee detects an obstacle too close to its forward trajectory, after the robot comes to a stop, the choreographer will automatically request a new trajectory from the current configured planner. If false, the robot will stop and wait for operator assistance. */ + const rapid::String32 SETTINGS_METHOD_SET_ENABLE_REPLAN_PARAM_ENABLE_REPLAN = "enableReplan"; + const rapid::DataType SETTINGS_METHOD_SET_ENABLE_REPLAN_DTYPE_ENABLE_REPLAN = rapid::RAPID_BOOL; + + //@copy-declaration /** Set the exposure value for either the nav or dock camera. */ + const rapid::String32 SETTINGS_METHOD_SET_EXPOSURE = "setExposure"; + + //@copy-declaration /** Which camera */ + const rapid::String32 SETTINGS_METHOD_SET_EXPOSURE_PARAM_CAMERA_NAME = "cameraName"; + const rapid::DataType SETTINGS_METHOD_SET_EXPOSURE_DTYPE_CAMERA_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** The value to set the exposurec to. */ + const rapid::String32 SETTINGS_METHOD_SET_EXPOSURE_PARAM_EXPOSURE = "exposure"; + const rapid::DataType SETTINGS_METHOD_SET_EXPOSURE_DTYPE_EXPOSURE = rapid::RAPID_FLOAT; + + //@copy-declaration /** Sets flashlight brightness. */ + const rapid::String32 SETTINGS_METHOD_SET_FLASHLIGHT_BRIGHTNESS = "setFlashlightBrightness"; + + //@copy-declaration /** Which flashlight. Note: 'Back' means 'Aft' and 'Front' means 'Forward'. */ + const rapid::String32 SETTINGS_METHOD_SET_FLASHLIGHT_BRIGHTNESS_PARAM_WHICH = "which"; + const rapid::DataType SETTINGS_METHOD_SET_FLASHLIGHT_BRIGHTNESS_DTYPE_WHICH = rapid::RAPID_STRING; + + //@copy-declaration /** Brightness value between 0 (off) and 1 (full brightness). Note that full brightness of an Astrobee flashlight is similar to that of an ordinary pocket flashlight, and may be uncomfortably bright if pointed toward crew eyes. When working with crew, it is advisable to use lower brightness values and/or take steps to avoid pointing the flashlight toward crew. */ + const rapid::String32 SETTINGS_METHOD_SET_FLASHLIGHT_BRIGHTNESS_PARAM_BRIGHTNESS = "brightness"; + const rapid::DataType SETTINGS_METHOD_SET_FLASHLIGHT_BRIGHTNESS_DTYPE_BRIGHTNESS = rapid::RAPID_FLOAT; + + //@copy-declaration /** Enables/disables holonomic mode.

Holonomic mode is sometimes called 'blind flying' because it relaxes the constraint to always point the HazCam in the direction of motion while translating in order to enable obstacle detection. When holonomic mode is enabled, the default trapezoidal planner will simultaneously translate to the target position and rotate to the target attitude. */ + const rapid::String32 SETTINGS_METHOD_SET_HOLONOMIC_MODE = "setHolonomicMode"; + + //@copy-declaration /** Set to true/false to enable/disable holonomic mode. */ + const rapid::String32 SETTINGS_METHOD_SET_HOLONOMIC_MODE_PARAM_ENABLE_HOLONOMIC = "enableHolonomic"; + const rapid::DataType SETTINGS_METHOD_SET_HOLONOMIC_MODE_DTYPE_ENABLE_HOLONOMIC = rapid::RAPID_BOOL; + + //@copy-declaration /** Sets mass and inertia parameters for Astrobee control.

The default inertial parameters for each robot are stored in an onboard configuration file, and should not need to be changed as long as the Astrobee remains in its baseline configuration (four batteries and no payloads installed). However, the inertial parameters may vary when the robot configuration changes. Examples are when a payload is installed or reconfigured (like deploying the arm). When configuration changes for a planned activity are known in advance, the standard management approach is to specify them in the inertiaConfig field of the relevant fplan. This command provides the same parameter update capability, but with more flexibility as to when it is applied (for example, you can change in the middle of a plan, during teleoperation, or when commanded by a guest science app). */ + const rapid::String32 SETTINGS_METHOD_SET_INERTIA = "setInertia"; + + //@copy-declaration /** An optional profile name for this inertia profile. The name has no functional effect, but may be reported in status telemetry and operator displays. */ + const rapid::String32 SETTINGS_METHOD_SET_INERTIA_PARAM_NAME = "name"; + const rapid::DataType SETTINGS_METHOD_SET_INERTIA_DTYPE_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Mass of the Astrobee assembly */ + const rapid::String32 SETTINGS_METHOD_SET_INERTIA_PARAM_MASS = "mass"; + const rapid::DataType SETTINGS_METHOD_SET_INERTIA_DTYPE_MASS = rapid::RAPID_FLOAT; + + //@copy-declaration /** Center of mass of the Astrobee assembly. Specified relative to Astrobee's body frame. */ + const rapid::String32 SETTINGS_METHOD_SET_INERTIA_PARAM_CENTER_OF_MASS = "centerOfMass"; + const rapid::DataType SETTINGS_METHOD_SET_INERTIA_DTYPE_CENTER_OF_MASS = rapid::RAPID_VEC3d; + + //@copy-declaration /** The moment of inertia tensor. Must be a symmetric matrix. Specified relative to the center of mass. */ + const rapid::String32 SETTINGS_METHOD_SET_INERTIA_PARAM_MATRIX = "matrix"; + const rapid::DataType SETTINGS_METHOD_SET_INERTIA_DTYPE_MATRIX = rapid::RAPID_MAT33f; + + //@copy-declaration /** Sets the map used for localization. */ + const rapid::String32 SETTINGS_METHOD_SET_MAP = "setMap"; + + //@copy-declaration /** Full path to the map file to use. 'default' uses the default map loaded on startup. */ + const rapid::String32 SETTINGS_METHOD_SET_MAP_PARAM_MAP_NAME = "mapName"; + const rapid::DataType SETTINGS_METHOD_SET_MAP_DTYPE_MAP_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Changes the value of Astrobee operating limits */ + const rapid::String32 SETTINGS_METHOD_SET_OPERATING_LIMITS = "setOperatingLimits"; + + //@copy-declaration /** An optional profile name for this set of operating limits. The name has no functional effect, but may be reported in status telemetry and operator displays. */ + const rapid::String32 SETTINGS_METHOD_SET_OPERATING_LIMITS_PARAM_PROFILE_NAME = "profileName"; + const rapid::DataType SETTINGS_METHOD_SET_OPERATING_LIMITS_DTYPE_PROFILE_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** Setting the flight mode updates the GN&C gains, hard limits, tolerances, etc., as specified in the world config file. See e.g. astrobee/astrobee/config/worlds/iss.config. Note that executing certain operations, such as docking, automatically switches the flight mode as needed. Values:

  • Off: Turns off propulsion.
  • Quiet: Sets propulsion impeller speed to QUIET (low speed) with gains and limits tuned to safely fly within the reduced performance.
  • Nominal: Sets propulsion impeller speed to NOMINAL (medium speed), with gains and limits tuned for typical utility flying.
  • Difficult: Sets propulsion impeller speed to AGGRESSIVE (high speed), with gains and limits tuned for maximum performance.
  • Precision: Sets propulsion impeller speed to AGGRESSIVE, with gains and limits tuned for precise motion (slow speed, tight position and attitude tolerances).
*/ + const rapid::String32 SETTINGS_METHOD_SET_OPERATING_LIMITS_PARAM_FLIGHT_MODE = "flightMode"; + const rapid::DataType SETTINGS_METHOD_SET_OPERATING_LIMITS_DTYPE_FLIGHT_MODE = rapid::RAPID_STRING; + + //@copy-declaration /** The maximum linear velocity to target while translating */ + const rapid::String32 SETTINGS_METHOD_SET_OPERATING_LIMITS_PARAM_TARGET_LINEAR_VELOCITY = "targetLinearVelocity"; + const rapid::DataType SETTINGS_METHOD_SET_OPERATING_LIMITS_DTYPE_TARGET_LINEAR_VELOCITY = rapid::RAPID_FLOAT; + + //@copy-declaration /** The maximum linear acceleration to target while translating */ + const rapid::String32 SETTINGS_METHOD_SET_OPERATING_LIMITS_PARAM_TARGET_LINEAR_ACCELERATION = "targetLinearAcceleration"; + const rapid::DataType SETTINGS_METHOD_SET_OPERATING_LIMITS_DTYPE_TARGET_LINEAR_ACCELERATION = rapid::RAPID_FLOAT; + + //@copy-declaration /** The maximum angular velocity to target while rotating */ + const rapid::String32 SETTINGS_METHOD_SET_OPERATING_LIMITS_PARAM_TARGET_ANGULAR_VELOCITY = "targetAngularVelocity"; + const rapid::DataType SETTINGS_METHOD_SET_OPERATING_LIMITS_DTYPE_TARGET_ANGULAR_VELOCITY = rapid::RAPID_FLOAT; + + //@copy-declaration /** The maximum angular acceleration to target while rotating */ + const rapid::String32 SETTINGS_METHOD_SET_OPERATING_LIMITS_PARAM_TARGET_ANGULAR_ACCELERATION = "targetAngularAcceleration"; + const rapid::DataType SETTINGS_METHOD_SET_OPERATING_LIMITS_DTYPE_TARGET_ANGULAR_ACCELERATION = rapid::RAPID_FLOAT; + + //@copy-declaration /** Minimum distance margin to maintain away from obstacles */ + const rapid::String32 SETTINGS_METHOD_SET_OPERATING_LIMITS_PARAM_COLLISION_DISTANCE = "collisionDistance"; + const rapid::DataType SETTINGS_METHOD_SET_OPERATING_LIMITS_DTYPE_COLLISION_DISTANCE = rapid::RAPID_FLOAT; + + //@copy-declaration /** Switches which trajectory planner is used when Astrobee needs to generate a trajectory to reach a target pose. Astrobee uses its trajectory planner when it receives a teleoperation motion command. However, when an fplan is generated by the Astrobee control station plan editor, it normally contains pre-computed trajectories for its motion segments; when Astrobee executes these fplans, the onboard trajectory planner is not used. */ + const rapid::String32 SETTINGS_METHOD_SET_PLANNER = "setPlanner"; + + //@copy-declaration /** Which planner to use:
  • Trapezoidal planner: Generates a straight-line trajectory from the start position to the target position, using trapezoidal velocity profiles for translation and rotation. If holonomic mode is disabled (usual case), the robot will (1) rotate to face the target position, (2) translate to the target position while facing forward, (3) rotate to the target attitude. If holonomic mode is enabled, the robot will simultaneously translate along a straight line to the target position while rotating to the target attitude.
  • QP planner: An experimental planner that generates a curved trajectory from start to target, avoiding intervening obstacles. As of 10/2020, using the QP planner is not recommended, because it has not yet been tested on the ISS.
*/ + const rapid::String32 SETTINGS_METHOD_SET_PLANNER_PARAM_PLANNER = "planner"; + const rapid::DataType SETTINGS_METHOD_SET_PLANNER_DTYPE_PLANNER = rapid::RAPID_STRING; + + //@copy-declaration /** Changes the frequency at which one DDS telemetry topic is published. */ + const rapid::String32 SETTINGS_METHOD_SET_TELEMETRY_RATE = "setTelemetryRate"; + + //@copy-declaration /** The DDS telemetry topic to manage. Note: As of Oct 2020, the "CommStatus" topic is not implemented. */ + const rapid::String32 SETTINGS_METHOD_SET_TELEMETRY_RATE_PARAM_TELEMETRY_NAME = "telemetryName"; + const rapid::DataType SETTINGS_METHOD_SET_TELEMETRY_RATE_DTYPE_TELEMETRY_NAME = rapid::RAPID_STRING; + + //@copy-declaration /** The frequency for publishing the topic. */ + const rapid::String32 SETTINGS_METHOD_SET_TELEMETRY_RATE_PARAM_RATE = "rate"; + const rapid::DataType SETTINGS_METHOD_SET_TELEMETRY_RATE_DTYPE_RATE = rapid::RAPID_FLOAT; + + //@copy-declaration /** Loads the most recently uplinked keepout zones file.

The Astrobee control station implements the protocol for managing keepout zones:

  1. Uplink a new keepout zone file using the RAPID compressed file protocol over DDS. (File uplink is not considered a command, so it does not appear in this command dictionary).
  2. Send this Settings.setZones command to load the uplinked zones file. Any errors in the file will be reported at this time.
  3. Use the Settings.setCheckZones command to enable/disable keepout zone checking.
*/ + const rapid::String32 SETTINGS_METHOD_SET_ZONES = "setZones"; + + const rapid::String32 ADMIN_LOCALIZATION_MODE_NONE = "None"; + const rapid::String32 ADMIN_LOCALIZATION_MODE_MAPPED_LANDMARKS = "MappedLandmarks"; + const rapid::String32 ADMIN_LOCALIZATION_MODE_ARTAGS = "ARTags"; + const rapid::String32 ADMIN_LOCALIZATION_MODE_HANDRAIL = "Handrail"; + const rapid::String32 ADMIN_LOCALIZATION_MODE_PERCH = "Perch"; + const rapid::String32 ADMIN_LOCALIZATION_MODE_TRUTH = "Truth"; + + const rapid::String32 ARM_ACTION_TYPE_PAN = "Pan"; + const rapid::String32 ARM_ACTION_TYPE_TILT = "Tilt"; + const rapid::String32 ARM_ACTION_TYPE_BOTH = "Both"; + + const rapid::String32 POWER_POWERED_COMPONENT_LASER_POINTER = "Laser Pointer"; + const rapid::String32 POWER_POWERED_COMPONENT_PAYLOAD_TOP_AFT = "Payload Top Aft"; + const rapid::String32 POWER_POWERED_COMPONENT_PAYLOAD_BOTTOM_AFT = "Payload Bottom Aft"; + const rapid::String32 POWER_POWERED_COMPONENT_PAYLOAD_BOTTOM_FRONT = "Payload Bottom Front"; + const rapid::String32 POWER_POWERED_COMPONENT_PMCS_AND_SIGNAL_LIGHTS = "PMC"; + + const rapid::String32 SETTINGS_CAMERA_MODE_BOTH = "Both"; + const rapid::String32 SETTINGS_CAMERA_MODE_RECORDING = "Recording"; + const rapid::String32 SETTINGS_CAMERA_MODE_STREAMING = "Streaming"; + + const rapid::String32 SETTINGS_CAMERA_NAME_SCI = "Science"; + const rapid::String32 SETTINGS_CAMERA_NAME_NAV = "Navigation"; + const rapid::String32 SETTINGS_CAMERA_NAME_HAZ = "Hazard"; + const rapid::String32 SETTINGS_CAMERA_NAME_DOCK = "Dock"; + const rapid::String32 SETTINGS_CAMERA_NAME_PERCH = "Perch"; + + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_224X171 = "224x171"; + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_320X240 = "320x240"; + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_480X270 = "480x270"; + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_640X480 = "640x480"; + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_960X540 = "960x540"; + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_1024X768 = "1024x768"; + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_1280X720 = "1280x720"; + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_1280X960 = "1280x960"; + const rapid::String32 SETTINGS_CAMERA_RESOLUTION_1920X1080 = "1920x1080"; + + const rapid::String32 SETTINGS_FLASHLIGHT_LOCATION_BACK = "Back"; + const rapid::String32 SETTINGS_FLASHLIGHT_LOCATION_FRONT = "Front"; + + const rapid::String32 SETTINGS_FLIGHT_MODE_OFF = "off"; + const rapid::String32 SETTINGS_FLIGHT_MODE_QUIET = "quiet"; + const rapid::String32 SETTINGS_FLIGHT_MODE_NOMINAL = "nominal"; + const rapid::String32 SETTINGS_FLIGHT_MODE_DIFFICULT = "difficult"; + const rapid::String32 SETTINGS_FLIGHT_MODE_PRECISION = "precision"; + + const rapid::String32 SETTINGS_PLANNER_TYPE_TRAPEZOIDAL = "trapezoidal"; + const rapid::String32 SETTINGS_PLANNER_TYPE_QUADRATIC_PROGRAM = "qp"; + + const rapid::String32 SETTINGS_TELEMETRY_TYPE_COMM_STATUS = "CommStatus"; + const rapid::String32 SETTINGS_TELEMETRY_TYPE_CPU_STATE = "CpuState"; + const rapid::String32 SETTINGS_TELEMETRY_TYPE_DISK_STATE = "DiskState"; + const rapid::String32 SETTINGS_TELEMETRY_TYPE_EKF_STATE = "EkfState"; + const rapid::String32 SETTINGS_TELEMETRY_TYPE_GNC_STATE = "GncState"; + const rapid::String32 SETTINGS_TELEMETRY_TYPE_PMC_CMD_STATE = "PmcCmdState"; + const rapid::String32 SETTINGS_TELEMETRY_TYPE_POSITION = "Position"; + const rapid::String32 SETTINGS_TELEMETRY_TYPE_SPARSE_MAPPING_POSE = "SparseMappingPose"; + + + }; + }; +}; diff --git a/communications/dds_msgs/idl/AstrobeeConstants.idl b/communications/dds_msgs/idl/AstrobeeConstants.idl new file mode 100644 index 0000000000..456e726ad6 --- /dev/null +++ b/communications/dds_msgs/idl/AstrobeeConstants.idl @@ -0,0 +1,57 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "BaseTypes.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** + //@copy-declaration * A list of commonly used topic names. If an Agent has multiple of the same message type, the topics + //@copy-declaration * should be named accordingly and listed in the AgentConfig topics. Custom topic names should + //@copy-declaration * have the standard name as the root, separated from the specialization by the "-" character, e.g. + //@copy-declaration * rapid_position_sample-relative + //@copy-declaration */ + + const String64 AGENT_STATE_TOPIC = "astrobee_agent_state"; + const String64 ARM_STATE_TOPIC = "astrobee_arm_state"; + const String64 COMM_STATE_TOPIC = "astrobee_comm_state"; + const String64 COMPONENT_CONFIG_TOPIC = "astrobee_component_config"; + const String64 COMPONENT_STATE_TOPIC = "astrobee_component_state"; + const String64 COMPRESSED_FILE_TOPIC = "astrobee_compressed_file"; + const String64 COMPRESSED_FILE_ACK_TOPIC = "astrobee_compressed_file_ack"; + const String64 CPU_CONFIG_TOPIC = "astrobee_cpu_config"; + const String64 CPU_STATE_TOPIC = "astrobee_cpu_state"; + const String64 DATA_TO_DISK_STATE_TOPIC = "astrobee_data_to disk_state"; + const String64 DATA_TOPICS_LIST_TOPIC = "astrobee_data_topics_list"; + const String64 DISK_CONFIG_TOPIC = "astrobee_disk_config"; + const String64 DISK_STATE_TOPIC = "astrobee_disk_state"; + const String64 DOCK_STATE_TOPIC = "astrobee_dock_state"; + const String64 EKF_STATE_TOPIC = "astrobee_ekf_state"; + const String64 EPS_CONFIG_TOPIC = "astrobee_eps_config"; + const String64 EPS_STATE_TOPIC = "astrobee_eps_state"; + const String64 FAULT_CONFIG_TOPIC = "astrobee_fault_config"; + const String64 FAULT_STATE_TOPIC = "astrobee_fault_state"; + const String64 GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC = "astrobee_generic_comms_advertisement_info"; + const String64 GENERIC_COMMS_CONTENT_TOPIC = "astrobee_generic_comms_content"; + const String64 GENERIC_COMMS_REQUEST_TOPIC = "astrobee_generic_comms_request"; + const String64 GNC_CONTROL_STATE_TOPIC = "astrobee_gnc_control_state"; + const String64 GNC_FAM_CMD_STATE_TOPIC = "astrobee_gnc_fam_cmd_state"; + const String64 GUEST_SCIENCE_CONFIG_TOPIC = "astrobee_guest_science_config"; + const String64 GUEST_SCIENCE_DATA_TOPIC = "astrobee_guest_science_data"; + const String64 GUEST_SCIENCE_STATE_TOPIC = "astrobee_guest_science_state"; + const String64 INERTIAL_PROPERTIES_TOPIC = "astrobee_inertial_properties"; + const String64 LOG_SAMPLE_TOPIC = "astrobee_log_sample"; + const String64 MOBILITY_SETTINGS_STATE_TOPIC = "astrobee_mobility_settings_state"; + const String64 PAYLOAD_CONFIG_TOPIC = "astrobee_payload_config"; + const String64 PAYLOAD_STATE_TOPIC = "astrobee_payload_state"; + const String64 PLAN_STATUS_TOPIC = "astrobee_plan_status"; + const String64 PMC_CMD_STATE_TOPIC = "astrobee_pmc_cmd_state"; + const String64 TELEMETRY_CONFIG_TOPIC = "astrobee_telemetry_config"; + const String64 TELEMETRY_STATE_TOPIC = "astrobee_telemetry_state"; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/CMakeLists.txt b/communications/dds_msgs/idl/CMakeLists.txt deleted file mode 100644 index bb20495333..0000000000 --- a/communications/dds_msgs/idl/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -message(STATUS "Copying IDL files out of astrobee_common") -file(GLOB DDS_IDL_FILES - "${CMAKE_SOURCE_DIR}/submodules/common/communications/dds/idl/*.idl") - -file(COPY ${DDS_IDL_FILES} DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) - -set(LIB_NAME "rapidExtAstrobee") - -include(CreateDdsMsgTargets) - -set(EXTRA_RTIDDS_IDL_ARGS -I${SORACORE_IDL_DIR}/rapidDds) -create_dds_msg_targets( - NAME ${LIB_NAME} - DIR ${CMAKE_CURRENT_BINARY_DIR} - INCLUDES "-I${SORACORE_IDL_DIR}/rapidDds") - -add_library(${LIB_NAME} ${rapidExtAstrobee_GENERATED}) -target_link_libraries(${LIB_NAME} - rapidDds - ) -target_compile_definitions(${LIB_NAME} PUBLIC ${RTIDDS_DEFINE_FLAGS}) -target_compile_options(${LIB_NAME} PUBLIC -Wno-write-strings) -target_compile_options(${LIB_NAME} PRIVATE -Wno-strict-aliasing) -target_include_directories(${LIB_NAME} PUBLIC - ${CMAKE_CURRENT_BINARY_DIR} - ${RTIDDS_INCLUDE_DIR} - ${SORACORE_INCLUDE_DIRS}) - -install(TARGETS ${LIB_NAME} DESTINATION lib) -install(FILES ${rapidExtAstrobee_GENERATED_HEADERS} - DESTINATION include/${LIB_NAME}) diff --git a/communications/dds_msgs/idl/CommState.idl b/communications/dds_msgs/idl/CommState.idl new file mode 100644 index 0000000000..4a1ce1053f --- /dev/null +++ b/communications/dds_msgs/idl/CommState.idl @@ -0,0 +1,52 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class CommStateTypeSupport; + //@copy-c-declaration class CommStateDataWriter; + //@copy-c-declaration class CommStateDataReader; + //@copy-c-declaration struct CommStateSeq; + + //@copy-declaration /** + //@copy-declaration * Network state of Astrobee + //@copy-declaration */ + valuetype CommState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef CommStateTypeSupport TypeSupport; + //@copy-c-declaration typedef CommStateDataWriter DataWriter; + //@copy-c-declaration typedef CommStateDataReader DataReader; + //@copy-c-declaration typedef CommStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef CommState Type; + + //@copy-declaration /** Whether the MLP wireless is connected */ + public boolean wirelessConnected; + + //@copy-declaration /** Name of the AP Astrobee is connected to */ + public String64 apName; + + //@copy-declaration /** BSSID of the connected AP */ + public String32 bssid; + + //@copy-declaration /** RSSI to the connected AP */ + public float rssi; + + //@copy-declaration /** What frequency (2.4/5.8) we are connected at */ + public float frequency; + + //@copy-declaration /** Which channel within the frequency? */ + public long channel; + + //@copy-declaration /** Whether we are connected to the LAN as well */ + public boolean lanConnected; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/ComponentConfig.idl b/communications/dds_msgs/idl/ComponentConfig.idl new file mode 100644 index 0000000000..d3ef1bc84a --- /dev/null +++ b/communications/dds_msgs/idl/ComponentConfig.idl @@ -0,0 +1,38 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + struct ComponentInfoConfig { + String32 name; + KeyTypeSequence16 metadata; + }; + + typedef sequence ComponentInfoConfigSequence16; + + //@copy-c-declaration class ComponentConfigTypeSupport; + //@copy-c-declaration class ComponentConfigDataWriter; + //@copy-c-declaration class ComponentConfigDataReader; + //@copy-c-declaration struct ComponentConfigSeq; + + //@copy-declaration /** + //@copy-declaration * A list of valid components to listen for states + //@copy-declaration */ + valuetype ComponentConfig : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef ComponentConfigTypeSupport TypeSupport; + //@copy-c-declaration typedef ComponentConfigDataWriter DataWriter; + //@copy-c-declaration typedef ComponentConfigDataReader DataReader; + //@copy-c-declaration typedef ComponentConfigSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef ComponentConfig Type; + + public ComponentInfoConfigSequence16 components; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/ComponentState.idl b/communications/dds_msgs/idl/ComponentState.idl new file mode 100644 index 0000000000..b93a2b9ead --- /dev/null +++ b/communications/dds_msgs/idl/ComponentState.idl @@ -0,0 +1,44 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + struct ComponentInfo { + boolean present; + boolean powered; + float temperature; + float current; + + ValueSequence16 metadata; + }; + + typedef sequence ComponentInfoSequence16; + + //@copy-c-declaration class ComponentStateTypeSupport; + //@copy-c-declaration class ComponentStateDataWriter; + //@copy-c-declaration class ComponentStateDataReader; + //@copy-c-declaration struct ComponentStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of a hardware component within Astrobee + //@copy-declaration * A state message is sent for each different component + //@copy-declaration */ + valuetype ComponentState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef ComponentStateTypeSupport TypeSupport; + //@copy-c-declaration typedef ComponentStateDataWriter DataWriter; + //@copy-c-declaration typedef ComponentStateDataReader DataReader; + //@copy-c-declaration typedef ComponentStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef ComponentState Type; + + public ComponentInfoSequence16 components; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/CompressedFile.idl b/communications/dds_msgs/idl/CompressedFile.idl new file mode 100644 index 0000000000..2d4746caa5 --- /dev/null +++ b/communications/dds_msgs/idl/CompressedFile.idl @@ -0,0 +1,55 @@ +#include "Message.idl" + +module rapid +{ + module ext + { + module astrobee + { + //@copy-declaration /** + //@copy-declaration * Compression format used for file + //@copy-declaration *
    + //@copy-declaration *
  • COMPRESSION_TYPE_NONE: no compression + //@copy-declaration *
  • COMPRESSION_TYPE_DEFLATE: zlib deflate compression + //@copy-declaration *
  • COMPRESSION_TYPE_BZ2: bzip2 + //@copy-declaration *
  • COMPRESSION_TYPE_GZ: GNU Zip + //@copy-declaration *
  • COMPRESSION_TYPE_ZIP: ZIP + //@copy-declaration *
+ //@copy-declaration */ + enum FileCompressionType{ + COMPRESSION_TYPE_NONE, + COMPRESSION_TYPE_DEFLATE, + COMPRESSION_TYPE_BZ2, + COMPRESSION_TYPE_GZ, + COMPRESSION_TYPE_ZIP + }; + + //@copy-c-declaration class CompressedFileTypeSupport; + //@copy-c-declaration class CompressedFileDataWriter; + //@copy-c-declaration class CompressedFileDataReader; + //@copy-c-declaration struct CompressedFileSeq; + + //@copy-declaration /** + //@copy-declaration * CompressedFile message delivers a compressed file, with the compression format used + //@copy-declaration */ + valuetype CompressedFile : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef CompressedFileTypeSupport TypeSupport; + //@copy-c-declaration typedef CompressedFileDataWriter DataWriter; + //@copy-c-declaration typedef CompressedFileDataReader DataReader; + //@copy-c-declaration typedef CompressedFileSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef CompressedFile Type; + + //@copy-declaration /** ID of this compressed file. See CompressedFileAck.idl */ + public long id; + + //@copy-declaration /** Store binary data of compressed file */ + public OctetSequence128K compressedFile; + + //@copy-declaration /** Compression format used on file, to allow proper decompression on receiving end */ + public FileCompressionType compressionType; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/CompressedFileAck.idl b/communications/dds_msgs/idl/CompressedFileAck.idl new file mode 100644 index 0000000000..53125e124c --- /dev/null +++ b/communications/dds_msgs/idl/CompressedFileAck.idl @@ -0,0 +1,31 @@ +#include "Message.idl" + +module rapid +{ + module ext + { + module astrobee + { + //@copy-c-declaration class CompressedFileAckTypeSupport; + //@copy-c-declaration class CompressedFileAckDataWriter; + //@copy-c-declaration class CompressedFileAckDataReader; + //@copy-c-declaration struct CompressedFileAckSeq; + + //@copy-declaration /** + //@copy-declaration * CompressedFileAck message delivers a compressed file, with the compression format used + //@copy-declaration */ + valuetype CompressedFileAck : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef CompressedFileAckTypeSupport TypeSupport; + //@copy-c-declaration typedef CompressedFileAckDataWriter DataWriter; + //@copy-c-declaration typedef CompressedFileAckDataReader DataReader; + //@copy-c-declaration typedef CompressedFileAckSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef CompressedFileAck Type; + + //@copy-declaration /** ID of the sent CompressedFile */ + public long id; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/CpuConfig.idl b/communications/dds_msgs/idl/CpuConfig.idl new file mode 100644 index 0000000000..89010a1355 --- /dev/null +++ b/communications/dds_msgs/idl/CpuConfig.idl @@ -0,0 +1,48 @@ +/* + * Copyright 2017 (c) 2017 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + typedef sequence UnsignedLongSequence8; + + //@copy-declaration /** + //@copy-declaration * The configuration of a cpu + //@copy-declaration * - name: machine name + //@copy-declaration * - num_cpus: number of cpus + //@copy-declaration * - max_frequencies: max frequency for each cpu + //@copy-declaration */ + struct CpuInfoConfig { + String16 name; + short num_cpus; + UnsignedLongSequence8 max_frequencies; + }; + + typedef sequence CpuInfoConfigSequence8; + + //@copy-c-declaration class CpuConfigTypeSupport; + //@copy-c-declaration class CpuConfigDataWriter; + //@copy-c-declaration class CpuConfigDataReader; + //@copy-c-declaration struct CpuConfigSeq; + + //@copy-declaration /** + //@copy-declaration * Config for the CpuState message. + //@copy-declaration */ + valuetype CpuConfig : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERISON_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERISON_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f') + //@copy-c-declaration typedef CpuConfigTypeSupport TypeSupport; + //@copy-c-declaration typedef CpuConfigDataWriter DataWriter; + //@copy-c-declaration typedef CpuConfigDataReader DataReader; + //@copy-c-declaration typedef CpuConfigSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef CpuConfig Type; + + public CpuInfoConfigSequence8 machines; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/CpuState.idl b/communications/dds_msgs/idl/CpuState.idl new file mode 100644 index 0000000000..b660026578 --- /dev/null +++ b/communications/dds_msgs/idl/CpuState.idl @@ -0,0 +1,59 @@ +/* + * Copyright 2017 (c) 2017 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + //@copy-declaration /** + //@copy-declaration * The stats on one of the cpus + //@copy-declaration * - total_load: total load percentage for the cpu + //@copy-declaration * - frequency: current operating frequency in Hz + //@copy-declaration */ + struct CpuInfo { + float total_load; + unsigned long frequency; + }; + + typedef sequence CpuInfoSequence8; + + //@copy-declaration /** + //@copy-declaration * The stats of one of the machines + //@copy-declaration * - name: machine name + //@copy-declaration * - avg_total_load: average total load percentage + //@copy-declaration * - temperature: average of all thermal zones + //@copy-declaration * - cpus: array with info for each cpu + //@copy-declaration */ + struct MachineInfo { + float avg_total_load; + float temperature; + CpuInfoSequence8 cpus; + }; + + typedef sequence MachineInfoSequence8; + + //@copy-c-declaration class CpuStateTypeSupport; + //@copy-c-declaration class CpuStateDataWriter; + //@copy-c-declaration class CpuStateDataReader; + //@copy-c-declaration struct CpuStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of the cpus on Astrobee + //@copy-declaration */ + valuetype CpuState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f') + //@copy-c-declaration typedef CpuStateTypeSupport TypeSupport; + //@copy-c-declaration typedef CpuStateDataWriter DataWriter; + //@copy-c-declaration typedef CpuStateDataReader DataReader; + //@copy-c-declaration typedef CpuStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef CpuState Type; + + //@copy-declaration /** Array with info for each machine */ + public MachineInfoSequence8 machines; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/DataToDiskState.idl b/communications/dds_msgs/idl/DataToDiskState.idl new file mode 100644 index 0000000000..f04c6871ea --- /dev/null +++ b/communications/dds_msgs/idl/DataToDiskState.idl @@ -0,0 +1,61 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** + //@copy-declaration * DownlinkOption indicates if and when the data in this rostopic is downlinked + //@copy-declaration *
    + //@copy-declaration *
  • NONE: topic is not saved to disk + //@copy-declaration *
  • IMMEDIATE: topic saved to disk; upon docking it is downlinked + //@copy-declaration *
  • DELAYED: topic saved to disk; upon docking it is transferred to ISS server for later downlink + //@copy-declaration *
+ //@copy-declaration */ + enum DownlinkOption { + DATA_IMMEDIATE, + DATA_DELAYED + }; + + //@copy-declaration /** Whether and how to save data from a rostopic + //@copy-declaration * - topicName: Name of rostopic + //@copy-declaration * - downlinkOption: when and if the topic should be downlinked + //@copy-declaration * - frequency: times per second to save the data (Hz) + //@copy-declaration */ + struct SaveSetting { + String128 topicName; + DownlinkOption downlinkOption; + float frequency; + }; + + typedef sequence SaveSettingSequence64; + + //@copy-c-declaration class DataToDiskStateTypeSupport; + //@copy-c-declaration class DataToDiskStateDataWriter; + //@copy-c-declaration class DataToDiskStateDataReader; + //@copy-c-declaration struct DataToDiskStateSeq; + + //@copy-declaration /** + //@copy-declaration * Save and downlink settings for all available rostopics + //@copy-declaration */ + valuetype DataToDiskState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef DataToDiskStateTypeSupport TypeSupport; + //@copy-c-declaration typedef DataToDiskStateDataWriter DataWriter; + //@copy-c-declaration typedef DataToDiskStateDataReader DataReader; + //@copy-c-declaration typedef DataToDiskStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef DataToDiskState Type; + + public String128 name; + public boolean recording; + public SaveSettingSequence64 topicSaveSettings; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/DataTopicsList.idl b/communications/dds_msgs/idl/DataTopicsList.idl new file mode 100644 index 0000000000..ed7c9c9fb7 --- /dev/null +++ b/communications/dds_msgs/idl/DataTopicsList.idl @@ -0,0 +1,35 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + typedef sequence String128Sequence256; + + //@copy-c-declaration class DataTopicsListTypeSupport; + //@copy-c-declaration class DataTopicsListDataWriter; + //@copy-c-declaration class DataTopicsListDataReader; + //@copy-c-declaration struct DataTopicsListSeq; + + //@copy-declaration /** + //@copy-declaration * Lists rostopics available to save to disk. + //@copy-declaration */ + valuetype DataTopicsList : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef DataTopicsListTypeSupport TypeSupport; + //@copy-c-declaration typedef DataTopicsListDataWriter DataWriter; + //@copy-c-declaration typedef DataTopicsListDataReader DataReader; + //@copy-c-declaration typedef DataTopicsListSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef DataTopicsList Type; + + public String128Sequence256 topics; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/DiskConfig.idl b/communications/dds_msgs/idl/DiskConfig.idl new file mode 100644 index 0000000000..1935c24eaa --- /dev/null +++ b/communications/dds_msgs/idl/DiskConfig.idl @@ -0,0 +1,44 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** The configuation of a disk + //@copy-declaration * - name: displayable name for the filesystem + //@copy-declaration * - capacity: total capacity in bytes of the disk + //@copy-declaration */ + struct DiskInfoConfig { + String32 name; + long long capacity; + }; + + typedef sequence DiskInfoConfigSequence8; + + //@copy-c-declaration class DiskConfigTypeSupport; + //@copy-c-declaration class DiskConfigDataWriter; + //@copy-c-declaration class DiskConfigDataReader; + //@copy-c-declaration struct DiskConfigSeq; + + //@copy-declaration /** + //@copy-declaration * Config for the DiskState message. + //@copy-declaration */ + valuetype DiskConfig : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef DiskConfigTypeSupport TypeSupport; + //@copy-c-declaration typedef DiskConfigDataWriter DataWriter; + //@copy-c-declaration typedef DiskConfigDataReader DataReader; + //@copy-c-declaration typedef DiskConfigSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef DiskConfig Type; + + public DiskInfoConfigSequence8 filesystems; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/DiskState.idl b/communications/dds_msgs/idl/DiskState.idl new file mode 100644 index 0000000000..9b37e332b0 --- /dev/null +++ b/communications/dds_msgs/idl/DiskState.idl @@ -0,0 +1,42 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** The stats on a disk within Astrobee + //@copy-declaration * - used: number of bytes used + //@copy-declaration */ + struct DiskInfo { + long long used; + }; + + typedef sequence DiskInfoSequence8; + + //@copy-c-declaration class DiskStateTypeSupport; + //@copy-c-declaration class DiskStateDataWriter; + //@copy-c-declaration class DiskStateDataReader; + //@copy-c-declaration struct DiskStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of the data partitions on Astrobee + //@copy-declaration */ + valuetype DiskState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef DiskStateTypeSupport TypeSupport; + //@copy-c-declaration typedef DiskStateDataWriter DataWriter; + //@copy-c-declaration typedef DiskStateDataReader DataReader; + //@copy-c-declaration typedef DiskStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef DiskState Type; + + public DiskInfoSequence8 filesystems; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/DockState.idl b/communications/dds_msgs/idl/DockState.idl new file mode 100644 index 0000000000..835705442f --- /dev/null +++ b/communications/dds_msgs/idl/DockState.idl @@ -0,0 +1,63 @@ +/* + * Copyright 2016 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + struct BerthState { + //@copy-declaration /** Whether this berth is occupied or not */ + boolean occupied; + + //@copy-declaration /** + //@copy-declaration * Name of Astrobee. If there is no Astrobee in + //@copy-declaration * the berth or if the dock doesn't know what the + //@copy-declaration * name of the Astrobee on it is, the name will + //@copy-declaration * be blank. + //@copy-declaration */ + String32 astrobeeName; + + //@copy-declaration /** Whether the freeflyer is awake or hibernated */ + boolean awake; + + //@copy-declaration /** Number of batteries plugged into the astrobee */ + short numBatteries; + + //@copy-declaration /** Maximum capacity of the batteries plugged in */ + long maxCapacity; + + //@copy-declaration /** Current capacity of the batteries plugged in */ + long currentCapacity; + }; + + //@copy-c-declaration class DockStateTypeSupport; + //@copy-c-declaration class DockStateDataWriter; + //@copy-c-declaration class DockStateDataReader; + //@copy-c-declaration struct DockStateSeq; + + //@copy-declaration /** + //@copy-declaration * State message for the dock. Used to tell which + //@copy-declaration * Astrobee is berthed, in which berth, and if + //@copy-declaration * the Astrobee is hibernated or awake. + //@copy-declaration */ + valuetype DockState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef DockStateTypeSupport TypeSupport; + //@copy-c-declaration typedef DockStateDataWriter DataWriter; + //@copy-c-declaration typedef DockStateDataReader DataReader; + //@copy-c-declaration typedef DockStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef DockState Type; + + //@copy-declaration /** State of berth 1 */ + public BerthState berthOne; + + //@copy-declaration /** State of berth 2*/ + public BerthState berthTwo; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/EkfState.idl b/communications/dds_msgs/idl/EkfState.idl new file mode 100644 index 0000000000..37378f43e4 --- /dev/null +++ b/communications/dds_msgs/idl/EkfState.idl @@ -0,0 +1,78 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + typedef sequence FloatSequence15; + typedef sequence FloatSequence50; + + //@copy-c-declaration class EkfStateTypeSupport; + //@copy-c-declaration class EkfStateDataWriter; + //@copy-c-declaration class EkfStateDataReader; + //@copy-c-declaration struct EkfStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of the ekf. + //@copy-declaration */ + valuetype EkfState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef EkfStateTypeSupport TypeSupport; + //@copy-c-declaration typedef EkfStateDataWriter DataWriter; + //@copy-c-declaration typedef EkfStateDataReader DataReader; + //@copy-c-declaration typedef EkfStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef EkfState Type; + + //@copy-declaration /** robot body pose */ + public Transform3D pose; + + //@copy-declaration /** the body velocity (m/s) */ + public Vec3d velocity; + + //@copy-declaration /** body rotational velocity (rad/s) */ + public Vec3d omega; + + //@copy-declaration /** estimated gyro bias (rad/s) */ + public Vec3d gyro_bias; + + //@copy-declaration /** acceleration in body frame (m/s/s) */ + public Vec3d accel; + + //@copy-declaration /** estimated accel bias (m/s/s) */ + public Vec3d accel_bias; + + //@copy-declaration /** Filter Health - covariance diagonal. + //@copy-declaration * 1-3 orientation, 4-6 gyro bias, 7-9 velocity, + //@copy-declaration * 10-12 accel bias, 13-15 position + //@copy-declaration */ + public FloatSequence15 cov_diag; + + //@copy-declaration /** confidence in EKF, 0 is good, 1 is a bit + //@copy-declaration * confused, 2 is lost + //@copy-declaration */ + public short confidence; + + //@copy-declaration /** Status byte sent by GNC */ + public short status; + + //@copy-declaration /** optical flow features this frame + //@copy-declaration * (0 if no update) + //@copy-declaration */ + public short of_count; + + //@copy-declaration /** ml features this frame (0 if no update) */ + public short ml_count; + + //@copy-declaration /** Global handrail pose */ + public Transform3D hr_global_pose; + + //@copy-declaration /** Mahalanobis distances for features */ + public FloatSequence50 ml_mahal_dists; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/EpsConfig.idl b/communications/dds_msgs/idl/EpsConfig.idl new file mode 100644 index 0000000000..8709882f99 --- /dev/null +++ b/communications/dds_msgs/idl/EpsConfig.idl @@ -0,0 +1,61 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** + //@copy-declaration * Location of the battery + //@copy-declaration *
    + //@copy-declaration *
  • SLOT_TOP_LEFT: Battery slot located at the top left side of the robot. + //@copy-declaration *
  • SLOT_TOP_RIGHT: Battery slot located at the top right side of the robot. + //@copy-declaration *
  • SLOT_BOTTOM_LEFT: Battery slot located at the bottom left side of the robot. + //@copy-declaration *
  • SLOT_BOTTOM_RIGHT: Battery slot located at the bottom right side of the robot. + //@copy-declaration *
+ //@copy-declaration */ + enum BatterySlot { + SLOT_TOP_LEFT, + SLOT_TOP_RIGHT, + SLOT_BOTTOM_LEFT, + SLOT_BOTTOM_RIGHT, + SLOT_UNKNOWN + }; + + //@copy-declaration /** The stats of a battery + //@copy-declaration * - slot: location of battery slot" + //@copy-declararion * - maxCapacity: Maximum capacity of the battery in this slot. + //@copy-declaration */ + struct BatteryInfoConfig { + BatterySlot slot; + float designedCapacity; + float currentMaxCapacity; + }; + + typedef sequence BatteryInfoConfigSequence4; + + //@copy-c-declaration class EpsConfigTypeSupport; + //@copy-c-declaration class EpsConfigDataWriter; + //@copy-c-declaration class EpsConfigDataReader; + //@copy-c-declaration struct EpsConfigSeq; + + //@copy-declaration /** + //@copy-declaration * The state of the data partitions on Astrobee + //@copy-declaration */ + valuetype EpsConfig : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef EpsConfigTypeSupport TypeSupport; + //@copy-c-declaration typedef EpsConfigDataWriter DataWriter; + //@copy-c-declaration typedef EpsConfigDataReader DataReader; + //@copy-c-declaration typedef EpsConfigSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef EpsConfig Type; + + public BatteryInfoConfigSequence4 batteries; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/EpsState.idl b/communications/dds_msgs/idl/EpsState.idl new file mode 100644 index 0000000000..61ff7a030d --- /dev/null +++ b/communications/dds_msgs/idl/EpsState.idl @@ -0,0 +1,56 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + struct BatteryInfo { + float percentage; + float temperature; + float voltage; + float current; + float remainingCapacity; + }; + + typedef sequence BatteryInfoSequence4; + + //@copy-c-declaration class EpsStateTypeSupport; + //@copy-c-declaration class EpsStateDataWriter; + //@copy-c-declaration class EpsStateDataReader; + //@copy-c-declaration struct EpsStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of the data partitions on Astrobee + //@copy-declaration */ + valuetype EpsState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef EpsStateTypeSupport TypeSupport; + //@copy-c-declaration typedef EpsStateDataWriter DataWriter; + //@copy-c-declaration typedef EpsStateDataReader DataReader; + //@copy-c-declaration typedef EpsStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef EpsState Type; + + public BatteryInfoSequence4 batteries; + + //@copy-declaration /** The percentage remaining of each battery added + //@copy-declaration * up. This value can be over 100. + //@copy-declaration */ + public float batteryTotal; + + //@copy-declaration /** The estimated time remaining in minutes. It is + //@copy-declaration * calculated by adding up the current capacity + //@copy-declaration * and current for all of the batteries and then + //@copy-declaration * dividing the current capacity by the current. + //@copy-declaration * It is then rounded to the nearest 10 minutes. + //@copy-declaration */ + public long estimatedMinutesRemaining; + }; + + + }; + }; +}; diff --git a/communications/dds_msgs/idl/FaultConfig.idl b/communications/dds_msgs/idl/FaultConfig.idl new file mode 100644 index 0000000000..8d1b7de9a0 --- /dev/null +++ b/communications/dds_msgs/idl/FaultConfig.idl @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All Rights Reserved. + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** Information specific to the fault. + //@copy-declaration * - subsystem: index into the subsystems sequence + //@copy-declaration * in the fault config message + //@copy-declaration * - node: index into the nodes sequence in the + //@copy-declaration * fault config message + //@copy-declaration * - faultId: id of the fault + //@copy-declaration * - warning: Whether the fault is a warning or not + //@copy-declaration * - faultDescription: short description of the fault + //@copy-declaration */ + struct FaultInfo { + short subsystem; + short node; + long faultId; + boolean warning; + String64 faultDescription; + }; + + typedef sequence FaultInfoSequence256; + + //@copy-c-declaration class FaultConfigTypeSupport; + //@copy-c-declaration class FaultConfigDataWriter; + //@copy-c-declaration class FaultConfigDataReader; + //@copy-c-declaration struct FaultConfigSeq; + + //@copy-declaration /** + //@copy-declaration * The Fault config for Astrobee + //@copy-declaration */ + valuetype FaultConfig : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef FaultConfigTypeSupport TypeSupport; + //@copy-c-declaration typedef FaultConfigDataWriter DataWriter; + //@copy-c-declaration typedef FaultConfigDataReader DataReader; + //@copy-c-declaration typedef FaultConfigSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef FaultConfig Type; + + //@copy-declaration /** Names of the subsystems in Astrobee */ + public String32Sequence16 subsystems; + //@copy-declaration /** Names of the nodes in Astrobee */ + public String32Sequence128 nodes; + //@copy-declaration /** A list of faults */ + public FaultInfoSequence256 faults; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/FaultState.idl b/communications/dds_msgs/idl/FaultState.idl new file mode 100644 index 0000000000..b169446e42 --- /dev/null +++ b/communications/dds_msgs/idl/FaultState.idl @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All Rights Reserved. + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** A fault within Astrobee. + //@copy-declaration * - timestamp: when this fault happened, in microseconds + //@copy-declaration * - code: an error code defined elsewhere + //@copy-declaration * - message: string containing why fault occurred + //@copy-declaration * - data: useful data that may accompany the fault + //@copy-declaration */ + struct Fault { + long long timestamp; + long code; + String128 message; + KeyTypeValueSequence8 data; + }; + + typedef sequence FaultSequence32; + + //@copy-c-declaration class FaultStateTypeSupport; + //@copy-c-declaration class FaultStateDataWriter; + //@copy-c-declaration class FaultStateDataReader; + //@copy-c-declaration struct FaultStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of any faults in Astrobee + //@copy-declaration */ + valuetype FaultState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef FaultStateTypeSupport TypeSupport; + //@copy-c-declaration typedef FaultStateDataWriter DataWriter; + //@copy-c-declaration typedef FaultStateDataReader DataReader; + //@copy-c-declaration typedef FaultStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef FaultState Type; + + //@copy-declaration /** Up to 32 different faults within Astrobee */ + public FaultSequence32 faults; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl new file mode 100644 index 0000000000..4267c03a31 --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl @@ -0,0 +1,46 @@ +/* + * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + typedef string<8192> String8K; + + //@copy-c-declaration class GenericCommsAdvertisementInfoTypeSupport; + //@copy-c-declaration class GenericCommsAdvertisementInfoDataWriter; + //@copy-c-declaration class GenericCommsAdvertisementInfoDataReader; + //@copy-c-declaration struct GenericCommsAdvertisementInfoSeq; + + //@copy-declaration /** + //@copy-declaration * Information of the topic type for generic comms. + //@copy-declaration */ + valuetype GenericCommsAdvertisementInfo : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsAdvertisementInfoTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoDataReader DataReader; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsAdvertisementInfo Type; + + //@copy-declaration /** Topic on which to republish */ + public String128 outputTopic; + + //@copy-declaration /** Whether republisher should advertise topic as latching */ + public boolean latching; + + //@copy-declaration /** ROS message data type name */ + public String128 dataType; + + //@copy-declaration /** ROS message md5sum of type */ + public String64 md5Sum; + + //@copy-declaration /** ROS message definition */ + public String8K msgDefinition; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GenericCommsContent.idl b/communications/dds_msgs/idl/GenericCommsContent.idl new file mode 100644 index 0000000000..5ab9cd5a20 --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsContent.idl @@ -0,0 +1,39 @@ +/* + * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class GenericCommsContentTypeSupport; + //@copy-c-declaration class GenericCommsContentDataWriter; + //@copy-c-declaration class GenericCommsContentDataReader; + //@copy-c-declaration struct GenericCommsContentSeq; + + //@copy-declaration /** + //@copy-declaration * Content of a generic comms ROS message + //@copy-declaration */ + valuetype GenericCommsContent : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsContentTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsContentDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsContentDataReader DataReader; + //@copy-c-declaration typedef GenericCommsContentSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsContent Type; + + //@copy-declaration /** Topic on which to republish */ + public String128 outputTopic; + + //@copy-declaration /** The md5 sum of msg type repeated from the original advertisement */ + public String64 md5Sum; + + //@copy-declaration /** Serialized content of the message */ + public OctetSequence128K data; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GenericCommsRequest.idl b/communications/dds_msgs/idl/GenericCommsRequest.idl new file mode 100644 index 0000000000..c3929eb328 --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsRequest.idl @@ -0,0 +1,33 @@ +/* + * Copy 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class GenericCommsRequestTypeSupport; + //@copy-c-declaration class GenericCommsRequestDataWriter; + //@copy-c-declaration class GenericCommsRequestDataReader; + //@copy-c-declaration struct GenericCommsRequestSeq; + + //@copy-declaration /** + //@copy-declaration * Request topic for the generic comms + //@copy-declaration */ + valuetype GenericCommsRequest : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsRequestTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsRequestDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsRequestDataReader DataReader; + //@copy-c-declaration typedef GenericCommsRequestSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsRequest Type; + + //@copy-declaration /** Topic for which metadata retransmission is requested */ + public String128 outputTopic; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GncControlState.idl b/communications/dds_msgs/idl/GncControlState.idl new file mode 100644 index 0000000000..95c748e077 --- /dev/null +++ b/communications/dds_msgs/idl/GncControlState.idl @@ -0,0 +1,42 @@ +/* + * Copyright 2017 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + struct Twist { + Vec3d linear; + Vec3d angular; + }; + + //@copy-c-declaration class GncControlStateTypeSupport; + //@copy-c-declaration class GncControlStateDataWriter; + //@copy-c-declaration class GncControlStateDataReader; + //@copy-c-declaration struct GncControlSeq; + + //@copy-declaration /** + //@copy-declaration * The control state of the GNC. + //@copy-declaration */ + valuetype GncControlState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f') + //@copy-c-declaration typedef GncControlStateTypeSupport TypeSupport; + //@copy-c-declaration typedef GncControlStateDataWriter DataWriter; + //@copy-c-declaration typedef GncControlStateDataReader DataReader; + //@copy-c-declaration typedef GncControlStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GncControlState Type; + + public Transform3D pose; + + public Twist twist; + + public Twist accel; + }; + }; + }; +}; + diff --git a/communications/dds_msgs/idl/GncFamCmdState.idl b/communications/dds_msgs/idl/GncFamCmdState.idl new file mode 100644 index 0000000000..6b87e942f0 --- /dev/null +++ b/communications/dds_msgs/idl/GncFamCmdState.idl @@ -0,0 +1,72 @@ +/* + * Copyright 2017 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** + //@copy-declaration * Represents force in free space. + //@copy-declaration */ + struct Wrench { + Vec3d force; + Vec3d torque; + }; + + //@copy-c-declaration class GncFamCmdStateTypeSupport; + //@copy-c-declaration class GncFamCmdStateDataWriter; + //@copy-c-declaration class GncFamCmdStateDataReader; + //@copy-c-declaration struct GncFamCmdStateSeq; + + //@copy-declaration /** + //@copy-declaration * Command sent from control to FAM. + //@copy-declaration */ + valuetype GncFamCmdState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f') + //@copy-c-declaration typedef GncFamCmdStateTypeSupport TypeSupport; + //@copy-c-declaration typedef GncFamCmdStateDataWriter DataWriter; + //@copy-c-declaration typedef GncFamCmdStateDataReader DataReader; + //@copy-c-declaration typedef GncFamCmdStateSeq seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GncFamCmdState Type; + + //@copy-declaration /** Force and torque */ + public Wrench wrench; + + //@copy-declaration /** Linear acceleration (wrench without estimated + //@copy-declaration * mass) + //@copy-declaration */ + public Vec3d accel; + + //@copy-declaration /** Angular acceleration (wrench without estimated + //@copy-declaration * mass) + //@copy-declaration */ + public Vec3d alpha; + + //@copy-declaration /** Status byte from GNC ICD */ + public short status; + + //@copy-declaration /** Position error */ + public Vec3d position_error; + + //@copy-declaration /** Integrated position error */ + public Vec3d position_error_integrated; + + //@copy-declaration /** Attitude error */ + public Vec3d attitude_error; + + //@copy-declaration /** Integrated attitude error */ + public Vec3d attitude_error_integrated; + + //@copy-declaration /** Magnitude of attitude error */ + public float attitude_error_mag; + + //@copy-declaration /** Control mode from GNC ICD */ + public short control_mode; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GuestScienceConfig.idl b/communications/dds_msgs/idl/GuestScienceConfig.idl new file mode 100644 index 0000000000..d79b668867 --- /dev/null +++ b/communications/dds_msgs/idl/GuestScienceConfig.idl @@ -0,0 +1,53 @@ +/* + * Copyright 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + struct GuestScienceCommand { + //@copy-declaration /** Name of command */ + String32 name; + //@copy-declaration /** Actual syntax of the command */ + String128 command; + }; + typedef sequence GuestScienceCommandSequence32; + + struct GuestScienceApk { + //@copy-declaration /** Full apk name */ + String128 apkName; + //@copy-declaration /** Short name of apk */ + String32 shortName; + //@copy-declaration /** Whether the apk is primary or secondary */ + boolean primary; + //@copy-declaration /** A list of commands the apk will accept */ + GuestScienceCommandSequence32 commands; + }; + typedef sequence GuestScienceApkSequence32; + + //@copy-c-declaration class GuestScienceConfigTypeSupport; + //@copy-c-declaration class GuestScienceConfigDataWriter; + //@copy-c-declaration class GuestScienceConfigDataReader; + //@copy-c-declaration struct GuestScienceConfigSeq; + + //@copy-declaration /** + //@copy-declaration * State message for guest science apk + //@copy-declaration */ + valuetype GuestScienceConfig : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GuestScienceConfigTypeSupport TypeSupport; + //@copy-c-declaration typedef GuestScienceConfigDataWriter DataWriter; + //@copy-c-declaration typedef GuestScienceConfigDataReader DataReader; + //@copy-c-declaration typedef GuestScienceConfigSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GuestScienceConfig Type; + + //@copy-declaration /** List of apk states */ + public GuestScienceApkSequence32 apkStates; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GuestScienceData.idl b/communications/dds_msgs/idl/GuestScienceData.idl new file mode 100644 index 0000000000..177fb911bd --- /dev/null +++ b/communications/dds_msgs/idl/GuestScienceData.idl @@ -0,0 +1,48 @@ +/* + * Copyright 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + //@copy-declaration /** + //@copy-declaration * Guest Science Data Type - used to express the type + //@copy-declaration * of data stored in the data octet + //@copy-declaration */ + enum GuestScienceDataType { + GUEST_SCIENCE_STRING, + GUEST_SCIENCE_JSON, + GUEST_SCIENCE_BINARY + }; + + //@copy-c-declaration class GuestScienceDataTypeSupport; + //@copy-c-declaration class GuestScienceDataDataWriter; + //@copy-c-declaration class GuestScienceDataDataReader; + //@copy-c-declaration struct GuestScienceDataSeq; + + //@copy-declaration /** + //@copy-declaration * Guest science data sent to the ground + //@copy-declaration */ + valuetype GuestScienceData : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GuestScienceDataTypeSupport TypeSupport; + //@copy-c-declaration typedef GuestScienceDataWriter DataWriter; + //@copy-c-declaration typedef GuestScienceDataReader DataReader; + //@copy-c-declaration typedef GuestScienceDataSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GuestScienceData Type; + + //@copy-declaration /** Full name of apk */ + public String128 apkName; + //@copy-declaration /** Type of data being sent */ + public GuestScienceDataType type; + //@copy-declaration /** String to classify the kind of data */ + public String32 topic; + //@copy-declaration /** Data from the apk */ + public OctetSequence2K data; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GuestScienceState.idl b/communications/dds_msgs/idl/GuestScienceState.idl new file mode 100644 index 0000000000..369bcdbd41 --- /dev/null +++ b/communications/dds_msgs/idl/GuestScienceState.idl @@ -0,0 +1,35 @@ +/* + * Copyright 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + typedef sequence BooleanSequence32; + + //@copy-c-declaration class GuestScienceStateTypeSupport; + //@copy-c-declaration class GuestScienceStateDataWriter; + //@copy-c-declaration class GuestScienceStateDataReader; + //@copy-c-declaration struct GuestScienceStateSeq; + + //@copy-declaration /** + //@copy-declaration * Guest science state, list of whether apks are + //@copy-declaration * running or not + //@copy-declaration */ + valuetype GuestScienceState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GuestScienceStateTypeSupport TypeSupport; + //@copy-c-declaration typedef GuestScienceStateDataWriter DataWriter; + //@copy-c-declaration typedef GuestScienceStateDataReader DataReader; + //@copy-c-declaration typedef GuestScienceStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GuestScienceState Type; + + //@copy-declaration /** A list of whether an apk is running or not */ + public BooleanSequence32 runningApks; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/InertialProperties.idl b/communications/dds_msgs/idl/InertialProperties.idl new file mode 100644 index 0000000000..b3b18abdba --- /dev/null +++ b/communications/dds_msgs/idl/InertialProperties.idl @@ -0,0 +1,42 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class InertialPropertiesTypeSupport; + //@copy-c-declaration class InertialPropertiesDataWriter; + //@copy-c-declaration class InertialPropertiesDataReader; + //@copy-c-declaration struct InertialPropertiesSeq; + + //@copy-declaration /** + //@copy-declaration * Network state of Astrobee + //@copy-declaration */ + valuetype InertialProperties : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef InertialPropertiesTypeSupport TypeSupport; + //@copy-c-declaration typedef InertialPropertiesDataWriter DataWriter; + //@copy-c-declaration typedef InertialPropertiesDataReader DataReader; + //@copy-c-declaration typedef InertialPropertiesSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef InertialProperties Type; + + //@copy-declaration /** Name of this set of mass properties */ + public String32 name; + + //@copy-declaration /** Mass of the Astrobee in kg, used for control calculations */ + public float mass; + + //@copy-declaration /** Center of mass of thhe Astrobee, used for control calculations */ + public Vec3d centerOfMass; + + //@copy-declaration /** Inertia matrix of Astrobee, used for control calculations */ + public Mat33f matrix; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/LogSample.idl b/communications/dds_msgs/idl/LogSample.idl new file mode 100644 index 0000000000..3171974cb3 --- /dev/null +++ b/communications/dds_msgs/idl/LogSample.idl @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All Rights Reserved. + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** + //@copy-declaration * Severity level constants + //@copy-declaration *
    + //@copy-declaration *
  • DEBUG: debug level + //@copy-declaration *
  • INFO: general level + //@copy-declaration *
  • WARN: warning level + //@copy-declaration *
  • ERROR: error level + //@copy-declaration *
  • FATAL: fatal/critical level + //@copy-declaration *
+ //@copy-declaration */ + enum SeverityLevel { + SEVERITY_LEVEL_DEBUG, + SEVERITY_LEVEL_INFO, + SEVERITY_LEVEL_WARN, + SEVERITY_LEVEL_ERROR, + SEVERITY_LEVEL_FATAL + }; + + //@copy-c-declaration class LogSampleTypeSupport; + //@copy-c-declaration class LogSampleDataWriter; + //@copy-c-declaration class LogSampleDataReader; + //@copy-c-declaration struct LogSampleSeg; + + //@copy-declaration /** + //@copy-declaration * Message containing ros output. + //@copy-declaration */ + valuetype LogSample : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef LogSampleTypeSupport TypeSupport; + //@copy-c-declaration typedef LogSampleDataWriter DataWriter; + //@copy-c-declaration typedef LogSampleDataReader DataReader; + //@copy-c-declaration typedef LogSampleSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef LogSample Type; + + //@copy-declaration /** @see SeverityLevel */ + public SeverityLevel level; + + //@copy-declaration /** Name of node publishing message */ + public String64 name; + + //@copy-declaration /** Message */ + public String1K msg; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/MobilitySettingsState.idl b/communications/dds_msgs/idl/MobilitySettingsState.idl new file mode 100644 index 0000000000..985e2f44d6 --- /dev/null +++ b/communications/dds_msgs/idl/MobilitySettingsState.idl @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All Rights Reserved. + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class MobilitySettingsStateTypeSupport; + //@copy-c-declaration class MobilitySettingsStateDataWriter; + //@copy-c-declaration class MobilitySettingsStateDataReader; + //@copy-c-declaration struct MobilitySettingsStateSeg; + + //@copy-declaration /** + //@copy-declaration * The mobility settings state of Astrobee. + //@copy-declaration */ + valuetype MobilitySettingsState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef MobilitySettingsStateTypeSupport TypeSupport; + //@copy-c-declaration typedef MobilitySettingsStateDataWriter DataWriter; + //@copy-c-declaration typedef MobilitySettingsStateDataReader DataReader; + //@copy-c-declaration typedef MobilitySettingsStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef MobilitySettingsState Type; + + //@copy-declaration /** Defines PMC gains, hard limits, and tolerances + //@copy-declaration * i.e. Nominal, quiet, aggressive, etc. */ + public String32 flightMode; + + //@copy-declaration /** Target speed in m/s */ + public float targetLinearVelocity; + + //@copy-declaration /** Target acceleration in m/s/s */ + public float targetLinearAccel; + + //@copy-declaration /** Target turning speed in rad/s */ + public float targetAngularVelocity; + + //@copy-declaration /** Target turning acceleration in rad/s/s */ + public float targetAngularAccel; + + //@copy-declaration /** The operational hard limit on the collision distance. */ + public float collisionDistance; + + //@copy-declaration /** Allow Astrobee to fly in a direction it does not have cameras pointing. */ + public boolean enableHolonomic; + + //@copy-declaration /** Set to false to disable checking for obstacles. */ + public boolean checkObstacles; + + //@copy-declaration /** Set to false to disable keepout checks. */ + public boolean checkKeepouts; + + //@copy-declaration /** Setting this to true allows Astrobee to + //@copy-declaration * auto return when battery gets low and there is + //@copy-declaration * a LOS. */ + public boolean enableAutoReturn; + + //@copy-declaration /** Set this to false if mobility should start the + //@copy-declaration * a segment based on its time stamp. */ + public boolean immediateEnabled; + + //@copy-declaration /** Sets the planner mobility uses. */ + public string planner; + + //@copy-declaration /** Setting this to true allows Astrobee to re-plan + //@copy-declaration * if there is an obstacle to close to the robot's + //@copy-declaration * forward trajectory. */ + public boolean replanningEnabled; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/PayloadConfig.idl b/communications/dds_msgs/idl/PayloadConfig.idl new file mode 100644 index 0000000000..5d8c1e51be --- /dev/null +++ b/communications/dds_msgs/idl/PayloadConfig.idl @@ -0,0 +1,35 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + typedef sequence String32Sequence4; + + //@copy-c-declaration class PayloadConfigTypeSupport; + //@copy-c-declaration class PayloadConfigDataWriter; + //@copy-c-declaration class PayloadConfigDataReader; + //@copy-c-declaration struct PayloadConfigSeq; + + //@copy-declaration /** + //@copy-declaration * Configuration message for a PayloadState message + //@copy-declaration */ + valuetype PayloadConfig : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef PayloadConfigTypeSupport TypeSupport; + //@copy-c-declaration typedef PayloadConfigDataWriter DataWriter; + //@copy-c-declaration typedef PayloadConfigDataReader DataReader; + //@copy-c-declaration typedef PayloadConfigSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef PayloadConfig Type; + + public String32Sequence4 payloadNames; + }; + }; + }; + +}; diff --git a/communications/dds_msgs/idl/PayloadState.idl b/communications/dds_msgs/idl/PayloadState.idl new file mode 100644 index 0000000000..d98e8afd09 --- /dev/null +++ b/communications/dds_msgs/idl/PayloadState.idl @@ -0,0 +1,33 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + typedef sequence BooleanSequence4; + + //@copy-c-declaration class PayloadStateTypeSupport; + //@copy-c-declaration class PayloadStateDataWriter; + //@copy-c-declaration class PayloadStateDataReader; + //@copy-c-declaration struct PayloadStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of Astrobee's payloads + //@copy-declaration */ + valuetype PayloadState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef PayloadStateTypeSupport TypeSupport; + //@copy-c-declaration typedef PayloadStateDataWriter DataWriter; + //@copy-c-declaration typedef PayloadStateDataReader DataReader; + //@copy-c-declaration typedef PayloadStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef PayloadState Type; + + public BooleanSequence4 powered; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/PlanStatus.idl b/communications/dds_msgs/idl/PlanStatus.idl new file mode 100644 index 0000000000..e4e35b173b --- /dev/null +++ b/communications/dds_msgs/idl/PlanStatus.idl @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All Rights Reserved. + */ + +#include "Ack.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-declaration /** The completed status of a thing in the past. + //@copy-declaration * Specifies the thing and the Completed status + //@copy-declaration * Also specifies the duration that the thing took + //@copy-declaration * @see AckCompletedStatus + //@copy-declaration */ + struct Status { + long point; + long command; + long duration; + AckCompletedStatus status; + }; + + //@copy-declaration /** A secquence of status events */ + typedef sequence StatusSequence; + + //@copy-c-declaration class PlanStatusTypeSupport; + //@copy-c-declaration class PlanStatusDataWriter; + //@copy-c-declaration class PlanStatusDataReader; + //@copy-c-declaration struct PlanStatusSeq; + + //@copy-declaration /** + //@copy-declaration * The status of the current plan that may or may not be running. + //@copy-declaration */ + valuetype PlanStatus : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef PlanStatusTypeSupport TypeSupport; + //@copy-c-declaration typedef PlanStatusDataWriter DataWriter; + //@copy-c-declaration typedef PlanStatusDataReader DataReader; + //@copy-c-declaration typedef PlanStatusSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef PlanStatus Type; + + //@copy-declaration /** the name of the currently executing/paused plan */ + public String128 planName; + + //@copy-declaration /** The current station or segment */ + public long currentPoint; + + //@copy-declaration /** The current command within the station/segment. -1 means no command */ + public long currentCommand; + + //@copy-declaration /** Status of the currently executing 'thing' @see AckStatus */ + public AckStatus currentStatus; + + //@copy-declaration /** Status of the last N things (N=64) @see AckCompletedStatus */ + public StatusSequence statusHistory; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/PmcCmdState.idl b/communications/dds_msgs/idl/PmcCmdState.idl new file mode 100644 index 0000000000..b59626918b --- /dev/null +++ b/communications/dds_msgs/idl/PmcCmdState.idl @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2015 United States Government as represented by the + * Administrator of the National Aeronautics and Space Adminstration. + * All Rights Reserved. + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + typedef sequence ShortSequence6; + + //@copy-declaration /** Command state for one PMC unit + //@copy-declaration * - motorSpeed: Motor speed. Unitless. 0-255 + //@copy-declaration * - nozzlePositions: Nozzle positions. Unitless. 0-255 + //@copy-declaration */ + struct PmcGoal { + short motorSpeed; + ShortSequence6 nozzlePositions; + }; + + typedef sequence PmcGoalSequence8; + + //@copy-c-declaration class PmcCmdStateTypeSupport; + //@copy-c-declaration class PmcCmdStateDataWriter; + //@copy-c-declaration class PmcCmdStateDataReader; + //@copy-c-declaration struct PmcCmdStateSeq; + + //@copy-declaration /** + //@copy-declaration * Command from the Force Allocation Module to the PMCs + //@copy-declaration */ + valuetype PmcCmdState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef PmcCmdStateTypeSupport TypeSupport; + //@copy-c-declaration typedef PmcCmdStateDataWriter DataWriter; + //@copy-c-declaration typedef PmcCmdStateDataReader DataReader; + //@copy-c-declaration typedef PmcCmdStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef PmcCmdState Type; + + //@copy-declaration /** Commands to the pmcs */ + public PmcGoalSequence8 goals; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/idl/TelemetryConfig.idl b/communications/dds_msgs/idl/TelemetryConfig.idl new file mode 100644 index 0000000000..a3c174b43d --- /dev/null +++ b/communications/dds_msgs/idl/TelemetryConfig.idl @@ -0,0 +1,68 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + enum CameraResolution { + // 4:3 resolutions + RESOLUTION_320_240, + RESOLUTION_640_480, + RESOLUTION_1024_768, + RESOLUTION_1280_960, + + // 16:9 resolutions + RESOLUTION_480_270, + RESOLUTION_960_540, + RESOLUTION_1280_720, + RESOLUTION_1920_1080, + + // Other resolutions + RESOLUTION_224_171, + + // Start up resolution + RESOLUTION_1_1 + }; + + typedef sequence CameraResolutionSequence8; + + enum CameraMode { + MODE_VIDEO, + MODE_FRAMES + }; + + struct CameraInfoConfig { + String32 name; + CameraResolutionSequence8 availResolutions; + CameraMode mode; + float maxFrameRate; + }; + + typedef sequence CameraInfoConfigSequence8; + + //@copy-c-declaration class TelemetryConfigTypeSupport; + //@copy-c-declaration class TelemetryConfigDataWriter; + //@copy-c-declaration class TelemetryConfigDataReader; + //@copy-c-declaration struct TelemetryConfigSeq; + + //@copy-declaration /** + //@copy-declaration * Configuration message for a TelemetryState message + //@copy-declaration */ + valuetype TelemetryConfig : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef TelemetryConfigTypeSupport TypeSupport; + //@copy-c-declaration typedef TelemetryConfigDataWriter DataWriter; + //@copy-c-declaration typedef TelemetryConfigDataReader DataReader; + //@copy-c-declaration typedef TelemetryConfigSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef TelemetryConfig Type; + + public CameraInfoConfigSequence8 cameras; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/TelemetryState.idl b/communications/dds_msgs/idl/TelemetryState.idl new file mode 100644 index 0000000000..b0ca30bbe6 --- /dev/null +++ b/communications/dds_msgs/idl/TelemetryState.idl @@ -0,0 +1,52 @@ +/* + * Copyright 2015 (c) 2015 Intelligent Robotics Group, NASA ARC + */ + +#include "TelemetryConfig.idl" + +module rapid { + module ext { + module astrobee { + + struct CameraInfo { + boolean streaming; + boolean recording; + CameraResolution resolution; + float frameRate; + float bandwidth; + }; + + typedef sequence CameraInfoSequence8; + + //@copy-c-declaration class TelemetryStateTypeSupport; + //@copy-c-declaration class TelemetryStateDataWriter; + //@copy-c-declaration class TelemetryStateDataReader; + //@copy-c-declaration struct TelemetryStateSeq; + + //@copy-declaration /** + //@copy-declaration * The state of telemetry flowing to GDS + //@copy-declaration */ + valuetype TelemetryState : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef TelemetryStateTypeSupport TypeSupport; + //@copy-c-declaration typedef TelemetryStateDataWriter DataWriter; + //@copy-c-declaration typedef TelemetryStateDataReader DataReader; + //@copy-c-declaration typedef TelemetryStateSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef TelemetryState Type; + + public float commStatusRate; + public float cpuStateRate; + public float diskStateRate; + public float ekfStateRate; + public float gncStateRate; + public float pmcCmdStateRate; + public float positionRate; + public float sparseMappingPoseRate; + + public CameraInfoSequence8 cameras; + }; + + }; + }; +}; diff --git a/communications/dds_msgs/package.xml b/communications/dds_msgs/package.xml new file mode 100644 index 0000000000..0f35132e90 --- /dev/null +++ b/communications/dds_msgs/package.xml @@ -0,0 +1,21 @@ + + + dds_msgs + 0.0.0 + + The dds_msgs package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + message_generation + message_runtime + catkin + + diff --git a/communications/dds_ros_bridge/CMakeLists.txt b/communications/dds_ros_bridge/CMakeLists.txt index c72a60d561..9f2e2886bf 100644 --- a/communications/dds_ros_bridge/CMakeLists.txt +++ b/communications/dds_ros_bridge/CMakeLists.txt @@ -1,64 +1,110 @@ # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(dds_ros_bridge) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +if (USE_DDS) +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + nodelet + config_reader + ff_util + ff_msgs + ff_hw_msgs + dds_msgs +) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) +find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options timer) + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") + +if (USE_CTC) + set(SORACORE_ROOT_DIR ${ARM_CHROOT_DIR}/usr) +else (USE_CTC) + set(SORACORE_ROOT_DIR /usr) +endif (USE_CTC) + +set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) + +SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# find Qt version according to OS +find_program(LSB_RELEASE_EXEC lsb_release) +execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) +else () + find_package(Qt4 4.6.0 REQUIRED QtXml) +endif () + +find_package(Miro REQUIRED) +find_package(RtiDds REQUIRED) +find_package(Soracore REQUIRED) + + catkin_package( LIBRARIES dds_ros_bridge - CATKIN_DEPENDS roscpp message_runtime std_msgs nav_msgs + CATKIN_DEPENDS nodelet config_reader ff_util ff_msgs ff_hw_msgs dds_msgs ) -set(DEPS - ff_msgs - ) +########### +## Build ## +########### -set(LIBS - config_reader - ff_nodelet - Qt4::QtXml - rapidIo - rapidExtAstrobee - ) - -set(INCLUDES - ${CMAKE_CURRENT_SOURCE_DIR}/include +# Specify additional locations of header files +include_directories( + include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${RTIDDS_INCLUDE_DIR} ${SORACORE_INCLUDE_DIRS} + ${MIRO_INCLUDE_DIR} + ${QT_INCLUDE_DIR} + ${QT_INCLUDE_DIR}/Qt + ${Boost_INCLUDE_DIRS} ) -set(RTI_QOS_FILES - ${CMAKE_SOURCE_DIR}/submodules/common/communications/dds/NDDS_DISCOVERY_PEERS - ${CMAKE_SOURCE_DIR}/submodules/common/communications/dds/RAPID_QOS_PROFILES.xml +file(GLOB cc_files + "src/*.cc" ) -set(CONFIG_DIR ${CMAKE_SOURCE_DIR}/astrobee/config/communications/dds) - -create_library(TARGET dds_ros_bridge - LIBS ${LIBS} - INC ${INCLUDES} - DEPS ${DEPS} -) - -create_tool_targets(DIR tools - LIBS dds_ros_bridge - INC ${INCLUDES} - DEPS ${DEPS} dds_ros_bridge +# Declare C++ libraries +add_library(dds_ros_bridge + ${cc_files} ) +target_compile_definitions(dds_ros_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) +add_dependencies(dds_ros_bridge ${catkin_EXPORTED_TARGETS}) +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + target_link_libraries(dds_ros_bridge rapidIo Qt5::Xml ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +else () + target_link_libraries(dds_ros_bridge rapidIo ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +endif () +############# +## Install ## +############# # Determine our module name get_filename_component(MODULE_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME) @@ -70,12 +116,23 @@ install(CODE "execute_process( ERROR_QUIET )") -install_launch_files() - -# Copy these files outside of an install to make available -# with rosrun/roslaunch -file(MAKE_DIRECTORY ${CONFIG_DIR}) -file(COPY ${RTI_QOS_FILES} - DESTINATION ${CONFIG_DIR}) +# Mark libraries for installation +install(TARGETS dds_ros_bridge + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) +else (USE_DDS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_DDS) \ No newline at end of file diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/astrobee_astrobee_bridge.h b/communications/dds_ros_bridge/include/dds_ros_bridge/astrobee_astrobee_bridge.h new file mode 100644 index 0000000000..f08ef95c02 --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/astrobee_astrobee_bridge.h @@ -0,0 +1,103 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef DDS_ROS_BRIDGE_ASTROBEE_ASTROBEE_BRIDGE_H_ +#define DDS_ROS_BRIDGE_ASTROBEE_ASTROBEE_BRIDGE_H_ + +#include + +#include +#include +#include +#include + +#include "config_reader/config_reader.h" + +#include "dds_ros_bridge/rapid_pub.h" +#include "dds_ros_bridge/rapid_sub_ros_pub.h" +#include "dds_ros_bridge/ros_sub_rapid_pub.h" + +#include "dds_ros_bridge/rapid_ekf_ros_ekf.h" +#include "dds_ros_bridge/rapid_gs_data_ros_gs_data.h" +#include "dds_ros_bridge/ros_ekf_rapid_ekf.h" +#include "dds_ros_bridge/ros_guest_science.h" + +#include "ff_msgs/ResponseOnly.h" + +#include "ff_common/ff_names.h" +#include "ff_util/ff_nodelet.h" + +// SoraCore Includes +#include "knDds/DdsSupport.h" +#include "knDds/DdsEntitiesFactory.h" +#include "knDds/DdsEntitiesFactorySvc.h" + +// miro includes +#include "miro/Configuration.h" +#include "miro/Robot.h" +#include "miro/Log.h" + +namespace kn { + class DdsEntitiesFactorySvc; +} // end namespace kn + +namespace dds_ros_bridge { + +class RosSubRapidPub; + +class AstrobeeAstrobeeBridge : public ff_util::FreeFlyerNodelet { + public: + AstrobeeAstrobeeBridge(); + ~AstrobeeAstrobeeBridge(); + + template + int BuildRosToRapid(const std::string& name, Args&& ... args); // NOLINT + + template + int BuildRapidToRos(Args&& ... args); // NOLINT + + protected: + virtual void Initialize(ros::NodeHandle *nh); + bool ReadParams(); + bool ReadSharedItemConf(config_reader::ConfigReader::Table &conf, + std::string &topic_name, bool &enable, float &rate); + + private: + bool Run(); + bool Start(ff_msgs::ResponseOnly::Request& req, + ff_msgs::ResponseOnly::Response& res); + + config_reader::ConfigReader config_params_; + + int components_; + bool started_; + bool run_on_start_; + + ros::NodeHandle nh_; + ros::ServiceServer trigger_srv_; + + std::map ros_sub_rapid_pubs_; + std::shared_ptr dds_entities_factory_; + std::string agent_name_, participant_name_, read_params_error_; + std::vector rapid_pubs_; + std::vector rapid_sub_ros_pubs_; +}; + +} // end namespace dds_ros_bridge + +#endif // DDS_ROS_BRIDGE_ASTROBEE_ASTROBEE_BRIDGE_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h b/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h index 108f009231..a488d1a988 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h @@ -54,15 +54,18 @@ #include "dds_ros_bridge/ros_inertia.h" #include "dds_ros_bridge/ros_log_sample.h" #include "dds_ros_bridge/ros_odom_rapid_position.h" +#include "dds_ros_bridge/ros_payload_state.h" #include "dds_ros_bridge/ros_plan_status_rapid_plan_status.h" #include "dds_ros_bridge/ros_pmc_cmd_state.h" +#include "dds_ros_bridge/ros_pose_rapid_position.h" #include "dds_ros_bridge/ros_string_rapid_text_message.h" #include "dds_ros_bridge/ros_sub_rapid_pub.h" #include "dds_ros_bridge/ros_telemetry_rapid_telemetry.h" +#include "dds_ros_bridge/ros_zones_rapid_compressed_file.h" #include "ff_msgs/SetRate.h" -#include "ff_util/ff_names.h" +#include "ff_common/ff_names.h" #include "ff_util/ff_nodelet.h" // SoraCore Includes @@ -91,7 +94,8 @@ enum RateType { EKF_STATE, GNC_STATE, PMC_CMD_STATE, - POSITION + POSITION, + SPARSE_MAPPING_POSE }; class RosSubRapidPub; @@ -104,117 +108,94 @@ class DdsRosBridge : public ff_util::FreeFlyerNodelet { /** * Build Ros subscribers to Rapid publishers * @param subTopic topic to subscribe to ex: "/camera/image" - * @param pubTopic topic suffix for RAPID ex: "-debug" - * @return number of RosSubRapidPubs + * @return successful */ - int BuildAccessControlStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildAckToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildAgentStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildArmJointSampleToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildArmStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildBatteryStateToRapid(const std::string& sub_topic_battery_state_TL, - const std::string& sub_topic_battery_state_TR, - const std::string& sub_topic_battery_state_BL, - const std::string& sub_topic_battery_state_BR, - const std::string& sub_topic_battery_temp_TL, - const std::string& sub_topic_battery_temp_TR, - const std::string& sub_topic_battery_temp_BL, - const std::string& sub_topic_battery_temp_BR, - const std::string& pub_topic, - const std::string& name); - int BuildCommandToRapid(const std::string& sub_topic, - const std::string& ac_sub_topic, - const std::string& failed_cmd_sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildCompressedImageToImage(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildCompressedFileToRapid(const std::string& sub_topic, - const std::string& pub_topic, + bool BuildAccessControlStateToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildAckToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildAgentStateToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildArmJointSampleToRapid(const std::string& sub_topic, const std::string& name); - int BuildCompressedFileAckToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildCpuStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, + bool BuildArmStateToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildBatteryStateToRapid(const std::string& sub_topic_battery_state_TL, + const std::string& sub_topic_battery_state_TR, + const std::string& sub_topic_battery_state_BL, + const std::string& sub_topic_battery_state_BR, + const std::string& sub_topic_battery_temp_TL, + const std::string& sub_topic_battery_temp_TR, + const std::string& sub_topic_battery_temp_BL, + const std::string& sub_topic_battery_temp_BR, + const std::string& name); + bool BuildCommandToRapid(const std::string& sub_topic, + const std::string& ac_sub_topic, + const std::string& failed_cmd_sub_topic, const std::string& name); - int BuildDataToDiskStateToRapid(const std::string& state_sub_topic, - const std::string& topics_sub_topic, - const std::string& pub_topic, + bool BuildCompressedFileToRapid(const std::string& sub_topic, const std::string& name); - int BuildDiskStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, + bool BuildCompressedFileAckToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildCompressedImageToImage(const std::string& sub_topic, + const std::string& name); + bool BuildCpuStateToRapid(const std::string& sub_topic, const std::string& name); - int BuildFaultConfigToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildFaultStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, + bool BuildDataToDiskStateToRapid(const std::string& state_sub_topic, + const std::string& topics_sub_topic, + const std::string& name); + bool BuildDiskStateToRapid(const std::string& sub_topic, const std::string& name); - int BuildGncFamCmdStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildGncControlStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildGuestScienceToRapid(const std::string& state_sub_topic, - const std::string& config_sub_topic, - const std::string& data_sub_topic, - const std::string& pub_topic, + bool BuildFaultConfigToRapid(const std::string& sub_topic, const std::string& name); - int BuildInertiaDataToRapid(const std::string& sub_topic, - const std::string& pub_topic, + bool BuildFaultStateToRapid(const std::string& sub_topic, const std::string& name); - int BuildLogSampleToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildOdomToPosition(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildPlanStatusToPlanStatus(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildPmcCmdStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, + bool BuildGncFamCmdStateToRapid(const std::string& sub_topic, const std::string& name); - int BuildStringToTextMessage(const std::string& sub_topic, - const std::string& pub_topic, + bool BuildGncControlStateToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildGuestScienceToRapid(const std::string& state_sub_topic, + const std::string& config_sub_topic, + const std::string& data_sub_topic, + const std::string& name); + bool BuildInertiaDataToRapid(const std::string& sub_topic, const std::string& name); - int BuildTelemetryToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); + bool BuildLogSampleToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildOdomToPosition(const std::string& sub_topic, + const std::string& name); + bool BuildPayloadStateToPayloadState(const std::string& sub_topic, + const std::string& name); + bool BuildPlanStatusToPlanStatus(const std::string& sub_topic, + const std::string& name); + bool BuildPmcCmdStateToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildPoseToPosition(const std::string& sub_topic, + const std::string& name); + bool BuildStringToTextMessage(const std::string& name); + bool BuildTelemetryToRapid(const std::string& sub_topic, + const std::string& name); + bool BuildZonesToRapidCompressedFile(const std::string& sub_topic, + const std::string& zones_srv, + const std::string& name); /** * Build Rapid subscribers to Ros publishers * @param subscribeTopic topix suffix for RAPID ex : "-debug" * @param pubTopic topic to publish to * - * @return number of RapidPubRosSubs + * @return successful */ - int BuildCommandToCommand(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); - int BuildCompressedFileToCompressedFile(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name); + bool BuildCommandToCommand(const std::string& pub_topic, + const std::string& name); + bool BuildCompressedFileToCompressedFile(const std::string& pub_topic, + const std::string& name); /** * Build Rapid publisher for one time message */ - int BuildCommandConfigToCommandConfig(const std::string& pub_topic, - const std::string& name); + bool BuildCommandConfigToCommandConfig(const std::string& name); bool SetTelem(float rate, std::string &err_msg, RateType type); @@ -224,12 +205,17 @@ class DdsRosBridge : public ff_util::FreeFlyerNodelet { bool SetEkfPositionRate(float rate, std::string &err_msg, RateType type); bool SetGncStateRate(float rate, std::string &err_msg); bool SetPmcStateRate(float rate, std::string &err_msg); + bool SetSparseMappingPoseRate(float rate, std::string &err_msg); bool SetTelemRateCallback(ff_msgs::SetRate::Request &req, ff_msgs::SetRate::Response &res); protected: virtual void Initialize(ros::NodeHandle *nh); + bool ReadTopicInfo(const std::string& topic_abbr, + const std::string& sub_or_pub, + std::string& sub_topic, + bool& use); bool ReadParams(); bool ReadRateParams(); diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_command_ros_command_plan.h b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_command_ros_command_plan.h index 821a2feca2..f3940d3d3e 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_command_ros_command_plan.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_command_ros_command_plan.h @@ -33,7 +33,7 @@ #include "ff_msgs/CommandArg.h" #include "ff_msgs/CommandStamped.h" -#include "AstrobeeCommandConstants.h" +#include "dds_msgs/AstrobeeCommandConstants.h" #include "knDds/DdsTypedSupplier.h" diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_compressed_file_ros_compressed_file.h b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_compressed_file_ros_compressed_file.h index 1938fe2cb2..5a22668a23 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_compressed_file_ros_compressed_file.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_compressed_file_ros_compressed_file.h @@ -31,9 +31,9 @@ #include "ff_msgs/CompressedFile.h" -#include "AstrobeeConstants.h" -#include "CompressedFile.h" -#include "CompressedFileSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/CompressedFile.h" +#include "dds_msgs/CompressedFileSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_ekf_ros_ekf.h b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_ekf_ros_ekf.h new file mode 100644 index 0000000000..93a41ca65c --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_ekf_ros_ekf.h @@ -0,0 +1,69 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef DDS_ROS_BRIDGE_RAPID_EKF_ROS_EKF_H_ +#define DDS_ROS_BRIDGE_RAPID_EKF_ROS_EKF_H_ + +#include + +#include + +#include +#include +#include + +#include "dds_ros_bridge/rapid_sub_ros_pub.h" +#include "dds_ros_bridge/util.h" + +#include "ff_msgs/EkfState.h" + +#include "dds_msgs/AstrobeeConstants.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "dds_msgs/EkfStateSupport.h" + +#include "rapidDds/RapidConstants.h" + +#include "rapidUtil/RapidHelper.h" + +namespace ff { + +class RapidEkfToRos : public RapidSubRosPub { + public: + RapidEkfToRos(const std::string& subscribe_topic, + const std::string& subscribe_partition, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + void CopyTransform3D(const rapid::Transform3D& transform, + geometry_msgs::Pose& pose); + void CopyVec3D(const rapid::Vec3d& vec_in, geometry_msgs::Vector3& vec_out); + + /** + * call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::EkfState const* ekf_sample); + + private: + std::string subscriber_partition_; +}; + +} // end namespace ff + +#endif // DDS_ROS_BRIDGE_RAPID_EKF_ROS_EKF_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_gs_data_ros_gs_data.h b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_gs_data_ros_gs_data.h new file mode 100644 index 0000000000..a11da1d691 --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_gs_data_ros_gs_data.h @@ -0,0 +1,66 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef DDS_ROS_BRIDGE_RAPID_GS_DATA_ROS_GS_DATA_H_ +#define DDS_ROS_BRIDGE_RAPID_GS_DATA_ROS_GS_DATA_H_ + +#include + +#include + +#include +#include +#include + +#include "dds_ros_bridge/rapid_sub_ros_pub.h" +#include "dds_ros_bridge/util.h" + +#include "ff_msgs/GuestScienceData.h" + +#include "dds_msgs/AstrobeeConstants.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "dds_msgs/GuestScienceDataSupport.h" + +#include "rapidDds/RapidConstants.h" + +#include "rapidUtil/RapidHelper.h" + +namespace ff { + +class RapidGuestScienceDataToRos : public RapidSubRosPub { + public: + RapidGuestScienceDataToRos(const std::string& subscribe_topic, + const std::string& subscribe_partition, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + + /** + * call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::GuestScienceData const* data); + + private: + std::string subscriber_partition_; +}; + +} // end namespace ff + +#endif // DDS_ROS_BRIDGE_RAPID_GS_DATA_ROS_GS_DATA_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_position_provider_ros_pose_helper.h b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_position_provider_ros_pose_helper.h new file mode 100644 index 0000000000..e909d41234 --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_position_provider_ros_pose_helper.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef DDS_ROS_BRIDGE_RAPID_POSITION_PROVIDER_ROS_POSE_HELPER_H_ +#define DDS_ROS_BRIDGE_RAPID_POSITION_PROVIDER_ROS_POSE_HELPER_H_ + +#include + +#include + +#include "dds_ros_bridge/util.h" + +#include "geometry_msgs/PoseStamped.h" + +#include "rapidIo/PositionProvider.h" + +namespace rapid { + +/** + * Provide a publisher which does not convert Quaternion to Rotation Matrix + * instead publish quaternion directly + * This publisher only does one copy into the RAPID data types + */ +class PositionProviderRosPoseHelper : public PositionProvider { + public: + PositionProviderRosPoseHelper(PositionTopicPairParameters const& params, + const std::string& entity_name); + void Publish(const geometry_msgs::PoseStamped::ConstPtr& pose); +}; + +} // end namespace rapid + +#endif // DDS_ROS_BRIDGE_RAPID_POSITION_PROVIDER_ROS_POSE_HELPER_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_agent_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_agent_state.h index 6c81a94006..d51fb94e8a 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_agent_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_agent_state.h @@ -35,9 +35,9 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeAgentStateSupport.h" -#include "AstrobeeConstants.h" -#include "MobilitySettingsStateSupport.h" +#include "dds_msgs/AstrobeeAgentStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/MobilitySettingsStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_arm_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_arm_state.h index edf88296b6..fadf46e604 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_arm_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_arm_state.h @@ -34,8 +34,8 @@ #include "rapidUtil/RapidHelper.h" -#include "ArmStateSupport.h" -#include "AstrobeeConstants.h" +#include "dds_msgs/ArmStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_battery_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_battery_state.h index d03251bc40..0083a3aaf3 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_battery_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_battery_state.h @@ -33,9 +33,9 @@ #include "sensor_msgs/BatteryState.h" #include "sensor_msgs/Temperature.h" -#include "AstrobeeConstants.h" -#include "EpsStateSupport.h" -#include "EpsConfigSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/EpsStateSupport.h" +#include "dds_msgs/EpsConfigSupport.h" #include "rapidUtil/RapidHelper.h" diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_command_config_rapid_command_config.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_command_config_rapid_command_config.h index 69d53f75e7..acc160f687 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_command_config_rapid_command_config.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_command_config_rapid_command_config.h @@ -36,7 +36,7 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeCommandConstants.h" +#include "dds_msgs/AstrobeeCommandConstants.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_compressed_file_ack.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_compressed_file_ack.h index 1dffa64233..1aa210e678 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_compressed_file_ack.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_compressed_file_ack.h @@ -33,8 +33,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "CompressedFileAckSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/CompressedFileAckSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_compressed_file_rapid_compressed_file.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_compressed_file_rapid_compressed_file.h index 34bc85c097..e3a957d9c1 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_compressed_file_rapid_compressed_file.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_compressed_file_rapid_compressed_file.h @@ -33,8 +33,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "CompressedFileSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/CompressedFileSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_cpu_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_cpu_state.h index acb06d0a1a..c820bc3f0e 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_cpu_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_cpu_state.h @@ -32,9 +32,9 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "CpuConfigSupport.h" -#include "CpuStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/CpuConfigSupport.h" +#include "dds_msgs/CpuStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_data_to_disk.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_data_to_disk.h index 9fd42415d6..a58e4f13d3 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_data_to_disk.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_data_to_disk.h @@ -33,9 +33,9 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "DataToDiskStateSupport.h" -#include "DataTopicsListSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/DataToDiskStateSupport.h" +#include "dds_msgs/DataTopicsListSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_disk_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_disk_state.h index bb890ca8b2..b9250bdae1 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_disk_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_disk_state.h @@ -34,9 +34,9 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "DiskStateSupport.h" -#include "DiskConfigSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/DiskStateSupport.h" +#include "dds_msgs/DiskConfigSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_ekf_rapid_ekf.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_ekf_rapid_ekf.h new file mode 100644 index 0000000000..ff1ba346d2 --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_ekf_rapid_ekf.h @@ -0,0 +1,75 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef DDS_ROS_BRIDGE_ROS_EKF_RAPID_EKF_H_ +#define DDS_ROS_BRIDGE_ROS_EKF_RAPID_EKF_H_ + +#include +#include + +#include "dds_ros_bridge/ros_sub_rapid_pub.h" +#include "dds_ros_bridge/rapid_position_provider_ros_helper.h" +#include "dds_ros_bridge/util.h" + +#include "ff_msgs/EkfState.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidDds/RapidConstants.h" + +#include "rapidIo/RapidIoParameters.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/EkfStateSupport.h" + +namespace ff { + +class RosEkfToRapid : public RosSubRapidPub { + public: + RosEkfToRapid(const std::string& subscribe_topic, + const std::string& pub_topic, + const std::string& rapid_pub_name, + const ros::NodeHandle& nh, + const unsigned int queue_size = 10); + + void CopyTransform3D(rapid::Transform3D& transform, + const geometry_msgs::Pose& pose); + void CopyVec3D(rapid::Vec3d& vec_out, const geometry_msgs::Vector3& vec_in); + void MsgCallback(const ff_msgs::EkfStateConstPtr& msg); + void PubEkf(const ros::TimerEvent& event); + void SetEkfPublishRate(float rate); + + private: + ff_msgs::EkfStateConstPtr ekf_msg_; + + using StateSupplier = + kn::DdsTypedSupplier; + using StateSupplierPtr = std::unique_ptr; + + StateSupplierPtr state_supplier_; + + bool ekf_sent_, pub_ekf_; + + ros::Timer ekf_timer_; +}; + +} // end namespace ff + +#endif // DDS_ROS_BRIDGE_ROS_EKF_RAPID_EKF_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_fault_config.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_fault_config.h index c3cdea3110..aba113753a 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_fault_config.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_fault_config.h @@ -32,9 +32,9 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "FaultConfigSupport.h" -#include "FaultStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/FaultConfigSupport.h" +#include "dds_msgs/FaultStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_fault_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_fault_state.h index 149f9489fc..629c750dc7 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_fault_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_fault_state.h @@ -32,8 +32,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "FaultStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/FaultStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_gnc_control_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_gnc_control_state.h index 729683cdfb..653c52847f 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_gnc_control_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_gnc_control_state.h @@ -18,8 +18,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "GncControlStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GncControlStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_gnc_fam_cmd_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_gnc_fam_cmd_state.h index 49e8e451a1..210c312410 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_gnc_fam_cmd_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_gnc_fam_cmd_state.h @@ -35,8 +35,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "GncFamCmdStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GncFamCmdStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_guest_science.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_guest_science.h index 3a861090d8..e54b9e2a1e 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_guest_science.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_guest_science.h @@ -37,10 +37,10 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "GuestScienceConfigSupport.h" -#include "GuestScienceDataSupport.h" -#include "GuestScienceStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GuestScienceConfigSupport.h" +#include "dds_msgs/GuestScienceDataSupport.h" +#include "dds_msgs/GuestScienceStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_inertia.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_inertia.h index 34d4281db4..fa112aec69 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_inertia.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_inertia.h @@ -30,8 +30,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "InertialPropertiesSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/InertialPropertiesSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_log_sample.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_log_sample.h index 7968382dd7..a9b467e0e2 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_log_sample.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_log_sample.h @@ -18,8 +18,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "LogSampleSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/LogSampleSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_odom_rapid_position.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_odom_rapid_position.h index d1201e560d..6284e38b5e 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_odom_rapid_position.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_odom_rapid_position.h @@ -36,8 +36,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "EkfStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/EkfStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_payload_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_payload_state.h new file mode 100644 index 0000000000..73e3fee1d5 --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_payload_state.h @@ -0,0 +1,67 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef DDS_ROS_BRIDGE_ROS_PAYLOAD_STATE_H_ +#define DDS_ROS_BRIDGE_ROS_PAYLOAD_STATE_H_ + +#include +#include +#include + +#include "dds_ros_bridge/ros_sub_rapid_pub.h" +#include "dds_ros_bridge/util.h" + +#include "ff_hw_msgs/EpsPowerState.h" +#include "ff_hw_msgs/EpsPowerStateValue.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/PayloadConfigSupport.h" +#include "dds_msgs/PayloadStateSupport.h" + +namespace ff { + +class RosPayloadStateToRapid : public RosSubRapidPub { + public: + RosPayloadStateToRapid(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + + void Callback(ff_hw_msgs::EpsPowerStateConstPtr const& state); + + private: + using StateSupplier = + kn::DdsTypedSupplier; + using StateSupplierPtr = std::unique_ptr; + + StateSupplierPtr state_supplier_; + + using ConfigSupplier = + kn::DdsTypedSupplier; + using ConfigSupplierPtr = std::unique_ptr; + + ConfigSupplierPtr config_supplier_; +}; + +} // end namespace ff + +#endif // DDS_ROS_BRIDGE_ROS_PAYLOAD_STATE_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_plan_status_rapid_plan_status.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_plan_status_rapid_plan_status.h index 9dc1241916..36bdbf780d 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_plan_status_rapid_plan_status.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_plan_status_rapid_plan_status.h @@ -35,8 +35,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "PlanStatusSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/PlanStatusSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pmc_cmd_state.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pmc_cmd_state.h index 97a971e9a9..de30077fec 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pmc_cmd_state.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pmc_cmd_state.h @@ -18,8 +18,8 @@ #include "rapidUtil/RapidHelper.h" -#include "AstrobeeConstants.h" -#include "PmcCmdStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/PmcCmdStateSupport.h" namespace ff { diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pose_rapid_position.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pose_rapid_position.h new file mode 100644 index 0000000000..2eb2ac7a39 --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pose_rapid_position.h @@ -0,0 +1,65 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef DDS_ROS_BRIDGE_ROS_POSE_RAPID_POSITION_H_ +#define DDS_ROS_BRIDGE_ROS_POSE_RAPID_POSITION_H_ + +#include + +#include "dds_ros_bridge/ros_sub_rapid_pub.h" +#include "dds_ros_bridge/rapid_position_provider_ros_pose_helper.h" +#include "dds_ros_bridge/util.h" + +#include "geometry_msgs/PoseStamped.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidDds/RapidConstants.h" + +#include "rapidIo/RapidIoParameters.h" + +#include "rapidUtil/RapidHelper.h" + +namespace ff { + +class RosPoseRapidPosition : public RosSubRapidPub { + public: + RosPoseRapidPosition(const std::string& subscribe_topic, + const std::string& pub_topic, + bool use_rate, + const ros::NodeHandle& nh, + const unsigned int queue_size = 10); + + void DataCallback(geometry_msgs::PoseStampedConstPtr const& data); + void PubPosition(const ros::TimerEvent& event); + void SetPositionPublishRate(float rate); + + private: + rapid::PositionTopicPairParameters params_; + std::shared_ptr provider_; + + geometry_msgs::PoseStampedConstPtr pose_msg_; + + bool use_rate_; + + ros::Timer position_timer_; +}; + +} // end namespace ff + +#endif // DDS_ROS_BRIDGE_ROS_POSE_RAPID_POSITION_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_telemetry_rapid_telemetry.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_telemetry_rapid_telemetry.h index fb48484f86..584b5db19f 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_telemetry_rapid_telemetry.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_telemetry_rapid_telemetry.h @@ -34,9 +34,9 @@ #include "ros/ros.h" -#include "AstrobeeConstants.h" -#include "TelemetryConfigSupport.h" -#include "TelemetryStateSupport.h" +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/TelemetryConfigSupport.h" +#include "dds_msgs/TelemetryStateSupport.h" namespace ff { @@ -56,6 +56,7 @@ class RosTelemetryRapidTelemetry : public RosSubRapidPub { void SetGncStateRate(float rate); void SetPmcCmdStateRate(float rate); void SetPositionRate(float rate); + void SetSparseMappingPoseRate(float rate); protected: bool AssembleConfig(config_reader::ConfigReader& config_params); diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_zones_rapid_compressed_file.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_zones_rapid_compressed_file.h new file mode 100644 index 0000000000..69f409455d --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_zones_rapid_compressed_file.h @@ -0,0 +1,73 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef DDS_ROS_BRIDGE_ROS_ZONES_RAPID_COMPRESSED_FILE_H_ +#define DDS_ROS_BRIDGE_ROS_ZONES_RAPID_COMPRESSED_FILE_H_ + +#include +#include +#include +#include +#include +#include + +#include "boost/iostreams/copy.hpp" +#include "boost/iostreams/device/back_inserter.hpp" +#include "boost/iostreams/device/array.hpp" +#include "boost/iostreams/filter/zlib.hpp" +#include "boost/iostreams/filtering_stream.hpp" + +#include "dds_ros_bridge/ros_sub_rapid_pub.h" +#include "dds_ros_bridge/util.h" + +#include "ff_msgs/GetZones.h" +#include "ff_msgs/Zone.h" + +#include "visualization_msgs/MarkerArray.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/CompressedFileSupport.h" + +namespace ff { + +class RosZonesToRapidCompressedFile : public RosSubRapidPub { + public: + RosZonesToRapidCompressedFile(const std::string& subscribe_topic, + const std::string& get_zones_srv, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + + void Callback(visualization_msgs::MarkerArray::ConstPtr const& zones); + + private: + using Supplier = kn::DdsTypedSupplier; + using SupplierPtr = std::unique_ptr; + + SupplierPtr file_supplier_; + + ros::ServiceClient get_zones_client_; +}; + +} // end namespace ff + +#endif // DDS_ROS_BRIDGE_ROS_ZONES_RAPID_COMPRESSED_FILE_H_ diff --git a/communications/dds_ros_bridge/nodelet_plugins.xml b/communications/dds_ros_bridge/nodelet_plugins.xml index 272160b821..e6c83429aa 100644 --- a/communications/dds_ros_bridge/nodelet_plugins.xml +++ b/communications/dds_ros_bridge/nodelet_plugins.xml @@ -6,4 +6,11 @@ Nodelet to act as a translator between dds and ros. + + + Nodelet to act as a bridge between Astrobees. + + diff --git a/communications/dds_ros_bridge/package.xml b/communications/dds_ros_bridge/package.xml index 50931e9e46..010526ba3c 100644 --- a/communications/dds_ros_bridge/package.xml +++ b/communications/dds_ros_bridge/package.xml @@ -15,22 +15,18 @@ Astrobee Flight Software catkin - cmake_modules - message_generation - ff_msgs - nav_msgs - roscpp - rossmbus - std_msgs nodelet - cmake_modules - message_runtime - ff_msgs - nav_msgs - roscpp - rossmbus - std_msgs + config_reader + ff_util + ff_msgs + ff_hw_msgs + dds_msgs nodelet + config_reader + ff_util + ff_msgs + ff_hw_msgs + dds_msgs diff --git a/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc b/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc new file mode 100644 index 0000000000..ee87fe924b --- /dev/null +++ b/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc @@ -0,0 +1,398 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "dds_ros_bridge/astrobee_astrobee_bridge.h" + +namespace dds_ros_bridge { +AstrobeeAstrobeeBridge::AstrobeeAstrobeeBridge() : + ff_util::FreeFlyerNodelet(NODE_ASTROBEE_ASTROBEE_BRIDGE, true), + components_(0), + started_(false), + agent_name_("Bumble"), + read_params_error_("") { +} + +AstrobeeAstrobeeBridge::~AstrobeeAstrobeeBridge() { +} + +void AstrobeeAstrobeeBridge::Initialize(ros::NodeHandle *nh) { + // Need to get robot name which is in the lua config files, add files, read + // files, and get robot name but don't get the rest of the config parameters + // since these params are used to create the classes that use rapid dds. Need + // to set up Miro/DDs before reading the parameters. + + config_params_.AddFile("communications/astrobee_astrobee_bridge.config"); + + if (!config_params_.ReadFiles()) { + ROS_FATAL("AstrobeeAstrobeeBridge: Error reading config files."); + exit(EXIT_FAILURE); + return; + } + + if (!config_params_.GetBool("started", &run_on_start_)) { + ROS_FATAL("AstrobeeAstrobeeBridge: Could not read started value."); + exit(EXIT_FAILURE); + return; + } + + if (!config_params_.GetStr("agent_name", &agent_name_)) { + ROS_FATAL("AstrobeeAstrobeeBridge: Could not read robot name."); + exit(EXIT_FAILURE); + return; + } + + // In simulation, the namespace is usually set to the robot name so we need to + // check if we are in simulation and get the right name + if (agent_name_ == "sim" || agent_name_ == "simulator") { + // The platform name should be the simulated robot name + agent_name_ = GetPlatform(); + + // If there is not robot name, set it to a default name so that we can + // connect to the bridge + if (agent_name_ == "") { + agent_name_ = "Bumble"; + } else { + // Make sure that first letter of robot name is capitialized. GDS only + // recognizes capitialized robot names. + agent_name_[0] = toupper(agent_name_[0]); + } + } + + nh_ = *nh; + + int fake_argc = 1; + + // TODO(tfmorse): make hardcoded values configurable + + // Make path to QOS and NDDS files + std::string config_path = ff_common::GetConfigDir(); + config_path += "/communications/dds_intercomms/"; + + // Create fake argv containing only the participant name + // Participant name needs to uniue so combine robot name with timestamp + ros::Time time = ros::Time::now(); + participant_name_ = agent_name_ + std::to_string(time.sec) + + std::string("-multi-bridge"); + char **fake_argv = new char*[1]; + fake_argv[0] = new char[(participant_name_.size() + 1)]; + std::strcpy(fake_argv[0], participant_name_.c_str()); // NOLINT + + /* fake miro log into thinking we have no arguments */ + Miro::Log::init(fake_argc, fake_argv); + Miro::Log::level(9); + + /* fake miro configuration into thinking we have no arguments */ + Miro::Configuration::init(fake_argc, fake_argv); + + Miro::RobotParameters *robot_params = Miro::RobotParameters::instance(); + kn::DdsEntitiesFactorySvcParameters *dds_params = + kn::DdsEntitiesFactorySvcParameters::instance(); + + /* get the defaults for *all the things!* */ + Miro::ConfigDocument *config = Miro::Configuration::document(); + config->setSection("Robot"); + config->getParameters("Miro::RobotParameters", *robot_params); + config->getParameters("kn::DdsEntitiesFactorySvcParameters", *dds_params); + + robot_params->name = agent_name_; + robot_params->namingContextName = robot_params->name; + + // Set values for default publisher and susbcriber + dds_params->publishers[0].name = agent_name_; + dds_params->publishers[0].partition = agent_name_; + dds_params->publishers[0].participant = participant_name_; + dds_params->subscribers[0].participant = participant_name_; + + // Clear config files so that dds only looks for the files we add + dds_params->participants[0].discoveryPeersFiles.clear(); + dds_params->configFiles.clear(); + + dds_params->participants[0].name = participant_name_; + dds_params->participants[0].participantName = participant_name_; + dds_params->participants[0].domainId = 38; + dds_params->participants[0].discoveryPeersFiles.push_back( + (config_path + "NDDS_DISCOVERY_PEERS")); + dds_params->configFiles.push_back((config_path + "RAPID_QOS_PROFILES.xml")); + + std::string local_subscriber = Miro::RobotParameters::instance()->name.c_str(); + + // Read subscribers from config file + std::vector default_subscribers; + config_reader::ConfigReader::Table rapid_sub_names; + std::string rapid_sub; + if (!config_params_.GetTable("rapid_sub_names", &rapid_sub_names)) { + ROS_FATAL("Multi-Astrobee Bridge: rapid_sub_names not specified!"); + exit(EXIT_FAILURE); + return; + } + for (int i = 1; i <= rapid_sub_names.GetSize(); i++) { + rapid_sub_names.GetStr(i, &rapid_sub); + default_subscribers.push_back(rapid_sub); + } + + // Register the subscribers into the parameters so they can be used later. + for (unsigned int i = 0; i < default_subscribers.size(); i++) { + if (local_subscriber != default_subscribers[i]) { + // TODO(rgarciar): Use () init instead of new + kn::DdsNodeParameters *subscriber = new kn::DdsNodeParameters(); + subscriber->name = default_subscribers[i]; + subscriber->partition = default_subscribers[i]; + subscriber->participant = participant_name_; + dds_params->subscribers.push_back(*subscriber); + } + } + + /** + * Use DdsEntitiesFactorySvc to create a new DdsEntitiesFactory + * which will create all objects: + * Participants DdsDomainParticpantRepository::instance() + * Publishers DdsPublisherRespoitory::instance() + * Subscribers DdsSubscriberRepository::instance() + * Topics + * and store in relevant repository + * based on DdsEntitesFactoryParameters + */ + dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); + dds_entities_factory_->init(dds_params); + + trigger_srv_ = nh->advertiseService( + SERVICE_COMMUNICATIONS_ENABLE_ASTROBEE_INTERCOMMS, + &AstrobeeAstrobeeBridge::Start, + this); + + if (run_on_start_) { + if (!Run()) { + ROS_ERROR_STREAM(read_params_error_); + } + } +} + +bool AstrobeeAstrobeeBridge::Start(ff_msgs::ResponseOnly::Request& req, + ff_msgs::ResponseOnly::Response & res) { + if (!Run()) { + ROS_ERROR_STREAM(read_params_error_); + res.success = false; + res.status = read_params_error_; + } else { + res.success = true; + } + + return true; +} + +bool AstrobeeAstrobeeBridge::Run() { + if (!started_ && !ReadParams()) { + return false; + } + started_ = true; + return true; +} + +bool AstrobeeAstrobeeBridge::ReadParams() { + config_reader::ConfigReader::Table share_topics, share_topic_groups, agents; + config_reader::ConfigReader::Table item_conf, agent, topics; + std::string item_name, topic_name, name, external_name; + bool enable, topic_enable; + float rate; + + components_ = 0; + + // Reload files. This allows the config file to be fixed if something is wrong + // during run time + if (!config_params_.ReadFiles()) { + read_params_error_ = "AstrobeeAstrobeeBridge: Error reading config files."; + return false; + } + // ROS -> RAPID + // ----------------------------- + // Load individual shared topics + if (!config_params_.GetTable("share_topics", &share_topics)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: share_topics table not specified!"; + return false; + } + + for (int i = 1; i <= share_topics.GetSize(); i++) { + share_topics.GetTable(i, &item_conf); + if (!ReadSharedItemConf(item_conf, item_name, enable, rate)) { + return false; + } + + // GNC_EKF configuration + // --------------------- + if (item_name == "TOPIC_GNC_EKF" && enable) { + if (rate < 0) { + read_params_error_ = "AstrobeeAstrobeeBridge: "; + read_params_error_ += item_name.c_str(); + read_params_error_ += " requires a non-negative rate."; + return false; + } + + BuildRosToRapid(item_name, TOPIC_GNC_EKF, + "", "", nh_); + + ff::RosEkfToRapid *ekf_to_rapid = static_cast + (ros_sub_rapid_pubs_[item_name].get()); + ekf_to_rapid->SetEkfPublishRate(rate); + } + + // Additional topics here... + // ------------------------- + } + + // Load shared topic groups + if (!config_params_.GetTable("share_topic_groups", &share_topic_groups)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: share_topic_groups table not specified!"; + return false; + } + + for (int i = 1; i <= share_topic_groups.GetSize(); i++) { + share_topic_groups.GetTable(i, &item_conf); + if (!ReadSharedItemConf(item_conf, item_name, enable, rate)) { + return false; + } + + // Guest Sciece GROUP + // ------------------ + if (item_name == "TOPIC_GROUP_GS" && enable) { + BuildRosToRapid(item_name, + TOPIC_GUEST_SCIENCE_MANAGER_STATE, + TOPIC_GUEST_SCIENCE_MANAGER_CONFIG, + TOPIC_GUEST_SCIENCE_DATA, "", nh_); + } + + // Additional topics here... + // ------------------------- + } + + // RAPID -> ROS + // ---------------------- + // Read listeners options + if (!config_params_.GetTable("agents", &agents)) { + read_params_error_ = "AstrobeeAstrobeeBridge: agents not specified!"; + return false; + } + + for (int i = 1; i <= agents.GetSize(); i++) { + // Agent configuration + agents.GetTable(i, &item_conf); + if (!item_conf.GetStr("name", &item_name)) { + read_params_error_ = "AstrobeeAstrobeeBridge: agent name not specified!"; + return false; + } + if (!item_conf.GetBool("enable", &enable)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: agent enable not specified!"; + return false; + } + if (!item_conf.GetTable("topics", &topics)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: agent topics not specified!"; + return false; + } + + // Skip if the configuration is for this agent. + // We don't want to translate topics coming from this robot already + // Also ignore if the agent is disabled + if (agent_name_ == item_name || !enable) { + continue; + } + + // Lower case the external agent name to use it like a namespace + external_name = item_name; + external_name[0] = tolower(external_name[0]); + + for (int j = 1; j <= topics.GetSize(); j++) { + // Agent topic configuration + topics.GetTable(j, &item_conf); + if (!item_conf.GetStr("name", &topic_name)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: agent topic name not specified!"; + return false; + } + if (!item_conf.GetBool("enable", &topic_enable)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: agent topic enable not specified!"; + return false; + } + + if (topic_name == "TOPIC_GNC_EKF" && topic_enable) { + BuildRapidToRos( + rapid::ext::astrobee::EKF_STATE_TOPIC, + item_name, + external_name + '/' + std::string(TOPIC_GNC_EKF), + nh_); + } + + if (topic_name == "TOPIC_GUEST_SCIENCE_DATA" && topic_enable) { + BuildRapidToRos( + rapid::ext::astrobee::GUEST_SCIENCE_DATA_TOPIC, + item_name, + external_name + '/' + std::string(TOPIC_GUEST_SCIENCE_DATA), + nh_); + } + + // Additional topics here... + } + } + return true; +} + +bool AstrobeeAstrobeeBridge::ReadSharedItemConf( + config_reader::ConfigReader::Table &conf, + std::string &topic_name, bool &enable, float &rate) { + if (!conf.GetStr("name", &topic_name)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: share topic name not specified!"; + return false; + } + if (!conf.GetBool("enable", &enable)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: share topic enable not specified!"; + return false; + } + if (!conf.GetReal("rate", &rate)) { + read_params_error_ = + "AstrobeeAstrobeeBridge: ekf state rate not specified!"; + return false; + } + + return true; +} + +template +int AstrobeeAstrobeeBridge::BuildRosToRapid(const std::string& name, + Args&&... args) { // NOLINT + ff::RosSubRapidPubPtr topic_to_rapid(new T(std::forward(args)...)); // NOLINT + components_++; + ros_sub_rapid_pubs_[name] = topic_to_rapid; + return ros_sub_rapid_pubs_.size(); +} + +template +int AstrobeeAstrobeeBridge::BuildRapidToRos(Args&& ... args) { // NOLINT + ff::RapidSubRosPubPtr topic_to_ros(new T(std::forward(args)...)); // NOLINT + components_++; + rapid_sub_ros_pubs_.push_back(topic_to_ros); + return rapid_sub_ros_pubs_.size(); +} + +} // end namespace dds_ros_bridge + +PLUGINLIB_EXPORT_CLASS(dds_ros_bridge::AstrobeeAstrobeeBridge, nodelet::Nodelet) diff --git a/communications/dds_ros_bridge/src/dds_ros_bridge.cc b/communications/dds_ros_bridge/src/dds_ros_bridge.cc index a03fd3acc2..0ed11d99d1 100644 --- a/communications/dds_ros_bridge/src/dds_ros_bridge.cc +++ b/communications/dds_ros_bridge/src/dds_ros_bridge.cc @@ -60,59 +60,110 @@ DdsRosBridge::DdsRosBridge() : DdsRosBridge::~DdsRosBridge() { } -int DdsRosBridge::BuildAccessControlStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr acs_to_acs( +bool DdsRosBridge::BuildAccessControlStateToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr acs_to_acs( new ff::RosAccessControlStateToRapid(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = acs_to_acs; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = acs_to_acs; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildAckToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr ack_to_ack(new ff::RosAckToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = ack_to_ack; - return ros_sub_rapid_pubs_.size(); +bool DdsRosBridge::BuildAckToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr ack_to_ack(new ff::RosAckToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = ack_to_ack; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildAgentStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr agent_to_agent(new ff::RosAgentStateToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = agent_to_agent; - return ros_sub_rapid_pubs_.size(); +bool DdsRosBridge::BuildAgentStateToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr agent_to_agent( + new ff::RosAgentStateToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = agent_to_agent; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildArmJointSampleToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr joint_to_joint( +bool DdsRosBridge::BuildArmJointSampleToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr joint_to_joint( new ff::RosArmJointSampleToRapid(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = joint_to_joint; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = joint_to_joint; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildArmStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr arm_to_arm(new ff::RosArmStateToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = arm_to_arm; - return ros_sub_rapid_pubs_.size(); +bool DdsRosBridge::BuildArmStateToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr arm_to_arm(new ff::RosArmStateToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = arm_to_arm; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildBatteryStateToRapid( +bool DdsRosBridge::BuildBatteryStateToRapid( const std::string& sub_topic_battery_state_TL, const std::string& sub_topic_battery_state_TR, const std::string& sub_topic_battery_state_BL, @@ -121,9 +172,13 @@ int DdsRosBridge::BuildBatteryStateToRapid( const std::string& sub_topic_battery_temp_TR, const std::string& sub_topic_battery_temp_BL, const std::string& sub_topic_battery_temp_BR, - const std::string& pub_topic, const std::string& name) { - ff::RosSubRapidPubPtr battery_to_battery( + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr battery_to_battery( new ff::RosBatteryStateToRapid(sub_topic_battery_state_TL, sub_topic_battery_state_TR, sub_topic_battery_state_BL, @@ -134,258 +189,560 @@ int DdsRosBridge::BuildBatteryStateToRapid( sub_topic_battery_temp_BR, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = battery_to_battery; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = battery_to_battery; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildCommandToRapid(const std::string& sub_topic, - const std::string& ac_sub_topic, - const std::string& failed_cmd_sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr command_to_command(new ff::RosCommandToRapid( - sub_topic, +bool DdsRosBridge::BuildCommandToRapid(const std::string& sub_topic, + const std::string& ac_sub_topic, + const std::string& failed_cmd_sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr command_to_command( + new ff::RosCommandToRapid(sub_topic, ac_sub_topic, failed_cmd_sub_topic, pub_topic, nh_, agent_name_)); - ros_sub_rapid_pubs_[name] = command_to_command; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = command_to_command; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildCompressedImageToImage(const std::string& sub_topic, - const std::string& pub_topic, +bool DdsRosBridge::BuildCompressedFileToRapid(const std::string& sub_topic, const std::string& name) { - ff::RosSubRapidPubPtr compressed_image_to_image( + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr compressed_file_to_rapid( + new ff::RosCompressedFileToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = compressed_file_to_rapid; + } + } else { + return false; + } + + return true; +} + +bool DdsRosBridge::BuildCompressedFileAckToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr compressed_file_ack_to_rapid( + new ff::RosCompressedFileAckToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = compressed_file_ack_to_rapid; + } + } else { + return false; + } + + return true; +} + +bool DdsRosBridge::BuildCompressedImageToImage(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr compressed_image_to_image( new ff::RosCompressedImageRapidImage(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = compressed_image_to_image; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = compressed_image_to_image; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildCpuStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr cpu_to_cpu(new ff::RosCpuStateToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = cpu_to_cpu; - return ros_sub_rapid_pubs_.size(); +bool DdsRosBridge::BuildCpuStateToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr cpu_to_cpu(new ff::RosCpuStateToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = cpu_to_cpu; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildDataToDiskStateToRapid( +bool DdsRosBridge::BuildDataToDiskStateToRapid( const std::string& state_sub_topic, const std::string& topics_sub_topic, - const std::string& pub_topic, const std::string& name) { - ff::RosSubRapidPubPtr data_to_disk_to_data_to_disk( + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr data_to_disk_to_data_to_disk( new ff::RosDataToDiskToRapid(state_sub_topic, topics_sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = data_to_disk_to_data_to_disk; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = data_to_disk_to_data_to_disk; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildDiskStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr disk_to_disk(new ff::RosDiskStateToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = disk_to_disk; - return ros_sub_rapid_pubs_.size(); +bool DdsRosBridge::BuildDiskStateToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr disk_to_disk(new ff::RosDiskStateToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = disk_to_disk; + } + } else { + return false; + } + + return true; +} + +bool DdsRosBridge::BuildFaultConfigToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr fault_to_fault( + new ff::RosFaultConfigToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = fault_to_fault; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildFaultConfigToRapid(const std::string& sub_topic, - const std::string& pub_topic, +bool DdsRosBridge::BuildFaultStateToRapid(const std::string& sub_topic, const std::string& name) { - ff::RosSubRapidPubPtr fault_to_fault(new ff::RosFaultConfigToRapid(sub_topic, + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr fault_to_fault( + new ff::RosFaultStateToRapid(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = fault_to_fault; - return ros_sub_rapid_pubs_.size(); -} + components_++; + ros_sub_rapid_pubs_[name] = fault_to_fault; + } + } else { + return false; + } -int DdsRosBridge::BuildFaultStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr fault_to_fault(new ff::RosFaultStateToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = fault_to_fault; - return ros_sub_rapid_pubs_.size(); + return true; } -int DdsRosBridge::BuildGncFamCmdStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr gnc_fam_cmd_to_gnc_fam_cmd( +bool DdsRosBridge::BuildGncFamCmdStateToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr gnc_fam_cmd_to_gnc_fam_cmd( new ff::RosGncFamCmdStateToRapid(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = gnc_fam_cmd_to_gnc_fam_cmd; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = gnc_fam_cmd_to_gnc_fam_cmd; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildGncControlStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr gnc_control_to_gnc_control( +bool DdsRosBridge::BuildGncControlStateToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr gnc_control_to_gnc_control( new ff::RosGncControlStateToRapid(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = gnc_control_to_gnc_control; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = gnc_control_to_gnc_control; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildGuestScienceToRapid(const std::string& state_sub_topic, - const std::string& config_sub_topic, - const std::string& data_sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr guest_science_to_guest_science( +bool DdsRosBridge::BuildGuestScienceToRapid(const std::string& state_sub_topic, + const std::string& config_sub_topic, + const std::string& data_sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr guest_science_to_guest_science( new ff::RosGuestScienceToRapid(state_sub_topic, config_sub_topic, data_sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = guest_science_to_guest_science; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = guest_science_to_guest_science; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildInertiaDataToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr inertia_to_inertia(new ff::RosInertiaToRapid(sub_topic, - pub_topic, - nh_)); +bool DdsRosBridge::BuildInertiaDataToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr inertia_to_inertia( + new ff::RosInertiaToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = inertia_to_inertia; + } + } else { + return false; + } - ros_sub_rapid_pubs_[name] = inertia_to_inertia; - return ros_sub_rapid_pubs_.size(); + return true; } -int DdsRosBridge::BuildLogSampleToRapid(const std::string& sub_topic, - const std::string& pub_topic, +bool DdsRosBridge::BuildLogSampleToRapid(const std::string& sub_topic, const std::string& name) { - ff::RosSubRapidPubPtr log_to_log(new ff::RosLogSampleToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = log_to_log; - return ros_sub_rapid_pubs_.size(); + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr log_to_log(new ff::RosLogSampleToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = log_to_log; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildOdomToPosition(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr odom_to_position(new ff::RosOdomRapidPosition(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = odom_to_position; - return ros_sub_rapid_pubs_.size(); +bool DdsRosBridge::BuildOdomToPosition(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr odom_to_position( + new ff::RosOdomRapidPosition(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = odom_to_position; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildPlanStatusToPlanStatus(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr plan_status_to_plan_status( + +bool DdsRosBridge::BuildPayloadStateToPayloadState(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr payload_state_to_payload_state( + new ff::RosPayloadStateToRapid(sub_topic, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = payload_state_to_payload_state; + } + } else { + return false; + } + + return true; +} + +bool DdsRosBridge::BuildPlanStatusToPlanStatus(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr plan_status_to_plan_status( new ff::RosPlanStatusRapidPlanStatus(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = plan_status_to_plan_status; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = plan_status_to_plan_status; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildPmcCmdStateToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr pmc_command_to_pmc_command( - new ff::RosPmcCmdStateToRapid(sub_topic, +bool DdsRosBridge::BuildPmcCmdStateToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr pmc_command_to_pmc_command( + new ff::RosPmcCmdStateToRapid(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = pmc_command_to_pmc_command; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = pmc_command_to_pmc_command; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildStringToTextMessage(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr str_to_text(new ff::RosStringRapidTextMessage(sub_topic, +bool DdsRosBridge::BuildPoseToPosition(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use, use_rate; + std::string use_exp = "use_rate_" + name; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + if (!config_params_.GetBool(use_exp.c_str(), &use_rate)) { + ROS_FATAL("DDS Bridge: use rate %s not specified!", name.c_str()); + return false; + } + + ff::RosSubRapidPubPtr pose_to_position( + new ff::RosPoseRapidPosition(sub_topic, + pub_topic, + use_rate, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = pose_to_position; + } + } else { + return false; + } + + return true; +} + +bool DdsRosBridge::BuildStringToTextMessage(const std::string& name) { + std::string pub_topic, sub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use) && + ReadTopicInfo(name, "sub", sub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr str_to_text( + new ff::RosStringRapidTextMessage(sub_topic, pub_topic, nh_)); - ros_sub_rapid_pubs_[name] = str_to_text; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = str_to_text; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildTelemetryToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr telemetry_to_telemetry( +bool DdsRosBridge::BuildTelemetryToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr telemetry_to_telemetry( new ff::RosTelemetryRapidTelemetry(sub_topic, pub_topic, nh_, config_params_)); - ros_sub_rapid_pubs_[name] = telemetry_to_telemetry; - return ros_sub_rapid_pubs_.size(); + components_++; + ros_sub_rapid_pubs_[name] = telemetry_to_telemetry; + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildCommandToCommand(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RapidSubRosPubPtr command_to_command( +bool DdsRosBridge::BuildZonesToRapidCompressedFile(const std::string& sub_topic, + const std::string& zones_srv, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr zones_to_compressed_file( + new ff::RosZonesToRapidCompressedFile(sub_topic, + zones_srv, + pub_topic, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = zones_to_compressed_file; + } + } else { + return false; + } + + return true; +} + +bool DdsRosBridge::BuildCommandToCommand(const std::string& pub_topic, + const std::string& name) { + std::string sub_topic; + bool use; + + if (ReadTopicInfo(name, "sub", sub_topic, use)) { + if (use) { + ff::RapidSubRosPubPtr command_to_command( new ff::RapidCommandRosCommand(sub_topic, pub_topic, nh_)); - rapid_sub_ros_pubs_.push_back(command_to_command); - return rapid_sub_ros_pubs_.size(); + components_++; + rapid_sub_ros_pubs_.push_back(command_to_command); + } + } else { + return false; + } + + return true; } -int DdsRosBridge::BuildCompressedFileToCompressedFile( - const std::string& sub_topic, +bool DdsRosBridge::BuildCompressedFileToCompressedFile( const std::string& pub_topic, const std::string& name) { - ff::RapidSubRosPubPtr compressed_file_to_compressed_file( + std::string sub_topic; + bool use; + + if (ReadTopicInfo(name, "sub", sub_topic, use)) { + if (use) { + ff::RapidSubRosPubPtr compressed_file_to_compressed_file( new ff::RapidCompressedFileRosCompressedFile(sub_topic, pub_topic, nh_)); - rapid_sub_ros_pubs_.push_back(compressed_file_to_compressed_file); - return rapid_sub_ros_pubs_.size(); -} + components_++; + rapid_sub_ros_pubs_.push_back(compressed_file_to_compressed_file); + } + } else { + return false; + } -int DdsRosBridge::BuildCompressedFileToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr compressed_file_to_rapid( - new ff::RosCompressedFileToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = compressed_file_to_rapid; - return ros_sub_rapid_pubs_.size(); + return true; } -int DdsRosBridge::BuildCompressedFileAckToRapid(const std::string& sub_topic, - const std::string& pub_topic, - const std::string& name) { - ff::RosSubRapidPubPtr compressed_file_ack_to_rapid( - new ff::RosCompressedFileAckToRapid(sub_topic, - pub_topic, - nh_)); - ros_sub_rapid_pubs_[name] = compressed_file_ack_to_rapid; - return ros_sub_rapid_pubs_.size(); -} +bool DdsRosBridge::BuildCommandConfigToCommandConfig(const std::string& name) { + std::string pub_topic; + bool use; -int DdsRosBridge::BuildCommandConfigToCommandConfig( - const std::string& pub_topic, - const std::string& name) { - // keep in scope to retain reliable durable - ff::RapidPubPtr command_config_to_command_config( + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + // keep in scope to retain reliable durable + ff::RapidPubPtr command_config_to_command_config( new ff::RosCommandConfigRapidCommandConfig(pub_topic, nh_, config_params_)); - rapid_pubs_.push_back(command_config_to_command_config); - return rapid_pubs_.size(); + components_++; + rapid_pubs_.push_back(command_config_to_command_config); + } + } else { + return false; + } + + return true; } bool DdsRosBridge::SetTelem(float rate, std::string &err_msg, RateType type) { @@ -420,6 +777,9 @@ bool DdsRosBridge::SetTelem(float rate, std::string &err_msg, RateType type) { case POSITION: RTRT->SetPositionRate(rate); break; + case SPARSE_MAPPING_POSE: + RTRT->SetSparseMappingPoseRate(rate); + break; default: err_msg = "DDS Bridge: Unknown type when setting telemetry rate!"; return false; @@ -552,6 +912,23 @@ bool DdsRosBridge::SetPmcStateRate(float rate, std::string &err_msg) { return true; } +bool DdsRosBridge::SetSparseMappingPoseRate(float rate, std::string &err_msg) { + if (ros_sub_rapid_pubs_.count("RSMPRP") == 0) { + err_msg = "DDS Bridge: Sparse mapping pose message not add but needed."; + return false; + } + + if (!SetTelem(rate, err_msg, SPARSE_MAPPING_POSE)) { + return false; + } + + ff::RosPoseRapidPosition *RSMP = static_cast + (ros_sub_rapid_pubs_["RSMPRP"].get()); + + RSMP->SetPositionPublishRate(rate); + return true; +} + bool DdsRosBridge::SetTelemRateCallback(ff_msgs::SetRate::Request &req, ff_msgs::SetRate::Response &res) { std::string err_msg; @@ -577,6 +954,9 @@ bool DdsRosBridge::SetTelemRateCallback(ff_msgs::SetRate::Request &req, case ff_msgs::SetRate::Request::POSITION: res.success = SetEkfPositionRate(req.rate, err_msg, POSITION); break; + case ff_msgs::SetRate::Request::SPARSE_MAPPING_POSE: + res.success = SetSparseMappingPoseRate(req.rate, err_msg); + break; default: res.success = false; err_msg = "DDS Bridge: Unknown which in set telemetry rate service."; @@ -751,632 +1131,265 @@ void DdsRosBridge::Initialize(ros::NodeHandle *nh) { return; } - srv_set_telem_rate_ = nh_.advertiseService( - SERVICE_COMMUNICATIONS_DDS_SET_TELEM_RATES, - &DdsRosBridge::SetTelemRateCallback, - this); + srv_set_telem_rate_ = nh_.advertiseService( + SERVICE_COMMUNICATIONS_DDS_SET_TELEM_RATES, + &DdsRosBridge::SetTelemRateCallback, + this); +} + +bool DdsRosBridge::ReadTopicInfo(const std::string& topic_abbr, + const std::string& sub_or_pub, + std::string& topic, + bool& use) { + std::string use_exp = "use_" + topic_abbr; + std::string topic_exp = sub_or_pub + "_topic_" + topic_abbr; + + if (!config_params_.GetBool(use_exp.c_str(), &use)) { + ROS_FATAL("DDS Bridge: use %s not specified!", topic_abbr.c_str()); + return false; + } + + if (use) { + if (!config_params_.GetStr(topic_exp.c_str(), &topic)) { + ROS_FATAL("DDS Bridge: sub topic %s not specified!", topic_abbr.c_str()); + return false; + } + } + + return true; } bool DdsRosBridge::ReadParams() { - std::string sub_topic, sub_topic2; + std::string sub_topic; std::string pub_topic; - bool use; components_ = 0; // rapid_command_ros_command => RCRC - if (!config_params_.GetBool("use_RCRC", &use)) { - ROS_FATAL("DDS Bridge: use RCRC not specified"); + if (!BuildCommandToCommand(TOPIC_COMMUNICATIONS_DDS_COMMAND, "RCRC")) { return false; } - if (use) { - if (!config_params_.GetStr("sub_topic_RCRC", &sub_topic)) { - ROS_FATAL("DDS Bridge: sub topic RCRC not specified!"); - return false; - } - - BuildCommandToCommand(sub_topic, TOPIC_COMMUNICATIONS_DDS_COMMAND, "RCRC"); - components_++; - } - // rapid_compressed_file_data_ros_compressed_file_data => RCFDRCFD - if (!config_params_.GetBool("use_RCFDRCFD", &use)) { - ROS_FATAL("DDS Bridge: use RCFDRCFD not specified!"); + if (!BuildCompressedFileToCompressedFile(TOPIC_COMMUNICATIONS_DDS_DATA, + "RCFDRCFD")) { return false; } - if (use) { - if (!config_params_.GetStr("sub_topic_RCFDRCFD", &sub_topic)) { - ROS_FATAL("DDS Bridge: sub topic RCFDRCFD not specified!"); - return false; - } - - BuildCompressedFileToCompressedFile(sub_topic, - TOPIC_COMMUNICATIONS_DDS_DATA, - "RCFDRCFD"); - components_++; - } else { - ROS_WARN("Not bridging data to disk compressed files!"); - } - // rapid_compressed_file_plan_ros_compressed_file_plan => RCFPRCFP - if (!config_params_.GetBool("use_RCFPRCFP", &use)) { - ROS_FATAL("DDS Bridge: use RCFPRCFP not specified!"); + if (!BuildCompressedFileToCompressedFile(TOPIC_COMMUNICATIONS_DDS_PLAN, + "RCFPRCFP")) { return false; } - if (use) { - if (!config_params_.GetStr("sub_topic_RCFPRCFP", &sub_topic)) { - ROS_FATAL("DDS Bridge: sub topic RCFPRCFP not specified!"); - return false; - } - - BuildCompressedFileToCompressedFile(sub_topic, - TOPIC_COMMUNICATIONS_DDS_PLAN, - "RCFPRCFP"); - components_++; - } else { - ROS_WARN("Not bridging plan compressed files!"); - } - // rapid_compressed_file_zones_ros_compressed_file_zones => RCFZRCFZ - if (!config_params_.GetBool("use_RCFZRCFZ", &use)) { - ROS_FATAL("DDS Bridge: use RCFZRCF not specified!"); + if (!BuildCompressedFileToCompressedFile(TOPIC_COMMUNICATIONS_DDS_ZONES, + "RCFZRCFZ")) { return false; } - if (use) { - if (!config_params_.GetStr("sub_topic_RCFZRCFZ", &sub_topic)) { - ROS_FATAL("DDS Bridge: sub topic RCFZRCFZ not specified!"); - return false; - } - - BuildCompressedFileToCompressedFile(sub_topic, - TOPIC_COMMUNICATIONS_DDS_ZONES, - "RCFZRCFZ"); - components_++; - } else { - ROS_WARN("Not bridging zone compressed files!"); - } - // ros_access_control_rapid_state => RACS - if (!config_params_.GetBool("use_RACS", &use)) { - ROS_FATAL("DDS Bridge: use RACS not specified!"); + if (!BuildAccessControlStateToRapid(TOPIC_MANAGEMENT_ACCESS_CONTROL_STATE, + "RACS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RACS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RACS not specified!"); - return false; - } - - BuildAccessControlStateToRapid(TOPIC_MANAGEMENT_ACCESS_CONTROL_STATE, - pub_topic, - "RACS"); - components_++; - } - // ros_ack_rapid_ack => RARA - if (!config_params_.GetBool("use_RARA", &use)) { - ROS_FATAL("DDS Bridge: use RARA not specified!"); + if (!BuildAckToRapid(TOPIC_MANAGEMENT_ACK, "RARA")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RARA", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RARA not specified!"); - return false; - } - - BuildAckToRapid(TOPIC_MANAGEMENT_ACK, pub_topic, "RARA"); - components_++; - } - // ros_agent_state_rapid_agent_state => RASRAS - if (!config_params_.GetBool("use_RASRAS", &use)) { - ROS_FATAL("DDS Bridge: use RASRAS not specified!"); + if (!BuildAgentStateToRapid(TOPIC_MANAGEMENT_EXEC_AGENT_STATE, "RASRAS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RASRAS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RASRAS not specified!"); - return false; - } - - BuildAgentStateToRapid(TOPIC_MANAGEMENT_EXEC_AGENT_STATE, - pub_topic, - "RASRAS"); - components_++; - } - // ros_arm_joint_sample_rapid_arm_joint_sample => RAJSRAJS - if (!config_params_.GetBool("use_RAJSRAJS", &use)) { - ROS_FATAL("DDS Bridge: use RAJSRAJS not specified!"); + if (!BuildArmJointSampleToRapid(TOPIC_BEHAVIORS_ARM_JOINT_SAMPLE, + "RAJSRAJS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RAJSRAJS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RAJSRAJS not specified!"); - return false; - } - - BuildArmJointSampleToRapid(TOPIC_BEHAVIORS_ARM_JOINT_SAMPLE, - pub_topic, - "RAJSRAJS"); - components_++; - } - // ros_arm_state_rapid_arm_state => RARMRARM - if (!config_params_.GetBool("use_RARMRARM", &use)) { - ROS_FATAL("DDS Bridge: use RARMRARM not specified!"); + if (!BuildArmStateToRapid(TOPIC_BEHAVIORS_ARM_ARM_STATE, "RARMRARM")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RARMRARM", &pub_topic)) { - ROS_FATAL("DDS_Bridge: pub topic RARMRARM not specified!"); - return false; - } - - BuildArmStateToRapid(TOPIC_BEHAVIORS_ARM_ARM_STATE, - pub_topic, - "RARMRARM"); - components_++; - } - // ros_battery_state_rapid_battery_config => RBSRBS - if (!config_params_.GetBool("use_RBSRBS", &use)) { - ROS_FATAL("DDS Bridge: pub topic RBSRBS not specified!"); + if (!BuildBatteryStateToRapid(TOPIC_HARDWARE_EPS_BATTERY_STATE_TL, + TOPIC_HARDWARE_EPS_BATTERY_STATE_TR, + TOPIC_HARDWARE_EPS_BATTERY_STATE_BL, + TOPIC_HARDWARE_EPS_BATTERY_STATE_BR, + TOPIC_HARDWARE_EPS_BATTERY_TEMP_TL, + TOPIC_HARDWARE_EPS_BATTERY_TEMP_TR, + TOPIC_HARDWARE_EPS_BATTERY_TEMP_BL, + TOPIC_HARDWARE_EPS_BATTERY_TEMP_BR, + "RBSRBS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RBSRBS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RBSRBS not specified!"); - return false; - } - - BuildBatteryStateToRapid(TOPIC_HARDWARE_EPS_BATTERY_STATE_TL, - TOPIC_HARDWARE_EPS_BATTERY_STATE_TR, - TOPIC_HARDWARE_EPS_BATTERY_STATE_BL, - TOPIC_HARDWARE_EPS_BATTERY_STATE_BR, - TOPIC_HARDWARE_EPS_BATTERY_TEMP_TL, - TOPIC_HARDWARE_EPS_BATTERY_TEMP_TR, - TOPIC_HARDWARE_EPS_BATTERY_TEMP_BL, - TOPIC_HARDWARE_EPS_BATTERY_TEMP_BR, - pub_topic, - "RBSRBS"); - components_++; - } - // ros_command_rapid_command => RosCRapC - if (!config_params_.GetBool("use_ROSCRAPC", &use)) { - ROS_FATAL("DDS Bridge: use ROSCRAPC not specified!"); + if (!BuildCommandToRapid(TOPIC_COMMAND, + TOPIC_COMMUNICATIONS_DDS_COMMAND, + TOPIC_MANAGEMENT_ACCESS_CONTROL_CMD, + "ROSCRAPC")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_ROSCRAPC", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic ROSCRAPC not specified!"); - return false; - } - - BuildCommandToRapid(TOPIC_COMMAND, - TOPIC_COMMUNICATIONS_DDS_COMMAND, - TOPIC_MANAGEMENT_ACCESS_CONTROL_CMD, - pub_topic, - "ROSCRAPC"); - components_++; - } - // ros_command_config_rapid_command_config => RCCRCC - if (!config_params_.GetBool("use_RCCRCC", &use)) { - ROS_FATAL("DDS Bridge: use RCCRCC not specified!"); + if (!BuildCommandConfigToCommandConfig("RCCRCC")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RCCRCC", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RCCRCC not specified!"); - return false; - } - - BuildCommandConfigToCommandConfig(pub_topic, "RCCRCC"); - components_++; - } - // ros_compressed_file_rapid_compressed_file => RosCFRapCF - if (!config_params_.GetBool("use_ROSCFRAPCF", &use)) { - ROS_FATAL("DDS Bridge: use ROSCFRAPCF not specified!"); + if (!BuildCompressedFileToRapid(TOPIC_MANAGEMENT_EXEC_PLAN, + "ROSCFRAPCF")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_ROSCFRAPCF", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic ROSCFRAPCF not specified!"); - return false; - } - - BuildCompressedFileToRapid(TOPIC_MANAGEMENT_EXEC_PLAN, - pub_topic, - "ROSCFRAPCF"); - components_++; - } - // ros_compressed_file_ack_rapid_compressed_file_ack => RCFARCFA - if (!config_params_.GetBool("use_RCFARCFA", &use)) { - ROS_FATAL("DDS Bridge: use RCFARCFA not specified!"); + if (!BuildCompressedFileAckToRapid(TOPIC_MANAGEMENT_EXEC_CF_ACK, + "RCFARCFA")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RCFARCFA", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RCFARCFA not specified!"); - return false; - } - - BuildCompressedFileAckToRapid(TOPIC_MANAGEMENT_EXEC_CF_ACK, - pub_topic, - "RCFARCFA"); - components_++; - } - // ros_compressed_dock_cam_image_rapid_image => RCDCIRI - if (!config_params_.GetBool("use_RCDCIRI", &use)) { - ROS_FATAL("DDS Bridge: use RCDCIRI not specified!"); + if (!BuildCompressedImageToImage(TOPIC_MANAGEMENT_IMG_SAMPLER_DOCK_CAM_STREAM, + "RCDCIRI")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RCDCIRI", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RCDCIRI not specified!"); - return false; - } - - BuildCompressedImageToImage(TOPIC_MANAGEMENT_IMG_SAMPLER_DOCK_CAM_STREAM, - pub_topic, - "RCDCIRI"); - components_++; - } - // ros_compressed_haz_cam_image_rapid_image => RCHCIRI - if (!config_params_.GetBool("use_RCHCIRI", &use)) { - ROS_FATAL("DDS Bridge: use RCHCIRI not specified!"); + if (!BuildCompressedImageToImage(TOPIC_MANAGEMENT_IMG_SAMPLER_HAZ_CAM_STREAM, + "RCHCIRI")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RCHCIRI", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RCHCIRI not specified."); - return false; - } - - BuildCompressedImageToImage(TOPIC_MANAGEMENT_IMG_SAMPLER_HAZ_CAM_STREAM, - pub_topic, - "RCHCIRI"); - components_++; - } - // ros_compressed_nav_cam_image_rapid_image => RCNCIRI - if (!config_params_.GetBool("use_RCNCIRI", &use)) { - ROS_FATAL("DDS Bridge: use RCNCIRI not specified!"); + if (!BuildCompressedImageToImage(TOPIC_MANAGEMENT_IMG_SAMPLER_NAV_CAM_STREAM, + "RCNCIRI")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RCNCIRI", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RCNCIRI not specified!"); - return false; - } - - BuildCompressedImageToImage(TOPIC_MANAGEMENT_IMG_SAMPLER_NAV_CAM_STREAM, - pub_topic, - "RCNCIRI"); - components_++; - } - // ros_compressed_perch_cam_image_rapid_image => RCPCIRI - if (!config_params_.GetBool("use_RCPCIRI", &use)) { - ROS_FATAL("DDS Bridge: use RCPCIRI not specified!"); + if (!BuildCompressedImageToImage( + TOPIC_MANAGEMENT_IMG_SAMPLER_PERCH_CAM_STREAM, + "RCPCIRI")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RCPCIRI", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RCPCIRI not specified!"); - return false; - } - - BuildCompressedImageToImage(TOPIC_MANAGEMENT_IMG_SAMPLER_PERCH_CAM_STREAM, - pub_topic, - "RCPCIRI"); - components_++; + // ros_compressed_science_cam_image_rapid_image => RCSCIRI + if (!BuildCompressedImageToImage(TOPIC_HARDWARE_SCI_CAM, "RCSCIRI")) { + return false; } // ros_cpu_state_rapid_cpu_state => RCSRCS - if (!config_params_.GetBool("use_RCSRCS", &use)) { - ROS_FATAL("DDS Bridge: use RCSRCS not specified!"); + if (!BuildCpuStateToRapid(TOPIC_MANAGEMENT_CPU_MONITOR_STATE, "RCSRCS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RCSRCS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RCSRCS not specified!"); - return false; - } - - BuildCpuStateToRapid(TOPIC_MANAGEMENT_CPU_MONITOR_STATE, - pub_topic, - "RCSRCS"); - components_++; - } - // ros_data_to_disk_rapid_data_to_disk => RDTDRDTD - if (!config_params_.GetBool("use_RDTDRDTD", &use)) { - ROS_FATAL("DDS Bridge: use RDTDRDTD not specified!"); + if (!BuildDataToDiskStateToRapid(TOPIC_MANAGEMENT_DATA_BAGGER_STATE, + TOPIC_MANAGEMENT_DATA_BAGGER_TOPICS, + "RDTDRDTD")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RDTDRDTD", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RDTDRDTD not specified!"); - return false; - } - - BuildDataToDiskStateToRapid(TOPIC_MANAGEMENT_DATA_BAGGER_STATE, - TOPIC_MANAGEMENT_DATA_BAGGER_TOPICS, - pub_topic, - "RDTDRDTD"); - - components_++; - } - // ros_disk_state_rapid_disk_state => RDSRDS - if (!config_params_.GetBool("use_RDSRDS", &use)) { - ROS_FATAL("DDS Bridge: use RDSRDS not specified!"); + if (!BuildDiskStateToRapid(TOPIC_MANAGEMENT_DISK_MONITOR_STATE, "RDSRDS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RDSRDS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RDSRDS not specified!"); - return false; - } - - BuildDiskStateToRapid(TOPIC_MANAGEMENT_DISK_MONITOR_STATE, - pub_topic, - "RDSRDS"); - components_++; - } - // ros_fault_config_rapid_fault_config => RFCRFC - if (!config_params_.GetBool("use_RFCRFC", &use)) { - ROS_FATAL("DDS Bridge: use RFCRFC not specified!"); + if (!BuildFaultConfigToRapid(TOPIC_MANAGEMENT_SYS_MONITOR_CONFIG, "RFCRFC")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RFCRFC", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RFCRFC not specified!"); - return false; - } - - BuildFaultConfigToRapid(TOPIC_MANAGEMENT_SYS_MONITOR_CONFIG, - pub_topic, - "RFCRFC"); - components_++; - } - // ros_fault_state_rapid_fault_state => RFSRFS - if (!config_params_.GetBool("use_RFSRFS", &use)) { - ROS_FATAL("DDS Bridge: use RFSRFS not specified!"); + if (!BuildFaultStateToRapid(TOPIC_MANAGEMENT_SYS_MONITOR_STATE, "RFSRFS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RFSRFS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RFSRFS not specified!"); - return false; - } - - BuildFaultStateToRapid(TOPIC_MANAGEMENT_SYS_MONITOR_STATE, - pub_topic, - "RFSRFS"); - components_++; - } - // ros_gnc_control_command_rapid_gnc_control_command => RGCCRGCC - if (!config_params_.GetBool("use_RGCCRGCC", &use)) { - ROS_FATAL("DDS Bridge: use RGCCRGCC not specified!"); + if (!BuildGncFamCmdStateToRapid(TOPIC_GNC_CTL_COMMAND, "RGCCRGCC")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RGCCRGCC", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RGCCRGCC not specified!"); - return false; - } - - BuildGncFamCmdStateToRapid(TOPIC_GNC_CTL_COMMAND, pub_topic, "RGCCRGCC"); - components_++; - } - // ros_gnc_control_shaper_rapid_gnc_control_shaper => RGCSRGCS - if (!config_params_.GetBool("use_RGCSRGCS", &use)) { - ROS_FATAL("DDS Bridge: use RGCSRGCS not specified!"); + if (!BuildGncControlStateToRapid(TOPIC_GNC_CTL_SHAPER, "RGCSRGCS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RGCSRGCS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RGCSRGCS not specified!"); - return false; - } - - BuildGncControlStateToRapid(TOPIC_GNC_CTL_SHAPER, pub_topic, "RGCSRGCS"); - components_++; - } - // ros_gnc_control_trajectory_rapid_gnc_control_trajectory => RGCTRGCT - if (!config_params_.GetBool("use_RGCTRGCT", &use)) { - ROS_FATAL("DDS Bridge: use RGCTRGCT not specified!"); + if (!BuildGncControlStateToRapid(TOPIC_GNC_CTL_TRAJ, "RGCTRGCT")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RGCTRGCT", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RGCTRGCT not specified!"); - return false; - } - - BuildGncControlStateToRapid(TOPIC_GNC_CTL_TRAJ, pub_topic, "RGCTRGCT"); - components_++; - } - // ros_guest_science_rapid_guest_science => RGSRGS - if (!config_params_.GetBool("use_RGSRGS", &use)) { - ROS_FATAL("DDS Bridge: use RGSRGS not specified!"); + if (!BuildGuestScienceToRapid(TOPIC_GUEST_SCIENCE_MANAGER_STATE, + TOPIC_GUEST_SCIENCE_MANAGER_CONFIG, + TOPIC_GUEST_SCIENCE_DATA, + "RGSRGS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RGSRGS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RGSRGS not specified!"); - return false; - } - - BuildGuestScienceToRapid(TOPIC_GUEST_SCIENCE_MANAGER_STATE, - TOPIC_GUEST_SCIENCE_MANAGER_CONFIG, - TOPIC_GUEST_SCIENCE_DATA, - pub_topic, - "RGSRGS"); - components_++; - } - // ros_inertia_rapid_inertia => RIRI - if (!config_params_.GetBool("use_RIRI", &use)) { - ROS_FATAL("DDS Bridge: use RIRI not specified!"); + if (!BuildInertiaDataToRapid(TOPIC_MOBILITY_INERTIA, "RIRI")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RIRI", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RIRI not specified!"); - return false; - } - - BuildInertiaDataToRapid(TOPIC_MOBILITY_INERTIA, pub_topic, "RIRI"); - - components_++; - } - // ros_log_rapid_log_sample => RLRLS - if (!config_params_.GetBool("use_RLRLS", &use)) { - ROS_FATAL("DDS Bridge: use RLRLS not specified!"); + // TODO(rgarciar): Change topic name for a constant + if (!BuildLogSampleToRapid("/rosout", "RLRLS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RLRLS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RLSLS not specified!"); - return false; - } - - // TODO(rgarciar): Change topic name for a constant - BuildLogSampleToRapid("/rosout", - pub_topic, - "RLRLS"); - components_++; - } - // ros_odom_rapid_position => RORP - if (!config_params_.GetBool("use_RORP", &use)) { - ROS_FATAL("DDS Bridge: use RORP not specified!"); + if (!BuildOdomToPosition(TOPIC_GNC_EKF, "RORP")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RORP", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RORP not specified!"); - return false; - } - - BuildOdomToPosition(TOPIC_GNC_EKF, pub_topic, "RORP"); - components_++; + // ros_payload_power_state_rapid_payload_power_state => RPPSRPPS + if (!BuildPayloadStateToPayloadState(TOPIC_HARDWARE_EPS_POWER_STATE, + "RPPSRPPS")) { + return false; } // ros_plan_status_rapid_plan_status => RPSRPS - if (!config_params_.GetBool("use_RPSRPS", &use)) { - ROS_FATAL("DDS Bridge: use RPSRPS not specified!"); + if (!BuildPlanStatusToPlanStatus(TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, + "RPSRPS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RPSRPS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RPSRPS not specified!"); - return false; - } - - BuildPlanStatusToPlanStatus(TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, - pub_topic, - "RPSRPS"); - components_++; - } - // ros_pmc_cmd_state_rapid_pmc_cmd_state => RPCSRPCS - if (!config_params_.GetBool("use_RPCSRPCS", &use)) { - ROS_FATAL("DDS Bridge: use RPCSRPCS not specified!"); + if (!BuildPmcCmdStateToRapid(TOPIC_HARDWARE_PMC_COMMAND, "RPCSRPCS")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RPCSRPCS", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RPCSRPCS not specified!"); - return false; - } - - BuildPmcCmdStateToRapid(TOPIC_HARDWARE_PMC_COMMAND, pub_topic, "RPCSRPCS"); - components_++; + // ros_sparse_mapping_pose_rapid_position => RSMPRP + if (!BuildPoseToPosition(TOPIC_SPARSE_MAPPING_POSE, "RSMPRP")) { + return false; } - // ros_string_rapid_text_message => RSRTM - if (!config_params_.GetBool("use_RSRTM", &use)) { - ROS_FATAL("DDS Bridge: use RSRTM not specified!"); + // ros_vive_pose_rapid_position => RVPRP + if (!BuildPoseToPosition("loc/truth", "RVPRP")) { return false; } - if (use) { - if (!config_params_.GetStr("sub_topic_RSRTM", &sub_topic)) { - ROS_FATAL("DDS Bridge: sub topic RSRTM not specified!"); - return false; - } - - if (!config_params_.GetStr("pub_topic_RSRTM", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RSRTM not specified!"); - return false; - } - - BuildStringToTextMessage(sub_topic, pub_topic, "RSRTM"); - components_++; + // ros_string_rapid_text_message => RSRTM + if (!BuildStringToTextMessage("RSRTM")) { + return false; } // ros_telemetry_rapid_telemetry => RTRT - if (!config_params_.GetBool("use_RTRT", &use)) { - ROS_FATAL("DDS Bridge: use RTRT not specified!"); + if (!BuildTelemetryToRapid(TOPIC_MANAGEMENT_CAMERA_STATE, "RTRT")) { return false; } - if (use) { - if (!config_params_.GetStr("pub_topic_RTRT", &pub_topic)) { - ROS_FATAL("DDS Bridge: pub topic RTRT not specified!"); - return false; - } - - BuildTelemetryToRapid(TOPIC_MANAGEMENT_CAMERA_STATE, pub_topic, "RTRT"); - components_++; + // ros_zones_rapid_compressed_file => RZRCF + if (!BuildZonesToRapidCompressedFile(TOPIC_MOBILITY_ZONES, + SERVICE_MOBILITY_GET_ZONES, + "RZRCF")) { + return false; } // Read and set the publishing rates @@ -1412,6 +1425,7 @@ bool DdsRosBridge::ReadRateParams() { float temp_rate; std::string err_msg; + // Get comm status pub rate if (!config_params_.GetReal("comm_status_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: comm state rate not specified!"); return false; @@ -1422,6 +1436,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get cpu state pub rate if (!config_params_.GetReal("cpu_state_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: cpu state rate not specified!"); return false; @@ -1432,6 +1447,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get disk state pub rate if (!config_params_.GetReal("disk_state_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: disk state rate not specified!"); return false; @@ -1442,6 +1458,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get ekf state pub rate if (!config_params_.GetReal("ekf_state_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: ekf state rate not specified!"); return false; @@ -1452,6 +1469,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get gnc state pub rate if (!config_params_.GetReal("gnc_state_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: gnc state rate not specified!"); return false; @@ -1462,6 +1480,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get pmc command pub rate if (!config_params_.GetReal("pmc_command_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: pmc command rate not specified!"); return false; @@ -1472,6 +1491,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get position pub rate if (!config_params_.GetReal("position_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: position rate not specified!"); return false; @@ -1482,6 +1502,17 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get sparse mapping pose pub rate + if (!config_params_.GetReal("sparse_mapping_pose_rate", &temp_rate)) { + ROS_FATAL("DDS Bridge: sparse mapping pose rate not specified!"); + return false; + } + + if (!SetSparseMappingPoseRate(temp_rate, err_msg)) { + ROS_FATAL("%s", err_msg.c_str()); + return false; + } + return true; } diff --git a/communications/dds_ros_bridge/src/rapid_ekf_ros_ekf.cc b/communications/dds_ros_bridge/src/rapid_ekf_ros_ekf.cc new file mode 100644 index 0000000000..a38286e3dd --- /dev/null +++ b/communications/dds_ros_bridge/src/rapid_ekf_ros_ekf.cc @@ -0,0 +1,107 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "dds_ros_bridge/rapid_ekf_ros_ekf.h" + +namespace ff { + +RapidEkfToRos::RapidEkfToRos( + const std::string& subscribe_topic, + const std::string& subscribe_partition, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RapidSubRosPub(subscribe_topic, + pub_topic, + nh, + "AstrobeeEkfStateProfile", + queue_size) { + // advertise ros topic + pub_ = nh_.advertise(pub_topic, queue_size); + subscriber_partition_ = subscribe_partition; + + // connect to ddsEventLoop + // @todo confirm topic suffix has '-' + try { + dds_event_loop_.connect(this, + subscribe_topic, // topic + subscribe_partition, // name + "AstrobeeEkfStateProfile", // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidEkfRosEkf exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidEkfRosEkf exception unknown"); + throw; + } + + // start thread + StartThread(); +} + +void RapidEkfToRos::CopyTransform3D(const rapid::Transform3D &transform, + geometry_msgs::Pose& pose) { + pose.position.x = transform.xyz[0]; + pose.position.y = transform.xyz[1]; + pose.position.z = transform.xyz[2]; + + pose.orientation.x = transform.rot[0]; + pose.orientation.y = transform.rot[1]; + pose.orientation.z = transform.rot[2]; + pose.orientation.w = transform.rot[3]; +} + +void RapidEkfToRos::CopyVec3D(const rapid::Vec3d& vec_in, + geometry_msgs::Vector3& vec_out) { + vec_out.x = vec_in[0]; + vec_out.y = vec_in[1]; + vec_out.z = vec_in[2]; +} + +void RapidEkfToRos::operator() (rapid::ext::astrobee::EkfState const* ekf_sample) { + ff_msgs::EkfState ekf_msg; + util::RapidHeader2Ros(ekf_sample->hdr, &ekf_msg.header, subscriber_partition_); + + CopyTransform3D(ekf_sample->pose, ekf_msg.pose); + + CopyVec3D(ekf_sample->velocity, ekf_msg.velocity); + CopyVec3D(ekf_sample->omega, ekf_msg.omega); + CopyVec3D(ekf_sample->gyro_bias, ekf_msg.gyro_bias); + CopyVec3D(ekf_sample->accel, ekf_msg.accel); + CopyVec3D(ekf_sample->accel_bias, ekf_msg.accel_bias); + + for (int i = 0; i < 15; i++) { + ekf_msg.cov_diag[i] = ekf_sample->cov_diag[i]; + } + + ekf_msg.confidence = ekf_sample->confidence; + ekf_msg.status = ekf_sample->status; + ekf_msg.of_count = ekf_sample->of_count; + ekf_msg.ml_count = ekf_sample->ml_count; + + CopyTransform3D(ekf_sample->hr_global_pose, ekf_msg.hr_global_pose); + + for (int i = 0; i < 15; i++) { + ekf_msg.ml_mahal_dists[i] = ekf_sample->ml_mahal_dists[i]; + } + + pub_.publish(ekf_msg); +} + +} // end namespace ff diff --git a/communications/dds_ros_bridge/src/rapid_gs_data_ros_gs_data.cc b/communications/dds_ros_bridge/src/rapid_gs_data_ros_gs_data.cc new file mode 100644 index 0000000000..d8bdbeebd5 --- /dev/null +++ b/communications/dds_ros_bridge/src/rapid_gs_data_ros_gs_data.cc @@ -0,0 +1,97 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "dds_ros_bridge/rapid_gs_data_ros_gs_data.h" + +namespace { + +#define GENERATE_GUEST_SCIENCE_CASE(NAME) \ + case rapid::ext::astrobee::GUEST_SCIENCE_##NAME: \ + return ff_msgs::GuestScienceData::NAME + +#define GENERATE_GUEST_SCIENCE_DEFAULT(NAME) \ + default: \ + ROS_FATAL("DDS: Unknown guest science data type: %d", type); \ + return ff_msgs::GuestScienceData::NAME + +uint8_t ConvertType(rapid::ext::astrobee::GuestScienceDataType type) { + switch (type) { + GENERATE_GUEST_SCIENCE_CASE(STRING); + GENERATE_GUEST_SCIENCE_CASE(JSON); + GENERATE_GUEST_SCIENCE_CASE(BINARY); + GENERATE_GUEST_SCIENCE_DEFAULT(STRING); + } +} + +} // end namespace + +namespace ff { + +RapidGuestScienceDataToRos::RapidGuestScienceDataToRos( + const std::string& subscribe_topic, + const std::string& subscribe_partition, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RapidSubRosPub(subscribe_topic, + pub_topic, + nh, + "AstrobeeGuestScienceDataProfile", + queue_size) { + // advertise ros topic + pub_ = nh_.advertise(pub_topic, queue_size); + subscriber_partition_ = subscribe_partition; + + // connect to ddsEventLoop + // @todo confirm topic suffix has '-' + try { + dds_event_loop_.connect(this, + subscribe_topic, // topic + subscribe_partition, // name + "AstrobeeGuestScienceDataProfile", // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidGuestScienceDataToRos exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidGuestScienceDataToRos exception unknown"); + throw; + } + + // start thread + StartThread(); +} + +void RapidGuestScienceDataToRos::operator() ( + rapid::ext::astrobee::GuestScienceData const* data) { + ff_msgs::GuestScienceData msg; + util::RapidHeader2Ros(data->hdr, &msg.header); + + msg.apk_name = data->apkName; + msg.data_type = ConvertType(data->type); + msg.topic = data->topic; + + unsigned char* buf = data->data.get_contiguous_buffer(); + msg.data.reserve(data->data.length()); + msg.data.resize(data->data.length()); + std::memmove(msg.data.data(), buf, data->data.length()); + + pub_.publish(msg); +} + +} // end namespace ff diff --git a/communications/dds_ros_bridge/src/rapid_position_provider_ros_pose_helper.cc b/communications/dds_ros_bridge/src/rapid_position_provider_ros_pose_helper.cc new file mode 100644 index 0000000000..ad9c463ba9 --- /dev/null +++ b/communications/dds_ros_bridge/src/rapid_position_provider_ros_pose_helper.cc @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "dds_ros_bridge/rapid_position_provider_ros_pose_helper.h" + +namespace rapid { + +PositionProviderRosPoseHelper::PositionProviderRosPoseHelper( + PositionTopicPairParameters const& params, + std::string const& entity_name) + : PositionProvider(params, entity_name) { +} + +void PositionProviderRosPoseHelper::Publish( + geometry_msgs::PoseStamped::ConstPtr const& pose) { + rapid::PositionSample& sample = m_dataSupplier.event(); + + sample.hdr.timeStamp = util::RosTime2RapidTime(pose->header.stamp); + sample.hdr.statusCode = 0; + + // pos + // double << float32 + sample.pose.xyz[0] = pose->pose.position.x; + sample.pose.xyz[1] = pose->pose.position.y; + sample.pose.xyz[2] = pose->pose.position.z; + + // orientation as rapid::RAPID_ROT_QUAT + // order specified in rapid::BaseTypesX Y Z W + // float << float3 + sample.pose.rot[0] = pose->pose.orientation.x; + sample.pose.rot[1] = pose->pose.orientation.y; + sample.pose.rot[2] = pose->pose.orientation.z; + sample.pose.rot[3] = pose->pose.orientation.w; + + m_dataSupplier.sendEvent(); +} + +} // end namespace rapid diff --git a/communications/dds_ros_bridge/src/ros_agent_state.cc b/communications/dds_ros_bridge/src/ros_agent_state.cc index ea19a338b9..170810c518 100644 --- a/communications/dds_ros_bridge/src/ros_agent_state.cc +++ b/communications/dds_ros_bridge/src/ros_agent_state.cc @@ -172,8 +172,8 @@ void ff::RosAgentStateToRapid::Callback( mob_msg.checkObstacles = status->check_obstacles; mob_msg.checkKeepouts = status->check_zones; mob_msg.enableAutoReturn = status->auto_return_enabled; - mob_msg.timeSyncEnabled = status->time_sync_enabled; mob_msg.immediateEnabled = status->immediate_enabled; + mob_msg.replanningEnabled = status->replanning_enabled; // Currently the code only supports planners 32 characters long if (status->planner.size() > 32) { diff --git a/communications/dds_ros_bridge/src/ros_compressed_image_rapid_image.cc b/communications/dds_ros_bridge/src/ros_compressed_image_rapid_image.cc index 22496e7199..079e385d49 100644 --- a/communications/dds_ros_bridge/src/ros_compressed_image_rapid_image.cc +++ b/communications/dds_ros_bridge/src/ros_compressed_image_rapid_image.cc @@ -63,8 +63,12 @@ std::string RosCompressedImageRapidImage::GetRapidMimeType( // only two accepted values jpeg or png if (ros_format.compare("jpeg") == 0) return rapid::MIME_IMAGE_JPEG; + if (ros_format.compare("mono8; jpeg compressed ") == 0) + return rapid::MIME_IMAGE_JPEG; if (ros_format.compare("png") == 0) return rapid::MIME_IMAGE_PNG; + + ROS_ERROR_STREAM("DDS ROS Bridge: Unknown camera format: " << ros_format << "."); return ""; } diff --git a/communications/dds_ros_bridge/src/ros_disk_state.cc b/communications/dds_ros_bridge/src/ros_disk_state.cc index b1a285b66e..c9e0eaf837 100644 --- a/communications/dds_ros_bridge/src/ros_disk_state.cc +++ b/communications/dds_ros_bridge/src/ros_disk_state.cc @@ -67,7 +67,7 @@ ff::RosDiskStateToRapid::RosDiskStateToRapid(const std::string& subscribe_topic, false); } -void ff::RosDiskStateToRapid::Callback(const ff_msgs::DiskStateStampedConstPtr& +void ff::RosDiskStateToRapid::Callback(ff_msgs::DiskStateStampedConstPtr const& state) { unsigned int index = 0, length = 0; bool change_config = false; diff --git a/communications/dds_ros_bridge/src/ros_ekf_rapid_ekf.cc b/communications/dds_ros_bridge/src/ros_ekf_rapid_ekf.cc new file mode 100644 index 0000000000..c0d2727e5b --- /dev/null +++ b/communications/dds_ros_bridge/src/ros_ekf_rapid_ekf.cc @@ -0,0 +1,163 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "dds_ros_bridge/ros_ekf_rapid_ekf.h" + +namespace ff { + +RosEkfToRapid::RosEkfToRapid(const std::string& subscribe_topic, + const std::string& pub_topic, + const std::string& rapid_pub_name, + const ros::NodeHandle& nh, + const unsigned int queue_size) : + RosSubRapidPub(subscribe_topic, pub_topic, nh, queue_size), + ekf_sent_(false), + pub_ekf_(false) { + state_supplier_.reset( + new RosEkfToRapid::StateSupplier(rapid::ext::astrobee::EKF_STATE_TOPIC + + pub_topic, rapid_pub_name, "AstrobeeEkfStateProfile", "")); + + // start subscriber + sub_ = nh_.subscribe(subscribe_topic, + queue_size, + &RosEkfToRapid::MsgCallback, this); + + // Initialize the state message + rapid::RapidHelper::initHeader(state_supplier_->event().hdr); + + state_supplier_->event().cov_diag.length(15); + state_supplier_->event().ml_mahal_dists.length(50); + + // Setup timers for publishing the position and ekf but don't start them since + // the rates are 0. The bridge will set this rate at the end of its init + // update: Andrew changed rate to 1.0 to avoid a runtime bounds error. Should + // not affect since autostart argument is set to false. + ekf_timer_ = nh_.createTimer(ros::Rate(1.0), + &RosEkfToRapid::PubEkf, + this, + false, + false); +} + +void RosEkfToRapid::MsgCallback(const ff_msgs::EkfStateConstPtr& msg) { + ekf_msg_ = msg; + + // Don't find the max of_count and ml_count or copy over the ml_mahal_dists if + // we aren't sending it to the ground + if (pub_ekf_) { + if (ekf_sent_) { + // If we just sent the ekf messages to the ground, restart the search for + // the max values + state_supplier_->event().of_count = msg->of_count; + state_supplier_->event().ml_count = msg->ml_count; + ekf_sent_ = false; + } else { + // Brian wants the max values sent to the ground, not the most recent + if (state_supplier_->event().of_count < msg->of_count) { + state_supplier_->event().of_count = msg->of_count; + } + if (state_supplier_->event().ml_count < msg->ml_count) { + state_supplier_->event().ml_count = msg->ml_count; + } + } + + // Brian wants the most recent mahalanobis distance where element 0 is a + // number + if (!std::isnan(msg->ml_mahal_dists[0])) { + for (int i = 0; i < 50; i++) { + state_supplier_->event().ml_mahal_dists[i] = msg->ml_mahal_dists[i]; + } + } + } +} + +void RosEkfToRapid::CopyTransform3D(rapid::Transform3D &transform, + const geometry_msgs::Pose& pose) { + transform.xyz[0] = pose.position.x; + transform.xyz[1] = pose.position.y; + transform.xyz[2] = pose.position.z; + + transform.rot[0] = pose.orientation.x; + transform.rot[1] = pose.orientation.y; + transform.rot[2] = pose.orientation.z; + transform.rot[3] = pose.orientation.w; +} + +void RosEkfToRapid::CopyVec3D(rapid::Vec3d& vec_out, + const geometry_msgs::Vector3& vec_in) { + vec_out[0] = vec_in.x; + vec_out[1] = vec_in.y; + vec_out[2] = vec_in.z; +} + +void RosEkfToRapid::PubEkf(const ros::TimerEvent& event) { + ekf_sent_ = true; + + // Make sure we have received an ekf message before trying to send it + if (ekf_msg_ == NULL) { + return; + } + + rapid::ext::astrobee::EkfState &msg = state_supplier_->event(); + + // Copy time + msg.hdr.timeStamp = util::RosTime2RapidTime(ekf_msg_->header.stamp); + + CopyTransform3D(msg.pose, ekf_msg_->pose); + + CopyVec3D(msg.velocity, ekf_msg_->velocity); + + CopyVec3D(msg.omega, ekf_msg_->omega); + + CopyVec3D(msg.gyro_bias, ekf_msg_->gyro_bias); + + CopyVec3D(msg.accel, ekf_msg_->accel); + + CopyVec3D(msg.accel_bias, ekf_msg_->accel_bias); + + for (int i = 0; i < 15; i++) { + msg.cov_diag[i] = ekf_msg_->cov_diag[i]; + } + + msg.confidence = ekf_msg_->confidence; + + msg.status = ekf_msg_->status; + + // Don't copy over of_count and ml_count since the max has already been found + + CopyTransform3D(msg.hr_global_pose, ekf_msg_->hr_global_pose); + + // Don't copy over ml_mahal_dists since it was copied over when we received + // the message + + // Send message + state_supplier_->sendEvent(); +} + +void RosEkfToRapid::SetEkfPublishRate(float rate) { + if (rate == 0) { + ekf_timer_.stop(); + pub_ekf_ = false; + } else { + pub_ekf_ = true; + ekf_timer_.setPeriod(ros::Duration(ros::Rate(rate))); + ekf_timer_.start(); // Start in case it was never started + } +} + +} // end namespace ff diff --git a/communications/dds_ros_bridge/src/ros_payload_state.cc b/communications/dds_ros_bridge/src/ros_payload_state.cc new file mode 100644 index 0000000000..61ca1b123b --- /dev/null +++ b/communications/dds_ros_bridge/src/ros_payload_state.cc @@ -0,0 +1,129 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "dds_ros_bridge/ros_payload_state.h" + +ff::RosPayloadStateToRapid::RosPayloadStateToRapid( + const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RosSubRapidPub(subscribe_topic, pub_topic, nh, queue_size) { + state_supplier_.reset(new ff::RosPayloadStateToRapid::StateSupplier( + rapid::ext::astrobee::PAYLOAD_STATE_TOPIC + pub_topic, "", + "AstrobeePayloadStateProfile", "")); + + config_supplier_.reset(new ff::RosPayloadStateToRapid::ConfigSupplier( + rapid::ext::astrobee::PAYLOAD_CONFIG_TOPIC + pub_topic, "", + "AstrobeePayloadConfigProfile", "")); + + sub_ = nh_.subscribe(subscribe_topic, + queue_size, + &RosPayloadStateToRapid::Callback, + this); + + rapid::ext::astrobee::PayloadConfig &config_msg = config_supplier_->event(); + rapid::ext::astrobee::PayloadState &state_msg = state_supplier_->event(); + + rapid::RapidHelper::initHeader(config_msg.hdr); + rapid::RapidHelper::initHeader(state_msg.hdr); + + // Initialize the serial number to be 0. Technically this could have been set + // in the initHeader function but it is the last argument and I didn't care to + // specify any of the other arguments + config_msg.hdr.serial = 0; + state_msg.hdr.serial = 0; + + // Resize the arrays + config_msg.payloadNames.length(4); + state_msg.powered.length(4); + + // Fill out config message + std::strncpy(config_msg.payloadNames[0], "Top Front", 9); + config_msg.payloadNames[0][9] = '\0'; + + std::strncpy(config_msg.payloadNames[1], "Top Aft", 7); + config_msg.payloadNames[1][7] = '\0'; + + std::strncpy(config_msg.payloadNames[2], "Bottom Front", 12); + config_msg.payloadNames[2][12] = '\0'; + + std::strncpy(config_msg.payloadNames[3], "Bottom Aft", 10); + config_msg.payloadNames[3][10] = '\0'; + + // Fill out the initial state message + state_msg.powered[0] = false; + state_msg.powered[1] = false; + state_msg.powered[2] = false; + state_msg.powered[3] = false; + + // Send config and state messages + config_msg.hdr.timeStamp = util::RosTime2RapidTime(ros::Time::now()); + config_supplier_->sendEvent(); + + state_msg.hdr.timeStamp = util::RosTime2RapidTime(ros::Time::now()); + state_supplier_->sendEvent(); +} + + +void ff::RosPayloadStateToRapid::Callback( + ff_hw_msgs::EpsPowerStateConstPtr const& state) { + bool publish = false; + unsigned int i; + + rapid::ext::astrobee::PayloadState &state_msg = state_supplier_->event(); + + for (i = 0; i < state->values.size(); i++) { + // Find and check if top front power has changed + if (state->values[i].name == "PAYLOAD_EN_TOP_FRONT") { + if (state->values[i].value != state_msg.powered[0]) { + state_msg.powered[0] = state->values[i].value; + publish = true; + } + } + + // Find and check if top aft power has changed + if (state->values[i].name == "PAYLOAD_EN_TOP_AFT") { + if (state->values[i].value != state_msg.powered[1]) { + state_msg.powered[1] = state->values[i].value; + publish = true; + } + } + + // Find and check if bottom front power has changed + if (state->values[i].name == "PAYLOAD_EN_BOT_FRONT") { + if (state->values[i].value != state_msg.powered[2]) { + state_msg.powered[2] = state->values[i].value; + publish = true; + } + } + + // Find and check if bottom aft power has changed + if (state->values[i].name == "PAYLOAD_EN_BOT_AFT") { + if (state->values[i].value != state_msg.powered[3]) { + state_msg.powered[3] = state->values[i].value; + publish = true; + } + } + } + + if (publish) { + state_msg.hdr.timeStamp = util::RosTime2RapidTime(ros::Time::now()); + state_supplier_->sendEvent(); + } +} diff --git a/communications/dds_ros_bridge/src/ros_plan_status_rapid_plan_status.cc b/communications/dds_ros_bridge/src/ros_plan_status_rapid_plan_status.cc index 16c2f4a730..be526aceea 100644 --- a/communications/dds_ros_bridge/src/ros_plan_status_rapid_plan_status.cc +++ b/communications/dds_ros_bridge/src/ros_plan_status_rapid_plan_status.cc @@ -44,8 +44,8 @@ ff::RosPlanStatusRapidPlanStatus::Callback( msg.hdr.timeStamp = util::RosTime2RapidTime(status->header.stamp); - std::strncpy(msg.planName, status->name.data(), 32); - msg.planName[31] = '\0'; // ensure null-terminated + std::strncpy(msg.planName, status->name.data(), 128); + msg.planName[127] = '\0'; // ensure null-terminated msg.currentPoint = status->point; msg.currentCommand = status->command; diff --git a/communications/dds_ros_bridge/src/ros_pmc_cmd_state.cc b/communications/dds_ros_bridge/src/ros_pmc_cmd_state.cc index 4fd8bcd946..4cd9279987 100644 --- a/communications/dds_ros_bridge/src/ros_pmc_cmd_state.cc +++ b/communications/dds_ros_bridge/src/ros_pmc_cmd_state.cc @@ -68,8 +68,7 @@ void RosPmcCmdStateToRapid::MsgCallback( pmc_msg_ = msg; } -void RosPmcCmdStateToRapid::PubPmcCmdState( - const ros::TimerEvent& event) { +void RosPmcCmdStateToRapid::PubPmcCmdState(const ros::TimerEvent& event) { if (pmc_msg_ == NULL) { return; } diff --git a/communications/dds_ros_bridge/src/ros_pose_rapid_position.cc b/communications/dds_ros_bridge/src/ros_pose_rapid_position.cc new file mode 100644 index 0000000000..0c58096b3d --- /dev/null +++ b/communications/dds_ros_bridge/src/ros_pose_rapid_position.cc @@ -0,0 +1,80 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "dds_ros_bridge/ros_pose_rapid_position.h" + +namespace ff { + +RosPoseRapidPosition::RosPoseRapidPosition(const std::string& subscribe_topic, + const std::string& pub_topic, + bool use_rate, + const ros::NodeHandle& nh, + const unsigned int queue_size) : + RosSubRapidPub(subscribe_topic, pub_topic, nh, queue_size) { + params_.config.poseEncoding = rapid::RAPID_ROT_QUAT; + + params_.topicSuffix += pub_topic; + + // instantiate provider + provider_.reset( + new rapid::PositionProviderRosPoseHelper(params_, "RosPoseRapidPosition")); + + // start subscriber + sub_ = nh_.subscribe(subscribe_topic, + queue_size, + &RosPoseRapidPosition::DataCallback, + this); + + // Setup timers for publishing the position but don't start them until the + // rate is set. The bridge will set the rate at the end of its init if needed + position_timer_ = nh_.createTimer(ros::Rate(1.0), + &RosPoseRapidPosition::PubPosition, + this, + false, + false); + + // This is used for ground testing and in those cases, we don't care about + // limiting the rate of the messge. + use_rate_ = use_rate; +} + +void RosPoseRapidPosition::DataCallback( + geometry_msgs::PoseStampedConstPtr const& data) { + if (use_rate_) { + pose_msg_ = data; + } else { + provider_->Publish(data); + } +} + +void RosPoseRapidPosition::PubPosition(const ros::TimerEvent& event) { + // Make sure we have received a pose message before trying to send it + if (pose_msg_ != NULL) { + provider_->Publish(pose_msg_); + } +} + +void RosPoseRapidPosition::SetPositionPublishRate(float rate) { + if (rate == 0) { + position_timer_.stop(); + } else { + position_timer_.setPeriod(ros::Duration(ros::Rate(rate))); + position_timer_.start(); // Start in case it was never started + } +} +} // end namespace ff diff --git a/communications/dds_ros_bridge/src/ros_telemetry_rapid_telemetry.cc b/communications/dds_ros_bridge/src/ros_telemetry_rapid_telemetry.cc index 22362f034d..ba77b20fca 100644 --- a/communications/dds_ros_bridge/src/ros_telemetry_rapid_telemetry.cc +++ b/communications/dds_ros_bridge/src/ros_telemetry_rapid_telemetry.cc @@ -59,6 +59,7 @@ RosTelemetryRapidTelemetry::RosTelemetryRapidTelemetry( state_supplier_->event().ekfStateRate = 0; state_supplier_->event().gncStateRate = 0; state_supplier_->event().positionRate = 0; + state_supplier_->event().sparseMappingPoseRate = 0; if (AssembleConfig(config_params)) { config_supplier_->sendEvent(); @@ -165,6 +166,15 @@ void RosTelemetryRapidTelemetry::SetPositionRate(float rate) { state_supplier_->sendEvent(); } +void RosTelemetryRapidTelemetry::SetSparseMappingPoseRate(float rate) { + if (rate < 0) { + state_supplier_->event().sparseMappingPoseRate = 0; + } else { + state_supplier_->event().sparseMappingPoseRate = rate; + } + state_supplier_->sendEvent(); +} + bool RosTelemetryRapidTelemetry::AssembleConfig( config_reader::ConfigReader& config_params) { std::string temp_name, temp_resolution, temp_mode; diff --git a/communications/dds_ros_bridge/src/ros_zones_rapid_compressed_file.cc b/communications/dds_ros_bridge/src/ros_zones_rapid_compressed_file.cc new file mode 100644 index 0000000000..fece24d4e2 --- /dev/null +++ b/communications/dds_ros_bridge/src/ros_zones_rapid_compressed_file.cc @@ -0,0 +1,180 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "dds_ros_bridge/ros_zones_rapid_compressed_file.h" + +ff::RosZonesToRapidCompressedFile::RosZonesToRapidCompressedFile( + const std::string& subscribe_topic, + const std::string& get_zones_srv, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RosSubRapidPub(subscribe_topic, pub_topic, nh, queue_size) { + file_supplier_.reset( + new ff::RosZonesToRapidCompressedFile::Supplier( + rapid::ext::astrobee::COMPRESSED_FILE_TOPIC + pub_topic, + "", "AstrobeeCurrentCompressedPlanProfile", "")); + + sub_ = nh_.subscribe(subscribe_topic, + queue_size, + &RosZonesToRapidCompressedFile::Callback, + this); + + get_zones_client_ = nh_.serviceClient(get_zones_srv); + + rapid::RapidHelper::initHeader(file_supplier_->event().hdr); +} + +void ff::RosZonesToRapidCompressedFile::Callback( + visualization_msgs::MarkerArray::ConstPtr const& zones) { + // The zones marker array seems to only be used for visualization and the data + // in the message isn't in the right format to send to the ground. However the + // message tells the bridge that the zones were loaded/changed and that it + // needs to query the choreographer for the zones. + ff_msgs::GetZones get_zones_srv; + + // Check to make sure the get zones service is running. If it isn't, all we + // can do is output the error. + if (!get_zones_client_.exists()) { + ROS_ERROR("Get zones service isn't running! Choreographer may have died!"); + return; + } + + // Check to make sure the service call worked + if (!get_zones_client_.call(get_zones_srv)) { + ROS_ERROR("Get zones service call returned false."); + return; + } + + // Sort zones first. The zones file that comes from GDS can contain multiple + // sets of keepins and keepouts and each keepin and keepout can contain + // multiple boxes. Thus we need to find all the keepins and keepouts that have + // the same name and we need to sort the boxes by their index. Since this code + // will run very infrequently, I'm okay with the sort taking time. + std::map> sorted_zones; + std::vector::iterator zone_it, zone_begin, zone_end, sort_it; + + zone_begin = get_zones_srv.response.zones.begin(); + zone_end = get_zones_srv.response.zones.end(); + for (zone_it = zone_begin; zone_it != zone_end; zone_it++) { + // Check to see if the zone is already in the map + if (sorted_zones.count(zone_it->name) > 0) { + sort_it = sorted_zones[zone_it->name].begin(); + // Use box index to insert the zone into the correct location in the + // vector + sorted_zones[zone_it->name].insert((sort_it + zone_it->index), *zone_it); + } else { + sorted_zones[zone_it->name].push_back(*zone_it); + } + } + + // Convert zones to json string/file + std::string zones_file_content = "{\n"; + + // Put timestamp in file content + // Convert timestamp from seconds to milliseconds + uint64_t time_milli = (uint64_t)get_zones_srv.response.timestamp.sec * 1000; + time_milli += (uint64_t)get_zones_srv.response.timestamp.nsec / 1000000; + zones_file_content += "\t\"timestamp\" : "; + zones_file_content += std::to_string(time_milli); + zones_file_content += "\",\n"; + zones_file_content += "\t\"zones\" : ["; + + std::map>::iterator map_it; + for (map_it = sorted_zones.begin(); map_it != sorted_zones.end(); map_it++) { + zones_file_content += " {\n\t\t\"sequence\": ["; + for (sort_it = map_it->second.begin(); + sort_it != map_it->second.end(); + sort_it++) { + zones_file_content += " [ "; + zones_file_content += std::to_string(sort_it->min.x) + ", "; + zones_file_content += std::to_string(sort_it->min.y) + ", "; + zones_file_content += std::to_string(sort_it->min.z) + ", "; + zones_file_content += std::to_string(sort_it->max.x) + ", "; + zones_file_content += std::to_string(sort_it->max.y) + ", "; + zones_file_content += std::to_string(sort_it->max.z); + zones_file_content += " ],"; + } + + // Remove comma + zones_file_content.pop_back(); + zones_file_content += " ], \n\t\t\"safe\" : "; + // The type and name will be the same for all zones so we can just use the + // first one. Also the type clutter is never used in the GDS zones so I + // don't think it is being used but we will mark it as a keepout just in + // case it is + if (map_it->second.begin()->type == ff_msgs::Zone::KEEPOUT || + map_it->second.begin()->type == ff_msgs::Zone::CLUTTER) { + zones_file_content += "false,\n"; + } else if (map_it->second.begin()->type == ff_msgs::Zone::KEEPIN) { + zones_file_content += "true,\n"; + } else { + ROS_ERROR("DDS ROS bridge: Unknown zone type in zone to compressed file"); + return; + } + + // Add zone name + zones_file_content += "\t\t\"name\" : \"" + map_it->second.begin()->name; + zones_file_content += "\"\n\t},"; + } + + // Remove comma + zones_file_content.pop_back(); + zones_file_content += " ]\n}"; + + // Fill rapid compressed file message + rapid::ext::astrobee::CompressedFile &msg = file_supplier_->event(); + msg.hdr.timeStamp = util::RosTime2RapidTime(get_zones_srv.response.timestamp); + + // Leave the id blank. This is only used when GDS sends a compressed file to + // Astrobee. + + // Set compression type + msg.compressionType = rapid::ext::astrobee::COMPRESSION_TYPE_DEFLATE; + + // Set data + // First compress the data + boost::iostreams::filtering_ostream out; + boost::iostreams::basic_array_source source(zones_file_content.c_str(), + zones_file_content.size()); + + std::vector data; + // This is the max size the compressed file can be in the rapid message + uintmax_t max_cf_size = 128 * 1024; + data.reserve(max_cf_size); + + out.push(boost::iostreams::zlib_compressor()); + out.push(boost::iostreams::back_inserter(data)); + + boost::iostreams::copy(source, out); + + if (data.size() >= max_cf_size) { + ROS_ERROR_STREAM("DDS ROS Bridge: The compressed file for the zones is " << + "too big to send to the ground."); + return; + } + + // Next set the data in the rapid message + msg.compressedFile.ensure_length(data.size(), data.size()); + unsigned char* buf = msg.compressedFile.get_contiguous_buffer(); + for (unsigned int i = 0; i < data.size(); i++) { + buf[i] = data[i]; + } + + file_supplier_->sendEvent(); +} diff --git a/communications/ff_hw_msgs/CMakeLists.txt b/communications/ff_hw_msgs/CMakeLists.txt index b2ad112ac7..27087b46b6 100644 --- a/communications/ff_hw_msgs/CMakeLists.txt +++ b/communications/ff_hw_msgs/CMakeLists.txt @@ -15,16 +15,34 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(ff_hw_msgs) -find_package(catkin2 REQUIRED COMPONENTS +find_package(catkin REQUIRED COMPONENTS message_generation - roscpp std_msgs sensor_msgs ) -create_msg_targets(DIR msg SDIR srv DEPS std_msgs sensor_msgs) +# Generate messages in the 'msg' folder +file(GLOB MSG_FILES msg/*.msg) +foreach(SRC ${MSG_FILES}) + get_filename_component(NAME ${SRC} NAME) + list(APPEND MSG_FILE_NAMES ${NAME}) +endforeach() +add_message_files(FILES ${MSG_FILE_NAMES}) + +# Generate services in the 'srv' folder +file(GLOB SRV_FILES srv/*.srv) +foreach(SRC ${SRV_FILES}) + get_filename_component(NAME ${SRC} NAME) + list(APPEND SRV_FILE_NAMES ${NAME}) +endforeach() +add_service_files(FILES ${SRV_FILE_NAMES}) + +generate_messages( + DEPENDENCIES std_msgs geometry_msgs sensor_msgs +) catkin_package( # INCLUDE_DIRS include @@ -35,4 +53,4 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} -) \ No newline at end of file +) diff --git a/communications/ff_hw_msgs/package.xml b/communications/ff_hw_msgs/package.xml index 6ffefff800..0f1520d52b 100644 --- a/communications/ff_hw_msgs/package.xml +++ b/communications/ff_hw_msgs/package.xml @@ -16,10 +16,8 @@ catkin message_generation - ros_cpp std_msgs sensor_msgs - ros_cpp std_msgs sensor_msgs diff --git a/communications/ff_msgs/CMakeLists.txt b/communications/ff_msgs/CMakeLists.txt index 5040c836e1..37d9842fc3 100644 --- a/communications/ff_msgs/CMakeLists.txt +++ b/communications/ff_msgs/CMakeLists.txt @@ -15,9 +15,10 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(ff_msgs) -find_package(catkin2 REQUIRED COMPONENTS +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs geometry_msgs @@ -26,8 +27,33 @@ find_package(catkin2 REQUIRED COMPONENTS sensor_msgs ) -create_msg_targets(DIR msg SDIR srv ADIR action - DEPS std_msgs geometry_msgs actionlib_msgs trajectory_msgs sensor_msgs) +# Generate messages in the 'msg' folder +file(GLOB MSG_FILES msg/*.msg) +foreach(SRC ${MSG_FILES}) + get_filename_component(NAME ${SRC} NAME) + list(APPEND MSG_FILE_NAMES ${NAME}) +endforeach() +add_message_files(FILES ${MSG_FILE_NAMES}) + +# Generate services in the 'srv' folder +file(GLOB SRV_FILES srv/*.srv) +foreach(SRC ${SRV_FILES}) + get_filename_component(NAME ${SRC} NAME) + list(APPEND SRV_FILE_NAMES ${NAME}) +endforeach() +add_service_files(FILES ${SRV_FILE_NAMES}) + +# Generate actions in the 'action' folder +file(GLOB ACTION_FILES action/*.action) +foreach(SRC ${ACTION_FILES}) + get_filename_component(NAME ${SRC} NAME) + list(APPEND ACTION_FILE_NAMES ${NAME}) +endforeach() +add_action_files(FILES ${ACTION_FILE_NAMES}) + +generate_messages( + DEPENDENCIES std_msgs geometry_msgs actionlib_msgs trajectory_msgs sensor_msgs std_msgs +) catkin_package( # INCLUDE_DIRS include diff --git a/communications/ff_msgs/action/Dock.action b/communications/ff_msgs/action/Dock.action index 430dd5e506..635e04cdee 100644 --- a/communications/ff_msgs/action/Dock.action +++ b/communications/ff_msgs/action/Dock.action @@ -28,6 +28,9 @@ uint8 BERTH_UNKNOWN = 0 # When we undock, we don't know what berth we are in uint8 BERTH_1 = 1 uint8 BERTH_2 = 2 +# Return to dock +bool return_dock + --- # Result diff --git a/communications/ff_msgs/action/Motion.action b/communications/ff_msgs/action/Motion.action index d0482fb039..cdcbd4b361 100644 --- a/communications/ff_msgs/action/Motion.action +++ b/communications/ff_msgs/action/Motion.action @@ -31,11 +31,16 @@ string OFF = off string NOMINAL = nominal string DIFFICULT = difficult string QUIET = quiet + +# These four values no longer used (2020/10). Keep for backward +# compatibility with archived ISS telemetry bags. string PERCHING = perching string UNPERCHING = unperching string DOCKING = docking string UNDOCKING = undocking +string PRECISION = precision + # EXECUTE ONLY : The segment ff_msgs/ControlState[] segment @@ -48,47 +53,48 @@ string reference_frame # Motion result int32 response # Motion action response -int32 ALREADY_THERE = 2 # MOVE: We are already at the location -int32 SUCCESS = 1 # ALL: Motion succeeded -int32 PREEMPTED = 0 # ALL: Motion preempted by thirdparty -int32 PLAN_FAILED = -1 # MOVE/EXEC: Plan/bootstrap failed -int32 VALIDATE_FAILED = -2 # MOVE/EXEC: No comms with mapper -int32 PMC_FAILED = -3 # MOVE/EXEC: PMC failed -int32 CONTROL_FAILED = -4 # ALL: Control failed -int32 OBSTACLE_DETECTED = -5 # ALL: Obstacle / replan disabled -int32 REPLAN_NOT_ENOUGH_TIME = -6 # MOVE/EXEC: Not enough time to replan -int32 REPLAN_FAILED = -7 # MOVE/EXEC: Replanning failed -int32 REVALIDATE_FAILED = -8 # MOVE/EXEC: Revalidating failed -int32 NOT_IN_WAITING_MODE = -9 # ALL: Internal failure -int32 INVALID_FLIGHT_MODE = -10 # ALL: No flight mode specified -int32 UNEXPECTED_EMPTY_SEGMENT = -11 # EXEC: Segment empty -int32 COULD_NOT_RESAMPLE = -12 # EXEC: Could not resample segment -int32 UNEXPECTED_EMPTY_STATES = -13 # MOVE: State vector empty -int32 INVALID_COMMAND = -14 # Command rejected -int32 CANNOT_QUERY_ROBOT_POSE = -15 # TF2 failed to find the current pose -int32 NOT_ON_FIRST_POSE = -16 # EXEC: Not on first pose of exec -int32 BAD_DESIRED_VELOCITY = -17 # Requested vel too high -int32 BAD_DESIRED_ACCELERATION = -18 # Requested accel too high -int32 BAD_DESIRED_OMEGA = -19 # Requested omega too high -int32 BAD_DESIRED_ALPHA = -20 # Requested alpha too high -int32 BAD_DESIRED_RATE = -21 # Requested rate too low -int32 TOLERANCE_VIOLATION_POSITION = -22 # Position tolerance violated -int32 TOLERANCE_VIOLATION_ATTITUDE = -23 # Attitude tolerance violated -int32 TOLERANCE_VIOLATION_VELOCITY = -24 # Velocity tolerance violated -int32 TOLERANCE_VIOLATION_OMEGA = -25 # Omega tolerance violated -int32 VIOLATES_RESAMPLING = -26 # Validation: could not resample@10Hz -int32 VIOLATES_KEEP_OUT = -27 # Validation: Keep out violation -int32 VIOLATES_KEEP_IN = -28 # Validation: Keep in violation -int32 VIOLATES_MINIMUM_FREQUENCY = -29 # Validation: Sample frequency too low -int32 VIOLATES_STATIONARY_ENDPOINT = -30 # Validation: Last setpoint not static -int32 VIOLATES_FIRST_IN_PAST = -31 # Validation: First timestamp in past -int32 VIOLATES_MINIMUM_SETPOINTS = -32 # Validation: Not enough setpoints -int32 VIOLATES_HARD_LIMIT_VEL = -33 # Validation: Velocity too high -int32 VIOLATES_HARD_LIMIT_ACCEL = -34 # Validation: Acceleration too high -int32 VIOLATES_HARD_LIMIT_OMEGA = -35 # Validation: Omega too high -int32 VIOLATES_HARD_LIMIT_ALPHA = -36 # Validation: Alpha too high -int32 CANCELLED = -37 # ALL: Motion cancelled by callee -int32 INVALID_REFERENCE_FRAME = -38 # ALL: Unknown reference frame +int32 ALREADY_THERE = 2 # MOVE: We are already at the location +int32 SUCCESS = 1 # ALL: Motion succeeded +int32 PREEMPTED = 0 # ALL: Motion preempted by thirdparty +int32 PLAN_FAILED = -1 # MOVE/EXEC: Plan/bootstrap failed +int32 VALIDATE_FAILED = -2 # MOVE/EXEC: No comms with mapper +int32 PMC_FAILED = -3 # MOVE/EXEC: PMC failed +int32 CONTROL_FAILED = -4 # ALL: Control failed +int32 OBSTACLE_DETECTED = -5 # ALL: Obstacle / replan disabled +int32 REPLAN_NOT_ENOUGH_TIME = -6 # MOVE/EXEC: Not enough time to replan +int32 REPLAN_FAILED = -7 # MOVE/EXEC: Replanning failed +int32 REVALIDATE_FAILED = -8 # MOVE/EXEC: Revalidating failed +int32 NOT_IN_WAITING_MODE = -9 # ALL: Internal failure +int32 INVALID_FLIGHT_MODE = -10 # ALL: No flight mode specified +int32 UNEXPECTED_EMPTY_SEGMENT = -11 # EXEC: Segment empty +int32 COULD_NOT_RESAMPLE = -12 # EXEC: Could not resample segment +int32 UNEXPECTED_EMPTY_STATES = -13 # MOVE: State vector empty +int32 INVALID_COMMAND = -14 # Command rejected +int32 CANNOT_QUERY_ROBOT_POSE = -15 # TF2 failed to find the current pose +int32 NOT_ON_FIRST_POSE = -16 # EXEC: Not on first pose of exec +int32 BAD_DESIRED_VELOCITY = -17 # Requested vel too high +int32 BAD_DESIRED_ACCELERATION = -18 # Requested accel too high +int32 BAD_DESIRED_OMEGA = -19 # Requested omega too high +int32 BAD_DESIRED_ALPHA = -20 # Requested alpha too high +int32 BAD_DESIRED_RATE = -21 # Requested rate too low +int32 TOLERANCE_VIOLATION_POSITION_ENDPOINT = -22 # Position tolerance violated +int32 TOLERANCE_VIOLATION_POSITION = -23 # Position tolerance violated +int32 TOLERANCE_VIOLATION_ATTITUDE = -24 # Attitude tolerance violated +int32 TOLERANCE_VIOLATION_VELOCITY = -25 # Velocity tolerance violated +int32 TOLERANCE_VIOLATION_OMEGA = -26 # Omega tolerance violated +int32 VIOLATES_RESAMPLING = -27 # Validation: could not resample@10Hz +int32 VIOLATES_KEEP_OUT = -28 # Validation: Keep out violation +int32 VIOLATES_KEEP_IN = -29 # Validation: Keep in violation +int32 VIOLATES_MINIMUM_FREQUENCY = -30 # Validation: Sample frequency too low +int32 VIOLATES_STATIONARY_ENDPOINT = -31 # Validation: Last setpoint not static +int32 VIOLATES_FIRST_IN_PAST = -32 # Validation: First timestamp in past +int32 VIOLATES_MINIMUM_SETPOINTS = -33 # Validation: Not enough setpoints +int32 VIOLATES_HARD_LIMIT_VEL = -34 # Validation: Velocity too high +int32 VIOLATES_HARD_LIMIT_ACCEL = -35 # Validation: Acceleration too high +int32 VIOLATES_HARD_LIMIT_OMEGA = -36 # Validation: Omega too high +int32 VIOLATES_HARD_LIMIT_ALPHA = -37 # Validation: Alpha too high +int32 CANCELLED = -38 # ALL: Motion cancelled by callee +int32 INVALID_REFERENCE_FRAME = -39 # ALL: Unknown reference frame # Human readable FSM result for debugging string fsm_result diff --git a/communications/ff_msgs/bmr/2021_06_02_AgentStateStamped_bool_fields.bmr b/communications/ff_msgs/bmr/2021_06_02_AgentStateStamped_bool_fields.bmr new file mode 100644 index 0000000000..c4f94bb292 --- /dev/null +++ b/communications/ff_msgs/bmr/2021_06_02_AgentStateStamped_bool_fields.bmr @@ -0,0 +1,483 @@ +class update_ff_msgs_AgentStateStamped_472e584da31bd1d215d53d4c8047de43(MessageUpdateRule): + old_type = "ff_msgs/AgentStateStamped" + old_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# State of the Astrobee, based off of rapid::ext::astrobee::AgentState + +# Header with timestamp +std_msgs/Header header + +# Operating state of the Astrobee +ff_msgs/OpState operating_state + +# Plan execution state. State is idle when there is no plan to be executed. Once +# a plan is uploaded, the state change to paused. Upon a run plan command, the +# state will change to executing. If a stop or pause plan command is received or +# a fault occurs, the state will be set to pause. Once the plan is completed, +# the state will go back to idle +ff_msgs/ExecState plan_execution_state + +# Guest science state. If a primary guest science apk is started, the state +# will go from idle to executing. Once the primarty apk is finished, the state +# will go back to idle +ff_msgs/ExecState guest_science_state + +# Mobility state of the Astrobee +ff_msgs/MobilityState mobility_state + +# Proximity to the dock when docking and undocking. Proximity to a handrail when +# perching or unperching. 0 the rest of the time. +float32 proximity + +# Name of profile configuration, i.e. Nominal, IgnoreObstacles, Faceforward, +# Quiet, etc. Profiles specify stuff like target velocity and acceleration, +# collision distance, etc. +string profile_name + +#Defines GN&C gains, hard limits, tolerances, etc. +string flight_mode + +# Maximum linear velocity to target while translating +float32 target_linear_velocity +# Maximum linear acceleration to target while translating +float32 target_linear_accel +# Maximum angular velocity to target while rotating +float32 target_angular_velocity +# Maximum angular acceleration to target while rotating +float32 target_angular_accel +# Minimum distance margin to maintain away from obstacles +float32 collision_distance + +# Specifies whether the Astrobee is allowed to fly blind i.e. not faceforward +bool holonomic_enabled + +# Specifies whether the Astrobee is checking for obstacles +bool check_obstacles + +# Specifies whether the Astrobee is checking the keepin and keepout zones +bool check_zones + +# Specifies whether the Astrobee is allowed to auto return. Please note, +# Astrobee will only use this flags when deciding if it should auto return. If +# the astrobee gets a command to auto return from the operator or guest science, +# it will auto return without checking this flag +bool auto_return_enabled + +# Specifies whether timesync is enabled. Hopefully this will be used to +# synchronize movements between Astrobees. +bool time_sync_enabled + +# Specifies whether the choreographer should execute a segment immediately or +# based on the time stamp in the segement +bool immediate_enabled + +# Specifies the current planner being used +string planner + +# Specifies the current world being used +string world + +# Number of seconds since Unix Epoch +uint32 boot_time + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id + +================================================================================ +MSG: ff_msgs/OpState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Operating States, based off of the enumeration constants +# in rapid::ext::astrobee::AgentState. +# +# *MUST* be kept in sync with the DDS IDL file in astrobee_common + +uint8 READY = 0 # Freeflyer is ready to accept commands +uint8 PLAN_EXECUTION = 1 # Freeflyer is executing a plan +uint8 TELEOPERATION = 2 # Freeflyer is executing a teleop command +uint8 AUTO_RETURN = 3 # Freeflyer is autonomously returning to the dock +# The freeflyer is either executing a fault response or there is a fault +# occurring in the system that prevents the freeflyer from moving +uint8 FAULT = 4 + +# Operating state +uint8 state + +================================================================================ +MSG: ff_msgs/ExecState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Execution States, based off of the enumeration constants in +# rapid::ext::astrobee::AgentState +# +# *MUST* be kept in sync with the DDS IDL file in astrobee_common + +uint8 IDLE = 0 # Process is idle +uint8 EXECUTING = 1 # Process is executing +uint8 PAUSED = 2 # Process is paused +uint8 ERROR = 3 # Process encountered an error + +# Execution state +uint8 state + +================================================================================ +MSG: ff_msgs/MobilityState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Mobility states, based off the enumeration constants in +# rapid::ext::astrobee::AgentState +# +# *MUST* be kept in sync with the DDS IDL file in astrobee_common + +uint8 DRIFTING = 0 # Astrobee's propulsion is off +uint8 STOPPING = 1 # Astrobee is either stopping or stopped +uint8 FLYING = 2 # Astrobee is flying +uint8 DOCKING = 3 # Astrobee is either docking or undocking +uint8 PERCHING = 4 # Astrobee is either perching or unperching + +# Mobility state +uint8 state + +# Specifies the progress of the action. For docking, this value can be N to -N +# where N through 1 specifies the progress of a docking action, 0 is docked, and +# -1 through -N specifies the process of an undocking action. For stopping, this +# value is either 1 or 0 where 1 means the robot is coming to a stop and 0 means +# the robot is stopped. For perching, this value can be N to -N where N through +# 1 specifies the progress of a perching action, 0 is perched, and -1 through +# -N specifies the process of an unperching action. +int32 sub_state +""" + + new_type = "ff_msgs/AgentStateStamped" + new_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# State of the Astrobee, based off of rapid::ext::astrobee::AgentState + +# Header with timestamp +std_msgs/Header header + +# Operating state of the Astrobee +ff_msgs/OpState operating_state + +# Plan execution state. State is idle when there is no plan to be executed. Once +# a plan is uploaded, the state change to paused. Upon a run plan command, the +# state will change to executing. If a stop or pause plan command is received or +# a fault occurs, the state will be set to pause. Once the plan is completed, +# the state will go back to idle +ff_msgs/ExecState plan_execution_state + +# Guest science state. If a primary guest science apk is started, the state +# will go from idle to executing. Once the primarty apk is finished, the state +# will go back to idle +ff_msgs/ExecState guest_science_state + +# Mobility state of the Astrobee +ff_msgs/MobilityState mobility_state + +# Proximity to the dock when docking and undocking. Proximity to a handrail when +# perching or unperching. 0 the rest of the time. +float32 proximity + +# Name of profile configuration, i.e. Nominal, IgnoreObstacles, Faceforward, +# Quiet, etc. Profiles specify stuff like target velocity and acceleration, +# collision distance, etc. +string profile_name + +#Defines GN&C gains, hard limits, tolerances, etc. +string flight_mode + +# Maximum linear velocity to target while translating +float32 target_linear_velocity +# Maximum linear acceleration to target while translating +float32 target_linear_accel +# Maximum angular velocity to target while rotating +float32 target_angular_velocity +# Maximum angular acceleration to target while rotating +float32 target_angular_accel +# Minimum distance margin to maintain away from obstacles +float32 collision_distance + +# Specifies whether the Astrobee is allowed to fly blind i.e. not faceforward +bool holonomic_enabled + +# Specifies whether the Astrobee is checking for obstacles +bool check_obstacles + +# Specifies whether the Astrobee is checking the keepin and keepout zones +bool check_zones + +# Specifies whether the Astrobee is allowed to auto return. Please note, +# Astrobee will only use this flags when deciding if it should auto return. If +# the astrobee gets a command to auto return from the operator or guest science, +# it will auto return without checking this flag +bool auto_return_enabled + +# Specifies whether the choreographer should execute a segment immediately or +# based on the time stamp in the segement +bool immediate_enabled + +# Specifies the current planner being used +string planner + +# Specifies whether re-planning is allowed +bool replanning_enabled + +# Specifies the current world being used +string world + +# Number of seconds since Unix Epoch +uint32 boot_time + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: ff_msgs/OpState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Operating States, based off of the enumeration constants +# in rapid::ext::astrobee::AgentState. +# +# *MUST* be kept in sync with the DDS IDL file in astrobee_common + +uint8 READY = 0 # Freeflyer is ready to accept commands +uint8 PLAN_EXECUTION = 1 # Freeflyer is executing a plan +uint8 TELEOPERATION = 2 # Freeflyer is executing a teleop command +uint8 AUTO_RETURN = 3 # Freeflyer is autonomously returning to the dock +# The freeflyer is either executing a fault response or there is a fault +# occurring in the system that prevents the freeflyer from moving +uint8 FAULT = 4 + +# Operating state +uint8 state + +================================================================================ +MSG: ff_msgs/ExecState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Execution States, based off of the enumeration constants in +# rapid::ext::astrobee::AgentState +# +# *MUST* be kept in sync with the DDS IDL file in astrobee_common + +uint8 IDLE = 0 # Process is idle +uint8 EXECUTING = 1 # Process is executing +uint8 PAUSED = 2 # Process is paused +uint8 ERROR = 3 # Process encountered an error + +# Execution state +uint8 state + +================================================================================ +MSG: ff_msgs/MobilityState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Mobility states, based off the enumeration constants in +# rapid::ext::astrobee::AgentState +# +# *MUST* be kept in sync with the DDS IDL file in astrobee_common + +uint8 DRIFTING = 0 # Astrobee's propulsion is off +uint8 STOPPING = 1 # Astrobee is either stopping or stopped +uint8 FLYING = 2 # Astrobee is flying +uint8 DOCKING = 3 # Astrobee is either docking or undocking +uint8 PERCHING = 4 # Astrobee is either perching or unperching + +# Mobility state +uint8 state + +# Specifies the progress of the action. For docking, this value can be N to -N +# where N through 1 specifies the progress of a docking action, 0 is docked, and +# -1 through -N specifies the process of an undocking action. For stopping, this +# value is either 1 or 0 where 1 means the robot is coming to a stop and 0 means +# the robot is stopped. For perching, this value can be N to -N where N through +# 1 specifies the progress of a perching action, 0 is perched, and -1 through +# -N specifies the process of an unperching action. +int32 sub_state +""" + + order = 0 + migrated_types = [ + ("Header","Header"), + ("OpState","OpState"), + ("ExecState","ExecState"), + ("MobilityState","MobilityState"),] + + valid = True + + def update(self, old_msg, new_msg): + self.migrate(old_msg.header, new_msg.header) + self.migrate(old_msg.operating_state, new_msg.operating_state) + self.migrate(old_msg.plan_execution_state, new_msg.plan_execution_state) + self.migrate(old_msg.guest_science_state, new_msg.guest_science_state) + self.migrate(old_msg.mobility_state, new_msg.mobility_state) + new_msg.proximity = old_msg.proximity + new_msg.profile_name = old_msg.profile_name + new_msg.flight_mode = old_msg.flight_mode + new_msg.target_linear_velocity = old_msg.target_linear_velocity + new_msg.target_linear_accel = old_msg.target_linear_accel + new_msg.target_angular_velocity = old_msg.target_angular_velocity + new_msg.target_angular_accel = old_msg.target_angular_accel + new_msg.collision_distance = old_msg.collision_distance + new_msg.holonomic_enabled = old_msg.holonomic_enabled + new_msg.check_obstacles = old_msg.check_obstacles + new_msg.check_zones = old_msg.check_zones + new_msg.auto_return_enabled = old_msg.auto_return_enabled + new_msg.immediate_enabled = old_msg.immediate_enabled + new_msg.planner = old_msg.planner + new_msg.world = old_msg.world + new_msg.boot_time = old_msg.boot_time + + # Add field: bool replanning_enabled. Set to False to accurately represent old behavior. + new_msg.replanning_enabled = False + # Delete field: time_sync_enabled. Data loss is ok because it was never really used. diff --git a/communications/ff_msgs/bmr/2021_08_04_FaultState_renumber_enum.bmr b/communications/ff_msgs/bmr/2021_08_04_FaultState_renumber_enum.bmr new file mode 100644 index 0000000000..9d48e55c3d --- /dev/null +++ b/communications/ff_msgs/bmr/2021_08_04_FaultState_renumber_enum.bmr @@ -0,0 +1,296 @@ +import subprocess +import sys + +bre_folder = subprocess.check_output(["catkin_find", "--first-only", "bag_processing", "scripts"]).decode("utf-8").strip() +sys.path.insert(0, bre_folder) +import utilities.bmr_renumber_enum as bre + +OLD_STATE_ENUM = """ +# No faults are occurring in system +uint8 FUNCTIONAL = 0 +# Faults are occurring in the system which may or may not leave the robot +# functional +uint8 FAULT = 1 +# A fault has occurred that indirectly affects the motion of the robot +uint8 BLOCKED = 2 +""" + +NEW_STATE_ENUM = """ +# System starting up +uint8 STARTING_UP = 0 +# No faults are occurring in system +uint8 FUNCTIONAL = 1 +# Faults are occurring in the system which may or may not leave the robot +# functional +uint8 FAULT = 2 +# A fault has occurred that indirectly affects the motion of the robot +uint8 BLOCKED = 3 +# Recovering from nodes dying on startup +uint8 RELOADING_NODELETS = 4 +""" + +RENUMBER_ENUM_STATE = bre.get_renumber_function(OLD_STATE_ENUM, NEW_STATE_ENUM) + +class update_ff_msgs_FaultState_3f21141637835b4058a2c2ea0e8588fd(MessageUpdateRule): + old_type = "ff_msgs/FaultState" + old_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Fault state message used to alert the ground of the current faults. It is also +# used to express to the executive that a fault has occurred that indirectly +# affects the motion of the robot. + +std_msgs/Header header + +# No faults are occurring in system +uint8 FUNCTIONAL = 0 +# Faults are occurring in the system which may or may not leave the robot +# functional +uint8 FAULT = 1 +# A fault has occurred that indirectly affects the motion of the robot +uint8 BLOCKED = 2 + +# Not sent to the ground, only used by the executive to determine what commands +# to accept. +uint8 state + +# Faults occurring in the astrobee system, can only send 32 faults down +ff_msgs/Fault[] faults + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id + +================================================================================ +MSG: ff_msgs/Fault +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Fault message is used to provide all the information about an occurring fault + +time time_of_fault # Time when fault occurred + +uint32 id # id specifying fault + +string msg # string specifying why the fault occurred + +ff_msgs/FaultData[] data # Data used for fault analysis + +================================================================================ +MSG: ff_msgs/FaultData +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Fault data messsage contains information of why the fault occurred + +uint8 DATA_TYPE_FLOAT = 0 # Data in this msg is of type float +uint8 DATA_TYPE_INT = 1 # Data in this msg is of type int +uint8 DATA_TYPE_STRING = 2 # Data in this msg is of type string + +string key # Specifies what the data in the msg is, can only be 32 chars long + +uint8 data_type # Specifies the type of data in the message + +float32 f # Value used for fault analysis, data_type must be 0 +int32 i # Value used for fault analysis, data_type must be 1 +string s # String used for fault analysis, data_type must be 2 +""" + + new_type = "ff_msgs/FaultState" + new_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Fault state message used to alert the ground of the current faults. It is also +# used to express to the executive that a fault has occurred that indirectly +# affects the motion of the robot. + +std_msgs/Header header + +# Not sent to the ground, only used by the executive to determine what commands +# to accept. +uint8 state +# System starting up +uint8 STARTING_UP = 0 +# No faults are occurring in system +uint8 FUNCTIONAL = 1 +# Faults are occurring in the system which may or may not leave the robot +# functional +uint8 FAULT = 2 +# A fault has occurred that indirectly affects the motion of the robot +uint8 BLOCKED = 3 +# Recovering from nodes dying on startup +uint8 RELOADING_NODELETS = 4 + +# A human readable version of the state - only really used for when nodes die on +# startup and need to be restarted. +string hr_state + +# Faults occurring in the astrobee system, can only send 32 faults down +ff_msgs/Fault[] faults + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: ff_msgs/Fault +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Fault message is used to provide all the information about an occurring fault + +time time_of_fault # Time when fault occurred + +uint32 id # id specifying fault + +string msg # string specifying why the fault occurred + +ff_msgs/FaultData[] data # Data used for fault analysis + +================================================================================ +MSG: ff_msgs/FaultData +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Fault data messsage contains information of why the fault occurred + +uint8 DATA_TYPE_FLOAT = 0 # Data in this msg is of type float +uint8 DATA_TYPE_INT = 1 # Data in this msg is of type int +uint8 DATA_TYPE_STRING = 2 # Data in this msg is of type string + +string key # Specifies what the data in the msg is, can only be 32 chars long + +uint8 data_type # Specifies the type of data in the message + +float32 f # Value used for fault analysis, data_type must be 0 +int32 i # Value used for fault analysis, data_type must be 1 +string s # String used for fault analysis, data_type must be 2 +""" + + order = 0 + migrated_types = [ + ("Header","Header"), + ("Fault","Fault"),] + + valid = True + + def update(self, old_msg, new_msg): + self.migrate(old_msg.header, new_msg.header) + self.migrate_array(old_msg.faults, new_msg.faults, "ff_msgs/Fault") + + # renumber legacy enumerated values to match new label numbering + new_msg.state = RENUMBER_ENUM_STATE(old_msg.state) + # fill new field with no-data value: empty string + new_msg.hr_state = '' diff --git a/communications/ff_msgs/bmr/2021_08_05_FlightMode_add_field.bmr b/communications/ff_msgs/bmr/2021_08_05_FlightMode_add_field.bmr new file mode 100644 index 0000000000..2e707337bc --- /dev/null +++ b/communications/ff_msgs/bmr/2021_08_05_FlightMode_add_field.bmr @@ -0,0 +1,214 @@ +class update_ff_msgs_FlightMode_21af9f940eeb9407d4d58829e8cfc200(MessageUpdateRule): + old_type = "ff_msgs/FlightMode" + old_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message captures all information in a flight mode + +Header header # Metadata + +string name # Name of the flight mode + +bool control_enabled # Is control enabled? + +float32 collision_radius # Collision radius in meters + +# Tolerances (all in SI units) +float32 tolerance_pos # Position tolerance in m +float32 tolerance_vel # Velocity tolerance in m/s +float32 tolerance_att # Attitude tolerance in rads +float32 tolerance_omega # Angular acceleration tolerance in rad/s +float32 tolerance_time # Acceptable lag betwee TX and RX of control + +# Controller gains +geometry_msgs/Vector3 att_kp # Positional proportional constant +geometry_msgs/Vector3 att_ki # Positional integrative constant +geometry_msgs/Vector3 omega_kd # Attidue derivative constant +geometry_msgs/Vector3 pos_kp # Positional proportional contant +geometry_msgs/Vector3 pos_ki # Positional integrative constant +geometry_msgs/Vector3 vel_kd # Positional derivative constant + +# Hard limit on planning +float32 hard_limit_vel # Position tolerance in m/s +float32 hard_limit_accel # Position tolerance in m/s^2 +float32 hard_limit_omega # Position tolerance in rads/s +float32 hard_limit_alpha # Position tolerance in rads/s^2 + +# Impeller speed +uint8 speed # Current speed gain +uint8 SPEED_MIN = 0 # Min acceptable gain +uint8 SPEED_OFF = 0 # Blowers off +uint8 SPEED_QUIET = 1 # Quiet mode +uint8 SPEED_NOMINAL = 2 # Nomainal mode +uint8 SPEED_AGGRESSIVE = 3 # Aggressive mode +uint8 SPEED_MAX = 3 # Max acceptable gain + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +""" + + new_type = "ff_msgs/FlightMode" + new_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message captures all information in a flight mode + +Header header # Metadata + +string name # Name of the flight mode + +bool control_enabled # Is control enabled? + +float32 collision_radius # Collision radius in meters + +# Tolerances (all in SI units) +float32 tolerance_pos_endpoint # Endpoint position tolerance in m +float32 tolerance_pos # Position tolerance in m +float32 tolerance_vel # Velocity tolerance in m/s +float32 tolerance_att # Attitude tolerance in rads +float32 tolerance_omega # Angular acceleration tolerance in rad/s +float32 tolerance_time # Acceptable lag betwee TX and RX of control + +# Controller gains +geometry_msgs/Vector3 att_kp # Positional proportional constant +geometry_msgs/Vector3 att_ki # Positional integrative constant +geometry_msgs/Vector3 omega_kd # Attidue derivative constant +geometry_msgs/Vector3 pos_kp # Positional proportional contant +geometry_msgs/Vector3 pos_ki # Positional integrative constant +geometry_msgs/Vector3 vel_kd # Positional derivative constant + +# Hard limit on planning +float32 hard_limit_vel # Position tolerance in m/s +float32 hard_limit_accel # Position tolerance in m/s^2 +float32 hard_limit_omega # Position tolerance in rads/s +float32 hard_limit_alpha # Position tolerance in rads/s^2 + +# Impeller speed +uint8 speed # Current speed gain +uint8 SPEED_MIN = 0 # Min acceptable gain +uint8 SPEED_OFF = 0 # Blowers off +uint8 SPEED_QUIET = 1 # Quiet mode +uint8 SPEED_NOMINAL = 2 # Nomainal mode +uint8 SPEED_AGGRESSIVE = 3 # Aggressive mode +uint8 SPEED_MAX = 3 # Max acceptable gain + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +""" + + order = 0 + migrated_types = [ + ("Header","Header"), + ("geometry_msgs/Vector3","geometry_msgs/Vector3"),] + + valid = True + + def update(self, old_msg, new_msg): + self.migrate(old_msg.header, new_msg.header) + new_msg.name = old_msg.name + new_msg.control_enabled = old_msg.control_enabled + new_msg.collision_radius = old_msg.collision_radius + new_msg.tolerance_pos = old_msg.tolerance_pos + new_msg.tolerance_vel = old_msg.tolerance_vel + new_msg.tolerance_att = old_msg.tolerance_att + new_msg.tolerance_omega = old_msg.tolerance_omega + new_msg.tolerance_time = old_msg.tolerance_time + self.migrate(old_msg.att_kp, new_msg.att_kp) + self.migrate(old_msg.att_ki, new_msg.att_ki) + self.migrate(old_msg.omega_kd, new_msg.omega_kd) + self.migrate(old_msg.pos_kp, new_msg.pos_kp) + self.migrate(old_msg.pos_ki, new_msg.pos_ki) + self.migrate(old_msg.vel_kd, new_msg.vel_kd) + new_msg.hard_limit_vel = old_msg.hard_limit_vel + new_msg.hard_limit_accel = old_msg.hard_limit_accel + new_msg.hard_limit_omega = old_msg.hard_limit_omega + new_msg.hard_limit_alpha = old_msg.hard_limit_alpha + new_msg.speed = old_msg.speed + + # New field. Fill by coping the value of tolerance_pos, which + # accurately represents the old behavior. + new_msg.tolerance_pos_endpoint = old_msg.tolerance_pos diff --git a/communications/ff_msgs/bmr/2021_08_25_MotionResult_renumber_enum.bmr b/communications/ff_msgs/bmr/2021_08_25_MotionResult_renumber_enum.bmr new file mode 100644 index 0000000000..ba5476ad4f --- /dev/null +++ b/communications/ff_msgs/bmr/2021_08_25_MotionResult_renumber_enum.bmr @@ -0,0 +1,529 @@ +import subprocess +import sys + +bre_folder = subprocess.check_output(["catkin_find", "--first-only", "bag_processing", "scripts"]).decode("utf-8").strip() +sys.path.insert(0, bre_folder) +import utilities.bmr_renumber_enum as bre + +OLD_RESPONSE_ENUM = """ +int32 ALREADY_THERE = 2 # MOVE: We are already at the location +int32 SUCCESS = 1 # ALL: Motion succeeded +int32 PREEMPTED = 0 # ALL: Motion preempted by thirdparty +int32 PLAN_FAILED = -1 # MOVE/EXEC: Plan/bootstrap failed +int32 VALIDATE_FAILED = -2 # MOVE/EXEC: No comms with mapper +int32 PMC_FAILED = -3 # MOVE/EXEC: PMC failed +int32 CONTROL_FAILED = -4 # ALL: Control failed +int32 OBSTACLE_DETECTED = -5 # ALL: Obstacle / replan disabled +int32 REPLAN_NOT_ENOUGH_TIME = -6 # MOVE/EXEC: Not enough time to replan +int32 REPLAN_FAILED = -7 # MOVE/EXEC: Replanning failed +int32 REVALIDATE_FAILED = -8 # MOVE/EXEC: Revalidating failed +int32 NOT_IN_WAITING_MODE = -9 # ALL: Internal failure +int32 INVALID_FLIGHT_MODE = -10 # ALL: No flight mode specified +int32 UNEXPECTED_EMPTY_SEGMENT = -11 # EXEC: Segment empty +int32 COULD_NOT_RESAMPLE = -12 # EXEC: Could not resample segment +int32 UNEXPECTED_EMPTY_STATES = -13 # MOVE: State vector empty +int32 INVALID_COMMAND = -14 # Command rejected +int32 CANNOT_QUERY_ROBOT_POSE = -15 # TF2 failed to find the current pose +int32 NOT_ON_FIRST_POSE = -16 # EXEC: Not on first pose of exec +int32 BAD_DESIRED_VELOCITY = -17 # Requested vel too high +int32 BAD_DESIRED_ACCELERATION = -18 # Requested accel too high +int32 BAD_DESIRED_OMEGA = -19 # Requested omega too high +int32 BAD_DESIRED_ALPHA = -20 # Requested alpha too high +int32 BAD_DESIRED_RATE = -21 # Requested rate too low +int32 TOLERANCE_VIOLATION_POSITION = -22 # Position tolerance violated +int32 TOLERANCE_VIOLATION_ATTITUDE = -23 # Attitude tolerance violated +int32 TOLERANCE_VIOLATION_VELOCITY = -24 # Velocity tolerance violated +int32 TOLERANCE_VIOLATION_OMEGA = -25 # Omega tolerance violated +int32 VIOLATES_RESAMPLING = -26 # Validation: could not resample@10Hz +int32 VIOLATES_KEEP_OUT = -27 # Validation: Keep out violation +int32 VIOLATES_KEEP_IN = -28 # Validation: Keep in violation +int32 VIOLATES_MINIMUM_FREQUENCY = -29 # Validation: Sample frequency too low +int32 VIOLATES_STATIONARY_ENDPOINT = -30 # Validation: Last setpoint not static +int32 VIOLATES_FIRST_IN_PAST = -31 # Validation: First timestamp in past +int32 VIOLATES_MINIMUM_SETPOINTS = -32 # Validation: Not enough setpoints +int32 VIOLATES_HARD_LIMIT_VEL = -33 # Validation: Velocity too high +int32 VIOLATES_HARD_LIMIT_ACCEL = -34 # Validation: Acceleration too high +int32 VIOLATES_HARD_LIMIT_OMEGA = -35 # Validation: Omega too high +int32 VIOLATES_HARD_LIMIT_ALPHA = -36 # Validation: Alpha too high +int32 CANCELLED = -37 # ALL: Motion cancelled by callee +int32 INVALID_REFERENCE_FRAME = -38 # ALL: Unknown reference frame +""" + +NEW_RESPONSE_ENUM = """ +int32 ALREADY_THERE = 2 # MOVE: We are already at the location +int32 SUCCESS = 1 # ALL: Motion succeeded +int32 PREEMPTED = 0 # ALL: Motion preempted by thirdparty +int32 PLAN_FAILED = -1 # MOVE/EXEC: Plan/bootstrap failed +int32 VALIDATE_FAILED = -2 # MOVE/EXEC: No comms with mapper +int32 PMC_FAILED = -3 # MOVE/EXEC: PMC failed +int32 CONTROL_FAILED = -4 # ALL: Control failed +int32 OBSTACLE_DETECTED = -5 # ALL: Obstacle / replan disabled +int32 REPLAN_NOT_ENOUGH_TIME = -6 # MOVE/EXEC: Not enough time to replan +int32 REPLAN_FAILED = -7 # MOVE/EXEC: Replanning failed +int32 REVALIDATE_FAILED = -8 # MOVE/EXEC: Revalidating failed +int32 NOT_IN_WAITING_MODE = -9 # ALL: Internal failure +int32 INVALID_FLIGHT_MODE = -10 # ALL: No flight mode specified +int32 UNEXPECTED_EMPTY_SEGMENT = -11 # EXEC: Segment empty +int32 COULD_NOT_RESAMPLE = -12 # EXEC: Could not resample segment +int32 UNEXPECTED_EMPTY_STATES = -13 # MOVE: State vector empty +int32 INVALID_COMMAND = -14 # Command rejected +int32 CANNOT_QUERY_ROBOT_POSE = -15 # TF2 failed to find the current pose +int32 NOT_ON_FIRST_POSE = -16 # EXEC: Not on first pose of exec +int32 BAD_DESIRED_VELOCITY = -17 # Requested vel too high +int32 BAD_DESIRED_ACCELERATION = -18 # Requested accel too high +int32 BAD_DESIRED_OMEGA = -19 # Requested omega too high +int32 BAD_DESIRED_ALPHA = -20 # Requested alpha too high +int32 BAD_DESIRED_RATE = -21 # Requested rate too low +int32 TOLERANCE_VIOLATION_POSITION_ENDPOINT = -22 # Position tolerance violated +int32 TOLERANCE_VIOLATION_POSITION = -23 # Position tolerance violated +int32 TOLERANCE_VIOLATION_ATTITUDE = -24 # Attitude tolerance violated +int32 TOLERANCE_VIOLATION_VELOCITY = -25 # Velocity tolerance violated +int32 TOLERANCE_VIOLATION_OMEGA = -26 # Omega tolerance violated +int32 VIOLATES_RESAMPLING = -27 # Validation: could not resample@10Hz +int32 VIOLATES_KEEP_OUT = -28 # Validation: Keep out violation +int32 VIOLATES_KEEP_IN = -29 # Validation: Keep in violation +int32 VIOLATES_MINIMUM_FREQUENCY = -30 # Validation: Sample frequency too low +int32 VIOLATES_STATIONARY_ENDPOINT = -31 # Validation: Last setpoint not static +int32 VIOLATES_FIRST_IN_PAST = -32 # Validation: First timestamp in past +int32 VIOLATES_MINIMUM_SETPOINTS = -33 # Validation: Not enough setpoints +int32 VIOLATES_HARD_LIMIT_VEL = -34 # Validation: Velocity too high +int32 VIOLATES_HARD_LIMIT_ACCEL = -35 # Validation: Acceleration too high +int32 VIOLATES_HARD_LIMIT_OMEGA = -36 # Validation: Omega too high +int32 VIOLATES_HARD_LIMIT_ALPHA = -37 # Validation: Alpha too high +int32 CANCELLED = -38 # ALL: Motion cancelled by callee +int32 INVALID_REFERENCE_FRAME = -39 # ALL: Unknown reference frame +""" + +RENUMBER_ENUM_RESPONSE = bre.get_renumber_function(OLD_RESPONSE_ENUM, NEW_RESPONSE_ENUM) + +class update_ff_msgs_MotionResult_90b50bd8775be477978777463874adb3(MessageUpdateRule): + old_type = "ff_msgs/MotionResult" + old_full_text = """ +# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== + +# Motion result +int32 response # Motion action response +int32 ALREADY_THERE = 2 # MOVE: We are already at the location +int32 SUCCESS = 1 # ALL: Motion succeeded +int32 PREEMPTED = 0 # ALL: Motion preempted by thirdparty +int32 PLAN_FAILED = -1 # MOVE/EXEC: Plan/bootstrap failed +int32 VALIDATE_FAILED = -2 # MOVE/EXEC: No comms with mapper +int32 PMC_FAILED = -3 # MOVE/EXEC: PMC failed +int32 CONTROL_FAILED = -4 # ALL: Control failed +int32 OBSTACLE_DETECTED = -5 # ALL: Obstacle / replan disabled +int32 REPLAN_NOT_ENOUGH_TIME = -6 # MOVE/EXEC: Not enough time to replan +int32 REPLAN_FAILED = -7 # MOVE/EXEC: Replanning failed +int32 REVALIDATE_FAILED = -8 # MOVE/EXEC: Revalidating failed +int32 NOT_IN_WAITING_MODE = -9 # ALL: Internal failure +int32 INVALID_FLIGHT_MODE = -10 # ALL: No flight mode specified +int32 UNEXPECTED_EMPTY_SEGMENT = -11 # EXEC: Segment empty +int32 COULD_NOT_RESAMPLE = -12 # EXEC: Could not resample segment +int32 UNEXPECTED_EMPTY_STATES = -13 # MOVE: State vector empty +int32 INVALID_COMMAND = -14 # Command rejected +int32 CANNOT_QUERY_ROBOT_POSE = -15 # TF2 failed to find the current pose +int32 NOT_ON_FIRST_POSE = -16 # EXEC: Not on first pose of exec +int32 BAD_DESIRED_VELOCITY = -17 # Requested vel too high +int32 BAD_DESIRED_ACCELERATION = -18 # Requested accel too high +int32 BAD_DESIRED_OMEGA = -19 # Requested omega too high +int32 BAD_DESIRED_ALPHA = -20 # Requested alpha too high +int32 BAD_DESIRED_RATE = -21 # Requested rate too low +int32 TOLERANCE_VIOLATION_POSITION = -22 # Position tolerance violated +int32 TOLERANCE_VIOLATION_ATTITUDE = -23 # Attitude tolerance violated +int32 TOLERANCE_VIOLATION_VELOCITY = -24 # Velocity tolerance violated +int32 TOLERANCE_VIOLATION_OMEGA = -25 # Omega tolerance violated +int32 VIOLATES_RESAMPLING = -26 # Validation: could not resample@10Hz +int32 VIOLATES_KEEP_OUT = -27 # Validation: Keep out violation +int32 VIOLATES_KEEP_IN = -28 # Validation: Keep in violation +int32 VIOLATES_MINIMUM_FREQUENCY = -29 # Validation: Sample frequency too low +int32 VIOLATES_STATIONARY_ENDPOINT = -30 # Validation: Last setpoint not static +int32 VIOLATES_FIRST_IN_PAST = -31 # Validation: First timestamp in past +int32 VIOLATES_MINIMUM_SETPOINTS = -32 # Validation: Not enough setpoints +int32 VIOLATES_HARD_LIMIT_VEL = -33 # Validation: Velocity too high +int32 VIOLATES_HARD_LIMIT_ACCEL = -34 # Validation: Acceleration too high +int32 VIOLATES_HARD_LIMIT_OMEGA = -35 # Validation: Omega too high +int32 VIOLATES_HARD_LIMIT_ALPHA = -36 # Validation: Alpha too high +int32 CANCELLED = -37 # ALL: Motion cancelled by callee +int32 INVALID_REFERENCE_FRAME = -38 # ALL: Unknown reference frame + +# Human readable FSM result for debugging +string fsm_result + +# The flight mode parameters used +ff_msgs/FlightMode flight_mode + +# The final segment that was flown +ff_msgs/ControlState[] segment + + +================================================================================ +MSG: ff_msgs/FlightMode +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message captures all information in a flight mode + +Header header # Metadata + +string name # Name of the flight mode + +bool control_enabled # Is control enabled? + +float32 collision_radius # Collision radius in meters + +# Tolerances (all in SI units) +float32 tolerance_pos # Position tolerance in m +float32 tolerance_vel # Velocity tolerance in m/s +float32 tolerance_att # Attitude tolerance in rads +float32 tolerance_omega # Angular acceleration tolerance in rad/s +float32 tolerance_time # Acceptable lag betwee TX and RX of control + +# Controller gains +geometry_msgs/Vector3 att_kp # Positional proportional constant +geometry_msgs/Vector3 att_ki # Positional integrative constant +geometry_msgs/Vector3 omega_kd # Attidue derivative constant +geometry_msgs/Vector3 pos_kp # Positional proportional contant +geometry_msgs/Vector3 pos_ki # Positional integrative constant +geometry_msgs/Vector3 vel_kd # Positional derivative constant + +# Hard limit on planning +float32 hard_limit_vel # Position tolerance in m/s +float32 hard_limit_accel # Position tolerance in m/s^2 +float32 hard_limit_omega # Position tolerance in rads/s +float32 hard_limit_alpha # Position tolerance in rads/s^2 + +# Impeller speed +uint8 speed # Current speed gain +uint8 SPEED_MIN = 0 # Min acceptable gain +uint8 SPEED_OFF = 0 # Blowers off +uint8 SPEED_QUIET = 1 # Quiet mode +uint8 SPEED_NOMINAL = 2 # Nomainal mode +uint8 SPEED_AGGRESSIVE = 3 # Aggressive mode +uint8 SPEED_MAX = 3 # Max acceptable gain + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +================================================================================ +MSG: ff_msgs/ControlState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Full state vector containing Time, Pose, Vel, and Accel +# +# when {time} +# flight_mode {string} - disctates, gains, tolerances, etc. +# pose {Point position, Quaternion orientation} +# twist {Vector3 linear, Vector3 angular} +# accel {Vector3 linear, Vector3 angular} + +time when +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Twist accel + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of position and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: geometry_msgs/Twist +# This expresses velocity in free space broken into its linear and angular parts. +Vector3 linear +Vector3 angular +""" + + new_type = "ff_msgs/MotionResult" + new_full_text = """ +# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== + +# Motion result +int32 response # Motion action response +int32 ALREADY_THERE = 2 # MOVE: We are already at the location +int32 SUCCESS = 1 # ALL: Motion succeeded +int32 PREEMPTED = 0 # ALL: Motion preempted by thirdparty +int32 PLAN_FAILED = -1 # MOVE/EXEC: Plan/bootstrap failed +int32 VALIDATE_FAILED = -2 # MOVE/EXEC: No comms with mapper +int32 PMC_FAILED = -3 # MOVE/EXEC: PMC failed +int32 CONTROL_FAILED = -4 # ALL: Control failed +int32 OBSTACLE_DETECTED = -5 # ALL: Obstacle / replan disabled +int32 REPLAN_NOT_ENOUGH_TIME = -6 # MOVE/EXEC: Not enough time to replan +int32 REPLAN_FAILED = -7 # MOVE/EXEC: Replanning failed +int32 REVALIDATE_FAILED = -8 # MOVE/EXEC: Revalidating failed +int32 NOT_IN_WAITING_MODE = -9 # ALL: Internal failure +int32 INVALID_FLIGHT_MODE = -10 # ALL: No flight mode specified +int32 UNEXPECTED_EMPTY_SEGMENT = -11 # EXEC: Segment empty +int32 COULD_NOT_RESAMPLE = -12 # EXEC: Could not resample segment +int32 UNEXPECTED_EMPTY_STATES = -13 # MOVE: State vector empty +int32 INVALID_COMMAND = -14 # Command rejected +int32 CANNOT_QUERY_ROBOT_POSE = -15 # TF2 failed to find the current pose +int32 NOT_ON_FIRST_POSE = -16 # EXEC: Not on first pose of exec +int32 BAD_DESIRED_VELOCITY = -17 # Requested vel too high +int32 BAD_DESIRED_ACCELERATION = -18 # Requested accel too high +int32 BAD_DESIRED_OMEGA = -19 # Requested omega too high +int32 BAD_DESIRED_ALPHA = -20 # Requested alpha too high +int32 BAD_DESIRED_RATE = -21 # Requested rate too low +int32 TOLERANCE_VIOLATION_POSITION_ENDPOINT = -22 # Position tolerance violated +int32 TOLERANCE_VIOLATION_POSITION = -23 # Position tolerance violated +int32 TOLERANCE_VIOLATION_ATTITUDE = -24 # Attitude tolerance violated +int32 TOLERANCE_VIOLATION_VELOCITY = -25 # Velocity tolerance violated +int32 TOLERANCE_VIOLATION_OMEGA = -26 # Omega tolerance violated +int32 VIOLATES_RESAMPLING = -27 # Validation: could not resample@10Hz +int32 VIOLATES_KEEP_OUT = -28 # Validation: Keep out violation +int32 VIOLATES_KEEP_IN = -29 # Validation: Keep in violation +int32 VIOLATES_MINIMUM_FREQUENCY = -30 # Validation: Sample frequency too low +int32 VIOLATES_STATIONARY_ENDPOINT = -31 # Validation: Last setpoint not static +int32 VIOLATES_FIRST_IN_PAST = -32 # Validation: First timestamp in past +int32 VIOLATES_MINIMUM_SETPOINTS = -33 # Validation: Not enough setpoints +int32 VIOLATES_HARD_LIMIT_VEL = -34 # Validation: Velocity too high +int32 VIOLATES_HARD_LIMIT_ACCEL = -35 # Validation: Acceleration too high +int32 VIOLATES_HARD_LIMIT_OMEGA = -36 # Validation: Omega too high +int32 VIOLATES_HARD_LIMIT_ALPHA = -37 # Validation: Alpha too high +int32 CANCELLED = -38 # ALL: Motion cancelled by callee +int32 INVALID_REFERENCE_FRAME = -39 # ALL: Unknown reference frame + +# Human readable FSM result for debugging +string fsm_result + +# The flight mode parameters used +ff_msgs/FlightMode flight_mode + +# The final segment that was flown +ff_msgs/ControlState[] segment + + +================================================================================ +MSG: ff_msgs/FlightMode +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message captures all information in a flight mode + +Header header # Metadata + +string name # Name of the flight mode + +bool control_enabled # Is control enabled? + +float32 collision_radius # Collision radius in meters + +# Tolerances (all in SI units) +float32 tolerance_pos_endpoint # Endpoint position tolerance in m +float32 tolerance_pos # Position tolerance in m +float32 tolerance_vel # Velocity tolerance in m/s +float32 tolerance_att # Attitude tolerance in rads +float32 tolerance_omega # Angular acceleration tolerance in rad/s +float32 tolerance_time # Acceptable lag betwee TX and RX of control + +# Controller gains +geometry_msgs/Vector3 att_kp # Positional proportional constant +geometry_msgs/Vector3 att_ki # Positional integrative constant +geometry_msgs/Vector3 omega_kd # Attidue derivative constant +geometry_msgs/Vector3 pos_kp # Positional proportional contant +geometry_msgs/Vector3 pos_ki # Positional integrative constant +geometry_msgs/Vector3 vel_kd # Positional derivative constant + +# Hard limit on planning +float32 hard_limit_vel # Position tolerance in m/s +float32 hard_limit_accel # Position tolerance in m/s^2 +float32 hard_limit_omega # Position tolerance in rads/s +float32 hard_limit_alpha # Position tolerance in rads/s^2 + +# Impeller speed +uint8 speed # Current speed gain +uint8 SPEED_MIN = 0 # Min acceptable gain +uint8 SPEED_OFF = 0 # Blowers off +uint8 SPEED_QUIET = 1 # Quiet mode +uint8 SPEED_NOMINAL = 2 # Nomainal mode +uint8 SPEED_AGGRESSIVE = 3 # Aggressive mode +uint8 SPEED_MAX = 3 # Max acceptable gain + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +================================================================================ +MSG: ff_msgs/ControlState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Full state vector containing Time, Pose, Vel, and Accel +# +# when {time} +# flight_mode {string} - disctates, gains, tolerances, etc. +# pose {Point position, Quaternion orientation} +# twist {Vector3 linear, Vector3 angular} +# accel {Vector3 linear, Vector3 angular} + +time when +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Twist accel + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of position and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: geometry_msgs/Twist +# This expresses velocity in free space broken into its linear and angular parts. +Vector3 linear +Vector3 angular +""" + + order = 0 + migrated_types = [ + ("FlightMode","FlightMode"), + ("ControlState","ControlState"),] + + valid = True + + def update(self, old_msg, new_msg): + new_msg.fsm_result = old_msg.fsm_result + self.migrate(old_msg.flight_mode, new_msg.flight_mode) + self.migrate_array(old_msg.segment, new_msg.segment, "ff_msgs/ControlState") + + # renumber legacy enumerated values to match new label numbering + new_msg.response = RENUMBER_ENUM_RESPONSE(old_msg.response) diff --git a/communications/ff_msgs/bmr/2022_01_11_CpuStateStamped_add_field.bmr b/communications/ff_msgs/bmr/2022_01_11_CpuStateStamped_add_field.bmr new file mode 100644 index 0000000000..4e88ff37d5 --- /dev/null +++ b/communications/ff_msgs/bmr/2022_01_11_CpuStateStamped_add_field.bmr @@ -0,0 +1,250 @@ +class update_ff_msgs_CpuStateStamped_40cac5670a1d8d81e75089cf6eaba744(MessageUpdateRule): + old_type = "ff_msgs/CpuStateStamped" + old_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Cpu state message with timestamp. + +# Header with timestamp +std_msgs/Header header + +# Machine name (llp, hlp, mlp, etc) +string name + +# Load constants +string NICE=nice +string USER=user +string SYS=sys +string VIRT=virt +string TOTAL=total + +# The available fields within the load values, mostly uses the constants +# defined above. +string[] load_fields + +# Average loads for all processors combined +float32[] avg_loads + +# Temperature for a cpu (average of all thermal zones) +float32 temp + +# Information for each processor +# Size of the array specifies how many processors are on the board, whether +# or not all of them are enabled. +ff_msgs/CpuState[] cpus + + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id + +================================================================================ +MSG: ff_msgs/CpuState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# State of a CPU. + +# Processor is on (enabled) or not +bool enabled + +# The load (in percentages) of the cpu, for the fields given in +# CpuStateStamped +float32[] loads + +# Current operating frequency in Hz +uint32 frequency + +# Max frequency (may be less than theoretical limit of the processor) +uint32 max_frequency +""" + + new_type = "ff_msgs/CpuStateStamped" + new_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Cpu state message with timestamp. + +# Header with timestamp +std_msgs/Header header + +# Machine name (llp, hlp, mlp, etc) +string name + +# Load constants +string NICE=nice +string USER=user +string SYS=sys +string VIRT=virt +string TOTAL=total + +# The available fields within the load values, mostly uses the constants +# defined above. +string[] load_fields + +# Average loads for all processors combined +float32[] avg_loads + +# Temperature for a cpu (average of all thermal zones) +float32 temp + +# Information for each processor +# Size of the array specifies how many processors are on the board, whether +# or not all of them are enabled. +ff_msgs/CpuState[] cpus + +# Load usage of individual ROS nodes +ff_msgs/CpuNodeState[] load_nodes +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: ff_msgs/CpuState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# State of a CPU. + +# Processor is on (enabled) or not +bool enabled + +# The load (in percentages) of the cpu, for the fields given in +# CpuStateStamped +float32[] loads + +# Current operating frequency in Hz +uint32 frequency + +# Max frequency (may be less than theoretical limit of the processor) +uint32 max_frequency + +================================================================================ +MSG: ff_msgs/CpuNodeState +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# State of a CPU load for a node. + +# Node name +string name + +# The load (in percentages) of the cpu +float32 load +""" + + order = 0 + migrated_types = [ + ("Header","Header"), + ("CpuState","CpuState"),] + + valid = True + + def update(self, old_msg, new_msg): + self.migrate(old_msg.header, new_msg.header) + new_msg.name = old_msg.name + new_msg.load_fields = old_msg.load_fields + new_msg.avg_loads = old_msg.avg_loads + new_msg.temp = old_msg.temp + self.migrate_array(old_msg.cpus, new_msg.cpus, "ff_msgs/CpuState") + + # New field. Fill with no-data value. + new_msg.load_nodes = [] diff --git a/communications/ff_msgs/bmr/2022_03_09_FlightMode_delete_field.bmr b/communications/ff_msgs/bmr/2022_03_09_FlightMode_delete_field.bmr new file mode 100644 index 0000000000..83539c6fd7 --- /dev/null +++ b/communications/ff_msgs/bmr/2022_03_09_FlightMode_delete_field.bmr @@ -0,0 +1,216 @@ +class update_ff_msgs_FlightMode_64fef5727fd56589af93ac307057592a(MessageUpdateRule): + old_type = "ff_msgs/FlightMode" + old_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message captures all information in a flight mode + +Header header # Metadata + +string name # Name of the flight mode + +bool control_enabled # Is control enabled? + +float32 collision_radius # Collision radius in meters + +# Tolerances (all in SI units) +float32 tolerance_pos_endpoint # Endpoint position tolerance in m +float32 tolerance_pos # Position tolerance in m +float32 tolerance_vel # Velocity tolerance in m/s +float32 tolerance_att # Attitude tolerance in rads +float32 tolerance_omega # Angular acceleration tolerance in rad/s +float32 tolerance_time # Acceptable lag betwee TX and RX of control + +# Controller gains +geometry_msgs/Vector3 att_kp # Positional proportional constant +geometry_msgs/Vector3 att_ki # Positional integrative constant +geometry_msgs/Vector3 omega_kd # Attidue derivative constant +geometry_msgs/Vector3 pos_kp # Positional proportional contant +geometry_msgs/Vector3 pos_ki # Positional integrative constant +geometry_msgs/Vector3 vel_kd # Positional derivative constant + +# Hard limit on planning +float32 hard_limit_vel # Position tolerance in m/s +float32 hard_limit_accel # Position tolerance in m/s^2 +float32 hard_limit_omega # Position tolerance in rads/s +float32 hard_limit_alpha # Position tolerance in rads/s^2 + +# Impeller speed +uint8 speed # Current speed gain +uint8 SPEED_MIN = 0 # Min acceptable gain +uint8 SPEED_OFF = 0 # Blowers off +uint8 SPEED_QUIET = 1 # Quiet mode +uint8 SPEED_NOMINAL = 2 # Nomainal mode +uint8 SPEED_AGGRESSIVE = 3 # Aggressive mode +uint8 SPEED_MAX = 3 # Max acceptable gain + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +""" + + new_type = "ff_msgs/FlightMode" + new_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message captures all information in a flight mode + +Header header # Metadata + +string name # Name of the flight mode + +bool control_enabled # Is control enabled? + +# Tolerances (all in SI units) +float32 tolerance_pos_endpoint # Endpoint position tolerance in m +float32 tolerance_pos # Position tolerance in m +float32 tolerance_vel # Velocity tolerance in m/s +float32 tolerance_att # Attitude tolerance in rads +float32 tolerance_omega # Angular acceleration tolerance in rad/s +float32 tolerance_time # Acceptable lag betwee TX and RX of control + +# Controller gains +geometry_msgs/Vector3 att_kp # Positional proportional constant +geometry_msgs/Vector3 att_ki # Positional integrative constant +geometry_msgs/Vector3 omega_kd # Attidue derivative constant +geometry_msgs/Vector3 pos_kp # Positional proportional contant +geometry_msgs/Vector3 pos_ki # Positional integrative constant +geometry_msgs/Vector3 vel_kd # Positional derivative constant + +# Hard limit on planning +float32 hard_limit_vel # Position tolerance in m/s +float32 hard_limit_accel # Position tolerance in m/s^2 +float32 hard_limit_omega # Position tolerance in rads/s +float32 hard_limit_alpha # Position tolerance in rads/s^2 + +# Impeller speed +uint8 speed # Current speed gain +uint8 SPEED_MIN = 0 # Min acceptable gain +uint8 SPEED_OFF = 0 # Blowers off +uint8 SPEED_QUIET = 1 # Quiet mode +uint8 SPEED_NOMINAL = 2 # Nomainal mode +uint8 SPEED_AGGRESSIVE = 3 # Aggressive mode +uint8 SPEED_MAX = 3 # Max acceptable gain + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +""" + + order = 1 + migrated_types = [ + ("Header","Header"), + ("geometry_msgs/Vector3","geometry_msgs/Vector3"),] + + valid = True + + def update(self, old_msg, new_msg): + self.migrate(old_msg.header, new_msg.header) + new_msg.name = old_msg.name + new_msg.control_enabled = old_msg.control_enabled + new_msg.tolerance_pos_endpoint = old_msg.tolerance_pos_endpoint + new_msg.tolerance_pos = old_msg.tolerance_pos + new_msg.tolerance_vel = old_msg.tolerance_vel + new_msg.tolerance_att = old_msg.tolerance_att + new_msg.tolerance_omega = old_msg.tolerance_omega + new_msg.tolerance_time = old_msg.tolerance_time + self.migrate(old_msg.att_kp, new_msg.att_kp) + self.migrate(old_msg.att_ki, new_msg.att_ki) + self.migrate(old_msg.omega_kd, new_msg.omega_kd) + self.migrate(old_msg.pos_kp, new_msg.pos_kp) + self.migrate(old_msg.pos_ki, new_msg.pos_ki) + self.migrate(old_msg.vel_kd, new_msg.vel_kd) + new_msg.hard_limit_vel = old_msg.hard_limit_vel + new_msg.hard_limit_accel = old_msg.hard_limit_accel + new_msg.hard_limit_omega = old_msg.hard_limit_omega + new_msg.hard_limit_alpha = old_msg.hard_limit_alpha + new_msg.speed = old_msg.speed + + # Deleted field: float collision_radius + + # We determined that this field was not useful in the + # archived ISS telemetry, so it's ok to delete it when + # migrating old messages. (Collision distance was + # configured in multiple places, with only one of those + # values actually affecting the system behavior, and + # this particular field was confusingly not being used.) diff --git a/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr b/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr new file mode 100644 index 0000000000..ab9d8c43b9 --- /dev/null +++ b/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr @@ -0,0 +1,205 @@ +class update_ff_msgs_VisualLandmarks_bc9fc09f69d7758225ed41ee26f6375e(MessageUpdateRule): + old_type = "ff_msgs/VisualLandmarks" + old_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# The observation from a camera image, of image coordinates +# and associated 3D coordinates. Used for sparse mapping and AR tags. + +Header header # header with timestamp +uint32 camera_id # image ID, associated with registration pulse +geometry_msgs/Pose pose # estimated camera pose from features +ff_msgs/VisualLandmark[] landmarks # list of all landmarks + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of position and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: ff_msgs/VisualLandmark +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# A single observation of a feature, with an image coordinate +# and known 3D coordinate + +float32 x # known 3D position x +float32 y # known 3D position y +float32 z # known 3D position z +float32 u # feature image coordinate x +float32 v # feature image coordinate y +""" + + new_type = "ff_msgs/VisualLandmarks" + new_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# The observation from a camera image, of image coordinates +# and associated 3D coordinates. Used for sparse mapping and AR tags. + +Header header # header with timestamp +uint32 camera_id # image ID, associated with registration pulse +geometry_msgs/Pose pose # estimated camera pose from features +ff_msgs/VisualLandmark[] landmarks # list of all landmarks +float32 runtime # Time it took to calculate the pose and matching landmarks + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of position and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: ff_msgs/VisualLandmark +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# A single observation of a feature, with an image coordinate +# and known 3D coordinate + +float32 x # known 3D position x +float32 y # known 3D position y +float32 z # known 3D position z +float32 u # feature image coordinate x +float32 v # feature image coordinate y +""" + + order = 0 + migrated_types = [ + ("Header","Header"), + ("geometry_msgs/Pose","geometry_msgs/Pose"), + ("VisualLandmark","VisualLandmark"),] + + valid = True + + def update(self, old_msg, new_msg): + self.migrate(old_msg.header, new_msg.header) + new_msg.camera_id = old_msg.camera_id + self.migrate(old_msg.pose, new_msg.pose) + self.migrate_array(old_msg.landmarks, new_msg.landmarks, "ff_msgs/VisualLandmark") + #No matching field name in old message + new_msg.runtime = 0. diff --git a/communications/ff_msgs/bmr/noetic/noetic_BatteryState_add_field.bmr b/communications/ff_msgs/bmr/noetic/noetic_BatteryState_add_field.bmr new file mode 100644 index 0000000000..c8a50bd1b4 --- /dev/null +++ b/communications/ff_msgs/bmr/noetic/noetic_BatteryState_add_field.bmr @@ -0,0 +1,169 @@ +class update_sensor_msgs_BatteryState_476f837fa6771f6e16e3bf4ef96f8770(MessageUpdateRule): + old_type = "sensor_msgs/BatteryState" + old_full_text = """ +# Constants are chosen to match the enums in the linux kernel +# defined in include/linux/power_supply.h as of version 3.7 +# The one difference is for style reasons the constants are +# all uppercase not mixed case. + +# Power supply status constants +uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0 +uint8 POWER_SUPPLY_STATUS_CHARGING = 1 +uint8 POWER_SUPPLY_STATUS_DISCHARGING = 2 +uint8 POWER_SUPPLY_STATUS_NOT_CHARGING = 3 +uint8 POWER_SUPPLY_STATUS_FULL = 4 + +# Power supply health constants +uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 +uint8 POWER_SUPPLY_HEALTH_GOOD = 1 +uint8 POWER_SUPPLY_HEALTH_OVERHEAT = 2 +uint8 POWER_SUPPLY_HEALTH_DEAD = 3 +uint8 POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4 +uint8 POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5 +uint8 POWER_SUPPLY_HEALTH_COLD = 6 +uint8 POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7 +uint8 POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8 + +# Power supply technology (chemistry) constants +uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 +uint8 POWER_SUPPLY_TECHNOLOGY_NIMH = 1 +uint8 POWER_SUPPLY_TECHNOLOGY_LION = 2 +uint8 POWER_SUPPLY_TECHNOLOGY_LIPO = 3 +uint8 POWER_SUPPLY_TECHNOLOGY_LIFE = 4 +uint8 POWER_SUPPLY_TECHNOLOGY_NICD = 5 +uint8 POWER_SUPPLY_TECHNOLOGY_LIMN = 6 + +Header header +float32 voltage # Voltage in Volts (Mandatory) +float32 current # Negative when discharging (A) (If unmeasured NaN) +float32 charge # Current charge in Ah (If unmeasured NaN) +float32 capacity # Capacity in Ah (last full capacity) (If unmeasured NaN) +float32 design_capacity # Capacity in Ah (design capacity) (If unmeasured NaN) +float32 percentage # Charge percentage on 0 to 1 range (If unmeasured NaN) +uint8 power_supply_status # The charging status as reported. Values defined above +uint8 power_supply_health # The battery health metric. Values defined above +uint8 power_supply_technology # The battery chemistry. Values defined above +bool present # True if the battery is present + +float32[] cell_voltage # An array of individual cell voltages for each cell in the pack + # If individual voltages unknown but number of cells known set each to NaN +string location # The location into which the battery is inserted. (slot number or plug) +string serial_number # The best approximation of the battery serial number + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id +""" + + new_type = "sensor_msgs/BatteryState" + new_full_text = """ +# Constants are chosen to match the enums in the linux kernel +# defined in include/linux/power_supply.h as of version 3.7 +# The one difference is for style reasons the constants are +# all uppercase not mixed case. + +# Power supply status constants +uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0 +uint8 POWER_SUPPLY_STATUS_CHARGING = 1 +uint8 POWER_SUPPLY_STATUS_DISCHARGING = 2 +uint8 POWER_SUPPLY_STATUS_NOT_CHARGING = 3 +uint8 POWER_SUPPLY_STATUS_FULL = 4 + +# Power supply health constants +uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 +uint8 POWER_SUPPLY_HEALTH_GOOD = 1 +uint8 POWER_SUPPLY_HEALTH_OVERHEAT = 2 +uint8 POWER_SUPPLY_HEALTH_DEAD = 3 +uint8 POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4 +uint8 POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5 +uint8 POWER_SUPPLY_HEALTH_COLD = 6 +uint8 POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7 +uint8 POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8 + +# Power supply technology (chemistry) constants +uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 +uint8 POWER_SUPPLY_TECHNOLOGY_NIMH = 1 +uint8 POWER_SUPPLY_TECHNOLOGY_LION = 2 +uint8 POWER_SUPPLY_TECHNOLOGY_LIPO = 3 +uint8 POWER_SUPPLY_TECHNOLOGY_LIFE = 4 +uint8 POWER_SUPPLY_TECHNOLOGY_NICD = 5 +uint8 POWER_SUPPLY_TECHNOLOGY_LIMN = 6 + +Header header +float32 voltage # Voltage in Volts (Mandatory) +float32 temperature # Temperature in Degrees Celsius (If unmeasured NaN) +float32 current # Negative when discharging (A) (If unmeasured NaN) +float32 charge # Current charge in Ah (If unmeasured NaN) +float32 capacity # Capacity in Ah (last full capacity) (If unmeasured NaN) +float32 design_capacity # Capacity in Ah (design capacity) (If unmeasured NaN) +float32 percentage # Charge percentage on 0 to 1 range (If unmeasured NaN) +uint8 power_supply_status # The charging status as reported. Values defined above +uint8 power_supply_health # The battery health metric. Values defined above +uint8 power_supply_technology # The battery chemistry. Values defined above +bool present # True if the battery is present + +float32[] cell_voltage # An array of individual cell voltages for each cell in the pack + # If individual voltages unknown but number of cells known set each to NaN +float32[] cell_temperature # An array of individual cell temperatures for each cell in the pack + # If individual temperatures unknown but number of cells known set each to NaN +string location # The location into which the battery is inserted. (slot number or plug) +string serial_number # The best approximation of the battery serial number + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id +""" + + order = 0 + migrated_types = [ + ("Header","Header"),] + + valid = True + + def update(self, old_msg, new_msg): + self.migrate(old_msg.header, new_msg.header) + new_msg.voltage = old_msg.voltage + #No matching field name in old message + new_msg.temperature = 0. + new_msg.current = old_msg.current + new_msg.charge = old_msg.charge + new_msg.capacity = old_msg.capacity + new_msg.design_capacity = old_msg.design_capacity + new_msg.percentage = old_msg.percentage + new_msg.power_supply_status = old_msg.power_supply_status + new_msg.power_supply_health = old_msg.power_supply_health + new_msg.power_supply_technology = old_msg.power_supply_technology + new_msg.present = old_msg.present + new_msg.cell_voltage = old_msg.cell_voltage + new_msg.location = old_msg.location + new_msg.serial_number = old_msg.serial_number + + # ROS added field during transition to noetic: cell_temperature + # Fill with no-data value: empty list + new_msg.cell_temperature = [] diff --git a/communications/ff_msgs/bmr/rosbag_rewrite_types_rules.json b/communications/ff_msgs/bmr/rosbag_rewrite_types_rules.json new file mode 100644 index 0000000000..722bb862ea --- /dev/null +++ b/communications/ff_msgs/bmr/rosbag_rewrite_types_rules.json @@ -0,0 +1,14 @@ +{ + "rename_types": [ + ], + "fix_message_definition_topic_patterns": [ + "/gs/*", + "/command", + "/hw/cam_sci/compressed", + "/hw/cam_sci_info", + "/mgt/disk_monitor/state", + "/mgt/cpu_monitor/state", + "/signals", + "/cv/*" + ] +} diff --git a/communications/ff_msgs/msg/AgentStateStamped.msg b/communications/ff_msgs/msg/AgentStateStamped.msg index 1d9fa16173..43e84455d6 100644 --- a/communications/ff_msgs/msg/AgentStateStamped.msg +++ b/communications/ff_msgs/msg/AgentStateStamped.msg @@ -76,10 +76,6 @@ bool check_zones # it will auto return without checking this flag bool auto_return_enabled -# Specifies whether timesync is enabled. Hopefully this will be used to -# synchronize movements between Astrobees. -bool time_sync_enabled - # Specifies whether the choreographer should execute a segment immediately or # based on the time stamp in the segement bool immediate_enabled @@ -87,6 +83,9 @@ bool immediate_enabled # Specifies the current planner being used string planner +# Specifies whether re-planning is allowed +bool replanning_enabled + # Specifies the current world being used string world diff --git a/communications/ff_msgs/msg/CombinedNavState.msg b/communications/ff_msgs/msg/CombinedNavState.msg new file mode 100644 index 0000000000..f1d68b7ee2 --- /dev/null +++ b/communications/ff_msgs/msg/CombinedNavState.msg @@ -0,0 +1,22 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +Header header +geometry_msgs/PoseWithCovarianceStamped pose +VelocityWithCovariance velocity +ImuBiasWithCovariance imu_bias +PoseCovarianceStamped[] correlation_covariances diff --git a/communications/ff_msgs/msg/CombinedNavStateArray.msg b/communications/ff_msgs/msg/CombinedNavStateArray.msg new file mode 100644 index 0000000000..cfa5ceb98a --- /dev/null +++ b/communications/ff_msgs/msg/CombinedNavStateArray.msg @@ -0,0 +1,19 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +Header header +CombinedNavState[] combined_nav_states diff --git a/communications/ff_msgs/msg/CommandConstants.msg b/communications/ff_msgs/msg/CommandConstants.msg index e34415f832..89978a5e34 100644 --- a/communications/ff_msgs/msg/CommandConstants.msg +++ b/communications/ff_msgs/msg/CommandConstants.msg @@ -11,8 +11,6 @@ string PARAM_NAME_LOCALIZATION_MODE_TRUTH = Truth string PARAM_NAME_ACTION_TYPE_PAN = Pan string PARAM_NAME_ACTION_TYPE_TILT = Tilt string PARAM_NAME_ACTION_TYPE_BOTH = Both -string PARAM_NAME_DOWNLOAD_METHOD_IMMEDIATE = Immediate -string PARAM_NAME_DOWNLOAD_METHOD_DELAYED = Delayed string PARAM_NAME_POWERED_COMPONENT_LASER_POINTER = Laser Pointer string PARAM_NAME_POWERED_COMPONENT_PAYLOAD_TOP_AFT = Payload Top Aft string PARAM_NAME_POWERED_COMPONENT_PAYLOAD_BOTTOM_AFT = Payload Bottom Aft @@ -37,11 +35,11 @@ string PARAM_NAME_CAMERA_RESOLUTION_1280X960 = 1280x960 string PARAM_NAME_CAMERA_RESOLUTION_1920X1080 = 1920x1080 string PARAM_NAME_FLASHLIGHT_LOCATION_BACK = Back string PARAM_NAME_FLASHLIGHT_LOCATION_FRONT = Front -string PARAM_NAME_FLIGHT_MODE_NOMINAL = Nominal -string PARAM_NAME_FLIGHT_MODE_DIFFICULT = Difficult -string PARAM_NAME_FLIGHT_MODE_QUIET = Quiet -string PARAM_NAME_FLIGHT_MODE_DOCKING = Docking -string PARAM_NAME_FLIGHT_MODE_PERCHING = Perching +string PARAM_NAME_FLIGHT_MODE_OFF = off +string PARAM_NAME_FLIGHT_MODE_QUIET = quiet +string PARAM_NAME_FLIGHT_MODE_NOMINAL = nominal +string PARAM_NAME_FLIGHT_MODE_DIFFICULT = difficult +string PARAM_NAME_FLIGHT_MODE_PRECISION = precision string PARAM_NAME_PLANNER_TYPE_TRAPEZOIDAL = trapezoidal string PARAM_NAME_PLANNER_TYPE_QUADRATIC_PROGRAM = qp string PARAM_NAME_TELEMETRY_TYPE_COMM_STATUS = CommStatus @@ -51,6 +49,7 @@ string PARAM_NAME_TELEMETRY_TYPE_EKF_STATE = EkfState string PARAM_NAME_TELEMETRY_TYPE_GNC_STATE = GncState string PARAM_NAME_TELEMETRY_TYPE_PMC_CMD_STATE = PmcCmdState string PARAM_NAME_TELEMETRY_TYPE_POSITION = Position +string PARAM_NAME_TELEMETRY_TYPE_SPARSE_MAPPING_POSE = SparseMappingPose string CMD_NAME_GRAB_CONTROL = grabControl string CMD_NAME_REQUEST_CONTROL = requestControl @@ -60,24 +59,21 @@ string CMD_NAME_LOAD_NODELET = loadNodelet string CMD_NAME_NO_OP = noOp string CMD_NAME_REACQUIRE_POSITION = reacquirePosition string CMD_NAME_RESET_EKF = resetEkf -string CMD_NAME_SHUTDOWN = shutdown string CMD_NAME_SWITCH_LOCALIZATION = switchLocalization string CMD_NAME_UNLOAD_NODELET = unloadNodelet string CMD_NAME_UNTERMINATE = unterminate string CMD_NAME_WAKE = wake string CMD_NAME_WAKE_SAFE = wakeSafe -string CMD_NAME_WIPE_HLP = wipeHlp string CMD_NAME_ARM_PAN_AND_TILT = armPanAndTilt +string CMD_NAME_DEPLOY_ARM = deployArm string CMD_NAME_GRIPPER_CONTROL = gripperControl string CMD_NAME_STOP_ARM = stopArm string CMD_NAME_STOW_ARM = stowArm -string CMD_NAME_CLEAR_DATA = clearData -string CMD_NAME_DOWNLOAD_DATA = downloadData string CMD_NAME_SET_DATA_TO_DISK = setDataToDisk string CMD_NAME_START_RECORDING = startRecording -string CMD_NAME_STOP_DOWNLOAD = stopDownload string CMD_NAME_STOP_RECORDING = stopRecording string CMD_NAME_CUSTOM_GUEST_SCIENCE = customGuestScience +string CMD_NAME_RESTART_GUEST_SCIENCE = restartGuestScience string CMD_NAME_START_GUEST_SCIENCE = startGuestScience string CMD_NAME_STOP_GUEST_SCIENCE = stopGuestScience string CMD_NAME_AUTO_RETURN = autoReturn @@ -96,7 +92,7 @@ string CMD_NAME_SKIP_PLAN_STEP = skipPlanStep string CMD_NAME_WAIT = wait string CMD_NAME_POWER_OFF_ITEM = powerOffItem string CMD_NAME_POWER_ON_ITEM = powerOnItem -string CMD_NAME_GENERIC_COMMAND = genericCommand +string CMD_NAME_ENABLE_ASTROBEE_INTERCOMMS = enableAstrobeeIntercomms string CMD_NAME_SET_CAMERA = setCamera string CMD_NAME_SET_CAMERA_RECORDING = setCameraRecording string CMD_NAME_SET_CAMERA_STREAMING = setCameraStreaming @@ -104,13 +100,15 @@ string CMD_NAME_SET_CHECK_OBSTACLES = setCheckObstacles string CMD_NAME_SET_CHECK_ZONES = setCheckZones string CMD_NAME_SET_ENABLE_AUTO_RETURN = setEnableAutoReturn string CMD_NAME_SET_ENABLE_IMMEDIATE = setEnableImmediate +string CMD_NAME_SET_ENABLE_REPLAN = setEnableReplan +string CMD_NAME_SET_EXPOSURE = setExposure string CMD_NAME_SET_FLASHLIGHT_BRIGHTNESS = setFlashlightBrightness string CMD_NAME_SET_HOLONOMIC_MODE = setHolonomicMode string CMD_NAME_SET_INERTIA = setInertia +string CMD_NAME_SET_MAP = setMap string CMD_NAME_SET_OPERATING_LIMITS = setOperatingLimits string CMD_NAME_SET_PLANNER = setPlanner string CMD_NAME_SET_TELEMETRY_RATE = setTelemetryRate -string CMD_NAME_SET_TIME_SYNC = setTimeSync string CMD_NAME_SET_ZONES = setZones string CMD_SUBSYS_ACCESS_CONTROL = AccessControl diff --git a/communications/ff_msgs/msg/CpuNodeState.msg b/communications/ff_msgs/msg/CpuNodeState.msg new file mode 100644 index 0000000000..f9e9ad345c --- /dev/null +++ b/communications/ff_msgs/msg/CpuNodeState.msg @@ -0,0 +1,24 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# State of a CPU load for a node. + +# Node name +string name + +# The load (in percentages) of the cpu +float32 load diff --git a/communications/ff_msgs/msg/CpuStateStamped.msg b/communications/ff_msgs/msg/CpuStateStamped.msg index 4df093f118..2a6e2402f5 100644 --- a/communications/ff_msgs/msg/CpuStateStamped.msg +++ b/communications/ff_msgs/msg/CpuStateStamped.msg @@ -45,3 +45,5 @@ float32 temp # or not all of them are enabled. ff_msgs/CpuState[] cpus +# Load usage of individual ROS nodes +ff_msgs/CpuNodeState[] load_nodes \ No newline at end of file diff --git a/communications/ff_msgs/msg/DepthCorrespondence.msg b/communications/ff_msgs/msg/DepthCorrespondence.msg new file mode 100644 index 0000000000..2ef1b765ec --- /dev/null +++ b/communications/ff_msgs/msg/DepthCorrespondence.msg @@ -0,0 +1,21 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +ImagePoint source_image_point +ImagePoint target_image_point +geometry_msgs/Point source_3d_point +geometry_msgs/Point target_3d_point diff --git a/communications/ff_msgs/msg/DepthLandmarks.msg b/communications/ff_msgs/msg/DepthLandmarks.msg index 06e8c74933..7db1b67ded 100644 --- a/communications/ff_msgs/msg/DepthLandmarks.msg +++ b/communications/ff_msgs/msg/DepthLandmarks.msg @@ -17,9 +17,12 @@ # # An observation of a handrail from a depth image. -Header header # Image header, with time stamp -uint32 camera_id # Image ID, associated with registration -uint8 end_seen # Whether the handrail endpoint was detected -uint8 update_global_pose # Whether to update the global pose -geometry_msgs/Pose local_pose # Handrail center in the camera frame -ff_msgs/DepthLandmark[] landmarks # List of landmarks seen +Header header # Image header, with time stamp +uint32 camera_id # Image ID, associated with registration +uint8 end_seen # Whether the handrail endpoint was detected +uint8 update_global_pose # Whether to update the global pose +geometry_msgs/Pose sensor_T_handrail # Handrail center in the sensor frame +geometry_msgs/Point32[] sensor_t_line_points # Detected line points +geometry_msgs/Point[] sensor_t_line_endpoints # Detected line endpoints +geometry_msgs/Point32[] sensor_t_plane_points # Detected plane points +ff_msgs/DepthLandmark[] landmarks # List of landmarks seen TODO(rsoussan): This should be removed diff --git a/communications/ff_msgs/msg/DepthOdometry.msg b/communications/ff_msgs/msg/DepthOdometry.msg new file mode 100644 index 0000000000..d6461babfa --- /dev/null +++ b/communications/ff_msgs/msg/DepthOdometry.msg @@ -0,0 +1,24 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# + +Header header +ff_msgs/Odometry odometry +ff_msgs/DepthCorrespondence[] correspondences +bool valid_image_points +bool valid_points_3d +float32 runtime diff --git a/communications/ff_msgs/msg/FaultState.msg b/communications/ff_msgs/msg/FaultState.msg index 53d37dc603..bce0ddd799 100644 --- a/communications/ff_msgs/msg/FaultState.msg +++ b/communications/ff_msgs/msg/FaultState.msg @@ -21,17 +21,24 @@ std_msgs/Header header +# Not sent to the ground, only used by the executive to determine what commands +# to accept. +uint8 state +# System starting up +uint8 STARTING_UP = 0 # No faults are occurring in system -uint8 FUNCTIONAL = 0 +uint8 FUNCTIONAL = 1 # Faults are occurring in the system which may or may not leave the robot # functional -uint8 FAULT = 1 +uint8 FAULT = 2 # A fault has occurred that indirectly affects the motion of the robot -uint8 BLOCKED = 2 +uint8 BLOCKED = 3 +# Recovering from nodes dying on startup +uint8 RELOADING_NODELETS = 4 -# Not sent to the ground, only used by the executive to determine what commands -# to accept. -uint8 state +# A human readable version of the state - only really used for when nodes die on +# startup and need to be restarted. +string hr_state # Faults occurring in the astrobee system, can only send 32 faults down ff_msgs/Fault[] faults diff --git a/communications/ff_msgs/msg/FlightMode.msg b/communications/ff_msgs/msg/FlightMode.msg index 44dd6d466c..77f9d84091 100644 --- a/communications/ff_msgs/msg/FlightMode.msg +++ b/communications/ff_msgs/msg/FlightMode.msg @@ -23,9 +23,8 @@ string name # Name of the flight mode bool control_enabled # Is control enabled? -float32 collision_radius # Collision radius in meters - # Tolerances (all in SI units) +float32 tolerance_pos_endpoint # Endpoint position tolerance in m float32 tolerance_pos # Position tolerance in m float32 tolerance_vel # Velocity tolerance in m/s float32 tolerance_att # Attitude tolerance in rads diff --git a/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg b/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg new file mode 100644 index 0000000000..f0d49d208b --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg @@ -0,0 +1,17 @@ +# timestamp in header should be time of whatever triggered the advertisement +std_msgs/Header header + +#topic on which to republish +string output_topic + +#whether should be republished as a latching topic +bool latching + +#standard ROS meanings for message descriptions +# eg std_msgs/Int32 +string data_type +# eg da5909fbe378aeaf85e547e830cc1bb7 +string md5_sum +# eg a definition such as this file +string definition + diff --git a/communications/ff_msgs/msg/GenericCommsContent.msg b/communications/ff_msgs/msg/GenericCommsContent.msg new file mode 100644 index 0000000000..c768922328 --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsContent.msg @@ -0,0 +1,13 @@ +# timestamp in header should be time of receipt of original message +std_msgs/Header header + +#topic on which to republish +string output_topic + +#md5sum of msg type, repeated from advertisement +string type_md5_sum + +#serialized content of the message +#definition, etc. assumed to be known from preceding advertisement +uint8[] msg + diff --git a/communications/ff_msgs/msg/GenericCommsReset.msg b/communications/ff_msgs/msg/GenericCommsReset.msg new file mode 100644 index 0000000000..397561bffd --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsReset.msg @@ -0,0 +1,7 @@ +# timestamp in header should be time of whatever triggered the reset +std_msgs/Header header + +#topic for which metadata retransmission is requested +string topic + + diff --git a/communications/ff_msgs/msg/GraphLocState.msg b/communications/ff_msgs/msg/GraphLocState.msg new file mode 100644 index 0000000000..9f70ce4356 --- /dev/null +++ b/communications/ff_msgs/msg/GraphLocState.msg @@ -0,0 +1,32 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +std_msgs/Header header # header with timestamp +string child_frame_id # frame ID +# Latest Pose Estimate +geometry_msgs/PoseWithCovariance pose # world_T_body +# Stats +uint32 num_detected_ar_features +uint32 num_detected_ml_features +uint32 optimization_iterations +float32 optimization_time +float32 update_time # Include optimization_time and other operations to add data to graph +float32 duration +uint32 num_factors +uint32 num_ml_projection_factors +uint32 num_ml_pose_factors +uint32 num_states diff --git a/communications/ff_msgs/msg/GraphState.msg b/communications/ff_msgs/msg/GraphState.msg new file mode 100644 index 0000000000..623a808647 --- /dev/null +++ b/communications/ff_msgs/msg/GraphState.msg @@ -0,0 +1,52 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# + +# Deprecated, kept for backwards compatability +std_msgs/Header header # header with timestamp +string child_frame_id # frame ID +# State Estimates +geometry_msgs/Pose pose # world_T_body +geometry_msgs/Vector3 velocity # body velocity +geometry_msgs/Vector3 gyro_bias # estimated gyro bias +geometry_msgs/Vector3 accel_bias # estimated accel bias +# Covariances/Confidences +# covariance diagonal. 1-3 orientation, 4-6 gyro bias, 7-9 velocity, 10-12 accel bias, 13-15 position +float32[15] cov_diag +# confidence in estimate. 0 is good, 1 is a bit confused, 2 is lost +uint8 confidence +uint8 CONFIDENCE_GOOD = 0 # Tracking well +uint8 CONFIDENCE_POOR = 1 # Tracking poorly +uint8 CONFIDENCE_LOST = 2 # We are lost +# Stats +uint32 num_detected_of_features +uint32 num_detected_ar_features +uint32 num_detected_ml_features +uint32 iterations # Optimization iterations +float32 optimization_time +float32 update_time # Include optimization_time and other operations to add data to graph +float32 callbacks_time # Includes processing msgs and their callbacks +float32 nodelet_runtime # Total runtime of nodelet iteration. Includes update and callback time +uint32 num_factors +uint32 num_of_factors +uint32 num_ml_projection_factors +uint32 num_ml_pose_factors +uint32 num_states +# Status +bool standstill +bool estimating_bias # Are we busy estimating the bias? +uint8 fan_speed_mode # Used for imu filtering diff --git a/communications/ff_msgs/msg/GraphVIOState.msg b/communications/ff_msgs/msg/GraphVIOState.msg new file mode 100644 index 0000000000..c0c58f7f0a --- /dev/null +++ b/communications/ff_msgs/msg/GraphVIOState.msg @@ -0,0 +1,35 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +std_msgs/Header header # header with timestamp +string child_frame_id # frame ID +# CombinedNavState estimates currently in the graph +CombinedNavStateArray combined_nav_states +# Stats +uint32 num_detected_of_features +uint32 optimization_iterations +float32 optimization_time +float32 update_time # Include optimization_time and other operations to add data to graph +float32 duration +uint32 num_factors +uint32 num_of_factors +uint32 num_depth_factors +uint32 num_states +# Status +bool standstill +# TODO(rsoussan): Fill this in! Estimating bias expected for gnc ekf message! +bool estimating_bias # Are we busy estimating the bias? diff --git a/communications/ff_msgs/msg/ImagePoint.msg b/communications/ff_msgs/msg/ImagePoint.msg new file mode 100644 index 0000000000..7dddac685a --- /dev/null +++ b/communications/ff_msgs/msg/ImagePoint.msg @@ -0,0 +1,20 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# + +float32 x +float32 y diff --git a/communications/ff_msgs/msg/ImuBiasWithCovariance.msg b/communications/ff_msgs/msg/ImuBiasWithCovariance.msg new file mode 100644 index 0000000000..cb46d9b26b --- /dev/null +++ b/communications/ff_msgs/msg/ImuBiasWithCovariance.msg @@ -0,0 +1,25 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# + +geometry_msgs/Vector3 accelerometer_bias +geometry_msgs/Vector3 gyroscope_bias +# Ordered as accelerometer then gyroscope bias covarainces +# Top left 3x3 block is the accelerometer bias covariances, +# bottom right 3x3 block is the gyroscope bias covariances, +# and other blocks are accelerometer/gyroscope covariances. +float64[36] covariance diff --git a/communications/ff_msgs/msg/MemState.msg b/communications/ff_msgs/msg/MemState.msg new file mode 100644 index 0000000000..1b576d7c66 --- /dev/null +++ b/communications/ff_msgs/msg/MemState.msg @@ -0,0 +1,30 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# State of the Memory. + +# The memory load of the node, for the fields given in +string name +# Virtual Memory +uint32 virt # virtual memeory used in Mb +uint32 virt_peak # peak virtual memory used in Mb + +# Physical Memory +uint32 ram # physical memory used in Mb +uint32 ram_peak # peak physical memory used in Mb +float32 ram_perc # percentage of physical memory in % + diff --git a/communications/ff_msgs/msg/MemStateStamped.msg b/communications/ff_msgs/msg/MemStateStamped.msg new file mode 100644 index 0000000000..1046a40bed --- /dev/null +++ b/communications/ff_msgs/msg/MemStateStamped.msg @@ -0,0 +1,37 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Memory state message with timestamp. + +# Header with timestamp +std_msgs/Header header + +# Machine name (llp, hlp, mlp, etc) +string name + +# Physical Memory (RAM) +uint32 ram_total # total physical memeory in system Mb +uint32 ram_used # totalphysical memeory used in Mb + +# Virtual Memory +uint32 virt_total # total virtual memeory in system in Mb +uint32 virt_used # total virtual memeory used in Mb + +# Individual nodes +ff_msgs/MemState[] nodes + + diff --git a/communications/ff_msgs/msg/MotionState.msg b/communications/ff_msgs/msg/MotionState.msg index c47d9e7db2..e12a06c863 100644 --- a/communications/ff_msgs/msg/MotionState.msg +++ b/communications/ff_msgs/msg/MotionState.msg @@ -34,6 +34,7 @@ int8 PREPARING = 8 int8 CONTROLLING = 9 int8 REPLANNING = 10 int8 HALTING = 11 +int8 REPLAN_WAIT = 12 # A human readble version of the (event) -> [state] transition string fsm_event diff --git a/communications/ff_msgs/msg/Odometry.msg b/communications/ff_msgs/msg/Odometry.msg new file mode 100644 index 0000000000..c8d7c86fb0 --- /dev/null +++ b/communications/ff_msgs/msg/Odometry.msg @@ -0,0 +1,22 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# + +time source_time +time target_time +geometry_msgs/PoseWithCovariance sensor_F_source_T_target +geometry_msgs/PoseWithCovariance body_F_source_T_target diff --git a/communications/ff_msgs/msg/PerchState.msg b/communications/ff_msgs/msg/PerchState.msg index 85ee59d28c..bb939120ae 100644 --- a/communications/ff_msgs/msg/PerchState.msg +++ b/communications/ff_msgs/msg/PerchState.msg @@ -30,6 +30,7 @@ int8 RECOVERY_MOVING_TO_APPROACH_POSE = 15 int8 RECOVERY_OPENING_GRIPPER = 14 int8 INITIALIZING = 13 int8 UNKNOWN = 12 +# Used to check the perching/unperching ranges int8 PERCHING_MAX_STATE = 11 int8 PERCHING_SWITCHING_TO_HR_LOC = 11 int8 PERCHING_MOVING_TO_APPROACH_POSE = 10 @@ -50,6 +51,7 @@ int8 UNPERCHING_MOVING_TO_APPROACH_POSE = -4 int8 UNPERCHING_STOWING_ARM = -5 int8 UNPERCHING_SWITCHING_TO_ML_LOC = -6 int8 UNPERCHED = -7 +# Used to check the perching/unperching ranges int8 UNPERCHING_MAX_STATE = -7 # A human readable version of the (event) -> [state] transition diff --git a/communications/ff_msgs/msg/PoseCovarianceStamped.msg b/communications/ff_msgs/msg/PoseCovarianceStamped.msg new file mode 100644 index 0000000000..88272170c4 --- /dev/null +++ b/communications/ff_msgs/msg/PoseCovarianceStamped.msg @@ -0,0 +1,20 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# + +float64[36] covariance +time time diff --git a/communications/ff_msgs/msg/SerializedGraph.msg b/communications/ff_msgs/msg/SerializedGraph.msg new file mode 100644 index 0000000000..c40cb04674 --- /dev/null +++ b/communications/ff_msgs/msg/SerializedGraph.msg @@ -0,0 +1,21 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +std_msgs/Header header # header with timestamp +string child_frame_id # frame ID + +string serialized_graph diff --git a/communications/ff_msgs/msg/TimeSyncStamped.msg b/communications/ff_msgs/msg/TimeSyncStamped.msg new file mode 100644 index 0000000000..39a6a6592f --- /dev/null +++ b/communications/ff_msgs/msg/TimeSyncStamped.msg @@ -0,0 +1,30 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Message to send time difference between the mlp and llp + +# Header with timestamp +std_msgs/Header header + +# Processor that the heartbeat came from +string remote_processor + +# Current time on the mlp +time mlp_time + +# Time in the incoming heartbeat +time remote_time diff --git a/communications/ff_msgs/msg/VelocityWithCovariance.msg b/communications/ff_msgs/msg/VelocityWithCovariance.msg new file mode 100644 index 0000000000..23b8a5e6be --- /dev/null +++ b/communications/ff_msgs/msg/VelocityWithCovariance.msg @@ -0,0 +1,20 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# + +geometry_msgs/Vector3 velocity +float64[9] covariance diff --git a/communications/ff_msgs/msg/VisualLandmarks.msg b/communications/ff_msgs/msg/VisualLandmarks.msg index bc385658f4..e5d5eeef00 100644 --- a/communications/ff_msgs/msg/VisualLandmarks.msg +++ b/communications/ff_msgs/msg/VisualLandmarks.msg @@ -22,3 +22,4 @@ Header header # header with timestamp uint32 camera_id # image ID, associated with registration pulse geometry_msgs/Pose pose # estimated camera pose from features ff_msgs/VisualLandmark[] landmarks # list of all landmarks +float32 runtime # Time it took to calculate the pose and matching landmarks diff --git a/communications/ff_msgs/msg/readme.md b/communications/ff_msgs/msg/readme.md new file mode 100644 index 0000000000..ce2caa4b7f --- /dev/null +++ b/communications/ff_msgs/msg/readme.md @@ -0,0 +1,8 @@ + +\page ff_msgs Free Flyer ROS Messages + +Within this folder, the following messages are no longer published, but +we keep them for backward compatibility with archived Astrobee ISS +telemetry bags: + +- TimeDiffStamped.msg diff --git a/communications/ff_msgs/srv/GetFloat.srv b/communications/ff_msgs/srv/GetFloat.srv new file mode 100644 index 0000000000..7da14c297f --- /dev/null +++ b/communications/ff_msgs/srv/GetFloat.srv @@ -0,0 +1,22 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message sends some float64 data, which can a server parameter. + +--- +float64 data # float data +bool success # whether succeeded diff --git a/communications/ff_msgs/srv/GetOccupancyMap.srv b/communications/ff_msgs/srv/GetOccupancyMap.srv new file mode 100644 index 0000000000..db5c23721b --- /dev/null +++ b/communications/ff_msgs/srv/GetOccupancyMap.srv @@ -0,0 +1,26 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message allows the MOBILITY::MAPPER zones to be retrieved + +--- +time timestamp # When these zones were updated + +int8[] map # Occupancy map +geometry_msgs/Vector3 origin # Map origin +geometry_msgs/Vector3 dim # Map dimentions +float32 resolution # Map resolution diff --git a/communications/ff_msgs/srv/ResetMap.srv b/communications/ff_msgs/srv/ResetMap.srv new file mode 100644 index 0000000000..c43380f079 --- /dev/null +++ b/communications/ff_msgs/srv/ResetMap.srv @@ -0,0 +1,23 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# This message allows path planner implementations to register their +# capabilities with a planner node. + +string map_file # Full path to map file to use. 'default' uses the default map loaded on startup +--- +bool success # Did the update work? diff --git a/communications/ff_msgs/srv/ResponseOnly.srv b/communications/ff_msgs/srv/ResponseOnly.srv new file mode 100644 index 0000000000..4d55410a4b --- /dev/null +++ b/communications/ff_msgs/srv/ResponseOnly.srv @@ -0,0 +1,20 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +--- +bool success # Did the service work? +string status # If not, why did it fail? diff --git a/communications/ff_msgs/srv/SetExposure.srv b/communications/ff_msgs/srv/SetExposure.srv new file mode 100644 index 0000000000..301f3f94b2 --- /dev/null +++ b/communications/ff_msgs/srv/SetExposure.srv @@ -0,0 +1,23 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. # +# +# Service for setting camera exposure + +uint16 exposure +--- +bool success +string status_message diff --git a/communications/ff_msgs/srv/SetRate.srv b/communications/ff_msgs/srv/SetRate.srv index 6f21a95466..39065839da 100644 --- a/communications/ff_msgs/srv/SetRate.srv +++ b/communications/ff_msgs/srv/SetRate.srv @@ -17,13 +17,14 @@ # # This message sets the rates at which certain messages are published. -uint8 COMM_STATUS = 0 # Set the communication status message rate -uint8 CPU_STATE = 1 # Set the cpu state message rate -uint8 DISK_STATE = 2 # Set the disk state message rate -uint8 EKF_STATE = 3 # Set the ekf state message rate -uint8 GNC_STATE = 4 # Set the gnc state message rate -uint8 PMC_CMD_STATE = 5 # Set the pmc command state message rate -uint8 POSITION = 6 # Set the position message rate +uint8 COMM_STATUS = 0 # Set the communication status message rate +uint8 CPU_STATE = 1 # Set the cpu state message rate +uint8 DISK_STATE = 2 # Set the disk state message rate +uint8 EKF_STATE = 3 # Set the ekf state message rate +uint8 GNC_STATE = 4 # Set the gnc state message rate +uint8 PMC_CMD_STATE = 5 # Set the pmc command state message rate +uint8 POSITION = 6 # Set the position message rate +uint8 SPARSE_MAPPING_POSE = 7 # Set the sparse mapping pose message rate uint8 which # which message we are setting the rate for float32 rate # the rate diff --git a/communications/ff_msgs/srv/UnloadLoadNodelet.srv b/communications/ff_msgs/srv/UnloadLoadNodelet.srv index 6f0d1659b0..dab40ae2e0 100644 --- a/communications/ff_msgs/srv/UnloadLoadNodelet.srv +++ b/communications/ff_msgs/srv/UnloadLoadNodelet.srv @@ -12,8 +12,8 @@ string name # Name of nodelet. Needed for both unload and load. # if the type is specified in the system monitor config. string type -# Name of nodelet manager. Needed for load. May not be needed if the system -# monitor had received a heartbeat before being unloaded. +# Name of nodelet manager. Needed for both load and unload. May not be needed if +# the system monitor received a heartbeat before being unloaded. string manager_name string[] remap_source_args # Leave blank if no source args. Needed for load. diff --git a/communications/ground_dds_ros_bridge/CMakeLists.txt b/communications/ground_dds_ros_bridge/CMakeLists.txt new file mode 100644 index 0000000000..28379e056d --- /dev/null +++ b/communications/ground_dds_ros_bridge/CMakeLists.txt @@ -0,0 +1,115 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +cmake_minimum_required(VERSION 3.0) +project(ground_dds_ros_bridge) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +if (USE_DDS) +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + nodelet + config_reader + ff_util + ff_msgs + ff_hw_msgs + dds_msgs +) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) +find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams +thread program_options timer) + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") + +if (USE_CTC) + set(SORACORE_ROOT_DIR ${ARM_CHROOT_DIR}/usr) +else (USE_CTC) + set(SORACORE_ROOT_DIR /usr) +endif (USE_CTC) + +set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) + +SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# find Qt version according to OS +find_program(LSB_RELEASE_EXEC lsb_release) +execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) +else () + find_package(Qt4 4.6.0 REQUIRED QtXml) +endif () + +find_package(Miro REQUIRED) +find_package(RtiDds REQUIRED) +find_package(Soracore REQUIRED) + +catkin_package( + LIBRARIES ground_dds_ros_bridge + CATKIN_DEPENDS roscpp message_runtime std_msgs ff_msgs dds_msgs +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${RTIDDS_INCLUDE_DIR} + ${SORACORE_INCLUDE_DIRS} + ${MIRO_INCLUDE_DIR} + ${QT_INCLUDE_DIR} + ${QT_INCLUDE_DIR}/Qt + ${Boost_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(ground_dds_ros_bridge + ${cc_files} +) +target_compile_definitions(ground_dds_ros_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) +add_dependencies(ground_dds_ros_bridge ${catkin_EXPORTED_TARGETS}) +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + target_link_libraries(ground_dds_ros_bridge rapidIo Qt5::Xml ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +else () + target_link_libraries(ground_dds_ros_bridge rapidIo ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +endif () + +# Declare a C++ executable +add_executable(ground_dds_ros_bridge_node + tools/ground_dds_ros_bridge_node.cc +) +add_dependencies(ground_dds_ros_bridge_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(ground_dds_ros_bridge_node ground_dds_ros_bridge ${catkin_LIBRARIES}) + +else (USE_DDS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_DDS) diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ground_dds_ros_bridge.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ground_dds_ros_bridge.h new file mode 100644 index 0000000000..8212082dfe --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ground_dds_ros_bridge.h @@ -0,0 +1,111 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GROUND_DDS_ROS_BRIDGE_GROUND_DDS_ROS_BRIDGE_H_ +#define GROUND_DDS_ROS_BRIDGE_GROUND_DDS_ROS_BRIDGE_H_ + +#include + +#include +#include +#include +#include + +#include "config_reader/config_reader.h" + +#include "ground_dds_ros_bridge/rapid_access_control_state.h" +#include "ground_dds_ros_bridge/rapid_guest_science_data.h" +#include "ground_dds_ros_bridge/rapid_image.h" +#include "ground_dds_ros_bridge/rapid_position.h" +#include "ground_dds_ros_bridge/rapid_sub_ros_pub.h" +#include "ground_dds_ros_bridge/ros_command.h" +#include "ground_dds_ros_bridge/ros_sub_rapid_pub.h" + +#include "ff_common/ff_names.h" + +// SoraCore Includes +#include "knDds/DdsSupport.h" +#include "knDds/DdsEntitiesFactory.h" +#include "knDds/DdsEntitiesFactorySvc.h" + +// miro includes +#include "miro/Configuration.h" +#include "miro/Robot.h" +#include "miro/Log.h" + +namespace kn { + class DdsEntitiesFactorySvc; +} // end namespace kn + +namespace ground_dds_ros_bridge { + +class RosSubRapidPub; + +class GroundDdsRosBridge { + public: + GroundDdsRosBridge(); + ~GroundDdsRosBridge(); + + /** + * Build Rapid subscribers to Ros publishers + * @param pubTopic topic to publish to + * + * @return successful + */ + bool BuildAccessControlStateToRos(const std::string& pub_topic, + const std::string& name); + bool BuildGuestScienceDataToRos(const std::string& pub_topic, + const std::string& name); + bool BuildPositionToOdom(const std::string& pub_topic, + const std::string& name); + bool BuildSensorImageToRos(const std::string& pub_topic, + const std::string& name); + + /** + * Build Ros subscribers to Rapid publishers + * @param subTopic topic to subscribe to ex: "/camera/image" + * @return successful + */ + bool BuildCommandToRapid(const std::string& sub_topic, + const std::string& name); + + bool Initialize(ros::NodeHandle *nh); + + protected: + bool ReadTopicInfo(const std::string& topic_abbr, + const std::string& sub_or_pub, + std::string& sub_topic, + bool& use); + bool ReadParams(); + + private: + config_reader::ConfigReader config_params_; + + int components_, domain_id_; + + ros::NodeHandle nh_; + + std::vector rapid_sub_ros_pubs_; + std::vector ros_sub_rapid_pubs_; + std::shared_ptr dds_entities_factory_; + std::string participant_name_, agent_name_, connecting_robot_; +}; + +} // end namespace ground_dds_ros_bridge + +#endif // GROUND_DDS_ROS_BRIDGE_GROUND_DDS_ROS_BRIDGE_H_ diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_access_control_state.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_access_control_state.h new file mode 100644 index 0000000000..47d0b2a65b --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_access_control_state.h @@ -0,0 +1,47 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GROUND_DDS_ROS_BRIDGE_RAPID_ACCESS_CONTROL_STATE_H_ +#define GROUND_DDS_ROS_BRIDGE_RAPID_ACCESS_CONTROL_STATE_H_ + +#include + +#include "ground_dds_ros_bridge/rapid_sub_ros_pub.h" +#include "ground_dds_ros_bridge/util.h" + +#include "ff_msgs/AccessControlStateStamped.h" + +#include "rapidDds/AccessControlStateSupport.h" +#include "rapidDds/RapidConstants.h" + +namespace ff { + +class RapidAccessControlStateToRos : public RapidSubRosPub { + public: + RapidAccessControlStateToRos(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + + // Callback for ddsEventLoop + void operator() (rapid::AccessControlState const* access_control_state); +}; + +} // end namespace ff + +#endif // GROUND_DDS_ROS_BRIDGE_RAPID_ACCESS_CONTROL_STATE_H_ diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_guest_science_data.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_guest_science_data.h new file mode 100644 index 0000000000..ff41d9e6fb --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_guest_science_data.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific launguage governing permissions and limitations + * under the License. + */ + +#ifndef GROUND_DDS_ROS_BRIDGE_RAPID_GUEST_SCIENCE_DATA_H_ +#define GROUND_DDS_ROS_BRIDGE_RAPID_GUEST_SCIENCE_DATA_H_ + +#include +#include + +#include "ground_dds_ros_bridge/rapid_sub_ros_pub.h" +#include "ground_dds_ros_bridge/util.h" + +#include "ff_msgs/GuestScienceData.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GuestScienceDataSupport.h" + +namespace ff { + +class RapidGuestScienceDataToRos : public RapidSubRosPub { + public: + RapidGuestScienceDataToRos(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + + // Callback for ddsEventLoop + void operator() (rapid::ext::astrobee::GuestScienceData const* rapid_gs_data); +}; + +} // end namespace ff + +#endif // GROUND_DDS_ROS_BRIDGE_RAPID_GUEST_SCIENCE_DATA_H_ diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_image.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_image.h new file mode 100644 index 0000000000..1287f120b9 --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_image.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GROUND_DDS_ROS_BRIDGE_RAPID_IMAGE_H_ +#define GROUND_DDS_ROS_BRIDGE_RAPID_IMAGE_H_ + +#include +#include +#include + +#include "ground_dds_ros_bridge/rapid_sub_ros_pub.h" +#include "ground_dds_ros_bridge/util.h" + +#include "rapidDds/MIMETypesConstants.h" +#include "rapidDds/ImageSensorSample.h" +#include "rapidDds/ImageSensorSampleSupport.h" +#include "rapidDds/RapidConstants.h" + +#include "rapidIo/RapidIoParameters.h" + +#include "sensor_msgs/CompressedImage.h" + +namespace ff { + +class RapidImageToRos : public RapidSubRosPub { + public: + RapidImageToRos(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + + // Callback for ddsEventLoop + void operator() (rapid::ImageSensorSample const* rapid_img); +}; + +} // end namespace ff + +#endif // GROUND_DDS_ROS_BRIDGE_RAPID_IMAGE_H_ diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_position.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_position.h new file mode 100644 index 0000000000..b889038e21 --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_position.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GROUND_DDS_ROS_BRIDGE_RAPID_POSITION_H_ +#define GROUND_DDS_ROS_BRIDGE_RAPID_POSITION_H_ + +#include +#include + +#include "ground_dds_ros_bridge/rapid_sub_ros_pub.h" +#include "ground_dds_ros_bridge/util.h" + +#include "ff_msgs/EkfState.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/EkfStateSupport.h" + +namespace ff { + +class RapidPositionToRos : public RapidSubRosPub { + public: + RapidPositionToRos(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + + void CopyTransform3D(geometry_msgs::Pose& pose, + const rapid::Transform3D& transform); + + void CopyVec3D(geometry_msgs::Vector3& vec_out, + const rapid::Vec3d& vec_in); + + // Callback for ddsEventLoop + void operator() (rapid::ext::astrobee::EkfState const* rapid_ekf_state); +}; + +} // end namespace ff + +#endif // GROUND_DDS_ROS_BRIDGE_RAPID_POSITION_H_ diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_sub_ros_pub.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_sub_ros_pub.h new file mode 100644 index 0000000000..37717ae2e2 --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/rapid_sub_ros_pub.h @@ -0,0 +1,78 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GROUND_DDS_ROS_BRIDGE_RAPID_SUB_ROS_PUB_H_ +#define GROUND_DDS_ROS_BRIDGE_RAPID_SUB_ROS_PUB_H_ + +#include +#include +#include +#include + +#include "ros/ros.h" + +#include "knDds/DdsEventLoop.h" + +#include "knShare/Time.h" + +namespace ff { + +/** + * @brief base class for rapid subscriber to ros publisher + * @details base class for rapid subscriber to ros publisher. + * A kn::DdsEventLoop is run within its own thread of execution. + * Child classes must connect requeseted messege and callback + * to m_ddsEventLoop and call startThread() + */ +class RapidSubRosPub { + protected: + RapidSubRosPub(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const std::string& entity_name, + const unsigned int queue_size); + ~RapidSubRosPub(); + + /** + * Will start thread execution by calling threadExec() + */ + virtual void StartThread(); + + ros::NodeHandle nh_; + ros::Publisher pub_; + std::string subscribe_topic_; + std::string publish_topic_; + unsigned int queue_size_; + + std::atomic alive_; + std::thread thread_; + kn::DdsEventLoop dds_event_loop_; + + private: + /** + * Function to execute within seperate thread + * process DdsEventLoop at 10Hz + */ + virtual void ThreadExec(); +}; + +typedef std::shared_ptr RapidSubRosPubPtr; + +} // end namespace ff + +#endif // GROUND_DDS_ROS_BRIDGE_RAPID_SUB_ROS_PUB_H_ diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ros_command.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ros_command.h new file mode 100644 index 0000000000..e6de19ea21 --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ros_command.h @@ -0,0 +1,59 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GROUND_DDS_ROS_BRIDGE_ROS_COMMAND_H_ +#define GROUND_DDS_ROS_BRIDGE_ROS_COMMAND_H_ + +#include +#include +#include + +#include "ground_dds_ros_bridge/ros_sub_rapid_pub.h" +#include "ground_dds_ros_bridge/util.h" + +#include "ff_msgs/CommandArg.h" +#include "ff_msgs/CommandStamped.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "rapidDds/CommandSupport.h" +#include "rapidDds/RapidConstants.h" + +namespace ff { + +class RosCommandToRapid : public RosSubRapidPub { + public: + RosCommandToRapid(const std::string& subscribe_topic, + const std::string& pub_topic, + const std::string& connecting_robot, + const ros::NodeHandle &nh, + const unsigned int queue_size = 10); + + void CmdCallback(ff_msgs::CommandStampedConstPtr const& cmd); + private: + using CommandSupplier = kn::DdsTypedSupplier; + using CommandSupplierPtr = std::unique_ptr; + + CommandSupplierPtr command_supplier_; +}; + +} // end namespace ff + +#endif // GROUND_DDS_ROS_BRIDGE_ROS_COMMAND_H_ diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ros_sub_rapid_pub.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ros_sub_rapid_pub.h new file mode 100644 index 0000000000..5b7e419ae6 --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ros_sub_rapid_pub.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + + +#ifndef GROUND_DDS_ROS_BRIDGE_ROS_SUB_RAPID_PUB_H_ +#define GROUND_DDS_ROS_BRIDGE_ROS_SUB_RAPID_PUB_H_ + +#include +#include + +#include "ros/ros.h" + +namespace ff { + +class RosSubRapidPub { + protected: + RosSubRapidPub(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : nh_(nh), + subscribe_topic_(subscribe_topic), + publish_topic_(pub_topic), + queue_size_(queue_size) { + } + + ros::NodeHandle nh_; + ros::Subscriber sub_; + std::string subscribe_topic_; + std::string publish_topic_; + unsigned int queue_size_; +}; + +typedef std::shared_ptr RosSubRapidPubPtr; + +} // end namespace ff + +#endif // GROUND_DDS_ROS_BRIDGE_ROS_SUB_RAPID_PUB_H_ diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/util.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/util.h new file mode 100644 index 0000000000..933e4b40c7 --- /dev/null +++ b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/util.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GROUND_DDS_ROS_BRIDGE_UTIL_H_ +#define GROUND_DDS_ROS_BRIDGE_UTIL_H_ + +#include + +#include +#include + +#include + +#include + +#include + +#include + +namespace util { + + // Time-related helpers + ::ros::Time RapidTime2RosTime(const int64_t dds_time); + + int64_t RosTime2RapidTime(::ros::Time const& ros_time); + + // Header-related helpers + void RosHeader2Rapid(::std_msgs::Header const& ros_hdr, + ::rapid::Header* rapid_hdr, + int status = 0, + int serial = -1); + + void RapidHeader2Ros(::rapid::Header const& rapid_hdr, + ::std_msgs::Header* ros_hdr, + std::string const& frame_id = "world"); + +} // end namespace util + +#endif // GROUND_DDS_ROS_BRIDGE_UTIL_H_ diff --git a/communications/ground_dds_ros_bridge/launch/ground_dds_ros_bridge.launch b/communications/ground_dds_ros_bridge/launch/ground_dds_ros_bridge.launch new file mode 100644 index 0000000000..f61a20f71d --- /dev/null +++ b/communications/ground_dds_ros_bridge/launch/ground_dds_ros_bridge.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/communications/ground_dds_ros_bridge/package.xml b/communications/ground_dds_ros_bridge/package.xml new file mode 100644 index 0000000000..696673c70e --- /dev/null +++ b/communications/ground_dds_ros_bridge/package.xml @@ -0,0 +1,38 @@ + + + ground_dds_ros_bridge + 0.0.0 + + The ground_dds_ros_bridge package provides a bridge between dds messages and ros messages + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + cmake_modules + message_generation + ff_msgs + ff_util + nav_msgs + roscpp + rossmbus + std_msgs + nodelet + dds_msgs + cmake_modules + message_runtime + ff_msgs + ff_util + nav_msgs + roscpp + rossmbus + std_msgs + nodelet + dds_msgs + diff --git a/communications/ground_dds_ros_bridge/readme.md b/communications/ground_dds_ros_bridge/readme.md new file mode 100644 index 0000000000..41beb47821 --- /dev/null +++ b/communications/ground_dds_ros_bridge/readme.md @@ -0,0 +1,51 @@ +\page ground_dds_ros_bridge Ground DDS ROS Bridge + +The ground DDS ROS bridge acts like a translator between DDS and ROS. It listensto DDS telemetry coming from the robot and converts it to ROS messages. +Currently it only supports converting the access control state, guest science +data, dock camera compressed image, navigation camera compressed image, science +camera compressed image, and the robot position messages. It also converts +messages published on the ros command topic to a dds command and publishes them +on the dds command topic. Please note that as of now, the ground dds ros bridge +can only connect to one robot. + +The ground bridge will need to be configured for your specific situation. Please +see the next section for more information. + +# Configuration + +The ground dds ros bridge config file can be found here: + + $ASTROBEE_WS/src/astrobee/config/communications/ground_dds_ros_bridge.config + +The following sections explain what may need to be changed in the file. + +## Connecting Robot + +The ground bridge needs to know the name of the robot you want to connect to. +The default is Bumble. If you need to change this, please open the config file +and find the connecting_robot variable. Please change it to the name of the +robot you wish to connect to. Make sure to capitalize the first letter of the +name. + +## Domain id + +The domain id for simulation is 37 and this is default value. If you need the +space domain id for use during an activity, please ask your Astrobee point of +contactor. + +## Namespace + +If you are running the ground bridge in simulation, you will probably want to +use the namespace feature. When set to true, the ground bridge will publish +messages received over DDS to ros using the robot name as the namespace. This +will allow you to distigush topics/messages published by the core flight +software versus topics/messages published by the ground bridge. By default, it +is set to false. + +# Running the Ground DDS ROS Bridge + +The ground bridge is not run by default in simulation. To run it, you'll need to +set the ASTROBEE_CONFIG_DIR environment variable. In a terminal with the +astrobee setup sourced, run: + +ASTROBEE_CONFIG_DIR=$ASTROBEE_WS/src/astrobee/config/ rosrun ground_dds_ros_bridge ground_dds_ros_bridge_node diff --git a/communications/ground_dds_ros_bridge/src/ground_dds_ros_bridge.cc b/communications/ground_dds_ros_bridge/src/ground_dds_ros_bridge.cc new file mode 100644 index 0000000000..66948ae95d --- /dev/null +++ b/communications/ground_dds_ros_bridge/src/ground_dds_ros_bridge.cc @@ -0,0 +1,370 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "ground_dds_ros_bridge/ground_dds_ros_bridge.h" + +namespace { + +typedef std::vector NodeVector; + +void SubstituteROBOT_NAME(kn::DdsEntitiesFactorySvcParameters * params) { + { + NodeVector::iterator first, last = params->publishers.end(); + for (first = params->publishers.begin(); first != last; ++first) { + if (first->name == "") { + first->name = Miro::RobotParameters::instance()->name.c_str(); + } + if (first->partition == "") { + first->partition = Miro::RobotParameters::instance()->name.c_str(); + } + } + } + + { + NodeVector::iterator first, last = params->subscribers.end(); + for (first = params->subscribers.begin(); first != last; ++first) { + if (first->name == "") { + first->name = Miro::RobotParameters::instance()->name.c_str(); + } + if (first->partition == "") { + first->partition = Miro::RobotParameters::instance()->name.c_str(); + } + } + } +} + +} // namespace + +namespace ground_dds_ros_bridge { + +GroundDdsRosBridge::GroundDdsRosBridge() { + components_ = 0; + + agent_name_ = "ISAAC Ground Bridge"; + + connecting_robot_ = ""; +} + +GroundDdsRosBridge::~GroundDdsRosBridge() { +} + +/********************** Rapid Sub Ros Pub Functions ***************************/ +bool GroundDdsRosBridge::BuildAccessControlStateToRos( + const std::string& pub_topic, + const std::string& name) { + std::string sub_topic; + bool use; + + if (ReadTopicInfo(name, "sub", sub_topic, use)) { + if (use) { + ff::RapidSubRosPubPtr access_control_state_to_access_control_state( + new ff::RapidAccessControlStateToRos(sub_topic, + pub_topic, + nh_)); + components_++; + rapid_sub_ros_pubs_.push_back(access_control_state_to_access_control_state); + } + } else { + return false; + } + + return true; +} + +bool GroundDdsRosBridge::BuildGuestScienceDataToRos( + const std::string& pub_topic, + const std::string& name) { + std::string sub_topic; + bool use; + + if (ReadTopicInfo(name, "sub", sub_topic, use)) { + if (use) { + ff::RapidSubRosPubPtr guest_science_data_to_guest_science_data( + new ff::RapidGuestScienceDataToRos(sub_topic, + pub_topic, + nh_)); + components_++; + rapid_sub_ros_pubs_.push_back(guest_science_data_to_guest_science_data); + } + } else { + return false; + } + + return true; +} + +bool GroundDdsRosBridge::BuildPositionToOdom(const std::string& pub_topic, + const std::string& name) { + std::string sub_topic; + bool use; + + if (ReadTopicInfo(name, "sub", sub_topic, use)) { + if (use) { + ff::RapidSubRosPubPtr position_to_odom( + new ff::RapidPositionToRos(sub_topic, + pub_topic, + nh_)); + components_++; + rapid_sub_ros_pubs_.push_back(position_to_odom); + } + } else { + return false; + } + + return true; +} + +bool GroundDdsRosBridge::BuildSensorImageToRos(const std::string& pub_topic, + const std::string& name) { + std::string sub_topic; + bool use; + + if (ReadTopicInfo(name, "sub", sub_topic, use)) { + if (use) { + ff::RapidSubRosPubPtr sensor_image_to_compressed_image( + new ff::RapidImageToRos(sub_topic, + pub_topic, + nh_)); + components_++; + rapid_sub_ros_pubs_.push_back(sensor_image_to_compressed_image); + } + } else { + return false; + } + + return true; +} + +/********************* Ros Sub Rapid Pub Functions ****************************/ +bool GroundDdsRosBridge::BuildCommandToRapid(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + ff::RosSubRapidPubPtr command_to_command( + new ff::RosCommandToRapid(sub_topic, + pub_topic, + connecting_robot_, + nh_)); + components_++; + ros_sub_rapid_pubs_.push_back(command_to_command); + } + } else { + return false; + } + + return true; +} + + +bool GroundDdsRosBridge::Initialize(ros::NodeHandle *nh) { + config_params_.AddFile("communications/ground_dds_ros_bridge.config"); + + if (!config_params_.ReadFiles()) { + ROS_FATAL("DDS Bridge: Error reading config files."); + return false; + } + + // Need to get the connecting robot name and domain id first for the dds setup + // All other configuration parameters will be read in after dds is setup. + // Get the name of the robot the ground bridge is trying to connect to. + if (!config_params_.GetStr("connecting_robot", &connecting_robot_)) { + ROS_FATAL("Ground bridge: connecting robot not specified!"); + return false; + } + + // Get the domain id which will be different in space than on the ground + if (!config_params_.GetInt("domain_id", &domain_id_)) { + ROS_FATAL("Ground bridge: domain id not specified!"); + return false; + } + + nh_ = *nh; + + int fake_argc = 1; + + // TODO(kmhamil1): change this to not use freeflyer tools + + // Make path to QOS and NDDS files + std::string config_path = ff_common::GetConfigDir(); + config_path += "/communications/dds/"; + + // Create fake argv containing only the particaptant name + // Participant name needs to unique so combine bridge name with timestamp + ros::Time time = ros::Time::now(); + participant_name_ = agent_name_ + std::to_string(time.sec); + char **fake_argv = new char*[1]; + fake_argv[0] = new char[(participant_name_.size() + 1)]; + std::strcpy(fake_argv[0], participant_name_.c_str()); // NOLINT + + /* fake miro log into thinking we have no arguments */ + Miro::Log::init(fake_argc, fake_argv); + Miro::Log::level(9); + + /* fake miro configuration into thinking we have no arguments */ + Miro::Configuration::init(fake_argc, fake_argv); + + Miro::RobotParameters *ground_params = Miro::RobotParameters::instance(); + kn::DdsEntitiesFactorySvcParameters *dds_params = + kn::DdsEntitiesFactorySvcParameters::instance(); + + /* get the defaults for *all the things!* */ + Miro::ConfigDocument *config = Miro::Configuration::document(); + config->setSection("Robot"); + config->getParameters("Miro::RobotParameters", *ground_params); + config->getParameters("kn::DdsEntitiesFactorySvcParameters", *dds_params); + + ground_params->name = agent_name_; + ground_params->namingContextName = ground_params->name; + + // Add robot that we need to listen to + ground_params->teamMembers.push_back(connecting_robot_); + + SubstituteROBOT_NAME(dds_params); + + // Set values for default publisher and subscriber +/* dds_params->publishers[0].name = agent_name_; + dds_params->publishers[0].partition = agent_name_; + dds_params->publishers[0].participant = participant_name_; + + dds_params->subscribers[0].name = connecting_robot_; + dds_params->subscribers[0].partition = connecting_robot_; + dds_params->subscribers[0].participant = participant_name_; + + + kn::DdsNodeParameters *subscriber = new kn::DdsNodeParameters(); + subscriber->name = connecting_robot_; + subscriber->partition = connecting_robot_; + subscriber->participant = participant_name_; + dds_params->subscribers.push_back(*subscriber); +*/ + + // Clear config files so that dds only looks for the files we add + dds_params->participants[0].discoveryPeersFiles.clear(); + dds_params->configFiles.clear(); + + dds_params->participants[0].participantName = participant_name_; + dds_params->participants[0].domainId = domain_id_; + dds_params->participants[0].discoveryPeersFiles.push_back( + (config_path + "NDDS_DISCOVERY_PEERS")); + dds_params->configFiles.push_back((config_path + "RAPID_QOS_PROFILES.xml")); + + dds_params->subscribers[0].partition = connecting_robot_; + dds_params->publishers[0].partition = connecting_robot_; + + /** + * Use DdsEntitiesFactorySvc to create a new DdsEntitiesFactory + * which will create all objects: + * Participants DdsDomainParticpantRepository::instance() + * Publishers DdsPublisherRespoitory::instance() + * Subscribers DdsSubscriberRepository::instance() + * Topics + * and store in relevant repository + * based on DdsEntitesFactoryParameters + */ + dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); + dds_entities_factory_->init(dds_params); + + if (!ReadParams()) { + return false; + } + + return true; +} + +bool GroundDdsRosBridge::ReadTopicInfo(const std::string& topic_abbr, + const std::string& sub_or_pub, + std::string& topic, + bool& use) { + std::string use_exp = "use_" + topic_abbr; + std::string topic_exp = sub_or_pub + "_topic_" + topic_abbr; + + if (!config_params_.GetBool(use_exp.c_str(), &use)) { + ROS_FATAL("Ground bridge: use %s not specified!", topic_abbr.c_str()); + return false; + } + + if (use) { + if (!config_params_.GetStr(topic_exp.c_str(), &topic)) { + ROS_FATAL("Ground bridge: sub topic %s not specified!", + topic_abbr.c_str()); + return false; + } + } + + return true; +} + +bool GroundDdsRosBridge::ReadParams() { + std::string ns = "/"; + bool use_namespace = false; + components_ = 0; + + // Get boolean that signifies if the namespace needs to be the robot name + config_params_.GetBool("use_namespace", &use_namespace); + if (use_namespace) { + // First character needs to be uppercase for dds but lower case for fsw + std::string lowercase_robot = connecting_robot_; + lowercase_robot[0] = std::tolower(lowercase_robot[0]); + ns += lowercase_robot + "/"; + } + + // rapid_access_control_state_ros_access_control_state = RACSRACS + if (!BuildAccessControlStateToRos( + (ns + TOPIC_MANAGEMENT_ACCESS_CONTROL_STATE), + "RACSRACS")) { + return false; + } + + // rapid_guest_science_data_ros_guest_science_data => RGSDRGSD + if (!BuildGuestScienceDataToRos((ns + TOPIC_GUEST_SCIENCE_DATA), + "RGSDRGSD")) { + return false; + } + + // rapid_image_ros_compressed_science_cam_image => RIRCSCI + if (!BuildSensorImageToRos((ns + TOPIC_HARDWARE_SCI_CAM), "RIRCSCI")) { + return false; + } + + // rapid_image_ros_compressed_nav_cam_image => RIRCNCI + if (!BuildSensorImageToRos((ns + TOPIC_HARDWARE_NAV_CAM), "RIRCNCI")) { + return false; + } + + // rapid_image_ros_compressed_dock_cam_image => RIRCDCI + if (!BuildSensorImageToRos((ns + TOPIC_HARDWARE_DOCK_CAM), "RIRCDCI")) { + return false; + } + + // rapid_position_ros_odom => RPRO + if (!BuildPositionToOdom((ns + TOPIC_GNC_EKF), "RPRO")) { + return false; + } + + // ros_command_rapid_command => RCRC + if (!BuildCommandToRapid((ns + TOPIC_COMMAND), "RCRC")) { + return false; + } + + return true; +} + +} // end namespace ground_dds_ros_bridge diff --git a/communications/ground_dds_ros_bridge/src/rapid_access_control_state.cc b/communications/ground_dds_ros_bridge/src/rapid_access_control_state.cc new file mode 100644 index 0000000000..59c439eb06 --- /dev/null +++ b/communications/ground_dds_ros_bridge/src/rapid_access_control_state.cc @@ -0,0 +1,71 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "ground_dds_ros_bridge/rapid_access_control_state.h" + +namespace ff { + +RapidAccessControlStateToRos::RapidAccessControlStateToRos( + const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RapidSubRosPub(subscribe_topic, + pub_topic, + nh, + "RapidAccessControlStateToRos", + queue_size) { + // advertise ros topic, make it latched + pub_ = nh_.advertise(publish_topic_, + queue_size, + true); + + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + rapid::ACCESSCONTROL_STATE_TOPIC + subscribe_topic, // topic + "", // name + "RapidAccessControlStateProfile", // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidAccessControlStateToRos exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidAccessControlStateToRos exception unknown"); + throw; + } + + // start thread + StartThread(); +} + +void RapidAccessControlStateToRos::operator() ( + rapid::AccessControlState const* rapid_access_control_state) { + ff_msgs::AccessControlStateStamped state; + util::RapidHeader2Ros(rapid_access_control_state->hdr, &state.header); + + state.controller = rapid_access_control_state->controller; + + // Astrobee uses the requestors field for the cookie. Thus we only need the + // first element. + state.cookie = rapid_access_control_state->requestors[0]; + + pub_.publish(state); +} + +} // end namespace ff diff --git a/communications/ground_dds_ros_bridge/src/rapid_guest_science_data.cc b/communications/ground_dds_ros_bridge/src/rapid_guest_science_data.cc new file mode 100644 index 0000000000..2fa0227d59 --- /dev/null +++ b/communications/ground_dds_ros_bridge/src/rapid_guest_science_data.cc @@ -0,0 +1,81 @@ +/* Copyright (c) 2017, United State Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "ground_dds_ros_bridge/rapid_guest_science_data.h" + +namespace ff { + +RapidGuestScienceDataToRos::RapidGuestScienceDataToRos( + const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RapidSubRosPub(subscribe_topic, + pub_topic, + nh, + "RapidGuestScienceDataToRos", + queue_size) { + // advertise ros topic, make it latched + pub_ = nh_.advertise(publish_topic_, queue_size); + + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + rapid::ext::astrobee::GUEST_SCIENCE_DATA_TOPIC + subscribe_topic, // topic + "", // name + "AstrobeeGuestScienceDataProfile", // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidGuestScienceDataToRos exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidGuestScienceDataToRos exception unknown"); + throw; + } + + // start thread + StartThread(); +} + +void RapidGuestScienceDataToRos::operator() ( + rapid::ext::astrobee::GuestScienceData const* rapid_gs_data) { + ff_msgs::GuestScienceData data; + util::RapidHeader2Ros(rapid_gs_data->hdr, &data.header); + + data.apk_name = rapid_gs_data->apkName; + + if (rapid_gs_data->type == rapid::ext::astrobee::GUEST_SCIENCE_STRING) { + data.data_type = ff_msgs::GuestScienceData::STRING; + } else if (rapid_gs_data->type == rapid::ext::astrobee::GUEST_SCIENCE_JSON) { + data.data_type = ff_msgs::GuestScienceData::JSON; + } else if (rapid_gs_data->type == + rapid::ext::astrobee::GUEST_SCIENCE_BINARY) { + data.data_type = ff_msgs::GuestScienceData::BINARY; + } + + data.topic = rapid_gs_data->topic; + + unsigned char* buf = rapid_gs_data->data.get_contiguous_buffer(); + data.data.reserve(rapid_gs_data->data.length()); + data.data.resize(rapid_gs_data->data.length()); + std::memmove(data.data.data(), buf, rapid_gs_data->data.length()); + + pub_.publish(data); +} + +} // end namespace ff diff --git a/communications/ground_dds_ros_bridge/src/rapid_image.cc b/communications/ground_dds_ros_bridge/src/rapid_image.cc new file mode 100644 index 0000000000..9f923ca6e1 --- /dev/null +++ b/communications/ground_dds_ros_bridge/src/rapid_image.cc @@ -0,0 +1,77 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "ground_dds_ros_bridge/rapid_image.h" + +namespace ff { + +RapidImageToRos::RapidImageToRos(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RapidSubRosPub(subscribe_topic, + pub_topic, + nh, + "RapidImageToRos", + queue_size) { + // advertise ros topic + publish_topic_ = pub_topic + "/compressed"; + pub_ = + nh_.advertise(publish_topic_, queue_size); + + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + rapid::IMAGESENSOR_SAMPLE_TOPIC + subscribe_topic, // topic + "", // name + "RapidImageSensorSampleProfile", // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidImageToRos exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidImageToRos exception unknown"); + throw; + } + + // start thread + StartThread(); +} + +void RapidImageToRos::operator() (rapid::ImageSensorSample const* rapid_img) { + sensor_msgs::CompressedImage img; + util::RapidHeader2Ros(rapid_img->hdr, &img.header); + + // only accept 2 values; jpeg or png + if (std::strcmp(rapid_img->mimeType, rapid::MIME_IMAGE_JPEG) == 0) { + img.format = "jpeg"; + } else if (std::strcmp(rapid_img->mimeType, rapid::MIME_IMAGE_PNG) == 0) { + img.format = "png"; + } else { + ROS_ERROR_STREAM("Unknown image format in ground bridge. Type sent is " << rapid_img->mimeType); + } + + unsigned char* buf = rapid_img->data.get_contiguous_buffer(); + img.data.reserve(rapid_img->data.length()); + img.data.resize(rapid_img->data.length()); + std::memmove(img.data.data(), buf, rapid_img->data.length()); + + pub_.publish(img); +} + +} // end namespace ff diff --git a/communications/ground_dds_ros_bridge/src/rapid_position.cc b/communications/ground_dds_ros_bridge/src/rapid_position.cc new file mode 100644 index 0000000000..cf1441b0d3 --- /dev/null +++ b/communications/ground_dds_ros_bridge/src/rapid_position.cc @@ -0,0 +1,111 @@ +/* Copyright (c) 2017, United State Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "ground_dds_ros_bridge/rapid_position.h" + +namespace ff { + +RapidPositionToRos::RapidPositionToRos(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RapidSubRosPub(subscribe_topic, + pub_topic, + nh, + "RapidPositionToRos", + queue_size) { + // advertise ros topic, make it latched + pub_ = nh_.advertise(publish_topic_, queue_size); + + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + rapid::ext::astrobee::EKF_STATE_TOPIC + subscribe_topic, // topic + "", // name + "AstrobeeEkfStateProfile", // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidPositionToRos exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidPositionToRos exception unknown"); + throw; + } + + // start thread + StartThread(); +} + +void RapidPositionToRos::CopyTransform3D(geometry_msgs::Pose& pose, + const rapid::Transform3D& transform) { + pose.position.x = transform.xyz[0]; + pose.position.y = transform.xyz[1]; + pose.position.z = transform.xyz[2]; + + pose.orientation.x = transform.rot[0]; + pose.orientation.y = transform.rot[1]; + pose.orientation.z = transform.rot[2]; + pose.orientation.w = transform.rot[3]; +} + +void RapidPositionToRos::CopyVec3D(geometry_msgs::Vector3& vec_out, + const rapid::Vec3d& vec_in) { + vec_out.x = vec_in[0]; + vec_out.y = vec_in[1]; + vec_out.z = vec_in[2]; +} + +void RapidPositionToRos::operator() (rapid::ext::astrobee::EkfState const* + rapid_ekf_state) { + ff_msgs::EkfState state; + util::RapidHeader2Ros(rapid_ekf_state->hdr, &state.header); + + CopyTransform3D(state.pose, rapid_ekf_state->pose); + + CopyVec3D(state.velocity, rapid_ekf_state->velocity); + + CopyVec3D(state.omega, rapid_ekf_state->omega); + + CopyVec3D(state.gyro_bias, rapid_ekf_state->gyro_bias); + + CopyVec3D(state.accel, rapid_ekf_state->accel); + + CopyVec3D(state.accel_bias, rapid_ekf_state->accel_bias); + + for (int i = 0; i <15; i++) { + state.cov_diag[i] = rapid_ekf_state->cov_diag[i]; + } + + state.confidence = rapid_ekf_state->confidence; + + state.status = rapid_ekf_state->status; + + state.of_count = rapid_ekf_state->of_count; + + state.ml_count = rapid_ekf_state->ml_count; + + CopyTransform3D(state.hr_global_pose, rapid_ekf_state->hr_global_pose); + + for (int i = 0; i < 50; i++) { + state.ml_mahal_dists[i] = rapid_ekf_state->ml_mahal_dists[i]; + } + + pub_.publish(state); +} + +} // end namespace ff diff --git a/communications/ground_dds_ros_bridge/src/rapid_sub_ros_pub.cc b/communications/ground_dds_ros_bridge/src/rapid_sub_ros_pub.cc new file mode 100644 index 0000000000..5ed78c4afc --- /dev/null +++ b/communications/ground_dds_ros_bridge/src/rapid_sub_ros_pub.cc @@ -0,0 +1,49 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "ground_dds_ros_bridge/rapid_sub_ros_pub.h" + +namespace ff { + +RapidSubRosPub::RapidSubRosPub(const std::string& subscribe_topic, + const std::string& pub_topic, + const ros::NodeHandle &nh, + const std::string& entity_name, + const unsigned int queue_size) + : nh_(nh), subscribe_topic_(subscribe_topic), publish_topic_(pub_topic), + queue_size_(queue_size), alive_(true), dds_event_loop_(entity_name) { +} + +RapidSubRosPub::~RapidSubRosPub() { + alive_ = false; // Notify thread to exit + thread_.join(); +} + +void RapidSubRosPub::StartThread() { + // start joinable thread + thread_ = std::thread(&RapidSubRosPub::ThreadExec, this); +} + +void RapidSubRosPub::ThreadExec() { + while (alive_) { + // process events at 10hz + dds_event_loop_.processEvents(kn::milliseconds(100)); + } +} + +} // end namespace ff diff --git a/communications/ground_dds_ros_bridge/src/ros_command.cc b/communications/ground_dds_ros_bridge/src/ros_command.cc new file mode 100644 index 0000000000..083de3cb9c --- /dev/null +++ b/communications/ground_dds_ros_bridge/src/ros_command.cc @@ -0,0 +1,108 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "ground_dds_ros_bridge/ros_command.h" + +ff::RosCommandToRapid::RosCommandToRapid(const std::string& subscribe_topic, + const std::string& pub_topic, + const std::string& connecting_robot, + const ros::NodeHandle &nh, + const unsigned int queue_size) + : RosSubRapidPub(subscribe_topic, pub_topic, nh, queue_size) { + command_supplier_.reset( + new ff::RosCommandToRapid::CommandSupplier( + rapid::COMMAND_TOPIC + pub_topic, "", "RapidCommandProfile", "")); + + sub_ = nh_.subscribe(subscribe_topic, + queue_size, + &RosCommandToRapid::CmdCallback, + this); + + rapid::RapidHelper::initHeader(command_supplier_->event().hdr, + "", + connecting_robot); +} + +void ff::RosCommandToRapid::CmdCallback( + ff_msgs::CommandStampedConstPtr const& cmd) { + ROS_ERROR("Received a ros command."); + rapid::Command &msg = command_supplier_->event(); + + msg.hdr.timeStamp = util::RosTime2RapidTime(cmd->header.stamp); + + std::strncpy(msg.cmdName, cmd->cmd_name.data(), 64); + msg.cmdName[63] = '\0'; + + std::strncpy(msg.cmdId, cmd->cmd_id.data(), 64); + msg.cmdName[63] = '\0'; + + std::strncpy(msg.cmdSrc, cmd->cmd_src.data(), 32); + msg.cmdSrc[31] = '\0'; + + std::strncpy(msg.subsysName, cmd->subsys_name.data(), 32); + msg.subsysName[31] = '\0'; + + msg.arguments.length(cmd->args.size()); + for (unsigned int i = 0; i < cmd->args.size(); i++) { + switch (cmd->args[i].data_type) { + case ff_msgs::CommandArg::DATA_TYPE_BOOL: + msg.arguments[i]._d = rapid::RAPID_BOOL; + msg.arguments[i]._u.b = cmd->args[i].b; + break; + case ff_msgs::CommandArg::DATA_TYPE_DOUBLE: + msg.arguments[i]._d = rapid::RAPID_DOUBLE; + msg.arguments[i]._u.d = cmd->args[i].d; + break; + case ff_msgs::CommandArg::DATA_TYPE_FLOAT: + msg.arguments[i]._d = rapid::RAPID_FLOAT; + msg.arguments[i]._u.f = cmd->args[i].f; + break; + case ff_msgs::CommandArg::DATA_TYPE_INT: + msg.arguments[i]._d = rapid::RAPID_INT; + msg.arguments[i]._u.i = cmd->args[i].i; + break; + case ff_msgs::CommandArg::DATA_TYPE_LONGLONG: + msg.arguments[i]._d = rapid::RAPID_LONGLONG; + msg.arguments[i]._u.ll = cmd->args[i].ll; + break; + case ff_msgs::CommandArg::DATA_TYPE_STRING: + msg.arguments[i]._d = rapid::RAPID_STRING; + std::strncpy(msg.arguments[i]._u.s, cmd->args[i].s.data(), 128); + msg.arguments[i]._u.s[127] = '\0'; + break; + case ff_msgs::CommandArg::DATA_TYPE_VEC3d: + msg.arguments[i]._d = rapid::RAPID_VEC3d; + for (int j = 0; j < 3; j++) { + msg.arguments[i]._u.vec3d[j] = cmd->args[i].vec3d[j]; + } + break; + case ff_msgs::CommandArg::DATA_TYPE_MAT33f: + msg.arguments[i]._d = rapid::RAPID_MAT33f; + for (int j = 0; j < 9; j++) { + msg.arguments[i]._u.mat33f[j] = cmd->args[i].mat33f[j]; + } + break; + default: + ROS_ERROR("Ground DDS bridge: Unknown command data type: %d", + cmd->args[i].data_type); + } + } + + ROS_ERROR("Sending dds command."); + command_supplier_->sendEvent(); +} diff --git a/communications/ground_dds_ros_bridge/src/util.cc b/communications/ground_dds_ros_bridge/src/util.cc new file mode 100644 index 0000000000..f0a3e26691 --- /dev/null +++ b/communications/ground_dds_ros_bridge/src/util.cc @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +ros::Time util::RapidTime2RosTime(const int64_t dds_time) { + ros::Time t; + + // DDS time is in microseconds, we need nanoseconds + t.fromNSec(static_cast(dds_time) * 1000); + + return t; +} + +int64_t util::RosTime2RapidTime(ros::Time const& ros_time) { + return static_cast(ros_time.toNSec() / 1000ULL); +} + +void util::RosHeader2Rapid(std_msgs::Header const& ros_hdr, + rapid::Header* rapid_hdr, + int status, + int serial) { + rapid::RapidHelper::initHeader(*rapid_hdr); + if (serial >= 0) { + rapid_hdr->serial = serial; + } + rapid_hdr->statusCode = status; + rapid_hdr->timeStamp = util::RosTime2RapidTime(ros_hdr.stamp); +} + +void util::RapidHeader2Ros(rapid::Header const& rapid_hdr, + std_msgs::Header *ros_hdr, + std::string const& frame_id) { + ros_hdr->frame_id = frame_id; + ros_hdr->stamp = util::RapidTime2RosTime(rapid_hdr.timeStamp); +} diff --git a/communications/ground_dds_ros_bridge/tools/ground_dds_ros_bridge_node.cc b/communications/ground_dds_ros_bridge/tools/ground_dds_ros_bridge_node.cc new file mode 100644 index 0000000000..a793df3003 --- /dev/null +++ b/communications/ground_dds_ros_bridge/tools/ground_dds_ros_bridge_node.cc @@ -0,0 +1,31 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "ground_dds_ros_bridge/ground_dds_ros_bridge.h" + +int main(int argc, char **argv) { + ros::init(argc, argv, "ground_dds_ros_bridge"); + + ros::NodeHandle nh("~"); + ground_dds_ros_bridge::GroundDdsRosBridge gdrb; + if (gdrb.Initialize(&nh)) { + ros::spin(); + } + + return 0; +} diff --git a/communications/readme.md b/communications/readme.md index 81177cbefd..970753285b 100644 --- a/communications/readme.md +++ b/communications/readme.md @@ -1,3 +1,4 @@ \page comms Communications \subpage dds_ros_bridge +\subpage comms_bridge diff --git a/debian/.gitignore b/debian/.gitignore index 0297c35c51..ce3ffbdb53 100644 --- a/debian/.gitignore +++ b/debian/.gitignore @@ -1,7 +1,9 @@ tmp astrobee0 +astrobee-comms astrobee-dev astrobee-config +astrobee-comms files debhelper-build-stamp *.substvars diff --git a/debian/astrobee-comms.dirs b/debian/astrobee-comms.dirs new file mode 100644 index 0000000000..cdd16ade47 --- /dev/null +++ b/debian/astrobee-comms.dirs @@ -0,0 +1,4 @@ +opt/astrobee +opt/astrobee/lib +opt/astrobee/lib/pkgconfig +opt/astrobee/share diff --git a/debian/astrobee-comms.files b/debian/astrobee-comms.files new file mode 100644 index 0000000000..2a62ae9d47 --- /dev/null +++ b/debian/astrobee-comms.files @@ -0,0 +1,5 @@ +opt/astrobee/lib/libdds_ros_bridge.so +opt/astrobee/lib/libcomms_bridge.so +opt/astrobee/lib/librapidExtAstrobee.so +opt/astrobee/share/dds_ros_bridge +opt/astrobee/share/comms_bridge diff --git a/debian/astrobee-comms.lintian-overrides b/debian/astrobee-comms.lintian-overrides new file mode 100644 index 0000000000..3cf71d62d9 --- /dev/null +++ b/debian/astrobee-comms.lintian-overrides @@ -0,0 +1,15 @@ +# This will actually have no effect on lintian because of the current profile +# settings for this vendor (ubuntu). If you want to ignore these errors +# you must modify the following file: /usr/share/lintian/profiles/ubuntu/main.profile +# +# Add to the end: +# +# Overridable: yes +# Tags: dir-or-file-in-opt, +# +# See https://bugs.launchpad.net/ubuntu/+source/lintian/+bug/824122 for more. + +dir-or-file-in-opt + +# Ignore until we figure out how to use shlibs on chroot +missing-dependency-on-libc \ No newline at end of file diff --git a/debian/astrobee-config.dirs b/debian/astrobee-config.dirs deleted file mode 100644 index 498a2105f8..0000000000 --- a/debian/astrobee-config.dirs +++ /dev/null @@ -1,2 +0,0 @@ -opt/astrobee -opt/astrobee/config diff --git a/debian/astrobee-config.lintian-overrides b/debian/astrobee-config.lintian-overrides new file mode 100644 index 0000000000..e328147ae7 --- /dev/null +++ b/debian/astrobee-config.lintian-overrides @@ -0,0 +1,2 @@ +# See astrobee-comms.lintian-overrides for details +dir-or-file-in-opt \ No newline at end of file diff --git a/debian/astrobee-config.postinst b/debian/astrobee-config.postinst index 04844d7d98..5fcfc53f71 100755 --- a/debian/astrobee-config.postinst +++ b/debian/astrobee-config.postinst @@ -1,5 +1,7 @@ #!/bin/sh +set -e + if [ "$1" = "configure" ]; then chmod -R g+rwX /opt/astrobee/config if [ $(getent group users) ]; then diff --git a/debian/astrobee-dev.dirs b/debian/astrobee-dev.dirs deleted file mode 100644 index a8e84b5a73..0000000000 --- a/debian/astrobee-dev.dirs +++ /dev/null @@ -1,2 +0,0 @@ -opt/astrobee/include -opt/astrobee/lib/pkgconfig diff --git a/debian/astrobee-dev.lintian-overrides b/debian/astrobee-dev.lintian-overrides new file mode 100644 index 0000000000..e328147ae7 --- /dev/null +++ b/debian/astrobee-dev.lintian-overrides @@ -0,0 +1,2 @@ +# See astrobee-comms.lintian-overrides for details +dir-or-file-in-opt \ No newline at end of file diff --git a/debian/astrobee0.dirs b/debian/astrobee0.dirs deleted file mode 100644 index 45a0f176e7..0000000000 --- a/debian/astrobee0.dirs +++ /dev/null @@ -1,7 +0,0 @@ -opt/astrobee -opt/astrobee/bin -opt/astrobee/lib -opt/astrobee/lib/pkgconfig -opt/astrobee/lib/python2.7 -opt/astrobee/lib/astrobee -opt/astrobee/share diff --git a/debian/astrobee0.install b/debian/astrobee0.install index 40f42c6a85..e67ff4e3cd 100644 --- a/debian/astrobee0.install +++ b/debian/astrobee0.install @@ -8,6 +8,6 @@ opt/astrobee/version.txt opt/astrobee/.catkin opt/astrobee/bin/* opt/astrobee/lib/*.so -opt/astrobee/lib/python2.7/* +opt/astrobee/lib/python*/* opt/astrobee/lib/astrobee/* opt/astrobee/share/* diff --git a/debian/astrobee0.lintian-overrides b/debian/astrobee0.lintian-overrides new file mode 100644 index 0000000000..b8d3013480 --- /dev/null +++ b/debian/astrobee0.lintian-overrides @@ -0,0 +1,4 @@ +# See astrobee-comms.lintian-overrides for details +dir-or-file-in-opt +# Ignore until we figure out how to use shlibs on chroot +missing-dependency-on-libc \ No newline at end of file diff --git a/debian/changelog b/debian/changelog index 77967a7dab..1c8ed36b93 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,184 @@ +astrobee (0.19.1) testing; urgency=medium + + * Updated Map Matching with TEBLID and CLAHE + + -- Astrobee Flight Software Sun, 30 Jun 2024 15:20:46 -0700 + +astrobee (0.19.0) testing; urgency=medium + + * New split localizer + + -- Astrobee Flight Software Mon, 10 Jun 2024 09:54:26 -0700 + +astrobee (0.18.0) testing; urgency=medium + + * Astrobee now using OpenCV 4! Please see install updates in INSTALL.md + + -- Astrobee Flight Software Thu, 06 Jun 2024 16:22:41 -0700 + +astrobee (0.17.3) testing; urgency=medium + + * Comms bridge added rate feature + * Bug fixes + * User feedback improvements + + -- Astrobee Flight Software Thu, 09 May 2024 11:50:34 -0700 + +astrobee (0.17.2) UNRELEASED; urgency=medium + + * Adding Generic comms bridge + * Multiple bug fixes + * Ubuntu 20 only supported now + + -- Astrobee Flight Software Mon, 12 Feb 2024 12:33:07 -0800 + +astrobee (0.17.1) testing; urgency=medium + + * Multiple bug fixes + + -- Astrobee Flight Software Mon, 03 Apr 2023 10:59:33 -0700 + +astrobee (0.17.0) testing; urgency=medium + + * Full compatibility and debians built for Ubuntu 20 + * Deleted all simulink autocode for ctl and fam + + -- Astrobee Flight Software Thu, 16 Feb 2023 09:43:41 -0800 + +astrobee (0.16.7) testing; urgency=medium + + * Remove simulink auto-generated code + * improved documentation, other tools and fixes + + -- Astrobee Flight Software Wed, 25 Jan 2023 10:42:04 -0800 + +astrobee (0.16.6) testing; urgency=medium + + * switch map online possibility + * other fixes + + -- Astrobee Flight Software Fri, 03 Jun 2022 15:54:22 -0700 + +astrobee (0.16.5) testing; urgency=medium + + * Fix multiple bugs + + -- Astrobee Flight Software Mon, 23 May 2022 14:15:36 -0700 + +astrobee (0.16.4) testing; urgency=medium + + * Auto-exposure + * Add splice tool + * Add arm deploy to bridge + * Various bug fixes + + -- Astrobee Flight Software Tue, 03 May 2022 22:35:57 -0700 + +astrobee (0.16.3) testing; urgency=medium + + * Added bag tools + * Disabled mapper (can be enabled through service) + * Add debayer live + * Multiple other fixes and improvements + + -- Astrobee Flight Software Fri, 01 Apr 2022 09:33:42 -0700 + +astrobee (0.16.2) testing; urgency=medium + + * Performance improvements + * Small bug fixes + + -- Astrobee Flight Software Thu, 03 Feb 2022 15:55:05 -0800 + +astrobee (0.16.1) testing; urgency=medium + + * depth odometry + * better performance evaluation + * multiple other fixes and improvements + + -- Astrobee Flight Software Wed, 12 Jan 2022 14:55:11 -0800 + +astrobee (0.16.0) testing; urgency=medium + + * project compiles with catkin + * mapper performance improvement + * imu_augmentor performance improvement + * calibration improvements + * multiple other fixes + + -- Astrobee Flight Software Thu, 02 Dec 2021 13:57:39 -0800 + +astrobee (0.15.2) testing; urgency=medium + + * added python linters black and isort in the CI pipeline + * added perching arm reconnect service + * added nav and dock cam bayer image output option + * added code to report time for llp, mlp, hlp + * added end of motion check + + -- Astrobee Flight Software Thu, 30 Sep 2021 15:18:04 -0700 + +astrobee (0.15.1) testing; urgency=medium + + * Separated config files for DDS communications to avoid conflict + * Increased plan name length + * Pico proxy was added to the MLP launch file + * System monitor now attempts to reload nodelets that die on startup + * Other fixes/software and documentation improvements + + -- Astrobee Flight Software Mon, 02 Aug 2021 10:34:26 -0700 + +astrobee (0.15.0) testing; urgency=medium + + * Perching functional + * Ubuntu 20 compatibility + * Added astrobee to astrobee coms + * Various other fixes/software improvements + + -- Astrobee Flight Software Wed, 21 Jul 2021 02:21:10 -0700 + +astrobee (0.14.3) testing; urgency=medium + + * Rotation fallback fix + * Unified install instructions and Ubuntu 18 nasa install + * Various other minor fixes + * Note, Perching does not work in this release, use another version for this. + + -- Astrobee Flight Software Mon, 24 May 2021 23:17:32 -0700 + +astrobee (0.14.1) testing; urgency=medium + + * Dynamic IMU Filtering + * Added measurement spacing for visual odometry + * Tuning configurations + * Bug fixes + * Note, Perching does not work in this release, use another version for this. + + -- Astrobee Flight Software Wed, 03 Mar 2021 14:55:02 -0800 + +astrobee (0.14.0) testing; urgency=medium + + * Added graph_localizer package + * Added supporting packages for graph localizer including imu_augmetor, imu_integation, localization_common, localization_measurements + * Added tools for graph localizer including graph_bag and imu_bias_tester + * Added rviz plugins for graph localizer and associated packages in localization_rviz_plugins + * Added gtsam debian + * Note, Perching does not work in this release, use another version for this. + + -- Astrobee Flight Software Wed, 27 Jan 2021 13:43:11 -0800 + +astrobee (0.13.0) testing; urgency=medium + + * Multiple updates to documentation, doxygen is exported to github pages + * Adding Github actions to build and test the code and generate documentation + * astrobee_media now a submodule + * SciCam now generates compressed images + * Added memory monitor + * Cross compiling with docker + * Various other minor fixes + + -- Astrobee Flight Software Fri, 15 Jan 2021 10:37:25 -0800 + astrobee (0.12.0) testing; urgency=medium * Localization visualization improvements @@ -81,7 +262,7 @@ astrobee (0.9.3) testing; urgency=medium associated logging and fault generation) * Re-positioned the dock location on ISS from the mapping activity. - -- Astrobee Developer Mon, 08 Jul 2019 18:33:16 -0700 + -- Astrobee Flight Software Mon, 08 Jul 2019 18:33:16 -0700 astrobee (0.9.2) testing; urgency=medium @@ -98,7 +279,7 @@ astrobee (0.9.1) testing; urgency=medium * Camera driver double buffer fix * As usual, other bug fixes too - -- Astrobee Developer Wed, 29 May 2019 10:18:34 -0700 + -- Astrobee Flight Software Wed, 29 May 2019 10:18:34 -0700 astrobee (0.8.0) testing; urgency=medium diff --git a/debian/control b/debian/control index 49fb891d02..0a63118703 100644 --- a/debian/control +++ b/debian/control @@ -13,10 +13,10 @@ Section: libdevel Architecture: any Depends: astrobee0 (= ${binary:Version}), ${misc:Depends} libgoogle-glog-dev, libgtest-dev, libgflags-dev, - libace-dev, libqt4-dev, + libace-dev, libqt4-dev | qtbase5-dev, protobuf-compiler, libv4l-dev, libeigen3-dev, libluajit-5.1-dev, libjsoncpp-dev, libtinyxml-dev, libyaml-cpp-dev, libusb-1.0-0-dev, - libalvar-dev, libdbow2-dev, libopenmvg-dev, libroyale-dev, libceres-dev, rti-dev, + ros-noetic-ar-track-alvar, libdbow2-dev, libgtsam-dev, libopenmvg-dev, libroyale-dev, libceres-dev, rti-dev, libsoracore-dev, libmiro-dev, libdecomputil-dev, libjps3d-dev Description: Astrobee flight software development files. The Astrobee flight software development files. @@ -28,29 +28,46 @@ Depends: ${misc:Depends} Description: Astrobee flight software configuration. The Astrobee flight software configuration files. +Package: astrobee-comms +Section: comm +Architecture: any +Depends: ${misc:Depends} + astrobee-config (>= ${binary:Version}), + rti (>=1.0), libmiro0 (>=0.1), libsoracore1 (>=1.0), + ros-${ros-distro}-ros-base, ros-${ros-distro}-cpp-common, + ros-${ros-distro}-common-msgs, ros-${ros-distro}-nodelet, ros-${ros-distro}-image-transport, + ros-${ros-distro}-compressed-image-transport, ros-${ros-distro}-tf2, + ros-${ros-distro}-tf2-geometry-msgs, ros-${ros-distro}-tf2-msgs, ros-${ros-distro}-tf2-ros, + ros-${ros-distro}-tf2-sensor-msgs, + libgoogle-glog0v5 +Description: Astrobee flight software communications. + The communications portion of the Astrobee flight software. + Package: astrobee0 Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends} +Depends: ${misc:Depends} + ${ros-python}, astrobee-config (>= ${binary:Version}), - libalvar2 (>=2.0), libdbow21 (>=0.1), libopenmvg1 (>=1.0), libroyale1 (>=1.0), + astrobee-comms (>= ${binary:Version}), + ros-noetic-ar-track-alvar, libdbow21 (>=0.1-6), libgtsam (>=4.0), libopenmvg1 (>=1.0), libroyale1 (>=1.0), libceres1 (>=1.0), rti (>=1.0), libmiro0 (>=0.1), libsoracore1 (>=1.0), libdecomputil0 (>=0.1), libjps3d0 (>=0.1), libluajit-5.1-2, - ros-kinetic-ros-base, ros-kinetic-cpp-common, ros-kinetic-rosbag, - ros-kinetic-common-msgs, ros-kinetic-nodelet, ros-kinetic-pluginlib, - ros-kinetic-actionlib, ros-kinetic-image-transport, - ros-kinetic-compressed-image-transport, ros-kinetic-tf2, - ros-kinetic-tf2-eigen, ros-kinetic-tf2-geometry-msgs, - ros-kinetic-tf2-kdl, ros-kinetic-tf2-msgs, ros-kinetic-tf2-ros, - ros-kinetic-tf2-sensor-msgs, ros-kinetic-message-filters, - ros-kinetic-robot-state-publisher, ros-kinetic-rosconsole, ros-kinetic-roscpp, - ros-kinetic-rosgraph, ros-kinetic-rosgraph-msgs, - ros-kinetic-roslaunch, ros-kinetic-rosmaster, ros-kinetic-rosmsg, - ros-kinetic-rosnode, ros-kinetic-rosout, ros-kinetic-rosparam, - ros-kinetic-rosservice, ros-kinetic-rostopic, ros-kinetic-roswtf, - ros-kinetic-std-srvs, ros-kinetic-topic-tools, ros-kinetic-xmlrpcpp, - ros-kinetic-opencv3, - libjsoncpp1, libprotoc9v5, libgoogle-glog0v5, - libtinyxml2.6.2v5, libyaml-cpp0.5v5 + ros-${ros-distro}-ros-base, ros-${ros-distro}-cpp-common, ros-${ros-distro}-rosbag, + ros-${ros-distro}-common-msgs, ros-${ros-distro}-nodelet, ros-${ros-distro}-pluginlib, + ros-${ros-distro}-actionlib, ros-${ros-distro}-image-transport, + ros-${ros-distro}-compressed-image-transport, ros-${ros-distro}-tf2, + ros-${ros-distro}-tf2-eigen, ros-${ros-distro}-tf2-geometry-msgs, + ros-${ros-distro}-tf2-kdl, ros-${ros-distro}-tf2-msgs, ros-${ros-distro}-tf2-ros, + ros-${ros-distro}-tf2-sensor-msgs, ros-${ros-distro}-message-filters, + ros-${ros-distro}-robot-state-publisher, ros-${ros-distro}-rosconsole, ros-${ros-distro}-roscpp, + ros-${ros-distro}-rosgraph, ros-${ros-distro}-rosgraph-msgs, + ros-${ros-distro}-roslaunch, ros-${ros-distro}-rosmaster, ros-${ros-distro}-rosmsg, + ros-${ros-distro}-rosnode, ros-${ros-distro}-rosout, ros-${ros-distro}-rosparam, + ros-${ros-distro}-rosservice, ros-${ros-distro}-rostopic, ros-${ros-distro}-roswtf, + ros-${ros-distro}-std-srvs, ros-${ros-distro}-topic-tools, ros-${ros-distro}-xmlrpcpp, + libopencv-core4.2, libopencv-calib3d4.2, libopencv-highgui4.2, libopencv-imgcodecs4.2, libopencv-imgproc4.2, libopencv-features2d4.2, libopencv-xfeatures2d, + libjsoncpp1, libprotoc9v5 | libprotoc17, libgoogle-glog0v5, + libtinyxml2.6.2v5, libyaml-cpp0.5v5 | libyaml-cpp0.6 Description: Astrobee flight software. The Astrobee flight software. diff --git a/debian/copyright b/debian/copyright index 965895647f..d6a38dd606 100644 --- a/debian/copyright +++ b/debian/copyright @@ -1,6 +1,6 @@ Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ Upstream-Name: astrobee -Source: +Source: https://github.com/nasa/astrobee Files: * Copyright: 2017 Astrobee Team @@ -26,4 +26,7 @@ Files: debian/* Copyright: 2017 Astrobee Team License: Apache-2 +On Debian systems, the complete text of the Apache License, Version 2 +can be found in "/usr/share/common-licenses/Apache-2.0". + License: Apache-2 diff --git a/debian/rules b/debian/rules index 67dde28591..aa175d18ec 100755 --- a/debian/rules +++ b/debian/rules @@ -3,48 +3,59 @@ # output every command that modifies files on the build system. #export DH_VERBOSE = 1 - -# see FEATURE AREAS in dpkg-buildflags(1) -#export DEB_BUILD_OPTIONS = parallel=`nproc` -#export DEB_BUILD_MAINT_OPTIONS = hardening=+all - -# see ENVIRONMENT in dpkg-buildflags(1) -# package maintainers to append CFLAGS -#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic -# package maintainers to append LDFLAGS -#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed -export EXTRA_CMAKE_OPTS -export LIB_SEARCH_OPTIONS - ifeq ($(DEB_HOST_ARCH),armhf) - export EXTRA_CMAKE_OPTS=-DCMAKE_TOOLCHAIN_FILE=../scripts/build/ubuntu_cross.cmake -DUSE_CTC=true - # stripping doesn't work with the cross compile - #export DEB_BUILD_OPTIONS += nostrip - export LIB_SEARCH_OPTIONS = -l$(ARMHF_CHROOT_DIR)/opt/rti/ndds/lib/armv6vfphLinux3.xgcc4.7.2:$(ARMHF_CHROOT_DIR)/opt/ros/kinetic/lib:$(ARMHF_CHROOT_DIR)/opt/ros/kinetic/lib/arm-linux-gnueabihf:$(ARMHF_CHROOT_DIR)/lib:$(ARMHF_CHROOT_DIR)/usr/lib:$(ARMHF_CHROOT_DIR)/usr/lib/arm-linux-gnueabihf:$(ARMHF_CHROOT_DIR)/lib/arm-linux-gnueabihf -- --ignore-missing-info --admindir=$(ARMHF_CHROOT_DIR)/var/lib/dpkg - export DEB_MAINT_CC_SET = $(ARMHF_TOOLCHAIN)/bin/arm-linux-gnueabihf-gcc - export DEB_MAINT_CXX_SET = $(ARMHF_TOOLCHAIN)/bin/arm-linux-gnueabihf-g++ + export FSW_CONFIG_FLAGS = -a -w debian -n debian -p src/debian/tmp +else + export FSW_CONFIG_FLAGS = -l -w debian_native -n debian_native -p src/debian/tmp/opt/astrobee + # dpkg-buildpackage and catkin were both adding flags apparently, ignore dpkg's + export DEB_CXXFLAGS_MAINT_SET +endif + +export FSW_PYTHON_DEP = python +ifeq ($(ROS_PYTHON_VERSION),3) + export FSW_PYTHON_DEP = python3 endif -# skip build for config files -build-indep: ; - mkdir -p build +# This is a Makefile variable that will only be used by native since armhf uses --extend +CMAKE_PREFIX_PATH = $(CURDIR)/cmake:/opt/ros/$(ROS_DISTRO) %: - dh $@ --buildsystem=cmake --builddirectory=build/ --parallel - -# we always need release, otherwise it is too slow -override_dh_auto_configure: - mkdir -p build - cd build && cmake .. -DCMAKE_VERBOSE_MAKEFILE=ON -DCMAKE_INSTALL_PREFIX:PATH=/opt/astrobee -DCMAKE_BUILD_TYPE=Release $(EXTRA_CMAKE_OPTS) && cd .. + # We are using catkin now, but debhelper does not exactly support it + # Since debhelper xenial does not support 'none', set cmake instead + dh $@ --buildsystem=cmake + +# only configure for architecture dependent +override_dh_auto_configure-arch: + # configure the build + cd .. && src/scripts/configure.sh $(FSW_CONFIG_FLAGS) + # Make sure we install + catkin config --install + +# build and install for architecture dependent only +override_dh_auto_install-arch: + # Make sure we only extend what we need + cd .. && CMAKE_PREFIX_PATH=$(CMAKE_PREFIX_PATH) catkin build + +# override only for arch dependent, for astrobee-config let it use default +override_dh_install-arch: + # Clean pycache before copying directories + pyclean debian || py3clean debian + # Move DDS stuff out of debian/tmp (astrobee-comms.files) + # so we don't intall it again to astrobee0 + dh_movefiles + # Install the rest of the files as defined in .install + dh_install + +# dpkg-shlibdebs and dpkq-query have issues finding dependencies in our chroot +# Skip this step since it was not populating {shlibs:Depends} anyway. +override_dh_shlibdeps: -override_dh_auto_test: ; +override_dh_gencontrol: + # Generate control file with the correct ros distribution and python for dependencies + dh_gencontrol -- -Vros-distro=$(ROS_DISTRO) -Vros-python=$(FSW_PYTHON_DEP) -#override_dh_auto_install-idep: -# @ -# -#override_dh_auto_install: -# dh_auto_install -# -override_dh_shlibdeps: - dh_shlibdeps $(LIB_SEARCH_OPTIONS) +# skip for 'arch independent' (astrobee-config) +override_dh_auto_configure-indep override_dh_auto_install-indep: +# skip for all +override_dh_auto_build override_dh_auto_test: diff --git a/description/CMakeLists.txt b/description/CMakeLists.txt deleted file mode 100644 index e381a31a42..0000000000 --- a/description/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -# Check to see if correct version of media has been downloaded into tree. We -# dont do this if we are cross-compiling, as there is no need for it. We must -# also make sure the media is copied in a native install to a simulator. - -# Add the things that need to be on both the robot and the simulation -add_subdirectory(description) - -# Optional: install only for simulated context -if ((NOT USE_CTC) AND (NOT IS_BAMBOO_BUILD)) - set(FETCHCONTENT_QUIET OFF) - FetchContent_Declare(media - URL https://github.com/nasa/astrobee_media/archive/v0.0.7.tar.gz - URL_HASH SHA1=5ad19f0e64d039d852a5beb21ad07442d61f5790 - SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/media - ) - FetchContent_GetProperties(media) - if(NOT media_POPULATED) - FetchContent_Populate(media) - add_subdirectory(${media_SOURCE_DIR}/astrobee_granite) - add_subdirectory(${media_SOURCE_DIR}/astrobee_dock) - add_subdirectory(${media_SOURCE_DIR}/astrobee_iss) - add_subdirectory(${media_SOURCE_DIR}/astrobee_handrail_8_5) - add_subdirectory(${media_SOURCE_DIR}/astrobee_handrail_21_5) - add_subdirectory(${media_SOURCE_DIR}/astrobee_handrail_30) - add_subdirectory(${media_SOURCE_DIR}/astrobee_handrail_41_5) - add_subdirectory(${media_SOURCE_DIR}/astrobee_freeflyer) - endif() -endif ((NOT USE_CTC) AND (NOT IS_BAMBOO_BUILD)) diff --git a/description/description/CMakeLists.txt b/description/description/CMakeLists.txt index 6da9a5ccfc..8212f71f21 100644 --- a/description/description/CMakeLists.txt +++ b/description/description/CMakeLists.txt @@ -19,8 +19,12 @@ # dont do this if we are cross-compiling, as there is no need for it. We must # also make sure the media is copied in a native install to a simulator. +cmake_minimum_required(VERSION 3.0) project(description) +find_package(catkin REQUIRED COMPONENTS +) + catkin_package() install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/description/description/readme.md b/description/description/readme.md index f129657bc1..6de9890f9b 100644 --- a/description/description/readme.md +++ b/description/description/readme.md @@ -5,16 +5,16 @@ The Universal Robot Description Format (URDF) is a language for describing a rob This folder contains only the Xacro files for the free-flyer model. The dock, ISS and granite models are static URDFs that are included in the media distribution. This design choice enables us to load these entities from a world file, preventing race conditions in loading environmental elements with free-flyers in a simulated context. The way URDF works is best illustrated by this example, which is actually loosely representative of how the project's launch files actually work: - - - - - - - - +```xml + + + + + + +``` Line (1) specifies the namespace on which the robot will operate. Line (2) creates the namespace. Line (3) invokes xacro to write URDF to parameter /honey/robot_description. diff --git a/description/description/urdf/macro_perching_arm.urdf.xacro b/description/description/urdf/macro_perching_arm.urdf.xacro index 38496f7fc6..877d7e3c95 100644 --- a/description/description/urdf/macro_perching_arm.urdf.xacro +++ b/description/description/urdf/macro_perching_arm.urdf.xacro @@ -22,7 +22,8 @@ - + + false + link="${prefix}body" /> + link="${prefix}${bay}" /> - + false + link="${prefix}${bay}" /> + link="${prefix}${bay}_arm_proximal_link" /> - + false + link="${prefix}${bay}_arm_proximal_link" /> + link="${prefix}${bay}_arm_distal_link" /> - + false + link="${prefix}${bay}_arm_distal_link" /> + link="${prefix}${bay}_gripper_left_proximal_link" /> - + false + link="${prefix}${bay}_gripper_left_proximal_link" /> + link="${prefix}${bay}_gripper_left_distal_link" /> - + false + link="${prefix}${bay}_arm_distal_link" /> + link="${prefix}${bay}_gripper_right_proximal_link" /> - + false + link="${prefix}${bay}_gripper_right_proximal_link" /> + link="${prefix}${bay}_gripper_right_distal_link" /> 0 - --> diff --git a/description/description/urdf/model.urdf.xacro b/description/description/urdf/model.urdf.xacro index 6509730b76..4e872adaa8 100644 --- a/description/description/urdf/model.urdf.xacro +++ b/description/description/urdf/model.urdf.xacro @@ -25,9 +25,13 @@ + + + + - + false @@ -94,7 +98,7 @@ - + false @@ -104,8 +108,8 @@ - - + + @@ -127,7 +131,6 @@ - diff --git a/description/description/urdf/model_carriage.urdf.xacro b/description/description/urdf/model_carriage.urdf.xacro index d9f5e5d357..42bf9d8a4e 100644 --- a/description/description/urdf/model_carriage.urdf.xacro +++ b/description/description/urdf/model_carriage.urdf.xacro @@ -18,20 +18,20 @@ - + - + - + @@ -45,11 +45,11 @@ xyz="0 0 0" rpy="0 0 0" /> + link="${prefix}body" /> + link="${prefix}carriage" /> - + 100000.0 100000.0 0.0 diff --git a/description/description/urdf/model_eps.urdf.xacro b/description/description/urdf/model_eps.urdf.xacro index 37466a825b..8a7c17b7e1 100644 --- a/description/description/urdf/model_eps.urdf.xacro +++ b/description/description/urdf/model_eps.urdf.xacro @@ -21,9 +21,10 @@ > /${ns}/ - 10.0 - 0.05 - 5.0 + 10.0 + 0.04 + 0.05 + 5.0 true true false diff --git a/description/description/urdf/model_flashlights.urdf.xacro b/description/description/urdf/model_flashlights.urdf.xacro index a749c3cab1..123a3a21f7 100644 --- a/description/description/urdf/model_flashlights.urdf.xacro +++ b/description/description/urdf/model_flashlights.urdf.xacro @@ -19,15 +19,19 @@ - + /${ns}/ + flashlight_front + flashlight_front 10.0 0.02 0.02 0.03 - + /${ns}/ + flashlight_aft + flashlight_aft 10.0 0.02 0.02 diff --git a/description/description/urdf/model_monitors.urdf.xacro b/description/description/urdf/model_monitors.urdf.xacro index abec682148..216788c11b 100644 --- a/description/description/urdf/model_monitors.urdf.xacro +++ b/description/description/urdf/model_monitors.urdf.xacro @@ -22,13 +22,13 @@ /${ns}/ - + /${ns}/ /${ns}/ - + /${ns}/ diff --git a/description/description/urdf/sensor_dock_cam.urdf.xacro b/description/description/urdf/sensor_dock_cam.urdf.xacro index cbd286f4ca..d2b8c59965 100644 --- a/description/description/urdf/sensor_dock_cam.urdf.xacro +++ b/description/description/urdf/sensor_dock_cam.urdf.xacro @@ -18,7 +18,7 @@ - + 0 0 0 0 0 0 true diff --git a/description/description/urdf/sensor_haz_cam.urdf.xacro b/description/description/urdf/sensor_haz_cam.urdf.xacro index 9506d35f29..879a13b2b6 100644 --- a/description/description/urdf/sensor_haz_cam.urdf.xacro +++ b/description/description/urdf/sensor_haz_cam.urdf.xacro @@ -18,7 +18,7 @@ - + 0 0 0 0 0 0 true diff --git a/description/description/urdf/sensor_imu.urdf.xacro b/description/description/urdf/sensor_imu.urdf.xacro index 239b70d40e..1a71d487ec 100644 --- a/description/description/urdf/sensor_imu.urdf.xacro +++ b/description/description/urdf/sensor_imu.urdf.xacro @@ -18,7 +18,7 @@ - + 0 0 0 0 0 0 true diff --git a/description/description/urdf/sensor_nav_cam.urdf.xacro b/description/description/urdf/sensor_nav_cam.urdf.xacro index 9b5c5b70af..0478e20faa 100644 --- a/description/description/urdf/sensor_nav_cam.urdf.xacro +++ b/description/description/urdf/sensor_nav_cam.urdf.xacro @@ -18,7 +18,7 @@ - + 0 0 0 0 0 0 true diff --git a/description/description/urdf/sensor_perch_cam.urdf.xacro b/description/description/urdf/sensor_perch_cam.urdf.xacro index 1b380f5386..549d183019 100644 --- a/description/description/urdf/sensor_perch_cam.urdf.xacro +++ b/description/description/urdf/sensor_perch_cam.urdf.xacro @@ -18,7 +18,7 @@ - + 0 0 0 0 0 0 true diff --git a/description/description/urdf/sensor_sci_cam.urdf.xacro b/description/description/urdf/sensor_sci_cam.urdf.xacro deleted file mode 100644 index 2b8c1c4c09..0000000000 --- a/description/description/urdf/sensor_sci_cam.urdf.xacro +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - 0 0 0 0 0 0 - true - 0.0001 - - 1.0821 - - 1024 - 1024 - B8G8R8 - - - 0.1 - 20 - - - 0.0 - - - - /${ns}/ - - - - diff --git a/description/media b/description/media new file mode 160000 index 0000000000..1cb0620121 --- /dev/null +++ b/description/media @@ -0,0 +1 @@ +Subproject commit 1cb0620121099cc848f5a3db480464188dc91ea3 diff --git a/doc/404.html b/doc/404.html new file mode 100644 index 0000000000..6f956717b1 --- /dev/null +++ b/doc/404.html @@ -0,0 +1,47 @@ + + + + Page not found - Astrobee software documentation + + + +
+

404

+ +

Sorry, that page does not exist!

+ +

If you got here by using the menu at the top to change what version of the Astrobee software you want documentation for, please note that different software versions may define different documentation pages in some cases.

+ + +
+ + diff --git a/doc/DoxygenLayout.xml b/doc/DoxygenLayout.xml deleted file mode 100644 index 2bcc65c077..0000000000 --- a/doc/DoxygenLayout.xml +++ /dev/null @@ -1,226 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/doc/README.md b/doc/README.md new file mode 100644 index 0000000000..1e2f82d72b --- /dev/null +++ b/doc/README.md @@ -0,0 +1 @@ +Please visit the [Astrobee Documentation](https://nasa.github.io/astrobee/v/develop/) diff --git a/doc/deliv2csv.py b/doc/deliv2csv.py deleted file mode 100644 index c621f7895b..0000000000 --- a/doc/deliv2csv.py +++ /dev/null @@ -1,25 +0,0 @@ -import ruamel.yaml as yaml -from sys import stdout - -stream = file('deliverables.yaml', 'r') - -deliverables = yaml.round_trip_load(stream) - -ref_keys = deliverables[0].values()[0].keys() - -stdout.write("devliverable") -for k in ref_keys: - stdout.write("\t" + k) -stdout.write("\n") - -for d in deliverables: - stdout.write(d.keys()[0]) - for k in ref_keys: - s = "" - try: - v = d.values()[0][k] - s = str(d.values()[0][k]) - except: - pass - stdout.write("\t"+s) - stdout.write("\n") diff --git a/doc/diagrams/.gitignore b/doc/diagrams/.gitignore index d266e18fab..98f5145d29 100644 --- a/doc/diagrams/.gitignore +++ b/doc/diagrams/.gitignore @@ -4,6 +4,5 @@ dds_ros_bridge.dot executive.dot ff_nodelet.dot gazebo_sensor_plugin_perch_cam.dot -gnc_autocode.dot pmc_actuator_nodelet.dot sparse_mapping.dot diff --git a/doc/diagrams/Makefile b/doc/diagrams/Makefile index 52c3fc6591..a9011a6e1a 100644 --- a/doc/diagrams/Makefile +++ b/doc/diagrams/Makefile @@ -1,4 +1,4 @@ -#/bin/bash +#!/bin/bash # Define the path to the plantuml library # currently just assume that it has been copied locally @@ -13,12 +13,14 @@ pdf_dir = $(gen_dir)/pdf #uml_src = $(wildcard *.puml) uml_src = OperatingState.puml MobilityState.puml PlanExecutionState.puml \ - EpsState.puml FaultSequences.puml overview-horizontal.puml \ + EpsState.puml FaultSequences.puml \ nodes_overview.puml nodes_interactions.puml \ deployment.puml network.puml architecture.puml gnc_overview.puml \ sim_concepts_robot.puml sim_concepts_simulator.puml ars_overview.puml\ notations.puml llp_nodes.puml mlp_nodes.puml hlp_nodes.puml \ packages_linux.puml packages_android.puml color_palettes.puml +# Missing from list in forlder +# nodes_helper.puml colors.puml common.puml plantuml nodes.puml uml_svg = $(uml_src:%.puml=$(svg_dir)/%.svg) uml_png = $(uml_src:%.puml=$(png_dir)/%.png) @@ -29,7 +31,7 @@ dot_src = $(wildcard *.dot) dot_svg = $(dot_src:%.dot=$(svg_dir)/%.svg) dot_png = $(dot_src:%.dot=$(png_dir)/%.png) -plantuml_ver = 1.2019.1 +plantuml_ver = 1.2019.5 plantuml_zip = plantuml-jar-asl-$(plantuml_ver).zip plantuml_url = https://sourceforge.net/projects/plantuml/files/$(plantuml_ver)/ diff --git a/doc/diagrams/architecture.puml b/doc/diagrams/architecture.puml index 884001d8ea..ff1c837d74 100644 --- a/doc/diagrams/architecture.puml +++ b/doc/diagrams/architecture.puml @@ -65,7 +65,6 @@ frame localization { frame gnc { [CTL] [FAM] - [EKF] () "/gnc/control\nff_msgs::ControlAction" <> as ControlAction () "/gnc/ekf/set_input\nff_msgs::SetEkfInput" <> as EkfSetInput () "/gnc/ekf\nff_msgs::EkfState" <> as EkfState @@ -94,7 +93,7 @@ EXECUTIVE <<-->> ArmAction EXECUTIVE --> InertiaStamped InertiaStamped --> FAM InertiaStamped --> CTL -InertiaStamped --> EKF +InertiaStamped --> LOC DockAction -- DOCK @@ -103,14 +102,14 @@ DOCK <<-->> SwitchAction PERCH <<-->> SwitchAction SwitchAction -- LOCALIZATION_MANAGER LOCALIZATION_MANAGER --> EkfSetInput -EkfSetInput --|> EKF +EkfSetInput --|> LOC DOCK <<-->> MotionAction PERCH <<-->> MotionAction PERCH <<-->> ArmAction MotionAction -- CHOREOGRAPHER CHOREOGRAPHER <<-->> ControlAction CHOREOGRAPHER --> SpeedGain -SpeedGain --> EKF +SpeedGain --> LOC SpeedGain --> FAM ControlAction -- CTL CTL --> WrenchStamped @@ -123,9 +122,9 @@ ArmAction -- ARM WrenchStamped --> FAM FAM --> PmcCommand ARM --> JointGoals -EKF --> PoseStamped -EKF --> TwistStamped -EKF --> EkfState +LOC --> PoseStamped +LOC --> TwistStamped +LOC --> EkfState TwistStamped --> CTL PoseStamped --> CTL JointStates --> ARM diff --git a/doc/diagrams/gnc_overview.puml b/doc/diagrams/gnc_overview.puml index 573e4036db..ecf7bb313f 100644 --- a/doc/diagrams/gnc_overview.puml +++ b/doc/diagrams/gnc_overview.puml @@ -11,7 +11,6 @@ frame mob { } frame gnc { - gnc_node(EKF) gnc_node(CTL) gnc_node(FAM) action(Control, ff_msgs::ControlAction, /gnc/control) @@ -31,20 +30,20 @@ message(Landmarks, ff_msgs::VisualLandmarks, "/loc/{ml,ar,of}/features") latching_msg(Inertia, geometry_msgs::InertiaStamped, /mob/inertia) latching_msg(FlightMode, ff_msgs::FlightMode, /mob/flight_mode) -Landmarks -r-> EKF : " " +Landmarks -r-> LOC : " " FlightMode --> FAM FlightMode --> choregrapher -Inertia -up-> EKF +Inertia -up-> LOC Inertia -up-> CTL Inertia -up-> FAM -'EKF -r-> Pose -'EKF -r-> Vels +'LOC -r-> Pose +'LOC -r-> Vels 'Pose -r-> CTL 'Vels -r-> CTL -EKF -r-> CTL : "/loc/{pose,twist}\n[geometry_msgs::{Pose,Twist}Stamped]" +LOC -r-> CTL : "/loc/{pose,twist}\n[geometry_msgs::{Pose,Twist}Stamped]" CTL -r-> FAM : "/gnc/ctl/command\n[ff_msgs::FamCommand]" 'CTL -r-> Wrench : " " 'Wrench -r-> FAM : " " diff --git a/doc/diagrams/nodes.puml b/doc/diagrams/nodes.puml index 4d50d29f26..96dcb8a92a 100644 --- a/doc/diagrams/nodes.puml +++ b/doc/diagrams/nodes.puml @@ -110,7 +110,6 @@ node LLP { end_manager() !endif start_manager("GN&C") - gnc_node([ekf]) gnc_node([ctl]) gnc_node([fam]) end_manager() diff --git a/doc/diagrams/notations.puml b/doc/diagrams/notations.puml index 0116078556..6127e547a3 100644 --- a/doc/diagrams/notations.puml +++ b/doc/diagrams/notations.puml @@ -27,10 +27,8 @@ node "Processor (LLP,MLP,HLP)" { folder "Interfaces (Messages, Services, Actions) Legend" as l2 { - gnc_node(EKF) gnc_node(CTL) message(EkfState, ff_msgs::EkfState, /gnc/ekf) - EKF --> EkfState EkfState --> CTL action(DockAction, ff_msgs::DockAction, /proc/dock) diff --git a/doc/diagrams/sim_concepts_robot.puml b/doc/diagrams/sim_concepts_robot.puml index e60af48c92..15f435236c 100644 --- a/doc/diagrams/sim_concepts_robot.puml +++ b/doc/diagrams/sim_concepts_robot.puml @@ -27,13 +27,12 @@ folder "Astrobee Robot" { node LLP <> COLOR_CPU { 'frame GNC { - gnc_node(EKF) gnc_node(CTL) gnc_node(FAM) '} drv_node(epson_imu) drv_node(pmc_actuator) - EKF -> CTL + LOC -> CTL CTL -> FAM message(PmcCommand, ff_hw_msgs::PmcCommand, /hw/pmc/command) message(ImuTelem, sensor_msgs::Imu, /hw/imu) @@ -52,12 +51,12 @@ folder "Astrobee Robot" { NavImage --> image_sampler sparse_mapping --> Features - Features ---> EKF + Features ---> LOC image_sampler --> ImgSample epson_imu --> ImuTelem - ImuTelem -> EKF + ImuTelem -> LOC FAM -> PmcCommand PmcCommand --> pmc_actuator diff --git a/doc/diagrams/sim_concepts_simulator.puml b/doc/diagrams/sim_concepts_simulator.puml index 86623f3901..debc5e5334 100644 --- a/doc/diagrams/sim_concepts_simulator.puml +++ b/doc/diagrams/sim_concepts_simulator.puml @@ -25,11 +25,10 @@ folder "Development Computer" { mgt_node(image_sampler) loc_node("real robot\nsparse_mapping\nnode not running") 'frame GNC { - gnc_node(EKF) gnc_node(CTL) gnc_node(FAM) '} - EKF -> CTL + LOC -> CTL CTL -> FAM } @@ -43,12 +42,12 @@ folder "Development Computer" { NavImage --> image_sampler sparse_mapping --> Features - Features ---> EKF + Features ---> LOC image_sampler --> ImgSample IMU --> ImuTelem - ImuTelem --> EKF + ImuTelem --> LOC FAM -up-> PmcCommand PmcCommand -up-> PMC diff --git a/doc/documentation.html b/doc/documentation.html deleted file mode 120000 index 5bc112887b..0000000000 --- a/doc/documentation.html +++ /dev/null @@ -1 +0,0 @@ -html/index.html \ No newline at end of file diff --git a/doc/documentation.html b/doc/documentation.html new file mode 100644 index 0000000000..9382353efc --- /dev/null +++ b/doc/documentation.html @@ -0,0 +1,2 @@ + + diff --git a/doc/general_documentation/INSTALL.md b/doc/general_documentation/INSTALL.md new file mode 100644 index 0000000000..86418be858 --- /dev/null +++ b/doc/general_documentation/INSTALL.md @@ -0,0 +1,148 @@ +\page install-nonNASA Install for general users + +# Usage instructions for non-NASA users + +Make sure your system is up-to-date and: + + sudo apt-get install build-essential git + +## Machine setup + +### Checkout the project source code + +At this point you need to decide where you'd like to put the source code +(`ASTROBEE_WS`) on your machine: + + export ASTROBEE_WS=$HOME/astrobee + +First, clone the flight software repository and media: + + git clone https://github.com/nasa/astrobee.git $ASTROBEE_WS/src + pushd $ASTROBEE_WS/src + git submodule update --init --depth 1 description/media + popd + +If you are planning to work with guest science code, you will also need the +`astrobee_android` repository. You should checkout the repository as a submodule: + + git submodule update --init --depth 1 submodules/android + +### Dependencies + +Next, install all required dependencies: + +*Note: `root` access is necessary to install the compiled debian packages below* + +*Note: Before running this please ensure that your system is completely updated +by running 'sudo apt-get update' and then 'sudo apt-get upgrade'* + + pushd $ASTROBEE_WS + cd src/scripts/setup + ./add_ros_repository.sh + sudo apt-get update + cd debians + ./build_install_debians.sh + cd ../ + ./install_desktop_packages.sh + sudo rosdep init + rosdep update + popd + + +**Important**: you can safely ignore the following error messages, as they are simply letting you know that certain libraries cannot be found. These libraries are for internal NASA use only, and are not required by public users provided that software is launched with DDS disabled. + + E: Unable to locate package libroyale1 + E: Unable to locate package rti + E: Unable to locate package libmiro0 + E: Unable to locate package libsoracore1 + E: Unable to locate package libroyale-dev + E: Unable to locate package rti-dev + E: Unable to locate package libsoracore-dev + E: Unable to locate package libmiro-dev + +## Configuring the build + +### Note for the build setup + +When compiling, the `$WORKSPACE_PATH` defines where the `devel`, `build`, `logs` and +`install` directories are created. If you want to customize the `install` path then the +`$INSTALL_PATH` can be defined. By default, the configure script uses the following paths: + + - native build path: `$ASTROBEE_WS/build` + - native install path: `$ASTROBEE_WS/install` + +If you are satisfied with these paths, you can invoke the `configure.sh` without +the `-p` and `-w` options. For the simplicity of the instructions below, +we assume that `$WORKSPACE_PATH` and `$INSTALL_PATH` contain the location of the +build and install path. For example: + + export WORKSPACE_PATH=$ASTROBEE_WS + export INSTALL_PATH=$ASTROBEE_WS/install + +### Native build + +The configure script prepares your build directory for compiling the code. Note +that `configure.sh` is simply a wrapper around CMake that provides an easy way +of turning on and off options. To see which options are supported, simply run +`configure.sh -h`. + + pushd $ASTROBEE_WS + ./src/scripts/configure.sh -l -F -D + source ~/.bashrc + popd + +If you run a Zsh session, then + + pushd $ASTROBEE_WS + ./src/scripts/configure.sh -l -F -D + source ~/.zshrc + popd + +The configure script modifies your ``.bashrc``/``.zshrc`` to source ``setup.bash``/``setup.zsh`` for +the current ROS distribution and to set CMAKE_PREFIX_PATH. It is suggested +to examine it and see if all changes were made correctly. + +If you want to explicitly specify the workspace and install directories, use +instead: + + ./src/scripts/configure.sh -l -F -D -p $INSTALL_PATH -w $WORKSPACE_PATH + +*Note: If a workspace is specified but not an explicit install directory, +install location will be $WORKSPACE_PATH/install.* + +*Note: Make sure you use the -F and -D flags. If these flags are not used, the +code will not compile. The -F flag is used to turn off building the Picoflex. +This is only needed for running on the robot and is not needed for the +simulator. The -D is used to turn off building the dds bridge. The bridge is +used to communicate with our ground data system and is also not needed for the +simulator.* + +## Building the code + +To build, run `catkin build` in the `$WORKSPACE_PATH`. Note that depending on your host +machine, this might take in the order of tens of minutes to complete the first +time round. Future builds will be faster, as only changes to the code are +rebuilt, and not the entire code base. + + pushd $ASTROBEE_WS + catkin build + popd + +Note: In low-memory systems, it is common to run out of memory while trying to compile +ARS, which triggers a compilation error mentioning "arm-linux-gnueabihf-g++: internal +compiler error: Killed (program cc1plus)". A contributing factor is that +catkin build by default runs multiple jobs in parallel based on the number of cores +available in your environment, and all of these jobs draw on the same memory resources. +If you run into this compile error, try compiling again with the -j1 option to restrict +catkin to running one job at a time. + +For more information on running the simulator and moving the robot, please see the \ref running-the-sim. + + +## Cross Compiling + +Please contact your Astrobee point of contact if you need to cross compile the +code. + +For more information on running the simulator and moving the robot, please see +the \ref sim-readme. diff --git a/doc/general_documentation/NASA_INSTALL.md b/doc/general_documentation/NASA_INSTALL.md new file mode 100644 index 0000000000..a1a707c2be --- /dev/null +++ b/doc/general_documentation/NASA_INSTALL.md @@ -0,0 +1,268 @@ +\page install-NASA Install for NASA users + +# Usage instructions for NASA users + +Make sure your system is up-to-date and: + + sudo apt-get install build-essential git + +## Machine setup + +### Username + +If you are using a VM with a username that does not match your NDC username, +please configure the following variable: + + export NDC_USERNAME=your_ndc_username + + +### Access to the Astrobee Debian server + +The custom debian packages are currently distributed by the +`astrobee.ndc.nasa.gov server`. This server is currently on the ARC TI +private network. It is *critical* to be able to reach this server to +install the pre-built custom debian. If none of the solutions below +allow you to reach `astrobee.ndc.nasa.gov`, you can use the +instructions on how to build the Debian dependencies manually +following the INSTALL.md instructions (Dependencies section). + +#### If on the ARC TI private network + +This is the typical case for all wired computers in ARC TI, and simplifies +your life greatly. + +Verify that you are in this situation with the command below should succeed +(certificate will be added later; remove the Release.gpg file after being fetched). + + wget -v --no-check-certificate https://astrobee.ndc.nasa.gov/software_new/dists/focal/Release.gpg + +Before running the scripts in `scripts/setup` below, set this variable: + + export NO_TUNNEL=1 + +#### If not on the ARC TI private network + +If you are outside the NASA ARC private network, to reach `astrobee.ndc.nasa.gov`: + + 1. Use VPN to act like if you were inside the ARC TI private network and + obtain the correct kerberos credentials inside the VM with the following + command (note the capitalization): `kinit $NDC_USERNAME@NDC.NASA.GOV` + 2. setup your `.ssh/config` to do ssh forwarding. A tutorial on this method + is available at: https://babelfish.arc.nasa.gov/trac/freeflyer/wiki/SSHSetup + +For either solution, please verify that you can SSH to `m.ndc.nasa.gov` without +entering your password (`m` can be used to tunnel to `astrobee.ndc.nasa.gov`): + + `ssh $NDC_USERNAME@m.ndc.nasa.gov` + +The command should succeed without entering your password. Once this is verified, +exit this session on `m` with `+D`. + +- These notes apply to `install_desktop_16.04_packages.sh` and `make_chroot.sh` + + +### Checkout the project source code + +At this point you need to decide where you'd like to put the source code +(`ASTROBEE_WS`) on your machine: + + export ASTROBEE_WS=$HOME/astrobee + +First, clone the flight software repository: + + git clone https://github.com/nasa/astrobee.git --branch develop $ASTROBEE_WS/src + pushd $ASTROBEE_WS/src + git submodule update --init --depth 1 description/media + git submodule update --init --depth 1 submodules/platform + +You can either choose which optional submodules to clone and log depth with: + + git submodule update --init --depth 1 submodules/android + git submodule update --init --depth 1 submodules/avionics + +Or checkout all the submodules as: + + git submodule update --init --depth 1 + +(Note: re-enter your username and password for every submodule that is cloned) +The android module is necessary for guest science code; the avionics and platform +module is used when cross-compiling to test on the robot hardware. + +### Dependencies + +Next, install all required dependencies: + +*Note: `root` access is necessary to install the compiled debian packages below* + +*Note: Before running this please ensure that your system is completely updated +by running 'sudo apt-get update' and then 'sudo apt-get upgrade'* + + pushd $ASTROBEE_WS + cd src/scripts/setup + ./add_local_repository.sh + ./add_ros_repository.sh + ./install_desktop_packages.sh + popd + + +## Configuring the build + +### Note for build setup + +When compiling, the `$WORKSPACE_PATH` defines where the `devel`, `build`, `logs` and +`install` directories are created. If you want to customize the `install` path then the +`$INSTALL_PATH` can be defined. By default, the configure script uses the following paths: + + - native build path (WORKSPACE_PATH): `$ASTROBEE_WS` + - native install path (INSTALL_PATH): `$ASTROBEE_WS/install` + - armhf build path (WORKSPACE_PATH): `$ASTROBEE_WS/armhf` + - armhf install path (INSTALL_PATH): `$ASTROBEE_WS/armhf/opt/astrobee` + +You should set these values in your shell. + +If you are satisfied with these paths, you can invoke the `configure.sh` without +the `-p` and `-w` options. For the simplicity of the instructions below, +we assume that `$WORKSPACE_PATH` and `$INSTALL_PATH` contain the location of the +build and install path for either `native` or `armhf` platforms. + +## Native vs Cross-Compile + +At this point you need to decide whether you'd like to compile natively +[`native`] (run code against a simulator) or cross-compile for an ARM +target [`armhf`] (run the code on the robot itself). Please skip to the +relevant subsection. + +## Native - Running the code on your computer with simulator + +## Native build + +The configure script prepares your build directory for compiling the code. Note +that `configure.sh` is simply a wrapper around CMake that provides an easy way +of turning on and off options. To see which options are supported, simply run +`configure.sh -h`. + + pushd $ASTROBEE_WS + ./src/scripts/configure.sh -l + source ~/.bashrc + popd + +The configure script modifies your ``.bashrc`` to source ``setup.bash`` for +the current ROS distribution and to set CMAKE_PREFIX_PATH. It is suggested +to examine it and see if all changes were made correctly. + +If you want to explicitly specify the workspace and/or install directories, use +instead: + + ./scripts/configure.sh -l -p $INSTALL_PATH -w $WORKSPACE_PATH + +*Note: If a workspace is specified but not an explicit install directory, +install location will be $WORKSPACE_PATH/install.* + + +### Building the code + +To build, run `catkin build` in the `$WORKSPACE_PATH`. Note that depending on your host +machine, this might take in the order of tens of minutes to complete the first +time round. Future builds will be faster, as only changes to the code are +rebuilt, and not the entire code base. + + pushd $ASTROBEE_WS + catkin build + popd + +Note: In low-memory systems, it is common to run out of memory while trying to compile +ARS, which triggers a compilation error mentioning "arm-linux-gnueabihf-g++: internal +compiler error: Killed (program cc1plus)". A contributing factor is that +catkin build by default runs multiple jobs in parallel based on the number of cores +available in your environment, and all of these jobs draw on the same memory resources. +If you run into this compile error, try compiling again with the -j1 option to restrict +catkin to running one job at a time. + +For more information on running the simulator and moving the robot, please see the \ref running-the-sim. + + +## Cross-compile - Running the code on a real robot + +In order to do this, you will need to follow the cross-compile build +instructions. + +### Cross-compile setup + +If you are planning to compile code to run on the robot hardware, you will need +to install a cross-compile chroot and toolchain. Select two directories for +these: + + export ARMHF_CHROOT_DIR=$HOME/arm_cross/rootfs + export ARMHF_TOOLCHAIN=$HOME/arm_cross/toolchain/gcc + +Append these lines to your .bashrc file, as you will need these two variables +every time you cross compile. + +Next, download the cross toolchain and install the chroot: + + mkdir -p $ARMHF_TOOLCHAIN + cd $HOME/arm_cross + + DIST=$(. /etc/os-release && echo $UBUNTU_CODENAME) + if [ ${DIST} == "kinetic" ]; then + # Use pre-built toolchain for 16.04 + $ASTROBEE_WS/src/submodules/platform/fetch_toolchain.sh + else + # Use packaged toolchain for 20.04 + sudo apt install -y gcc-arm-linux-gnueabihf g++-arm-linux-gnueabihf + fi + sudo apt install -y qemu-user-static multistrap + + $ASTROBEE_WS/src/submodules/platform/rootfs/make_chroot.sh ${DIST} dev $ARMHF_CHROOT_DIR + +### Cross-compile build + +Cross compiling for the robot follows the same process, except the configure +script takes a `-a` flag instead of `-l`. + + pushd $ASTROBEE_WS + ./src/scripts/configure.sh -a + popd + +Or with explicit build and install paths: + + ./scripts/configure.sh -a -p $INSTALL_PATH -w $WORKSPACE_PATH + +*Warning: `$INSTALL_PATH` and `$WORKSPACE_PATH` used for cross compiling HAVE to be +different than the paths for native build! See above for the default values +for these.* + + Once the code has been built, it also installs the code to +a singular location. CMake remembers what `$INSTALL_PATH` you specified, and +will copy all products into this directory. + +### Install the code on the robot + +Once the installation has completed, copy the install directory to the robot. +This script assumes that you are connected to the Astrobee network, as it uses +rsync to copy the install directory to `~/armhf` on the two processors. It +takes the robot name as an argument. Here we use `p4d`. + + pushd $ASTROBEE_WS + ./src/scripts/install_to_astrobee.sh $INSTALL_PATH p4d + popd + +Here, p4d is the name of the robot, which may be different in your case. + +You are now ready to run the code. This code launches a visualization tool, +which starts the flight software as a background process. + + pushd $ASTROBEE_WS + python ./src/tools/gnc_visualizer/scripts/visualizer --proto4 + popd + +## Switching build profiles + +To alternate between native and armhf profiles: + + catkin profile set native + catkin profile set armhf + +# Further information + +Please refer to the [wiki](https://github.com/nasa/astrobee/wiki). diff --git a/doc/general_documentation/adding_a_command.md b/doc/general_documentation/adding_a_command.md new file mode 100644 index 0000000000..74d8ac3c9b --- /dev/null +++ b/doc/general_documentation/adding_a_command.md @@ -0,0 +1,85 @@ +\page adding_a_command Adding a ground command to the Astrobee FSW + +# Introduction + +Adding a ground command to the flight software is a pretty involved task so +please make sure you have a full understanding of the system before you attempt +it. There are four parts to adding a command: adding the command to the command +spreedsheet, adding the command to the plan schema, generating the fsw and +Android files, and adding the command to the executive. Please make sure to +complete all four parts to ensure your command is fully intergrated into the +system. + +# Update the Commands Spreadsheet + +The command spreadsheet can be found in freeflyer_docs/FlightSoftware and is +named commands.xml. This spreadsheet keeps track of the ground commands the fsw +accepts, which commands can run while other commands are running, the operating +and mobility states the commands are accepted in, whether the command is accept +in a plan or not, and the status of the command in the fsw. Please make sure to +add your command to each of the relevant sheets. If you have questions, please +contact Katie Hamilton. + +# Add Command to the Plan Schema + +The plan schema can be found in astrobee/commands. Please add your command to +the freeFlyerPlanSchema.json file. Make sure to follow the json format for a +command. + +# File Generation + +When adding a command, some fsw and GDS files need to be updated. There are +scripts that will update everything for you. After adding your command to the +plan schema, please follow the next three subsections to generate the files. + +## Setup + +The scripts for auto-generating source code files from the command dictionary +depend on the `xgds_planner2` library, which can be installed as follows: + + sudo pip install git+https://github.com/trey0/xgds_planner2.git@just_xpjson + +## Generate FSW files + +To generate the fsw files and copy them to correct location, please run the +following line from the top level directory of astrobee: + + ./scripts/build/processCommandSchema.sh + +After running this script, please make sure your changes compile. To do this, +navigate to the folder you build the fsw in, rebuild the cache, and then compile +the code. Don't forget to commit and push your changes. If you want your command +incorporated into GDS, please let whoever is in charge of GDS know that the +command constants idl has changed. + +## Generate Android files + +Make sure you have the Astrobee Android repo. If you don't, please see the +install instructions on how to get it. Navigate to the astrobee api scripts +folder in the astrobee Android repo (astrobee_api/scripts). These scripts assume +that the android repo is located in the fsw submodules folder. If it is located +outside of the fsw, please set the $SOURCE_PATH environment variable to the fsw +location. Also if you added a new enum command type, you will need to add an +include line to both the genBaseRobot and genBaseRobotImpl scripts. To generate +the android files, please run the following in the scripts folder: + + cd astrobee_api/scripts + # if needed, set the source path + export SOURCE_PATH=path/to/astrobee + ./processCommandSchema.sh + +The script will copy the generated files to the correct locations. Please make +sure that the astrobee_api builds with no errors. To do this, go to the top +level astrobee api directory and build the code: + + cd astrobee_api/ + ./gradlew build + +Once everything compiles, make sure you commit and push your changes. + + +# Add to Executive + +The last step is to add the command to the executive. Please make sure you have +a full understanding of the executive and operating state classes before adding +the command. If you have questions, please contact Katie Hamilton. diff --git a/doc/general_documentation/astrobee_usage.md b/doc/general_documentation/astrobee_usage.md new file mode 100644 index 0000000000..9bb31c1dcc --- /dev/null +++ b/doc/general_documentation/astrobee_usage.md @@ -0,0 +1,28 @@ + +# Tutorials + +\subpage astrobee + +\subpage running-the-sim + +\subpage advanced-sim-info + +\subpage teleop + +\subpage astrobee-code-style + +\subpage conventions + +\subpage new_robot + +\subpage doc + +\subpage release + +\subpage managing_debians + +\subpage adding_a_command + +\subpage using_telemetry + +\subpage maintaining_telemetry diff --git a/doc/general_documentation/code_style.md b/doc/general_documentation/code_style.md new file mode 100644 index 0000000000..d33529d38b --- /dev/null +++ b/doc/general_documentation/code_style.md @@ -0,0 +1,3 @@ +\page astrobee-code-style Astrobee code style + +The Astrobee code uses [cpplint](https://en.wikipedia.org/wiki/Cpplint) to enforce a consistent and uniform coding style. Code which fails this tool cannot be committed with Git. It is suggested that the [clang-format-8](https://launchpad.net/ubuntu/focal/+package/clang-format-8) program be installed on your machine. The *git commit* command will invoke this tool which will fix most (but not all) of the formatting errors using as a guide the ``.clang-format`` style file at the base of the Astrobee repository. diff --git a/doc/general_documentation/command_dictionary.md b/doc/general_documentation/command_dictionary.md new file mode 100644 index 0000000000..ceeb897de4 --- /dev/null +++ b/doc/general_documentation/command_dictionary.md @@ -0,0 +1,5 @@ +\page command_dictionary Command Dictionary + +These are the ground commands to the flight Software: + +\htmlinclude AstrobeeCommandDictionary.html diff --git a/doc/conventions.md b/doc/general_documentation/conventions.md similarity index 100% rename from doc/conventions.md rename to doc/general_documentation/conventions.md diff --git a/doc/general_documentation/documentation.md b/doc/general_documentation/documentation.md new file mode 100644 index 0000000000..c42b5e430c --- /dev/null +++ b/doc/general_documentation/documentation.md @@ -0,0 +1,96 @@ +\page doc Tools used for FSW documentation + +## Code documentation + +Code should be documented using doxygen. + +*Add here an explanation of the overall structure and how to create subsystem + pages (or just provide a default template).* + +## UML diagraming + +Astrobee UML diagrams are created using [plantuml](http://plantuml.com/) that +transform text files into beautiful UML 2 compliant diagrams. + +Specific notations used in the diagrams to represent ROS communication schemes +is described in: +[Astrobee UML Notations](doc/diagrams/notations.png) + +Despite the ads overloaded website, plantuml is free and [open +source](https://github.com/plantuml/plantuml). The plantuml [Reference +Guide](http://plantuml.com/PlantUML_Language_Reference_Guide.pdf) walks to all +the diagrams types. + +Plantuml relies on [Graphviz](http://www.graphviz.org/) that should be already +installed since ROS is also using this tool to render runtime node/topic graphs. +There are many ways to write/generate plantuml diagrams: +http://plantuml.com/running. Choose the integration that best suit you, but vim + +command line java works perfectly. + +Note: some diagrams are not generated correctly when using graphviz 2.40 (layout +stretched vertically). So for now please use graphviz 2.38. + +Note: to re-active the 2.38 that keep being updated on Mac: + + sudo port activate graphviz @2.38.0_3+pangocairo + +To generate the diagrams from the `doc\diagrams` directory: +``` +make +# or to get PNG versions: +make png +``` + +## ROS Messages, Services and Actions + +Our toolchain incudes Doxygen python filters to interpret `*.msg`, `*.action` +and `*.srv` files. Internally, the filters convert the messages, topics and +action files to markdown, and renders them in a hierarchy of Doxygen 'module' +pages. So, they will appear in the same structure as the written markdown +documentation, and not as classes / structs or other types. + +In order for the files to be interpreted correctly, one needs to abide by a +couple of simple rules. +1. All header lines must begin with a '#' comment character. The end of the + header is signified by an empty line. All comments following the newline will + be treated as leading comments for the first declared variable. +1. Any header lines containing the word 'copyright' will be stripped from the + documentation. All hashes ans newlines will also be stripped from the + documentation. You are therefore discouraged from including any comment that + relies on formatting using tabs or spaces. +1. You may document a variable above (leading) or in-line (to the right of) it's + declaration. However, you cannot do both! An inline comment will mute a + leading comment. + +# ROS Unit Tests + +The ROS test framework (rostest) is build upon the Google test framework. The +idea behind a ROS test is that it first launches a collection of ROS nodes, and +then starts the unit test, which runs tests against the ROS nodes. + +Writing a unit test for a package involves modifying the package's +`CMakeLists.txt` and `package.xml` files, and writing a collection of test +cases. To see an example, have a look in the `./tools/visualeyez` folder. + +To build all ROS tests, use the following: + + make -j6 tests + +To execute all ROS tests run the following: + + make -j6 run_tests + +Note that by default all ROS console messages (ROS_INFO, ROS_WARN, etc.) called +from within the test cases are suppressed. To print all messages, as well as a +more comprehensive test result, add the `--text` argument: +``` +rostest <*.test> --text +``` + +Sometimes you want to debug your unit tests, but it's unclear on which port +rostest started the ROS master. A simpler approach is to manually start the ROS +master using `roscore` and then latch the rostest to this master with the +`reuse-master` argument as follows: +``` +rostest <*.test> --reuse-master +``` \ No newline at end of file diff --git a/doc/general_documentation/flight_release.md b/doc/general_documentation/flight_release.md new file mode 100644 index 0000000000..18ebc2f7e7 --- /dev/null +++ b/doc/general_documentation/flight_release.md @@ -0,0 +1,98 @@ +\page release Creating a Flight Release + +This page describes the steps required to complete a flight release. It assumes +that your environment is configured to cross-compile the fsw for the `armhf` +architecture. + +## Create a Release Branch + +Release branches will be created on a user's fork. To make a release branch, +checkout and update develop. Then make a release branch off of develop. Please +name the branch release-0.x.x. + + +Please note that creating a release branch and specifying a version number +indicates that the code on the branch is frozen with respect to further +feature changes. However, as you identify bugs during lab testing before +deployment to the ISS, you can still apply bug fixes on that release branch +without incrementing the version number. Of course, that means that as you +iterate, you might end up generating multiple debs with the same version number. +For configuration management purposes, when checking whether two deployments +used identical debs, you should always compare their git commit hashes rather +than their version numbers. + +## Update the Release Version + +The release version must be updated before creating the Debian. Please use +the update release script to do this. Before running the script, get a list of +features that have been added since the last release. This list will need to be +added to the change log that gets edited when the script is run. Also, in the +change log, change UNRELEASED to testing and change the email address to the +astrobee fsw email address as in previous log entries. + + ./scripts/setup/debians/update_release.sh 0.x.x + + +*Note: Make sure that your EDITOR variable is set to your preferred editor +before invoking this script. Using vim seems to generate an unimportant error.* + +## Commit and Push Version Change + +Please make sure to commit and push all the files that were changed. + + +## Create the Debians + +Your environment needs to be configured properly to cross-compile Astrobee +Robot Software for the `armf` architecture. See +[NASA Install documentation](NASA_INSTALL.md). + + ./scripts/build/build_debian.sh + + +## Testing the Release + +On confluence, there is a [release testing procedure](https://babelfish.arc.nasa.gov/confluence/display/FFFSW/IRG-FFTEST302+-+Astrobee+Release+Testing). +This procedure will walk you through all the testing that needs to be done to +make sure the debian is flight ready. Please create a copy of the procedure by +clicking on the ellipsis at the top right corner of the page and selecting copy. +In the popup, in the 'Parent page' text box, please enter 'Executed Procedures' +and click 'Copy'. Remove 'Copy of' from the title and add the version and date +after the test number such that it looks like +'IRG-FFTEST302 - 0.x.x -YYYY-MM-DD - Astrobee Release Testing'. Be sure to fill +out the 'As-Ran Summary' table and the 'Result' and 'Notes' column for every +step. After running through the procedure, please provide a brief summary of +your results in the +[release testing results table](https://babelfish.arc.nasa.gov/confluence/display/FFFSW/Release+Testing+Results). + +## Fix Bugs + +If there are bugs that arise during testing, please fix the bugs, push the +fixes, recreate the debians, and repeat the release testing procedure. + +## Finish Release + +Once the debians pass all the required testing, it is ready to become an +official flight release. Please make one pull request on astrobee develop. +Set Brian, Katie, or Marina as the reviewer. They will review the request +and do the merging into develop and master. You will also need to copy the +debians to a temporary location on volar. + + scp astrobee0_0.x.x_armhf.deb \ + @volar:/home/p-free-flyer/free-flyer/FSW/ars_debs/release_candidate/ + scp astrobee-config_0.x.x_all.deb \ + @volar:/home/p-free-flyer/free-flyer/FSW/ars_debs/release_candidate/ + scp astrobee-dev_0.x.x_armhf.deb \ + @volar:/home/p-free-flyer/free-flyer/FSW/ars_debs/release_candidate/ + +After the debians have been copied to volar, make sure the group permissions are +set to read and right. Finally, please email Ruben Garcia Ruiz so that he can +sign it and stage it. + +### Merging into Develop and Master + +This section is for reviewers only! After reviewing the pull request and +verifying that the release passed all sections of the release testing procedure, +please merge the pull request into develop. You will then need to merge the +develop branch into master (squash and merge). After this merge, please create +a github release and merge master back into develop (merge commit). diff --git a/doc/general_documentation/maintaining_telemetry.md b/doc/general_documentation/maintaining_telemetry.md new file mode 100644 index 0000000000..231a0782c8 --- /dev/null +++ b/doc/general_documentation/maintaining_telemetry.md @@ -0,0 +1,314 @@ + +\page maintaining_telemetry Maintaining Astrobee Telemetry Backward Compatibility + +## Motivation and scope + +The Astrobee Facility has built up a valuable archive of telemetry bags +recorded by Astrobee robots on the ISS since commissioning in April +2019. Astrobee's flight software (FSW) has also changed substantially over +that time, including several changes to the definitions of the message +types contained in the telemetry bags. As a result, it has become more +difficult over time to analyze or play back older legacy bags with the +latest version of FSW. + +This document establishes objectives for backward compatibility and +proposes a maintenance approach. + +## Telemetry maintenance objectives + +1. Astrobee developers should be able to `rosbag play` archived Astrobee + ISS telemetry bags with the latest FSW, as a way to test the software + in the ISS environment. For example, localization system development + has relied heavily on bag playback as a way to iterate and overcome + challenges related to the complex ISS interior that can't be + replicated on the ground. + +2. External users without prior ROS experience should be able to analyze + archived Astrobee ISS telemetry bags with minimal training by + following simple, documented processes. These users could be Astrobee + guest scientists or could be third parties with less Astrobee + background and insider access who are opportunistically finding new + uses for our archived data. (For example, making new observations in + our ISS imagery.) + +3. We also have a priority list of bad outcomes to avoid: + + - Misleading users. If the message semantics has changed such that + trying to interpret legacy messages with the new semantics is + actively misleading, this is the worst case. + + - Breaking all messages. Some bag analysis tools are sensitive and + crash if the bag has even a single problematic message definition. + We should strive hard to avoid this problem. + + - Loss of ability to analyze specific message types or specific + fields. This may be more acceptable, depending on the specific + user needs. + +4. We should minimize the burden on Astrobee developers and the Astrobee + Facility ops team required to realize these goals. + +## Telemetry maintenance requirements + +1. Developers should have guidance about steps they can take to maintain + legacy bag compatibility with minimum effort (this document). + +2. Any required bag processing should be fully automated to the greatest + extent possible, minimizing burden on the Astrobee Facility and + users. + +3. Bags processed for developers or external users to work with should + pass `rosbag check`. This tool applies a number of tests, but is + principally designed to check that the bag's messages can be played + back with the latest version of the FSW ROS environment (the message + definitions are compatible for all of the message types recorded in + the bag). + +4. Along the same lines, another test bags should pass is `rostopic echo + -b -a`. This forces message deserialization. + +4. Where message semantics have changed, we should either remap the + legacy message contents to accurately reflect the latest semantics, + or rename the legacy message type to clarify that it can't be + interpreted like the current type. + +5. ROS message types defined by guest scientists: + + - If legacy bag compatibility is required for these messages, it + should be the responsibility of the guest science developers. + + - As a fallback, the FSW team should provide a tool for filtering + out no-longer-compatible guest science messages, in case the other + messages in the bag are still valuable. + +6. Out of scope: The primary focus is on legacy messages recorded in ISS + telemetry bags, or particularly important ground experiments. We + will avoid investing effort in backward compatibility with old + message definitions that were never recorded on the ISS. For example, + certain older versions of Astrobee perching messages were tested on + the ground, but never deployed on the ISS, and in those cases + backward compatibility is not important. + +## Telemetry maintenance approach + +We will provide a fully automated script that can fix arbitrary legacy +ISS Astrobee bags to be compatible with the latest FSW. + +The starting point for the script is the standard [`rosbag` +migration](http://wiki.ros.org/rosbag/migration) system provided with +ROS. If we invest in maintaining the necessary `*.bmr` bag migration +rules, the `rosbag fix` script can fix most but not all message +definition problems. + +One remaining problem is incomplete message definitions in messages +published by `rosjava` nodes. We have a script that can fix these +message definitions. It must be run before `rosbag fix` in order to +avoid a crash. + +The other remaining problem arises when there are substantial changes in +a message definition that make migration to the new definition +problematic. For example: + +- If a field has been deleted, any attempt to migrate an old message + instance to the new definition will necessarily involve data loss. + +- If the change is sufficiently complicated, it may not be worth the + effort to figure out how to reliably remap old data into the new + structure. + +When migration is problematic, we will instead "freeze" the old message +definition by copying it to a legacy message type name, and rename the +message type in legacy bags to use the frozen type name. + +## Developer guidelines for modifying messages + +- Begin by trying hard to get the initial design of any new messages + correct on the first pass, avoiding later changes that will cause a + migration hassle. + +- However, you can change a message's definition without worrying about + backward compatibility considerations up until the point when precious + (ISS) data is recorded. + +- Try to avoid message definition changes that are problematic for + backward compatibility. + + - Adding a field is easy. However, if there is legacy data that needs + to be migrated, try to design the new field to accommodate a clear + no-data value that can be used to migrate old messages that didn't + contain the field. (For example, rather than a `bool` field, you + might prefer to use an `int8` field with enumerated values that + include an explicit no-data value.) + + - Deleting a field is problematic for migration, assuming the field + had any value when recorded in legacy messages. (Migrating to the + new message type could lose precious data.) This might or might not + be acceptable, depending on the situation. + + - Try hard to avoid renumbering the label values for enumerated + types. If you do that, message migration will have to remap the + legacy values to the new values (which is complicated and + error-prone), or risk keeping values that are actively misleading + for data analysis. Instead, you should assign never-before-used + values as needed for new labels. If some old labels are no longer + used in new messages, you should still keep them in the message + definition as documentation for interpreting legacy + messages. However, it's encouraged to clarify their legacy status + (e.g., add a comment, or even prepend `OLD_` to the label text). + + - If the message changes are so involved that you can't figure out + how you would migrate old message data to the new message type, + consider whether it might be better to simply define a brand-new + message type. + +- Legacy message types that were recorded in precious (ISS) bags but are + no longer published by the current software should not be deleted by + default, because they remain useful for backward compatibility with + the archived bags. Instead, document the legacy status of the message + (e.g., add it to a list of messages that are only present for backward + compatibility). + +- When you make a message definition change that is compatible with + `rosbag` migration, please define a migration rule for the message at + the same time, following the [migration + instructions](http://wiki.ros.org/rosbag/migration), ideally using + section 3.1 "Preferred Approach". + + - Place your `*.bmr` file in the `bmr` folder of the appropriate ROS + package. See + [`ff_msgs/bmr`](https://github.com/nasa/astrobee/tree/develop/communications/ff_msgs/bmr) + for an example of naming and styling conventions. + + - It is not necessary to export your `*.bmr` file in `package.xml` as + documented in section 4. Unfortunately, that helps only if `rosdep` + is configured on your host, which we can't always count on. + Instead, our automated bag processing keeps a list of folders to + search for migration rules. + + - Once your `*.bmr` file is added, `rosbag_fix_all.py` will apply + your migration rules as needed. + + - Verify your new rule, as described below. + +- If you are contemplating a new message definition change that is too + involved for migration to be feasible, you should probably just define + a brand-new message type instead and avoid any migration + hassle. However, we already have past message changes of this flavor + built up as technical debt. We deal with them by "freezing" a legacy + copy of the old message definition. Follow these conventions: + + - Messages are frozen by copying their definition files (e.g., + `*.msg` or `*.action`). + + - To avoid clutter, the frozen copies may be placed in their own + package, for example `ff_legacy_msgs` for messages from + `ff_msgs` or `ff_hw_msgs`. + + - Files defining frozen messages should be named by appending a + version number to the original filename. For example, + `Inspection.action` would become `InspectionV1.action`. + + - When you freeze a parent type, you should also freeze any subtypes + contained in its fields, if there is any chance their definition + could change in the future. + + - Add a rule for renaming the message type, as follows: + + - Append your rule to the list in the `rename_types` field of + the file named `bmr/rewrite_types_rules.json` within the + relevant ROS package, such as `ff_msgs`. + + - Your rule must specify these four fields: + + - `old_type`: The old message type name in + `some_pkg/SomeMsg` format. + + - `old_type_md5sum`: You can determine the MD5 sum for the + old type by examining the output of `rosbag info` run on a + bag that contains the legacy message, or by running + `rosmsg md5 ` with the correct legacy version of + FSW checked out. + + - `new_type`: The new (frozen) message type name. + + - `new_type_md5sum`: Determine with `rosmsg md5 `. + + - The rule is interpreted as follows: if a bag contains messages + that match both `type = old_type` and `type_md5sum = + old_type_md5sum`, those messages will have their type and MD5 + sum information rewritten to the new values. + + - If you created a new rules file for the package, you may need to + add it to the list of rules files that `Makefile.rosbag_fix_all` + looks for. + + - Once your renaming rule is added, `rosbag_fix_all.py` will apply + it as needed. + + - TODO: Point to an example. (There is already an example written, + but it will be merged separately later.) + + - Verify your new rule as described below. + +- Verifying message rewrite rules: + + - The same approach applies, whether you wrote a migration rule or a + type renaming rule. + + - Run `rosbag_fix_all.py` on an archived bag containing the legacy + message. It automatically applies various consistency checks. + + - Run `rostopic echo -b -p ` on the fixed bag and spot + check the field values in the output CSV file. + +## Ops approach for fixing bags + +As long as FSW keeps evolving with changing message definitions, no +matter how many times we apply our automated fix script to an archived +bag, a future FSW change could always break its compatibility again. + +Therefore, we should take a layered approach to fixing bags: + +- Whenever we make some use of an archived bag, particularly when we + publicly release the data, we should try to fix the bag at that time + (and create any retroactive migration rules as needed to support + that). Regular usage of the fixing script should also help to avoid + bit-rot. + +- Since future FSW changes will continue to break bags, and we don't + want to take on the burden of reprocessing all the previously released + bags after every breaking message definition change, we should also + provide a simple, documented approach for external users to fix them. + +Usage instructions for fixing bags are documented in: \ref +using_telemetry + +## Complementary backward compatibility approach + +This document focuses on maintaining backward compatibility of legacy +bags with the ROS tools such as `rosbag` and `rostopic` that need the +old messages to be compatible with the latest message +definition. Clearly, this simplifies uses like testing the latest FSW +using legacy bag playback. + +However, if all you need is offline analysis outside the ROS tools, the +[`bagpy` library](https://jmscslgroup.github.io/bagpy/) for Python 3 is +a viable alternative. It relies on the fact that the `rosbag` format +includes complete embedded message definitions, allowing it to +deserialize the binary messages and output CSV with named fields, even +if the latest ROS packages that define the messages no longer have +consistent message definitions. In fact, it doesn't even require a full +ROS installation. Fixed bags will never break from the `bagpy` analysis +perspective. Using `bagpy` may be especially attractive for users +who lack prior ROS experience. + +## Retroactive migration technical debt + +Since Astrobee went through ISS commissioning in April 2019, there have +been several message definition changes that lack corresponding +migration rules. + +Our developers should retroactively create the necessary migration rules +on a lazy as-needed basis. The most common triggering event to create a +new rule would be noticing that `rosbag_fix_all.py` fails on an archived +bag during the public data release process. diff --git a/doc/managing_debians.md b/doc/general_documentation/managing_debians.md similarity index 80% rename from doc/managing_debians.md rename to doc/general_documentation/managing_debians.md index b9bcebf570..12d9131f6f 100644 --- a/doc/managing_debians.md +++ b/doc/general_documentation/managing_debians.md @@ -12,7 +12,6 @@ Step 1 is slightly different for freeflyer or freeflyer_avionics. Setup: - export FF_SOURCE= export FF_AVIONICS= More information on how Astrobee is using @@ -24,29 +23,11 @@ More information on how Astrobee is using Your environment needs to be configured properly to cross-compile Astrobee Robot Software for the `armhf` architecture: -[see NASA Install documentation](../NASA_INSTALL.md) +[see NASA Install documentation](NASA_INSTALL.md) -### Update the release version +### Creating a release - cd $FF_SOURCE - ./scripts/setup/debians/update_release.sh - -*Note: make sure that your EDITOR variable is set to your preferred editor -before invoking this script* - -### Push the changelog - - git commit -m "meaningful message" files_that_changed - git push - -### Create an official release - -If creating a Debian for deployment on a flight robot, or a public release, -make sure to create a git release following the process described in -`./gitrelease.md` - -*It is required that all Debians deployed on space have an associated -release branch* +Please see the [flight release documentation](flight_release.md) for informationon how to create a release and update the release version. ### Create the Debian package diff --git a/doc/new_robot.md b/doc/general_documentation/new_robot.md similarity index 100% rename from doc/new_robot.md rename to doc/general_documentation/new_robot.md diff --git a/doc/general_documentation/public_data.md b/doc/general_documentation/public_data.md new file mode 100644 index 0000000000..fda82b33f8 --- /dev/null +++ b/doc/general_documentation/public_data.md @@ -0,0 +1,363 @@ +\page using_telemetry Using Astrobee Robot Telemetry Logs + +The ISS Astrobee Facility has established an [Astrobee ISS telemetry log public release folder](https://nasagov.box.com/s/4ign43svk39guhy9ev8t5xkzui6tqjm1). + +Astrobee telemetry logs are also called "bag files" and use the +[rosbag](http://wiki.ros.org/rosbag) format. + +## Understanding telemetry in context + +This document focuses primarily on how to use Astrobee robot telemetry +logs. However, for best understanding of an Astrobee ISS activity, you +may benefit from context clues found in other information resources, +including: + +- Debrief notes. Immediately following an ISS activity, the Astrobee + ground team assembles debrief notes that review the activity + objectives, which objectives succeeded or failed, what anomalies were + observed and follow-up actions were required, etc. These notes are + usually archived on the NASA-internal [Free Flyer Ops + wiki](https://babelfish.arc.nasa.gov/confluence/display/FFOPS/). + +- Action logs. During the activity, certain notes may be logged live, + for example, as timestamped comments on a NASA-internal wiki + page. Typical events noted in the log might include when the robot + software was reset, when a crew procedure step started/finished, when + an anomaly was observed, etc. + +- ISS crew procedures. Astronauts often perform setup/teardown tasks + around an Astrobee activity (such as setting "nominal" light levels in + the module, clearing clutter out of the module, installing Astrobee + payloads, setting up ISS camcorders to live stream the activity, + etc.). For some activities, astronauts play a much larger role, such + as manually flying an Astrobee around to collect imagery. The crew + procedure will tell you what instructions the astronaut was following. + +- Ground procedures. The concept of operations for a typical Astrobee + activity includes a team of operators on the ground playing multiple + roles. These operators follow a ground procedure that often includes + much more detail about the planned robot actions than the crew + procedure. Astrobee ground procedures follow a table format which + includes a column for the operator to record notes. If the operator + filled out this column, it can provide valuable context. + +- ISS imagery. Many Astrobee activities are recorded live by ISS + camcorders pre-positioned by an astronaut. In other cases, astronauts + may opportunistically grab a camera to capture some video or a few + photos. After privacy review, this imagery can typically be found by + searching for "astrobee" on the NASA-internal site [Imagery + Online](https://io.jsc.nasa.gov/). A third-party camera view can be + invaluable for understanding what actually happened during an + activity, especially if there were significant anomalies. + +- Post-activity data processing. + - Certain simple processing steps are almost always applied to the + telemetry logs before you receive them. For example: + - Telemetry bag files that were split on the robot may have been + merged into a single bag file. + - Bag file problems such as missing message definition metadata may + have been repaired. + - However, other types of post-activity processing are performed on a + case-by-case basis, and the results may not be included in the ISS + log file you receive. For example: + - It is common to log Astrobee NavCam imagery during an ISS activity + so that post-activity processing can generate higher-quality + position estimates using a costly bundle-adjustment algorithm that + can't run in real-time onboard the robot. When this type of + processing has been performed, you may prefer to request and use + the higher-quality post-activity position estimate vs. the + real-time position estimate logged on the ISS. + +- Research publications. If the activity results were published, the + publications are often the best resource for detailed context. + +In some cases, you may have access to the publicly released Astrobee +telemetry log but not to these other resources documenting the ISS +activity. Our data release processes are still evolving. For now, if you +can't find a resource you need, feel free to ask for more information. + +## Basics of the bag format + +Astrobee telemetry logs are also called "bag files" and use the +[rosbag](http://wiki.ros.org/rosbag) format. + +The [ROS](https://www.ros.org/) middleware framework allows a complex +robot software architecture to be implemented in distributed +fashion. Individual software components are implemented as nodes that +communicate using anonymous publish/subscribe message passing. + +Each message type has a format defined in a `*.msg` file that describes +what fields are present in the message. When a node starts publishing a +message, it first specifies both the message type and the "topic" name +it will use for publishing. This same topic can be used by other nodes +to subscribe and start receiving the message. + +A ROS bag file includes two main types of information: +- Metadata about what topics are included and the message type for each + topic +- The actual contents of the timestamped messages + +## Finding the right bag + +In the [Astrobee ISS telemetry log public release +folder](https://nasagov.box.com/s/4ign43svk39guhy9ev8t5xkzui6tqjm1), the +telemetry is packaged as ZIP archives, ordered by ISS activity date. + +The ZIP archive file name includes the Astrobee robot serial number and +robot name. For example, in "2021-03-26_SN003_bumble.zip", +"SN003_bumble" means Astrobee serial number 3, also known as "Bumble". +If multiple robots were involved in an activity, there will be multiple +ZIP files. + +When a ZIP archive is unpacked, it may contain multiple bag files. To +explain why, we should first note that Astrobee flight software is able +to log two concurrent telemetry streams: + +1. The `immediate` stream contains relatively low-bandwidth housekeeping + information useful for robot debugging. The list of topics logged in + this stream stays fairly consistent over all Astrobee activities, + only occasionally changing due to Astrobee baseline flight software + upgrades. Bags that record the `immediate` stream typically have a + filename ending in `ars_default.bag`. (Where "ars" stands for + "Astrobee Robot Software".) Recording start/stop for this stream is + usually related to starting or restarting the Astrobee baseline + flight software. + +2. The `delayed` stream is configured much more dynamically. Typically, + the Astrobee user defining the objectives of the activity will + develop one or more logging profiles (i.e., lists of topics to log) + that suit their objectives. During the course of a single ISS + activity, ground operators can start and stop bag recording as well + as switching logging profiles, as directed by the ground + procedure. The tail end of the filename for each `delayed` stream bag + file is also set by the operator. Recording start/stop is frequently + used to separate the bag files relating to different procedure steps + and give them meaningful names. Because high-bandwidth imagery + logging can quickly exhaust Astrobee's onboard storage, `delayed` + stream bag recording is often disabled for most of the activity. + +To reiterate, these two streams are logged concurrently, so it is common +to have an `immediate` bag file and a `delayed` bag file that cover +overlapping time intervals, recording different topics. The fact that +the two streams are stored in separate bag files is not a barrier to +analyzing them together: if needed, we have tools that enable you to +merge bag files in message timestamp order. + +The first part of each bag filename gives the date and time when +recording of the bag file started, specified in the UTC (also known as +GMT) time zone used onboard the ISS. For long activities that generate +many bag files, you will probably want to use context information such +as the ground procedure to determine which bag files are relevant for +your needs. + +## Install before working with bags + +You can get both the ROS bag file tools and the message definitions +necessary for working with Astrobee bag files by following the +installation instructions for the Astrobee robot software: \ref +install-nonNASA + +Depending on the activity, if there was guest science software installed +and publishing additional message types, you may need to get the +necessary message definitions by installing that software as well. For +example, here are the [ISAAC software installation +instructions](https://nasa.github.io/isaac/html/md_INSTALL.html). + +We also note that, if `rosbag` processing is all you need, it should +suffice to do a minimal install of just the `ff_msgs` and `ff_hw_msgs` +ROS packages from the [`astrobee`](https://github.com/nasa/astrobee) +repo without building the entire Astrobee software system and without +installing all of its many third-party dependencies. And likewise, just +the `isaac_msgs` and `isaac_hw_msgs` ROS packages from the +[`isaac_msgs`](https://github.com/nasa/isaac_msgs) repo should suffice +for ISAAC messages. (But that slimmed-down install process has not yet +been tested and documented.) + +## Preparing the bag for analysis + +As Astrobee flight software continues to evolve, its telemetry message +definitions occasionally change in ways that break backward +compatibility for analysis of archived telemetry bags. + +We recommend "fixing" a bag (rewriting it for compatibility with the +latest message definitions) before using it, as follows: + +```console +source $ASTROBEE_WS/devel/setup.bash +rosrun bag_processing rosbag_fix_all.py 1.bag 2.bag +# should produce fixed output: 1.fix_all.bag 2.fix_all.bag +``` + +However, fixing the bag may not be needed if you plan to analyze it +only with `bagpy` and not with the core ROS tools. + +## Figuring out what's in the bag + +There are many broader [tutorials on +rosbag](http://wiki.ros.org/rosbag/Tutorials), and a full discussion is +beyond the scope of this document. + +However, here are a few shortcuts for finding what's in a bag: + +- `rosbag info some.bag`: Lists topics contained in the bag, + their message types, and other information. +- `rosmsg info package_name/MsgType`: Lists the fields contained in a + message type. + +You can also browse an overview of Astrobee message types in these +folders: + +- [`ff_msgs`](https://github.com/nasa/astrobee/tree/master/communications/ff_msgs/msg) +- [`ff_hw_msgs`](https://github.com/nasa/astrobee/tree/master/communications/ff_hw_msgs/msg) +- [`isaac_msgs`](https://github.com/nasa/isaac_msgs/tree/master/isaac_msgs/msg) +- [`isaac_hw_msgs`](https://github.com/nasa/isaac_msgs/tree/master/isaac_hw_msgs/msg) + +## Common bag processing tasks + +### Converting a bag to CSV format + +The following example CSV export uses a bag from the publicly released ZIP archive +`2021-03-26_SN003_bumble.zip`: + +```console +source $ASTROBEE_WS/devel/setup.bash +BAG=20210326_1855_phase1Loc_survey_bay_5_attempt_2.bag + +# generate a smaller bag containing only the topic of interest (filtering out any bad message types) +rosrun bag_processing rosbag_topic_filter.py $BAG -a /loc/pose loc_pose.bag +# export to CSV +rostopic echo -b loc_pose.bag -p /loc/pose > loc_pose.csv +``` + +### Combining different message topics based on their timestamps + +Often we want to analyze two message topics together, combining the +fields of messages on both topics that were received around the same +time. For example, we might want to join sensor measurement messages +with pose messages to generate combined sensor+pose records that can be +used to plot a map. + +We provide the `csv_join.py` script to perform this task on exported CSV +files. The following example uses a bag from the publicly released ZIP +archive `2021-03-26_SN003_bumble.zip`: + +```console +source $ASTROBEE_WS/devel/setup.bash +BAG=20210326_1855_phase1Loc_survey_bay_5_attempt_2.bag + +rosrun bag_processing rosbag_topic_filter.py $BAG -a /hw/wifi hw_wifi.bag +rostopic echo -b hw_wifi.bag -p /hw/wifi > hw_wifi.csv + +rosrun bag_processing rosbag_topic_filter.py $BAG -a /loc/pose loc_pose.bag +rostopic echo -b loc_pose.bag -p /loc/pose > loc_pose.csv + +rosrun bag_processing csv_join.py hw_wifi.csv loc_pose.csv wifi_plus_pose.csv +``` + +### Displaying imagery found within a bag file + +Try using the [`rqt_image_view`](http://wiki.ros.org/rqt_image_view) or +[`rqt_bag`](http://wiki.ros.org/rqt_bag) tools. + +TODO: Test and provide a detailed example with an Astrobee bag. + +### Exporting imagery found within a bag file + +There are multiple tutorials about how to do this with ROS bags. Not +clear what is the best method. Or an internal example we could build +from: the Astrobee Facility has a script that extracts imagery in order +to generate a movie for NASA imagery release review. + +TODO: Test and provide a detailed example with an Astrobee bag. + +## Potential issues + +### Incomplete bag metadata + +When processing a bag file, you may see an error message like this: + + genmsg.msg_loader.MsgNotFound: Cannot locate message [Header]: unknown package [std_msgs] on search path [{}] + +It typically indicates that the bag file metadata is missing some +required message definition information, most commonly due to a +[known](https://github.com/rosjava/rosjava_bootstrap/issues/16) +[bug](https://github.com/nasa/astrobee/issues/402) with messages +published by nodes using `rosjava`, which we use on the Astrobee HLP). + +If you encounter this problem, first make sure you [fixed the bag as +described above](#preparing-the-bag-for-analysis). + +If that doesn't work, you can try filtering out the bad topics instead of +trying to fix their message definitions: + +```console +source $ASTROBEE_WS/devel/setup.bash +rosrun bag_processing rosbag_topic_filter.py in.bag -r "/gs/*" -r /hw/cam_sci/compressed fixed.bag +``` + +And this command may help determine which topics are problematic +in case more topics need to be fixed with the filtering approach: + +```console +source $ASTROBEE_WS/devel/setup.bash +rosrun bag_processing rosbag_detect_bad_topics.py in.bag +``` + +As our processes improve, we hope to ensure future bag files have this +metadata issue fixed before public data release, so you will not have to +deal with it. + +### Bags containing messages with outdated message definitions + +When processing a bag file, you may see an error message indicating +that some messages need to be migrated. + +This is because, as Astrobee flight software continues to evolve, its +telemetry message definitions occasionally change in ways that break +backward compatibility for analysis of archived telemetry bags. + +If you encounter this problem, [fix the bag as described +above](#preparing-the-bag-for-analysis). + +### Timestamp clock skew + +Astrobee has multiple processors (the LLP, MLP, and HLP) that have +independent clocks. We describe the difference between their times +as clock skew. + +In some cases, clock skew can be large enough to impact your telemetry +analysis when trying to combine messages logged on different processors. +For example, you might want to determine the robot's position when it +captured a SciCam image (logged on the HLP) using pose messages (logged +on the MLP). In that case, clock skew between HLP and MLP would +introduce a position error proportional to the robot's velocity times +the clock skew time delta. + +Astrobee's processor clocks are nominally kept in sync using the Network +Time Protocol (NTP), but that synchronization has not always been +reliable. NTP is designed for hosts with long uptimes and tries to bring +them into sync gradually. It tends not to work as well in the Astrobee +context where each processor's clock starts completely uninitialized +(the clocks are not battery backed), and robot operations don't allow +much time for sync before starting important telemetry logging. + +Astrobee processes for mitigating clock skew have gradually improved, +including: +- Improved startup procedures do a better job of forcing the clocks to + sync precisely at startup. Older bags are likely to suffer more + clock skew. +- Astrobee `immediate` stream bag files now include clock skew + measurements. You can analyze clock skew with the [`clock_skew.py` + script](https://github.com/nasa/astrobee/blob/master/management/sys_monitor/tools/clock_skew.py). + In theory, you could also use these measurements to correct for clock + skew, but that has not been implemented. + +Note that the same clock skew issues can also arise between Astrobee's +processors and: +- The processor of a payload installed in the Astrobee +- Another Astrobee +- Other timestamped data, such as ISS camcorder video + +In these cases, you can check procedures and notes for the activity to +determine if any special measures were taken to prevent or to measure +clock skew. diff --git a/doc/subsystems.md b/doc/general_documentation/subsystems.md similarity index 91% rename from doc/subsystems.md rename to doc/general_documentation/subsystems.md index 0e61042165..0633a79b72 100644 --- a/doc/subsystems.md +++ b/doc/general_documentation/subsystems.md @@ -25,7 +25,7 @@ Definition of topic names are maintained in \subpage shared:
- \subpage localization - directory: `localization` - topic prefix: `loc` - - We keep this name, everything necessary for localization, pose estimation is performed in the GNC EKF... + - Everything necessary for localization - include: - Sparse Mapping Features - AR Tags @@ -52,8 +52,6 @@ Definition of topic names are maintained in \subpage shared:
- directory: `gnc` - topic prefix: `gnc` - include: - - EKF - - Visual Odometry - Control ### Behaviors @@ -69,13 +67,16 @@ Definition of topic names are maintained in \subpage shared:
### Communication - \subpage comms - - directory: `communication` + - directory: `communications` - topic prefix: `comm` - This is not a subsystem like the others... Does the bridge still get a prefix and publish all "FF-ROS-Commands" under it? [ted] - include: - Bridge - Msg / Srv / Action +### Description (URDF Robot Description) + - \subpage urdf + ### Drivers (hardware) - \subpage hw - directory: `hardware` diff --git a/doc/gitrelease.md b/doc/gitrelease.md deleted file mode 100644 index 7d79e554dd..0000000000 --- a/doc/gitrelease.md +++ /dev/null @@ -1,87 +0,0 @@ -# Generate releases and synching with Github - -## Create a release branch - -1. Test that all new features are functional in all environments - -2. Create a branch release -``` -git checkout develop -git pull -git checkout -b release-0.x.0 -``` - -3. Update the version number and release notes: ./scripts/setup/debians/update\_release.sh VERSION\_NUMBER -``` -git commit -m "Bumped version number to 0.1.x" -``` - -4. Push the branch to the server -``` -git push -u origin release-0.x.0 -``` - -5. Fold the release into master and tag -``` -git checkout master -git merge --no-ff release-0.1.x -# Add message: applied release-0.1.x to master -git push origin master -git tag -a v0.1.x -# Add message: tagged new version... more descriptive message" -git push origin tag v0.1.x -``` - -6. Integrate the release into develop -``` -git checkout develop -git merge --no-ff release-0.1.x -# Add a message: applied release-0.1.x to develop -git push origin develop -``` - -## Propagate a release/hotfix to Github - -### Add remote (only if first time) -``` -git remote add github git@github.com:nasa/astrobee.git -``` -(keys need to be setup correctly) - -### Sync the repos - -1. Work on the public branch -``` -git checkout public -``` - -2. Squash history between releases -``` -git merge --squash master -git commit -# Remove all the summary commit -# Add a meaningful message for the public repo with summary -# and detailed message -``` - -3. Push to github -``` -git push github public:master -``` - -4. Push to babelfish -``` -git push origin public -``` - -5. Sync github with babelfish -``` -git checkout master -git merge --no-ff public -# Just synchronization, no message necessary -git push origin master -git checkout develop -git merge --no-ff public -# Just synchronization, no message necessary -git push origin develop -``` diff --git a/doc/images/astrobee-logo.png b/doc/images/astrobee-logo.png index 57a9f7ac6e..c29fa6ff81 100644 Binary files a/doc/images/astrobee-logo.png and b/doc/images/astrobee-logo.png differ diff --git a/doc/images/astrobee.png b/doc/images/astrobee.png index 3500a9b23d..eb1ece9a81 100644 Binary files a/doc/images/astrobee.png and b/doc/images/astrobee.png differ diff --git a/doc/images/calibration/calibrated_reprojection_from_all_targets_absolute_image.png b/doc/images/calibration/calibrated_reprojection_from_all_targets_absolute_image.png new file mode 100644 index 0000000000..364b504187 Binary files /dev/null and b/doc/images/calibration/calibrated_reprojection_from_all_targets_absolute_image.png differ diff --git a/doc/images/calibration/detection_image.jpg b/doc/images/calibration/detection_image.jpg new file mode 100644 index 0000000000..8a4e30d856 Binary files /dev/null and b/doc/images/calibration/detection_image.jpg differ diff --git a/doc/images/calibration/distorted.png b/doc/images/calibration/distorted.png new file mode 100644 index 0000000000..1486f294f8 Binary files /dev/null and b/doc/images/calibration/distorted.png differ diff --git a/doc/images/calibration/fov_undistorted.png b/doc/images/calibration/fov_undistorted.png new file mode 100644 index 0000000000..d93d72d803 Binary files /dev/null and b/doc/images/calibration/fov_undistorted.png differ diff --git a/doc/images/calibration/norm_errors.png b/doc/images/calibration/norm_errors.png new file mode 100644 index 0000000000..2a889bc4e5 Binary files /dev/null and b/doc/images/calibration/norm_errors.png differ diff --git a/doc/images/calibration/x_errors.png b/doc/images/calibration/x_errors.png new file mode 100644 index 0000000000..c9d5701356 Binary files /dev/null and b/doc/images/calibration/x_errors.png differ diff --git a/doc/images/calibration/y_errors.png b/doc/images/calibration/y_errors.png new file mode 100644 index 0000000000..08457e4bca Binary files /dev/null and b/doc/images/calibration/y_errors.png differ diff --git a/doc/images/favicon.png b/doc/images/favicon.png new file mode 100644 index 0000000000..42a26bc0c6 Binary files /dev/null and b/doc/images/favicon.png differ diff --git a/doc/readme.md b/doc/readme.md deleted file mode 100644 index 2059f41c5f..0000000000 --- a/doc/readme.md +++ /dev/null @@ -1,98 +0,0 @@ -\page doc Documentation - -# Tools used for FSW documentation - -## Code documentation - -Code should be documented using doxygen. - -*Add here an explanation of the overall structure and how to create subsystem - pages (or just provide a default template).* - -## UML diagraming - -Astrobee UML diagrams are created using [plantuml](http://plantuml.com/) that -transform text files into beautiful UML 2 compliant diagrams. - -Specific notations used in the diagrams to represent ROS communication schemes -is described in: -[Astrobee UML Notations](doc/diagrams/notations.png) - -Despite the ads overloaded website, plantuml is free and [open -source](https://github.com/plantuml/plantuml). The plantuml [Reference -Guide](http://plantuml.com/PlantUML_Language_Reference_Guide.pdf) walks to all -the diagrams types. - -Plantuml relies on [Graphviz](http://www.graphviz.org/) that should be already -installed since ROS is also using this tool to render runtime node/topic graphs. -There are many ways to write/generate plantuml diagrams: -http://plantuml.com/running. Choose the integration that best suit you, but vim + -command line java works perfectly. - -Note: some diagrams are not generated correctly when using graphviz 2.40 (layout -stretched vertically). So for now please use graphviz 2.38. - -Note: to re-active the 2.38 that keep being updated on Mac: - - sudo port activate graphviz @2.38.0_3+pangocairo - -To generate the diagrams from the `doc\diagrams` directory: -``` -make -# or to get PNG versions: -make png -``` - -## ROS Messages, Services and Actions - -Our toolchain incudes Doxygen python filters to interpret `*.msg`, `*.action` -and `*.srv` files. Internally, the filters convert the messages, topics and -action files to markdown, and renders them in a hierarchy of Doxygen 'module' -pages. So, they will appear in the same structure as the written markdown -documentation, and not as classes / structs or other types. - -In order for the files to be interpreted correctly, one needs to abide by a -couple of simple rules. -1. All header lines must begin with a '#' comment character. The end of the - header is signified by an empty line. All comments following the newline will - be treated as leading comments for the first declared variable. -1. Any header lines containing the word 'copyright' will be stripped from the - documentation. All hashes ans newlines will also be stripped from the - documentation. You are therefore discouraged from including any comment that - relies on formatting using tabs or spaces. -1. You may document a variable above (leading) or in-line (to the right of) it's - declaration. However, you cannot do both! An inline comment will mute a - leading comment. - -# ROS Unit Tests - -The ROS test framework (rostest) is build upon the Google test framework. The -idea behind a ROS test is that it first launches a collection of ROS nodes, and -then starts the unit test, which runs tests against the ROS nodes. - -Writing a unit test for a package involves modifying the package's -`CMakeLists.txt` and `package.xml` files, and writing a collection of test -cases. To see an example, have a look in the `./tools/visualeyez` folder. - -To build all ROS tests, use the following: - - make -j6 tests - -To execute all ROS tests run the following: - - make -j6 run_tests - -Note that by default all ROS console messages (ROS_INFO, ROS_WARN, etc.) called -from within the test cases are suppressed. To print all messages, as well as a -more comprehensive test result, add the `--text` argument: -``` -rostest <*.test> --text -``` - -Sometimes you want to debug your unit tests, but it's unclear on which port -rostest started the ROS master. A simpler approach is to manually start the ROS -master using `roscore` and then latch the rostest to this master with the -`reuse-master` argument as follows: -``` -rostest <*.test> --reuse-master -``` \ No newline at end of file diff --git a/doc/scripts/action2dox.py b/doc/scripts/action2dox.py index 6a0565275f..5c4e22092a 100755 --- a/doc/scripts/action2dox.py +++ b/doc/scripts/action2dox.py @@ -1,15 +1,15 @@ #!/usr/bin/env python # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -19,27 +19,28 @@ import os import sys from os.path import basename + from common import * # What to search for -token = '/action/' +token = "/action/" # The full path to the file -path = sys.argv[1]; -idx_dot = path.rfind('.'); -idx_msg = path.find(token); +path = sys.argv[1] +idx_dot = path.rfind(".") +idx_msg = path.find(token) package = basename(path[:idx_msg]) -message = basename(path[idx_msg+len(token):idx_dot]); +message = basename(path[idx_msg + len(token) : idx_dot]) # Load the message template -template = load_file('./doc/scripts/templates/action.template') -header = load_file('./doc/scripts/templates/license.template') +template = load_file("./doc/scripts/templates/action.template") +header = load_file("./doc/scripts/templates/license.template") # Load the raw data describing the message chunks = split_into_chunks(path) if len(chunks) != 3: - sys.stderr.write("File '%s' does not have three chunks" % (filename)) - sys.exit(1) + sys.stderr.write("File '%s' does not have three chunks" % (filename)) + sys.exit(1) # Remove the license as well as any whitespace padding chunks[0] = chunks[0].replace(header, "").rstrip().strip() @@ -47,29 +48,31 @@ # Find the description desc = "" goal = "" -state = 0 # 0: padding, 1: header, 2:data +state = 0 # 0: padding, 1: header, 2:data for line in chunks[0].splitlines(): - rec = line.replace("#","").rstrip().strip() - if len(rec) < 2: - state += 1; - if len(rec) > 0: - if state == 1: - desc += line.replace("#","").rstrip().strip() + " " - else: - goal += line + "\n" + rec = line.replace("#", "").rstrip().strip() + if len(rec) < 2: + state += 1 + if len(rec) > 0: + if state == 1: + desc += line.replace("#", "").rstrip().strip() + " " + else: + goal += line + "\n" desc = desc.rstrip().strip() goal = goal.rstrip().strip() -#This is the data that will be injected into the template -data = {'file': path, - 'extension': 'action', - 'type': 'Action', - 'package': package, - 'message': message, - 'description': desc, - 'goal': extract_message(goal), - 'response': extract_message(chunks[1]), - 'feedback': extract_message(chunks[2]) } +# This is the data that will be injected into the template +data = { + "file": path, + "extension": "action", + "type": "Action", + "package": package, + "message": message, + "description": desc, + "goal": extract_message(goal), + "response": extract_message(chunks[1]), + "feedback": extract_message(chunks[2]), +} # print description -print template % data \ No newline at end of file +print((template % data)) diff --git a/doc/scripts/common.py b/doc/scripts/common.py index 2ac5cf2338..9f1eeb2aa2 100644 --- a/doc/scripts/common.py +++ b/doc/scripts/common.py @@ -1,15 +1,15 @@ #!/usr/bin/env python # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -18,77 +18,83 @@ import os import sys - from os.path import basename + def typerlink(typ): - tok = typ.replace("[","") - tok = tok.replace("]","") - tok = tok.split("/") - if len(tok) == 1: + tok = typ.replace("[", "") + tok = tok.replace("]", "") + tok = tok.split("/") + if len(tok) == 1: + return typ + package = tok[0].replace("_", "__") + message = tok[1].replace("_", "__") + if tok[0].strip() in ("ff_msgs", "ff_hw_msgs", "vpp_msgs"): + return '%s' % (package, message, typ) return typ - package = tok[0].replace("_","__") - message = tok[1].replace("_","__") - if tok[0].strip() in ("ff_msgs","ff_hw_msgs","vpp_msgs"): - return '%s' % (package,message,typ) - return typ + # Load and return a message template def split_into_chunks(filename): - chunks = [] - buff = "" - for line in open(filename): - if line.startswith('---'): - chunks.append(buff) - buff = "" - else: - buff += line - chunks.append(buff) - return chunks + chunks = [] + buff = "" + for line in open(filename): + if line.startswith("---"): + chunks.append(buff) + buff = "" + else: + buff += line + chunks.append(buff) + return chunks + def extract_message(str): - variables = [] - constants = [] - desc = "" - for line in str.splitlines(): - rec = line.strip() - if len(rec) == 0: continue - if rec[0] == "#": - desc += line.replace("#","").strip() + " " - else: - idx = rec.find("#") - if idx >= 0: - desc = rec[idx+1:].strip() - rec = rec[:idx] - lst = rec.split("=") - tok = (" ".join(lst[0].split())).split() - dtype = tok[0].strip() - dname = tok[1].strip() - if len(lst) > 1: - dname += " = " + lst[1].strip(); - constants.append("| %s | **%s** | %s |" % (typerlink(dtype),dname,desc)) - desc = "" - else: - variables.append("| %s | `%s` | %s |" % (typerlink(dtype),dname,desc)) - desc = "" + variables = [] + constants = [] + desc = "" + for line in str.splitlines(): + rec = line.strip() + if len(rec) == 0: + continue + if rec[0] == "#": + desc += line.replace("#", "").strip() + " " + else: + idx = rec.find("#") + if idx >= 0: + desc = rec[idx + 1 :].strip() + rec = rec[:idx] + lst = rec.split("=") + tok = (" ".join(lst[0].split())).split() + dtype = tok[0].strip() + dname = tok[1].strip() + if len(lst) > 1: + dname += " = " + lst[1].strip() + constants.append( + "| %s | **%s** | %s |" % (typerlink(dtype), dname, desc) + ) + desc = "" + else: + variables.append("| %s | `%s` | %s |" % (typerlink(dtype), dname, desc)) + desc = "" + + buff = "" + for line in variables: + buff += line + "\n" + for line in constants: + buff += line + "\n" + if len(buff) == 0: + buff = "| | | |" + return buff - buff = "" - for line in variables: - buff += line +"\n" - for line in constants: - buff += line +"\n" - if len(buff) == 0: - buff = "| | | |" - return buff # Load and return a message template def load_file(filename): - if not os.path.isfile(filename): - sys.stderr.write("Cannot locate file '%s'\n" % (filename)) - sys.exit(1) - with open(filename, 'r') as f: - content = f.read() - if not content: - sys.stderr.write("File '%s' is empty\n" % (filename)) - sys.exit(1) - return content + if not os.path.isfile(filename): + sys.stderr.write("Cannot locate file '%s'\n" % (filename)) + sys.exit(1) + with open(filename, "r") as f: + content = f.read() + if not content: + sys.stderr.write("File '%s' is empty\n" % (filename)) + sys.exit(1) + return content diff --git a/doc/scripts/copy_html_link.py b/doc/scripts/copy_html_link.py new file mode 100755 index 0000000000..2abe21f1dd --- /dev/null +++ b/doc/scripts/copy_html_link.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Recursively copy a folder as "HTML symlinks" -- this means writing files +in the target folder with the same relative paths as in the source +folder, but instead of copying the file content, each output file is a +minimal HTML page that will redirect to the original file in the source +folder. +""" + +import argparse +import os + +HTML_TEMPLATE = """ + + +""".lstrip() + + +def copy_html_link(src_path_in, tgt_path_in, verbose): + src_path = os.path.realpath(src_path_in) + tgt_path = os.path.realpath(tgt_path_in) + src_rel_tgt = os.path.relpath(src_path, tgt_path) + count = 0 + for dir_path, dirs, files in os.walk(src_path): + dir_path_suffix = dir_path.replace(src_path, "") + dir_path_suffix = dir_path_suffix.lstrip("/") + for f in files: + if not dir_path_suffix: + depth = 0 + else: + depth = len(dir_path_suffix.split("/")) + up_depth = "/".join([".."] * depth) + src_f_rel_tgt_f = os.path.join(up_depth, src_rel_tgt, dir_path_suffix, f) + out_path = os.path.join(tgt_path, dir_path_suffix, f) + out_dir = os.path.dirname(out_path) + if not os.path.isdir(out_dir): + os.makedirs(out_dir, exist_ok=True) + with open(out_path, "w") as out: + out.write(HTML_TEMPLATE.format(src_f_rel_tgt_f)) + count += 1 + if verbose: + print("%s -> %s" % (out_path, src_f_rel_tgt_f)) + print("wrote %s HTML redirect files" % count) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "src_path", + type=str, + help="source path", + ) + parser.add_argument( + "tgt_path", + type=str, + help="target path", + ) + parser.add_argument( + "-v", + "--verbose", + action="store_true", + default=False, + help="make output more verbose", + ) + + args = parser.parse_args() + + copy_html_link(args.src_path, args.tgt_path, args.verbose) diff --git a/doc/scripts/count_sloc.bash b/doc/scripts/count_sloc.bash index 6dbd66a890..dcb02478ef 100755 --- a/doc/scripts/count_sloc.bash +++ b/doc/scripts/count_sloc.bash @@ -5,7 +5,7 @@ confdir=`dirname $0` rootpath=`cd $confdir/../..; pwd` -dirs_to_exclude=doc/html,gnc/matlab,submodules/avionics,scripts/git,android/picoflexx +dirs_to_exclude=doc/html,submodules/avionics,scripts/git,android/picoflexx lang_defs=$rootpath/doc/scripts/languages_definition.txt cloc --force-lang-def=$lang_defs --exclude-dir=$dirs_to_exclude $rootpath diff --git a/doc/scripts/csv2dox.py b/doc/scripts/csv2dox.py index 7fdc103d6c..208bedc721 100755 --- a/doc/scripts/csv2dox.py +++ b/doc/scripts/csv2dox.py @@ -1,47 +1,56 @@ #!/usr/bin/env python # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. +import csv import os import sys -import csv + from common import * + def hyperlink(url, name): - return '%s' % (url,name) + return '%s' % (url, name) + # Grab the CSV content content = "" header = True -with open(sys.argv[1], 'rb') as csvfile: - csvreader = csv.reader(csvfile, delimiter=',', quotechar='"') - for row in csvreader: - if header: - header = False - else: - content += '| ' + hyperlink(row[2], row[1]) + ' | ' + row[3] + ' | ' + row[4] + ' |\n' +with open(sys.argv[1], "rb") as csvfile: + csvreader = csv.reader(csvfile, delimiter=",", quotechar='"') + for row in csvreader: + if header: + header = False + else: + content += ( + "| " + + hyperlink(row[2], row[1]) + + " | " + + row[3] + + " | " + + row[4] + + " |\n" + ) # This is the data that will be injected into the template -data = { - 'content': content -} +data = {"content": content} # Load the message template -template = load_file('./doc/scripts/templates/csv.template') +template = load_file("./doc/scripts/templates/csv.template") # Populate the template -print template % data \ No newline at end of file +print((template % data)) diff --git a/doc/scripts/deliv2csv.py b/doc/scripts/deliv2csv.py new file mode 100644 index 0000000000..f458349f41 --- /dev/null +++ b/doc/scripts/deliv2csv.py @@ -0,0 +1,26 @@ +from sys import stdout + +import ruamel.yaml as yaml + +stream = file("deliverables.yaml", "r") + +deliverables = yaml.round_trip_load(stream) + +ref_keys = list(list(deliverables[0].values())[0].keys()) + +stdout.write("devliverable") +for k in ref_keys: + stdout.write("\t" + k) +stdout.write("\n") + +for d in deliverables: + stdout.write(list(d.keys())[0]) + for k in ref_keys: + s = "" + try: + v = list(d.values())[0][k] + s = str(list(d.values())[0][k]) + except: + pass + stdout.write("\t" + s) + stdout.write("\n") diff --git a/doc/scripts/generate_sample_depsgraph.sh b/doc/scripts/generate_sample_depsgraph.sh index 9285f49ede..2a82ccaf6c 100755 --- a/doc/scripts/generate_sample_depsgraph.sh +++ b/doc/scripts/generate_sample_depsgraph.sh @@ -10,10 +10,10 @@ scriptdir=`dirname $0` -graph_names="ff_nodelet gnc_autocode pmc_actuator_nodelet sparse_mapping \ +graph_names="ff_nodelet pmc_actuator_nodelet sparse_mapping \ executive dds_ros_bridge gazebo_sensor_plugin_perch_cam" -BUILD_PATH=${BUILD_PATH:-${HOME}/freeflyer_build/native} +BUILD_PATH=${BUILD_PATH:-${HOME}/astrobee_build/native} if [ ! -f $BUILD_PATH/CMakeCache.txt ] then diff --git a/doc/scripts/msg2dox.py b/doc/scripts/msg2dox.py index 75fdc9f4ac..cb1cc9783a 100755 --- a/doc/scripts/msg2dox.py +++ b/doc/scripts/msg2dox.py @@ -1,15 +1,15 @@ #!/usr/bin/env python # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -19,27 +19,28 @@ import os import sys from os.path import basename + from common import * # What to search for -token = '/msg/' +token = "/msg/" # The full path to the file -path = sys.argv[1]; -idx_dot = path.rfind('.'); -idx_msg = path.find(token); +path = sys.argv[1] +idx_dot = path.rfind(".") +idx_msg = path.find(token) package = basename(path[:idx_msg]) -message = basename(path[idx_msg+len(token):idx_dot]); +message = basename(path[idx_msg + len(token) : idx_dot]) # Load the message template -template = load_file('./doc/scripts/templates/msg.template') -header = load_file('./doc/scripts/templates/license.template') +template = load_file("./doc/scripts/templates/msg.template") +header = load_file("./doc/scripts/templates/license.template") # Load the raw data describing the message chunks = split_into_chunks(path) if len(chunks) != 1: - sys.stderr.write("File '%s' does not have one chunk" % (filename)) - sys.exit(1) + sys.stderr.write("File '%s' does not have one chunk" % (filename)) + sys.exit(1) # Remove the license as well as any whitespace padding chunks[0] = chunks[0].replace(header, "").rstrip().strip() @@ -47,26 +48,28 @@ # Find the description desc = "" data = "" -state = 0 # 0: padding, 1: header, 2:data +state = 0 # 0: padding, 1: header, 2:data for line in chunks[0].splitlines(): - rec = line.replace("#","").rstrip().strip() - if len(rec) < 2: - state += 1; - if len(rec) > 0: - if state == 1: - desc += line.replace("#","").rstrip().strip() + " " - else: - data += line + "\n" + rec = line.replace("#", "").rstrip().strip() + if len(rec) < 2: + state += 1 + if len(rec) > 0: + if state == 1: + desc += line.replace("#", "").rstrip().strip() + " " + else: + data += line + "\n" desc = desc.rstrip().strip() data = data.rstrip().strip() # This is the data that will be injected into the template -data = {'file': path, - 'extension': 'msg', - 'type': 'Message', - 'package': package, - 'message': message, - 'description': desc, - 'content': extract_message(data) } +data = { + "file": path, + "extension": "msg", + "type": "Message", + "package": package, + "message": message, + "description": desc, + "content": extract_message(data), +} -print template % data \ No newline at end of file +print((template % data)) diff --git a/doc/scripts/puml2dox.py b/doc/scripts/puml2dox.py index fea1aa27df..62f302fc8e 100755 --- a/doc/scripts/puml2dox.py +++ b/doc/scripts/puml2dox.py @@ -21,42 +21,43 @@ import os import re import sys + from common import * -doc_dir = os.path.normpath(os.path.join( - os.path.abspath(os.path.dirname(sys.argv[0])), '../' - )) +doc_dir = os.path.normpath( + os.path.join(os.path.abspath(os.path.dirname(sys.argv[0])), "../") +) puml_path = os.path.relpath(sys.argv[1], os.getcwd()) name_no_ext = os.path.splitext(os.path.basename(puml_path))[0] -plantuml_jar = doc_dir+'/diagrams/plantuml/plantuml.jar' +plantuml_jar = doc_dir + "/diagrams/plantuml/plantuml.jar" -template = load_file(doc_dir+'/scripts/templates/diagram.template') +template = load_file(doc_dir + "/scripts/templates/diagram.template") # # Extract diagram width # -max_width=900 -svg = open(doc_dir+'/diagrams/gen/svg/'+name_no_ext+'.svg', "r") -first=svg.readline() -pattern=re.compile('viewBox="0 0 ([0-9]+) ([0-9]+)"') -result=re.search(pattern, first) +max_width = 900 +svg = open(doc_dir + "/diagrams/gen/svg/" + name_no_ext + ".svg", "r") +first = svg.readline() +pattern = re.compile('viewBox="0 0 ([0-9]+) ([0-9]+)"') +result = re.search(pattern, first) if not result: - print "width not found!" + print("width not found!") width = max_width else: width = int(result.group(1)) if width > max_width: - scaled_width=max_width + scaled_width = max_width else: - scaled_width=width + scaled_width = width # # Generate the PNG image from the PUML diagram # -output_dir = os.path.join(doc_dir+'/html') +output_dir = os.path.join(doc_dir + "/html") # plantuml '-output' option is always relative to the source file, # so we need to compute the generation directory relative to it! relative_gen_dir = os.path.relpath(output_dir, os.path.dirname(puml_path)) @@ -70,15 +71,15 @@ # Generate a page that includes the diagram image # # html_relative_path = os.path.join('./', name_no_ext+'.png') -html_relative_path = os.path.join('../diagrams/gen/svg', name_no_ext+'.svg') -package = puml_path.replace(os.getcwd(), '').split(os.sep)[0] +html_relative_path = os.path.join("../diagrams/gen/svg", name_no_ext + ".svg") +package = puml_path.replace(os.getcwd(), "").split(os.sep)[0] data = { - 'package' : package, - 'diagram_name' : name_no_ext, - 'diagram_path' : html_relative_path, - 'width' : scaled_width + "package": package, + "diagram_name": name_no_ext, + "diagram_path": html_relative_path, + "width": scaled_width, } -print(template % data) +print((template % data)) diff --git a/doc/scripts/simplify_cmake_depsgraph.py b/doc/scripts/simplify_cmake_depsgraph.py index 86cc6d99fc..ae3b6fd7e4 100755 --- a/doc/scripts/simplify_cmake_depsgraph.py +++ b/doc/scripts/simplify_cmake_depsgraph.py @@ -4,15 +4,15 @@ # Simplify a cmake generated dependency graph. # # Cmake has the capability to generate executable/librairies dependency graph. -# However, the generated graphs are unreadable because every library file is +# However, the generated graphs are unreadable because every library file is # represented as a unique node. This script groups the libraries files by # package name. # The 'groups' variable list the patterns used to re-group the various nodes # from a same package into a single node. The groups list is specifically -# crafted for ARS: it mostly address the ROS and Gazebo libs, plus some +# crafted for ARS: it mostly address the ROS and Gazebo libs, plus some # key other dependencies. # -# Usage: +# Usage: # 1. Generate the dependency graphs with something like # cd $BUILD_PATH # cmake --graphviz=deps/ars . @@ -24,10 +24,10 @@ # -import sys import re +import sys -groups = [ +groups = [ ("nodeROS", "ROS Libraries", "/opt/ros/.+"), ("nodeGazebo", "gazebo", "/usr/lib/.+/libgazebo[_a-z0-9]*.so"), ("nodeBoost", "boost", "/usr/lib/.+/libboost.+\.so"), @@ -37,45 +37,47 @@ ("nodeGflags", "gflags", "/usr/lib/.+/libgflag.*\.so"), ("nodeGlog", "glog", "/usr/lib/.+/libglog.*\.so"), ("nodeLinux", "Linux System Libraries", "/usr/lib/.*.so") -# Not sure if libPocoFoundation should be listed individually -# ("nodeLinux", "Linux System Libraries", "/usr/lib/libPoco.*.so"), -# ("nodeLinux", "Linux System Libraries", "/usr/lib/.+linux-gnu/.+\.so") - ] + # Not sure if libPocoFoundation should be listed individually + # ("nodeLinux", "Linux System Libraries", "/usr/lib/libPoco.*.so"), + # ("nodeLinux", "Linux System Libraries", "/usr/lib/.+linux-gnu/.+\.so") +] nodes = list() + def process_dot(file): global nodes lines = file.readlines() - + # Identify groups of libraries # outer loop is groups: this way the order of the group list is respected # and it allows to glob larger pattern after more specific patterns # have already been processed for g in groups: for i, l in enumerate(lines): - pattern=re.compile('\s"(node[0-9]+)"\s\[\slabel="('+g[2]+')"\s.+') - result=re.search(pattern, l) + pattern = re.compile('\s"(node[0-9]+)"\s\[\slabel="(' + g[2] + ')"\s.+') + result = re.search(pattern, l) if result: lines.pop(i) lines.insert(i, l.replace(result.group(2), g[1])) - nodes.append( (result.group(1), g[0]) ) + nodes.append((result.group(1), g[0])) # Replace nodes with common group node name for n in nodes: - lines = [ l.replace(n[0], n[1]) for l in lines ] - + lines = [l.replace(n[0], n[1]) for l in lines] + # Add strict to avoid multiple edges - lines[0] = "strict "+lines[0] - + lines[0] = "strict " + lines[0] + # Output the new file for l in lines: print(l, end=" ") - + + if len(sys.argv) < 2: print("provide input file as first arg") exit f = open(sys.argv[1], "r") -process_dot(f) -#print(nodes) +process_dot(f) +# print(nodes) diff --git a/doc/scripts/srv2dox.py b/doc/scripts/srv2dox.py index fff0ed5848..bac14b3b68 100755 --- a/doc/scripts/srv2dox.py +++ b/doc/scripts/srv2dox.py @@ -1,15 +1,15 @@ #!/usr/bin/env python # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -19,27 +19,28 @@ import os import sys from os.path import basename + from common import * # What to search for -token = '/srv/' +token = "/srv/" # The full path to the file -path = sys.argv[1]; -idx_dot = path.rfind('.'); -idx_msg = path.find(token); +path = sys.argv[1] +idx_dot = path.rfind(".") +idx_msg = path.find(token) package = basename(path[:idx_msg]) -message = basename(path[idx_msg+len(token):idx_dot]); +message = basename(path[idx_msg + len(token) : idx_dot]) # Load the message template -template = load_file('./doc/scripts/templates/srv.template') -header = load_file('./doc/scripts/templates/license.template') +template = load_file("./doc/scripts/templates/srv.template") +header = load_file("./doc/scripts/templates/license.template") # Load the raw data describing the message chunks = split_into_chunks(path) if len(chunks) != 2: - sys.stderr.write("File '%s' does not have two chunks" % (filename)) - sys.exit(1) + sys.stderr.write("File '%s' does not have two chunks" % (filename)) + sys.exit(1) # Remove the license as well as any whitespace padding chunks[0] = chunks[0].replace(header, "").rstrip().strip() @@ -47,28 +48,30 @@ # Find the description desc = "" data = "" -state = 0 # 0: padding, 1: header, 2:data +state = 0 # 0: padding, 1: header, 2:data for line in chunks[0].splitlines(): - rec = line.replace("#","").rstrip().strip() - if len(rec) < 2: - state += 1; - if len(rec) > 0: - if state == 1: - desc += line.replace("#","").rstrip().strip() + " " - else: - data += line + "\n" + rec = line.replace("#", "").rstrip().strip() + if len(rec) < 2: + state += 1 + if len(rec) > 0: + if state == 1: + desc += line.replace("#", "").rstrip().strip() + " " + else: + data += line + "\n" desc = desc.rstrip().strip() data = data.rstrip().strip() # This is the data that will be injected into the template -data = {'file': path, - 'extension': 'srv', - 'type': 'Service', - 'package': package, - 'message': message, - 'description': desc, - 'request': extract_message(data), - 'response': extract_message(chunks[1]) } +data = { + "file": path, + "extension": "srv", + "type": "Service", + "package": package, + "message": message, + "description": desc, + "request": extract_message(data), + "response": extract_message(chunks[1]), +} -#print description -print template % data \ No newline at end of file +# print description +print((template % data)) diff --git a/doc/style/doc_version_select.js b/doc/style/doc_version_select.js new file mode 100644 index 0000000000..550f9f5fc5 --- /dev/null +++ b/doc/style/doc_version_select.js @@ -0,0 +1,53 @@ + +// The line below will be replaced to include any additional +// auto-detected versions. See script in docs.yaml. +var allVersions = ["develop", "master", "ros2"]; + +function buildSelect(currentVersion) { + if (!allVersions.includes(currentVersion)) { + allVersions.unshift(currentVersion); + } + + // couldn't get external CSS stylesheet to apply here for some reason + var buf = ['Astrobee Version:"); + + return buf.join(""); +} + +function detectCurrentVersion() { + const version_regex = /\/v\/(.*)\//; + var match = version_regex.exec(window.location.pathname); + if (match) { + return match[1]; + } else { + return "(unknown)"; + } +} + +function onSelectorChange() { + var selector = document.getElementById("versionselector"); + var currentVersion = detectCurrentVersion(); + var selectedVersion = selector.value; + window.location.pathname = window.location.pathname.replace(currentVersion, selectedVersion); +} + +function initVersionSelector() { + var currentVersion = detectCurrentVersion(); + var projNumDiv = document.getElementById("projectnumber"); + projNumDiv.innerHTML = buildSelect(currentVersion); + var selector = document.getElementById("versionselector"); + // couldn't get external CSS stylesheet to apply here for some reason + document.getElementById('projectnumber').style = "position: relative; top: -0.3em;"; + + selector.addEventListener("change", onSelectorChange); +} + +initVersionSelector(); diff --git a/doc/style/freeflyer.css b/doc/style/freeflyer.css index ddb6b2a6b2..ed4002a35b 100644 --- a/doc/style/freeflyer.css +++ b/doc/style/freeflyer.css @@ -1,371 +1,1810 @@ -h1, .h1, h2, .h2, h3, .h3{ - font-weight: 200 !important; +/* Added in */ + +.ui-resizable-e { + background-image: none; + background-size: 100%; + background-repeat: repeat-y; + background-attachment: scroll; + cursor: ew-resize; + height: 100%; + right: 0; + top: 0; + width: 3px; + background-color: #a1a4a7; } -#navrow1, #navrow2, #navrow3, #navrow4, #navrow5{ - border-bottom: 1px solid #FFFFFF; +.sm-dox a, .sm-dox a:focus, .sm-dox a:hover, .sm-dox a:active { + background-image: none; + padding: 0 12px; + padding-right: 43px; + font-size: 13px; + font-weight: bold; + line-height: 36px; + text-decoration: none; + text-shadow: none; + color: #f6f6f6; + outline: 0; } -.adjust-right { -margin-left: 30px !important; -font-size: 1.15em !important; +.sm-dox a:hover { + background-image: none; + color: white; + text-shadow:none; + background-color: #a1a4a7 } -.navbar{ - border: 0px solid #222 !important; - background-color: #ffffff !important; + +.arrow { + color: #1c1c1c; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; + cursor: pointer; + font-size: 80%; + display: inline-block; + width: 16px; + height: 22px; } -table{ - white-space:pre-wrap !important; + +/* The standard CSS for doxygen 1.9.0 */ + +body, table, div, p, dl { + font: 300 14px/24px Roboto,sans-serif; + background-color: #f6f6f6; } -/* - =========================== - */ +p.reference, p.definition { + font: 300 14px/22px Roboto,sans-serif; +} -/* Sticky footer styles --------------------------------------------------- */ -html, -body { - height: 100%; - /* The html and body elements cannot have any padding or margin. */ +/* @group Heading Levels */ +h1 { + font: 400 30px/22px Roboto,sans-serif; } -/* Wrapper for page content to push down footer */ -#wrap { - min-height: 100%; - height: auto; - /* Negative indent footer by its height */ - margin: 0 auto -60px; - /* Pad bottom by footer height */ - padding: 0 0 60px; +.title { + font: 500 25px/28px Roboto,sans-serif; + margin: 10px 2px; } -/* Set the fixed height of the footer here */ -#footer { - font-size: 0.9em; - padding: 8px 0px; - background-color: #f5f5f5; +h2 { + font: 400 22px/22px Roboto,sans-serif; } -.footer-row { - line-height: 44px; +h2.groupheader { + border-bottom: 1px solid #879ECB; + color: #354C7B; + font-size: 120%; + font-weight: normal; + margin-top: 1.75em; + padding-top: 8px; + padding-bottom: 4px; + width: 100%; } -#footer > .container { - padding-left: 15px; - padding-right: 15px; +h3.groupheader { + font-size: 100%; } -.footer-follow-icon { - margin-left: 3px; - text-decoration: none !important; +h1, h2, h3, h4, h5, h6 { + -webkit-transition: text-shadow 0.5s linear; + -moz-transition: text-shadow 0.5s linear; + -ms-transition: text-shadow 0.5s linear; + -o-transition: text-shadow 0.5s linear; + transition: text-shadow 0.5s linear; + margin-right: 15px; } -.footer-follow-icon img { - width: 20px; +h1.glow, h2.glow, h3.glow, h4.glow, h5.glow, h6.glow { + text-shadow: 0 0 15px cyan; } -.footer-link { - padding-top: 5px; - display: inline-block; - color: #999999; - text-decoration: none; +dt { + font-weight: bold; } -.footer-copyright { - text-align: center; +ul.multicol { + -moz-column-gap: 1em; + -webkit-column-gap: 1em; + column-gap: 1em; + -moz-column-count: 3; + -webkit-column-count: 3; + column-count: 3; +} + +p.startli, p.startdd { + margin-top: 2px; } +th p.starttd, th p.intertd, th p.endtd { + font-size: 100%; + font-weight: 700; +} -@media (min-width: 992px) { - .footer-row { - text-align: left; - } +p.starttd { + margin-top: 0px; +} - .footer-icons { - text-align: right; - } +p.endli { + margin-bottom: 0px; +} + +p.enddd { + margin-bottom: 4px; +} + +p.endtd { + margin-bottom: 2px; } -@media (max-width: 991px) { - .footer-row { + +p.interli { +} + +p.interdd { +} + +p.intertd { +} + +/* @end */ + +caption { + font-weight: bold; +} + +span.legend { + font-size: 70%; text-align: center; - } +} - .footer-icons { +h3.version { + font-size: 90%; text-align: center; - } } -/* DOXYGEN Code Styles ------------------------------------ */ +div.qindex, div.navtab{ + background-color: #EBEFF6; + border: 1px solid #A3B4D7; + text-align: center; +} +div.qindex, div.navpath { + width: 100%; + line-height: 140%; +} + +div.navtab { + margin-right: 15px; +} + +/* @group Link Styling */ + +a { + color: #1c1c1c; + font-weight: normal; + text-decoration: none; +} + +.contents a:visited { + color: #1E90FF; +} + +a:hover { + text-decoration: underline; +} a.qindex { - font-weight: bold; + font-weight: bold; } a.qindexHL { - font-weight: bold; - background-color: #9CAFD4; - color: #ffffff; - border: 1px double #869DCA; + font-weight: bold; + background-color: #9CAFD4; + color: #FFFFFF; + border: 1px double #869DCA; } .contents a.qindexHL:visited { - color: #ffffff; + color: #FFFFFF; +} + +a.el { + color: #1E90FF; + font-weight: bold; +} + +a.elRef { } a.code, a.code:visited, a.line, a.line:visited { - color: #4665A2; + color: #4665A2; } a.codeRef, a.codeRef:visited, a.lineRef, a.lineRef:visited { - color: #4665A2; + color: #4665A2; } /* @end */ dl.el { - margin-left: -1cm; + margin-left: -1cm; +} + +ul { + overflow: hidden; /*Fixed: list item bullets overlap floating elements*/ +} + +#titlearea +{ + padding: 0px; + margin: 0px; + width: 100%; + border-bottom: 1px solid #32363d; +} + +#side-nav { + overflow: visible; /* reset ul rule for scroll bar in GENERATE_TREEVIEW window */ + background-image: none; + left: auto; +} + +#main-nav ul, #main-nav div { + overflow: visible; /* reset ul rule for the navigation bar drop down lists */ + /* Added mgouveia*/ + border-bottom: 1px solid #32363d; + background-image: none; + background-color: #2b2b2b; +} + +#nav-tree { + padding: 3px 6px 20px 10px; + background-image: none; + background-color: #a1a4a7; +} + +#nav-tree .item { + background-color: #a1a4a7; +} + +#nav-tree .selected { + background-image: none; + text-shadow: none; + background-color: #eeeeee; +} + +#nav-tree .selected a { + color: #1c1c1c; +} + +#nav-tree a { + color: #1c1c1c; +} + +#nav-sync { + background-color: #a1a4a7; +} + +.fragment { + text-align: left; + direction: ltr; + overflow-x: auto; /*Fixed: fragment lines overlap floating elements*/ + overflow-y: hidden; } pre.fragment { - border: 1px solid #C4CFE5; - background-color: #FBFCFD; - padding: 4px 6px; - margin: 4px 8px 4px 2px; - overflow: auto; - word-wrap: break-word; - font-size: 9pt; - line-height: 125%; - font-family: monospace, fixed; - font-size: 105%; + border: 1px solid #C4CFE5; + background-color: #FBFCFD; + padding: 4px 6px; + margin: 4px 8px 4px 2px; + overflow: auto; + word-wrap: break-word; + font-size: 9pt; + line-height: 125%; + font-family: monospace, fixed; + font-size: 105%; } div.fragment { - padding: 4px 6px; - margin: 4px 8px 4px 2px; - border: 1px solid #C4CFE5; + padding: 0 0 1px 0; /*Fixed: last line underline overlap border*/ + margin: 4px 8px 4px 2px; + background-color: #FBFCFD; + border: 1px solid #C4CFE5; } div.line { - font-family: Consolas, "Liberation Mono", Menlo, Courier, monospace; - font-size: 12px; - min-height: 13px; - line-height: 1.0; - text-wrap: unrestricted; - white-space: -moz-pre-wrap; /* Moz */ - white-space: -pre-wrap; /* Opera 4-6 */ - white-space: -o-pre-wrap; /* Opera 7 */ - white-space: pre-wrap; /* CSS3 */ - word-wrap: normal; /* IE 5.5+ */ - text-indent: -53px; - padding-left: 53px; - padding-bottom: 0px; - margin: 0px; - -webkit-transition-property: background-color, box-shadow; - -webkit-transition-duration: 0.5s; - -moz-transition-property: background-color, box-shadow; - -moz-transition-duration: 0.5s; - -ms-transition-property: background-color, box-shadow; - -ms-transition-duration: 0.5s; - -o-transition-property: background-color, box-shadow; - -o-transition-duration: 0.5s; - transition-property: background-color, box-shadow; - transition-duration: 0.5s; -} -div.line:hover{ - background-color: #FBFF00; + font-family: monospace, fixed; + font-size: 13px; + min-height: 13px; + line-height: 1.0; + text-wrap: unrestricted; + white-space: -moz-pre-wrap; /* Moz */ + white-space: -pre-wrap; /* Opera 4-6 */ + white-space: -o-pre-wrap; /* Opera 7 */ + white-space: pre-wrap; /* CSS3 */ + word-wrap: break-word; /* IE 5.5+ */ + text-indent: -53px; + padding-left: 53px; + padding-bottom: 0px; + margin: 0px; + -webkit-transition-property: background-color, box-shadow; + -webkit-transition-duration: 0.5s; + -moz-transition-property: background-color, box-shadow; + -moz-transition-duration: 0.5s; + -ms-transition-property: background-color, box-shadow; + -ms-transition-duration: 0.5s; + -o-transition-property: background-color, box-shadow; + -o-transition-duration: 0.5s; + transition-property: background-color, box-shadow; + transition-duration: 0.5s; +} + +div.line:after { + content:"\000A"; + white-space: pre; } div.line.glow { - background-color: cyan; - box-shadow: 0 0 10px cyan; + background-color: cyan; + box-shadow: 0 0 10px cyan; } span.lineno { - padding-right: 4px; - text-align: right; - color:rgba(0,0,0,0.3); - border-right: 1px solid #EEE; - border-left: 1px solid #EEE; - background-color: #FFF; - white-space: pre; - font-family: Consolas, "Liberation Mono", Menlo, Courier, monospace ; + padding-right: 4px; + text-align: right; + border-right: 2px solid #0F0; + background-color: #E8E8E8; + white-space: pre; } span.lineno a { - background-color: #FAFAFA; - cursor:pointer; + background-color: #D8D8D8; } span.lineno a:hover { - background-color: #EFE200; - color: #1e1e1e; + background-color: #C8C8C8; +} + +.lineno { + -webkit-touch-callout: none; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; +} + +div.ah, span.ah { + background-color: black; + font-weight: bold; + color: #FFFFFF; + margin-bottom: 3px; + margin-top: 3px; + padding: 0.2em; + border: solid thin #333; + border-radius: 0.5em; + -webkit-border-radius: .5em; + -moz-border-radius: .5em; + box-shadow: 2px 2px 3px #999; + -webkit-box-shadow: 2px 2px 3px #999; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px; + background-image: -webkit-gradient(linear, left top, left bottom, from(#eee), to(#000),color-stop(0.3, #444)); + background-image: -moz-linear-gradient(center top, #eee 0%, #444 40%, #000 110%); +} + +div.classindex ul { + list-style: none; + padding-left: 0; +} + +div.classindex span.ai { + display: inline-block; } div.groupHeader { - margin-left: 16px; - margin-top: 12px; - font-weight: bold; + margin-left: 16px; + margin-top: 12px; + font-weight: bold; } div.groupText { - margin-left: 16px; - font-style: italic; + margin-left: 16px; + font-style: italic; +} + +body { + background-color: #eeeeee; + color: black; + max-width: 1400px; + margin: auto; + +div.contents { + margin-top: 10px; + margin-left: 12px; + margin-right: 8px; +} + +td.indexkey { + background-color: #EBEFF6; + font-weight: bold; + border: 1px solid #C4CFE5; + margin: 2px 0px 2px 0; + padding: 2px 10px; + white-space: nowrap; + vertical-align: top; +} + +td.indexvalue { + background-color: #EBEFF6; + border: 1px solid #C4CFE5; + padding: 2px 10px; + margin: 2px 0px; +} + +tr.memlist { + background-color: #EEF1F7; +} + +p.formulaDsp { + text-align: center; +} + +img.formulaDsp { + +} + +img.formulaInl, img.inline { + vertical-align: middle; +} + +div.center { + text-align: center; + margin-top: 0px; + margin-bottom: 0px; + padding: 0px; +} + +div.center img { + border: 0px; +} + +address.footer { + text-align: right; + padding-right: 12px; +} + +img.footer { + border: 0px; + vertical-align: middle; } /* @group Code Colorization */ span.keyword { - color: #008000 + color: #008000 } span.keywordtype { - color: #604020 + color: #604020 } span.keywordflow { - color: #e08000 + color: #e08000 } span.comment { - color: #800000 + color: #800000 } span.preprocessor { - color: #806020 + color: #806020 } span.stringliteral { - color: #002080 + color: #002080 } span.charliteral { - color: #008080 + color: #008080 } -span.vhdldigit { - color: #ff00ff +span.vhdldigit { + color: #ff00ff } -span.vhdlchar { - color: #000000 +span.vhdlchar { + color: #000000 } -span.vhdlkeyword { - color: #700070 +span.vhdlkeyword { + color: #700070 } -span.vhdllogic { - color: #ff0000 +span.vhdllogic { + color: #ff0000 } blockquote { - background-color: #F7F8FB; - border-left: 2px solid #9CAFD4; - margin: 0 24px 0 4px; - padding: 0 12px 0 16px; + background-color: #F7F8FB; + border-left: 2px solid #9CAFD4; + margin: 0 24px 0 4px; + padding: 0 12px 0 16px; +} + +blockquote.DocNodeRTL { + border-left: 0; + border-right: 2px solid #9CAFD4; + margin: 0 4px 0 24px; + padding: 0 16px 0 12px; +} + +/* @end */ + + +.search { + color: #003399; + font-weight: bold; } -/*---------------- Search Box */ +form.search { + margin-bottom: 0px; + margin-top: 0px; +} -#search-box { - margin: 10px 0px; +input.search { + font-size: 75%; + color: #000080; + font-weight: normal; + background-color: #e8eef2; } -#search-box .close { - display: none; - position: absolute; - right: 0px; - padding: 6px 12px; - z-index: 5; + + +td.tiny { + font-size: 75%; } -/*---------------- Search results window */ +.dirtab { + padding: 4px; + border-collapse: collapse; + border: 1px solid #A3B4D7; +} -#search-results-window { - display: none; +th.dirtab { + background: #EBEFF6; + font-weight: bold; } -iframe#MSearchResults { - width: 100%; - height: 15em; +hr { + height: 0px; + border: none; + border-top: 1px solid #4A6AAA; +} + +hr.footer { + height: 1px; +} + +/* @group Member Descriptions */ + +table.memberdecls { + border-spacing: 0px; + padding: 0px; +} + +.memberdecls td, .fieldtable tr { + -webkit-transition-property: background-color, box-shadow; + -webkit-transition-duration: 0.5s; + -moz-transition-property: background-color, box-shadow; + -moz-transition-duration: 0.5s; + -ms-transition-property: background-color, box-shadow; + -ms-transition-duration: 0.5s; + -o-transition-property: background-color, box-shadow; + -o-transition-duration: 0.5s; + transition-property: background-color, box-shadow; + transition-duration: 0.5s; +} + +.memberdecls td.glow, .fieldtable tr.glow { + background-color: cyan; + box-shadow: 0 0 15px cyan; +} + +.mdescLeft, .mdescRight, +.memItemLeft, .memItemRight, +.memTemplItemLeft, .memTemplItemRight, .memTemplParams { + background-color: #F9FAFC; + border: none; + margin: 4px; + padding: 1px 0 0 8px; +} + +.mdescLeft, .mdescRight { + padding: 0px 8px 4px 8px; + color: #555; +} + +.memSeparator { + border-bottom: 1px solid #DEE4F0; + line-height: 1px; + margin: 0px; + padding: 0px; +} + +.memItemLeft, .memTemplItemLeft { + white-space: nowrap; +} + +.memItemRight, .memTemplItemRight { + width: 100%; +} + +.memTemplParams { + color: #4665A2; + white-space: nowrap; + font-size: 80%; +} + +/* @end */ + +/* @group Member Details */ + +/* Styles for detailed member documentation */ + +.memtitle { + padding: 8px; + border-top: 1px solid #A8B8D9; + border-left: 1px solid #A8B8D9; + border-right: 1px solid #A8B8D9; + border-top-right-radius: 4px; + border-top-left-radius: 4px; + margin-bottom: -1px; + background-image: url('nav_f.png'); + background-repeat: repeat-x; + background-color: #E2E8F2; + line-height: 1.25; + font-weight: 300; + float:left; +} + +.permalink +{ + font-size: 65%; + display: inline-block; + vertical-align: middle; +} + +.memtemplate { + font-size: 80%; + color: #4665A2; + font-weight: normal; + margin-left: 9px; +} + +.memnav { + background-color: #EBEFF6; + border: 1px solid #A3B4D7; + text-align: center; + margin: 2px; + margin-right: 15px; + padding: 2px; +} + +.mempage { + width: 100%; +} + +.memitem { + padding: 0; + margin-bottom: 10px; + margin-right: 5px; + -webkit-transition: box-shadow 0.5s linear; + -moz-transition: box-shadow 0.5s linear; + -ms-transition: box-shadow 0.5s linear; + -o-transition: box-shadow 0.5s linear; + transition: box-shadow 0.5s linear; + display: table !important; + width: 100%; +} + +.memitem.glow { + box-shadow: 0 0 15px cyan; +} + +.memname { + font-weight: 400; + margin-left: 6px; +} + +.memname td { + vertical-align: bottom; +} + +.memproto, dl.reflist dt { + border-top: 1px solid #A8B8D9; + border-left: 1px solid #A8B8D9; + border-right: 1px solid #A8B8D9; + padding: 6px 0px 6px 0px; + color: #253555; + font-weight: bold; + text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9); + background-color: #DFE5F1; + /* opera specific markup */ + box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + border-top-right-radius: 4px; + /* firefox specific markup */ + -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px; + -moz-border-radius-topright: 4px; + /* webkit specific markup */ + -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + -webkit-border-top-right-radius: 4px; + +} + +.overload { + font-family: "courier new",courier,monospace; + font-size: 65%; +} + +.memdoc, dl.reflist dd { + border-bottom: 1px solid #A8B8D9; + border-left: 1px solid #A8B8D9; + border-right: 1px solid #A8B8D9; + padding: 6px 10px 2px 10px; + background-color: #FBFCFD; + border-top-width: 0; + background-image:url('nav_g.png'); + background-repeat:repeat-x; + background-color: #FFFFFF; + /* opera specific markup */ + border-bottom-left-radius: 4px; + border-bottom-right-radius: 4px; + box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + /* firefox specific markup */ + -moz-border-radius-bottomleft: 4px; + -moz-border-radius-bottomright: 4px; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px; + /* webkit specific markup */ + -webkit-border-bottom-left-radius: 4px; + -webkit-border-bottom-right-radius: 4px; + -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); +} + +dl.reflist dt { + padding: 5px; +} + +dl.reflist dd { + margin: 0px 0px 10px 0px; + padding: 5px; +} + +.paramkey { + text-align: right; } -.SRChildren { - padding-left: 3ex; padding-bottom: .5em +.paramtype { + white-space: nowrap; } -.SRPage .SRChildren { - display: none; + +.paramname { + color: #602020; + white-space: nowrap; } -a.SRScope { - display: block; +.paramname em { + font-style: normal; } -a.SRSymbol:focus, a.SRSymbol:active, -a.SRScope:focus, a.SRScope:active { - text-decoration: underline; +.paramname code { + line-height: 14px; } -span.SRScope { - padding-left: 4px; + +.params, .retval, .exception, .tparams { + margin-left: 0px; + padding-left: 0px; +} + +.params .paramname, .retval .paramname, .tparams .paramname, .exception .paramname { + font-weight: bold; + vertical-align: top; +} + +.params .paramtype, .tparams .paramtype { + font-style: italic; + vertical-align: top; +} + +.params .paramdir, .tparams .paramdir { + font-family: "courier new",courier,monospace; + vertical-align: top; } -.SRResult { - display: none; + +table.mlabels { + border-spacing: 0px; +} + +td.mlabels-left { + width: 100%; + padding: 0px; +} + +td.mlabels-right { + vertical-align: bottom; + padding: 0px; + white-space: nowrap; +} + +span.mlabels { + margin-left: 8px; } -/* class and file list */ -.directory .icona, -.directory .arrow { - height: auto; +span.mlabel { + background-color: #728DC1; + border-top:1px solid #5373B4; + border-left:1px solid #5373B4; + border-right:1px solid #C4CFE5; + border-bottom:1px solid #C4CFE5; + text-shadow: none; + color: white; + margin-right: 4px; + padding: 2px 3px; + border-radius: 3px; + font-size: 7pt; + white-space: nowrap; + vertical-align: middle; } -.directory .icona .icon { - height: 16px; + + + +/* @end */ + +/* these are for tree view inside a (index) page */ + +div.directory { + margin: 10px 0px; + border-top: 1px solid #9CAFD4; + border-bottom: 1px solid #9CAFD4; + width: 100%; } -.directory .icondoc { - background-position: 0px 0px; - height: 20px; + +.directory table { + border-collapse:collapse; } -.directory .iconfopen { - background-position: 0px 0px; + +.directory td { + margin: 0px; + padding: 0px; + vertical-align: top; } + .directory td.entry { - padding: 7px 8px 6px 8px; + white-space: nowrap; + padding-right: 6px; + padding-top: 3px; } -.table > tbody > tr > td.memSeparator { - line-height: 0; - .table-hover; +.directory td.entry a { + outline:none; +} +.directory td.entry a img { + border: none; } -.memItemLeft, .memTemplItemLeft { - white-space: normal; +.directory td.desc { + width: 100%; + padding-left: 6px; + padding-right: 6px; + padding-top: 3px; + border-left: 1px solid rgba(0,0,0,0.05); } -/* enumerations */ -.panel-body thead > tr { - background-color: #e0e0e0; +.directory tr.even { + padding-left: 6px; + background-color: #F7F8FB; } -/* todo lists */ -.todoname, -.todoname a { - font-weight: bold; +.directory img { + vertical-align: -30%; } -/* Class title */ -.summary { - margin-top: 25px; +.directory .levels { + white-space: nowrap; + width: 100%; + text-align: right; + font-size: 9pt; } -.page-header { - margin: 20px 0px !important; + +.directory .levels span { + cursor: pointer; + padding-left: 2px; + padding-right: 2px; + color: #3D578C; } -.page-header .title { - display: inline-block; + +.icon { + font-family: Arial, Helvetica; + font-weight: bold; + font-size: 12px; + height: 14px; + width: 16px; + display: inline-block; + background-color: #728DC1; + color: white; + text-align: center; + border-radius: 4px; + margin-left: 2px; + margin-right: 2px; +} + +.icona { + width: 24px; + height: 22px; + display: inline-block; +} + +.iconfopen { + width: 24px; + height: 18px; + margin-bottom: 4px; + background-image:url('folderopen.png'); + background-position: 0px -4px; + background-repeat: repeat-y; + vertical-align:top; + display: inline-block; +} + +.iconfclosed { + width: 24px; + height: 18px; + margin-bottom: 4px; + background-image:url('folderclosed.png'); + background-position: 0px -4px; + background-repeat: repeat-y; + vertical-align:top; + display: inline-block; +} + +.icondoc { + width: 24px; + height: 18px; + margin-bottom: 4px; + background-image:url('doc.png'); + background-position: 0px -4px; + background-repeat: repeat-y; + vertical-align:top; + display: inline-block; +} + +table.directory { + font: 400 14px Roboto,sans-serif; +} + +/* @end */ + +div.dynheader { + margin-top: 8px; + -webkit-touch-callout: none; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; +} + +address { + font-style: normal; + color: #2A3D61; +} + +table.doxtable caption { + caption-side: top; +} + +table.doxtable { + border-collapse:collapse; + margin-top: 4px; + margin-bottom: 4px; +} + +table.doxtable td, table.doxtable th { + border: 1px solid #2D4068; + padding: 3px 7px 2px; +} + +table.doxtable th { + background-color: #374F7F; + color: #FFFFFF; + font-size: 110%; + padding-bottom: 4px; + padding-top: 5px; +} + +table.fieldtable { + /*width: 100%;*/ + margin-bottom: 10px; + border: 1px solid #A8B8D9; + border-spacing: 0px; + -moz-border-radius: 4px; + -webkit-border-radius: 4px; + border-radius: 4px; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px; + -webkit-box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15); + box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15); +} + +.fieldtable td, .fieldtable th { + padding: 3px 7px 2px; +} + +.fieldtable td.fieldtype, .fieldtable td.fieldname { + white-space: nowrap; + border-right: 1px solid #A8B8D9; + border-bottom: 1px solid #A8B8D9; + vertical-align: top; +} + +.fieldtable td.fieldname { + padding-top: 3px; +} + +.fieldtable td.fielddoc { + border-bottom: 1px solid #A8B8D9; + /*width: 100%;*/ +} + +.fieldtable td.fielddoc p:first-child { + margin-top: 0px; +} + +.fieldtable td.fielddoc p:last-child { + margin-bottom: 2px; +} + +.fieldtable tr:last-child td { + border-bottom: none; +} + +.fieldtable th { + background-image:url('nav_f.png'); + background-repeat:repeat-x; + background-color: #E2E8F2; + font-size: 90%; + color: #253555; + padding-bottom: 4px; + padding-top: 5px; + text-align:left; + font-weight: 400; + -moz-border-radius-topleft: 4px; + -moz-border-radius-topright: 4px; + -webkit-border-top-left-radius: 4px; + -webkit-border-top-right-radius: 4px; + border-top-left-radius: 4px; + border-top-right-radius: 4px; + border-bottom: 1px solid #A8B8D9; +} + + +.tabsearch { + top: 0px; + left: 10px; + height: 36px; + background-image: url('tab_b.png'); + z-index: 101; + overflow: hidden; + font-size: 13px; +} + +.navpath ul +{ + font-size: 11px; + background-image:none; + background-repeat:repeat-x; + background-position: 0 -5px; + height:30px; + line-height:30px; + color:#8AA0CC; + border:solid 1px #C2CDE4; + overflow:hidden; + margin:0px; + padding:0px; +} + +.navpath li +{ + list-style-type:none; + float:left; + padding-left:10px; + padding-right:15px; + background-image:url('bc_s.png'); + background-repeat:no-repeat; + background-position:right; + color:#364D7C; +} + +.navpath li.navelem a +{ + height:32px; + display:block; + text-decoration: none; + outline: none; + color: #283A5D; + font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif; + text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9); + text-decoration: none; +} + +.navpath li.navelem a:hover +{ + color:#6884BD; +} + +.navpath li.footer +{ + list-style-type:none; + float:right; + padding-left:10px; + padding-right:15px; + background-image:none; + background-repeat:no-repeat; + background-position:right; + color:#364D7C; + font-size: 8pt; +} + + +div.summary +{ + float: right; + font-size: 8pt; + padding-right: 5px; + width: 50%; + text-align: right; +} + +div.summary a +{ + white-space: nowrap; +} + +table.classindex +{ + margin: 10px; + white-space: nowrap; + margin-left: 3%; + margin-right: 3%; + width: 94%; + border: 0; + border-spacing: 0; + padding: 0; +} + +div.ingroups +{ + font-size: 8pt; + width: 50%; + text-align: left; +} + +div.ingroups a +{ + white-space: nowrap; +} + +div.header +{ + background-image:url('nav_h.png'); + background-repeat:repeat-x; + background-color: #F9FAFC; + margin: 0px; + border-bottom: 1px solid #C4CFE5; +} + +div.headertitle +{ + padding: 5px 5px 5px 10px; +} + +.PageDocRTL-title div.headertitle { + text-align: right; + direction: rtl; +} + +dl { + padding: 0 0 0 0; +} + +/* dl.note, dl.warning, dl.attention, dl.pre, dl.post, dl.invariant, dl.deprecated, dl.todo, dl.test, dl.bug, dl.examples */ +dl.section { + margin-left: 0px; + padding-left: 0px; +} + +dl.section.DocNodeRTL { + margin-right: 0px; + padding-right: 0px; +} + +dl.note { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #D0C000; +} + +dl.note.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #D0C000; +} + +dl.warning, dl.attention { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #FF0000; +} + +dl.warning.DocNodeRTL, dl.attention.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #FF0000; +} + +dl.pre, dl.post, dl.invariant { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #00D000; +} + +dl.pre.DocNodeRTL, dl.post.DocNodeRTL, dl.invariant.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #00D000; +} + +dl.deprecated { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #505050; +} + +dl.deprecated.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #505050; +} + +dl.todo { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #00C0E0; +} + +dl.todo.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #00C0E0; +} + +dl.test { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #3030E0; +} + +dl.test.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #3030E0; +} + +dl.bug { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #C08050; +} + +dl.bug.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #C08050; +} + +dl.section dd { + margin-bottom: 6px; +} + + +#projectlogo +{ + text-align: center; + vertical-align: bottom; + border-collapse: separate; +} + +#projectlogo img +{ + border: 0px none; +} + +#projectalign +{ + vertical-align: middle; +} + +#projectname +{ + font: 300% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 2px 0px; +} + +#projectbrief +{ + font: 120% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 0px; +} + +#projectnumber +{ + font: 50% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 0px; +} + +#titlearea +{ + padding: 0px; + margin: 0px; + width: 100%; + border-bottom: 1px solid #32363d; +} + +.image +{ + text-align: center; +} + +.dotgraph +{ + text-align: center; +} + +.mscgraph +{ + text-align: center; +} + +.plantumlgraph +{ + text-align: center; +} + +.diagraph +{ + text-align: center; +} + +.caption +{ + font-weight: bold; +} + +div.zoom +{ + border: 1px solid #90A5CE; +} + +dl.citelist { + margin-bottom:50px; +} + +dl.citelist dt { + color:#334975; + float:left; + font-weight:bold; + margin-right:10px; + padding:5px; +} + +dl.citelist dd { + margin:2px 0; + padding:5px 0; } -.page-header .pull-right { - margin-top: 0.3em; - margin-left: 0.5em; + +div.toc { + padding: 14px 25px; + background-color: #F4F6FA; + border: 1px solid #D8DFEE; + border-radius: 7px 7px 7px 7px; + float: right; + height: auto; + margin: 0 8px 10px 10px; + width: 200px; +} + +.PageDocRTL-title div.toc { + float: left !important; + text-align: right; +} + +div.toc li { + background: url("bdwn.png") no-repeat scroll 0 5px transparent; + font: 10px/1.2 Verdana,DejaVu Sans,Geneva,sans-serif; + margin-top: 5px; + padding-left: 10px; + padding-top: 2px; +} + +.PageDocRTL-title div.toc li { + background-position-x: right !important; + padding-left: 0 !important; + padding-right: 10px; +} + +div.toc h3 { + font: bold 12px/1.2 Arial,FreeSans,sans-serif; + color: #4665A2; + border-bottom: 0 none; + margin: 0; +} + +div.toc ul { + list-style: none outside none; + border: medium none; + padding: 0px; +} + +div.toc li.level1 { + margin-left: 0px; +} + +div.toc li.level2 { + margin-left: 15px; +} + +div.toc li.level3 { + margin-left: 30px; +} + +div.toc li.level4 { + margin-left: 45px; +} + +.PageDocRTL-title div.toc li.level1 { + margin-left: 0 !important; + margin-right: 0; +} + +.PageDocRTL-title div.toc li.level2 { + margin-left: 0 !important; + margin-right: 15px; +} + +.PageDocRTL-title div.toc li.level3 { + margin-left: 0 !important; + margin-right: 30px; +} + +.PageDocRTL-title div.toc li.level4 { + margin-left: 0 !important; + margin-right: 45px; +} + +.inherit_header { + font-weight: bold; + color: gray; + cursor: pointer; + -webkit-touch-callout: none; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; +} + +.inherit_header td { + padding: 6px 0px 2px 5px; +} + +.inherit { + display: none; +} + +tr.heading h2 { + margin-top: 12px; + margin-bottom: 4px; +} + +/* tooltip related style info */ + +.ttc { + position: absolute; + display: none; +} + +#powerTip { + cursor: default; + white-space: nowrap; + background-color: white; + border: 1px solid gray; + border-radius: 4px 4px 4px 4px; + box-shadow: 1px 1px 7px gray; + display: none; + font-size: smaller; + max-width: 80%; + opacity: 0.9; + padding: 1ex 1em 1em; + position: absolute; + z-index: 2147483647; +} + +#powerTip div.ttdoc { + color: grey; + font-style: italic; +} + +#powerTip div.ttname a { + font-weight: bold; +} + +#powerTip div.ttname { + font-weight: bold; +} + +#powerTip div.ttdeci { + color: #006318; +} + +#powerTip div { + margin: 0px; + padding: 0px; + font: 12px/16px Roboto,sans-serif; +} + +#powerTip:before, #powerTip:after { + content: ""; + position: absolute; + margin: 0px; +} + +#powerTip.n:after, #powerTip.n:before, +#powerTip.s:after, #powerTip.s:before, +#powerTip.w:after, #powerTip.w:before, +#powerTip.e:after, #powerTip.e:before, +#powerTip.ne:after, #powerTip.ne:before, +#powerTip.se:after, #powerTip.se:before, +#powerTip.nw:after, #powerTip.nw:before, +#powerTip.sw:after, #powerTip.sw:before { + border: solid transparent; + content: " "; + height: 0; + width: 0; + position: absolute; +} + +#powerTip.n:after, #powerTip.s:after, +#powerTip.w:after, #powerTip.e:after, +#powerTip.nw:after, #powerTip.ne:after, +#powerTip.sw:after, #powerTip.se:after { + border-color: rgba(255, 255, 255, 0); +} + +#powerTip.n:before, #powerTip.s:before, +#powerTip.w:before, #powerTip.e:before, +#powerTip.nw:before, #powerTip.ne:before, +#powerTip.sw:before, #powerTip.se:before { + border-color: rgba(128, 128, 128, 0); +} + +#powerTip.n:after, #powerTip.n:before, +#powerTip.ne:after, #powerTip.ne:before, +#powerTip.nw:after, #powerTip.nw:before { + top: 100%; } -.page-header .label { - font-size: 50%; + +#powerTip.n:after, #powerTip.ne:after, #powerTip.nw:after { + border-top-color: #FFFFFF; + border-width: 10px; + margin: 0px -10px; +} +#powerTip.n:before { + border-top-color: #808080; + border-width: 11px; + margin: 0px -11px; +} +#powerTip.n:after, #powerTip.n:before { + left: 50%; +} + +#powerTip.nw:after, #powerTip.nw:before { + right: 14px; +} + +#powerTip.ne:after, #powerTip.ne:before { + left: 14px; +} + +#powerTip.s:after, #powerTip.s:before, +#powerTip.se:after, #powerTip.se:before, +#powerTip.sw:after, #powerTip.sw:before { + bottom: 100%; +} + +#powerTip.s:after, #powerTip.se:after, #powerTip.sw:after { + border-bottom-color: #FFFFFF; + border-width: 10px; + margin: 0px -10px; +} + +#powerTip.s:before, #powerTip.se:before, #powerTip.sw:before { + border-bottom-color: #808080; + border-width: 11px; + margin: 0px -11px; +} + +#powerTip.s:after, #powerTip.s:before { + left: 50%; } + +#powerTip.sw:after, #powerTip.sw:before { + right: 14px; +} + +#powerTip.se:after, #powerTip.se:before { + left: 14px; +} + +#powerTip.e:after, #powerTip.e:before { + left: 100%; +} +#powerTip.e:after { + border-left-color: #FFFFFF; + border-width: 10px; + top: 50%; + margin-top: -10px; +} +#powerTip.e:before { + border-left-color: #808080; + border-width: 11px; + top: 50%; + margin-top: -11px; +} + +#powerTip.w:after, #powerTip.w:before { + right: 100%; +} +#powerTip.w:after { + border-right-color: #FFFFFF; + border-width: 10px; + top: 50%; + margin-top: -10px; +} +#powerTip.w:before { + border-right-color: #808080; + border-width: 11px; + top: 50%; + margin-top: -11px; +} + +@media print +{ + #top { display: none; } + #side-nav { display: none; } + #nav-path { display: none; } + body { overflow:visible; } + h1, h2, h3, h4, h5, h6 { page-break-after: avoid; } + .summary { display: none; } + .memitem { page-break-inside: avoid; } + #doc-content + { + margin-left:0 !important; + height:auto !important; + width:auto !important; + overflow:inherit; + display:inline; + } +} + +/* @group Markdown */ + +table.markdownTable { + border-collapse:collapse; + margin-top: 4px; + margin-bottom: 4px; +} + +table.markdownTable td, table.markdownTable th { + border: 1px solid #2D4068; + padding: 3px 7px 2px; +} + +table.markdownTable tr { +} + +th.markdownTableHeadLeft, th.markdownTableHeadRight, th.markdownTableHeadCenter, th.markdownTableHeadNone { + background-color: #374F7F; + color: #FFFFFF; + font-size: 110%; + padding-bottom: 4px; + padding-top: 5px; +} + +th.markdownTableHeadLeft, td.markdownTableBodyLeft { + text-align: left +} + +th.markdownTableHeadRight, td.markdownTableBodyRight { + text-align: right +} + +th.markdownTableHeadCenter, td.markdownTableBodyCenter { + text-align: center +} + +.DocNodeRTL { + text-align: right; + direction: rtl; +} + +.DocNodeLTR { + text-align: left; + direction: ltr; +} + +table.DocNodeRTL { + width: auto; + margin-right: 0; + margin-left: auto; +} + +table.DocNodeLTR { + width: auto; + margin-right: auto; + margin-left: 0; +} + +tt, code, kbd, samp +{ + display: inline-block; + direction:ltr; +} +/* @end */ + +u { + text-decoration: underline; +} + diff --git a/doc/style/freeflyer_footer.html b/doc/style/freeflyer_footer.html index b80c6ca437..ed4fffc8aa 100644 --- a/doc/style/freeflyer_footer.html +++ b/doc/style/freeflyer_footer.html @@ -1,26 +1,13 @@ - + - + - - - - -
-$generatedby   -doxygen - $doxygenversion +$generatedby doxygen $doxygenversion
+ - \ No newline at end of file + diff --git a/doc/style/freeflyer_header.html b/doc/style/freeflyer_header.html index a849076463..5d128fd88c 100644 --- a/doc/style/freeflyer_header.html +++ b/doc/style/freeflyer_header.html @@ -1,45 +1,58 @@ - - + + - - - - + + + + + +$projectname: $title +$title + + - - - - + + +$treeview +$search +$mathjax + +$extrastylesheet + + +
- $projectname: $title - $title - - - $treeview - $search - $mathjax - - $extrastylesheet - - - - - - - -
-
-
-
-
-
- \ No newline at end of file + +
+ + + + + + + + + + + + + + + + + + + + + +
+
$projectname +  $projectnumber +
+
$projectbrief
+ 		+
$projectbrief
+ 		$searchbox
+
+ + diff --git a/doc/style/freeflyer_layout.xml b/doc/style/freeflyer_layout.xml index 2109ef3c9d..f4588fad51 100644 --- a/doc/style/freeflyer_layout.xml +++ b/doc/style/freeflyer_layout.xml @@ -1,25 +1,39 @@ - + - - - - - + + + + + - - + + + + + + + - - + + + + + + + + + + + - - - + + + - + @@ -87,8 +101,13 @@ + + + + + @@ -98,6 +117,8 @@ + + @@ -113,11 +134,16 @@ + + + + + @@ -128,6 +154,8 @@ + + @@ -139,15 +167,16 @@ - - + + + @@ -161,11 +190,14 @@ + + + diff --git a/external/CATKIN_IGNORE b/external/CATKIN_IGNORE deleted file mode 100644 index 8d1c8b69c3..0000000000 --- a/external/CATKIN_IGNORE +++ /dev/null @@ -1 +0,0 @@ - diff --git a/external/CMakeLists.txt b/external/CMakeLists.txt deleted file mode 100644 index 09e17b861b..0000000000 --- a/external/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -# all the external packages have lots of warnings, ignore them -#STRING(REGEX REPLACE "-Wall" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -#STRING(REGEX REPLACE "-Wall" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") -#STRING(REGEX REPLACE "-Werror" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -#STRING(REGEX REPLACE "-Werror" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") -#set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -w") -#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") - -# gtest is needed because no binaries distributed with ubuntu!!! -add_subdirectory(gtest) -set(GTEST_LIBRARIES ${GTEST_LIBRARIES} PARENT_SCOPE) -set(GTEST_INCLUDE_DIRS ${GTEST_INCLUDE_DIRS} PARENT_SCOPE) - diff --git a/external/gtest/CMakeLists.txt b/external/gtest/CMakeLists.txt index 0d3aad1089..2255c189ff 100644 --- a/external/gtest/CMakeLists.txt +++ b/external/gtest/CMakeLists.txt @@ -28,8 +28,15 @@ CMakeExternalProject( set(GTEST_INCLUDE_DIRS ${gtest_noros_SOURCE_DIR}/include PARENT_SCOPE) if (BUILD_SHARED_LIBS) - set(GTEST_LIBRARIES ${gtest_noros_BINARY_DIR}/libgtest${CMAKE_SHARED_LIBRARY_SUFFIX} - ${gtest_noros_BINARY_DIR}/libgtest_main${CMAKE_SHARED_LIBRARY_SUFFIX}) + + if("${ROS_DISTRO}" STREQUAL "noetic") + set(GTEST_LIBRARIES ${gtest_noros_BINARY_DIR}/lib/libgtest${CMAKE_SHARED_LIBRARY_SUFFIX} + ${gtest_noros_BINARY_DIR}/lib/libgtest_main${CMAKE_SHARED_LIBRARY_SUFFIX}) + else() + set(GTEST_LIBRARIES ${gtest_noros_BINARY_DIR}/libgtest${CMAKE_SHARED_LIBRARY_SUFFIX} + ${gtest_noros_BINARY_DIR}/libgtest_main${CMAKE_SHARED_LIBRARY_SUFFIX}) + endif() + else (BUILD_SHARED_LIBS) set(GTEST_LIBRARIES ${gtest_noros_BINARY_DIR}/libgtest.a ${gtest_noros_BINARY_DIR}/libgtest_main.a) diff --git a/gnc/CMakeLists.txt b/gnc/CMakeLists.txt deleted file mode 100644 index a4cca4df8e..0000000000 --- a/gnc/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -add_subdirectory(ctl) -add_subdirectory(ekf) -add_subdirectory(fam) -add_subdirectory(gnc_autocode) -add_subdirectory(sim_wrapper) diff --git a/gnc/ctl/CMakeLists.txt b/gnc/ctl/CMakeLists.txt index e01a6a06f2..1723839bed 100644 --- a/gnc/ctl/CMakeLists.txt +++ b/gnc/ctl/CMakeLists.txt @@ -15,20 +15,55 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(ctl) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + ff_msgs + ff_util + config_reader +) +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + catkin_package( - LIBRARIES ctl - CATKIN_DEPENDS roscpp sensor_msgs nav_msgs - DEPENDS ff_msgs ff_hw_msgs ff_common + LIBRARIES + ctl + CATKIN_DEPENDS + roscpp + nodelet + ff_msgs + ff_util + config_reader ) -create_library(TARGET ctl - LIBS ${catkin_LIBRARIES} ${EIGEN_LIBRARIES} gnc_autocode msg_conversions ff_common config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - DEPS ff_msgs ff_hw_msgs) +########### +## Build ## +########### -if(CATKIN_ENABLE_TESTING) +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +# Declare C++ libraries +add_library(ctl + src/ctl.cc + src/ctl_nodelet.cc + src/ctl_ros.cc +) +add_dependencies(ctl ${catkin_EXPORTED_TARGETS}) +target_link_libraries(ctl ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING AND ENABLE_INTEGRATION_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(test_ctl test/test_ctl.test test/test_ctl_nominal.cc @@ -38,4 +73,32 @@ if(CATKIN_ENABLE_TESTING) ) endif() -install_launch_files() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + diff --git a/gnc/ctl/ctl_fsm.dot b/gnc/ctl/ctl_fsm.dot new file mode 100644 index 0000000000..a5c5f9b68f --- /dev/null +++ b/gnc/ctl/ctl_fsm.dot @@ -0,0 +1,27 @@ +# dot -Tpdf ctl_fsm.dot -o ctl_fsm.pdf +digraph G { + graph [label="CTL\n", labelloc=t, fontsize=50]; + size="11.7,8.3!"; + node [shape=box, width=2.5, fontsize=12, fontname="helvetica"]; + edge [arrowsize=2, weight=2., fontsize=12, fontname="helvetica"]; + fontname = "helvetica"; + + node [shape=diamond, style=filled, fillcolor=lightblue, color=black]; + WAITING; + + node [shape=box, style=filled, fillcolor=lightblue, color=black]; + NOMINAL; + + # Actions + WAITING -> NOMINAL + [label="[0]\nGOAL_NOMINAL\nControl(NOMINAL, Segment)", color=black]; + NOMINAL -> STOPPING + [label="[1]\nGOAL_COMPLETE\nControl(STOP)", color=black]; + NOMINAL -> WAITING + [label="[2]\nGOAL_CANCEL\nControl(STOP)\nResult(CANCELLED)", color=black]; + WAITING -> STOPPING + [label="[3]\nGOAL_STOP\nControl(STOP)", color=black]; + STOPPING -> WAITING + [label="[4]\nGOAL_COMPLETE\nResult(SUCCESS)", color=black]; + +} diff --git a/gnc/ctl/include/ctl/ctl.h b/gnc/ctl/include/ctl/ctl.h index 4387e1a9b8..89ae651977 100644 --- a/gnc/ctl/include/ctl/ctl.h +++ b/gnc/ctl/include/ctl/ctl.h @@ -1,14 +1,14 @@ /* Copyright (c) 2017, United States Government, as represented by the * Administrator of the National Aeronautics and Space Administration. - * + * * All rights reserved. - * + * * The Astrobee platform is licensed under the Apache License, Version 2.0 * (the "License"); you may not use this file except in compliance with the * License. You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -19,167 +19,133 @@ #ifndef CTL_CTL_H_ #define CTL_CTL_H_ -// Autocode includes -#include +#include -// For includes -#include +namespace config_reader { + class ConfigReader; +} -// Standard messages -#include -#include -#include - -// FSW actions -#include - -// FSW messages -#include -#include -#include - -// Libraries to handle flight config and segment processing -#include -#include -#include -#include - -// Libraries to read LIA config file -#include - -// STL includes -#include -#include -#include -#include +namespace constants { +const unsigned int ase_status_converged = 0U; +const unsigned int ctl_idle_mode = 0U; +const unsigned int ctl_stopping_mode = 1U; +const unsigned int ctl_stopped_mode = 3U; +} // namespace constants namespace ctl { -using FSM = ff_util::FSM; - -/** - * @brief Controller implementation using GNC module - * @details Controller implementation using GNC module - */ -class Ctl { - public: - // Declaration of all possible states - enum : ff_util::FSM::State { - WAITING = 1, - NOMINAL = 2, - STOPPING = 3 - }; - - // Declaration of all possible events - enum : ff_util::FSM::Event { - GOAL_COMPLETE = (1<<0), - GOAL_NOMINAL = (1<<1), - GOAL_CANCEL = (1<<2), - GOAL_STOP = (1<<3) - }; - - // Maximum acceptable latency - static constexpr double MAX_LATENCY = 0.5; - - /** - * Ctl allocate, register, initialize model - */ - explicit Ctl(ros::NodeHandle* nh, std::string const& name); - /** - * destruct model - */ - ~Ctl(); - - // Terminate execution in either IDLE or STOP mode - FSM::State Result(int32_t response); - - // GENERAL MESSAGE CALLBACKS - - // Called when the internal state changes - void UpdateCallback(FSM::State const& state, FSM::Event const& event); - - // Called when a pose estimate is available - void EkfCallback(const ff_msgs::EkfState::ConstPtr& state); +struct ControlState { + Eigen::Vector3f est_P_B_ISS_ISS; + Eigen::Quaternionf est_quat_ISS2B; + Eigen::Vector3f est_V_B_ISS_ISS; + Eigen::Vector3f est_omega_B_ISS_B; - // Called when localization has a new pose data - void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& truth); + Eigen::Matrix inertia; - // Called when localization has new twist data - void TwistCallback(const geometry_msgs::TwistStamped::ConstPtr& truth); + // configuration + Eigen::Vector3f att_kp; + Eigen::Vector3f att_ki; + Eigen::Vector3f omega_kd; + Eigen::Vector3f pos_kp; + Eigen::Vector3f pos_ki; + Eigen::Vector3f vel_kd; - // Called when management updates inertial info - void InertiaCallback(const geometry_msgs::InertiaStamped::ConstPtr& inertia); - - // Called when a timer has called back to progress control to next setpoint - void ControlTimerCallback(const ros::TimerEvent & event); - - // Called when the choreographer updates flight modes - void FlightModeCallback(const ff_msgs::FlightMode::ConstPtr& mode); - - // Command GNC directly, bypassing the action-base dinterface - void SetpointCallback(const ff_msgs::ControlState::ConstPtr& command); - - // Used to feed segments - void TimerCallback(const ros::TimerEvent& event); - - // ACTION CLIENT - - // Called when a new goal arrives - void GoalCallback(ff_msgs::ControlGoalConstPtr const& control_goal); - - // Called when a goal is preempted - void PreemptCallback(); - - // Called when a goal is cancelled - void CancelCallback(); - - // ORIGINAL GNC FUNCTIONS + float mass; + uint8_t est_confidence; +}; - // Update control to take a new setpoint - bool Control(uint8_t const mode, - ff_msgs::ControlCommand const& poseVel = ff_msgs::ControlCommand()); +struct ControlCommand { + Eigen::Vector3f P_B_ISS_ISS; + Eigen::Quaternionf quat_ISS2B; + Eigen::Vector3f V_B_ISS_ISS; + Eigen::Vector3f A_B_ISS_ISS; + Eigen::Vector3f omega_B_ISS_ISS; + Eigen::Vector3f alpha_B_ISS_ISS; - // Step control forward - bool Step(void); + uint8_t mode; +}; - ctl_msg* GetCtlMsg(void) {return &gnc_.ctl_;} - cmd_msg* GetCmdMsg(void) {return &gnc_.cmd_;} +struct ControlOutput { + Eigen::Vector3f body_force_cmd; + Eigen::Vector3f body_torque_cmd; + Eigen::Vector3f body_accel_cmd; + Eigen::Vector3f body_alpha_cmd; + Eigen::Vector3f pos_err; + Eigen::Vector3f pos_err_int; + Eigen::Vector3f att_err; + Eigen::Vector3f att_err_int; + + Eigen::Vector3f traj_pos; + Eigen::Quaternionf traj_quat; + Eigen::Vector3f traj_vel; + Eigen::Vector3f traj_accel; + Eigen::Vector3f traj_omega; + Eigen::Vector3f traj_alpha; + + float att_err_mag; + + float traj_error_pos; + float traj_error_att; + float traj_error_vel; + float traj_error_omega; + + uint8_t ctl_status; +}; - // Read the control parameters from the LUA config file - void ReadParams(void); +class Control { + public: + Control(void); - // Simple extension to allow NODELET_* logging calls - std::string getName(); + virtual void Initialize(void); + virtual void Step(float time_delta, ControlState & state, ControlCommand & cmd, ControlOutput* out); + virtual void ReadParams(config_reader::ConfigReader* config); private: - // Proxy to gnc - gnc_autocode::GncCtlAutocode gnc_; - - std::mutex mutex_cmd_msg_, mutex_segment_; - - ros::Subscriber truth_pose_sub_, inertia_sub_, flight_mode_sub_; - ros::Subscriber twist_sub_, pose_sub_, ekf_sub_, command_sub_; - ros::Publisher ctl_pub_, traj_pub_, segment_pub_, progress_pub_; - ros::Timer timer_; - - ff_util::FreeFlyerActionServer action_; - ff_util::FSM fsm_; - ff_util::Segment segment_; - ff_util::Segment::iterator setpoint_; - ff_msgs::ControlFeedback feedback_; - - config_reader::ConfigReader config_; - ff_util::PerfTimer pt_ctl_; - ros::Timer config_timer_; - - std::string name_; - bool inertia_received_; - bool control_enabled_; - bool use_truth_; - float stopping_vel_thresh_squared_; - float stopping_omega_thresh_squared_; + int mode_cmd_; + bool stopped_mode_; + Eigen::Vector3f prev_filter_vel_; + Eigen::Vector3f prev_filter_omega_; + int prev_mode_cmd_[5]; // for the 4 ticks required to switch to stopped; newest val at index 0 + Eigen::Vector3f prev_position_; + Eigen::Quaternionf prev_att_; + Eigen::Vector3f linear_integrator_; + Eigen::Vector3f rotational_integrator_; + + bool FilterThreshold(Eigen::Vector3f vec, float threshhold, Eigen::Vector3f & previous); + float ButterWorthFilter(float input, float delay_val, float* sum_out); + float QuatError(Eigen::Quaternionf cmd, Eigen::Quaternionf actual); + void UpdateCtlStatus(const ControlState & state, ControlOutput* out); + Eigen::Vector3f SafeDivide(const Eigen::Vector3f & num, const Eigen::Vector3f & denom); + void FindPosErr(const ControlState & state, const ControlCommand & cmd, ControlOutput* out); + Eigen::Vector3f DiscreteTimeIntegrator(const Eigen::Vector3f input, Eigen::Vector3f & accumulator, + uint8_t ctl_status, float upper_limit, float lower_limit); + void FindBodyForceCmd(const ControlState & state, ControlOutput* out); + Eigen::Vector3f RotateVectorAtoB(Eigen::Vector3f, Eigen::Quaternionf); + Eigen::Vector3f SaturateVector(Eigen::Vector3f, float limit); + void FindBodyAlphaTorqueCmd(const ControlState & state, ControlOutput* out); + void FindAttErr(const ControlState & state, ControlOutput* out); + + void ForwardTrajectory(float time_delta, const ControlState & state, const ControlCommand & cmd, + ControlOutput* out); + void UpdateMode(const ControlState & state, const ControlCommand & cmd); + void UpdatePrevious(const ControlState & state); + + Eigen::Matrix OmegaMatrix(Eigen::Vector3f input); + + float tun_vel_gain; + Eigen::Vector3f tun_accel_gain; + Eigen::Vector3f tun_alpha_gain; + float tun_ctl_stopping_omega_thresh; + float tun_ctl_stopping_vel_thresh; + float tun_ctl_stopped_pos_thresh; + float tun_ctl_stopped_quat_thresh; + float tun_ctl_pos_sat_upper; + float tun_ctl_pos_sat_lower; + float tun_ctl_linear_force_limit; + float tun_ctl_att_sat_upper; + float tun_ctl_att_sat_lower; }; - } // end namespace ctl #endif // CTL_CTL_H_ + diff --git a/gnc/ctl/include/ctl/ctl_ros.h b/gnc/ctl/include/ctl/ctl_ros.h new file mode 100644 index 0000000000..4a8e3ee2ba --- /dev/null +++ b/gnc/ctl/include/ctl/ctl_ros.h @@ -0,0 +1,197 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef CTL_CTL_ROS_H_ +#define CTL_CTL_ROS_H_ + +#include + +// For includes +#include + +// Standard messages +#include +#include +#include + +// Service message +#include + +// FSW actions +#include + +// FSW messages +#include +#include +#include + +// Libraries to handle flight config and segment processing +#include +#include +#include +#include + +// Libraries to read LIA config file +#include + +// STL includes +#include +#include +#include +#include + +namespace ctl { + +using FSM = ff_util::FSM; + +/** + * @brief Controller implementation using GNC module + * @details Controller implementation using GNC module + */ +class Ctl { + public: + // Declaration of all possible states + enum : ff_util::FSM::State { + WAITING = 1, + NOMINAL = 2, + STOPPING = 3 + }; + + // Declaration of all possible events + enum : ff_util::FSM::Event { + GOAL_COMPLETE = (1<<0), + GOAL_NOMINAL = (1<<1), + GOAL_CANCEL = (1<<2), + GOAL_STOP = (1<<3) + }; + + // Maximum acceptable latency + static constexpr double MAX_LATENCY = 0.5; + + /** + * Ctl allocate, register, initialize model + */ + explicit Ctl(ros::NodeHandle* nh, std::string const& name); + /** + * destruct model + */ + ~Ctl(); + + // Terminate execution in either IDLE or STOP mode + FSM::State Result(int32_t response); + + // SERVICE CALLBACKS + + // Enable/Disable Onboard Controller + bool EnableCtl(std_srvs::SetBoolRequest&req, std_srvs::SetBoolResponse &response); + + // GENERAL MESSAGE CALLBACKS + + // Called when the internal state changes + void UpdateCallback(FSM::State const& state, FSM::Event const& event); + + // Called when a pose estimate is available + void EkfCallback(const ff_msgs::EkfState::ConstPtr& state); + + // Called when localization has a new pose data + void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& truth); + + // Called when localization has new twist data + void TwistCallback(const geometry_msgs::TwistStamped::ConstPtr& truth); + + // Called when management updates inertial info + void InertiaCallback(const geometry_msgs::InertiaStamped::ConstPtr& inertia); + + // Called when a timer has called back to progress control to next setpoint + void ControlTimerCallback(const ros::TimerEvent & event); + + // Called when the choreographer updates flight modes + void FlightModeCallback(const ff_msgs::FlightMode::ConstPtr& mode); + + // Command GNC directly, bypassing the action-base dinterface + void SetpointCallback(const ff_msgs::ControlState::ConstPtr& command); + + // Used to feed segments + void TimerCallback(const ros::TimerEvent& event); + + // ACTION CLIENT + + // Called when a new goal arrives + void GoalCallback(ff_msgs::ControlGoalConstPtr const& control_goal); + + // Called when a goal is preempted + void PreemptCallback(); + + // Called when a goal is cancelled + void CancelCallback(); + + // ORIGINAL GNC FUNCTIONS + + // Update control to take a new setpoint + bool Command(uint8_t const mode, + ff_msgs::ControlCommand const& poseVel = ff_msgs::ControlCommand()); + + // Step control forward + bool Step(ros::Time curr_time); + + // Read the control parameters from the LUA config file + void ReadParams(void); + + // Simple extension to allow NODELET_* logging calls + std::string getName(); + + private: + // Proxy to gnc + Control controller_; + + std::mutex mutex_cmd_msg_, mutex_segment_; + + ros::Subscriber truth_pose_sub_, inertia_sub_, flight_mode_sub_; + ros::Subscriber twist_sub_, pose_sub_, ekf_sub_, command_sub_; + ros::Publisher ctl_pub_, traj_pub_, segment_pub_, progress_pub_; + ros::ServiceServer enable_srv_; + ros::Timer timer_; + + ff_util::FreeFlyerActionServer action_; + ff_util::FSM fsm_; + ff_util::Segment segment_; + ff_util::Segment::iterator setpoint_; + ff_msgs::ControlFeedback feedback_; + ff_msgs::ControlCommand command_; + + config_reader::ConfigReader config_; + ff_util::PerfTimer pt_ctl_; + ros::Timer config_timer_; + + uint8_t mode_; + ControlState state_; + + std::string name_; + bool inertia_received_; + bool control_enabled_; + bool flight_enabled_; + bool use_truth_; + float stopping_vel_thresh_squared_; + float stopping_omega_thresh_squared_; + + float GetCommand(ControlCommand* cmd, ros::Time tim); +}; + +} // end namespace ctl + +#endif // CTL_CTL_ROS_H_ diff --git a/gnc/ctl/package.xml b/gnc/ctl/package.xml index 0d84445096..d2654d71af 100644 --- a/gnc/ctl/package.xml +++ b/gnc/ctl/package.xml @@ -16,18 +16,16 @@ catkin roscpp - sensor_msgs - vpp_msgs - nav_msgs - ff_msgs nodelet + ff_msgs + ff_util + config_reader roscpp - rt_analyze - sensor_msgs - vpp_msgs - nav_msgs - ff_msgs nodelet + ff_msgs + ff_util + ff_msgs + config_reader diff --git a/gnc/ctl/readme.md b/gnc/ctl/readme.md index 333f50016a..e2d6f4f9e1 100644 --- a/gnc/ctl/readme.md +++ b/gnc/ctl/readme.md @@ -1,7 +1,13 @@ -\page ctl GNC Control Wrapper +\page ctl Control (CTL) -The control subsystem takes the pose from the EKF, as well as control commands, to determine -the forces and torques that should be applied to the robot for smooth control. +The control subsystem takes the pose from the EKF, as well as control commands, to +determine the forces and torques that should be applied to the robot for smooth control. +If executes the given segment based on a trajectory given by the choreographer, the fault +checking on the execution is done in the choreographer based on the feedback. + +The layout of this module is a bit complicated for historical reasons--- the ctl\_nodelet +is a thin wrapper to the Ctl class in ctl\_ros.h which interfaces between different +modules via ROS. The Control class in ctl.h then implements the main underlying functionality. # Inputs @@ -16,3 +22,27 @@ the forces and torques that should be applied to the robot for smooth control. * `gnc/ctl/segment`: The current segment the control subsystem is traversing. * `gnc/ctl/progress`: The progress in executing the current segment. +# GNC Control Wrapper behavior + +\dotfile ctl_fsm "GNC Control Wrapper finite state machine" width=10cm + +* WAITING: No setpoints sent to the internal Matlab controller. +* NOMINAL: Waits until the start time for deferred executions, commands the setpoints to the internal Matlab in nominal mode until the desired trajectory is finished. +* STOPPING: Commands internal Matlab into stopping mode. When the velocity and angular velocity fall under a certain stop threshould, it succeeds. + + +# Modes + +The controller internally has four modes: +* Idle: No control is present. +* Stopping: Position error is calculated based on the delayed current position and the current position. Velocity and acceleration commands are zero. +* Nominal: It follows the trajectory given for position, velocity and acceleraion. +* Stopped: Position/Attitude error is zero. Control Velocity and acceleration goals are zero. + + +# Enable/Disable Control + +In the case a new controller is to be tested, it is possible to enable/disble the current +controller using the service `gnc/ctl/enable`. `true` will enable the control and `false` +will disable it. If the requested state is the one already active, then the service does +not return success. diff --git a/gnc/ctl/src/ctl.cc b/gnc/ctl/src/ctl.cc index aea1e87a21..15578ab8d7 100644 --- a/gnc/ctl/src/ctl.cc +++ b/gnc/ctl/src/ctl.cc @@ -16,568 +16,368 @@ * under the License. */ +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ctl { + +void NormalizeQuaternion(Eigen::Quaternionf & out) { + // enfore positive scalar + if (out.w() < 0) { + out.coeffs() = -out.coeffs(); + } + // out.normalize(); -#include + float mag = sqrt(static_cast(out.x() * out.x() + out.y() * out.y() + out.z() * out.z() + out.w() * out.w())); + if (mag > 1e-7) { + out.x() = out.x() / mag; out.y() = out.y() / mag; out.z() = out.z() / mag; out.w() = out.w() / mag; + } +} -#include -#include -#include -#include -#include +Control::Control(void) { + Initialize(); +} + +void Control::Step(float time_delta, ControlState & state, ControlCommand & cmd, ControlOutput* out) { + // command shaper + ForwardTrajectory(time_delta, state, cmd, out); -#include -#include - -// parameters ctl_controller0_P are set in -// matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_data.c - -using RESPONSE = ff_msgs::ControlResult; - -namespace mc = msg_conversions; - -namespace ctl { // Nodehandle for /gnc/wrapper - -Ctl::Ctl(ros::NodeHandle* nh, std::string const& name) : - fsm_(WAITING, std::bind(&Ctl::UpdateCallback, - this, std::placeholders::_1, std::placeholders::_2)), - name_(name), inertia_received_(false), control_enabled_(false) { - // Add the state transition lambda functions - refer to the FSM diagram - // [0] - fsm_.Add(WAITING, - GOAL_NOMINAL, - [this](FSM::Event const& event) -> FSM::State { - // If the timestamp is in the past, then we need to check its not too - // stale. If it is stale, it might be indicative of timesync issues - // between the MLP and LLP so we should reject the command - mutex_segment_.lock(); - ros::Duration delta = segment_.front().when - ros::Time::now(); - if (delta.toSec() < -MAX_LATENCY) { - mutex_segment_.unlock(); - return Result(RESPONSE::TIMESYNC_ISSUE); - } - // For deferred executions - timer_.stop(); - timer_.setPeriod(delta, true); - timer_.start(); - mutex_segment_.unlock(); - // Set to stopping mode until we start the segment - Control(ff_msgs::ControlCommand::MODE_STOP); - // Update state - return NOMINAL; - }); - // [1] - fsm_.Add(NOMINAL, - GOAL_COMPLETE, - [this](FSM::Event const& event) -> FSM::State { - if (!Control(ff_msgs::ControlCommand::MODE_STOP)) - return Result(RESPONSE::CONTROL_FAILED); - return STOPPING; - }); - // [2] - fsm_.Add(NOMINAL, - GOAL_CANCEL, - [this](FSM::Event const& event) -> FSM::State { - if (!Control(ff_msgs::ControlCommand::MODE_STOP)) - return Result(RESPONSE::CONTROL_FAILED); - return Result(RESPONSE::CANCELLED); - }); - // [3] - fsm_.Add(WAITING, - GOAL_STOP, - [this](FSM::Event const& event) -> FSM::State { - if (!Control(ff_msgs::ControlCommand::MODE_STOP)) - return Result(RESPONSE::CONTROL_FAILED); - return STOPPING; - }); - // [4] - fsm_.Add(STOPPING, - GOAL_COMPLETE, - [this](FSM::Event const& event) -> FSM::State { - return Result(RESPONSE::SUCCESS); - }); - - // Set the operating mode to STOP by default, so that when the speed ramps - // up we don't drift off position because of small forces and torques - mutex_cmd_msg_.lock(); - gnc_.ctl_input_.ctl_mode_cmd = ff_msgs::ControlCommand::MODE_STOP; - mutex_cmd_msg_.unlock(); - - // Initialize GNC - gnc_.Initialize(); - - config_.AddFile("gnc.config"); - config_.AddFile("geometry.config"); - ReadParams(); - config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { - config_.CheckFilesUpdated(std::bind(&Ctl::ReadParams, this));}, false, true); - pt_ctl_.Initialize("ctl"); - - // Subscribers - ekf_sub_ = nh->subscribe( - TOPIC_GNC_EKF, 1, &Ctl::EkfCallback, this); - pose_sub_ = nh->subscribe( - TOPIC_LOCALIZATION_POSE, 1, &Ctl::PoseCallback, this); - twist_sub_ = nh->subscribe( - TOPIC_LOCALIZATION_TWIST, 1, &Ctl::TwistCallback, this); - inertia_sub_ = nh->subscribe( - TOPIC_MOBILITY_INERTIA, 1, &Ctl::InertiaCallback, this); - flight_mode_sub_ = nh->subscribe( - TOPIC_MOBILITY_FLIGHT_MODE, 1, &Ctl::FlightModeCallback, this); - command_sub_ = nh->subscribe( - TOPIC_GNC_CTL_SETPOINT, 1, &Ctl::SetpointCallback, this); - - // Advertised messages - ctl_pub_ = nh->advertise( - TOPIC_GNC_CTL_COMMAND, 5); - traj_pub_ = nh->advertise( - TOPIC_GNC_CTL_TRAJ, 5); - segment_pub_ = nh->advertise( - TOPIC_GNC_CTL_SEGMENT, 5, true); - - // Action client to accept control - action_.SetGoalCallback( - std::bind(&Ctl::GoalCallback, this, std::placeholders::_1)); - action_.SetPreemptCallback(std::bind(&Ctl::PreemptCallback, this)); - action_.SetCancelCallback(std::bind(&Ctl::CancelCallback, this)); - action_.Create(nh, ACTION_GNC_CTL_CONTROL); - - // This timer will be used to throttle control to GNC - timer_ = nh->createTimer(ros::Duration(0), - &Ctl::TimerCallback, this, true, false); + // cex_control_executive + UpdateMode(state, cmd); + UpdateCtlStatus(state, out); + + // clc_closed_loop_controller + // Linear Control + FindPosErr(state, cmd, out); + FindBodyForceCmd(state, out); + + // Rotational Control + FindAttErr(state, out); + FindBodyAlphaTorqueCmd(state, out); + + UpdatePrevious(state); } +// Defined in Indirect Kalman Filter for 3d attitude Estimation - Trawn, Roumeliotis eq 63 +Eigen::Matrix Control::OmegaMatrix(Eigen::Vector3f input) { + Eigen::Matrix out; + out(0, 0) = 0; + out(1, 0) = -input[2]; + out(2, 0) = input[1]; + out(3, 0) = -input[0]; + + out(0, 1) = input[2]; + out(1, 1) = 0; + out(2, 1) = -input[0]; + out(3, 1) = -input[1]; + + out(0, 2) = -input[1]; + out(1, 2) =input[0]; + out(2, 2) = 0; + out(3, 2) = -input[2]; + + out(0, 3) = input[0]; + out(1, 3) = input[1]; + out(2, 3) = input[2]; + out(3, 3) = 0; + + return out; +} -// Destructor -Ctl::~Ctl() {} - -FSM::State Ctl::Result(int32_t response) { - NODELET_DEBUG_STREAM("Control action completed with code " << response); - // Stop the platform and - if (!Control(ff_msgs::ControlCommand::MODE_STOP)) - NODELET_ERROR("Could not stop the platform at end of control action"); - // Send a response - static ff_msgs::ControlResult result; - result.response = response; - mutex_segment_.lock(); - if (!segment_.empty()) { - result.segment = segment_; - result.index = std::distance(setpoint_, segment_.begin()); +void Control::ForwardTrajectory(float time_delta, const ControlState & state, const ControlCommand & cmd, + ControlOutput* out) { + // forward integrate + out->traj_pos = cmd.P_B_ISS_ISS + cmd.V_B_ISS_ISS * time_delta + + 0.5 * cmd.A_B_ISS_ISS * time_delta * time_delta; + out->traj_vel = cmd.V_B_ISS_ISS + cmd.A_B_ISS_ISS * time_delta; + out->traj_accel = cmd.A_B_ISS_ISS; + out->traj_alpha = cmd.alpha_B_ISS_ISS; + out->traj_omega = cmd.omega_B_ISS_ISS + cmd.alpha_B_ISS_ISS * time_delta; + + Eigen::Matrix omega_omega = OmegaMatrix(cmd.omega_B_ISS_ISS); + Eigen::Matrix omega_alpha = OmegaMatrix(cmd.alpha_B_ISS_ISS); + + Eigen::Matrix a = 0.5 * time_delta * (0.5 * time_delta * omega_alpha + omega_omega); + a = a.exp(); + a += (1.0/48) * time_delta * time_delta * time_delta * (omega_alpha * omega_omega - omega_omega * omega_alpha); + out->traj_quat.coeffs() = a * cmd.quat_ISS2B.coeffs(); + NormalizeQuaternion(out->traj_quat); + + out->traj_error_pos = (out->traj_pos - state.est_P_B_ISS_ISS).norm(); + out->traj_error_vel = (out->traj_vel - state.est_V_B_ISS_ISS).norm(); + out->traj_error_omega = (out->traj_omega - state.est_omega_B_ISS_B).norm(); + + out->traj_error_att = QuatError(out->traj_quat, state.est_quat_ISS2B); +} + +void Control::FindAttErr(const ControlState & state, ControlOutput* out) { + Eigen::Quaternionf q_cmd = out->traj_quat; + if (stopped_mode_) + q_cmd = prev_att_; + + Eigen::Quaternionf q_out = state.est_quat_ISS2B.conjugate() * q_cmd; + + NormalizeQuaternion(q_out); + + out->att_err_mag = 2 * acos(static_cast(q_out.w())); + out->att_err << q_out.x(), q_out.y(), q_out.z(); + + Eigen::Vector3f Ki_rot = SafeDivide(state.att_ki, state.omega_kd); + Eigen::Vector3f in = (out->att_err.array() * Ki_rot.array()).matrix(); + out->att_err_int = DiscreteTimeIntegrator(in, rotational_integrator_, out->ctl_status, tun_ctl_att_sat_upper, + tun_ctl_att_sat_lower); +} + +void Control::FindBodyAlphaTorqueCmd(const ControlState & state, ControlOutput* out) { + Eigen::Vector3f cmd_omega = out->traj_omega; + Eigen::Vector3f cmd_alpha = out->traj_alpha; + if (stopped_mode_) { + cmd_omega.setZero(); + cmd_alpha.setZero(); } - if (response == RESPONSE::SUCCESS) { - NODELET_INFO_STREAM("Result: Control succeeded, result sent to client"); - action_.SendResult(ff_util::FreeFlyerActionState::SUCCESS, result); - } else if (response == RESPONSE::PREEMPTED) { - NODELET_INFO_STREAM("Result: Control Preempted, result sent to client"); - action_.SendResult(ff_util::FreeFlyerActionState::PREEMPTED, result); - } else { - NODELET_INFO_STREAM("Result: Control action aborted, result sent to client"); - action_.SendResult(ff_util::FreeFlyerActionState::ABORTED, result); + Eigen::Vector3f rate_error = -state.est_omega_B_ISS_B; + if (out->ctl_status > 1) { + Eigen::Vector3f Kp_rot = SafeDivide(state.att_kp, state.omega_kd); + rate_error = cmd_omega + out->att_err_int + (Kp_rot.array() * out->att_err.array()).matrix() - + state.est_omega_B_ISS_B; } - // Publish an empty segment to notify that nominal mode is over - static ff_msgs::Segment msg; - segment_pub_.publish(msg); - // Clear local variables - timer_.stop(); - segment_.clear(); - setpoint_ = segment_.end(); - mutex_segment_.unlock(); - // Always return to the waiting state - return WAITING; + auto Kd_rot = state.omega_kd.array() * state.inertia.diagonal().array(); + rate_error = (rate_error.array() * Kd_rot).matrix(); + + out->body_alpha_cmd = state.inertia.inverse() * rate_error; + + if (out->ctl_status == 0) { + out->body_torque_cmd.setZero(); + } else { + cmd_alpha = (tun_alpha_gain.array() * cmd_alpha.array()).matrix(); + + out->body_torque_cmd = state.inertia * cmd_alpha + rate_error - + (state.inertia * state.est_omega_B_ISS_B).cross(state.est_omega_B_ISS_B); + } } -void Ctl::UpdateCallback(FSM::State const& state, FSM::Event const& event) { - // Debug events - std::string str = "UNKNOWN"; - switch (event) { - case GOAL_NOMINAL: str = "GOAL_NOMINAL"; break; - case GOAL_COMPLETE: str = "GOAL_COMPLETE"; break; - case GOAL_CANCEL: str = "GOAL_CANCEL"; break; - case GOAL_STOP: str = "GOAL_STOP"; break; +void Control::FindBodyForceCmd(const ControlState & state, ControlOutput* out) { + if (out->ctl_status == 0) { + out->body_force_cmd.setZero(); + out->body_accel_cmd.setZero(); + return; + } + Eigen::Vector3f target_vel = out->traj_vel; + Eigen::Vector3f target_accel = out->traj_accel; + if (stopped_mode_) { + target_vel.setZero(); + target_accel.setZero(); } - NODELET_DEBUG_STREAM("Received event " << str); - // Debug state changes - switch (state) { - case WAITING: str = "WAITING"; break; - case STOPPING: str = "STOPPING"; break; - case NOMINAL: str = "NOMINAL"; break; + + Eigen::Vector3f Kp_lin = SafeDivide(state.pos_kp, state.vel_kd); + Eigen::Vector3f v = Eigen::Vector3f::Zero() - state.est_V_B_ISS_ISS; + if (out->ctl_status > 1) { + // find desired velocity from position error + v = tun_vel_gain * (target_vel - state.est_V_B_ISS_ISS) + (Kp_lin.array() * out->pos_err.array()).matrix() + + out->pos_err_int; } - NODELET_DEBUG_STREAM("State changed to " << str); + Eigen::Vector3f a = RotateVectorAtoB(v, state.est_quat_ISS2B); + a = (a.array() * state.vel_kd.array()).matrix(); + + Eigen::Vector3f b = RotateVectorAtoB((target_accel.array() * tun_accel_gain.array()).matrix(), + state.est_quat_ISS2B); + + out->body_force_cmd = SaturateVector(state.mass * (a + b), tun_ctl_linear_force_limit); + out->body_accel_cmd = out->body_force_cmd / state.mass; } -// Callback in regular mode -void Ctl::EkfCallback(const ff_msgs::EkfState::ConstPtr& state) { - if (!use_truth_) { - mutex_cmd_msg_.lock(); - auto & ctl = gnc_.ctl_input_; - msg_conversions::ros_to_array_vector(state->omega, ctl.est_omega_B_ISS_B); - msg_conversions::ros_to_array_vector(state->velocity, ctl.est_V_B_ISS_ISS); - msg_conversions::ros_to_array_point(state->pose.position, ctl.est_P_B_ISS_ISS); - msg_conversions::ros_to_array_quat(state->pose.orientation, ctl.est_quat_ISS2B); - ctl.est_confidence = state->confidence; - ctl.current_time_sec = state->header.stamp.sec; - ctl.current_time_nsec = state->header.stamp.nsec; - mutex_cmd_msg_.unlock(); - - // Check if Astrobee is dynamically stopped - if (fsm_.GetState() == STOPPING) { - double v_x = state->velocity.x, v_y = state->velocity.y, v_z = state->velocity.z; - double omega_x = state->omega.x, omega_y = state->omega.y, omega_z = state->omega.z; - double v_magnitude = sqrt(v_x*v_x + v_y*v_y + v_z*v_z); - double omega_magnitude = sqrt(omega_x*omega_x + omega_y*omega_y + omega_z*omega_z); - NODELET_DEBUG_STREAM("Validating velocity when stopping: |v|= " << (float) v_magnitude - << " |omega|= " << (float) omega_magnitude); - - if ((v_magnitude <= sqrt(stopping_vel_thresh_squared_)) && - (omega_magnitude <= sqrt(stopping_omega_thresh_squared_))) { - NODELET_INFO_STREAM("Stopping Complete: |v| <= " << (float) sqrt(stopping_vel_thresh_squared_) - << " and |omega| <= " << (float) sqrt(stopping_omega_thresh_squared_)); - fsm_.Update(GOAL_COMPLETE); - } - } +Eigen::Vector3f Control::SaturateVector(Eigen::Vector3f v, float limit) { + float mag = v.norm(); - // advance control forward whenever the pose is updated - pt_ctl_.Tick(); - Step(); - pt_ctl_.Tock(); + if (mag < limit) { + return v; + } else { + return limit / mag * v; } } -// Callback in ground truth mode -void Ctl::PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& truth) { - if (use_truth_) { - mutex_cmd_msg_.lock(); - auto& ctl = gnc_.ctl_input_; - msg_conversions::ros_to_array_point(truth->pose.position, ctl.est_P_B_ISS_ISS); - msg_conversions::ros_to_array_quat(truth->pose.orientation, ctl.est_quat_ISS2B); - ctl.est_confidence = 0; // Localization manager now deals with this - ctl.current_time_sec = truth->header.stamp.sec; - ctl.current_time_nsec = truth->header.stamp.nsec; - mutex_cmd_msg_.unlock(); - // advance control forward whenever the pose is updated - pt_ctl_.Tick(); - Step(); - pt_ctl_.Tock(); - } +Eigen::Vector3f Control::RotateVectorAtoB(const Eigen::Vector3f v, const Eigen::Quaternionf q) { + return q.normalized().conjugate().toRotationMatrix() * v; } -// Called when localization has new twist data -void Ctl::TwistCallback(const geometry_msgs::TwistStamped::ConstPtr& truth) { - if (use_truth_) { - mutex_cmd_msg_.lock(); - auto& ctl = gnc_.ctl_input_; - mc::ros_to_array_vector(truth->twist.linear, ctl.est_V_B_ISS_ISS); - mc::ros_to_array_vector(truth->twist.angular, ctl.est_omega_B_ISS_B); - mutex_cmd_msg_.unlock(); - // Don't advance control, until the pose arrives +Eigen::Vector3f Control::DiscreteTimeIntegrator(const Eigen::Vector3f input, Eigen::Vector3f & accumulator, + uint8_t ctl_status, float upper_limit, float lower_limit) { + Eigen::Vector3f output; + output.setZero(); + if (ctl_status <= 1) { + accumulator.setZero(); + return output; + } + accumulator += input / 62.5; + output = accumulator; + for (int i = 0; i < 3; i++) { + if (output[i] > upper_limit) { + output[i] = upper_limit; + } else if (output[i] < lower_limit) { + output[i] = lower_limit; + } } + return output; } -// Called when management updates inertial info -void Ctl::InertiaCallback(const geometry_msgs::InertiaStamped::ConstPtr& inertia) { - std::lock_guard lock(mutex_cmd_msg_); - auto& input = gnc_.ctl_input_; - input.mass = inertia->inertia.m; - // mc::ros_to_array_vector(inertia->com, input.center_of_mass); // no longer exists - input.inertia_matrix[0] = inertia->inertia.ixx; - input.inertia_matrix[1] = inertia->inertia.ixy; - input.inertia_matrix[2] = inertia->inertia.ixz; - input.inertia_matrix[3] = inertia->inertia.ixy; - input.inertia_matrix[4] = inertia->inertia.iyy; - input.inertia_matrix[5] = inertia->inertia.iyz; - input.inertia_matrix[6] = inertia->inertia.ixz; - input.inertia_matrix[7] = inertia->inertia.iyz; - input.inertia_matrix[8] = inertia->inertia.izz; - inertia_received_ = true; -} +void Control::FindPosErr(const ControlState & state, const ControlCommand & cmd, ControlOutput* out) { + Eigen::Vector3f target = out->traj_pos; + if (stopped_mode_) + target = prev_position_; + out->pos_err = target - state.est_P_B_ISS_ISS; -// Called when choreographer updates the flight mode -void Ctl::FlightModeCallback(const ff_msgs::FlightMode::ConstPtr& mode) { - std::lock_guard lock(mutex_cmd_msg_); - auto& input = gnc_.ctl_input_; - mc::ros_to_array_vector(mode->att_kp, input.att_kp); - mc::ros_to_array_vector(mode->att_ki, input.att_ki); - mc::ros_to_array_vector(mode->omega_kd, input.omega_kd); - mc::ros_to_array_vector(mode->pos_kp, input.pos_kp); - mc::ros_to_array_vector(mode->pos_ki, input.pos_ki); - mc::ros_to_array_vector(mode->vel_kd, input.vel_kd); - input.speed_gain_cmd = mode->speed; - control_enabled_ = - (input.speed_gain_cmd > 0 ? mode->control_enabled : false); + Eigen::Vector3f Ki_lin = SafeDivide(state.pos_ki, state.vel_kd); + out->pos_err_int = DiscreteTimeIntegrator((Ki_lin.array() * out->pos_err.array()).matrix(), + linear_integrator_, out->ctl_status, tun_ctl_pos_sat_upper, tun_ctl_pos_sat_lower); } -// Command GNC directly, bypassing the action-base dinterface -void Ctl::SetpointCallback(const ff_msgs::ControlState::ConstPtr& command) { - // Package up a command using the curren timestamp - ff_msgs::ControlCommand msg; - msg.header.stamp = ros::Time::now(); - msg.header.frame_id = "world"; - msg.mode = ff_msgs::ControlCommand::MODE_NOMINAL; - msg.current = *command; - msg.current.when = ros::Time::now(); - msg.next = msg.current; - // Send the command - if (!Control(msg.mode, msg)) - ROS_WARN("Could not send direct control command"); +Eigen::Vector3f Control::SafeDivide(const Eigen::Vector3f & num, const Eigen::Vector3f & denom) { + Eigen::Vector3f out; + for (int i = 0; i < 3; i++) { + out[i] = (denom[i] == 0 ? 0 : num[i] / denom[i]); + } + return out; } -// When the previous setpoint is sent a timer is created, which fires on the -// edge of the next setpoint. This essentially provides us with a non-blocking -// method for feeding control. -void Ctl::TimerCallback(const ros::TimerEvent& event) { - static ff_msgs::ControlCommand msg; - msg.mode = ff_msgs::ControlCommand::MODE_NOMINAL; - do { - mutex_segment_.lock(); - // Get the index of the current setpoint - size_t idx = std::distance(segment_.begin(), setpoint_); - // If the setpoint is pointing to the last segment, that's it! - if (setpoint_ == segment_.end()) { - mutex_segment_.unlock(); - NODELET_DEBUG_STREAM("Final setpoint " << idx); - return fsm_.Update(GOAL_COMPLETE); - } - // Otherwise, we have at least one more setpoint - msg.current = *setpoint_; - // If we only have one setpoint, just copy the first one - if (++setpoint_ == segment_.end()) { - mutex_segment_.unlock(); - NODELET_DEBUG_STREAM("Penultimate setpoint " << idx); - msg.next = msg.current; - break; - // We have two valid set points - if the times are equal, then we will - // treat the last setpoint as the valid one and move on... + +void Control::UpdateCtlStatus(const ControlState & state, ControlOutput* out) { + float pos_err = (prev_position_ - state.est_P_B_ISS_ISS).squaredNorm(); + float quat_err = QuatError(state.est_quat_ISS2B, prev_att_); + + // is tun_ctl_stopped_pos_thresh supposed to be squared? + if (((pos_err > tun_ctl_stopped_pos_thresh) || (abs(quat_err) > tun_ctl_stopped_quat_thresh)) && + (mode_cmd_ == constants::ctl_stopped_mode)) { + out->ctl_status = constants::ctl_stopping_mode; + } else { + if (stopped_mode_) { + out->ctl_status = constants::ctl_stopped_mode; } else { - msg.next = *setpoint_; - if (msg.current.when != msg.next.when) { - mutex_segment_.unlock(); - NODELET_DEBUG_STREAM("Progressing to setpoint " << idx); - break; - } + out->ctl_status = mode_cmd_; } - mutex_segment_.unlock(); - NODELET_DEBUG_STREAM("Skipping setpoint " << idx); - } while (msg.current.when == msg.next.when); - // Send the control - NODELET_DEBUG_STREAM(msg); - if (!Control(ff_msgs::ControlCommand::MODE_NOMINAL, msg)) { - Result(RESPONSE::CONTROL_FAILED); - return; } - // Set off a timer to wait until the next setpoint - mutex_segment_.lock(); - ros::Duration delta = msg.next.when - ros::Time::now(); - timer_.stop(); - timer_.setPeriod(delta, true); - timer_.start(); - mutex_segment_.unlock(); - NODELET_DEBUG_STREAM("Sleep: " << delta); } -// Goal callback -void Ctl::GoalCallback(ff_msgs::ControlGoalConstPtr const& goal) { - NODELET_INFO_STREAM("GoalCallback: new control goal received"); - // What have we been instructed to do? - switch (goal->command) { - // IDLE - returns instantly - case ff_msgs::ControlGoal::IDLE: - NODELET_DEBUG_STREAM("Received IDLE command"); - if (!Control(ff_msgs::ControlCommand::MODE_IDLE)) - Result(RESPONSE::CONTROL_FAILED); - Result(RESPONSE::SUCCESS); - return; - // STOP - returns instantly - case ff_msgs::ControlGoal::STOP: - NODELET_DEBUG_STREAM("Received STOP command"); - if (!Control(ff_msgs::ControlCommand::MODE_STOP)) - Result(RESPONSE::CONTROL_FAILED); - fsm_.Update(GOAL_STOP); - return; - // NOMINAL - waits until completion - case ff_msgs::ControlGoal::NOMINAL: { - NODELET_DEBUG_STREAM("Received NOMINAL command with segment..."); - // NODELET_DEBUG_STREAM(goal->segment); - if (goal->segment.empty()) { - Result(RESPONSE::EMPTY_SEGMENT); - return; - } - // Save the segment internally - mutex_segment_.lock(); - segment_ = goal->segment; // Copy over the segment to be processed - setpoint_ = segment_.begin(); // We are processing the first setpoint - mutex_segment_.unlock(); - // Publish the segment to used by the sentinel (and perhaps others) - static ff_msgs::Segment msg; - msg.segment = goal->segment; - segment_pub_.publish(msg); - - // Start executing and return - fsm_.Update(GOAL_NOMINAL); - return; - } - // No other control mode makes sense - default: - break; +// update the previous as the last part of the step if it is not in stopped mode +void Control::UpdatePrevious(const ControlState & state) { + if (!stopped_mode_) { + prev_position_ = state.est_P_B_ISS_ISS; + prev_att_ = state.est_quat_ISS2B; } - Result(RESPONSE::INVALID_COMMAND); } -// Cancellation -void Ctl::CancelCallback() { - NODELET_DEBUG_STREAM("CancelCallback called"); - fsm_.Update(GOAL_CANCEL); +// the quaternian_error1 block that performs q_cmd - q_actual * q_error +float Control::QuatError(const Eigen::Quaternionf cmd, const Eigen::Quaternionf actual) { + Eigen::Quaternion out = actual.conjugate() * cmd; + NormalizeQuaternion(out); + return fabs(static_cast(acos(static_cast(out.w())))) * 2; } -// Preemption - this might be a replan -void Ctl::PreemptCallback() { - NODELET_DEBUG_STREAM("PreemptCallback called"); - fsm_.Update(GOAL_CANCEL); +float Control::ButterWorthFilter(float input, float delay_val, float* sum_out) { + const long double butterworth_gain_1 = 0.0031317642291927056; + const long double butterworth_gain_2 = -0.993736471541614597; + float tmp_out = input * butterworth_gain_1; + float previous_gain = delay_val * butterworth_gain_2; + tmp_out = tmp_out - previous_gain; + float output = tmp_out + delay_val; + *sum_out += output * output; + return tmp_out; } -// Update a control with a new command -bool Ctl::Control(uint8_t const mode, ff_msgs::ControlCommand const& poseVel) { - // Set the operating mode - std::lock_guard lock(mutex_cmd_msg_); - auto& input= gnc_.ctl_input_; - input.ctl_mode_cmd = mode; - if (mode != ff_msgs::ControlCommand::MODE_NOMINAL) - return true; - - // NOMINAL: Current setpoint - auto& curr = poseVel.current; - input.cmd_state_a.timestamp_sec = curr.when.sec; - input.cmd_state_a.timestamp_nsec = curr.when.nsec; - mc::ros_to_array_point(curr.pose.position, input.cmd_state_a.P_B_ISS_ISS); - mc::ros_to_array_quat(curr.pose.orientation, input.cmd_state_a.quat_ISS2B); - mc::ros_to_array_vector(curr.twist.linear, input.cmd_state_a.V_B_ISS_ISS); - mc::ros_to_array_vector(curr.accel.linear, input.cmd_state_a.A_B_ISS_ISS); - mc::ros_to_array_vector(curr.twist.angular, - input.cmd_state_a.omega_B_ISS_B); - mc::ros_to_array_vector(curr.accel.angular, - input.cmd_state_a.alpha_B_ISS_B); - - // NOMINAL: Next setpoint - auto& next = poseVel.next; - input.cmd_state_b.timestamp_sec = next.when.sec; - input.cmd_state_b.timestamp_nsec = next.when.nsec; - mc::ros_to_array_point(next.pose.position, input.cmd_state_b.P_B_ISS_ISS); - mc::ros_to_array_quat(next.pose.orientation, input.cmd_state_b.quat_ISS2B); - mc::ros_to_array_vector(next.twist.linear, input.cmd_state_b.V_B_ISS_ISS); - mc::ros_to_array_vector(next.accel.linear, input.cmd_state_b.A_B_ISS_ISS); - mc::ros_to_array_vector(next.twist.angular, - input.cmd_state_b.omega_B_ISS_B); - mc::ros_to_array_vector(next.accel.angular, - input.cmd_state_b.alpha_B_ISS_B); - - // SUCCESS - return true; +bool Control::FilterThreshold(Eigen::Vector3f vec, float threshold, Eigen::Vector3f & previous) { + float sum = 0; + for (int i = 0; i < 3; i++) + previous[i] = ButterWorthFilter(vec[i], previous[i], &sum); + + return sum < threshold; } -bool Ctl::Step(void) { - // Step GNC forward - { - if (!inertia_received_) { - NODELET_DEBUG_STREAM_THROTTLE(10, "GNC step waiting for inertia"); - return false; - } - if (!control_enabled_) { - NODELET_DEBUG_STREAM_THROTTLE(10, "GNC control disabled in flight mode"); - return false; - } - std::lock_guard cmd_lock(mutex_cmd_msg_); - gnc_.Step(); +void Control::UpdateMode(const ControlState & state, const ControlCommand & cmd) { + if (state.est_confidence != constants::ase_status_converged) { + mode_cmd_ = constants::ctl_idle_mode; + } else { + mode_cmd_ = cmd.mode; } - // Output of the Step() function is FAM control (ctl) - auto& cmd = gnc_.cmd_; - auto& ctl = gnc_.ctl_; - auto& input= gnc_.ctl_input_; - - // Publish the FAM command - static ff_msgs::FamCommand cmd_msg_; - cmd_msg_.header.stamp = ros::Time::now(); - cmd_msg_.header.frame_id = "body"; - cmd_msg_.wrench.force = mc::array_to_ros_vector(ctl.body_force_cmd); - cmd_msg_.wrench.torque = mc::array_to_ros_vector(ctl.body_torque_cmd); - cmd_msg_.accel = mc::array_to_ros_vector(ctl.body_accel_cmd); - cmd_msg_.alpha = mc::array_to_ros_vector(ctl.body_alpha_cmd); - cmd_msg_.position_error = mc::array_to_ros_vector(ctl.pos_err); - cmd_msg_.position_error_integrated = mc::array_to_ros_vector(ctl.pos_err_int); - cmd_msg_.attitude_error = mc::array_to_ros_vector(ctl.att_err); - cmd_msg_.attitude_error_integrated = mc::array_to_ros_vector(ctl.att_err_int); - cmd_msg_.attitude_error_mag = ctl.att_err_mag; - cmd_msg_.status = ctl.ctl_status; - cmd_msg_.control_mode = cmd.cmd_mode; - ctl_pub_.publish(cmd_msg_); - - // Publish the traj message - static ff_msgs::ControlState current; - current.when = cmd_msg_.header.stamp; - current.pose.position = msg_conversions::array_to_ros_point(cmd.traj_pos); - current.pose.orientation = msg_conversions::array_to_ros_quat(cmd.traj_quat); - current.twist.linear = msg_conversions::array_to_ros_vector(cmd.traj_vel); - current.twist.angular = msg_conversions::array_to_ros_vector(cmd.traj_omega); - current.accel.linear = msg_conversions::array_to_ros_vector(cmd.traj_accel); - current.accel.angular = msg_conversions::array_to_ros_vector(cmd.traj_alpha); - traj_pub_.publish(current); - - // Publish the current setpoint, - if ((fsm_.GetState() == NOMINAL) || (fsm_.GetState() == STOPPING)) { - std::lock_guard lock(mutex_segment_); - static ff_msgs::ControlFeedback feedback; - feedback.setpoint.when = cmd_msg_.header.stamp; - feedback.setpoint.pose.position = mc::array_to_ros_point(cmd.traj_pos); - feedback.setpoint.pose.orientation = mc::array_to_ros_quat(cmd.traj_quat); - feedback.setpoint.twist.linear = mc::array_to_ros_vector(cmd.traj_vel); - feedback.setpoint.twist.angular = mc::array_to_ros_vector(cmd.traj_omega); - feedback.setpoint.accel.linear = mc::array_to_ros_vector(cmd.traj_accel); - feedback.setpoint.accel.angular = mc::array_to_ros_vector(cmd.traj_alpha); - feedback.index = std::distance(setpoint_, segment_.begin()); - // Sometimese segments arrive with a first setpoint that has a time stamp - // sometime in the future. In this case we will be in STOPPED mode until - // the callback timer sets the CMC mode to NOMINAL. In the interim the - // errors being sent back from the controller make no sense. - switch (input.ctl_mode_cmd) { - case ff_msgs::ControlCommand::MODE_NOMINAL: - feedback.error_position = ctl.traj_error_pos; - feedback.error_attitude = ctl.traj_error_att; - feedback.error_velocity = ctl.traj_error_vel; - feedback.error_omega = ctl.traj_error_omega; - break; - default: - feedback.error_position = 0; - feedback.error_attitude = 0; - feedback.error_velocity = 0; - feedback.error_omega = 0; - break; - } - action_.SendFeedback(feedback); + // shift exisitng elements to the right + for (int i = 3; i >= 0; i--) + prev_mode_cmd_[i + 1] = prev_mode_cmd_[i]; + prev_mode_cmd_[0] = mode_cmd_; + + // need to run these outside of if condition to make sure that they are being ran every cycle + bool vel_below_threshold = FilterThreshold(state.est_V_B_ISS_ISS, tun_ctl_stopping_vel_thresh, + prev_filter_vel_); + bool omega_below_threshold = FilterThreshold(state.est_omega_B_ISS_B, tun_ctl_stopping_omega_thresh, + prev_filter_omega_); + + stopped_mode_ = false; + if (vel_below_threshold && omega_below_threshold) { + // weird that this doesn't use current command + if ((prev_mode_cmd_[4] == constants::ctl_stopping_mode) && (prev_mode_cmd_[4] == prev_mode_cmd_[3]) && + (prev_mode_cmd_[3] == prev_mode_cmd_[2]) && (prev_mode_cmd_[2] == prev_mode_cmd_[1])) + stopped_mode_ = true; } - - // GNC stepped successfully - return true; } -std::string Ctl::getName() { return name_; } +void Control::Initialize(void) { + mode_cmd_ = 0; + stopped_mode_ = false; + prev_filter_vel_.setZero(); + prev_filter_omega_.setZero(); + prev_mode_cmd_[0] = 0; + prev_mode_cmd_[1] = 0; + prev_mode_cmd_[2] = 0; + prev_mode_cmd_[3] = 0; + prev_position_.setZero(); + prev_att_.x() = 0; prev_att_.y() = 0; prev_att_.z() = 0; prev_att_.w() = 0; + linear_integrator_.setZero(); + rotational_integrator_.setZero(); +} -// Chainload the readparam call -void Ctl::ReadParams(void) { - if (!config_.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; - } - gnc_.ReadParams(&config_); - if (!config_.GetBool("tun_debug_ctl_use_truth", &use_truth_)) - ROS_FATAL("tun_debug_ctl_use_truth not specified."); - // Set linear- and angular velocity threshold for ekf - // to decide that Astrobee is dynamically stopped. - if (!config_.GetReal("tun_ctl_stopping_vel_thresh", &stopping_vel_thresh_squared_)) - ROS_FATAL("tun_ctl_stopping_vel_thresh not specified."); - if (!config_.GetReal("tun_ctl_stopping_omega_thresh", &stopping_omega_thresh_squared_)) - ROS_FATAL("tun_ctl_stopping_omega_thresh not specified."); +void Control::ReadParams(config_reader::ConfigReader* config) { + if (!config->GetReal("tun_vel_gain", &tun_vel_gain)) + ROS_FATAL("Unspecified tun_vel_gain."); + Eigen::Vector3d temp; + if (!msg_conversions::config_read_vector(config, "tun_accel_gain", &temp)) + ROS_FATAL("Unspecified tun_accel_gain."); + tun_accel_gain = temp.cast(); + if (!msg_conversions::config_read_vector(config, "tun_alpha_gain", &temp)) + ROS_FATAL("Unspecified tun_alpha_gain."); + tun_alpha_gain = temp.cast(); + if (!config->GetReal("tun_ctl_stopped_pos_thresh", &tun_ctl_stopped_pos_thresh)) + ROS_FATAL("Unspecified tun_ctl_stopped_pos_thresh."); + if (!config->GetReal("tun_ctl_stopped_quat_thresh", &tun_ctl_stopped_quat_thresh)) + ROS_FATAL("Unspecified tun_ctl_stopped_quat_thresh."); + if (!config->GetReal("tun_ctl_stopping_omega_thresh", &tun_ctl_stopping_omega_thresh)) + ROS_FATAL("Unspecified tun_ctl_stopping_omega_thresh."); + if (!config->GetReal("tun_ctl_stopping_vel_thresh", &tun_ctl_stopping_vel_thresh)) + ROS_FATAL("Unspecified tun_ctl_stopping_vel_thresh."); + if (!config->GetReal("tun_ctl_att_sat_lower", &tun_ctl_att_sat_lower)) + ROS_FATAL("Unspecified tun_ctl_att_sat_lower."); + if (!config->GetReal("tun_ctl_att_sat_upper", &tun_ctl_att_sat_upper)) + ROS_FATAL("Unspecified tun_ctl_att_sat_upper."); + if (!config->GetReal("tun_ctl_linear_force_limit", &tun_ctl_linear_force_limit)) + ROS_FATAL("Unspecified tun_ctl_linear_force_limit."); + if (!config->GetReal("tun_ctl_pos_sat_lower", &tun_ctl_pos_sat_lower)) + ROS_FATAL("Unspecified tun_ctl_pos_sat_lower."); + if (!config->GetReal("tun_ctl_pos_sat_upper", &tun_ctl_pos_sat_upper)) + ROS_FATAL("Unspecified tun_ctl_pos_sat_upper."); } } // end namespace ctl diff --git a/gnc/ctl/src/ctl_nodelet.cc b/gnc/ctl/src/ctl_nodelet.cc index 07a83172cd..e850dc0054 100644 --- a/gnc/ctl/src/ctl_nodelet.cc +++ b/gnc/ctl/src/ctl_nodelet.cc @@ -16,7 +16,7 @@ * under the License. */ -#include +#include #include #include @@ -24,8 +24,6 @@ #include #include -#include - #include namespace ctl { @@ -38,7 +36,6 @@ class CtlNodelet : public ff_util::FreeFlyerNodelet { void Initialize(ros::NodeHandle *nh) { // Bootstrap our environment ff_common::InitFreeFlyerApplication(getMyArgv(), false); - gnc_autocode::InitializeAutocode(this); ctl_.reset(new ctl::Ctl(this->GetPlatformHandle(true), getName())); } diff --git a/gnc/ctl/src/ctl_ros.cc b/gnc/ctl/src/ctl_ros.cc new file mode 100644 index 0000000000..21529cee27 --- /dev/null +++ b/gnc/ctl/src/ctl_ros.cc @@ -0,0 +1,579 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include + +using RESPONSE = ff_msgs::ControlResult; + +namespace mc = msg_conversions; + +namespace ctl { // Nodehandle for /gnc/wrapper + +Ctl::Ctl(ros::NodeHandle* nh, std::string const& name) : + fsm_(WAITING, std::bind(&Ctl::UpdateCallback, + this, std::placeholders::_1, std::placeholders::_2)), + name_(name), inertia_received_(false), control_enabled_(true), flight_enabled_(false) { + // Add the state transition lambda functions - refer to the FSM diagram + // [0] + fsm_.Add(WAITING, + GOAL_NOMINAL, + [this](FSM::Event const& event) -> FSM::State { + // If the timestamp is in the past, then we need to check its not too + // stale. If it is stale, it might be indicative of timesync issues + // between the MLP and LLP so we should reject the command + mutex_segment_.lock(); + ros::Duration delta = segment_.front().when - ros::Time::now(); + if (delta.toSec() < -MAX_LATENCY) { + mutex_segment_.unlock(); + return Result(RESPONSE::TIMESYNC_ISSUE); + } + // For deferred executions + timer_.stop(); + timer_.setPeriod(delta, true); + timer_.start(); + mutex_segment_.unlock(); + // Set to stopping mode until we start the segment + Command(ff_msgs::ControlCommand::MODE_STOP); + // Update state + return NOMINAL; + }); + // [1] + fsm_.Add(NOMINAL, + GOAL_COMPLETE, + [this](FSM::Event const& event) -> FSM::State { + if (!Command(ff_msgs::ControlCommand::MODE_STOP)) + return Result(RESPONSE::CONTROL_FAILED); + return STOPPING; + }); + // [2] + fsm_.Add(NOMINAL, + GOAL_CANCEL, + [this](FSM::Event const& event) -> FSM::State { + if (!Command(ff_msgs::ControlCommand::MODE_STOP)) + return Result(RESPONSE::CONTROL_FAILED); + return Result(RESPONSE::CANCELLED); + }); + // [3] + fsm_.Add(WAITING, + GOAL_STOP, + [this](FSM::Event const& event) -> FSM::State { + if (!Command(ff_msgs::ControlCommand::MODE_STOP)) + return Result(RESPONSE::CONTROL_FAILED); + return STOPPING; + }); + // [4] + fsm_.Add(STOPPING, + GOAL_COMPLETE, + [this](FSM::Event const& event) -> FSM::State { + return Result(RESPONSE::SUCCESS); + }); + + // Set the operating mode to STOP by default, so that when the speed ramps + // up we don't drift off position because of small forces and torques + mutex_cmd_msg_.lock(); + mode_ = ff_msgs::ControlCommand::MODE_STOP; + mutex_cmd_msg_.unlock(); + + controller_.Initialize(); + + config_.AddFile("gnc.config"); + config_.AddFile("geometry.config"); + ReadParams(); + config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { + config_.CheckFilesUpdated(std::bind(&Ctl::ReadParams, this));}, false, true); + pt_ctl_.Initialize("ctl"); + + // Subscribers + ekf_sub_ = nh->subscribe( + TOPIC_GNC_EKF, 1, &Ctl::EkfCallback, this); + pose_sub_ = nh->subscribe( + TOPIC_LOCALIZATION_POSE, 1, &Ctl::PoseCallback, this); + twist_sub_ = nh->subscribe( + TOPIC_LOCALIZATION_TWIST, 1, &Ctl::TwistCallback, this); + inertia_sub_ = nh->subscribe( + TOPIC_MOBILITY_INERTIA, 1, &Ctl::InertiaCallback, this); + flight_mode_sub_ = nh->subscribe( + TOPIC_MOBILITY_FLIGHT_MODE, 1, &Ctl::FlightModeCallback, this); + command_sub_ = nh->subscribe( + TOPIC_GNC_CTL_SETPOINT, 1, &Ctl::SetpointCallback, this); + + // Advertised messages + ctl_pub_ = nh->advertise( + TOPIC_GNC_CTL_COMMAND, 5); + traj_pub_ = nh->advertise( + TOPIC_GNC_CTL_TRAJ, 5); + segment_pub_ = nh->advertise( + TOPIC_GNC_CTL_SEGMENT, 5, true); + + // Enable / Disable control module + enable_srv_ = nh->advertiseService(SERVICE_GNC_CTL_ENABLE, + &Ctl::EnableCtl, this); + + // Action client to accept control + action_.SetGoalCallback( + std::bind(&Ctl::GoalCallback, this, std::placeholders::_1)); + action_.SetPreemptCallback(std::bind(&Ctl::PreemptCallback, this)); + action_.SetCancelCallback(std::bind(&Ctl::CancelCallback, this)); + action_.Create(nh, ACTION_GNC_CTL_CONTROL); + + // This timer will be used to throttle control to GNC + timer_ = nh->createTimer(ros::Duration(0), + &Ctl::TimerCallback, this, true, false); +} + + +// Destructor +Ctl::~Ctl() {} + +FSM::State Ctl::Result(int32_t response) { + NODELET_DEBUG_STREAM("Control action completed with code " << response); + // Stop the platform and + if (!Command(ff_msgs::ControlCommand::MODE_STOP)) + NODELET_ERROR("Could not stop the platform at end of control action"); + // Send a response + static ff_msgs::ControlResult result; + result.response = response; + mutex_segment_.lock(); + if (!segment_.empty()) { + result.segment = segment_; + result.index = std::distance(setpoint_, segment_.begin()); + } + + if (response == RESPONSE::SUCCESS) { + NODELET_INFO_STREAM("Result: Control succeeded, result sent to client"); + action_.SendResult(ff_util::FreeFlyerActionState::SUCCESS, result); + } else if (response == RESPONSE::PREEMPTED) { + NODELET_INFO_STREAM("Result: Control Preempted, result sent to client"); + action_.SendResult(ff_util::FreeFlyerActionState::PREEMPTED, result); + } else { + NODELET_INFO_STREAM("Result: Control action aborted, result sent to client"); + action_.SendResult(ff_util::FreeFlyerActionState::ABORTED, result); + } + + // Publish an empty segment to notify that nominal mode is over + static ff_msgs::Segment msg; + segment_pub_.publish(msg); + // Clear local variables + timer_.stop(); + segment_.clear(); + setpoint_ = segment_.end(); + mutex_segment_.unlock(); + // Always return to the waiting state + return WAITING; +} + +bool Ctl::EnableCtl(std_srvs::SetBoolRequest&req, std_srvs::SetBoolResponse &response) { + bool request = req.data; + + // Check if we are asking for the current state + if (request == control_enabled_) { + response.success = false; + response.message = "Requested current state."; + return true; + } + + // Check required action + response.success = true; + control_enabled_ = request; + if (control_enabled_ == false) { + response.message = "Onboard controller disabled."; + NODELET_DEBUG_STREAM("Onboard controller disabled."); + } else { + response.message = "Onboard controller enabled."; + NODELET_DEBUG_STREAM("Onboard controller enabled."); + } + return true; +} + +void Ctl::UpdateCallback(FSM::State const& state, FSM::Event const& event) { + // Debug events + std::string str = "UNKNOWN"; + switch (event) { + case GOAL_NOMINAL: str = "GOAL_NOMINAL"; break; + case GOAL_COMPLETE: str = "GOAL_COMPLETE"; break; + case GOAL_CANCEL: str = "GOAL_CANCEL"; break; + case GOAL_STOP: str = "GOAL_STOP"; break; + } + NODELET_DEBUG_STREAM("Received event " << str); + // Debug state changes + switch (state) { + case WAITING: str = "WAITING"; break; + case STOPPING: str = "STOPPING"; break; + case NOMINAL: str = "NOMINAL"; break; + } + NODELET_DEBUG_STREAM("State changed to " << str); +} + +// Callback in regular mode +void Ctl::EkfCallback(const ff_msgs::EkfState::ConstPtr& state) { + if (!use_truth_) { + mutex_cmd_msg_.lock(); + state_.est_omega_B_ISS_B = msg_conversions::ros_to_eigen_vector(state->omega).cast(); + state_.est_V_B_ISS_ISS = msg_conversions::ros_to_eigen_vector(state->velocity).cast(); + state_.est_P_B_ISS_ISS = msg_conversions::ros_point_to_eigen_vector(state->pose.position).cast(); + state_.est_quat_ISS2B = msg_conversions::ros_to_eigen_quat(state->pose.orientation).cast(); + state_.est_confidence = state->confidence; + mutex_cmd_msg_.unlock(); + + // Check if Astrobee is dynamically stopped + if (fsm_.GetState() == STOPPING) { + double v_x = state->velocity.x, v_y = state->velocity.y, v_z = state->velocity.z; + double omega_x = state->omega.x, omega_y = state->omega.y, omega_z = state->omega.z; + double v_magnitude = sqrt(v_x*v_x + v_y*v_y + v_z*v_z); + double omega_magnitude = sqrt(omega_x*omega_x + omega_y*omega_y + omega_z*omega_z); + NODELET_DEBUG_STREAM("Validating velocity when stopping: |v|= " << (float) v_magnitude + << " |omega|= " << (float) omega_magnitude); + + if ((v_magnitude <= sqrt(stopping_vel_thresh_squared_)) && + (omega_magnitude <= sqrt(stopping_omega_thresh_squared_))) { + NODELET_INFO_STREAM("Stopping Complete: |v| <= " << (float) sqrt(stopping_vel_thresh_squared_) + << " and |omega| <= " << (float) sqrt(stopping_omega_thresh_squared_)); + fsm_.Update(GOAL_COMPLETE); + } + } + + // advance control forward whenever the pose is updated + pt_ctl_.Tick(); + Step(state->header.stamp); + pt_ctl_.Tock(); + } +} + +// Callback in ground truth mode +void Ctl::PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& truth) { + if (use_truth_) { + mutex_cmd_msg_.lock(); + state_.est_P_B_ISS_ISS = msg_conversions::ros_point_to_eigen_vector(truth->pose.position).cast(); + state_.est_quat_ISS2B = msg_conversions::ros_to_eigen_quat(truth->pose.orientation).cast(); + state_.est_confidence = 0; // Localization manager now deals with this + mutex_cmd_msg_.unlock(); + // advance control forward whenever the pose is updated + pt_ctl_.Tick(); + Step(truth->header.stamp); + pt_ctl_.Tock(); + } +} + +// Called when localization has new twist data +void Ctl::TwistCallback(const geometry_msgs::TwistStamped::ConstPtr& truth) { + if (use_truth_) { + mutex_cmd_msg_.lock(); + state_.est_V_B_ISS_ISS = mc::ros_to_eigen_vector(truth->twist.linear).cast(); + state_.est_omega_B_ISS_B = mc::ros_to_eigen_vector(truth->twist.angular).cast(); + mutex_cmd_msg_.unlock(); + // Don't advance control, until the pose arrives + } +} + +// Called when management updates inertial info +void Ctl::InertiaCallback(const geometry_msgs::InertiaStamped::ConstPtr& inertia) { + std::lock_guard lock(mutex_cmd_msg_); + state_.mass = inertia->inertia.m; + auto& i = inertia->inertia; + state_.inertia << i.ixx, i.ixy, i.ixz, i.ixy, i.iyy, i.iyz, i.ixz, i.iyz, i.izz; + inertia_received_ = true; +} + +// Called when choreographer updates the flight mode +void Ctl::FlightModeCallback(const ff_msgs::FlightMode::ConstPtr& mode) { + std::lock_guard lock(mutex_cmd_msg_); + state_.att_kp = mc::ros_to_eigen_vector(mode->att_kp).cast(); + state_.att_ki = mc::ros_to_eigen_vector(mode->att_ki).cast(); + state_.omega_kd = mc::ros_to_eigen_vector(mode->omega_kd).cast(); + state_.pos_kp = mc::ros_to_eigen_vector(mode->pos_kp).cast(); + state_.pos_ki = mc::ros_to_eigen_vector(mode->pos_ki).cast(); + state_.vel_kd = mc::ros_to_eigen_vector(mode->vel_kd).cast(); + flight_enabled_ = + (mode->speed > 0 ? mode->control_enabled : false); +} + +// Command GNC directly, bypassing the action-base dinterface +void Ctl::SetpointCallback(const ff_msgs::ControlState::ConstPtr& command) { + // Package up a command using the curren timestamp + ff_msgs::ControlCommand msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "world"; + msg.mode = ff_msgs::ControlCommand::MODE_NOMINAL; + msg.current = *command; + msg.current.when = ros::Time::now(); + msg.next = msg.current; + // Send the command + if (!Command(msg.mode, msg)) + ROS_WARN("Could not send direct control command"); +} + +// When the previous setpoint is sent a timer is created, which fires on the +// edge of the next setpoint. This essentially provides us with a non-blocking +// method for feeding control. +void Ctl::TimerCallback(const ros::TimerEvent& event) { + static ff_msgs::ControlCommand msg; + msg.mode = ff_msgs::ControlCommand::MODE_NOMINAL; + do { + mutex_segment_.lock(); + // Get the index of the current setpoint + size_t idx = std::distance(segment_.begin(), setpoint_); + // If the setpoint is pointing to the last segment, that's it! + if (setpoint_ == segment_.end()) { + mutex_segment_.unlock(); + NODELET_DEBUG_STREAM("Final setpoint " << idx); + return fsm_.Update(GOAL_COMPLETE); + } + // Otherwise, we have at least one more setpoint + msg.current = *setpoint_; + // If we only have one setpoint, just copy the first one + if (++setpoint_ == segment_.end()) { + mutex_segment_.unlock(); + NODELET_DEBUG_STREAM("Penultimate setpoint " << idx); + msg.next = msg.current; + break; + // We have two valid set points - if the times are equal, then we will + // treat the last setpoint as the valid one and move on... + } else { + msg.next = *setpoint_; + if (msg.current.when != msg.next.when) { + mutex_segment_.unlock(); + NODELET_DEBUG_STREAM("Progressing to setpoint " << idx); + break; + } + } + mutex_segment_.unlock(); + NODELET_DEBUG_STREAM("Skipping setpoint " << idx); + } while (msg.current.when == msg.next.when); + // Send the control + NODELET_DEBUG_STREAM(msg); + if (!Command(ff_msgs::ControlCommand::MODE_NOMINAL, msg)) { + Result(RESPONSE::CONTROL_FAILED); + return; + } + // Set off a timer to wait until the next setpoint + mutex_segment_.lock(); + ros::Duration delta = msg.next.when - ros::Time::now(); + timer_.stop(); + timer_.setPeriod(delta, true); + timer_.start(); + mutex_segment_.unlock(); + NODELET_DEBUG_STREAM("Sleep: " << delta); +} + +// Goal callback +void Ctl::GoalCallback(ff_msgs::ControlGoalConstPtr const& goal) { + NODELET_INFO_STREAM("GoalCallback: new control goal received"); + // What have we been instructed to do? + switch (goal->command) { + // IDLE - returns instantly + case ff_msgs::ControlGoal::IDLE: + NODELET_DEBUG_STREAM("Received IDLE command"); + if (!Command(ff_msgs::ControlCommand::MODE_IDLE)) + Result(RESPONSE::CONTROL_FAILED); + Result(RESPONSE::SUCCESS); + return; + // STOP - returns instantly + case ff_msgs::ControlGoal::STOP: + NODELET_DEBUG_STREAM("Received STOP command"); + if (!Command(ff_msgs::ControlCommand::MODE_STOP)) + Result(RESPONSE::CONTROL_FAILED); + fsm_.Update(GOAL_STOP); + return; + // NOMINAL - waits until completion + case ff_msgs::ControlGoal::NOMINAL: { + NODELET_DEBUG_STREAM("Received NOMINAL command with segment..."); + // NODELET_DEBUG_STREAM(goal->segment); + if (goal->segment.empty()) { + Result(RESPONSE::EMPTY_SEGMENT); + return; + } + // Save the segment internally + mutex_segment_.lock(); + segment_ = goal->segment; // Copy over the segment to be processed + setpoint_ = segment_.begin(); // We are processing the first setpoint + mutex_segment_.unlock(); + // Publish the segment to used by the sentinel (and perhaps others) + static ff_msgs::Segment msg; + msg.segment = goal->segment; + segment_pub_.publish(msg); + + // Start executing and return + fsm_.Update(GOAL_NOMINAL); + return; + } + // No other control mode makes sense + default: + break; + } + Result(RESPONSE::INVALID_COMMAND); +} + +// Cancellation +void Ctl::CancelCallback() { + NODELET_DEBUG_STREAM("CancelCallback called"); + fsm_.Update(GOAL_CANCEL); +} + +// Preemption - this might be a replan +void Ctl::PreemptCallback() { + NODELET_DEBUG_STREAM("PreemptCallback called"); + fsm_.Update(GOAL_CANCEL); +} + +// Update a control with a new command +bool Ctl::Command(uint8_t const mode, ff_msgs::ControlCommand const& poseVel) { + // Set the operating mode + std::lock_guard lock(mutex_cmd_msg_); + mode_ = mode; + if (mode != ff_msgs::ControlCommand::MODE_NOMINAL) + return true; + + command_ = poseVel; + + return true; +} + +float Ctl::GetCommand(ControlCommand* cmd, ros::Time tim) { + auto* curr = &command_.next; + if (curr->when > tim) + curr = &command_.current; + cmd->P_B_ISS_ISS = mc::ros_point_to_eigen_vector(curr->pose.position).cast(); + cmd->quat_ISS2B = mc::ros_to_eigen_quat(curr->pose.orientation).cast(); + cmd->V_B_ISS_ISS = mc::ros_to_eigen_vector(curr->twist.linear).cast(); + cmd->A_B_ISS_ISS = mc::ros_to_eigen_vector(curr->accel.linear).cast(); + cmd->omega_B_ISS_ISS = mc::ros_to_eigen_vector(curr->twist.angular).cast(); + cmd->alpha_B_ISS_ISS = mc::ros_to_eigen_vector(curr->accel.angular).cast(); + cmd->mode = mode_; + return (tim - curr->when).toSec(); +} + +bool Ctl::Step(ros::Time curr_time) { + if (!inertia_received_) { + NODELET_DEBUG_STREAM_THROTTLE(10, "GNC step waiting for inertia"); + return false; + } + if (!flight_enabled_ || !control_enabled_) { + NODELET_DEBUG_STREAM_THROTTLE(10, "GNC control disabled in flight mode"); + return false; + } + + ControlCommand command; + ControlOutput output; + float time_delta; + { + std::lock_guard cmd_lock(mutex_cmd_msg_); + + time_delta = GetCommand(&command, curr_time); + } + + // Step GNC forward + controller_.Step(time_delta, state_, command, &output); + + // Publish the FAM command + static ff_msgs::FamCommand cmd_msg_; + cmd_msg_.header.stamp = curr_time; + cmd_msg_.header.frame_id = "body"; + cmd_msg_.wrench.force = mc::eigen_to_ros_vector(output.body_force_cmd.cast()); + cmd_msg_.wrench.torque = mc::eigen_to_ros_vector(output.body_torque_cmd.cast()); + cmd_msg_.accel = mc::eigen_to_ros_vector(output.body_accel_cmd.cast()); + cmd_msg_.alpha = mc::eigen_to_ros_vector(output.body_alpha_cmd.cast()); + cmd_msg_.position_error = mc::eigen_to_ros_vector(output.pos_err.cast()); + cmd_msg_.position_error_integrated = mc::eigen_to_ros_vector(output.pos_err_int.cast()); + cmd_msg_.attitude_error = mc::eigen_to_ros_vector(output.att_err.cast()); + cmd_msg_.attitude_error_integrated = mc::eigen_to_ros_vector(output.att_err_int.cast()); + cmd_msg_.attitude_error_mag = output.att_err_mag; + cmd_msg_.status = output.ctl_status; + cmd_msg_.control_mode = mode_; + + ctl_pub_.publish(cmd_msg_); + + // Publish the traj message + static ff_msgs::ControlState current; + current.when = curr_time; + current.pose.position = msg_conversions::eigen_to_ros_point(output.traj_pos.cast()); + current.pose.orientation = msg_conversions::eigen_to_ros_quat(output.traj_quat.cast()); + current.twist.linear = msg_conversions::eigen_to_ros_vector(output.traj_vel.cast()); + current.twist.angular = msg_conversions::eigen_to_ros_vector(output.traj_omega.cast()); + current.accel.linear = msg_conversions::eigen_to_ros_vector(output.traj_accel.cast()); + current.accel.angular = msg_conversions::eigen_to_ros_vector(output.traj_alpha.cast()); + traj_pub_.publish(current); + + // Publish the current setpoint, + if ((fsm_.GetState() == NOMINAL) || (fsm_.GetState() == STOPPING)) { + std::lock_guard lock(mutex_segment_); + static ff_msgs::ControlFeedback feedback; + feedback.setpoint.when = curr_time; + feedback.setpoint.pose.position = mc::eigen_to_ros_point(output.traj_pos.cast()); + feedback.setpoint.pose.orientation = mc::eigen_to_ros_quat(output.traj_quat.cast()); + feedback.setpoint.twist.linear = mc::eigen_to_ros_vector(output.traj_vel.cast()); + feedback.setpoint.twist.angular = mc::eigen_to_ros_vector(output.traj_omega.cast()); + feedback.setpoint.accel.linear = mc::eigen_to_ros_vector(output.traj_accel.cast()); + feedback.setpoint.accel.angular = mc::eigen_to_ros_vector(output.traj_alpha.cast()); + feedback.index = std::distance(setpoint_, segment_.begin()); + // Sometimese segments arrive with a first setpoint that has a time stamp + // sometime in the future. In this case we will be in STOPPED mode until + // the callback timer sets the CMC mode to NOMINAL. In the interim the + // errors being sent back from the controller make no sense. + switch (mode_) { + case ff_msgs::ControlCommand::MODE_NOMINAL: + feedback.error_position = output.traj_error_pos; + feedback.error_attitude = output.traj_error_att; + feedback.error_velocity = output.traj_error_vel; + feedback.error_omega = output.traj_error_omega; + break; + default: + feedback.error_position = 0; + feedback.error_attitude = 0; + feedback.error_velocity = 0; + feedback.error_omega = 0; + break; + } + action_.SendFeedback(feedback); + } + + // GNC stepped successfully + return true; +} + +std::string Ctl::getName() { return name_; } + +// Chainload the readparam call +void Ctl::ReadParams(void) { + if (!config_.ReadFiles()) { + ROS_ERROR("Failed to read config files."); + return; + } + controller_.ReadParams(&config_); + if (!config_.GetBool("tun_debug_ctl_use_truth", &use_truth_)) + ROS_FATAL("tun_debug_ctl_use_truth not specified."); + // Set linear- and angular velocity threshold for ekf + // to decide that Astrobee is dynamically stopped. + if (!config_.GetReal("tun_ctl_stopping_vel_thresh", &stopping_vel_thresh_squared_)) + ROS_FATAL("tun_ctl_stopping_vel_thresh not specified."); + if (!config_.GetReal("tun_ctl_stopping_omega_thresh", &stopping_omega_thresh_squared_)) + ROS_FATAL("tun_ctl_stopping_omega_thresh not specified."); +} + +} // end namespace ctl diff --git a/gnc/ctl/statemachine.dot b/gnc/ctl/statemachine.dot deleted file mode 100644 index 830868a87e..0000000000 --- a/gnc/ctl/statemachine.dot +++ /dev/null @@ -1,27 +0,0 @@ -# dot -Tpdf statemachine.dot -o statemachine.pdf -digraph G { - graph [label="CTL\n", labelloc=t, fontsize=50]; - size="11.7,8.3!"; - node [shape=box, width=2.5, fontsize=12, fontname="helvetica"]; - edge [arrowsize=2, weight=2., fontsize=12, fontname="helvetica"]; - fontname = "helvetica"; - - node [shape=diamond, style=filled, fillcolor=lightblue, color=black]; - WAITING; - - node [shape=box, style=filled, fillcolor=lightblue, color=black]; - NOMINAL; - - # Actions - WAITING -> NOMINAL - [label="[0]\nGOAL_NOMINAL\nControl(NOMINAL, Segment)", color=black]; - NOMINAL -> WAITING - [label="[1]\nGOAL_COMPLETE\nControl(STOP)\nResult(SUCCESS)", color=black]; - NOMINAL -> WAITING - [label="[2]\nGOAL_CANCEL\nControl(STOP)\nResult(CANCELLED)", color=black]; - WAITING -> STOPPING - [label="[3]\nGOAL_STOP\nControl(STOP)", color=black]; - STOPPING -> WAITING - [label="[4]\nGOAL_COMPLETE\nResult(SUCCESS)", color=black]; - -} diff --git a/gnc/ctl/test/test_ctl.test b/gnc/ctl/test/test_ctl.test index 2cc8cc2ad0..b5383985ef 100644 --- a/gnc/ctl/test/test_ctl.test +++ b/gnc/ctl/test/test_ctl.test @@ -17,7 +17,6 @@ - diff --git a/gnc/ctl/test/test_ctl_nominal.cc b/gnc/ctl/test/test_ctl_nominal.cc index 86ad6f427c..7d867fac39 100644 --- a/gnc/ctl/test/test_ctl_nominal.cc +++ b/gnc/ctl/test/test_ctl_nominal.cc @@ -16,6 +16,11 @@ * under the License. */ +// Test ctl nominal +// This test starts the entire simulation stack. Once the ekf acquired position +// and published, a goal is sent to the controller for 3 poses. When the goal +// is executed and finished, the test passes if result is SUCCESS + // Required for the test framework #include @@ -23,15 +28,15 @@ #include #include -#include +#include #include #include #include #include // Publisher and subscribers -ros::Subscriber sub; -std::shared_ptr> ac; +ros::Subscriber sub_; +std::shared_ptr> ac_; // Called once when the goal completes void Done(const actionlib::SimpleClientGoalState& state, @@ -49,7 +54,7 @@ void Feedback(const ff_msgs::ControlFeedbackConstPtr& feedback) {} // When new state info is available void StateCallback(const ff_msgs::EkfStateConstPtr& state) { // Only one message wanted - sub.shutdown(); + sub_.shutdown(); // Send the Goal ROS_INFO("Sending goal to move to a pose"); ff_msgs::ControlGoal goal; @@ -95,23 +100,23 @@ void StateCallback(const ff_msgs::EkfStateConstPtr& state) { t3.pose.orientation.w = 0.999983; goal.segment.push_back(t3); // Send the goal! - ac->sendGoal(goal, &Done, &Active, &Feedback); + ac_->sendGoal(goal, &Done, &Active, &Feedback); } // Holonomic test TEST(ctl_nominal, Holonomic) { - // The default namespace is given by the group hierarchy in the luanch file + // The default namespace is given by the group hierarchy in the launch file ros::NodeHandle nh; // Wait for the servcer - ac = std::shared_ptr>( + ac_ = std::shared_ptr>( new actionlib::SimpleActionClient( nh, ACTION_GNC_CTL_CONTROL, true)); ROS_INFO("Waiting for action server to start."); - ac->waitForServer(); + ac_->waitForServer(); ROS_INFO("Action server started, sending goal."); // Syaty the node handle and wait for the state ros::NodeHandle n; - sub = n.subscribe(TOPIC_GNC_EKF, 1000, &StateCallback); + sub_ = n.subscribe(TOPIC_GNC_EKF, 1000, &StateCallback); ros::spin(); } diff --git a/gnc/ekf/CMakeLists.txt b/gnc/ekf/CMakeLists.txt deleted file mode 100644 index 1a3769a6ea..0000000000 --- a/gnc/ekf/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -project(ekf) - -catkin_package( - LIBRARIES ekf - CATKIN_DEPENDS roscpp sensor_msgs nav_msgs - DEPENDS ff_msgs ff_hw_msgs ff_common -) - -create_library(TARGET ekf - LIBS ${catkin_LIBRARIES} ${EIGEN_LIBRARIES} gnc_autocode msg_conversions camera ff_common config_reader ff_nodelet ff_flight - INC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - DEPS ff_msgs ff_hw_msgs) - -install_launch_files() diff --git a/gnc/ekf/include/ekf/ekf.h b/gnc/ekf/include/ekf/ekf.h deleted file mode 100644 index aae175a0f7..0000000000 --- a/gnc/ekf/include/ekf/ekf.h +++ /dev/null @@ -1,189 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_EKF_H_ -#define EKF_EKF_H_ - -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace ekf { - -typedef struct { - float x; - float y; -} OFObservation; - -typedef struct { - uint32_t feature_id; - std::vector obs; - int missing_frames; // number of frames skipped -} OFFeature; - -/** - * @brief Ekf implementation using GNC module - * @details Ekf implementation using GNC module - */ -class Ekf { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Ekf(void); - ~Ekf(); - - void Reset(); - - kfl_msg* GetOutput(void) {return &gnc_.kfl_;} - - void ReadParams(config_reader::ConfigReader* config); - void SetBias(Eigen::Vector3f gyro_bias, Eigen::Vector3f accel_bias); - - void PrepareStep(const sensor_msgs::Imu & imu, const geometry_msgs::Quaternion & quat); - int Step(ff_msgs::EkfState* state); - - void SparseMapUpdate(const ff_msgs::VisualLandmarks & vl); - bool ARTagUpdate(const ff_msgs::VisualLandmarks & vl); // returns true if reset transform - bool HRTagUpdate(const ff_msgs::DepthLandmarks & dl); // returns true if reset transform - void OpticalFlowUpdate(const ff_msgs::Feature2dArray & of); - void HandrailUpdate(const ff_msgs::DepthLandmarks & dl); - - // transform by from dock coordinates to world coordinates - Eigen::Affine3d GetDockToWorldTransform(void) {return dock_to_world_;} - - // transform by from perch coordinates to world coordinates - Eigen::Affine3d GetHandrailToWorldTransform(void) {return handrail_to_world_;} - - void SparseMapRegister(const ff_msgs::CameraRegistration & reg); - void ARTagRegister(const ff_msgs::CameraRegistration & reg); - void OpticalFlowRegister(const ff_msgs::CameraRegistration & reg); - void HandrailRegister(const ff_msgs::CameraRegistration & reg); - - void SetSpeedGain(const uint8_t gain); - - void SetResetCallback(std::function callback); - void ResetAR(void); // called when switching to AR mode - void ResetHR(void); // called when switching to HR (HandRail) mode - - Eigen::Affine3d GetNavCamToBody(void) const {return nav_cam_to_body_;} - - protected: - /** Functions **/ - /** - * If we're totally lose, this function reinitializes - * the EKF with a pose gained from visual landmarks. - **/ - void ResetPose(const Eigen::Affine3d & camera_to_body, geometry_msgs::Pose const& pose); - void ApplyReset(void); - - void DepthLandmarksUpdate(const ff_msgs::DepthLandmarks & dl); - void VisualLandmarksUpdate(const ff_msgs::VisualLandmarks & vl); - void VisualLandmarksRegister(const ff_msgs::CameraRegistration & reg); - - /** - * Writes the inputs for one time step to a file - * that can later be parsed and tested in matlab. - **/ - void WriteToFile(void); - - void UpdateState(ff_msgs::EkfState* state); - - /** Variables **/ - // this calls the simulink code that actually runs the EKF - gnc_autocode::GncEkfAutocode gnc_; - - // we write to these, then copy to gnc_ - cvs_landmark_msg vis_; - cvs_registration_pulse reg_; - cvs_optical_flow_msg of_; - cvs_handrail_msg hand_; - imu_msg imu_; - int cmc_mode_; - - // EKF wrapper needs to capture the first landmark message to do PnP before - // it can initialize the EKF autocode and let it run. This variable holds - // that state and is atomic so that it doesn't require a mutex. - bool reset_ekf_; - // remember pose to reset to do in step function to avoid race conditions - Eigen::Affine3d reset_camera_to_body_; - geometry_msgs::Pose reset_pose_; - bool reset_ready_; - - // optional: called when a reset occurs - std::function reset_callback_; - - // vector of feature ids and observations, observations are features ids and positions - // this list is sorted by feature id - std::map optical_flow_features_; - // the number of features for each augmentation - std::vector optical_flow_augs_feature_counts_; - std::vector deleting_augs_; - // the times of each augmentation - std::vector optical_flow_augs_times_; - bool processing_of_reg_, of_inputs_delayed_; - - // in output to file mode, the file to output to - FILE* output_file_; - - // only save this for writing to a file later - geometry_msgs::Pose last_estimate_pose_; - - - /** Configuration Constants **/ - - // transform from camera frame to body frame - Eigen::Affine3d camera_to_body_, nav_cam_to_body_, perch_cam_to_body_, dock_cam_to_body_, imu_to_body_; - float nav_cam_focal_length_, dock_cam_focal_length_; - int ml_max_features_; - int min_of_observations_; - unsigned int of_history_size_, of_max_features_; - int dl_max_features_; - - // camera ids currently stored in the EKF's augmented state - uint32_t vl_camera_id_; - uint32_t of_camera_id_; - uint32_t dl_camera_id_; - - /** ar offset related **/ - bool reset_dock_pose_; - Eigen::Affine3d dock_to_world_; - - /** hr offset relatedd **/ - bool reset_handrail_pose_; - Eigen::Affine3d handrail_to_world_; -}; -} // end namespace ekf - -#endif // EKF_EKF_H_ diff --git a/gnc/ekf/include/ekf/ekf_wrapper.h b/gnc/ekf/include/ekf/ekf_wrapper.h deleted file mode 100644 index e392e1b5d4..0000000000 --- a/gnc/ekf/include/ekf/ekf_wrapper.h +++ /dev/null @@ -1,216 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_EKF_WRAPPER_H_ -#define EKF_EKF_WRAPPER_H_ - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include // NOLINT -#include -#include -#include -#include -#include -#include - -namespace ekf { - -/** - * @brief ROS wrapper around EKF GNC code. - * @details ROS wrapper around EKF GNC code. - */ -class EkfWrapper { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - explicit EkfWrapper(ros::NodeHandle* nh, std::string const& name); - ~EkfWrapper(); - - void Run(std::atomic const& killed); - - /** - * This moves the EKF forward one time step - * and publishes the EKF pose. - * - * Returns zero if EKF was not run, non-zero if it was. - */ - int Step(); - - protected: - void ReadParams(void); - /** - * Initialize services and topics besides IMU. - **/ - void InitializeEkf(void); - /** - * Publishes a ROS message containing the state of the EKF. - **/ - void PublishState(const ff_msgs::EkfState & state); - - /** - * Resets the EKF. - **/ - bool ResetService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); //NOLINT - - bool ResetHandrailService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); - - /** - - * Initializes the bias and saves the results to a file. - **/ - bool InitializeBiasService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); //NOLINT - /** - * Set the input mode between mapped landmarks, AR tags, and handrail. - **/ - bool SetInputService(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res); //NOLINT - - /** - * Actually does the bias estimation, called from IMU callback. - **/ - void EstimateBias(sensor_msgs::Imu::ConstPtr const& imu); - - /** - * Callback functions. These all record the information - * and then pass it on to the EKF the next time the Step function - * is called. - **/ - void ImuCallBack(sensor_msgs::Imu::ConstPtr const& imu); - void OpticalFlowCallBack(ff_msgs::Feature2dArray::ConstPtr const& of); - void VLVisualLandmarksCallBack(ff_msgs::VisualLandmarks::ConstPtr const& vl); - void ARVisualLandmarksCallBack(ff_msgs::VisualLandmarks::ConstPtr const& vl); - void DepthLandmarksCallBack(ff_msgs::DepthLandmarks::ConstPtr const& vl); - void VLRegisterCamera(ff_msgs::CameraRegistration::ConstPtr const& cr); - void ARRegisterCamera(ff_msgs::CameraRegistration::ConstPtr const& cr); - void RegisterDepthCamera(ff_msgs::CameraRegistration::ConstPtr const& cr); - void RegisterOpticalFlowCamera(ff_msgs::CameraRegistration::ConstPtr const& cr); - void GroundTruthCallback(geometry_msgs::PoseStamped::ConstPtr const& truth); - void GroundTruthTwistCallback(geometry_msgs::TwistStamped::ConstPtr const& truth); - void FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode); - - /** - * Callback when a subscriber joins or disconnects from the feature topic - **/ - void SubscriberCallback(); - void PublishFeatures(ff_msgs::VisualLandmarks::ConstPtr const& l); - void PublishFeatures(ff_msgs::DepthLandmarks::ConstPtr const& l); - - /** - * Callback when the EKF resets - **/ - void ResetCallback(); - - /** Variables **/ - // the actual EKF - Ekf ekf_; - ff_msgs::EkfState state_; - - bool ekf_initialized_; - - // most recent imu message - sensor_msgs::Imu imu_; - geometry_msgs::Quaternion quat_; - int imus_dropped_; - - // Truth messages - geometry_msgs::PoseStamped truth_pose_; - geometry_msgs::TwistStamped truth_twist_; - - // EKF wrapper needs to capture the first landmark message to do PnP before - // it can initialize the EKF autocode and let it run. This variable holds - // that state and is atomic so that it doesn't require a mutex. - std::atomic have_imu_; - std::atomic input_mode_; - - /** Configuration Constants **/ - - /** Ros **/ - config_reader::ConfigReader config_; - ff_util::PerfTimer pt_ekf_; - ros::Timer config_timer_; - - ros::NodeHandle* nh_; - // topic subscribers - ros::Subscriber imu_sub_, of_sub_, vl_sub_, ar_sub_, dl_sub_, truth_sub_, truth_twist_sub_; - ros::Subscriber vl_reg_sub_, ar_reg_sub_, of_reg_sub_, dl_reg_sub_; - ros::Subscriber flight_mode_sub_; - - // publisher - ros::Publisher state_pub_; - ros::Publisher feature_pub_; - ros::Publisher pose_pub_, twist_pub_; - ros::Publisher reset_pub_; - tf2_ros::TransformBroadcaster transform_pub_; - ros::ServiceServer reset_srv_, bias_srv_, input_mode_srv_, reset_hr_srv_; - - /** Threading **/ - - // mutex for msgs, concurrency protection - std::mutex mutex_act_msg_; - std::mutex mutex_imu_msg_; - std::mutex mutex_of_msg_; - std::mutex mutex_vl_msg_; - std::mutex mutex_dl_msg_; - std::mutex mutex_truth_msg_; - - // cv to wait for an imu reading - std::condition_variable cv_imu_; - - /** IMU Bias reset variables **/ - std::string bias_file_; - bool estimating_bias_; - float bias_reset_sums_[6]; - int bias_reset_count_; - int bias_required_observations_; - - /** Feature drawing **/ - std::string platform_name_; - bool disp_features_; - sensor_msgs::PointCloud2 features_; - - // Prevents needing to call ros::ok() from a thread - std::atomic killed_; -}; - -} // end namespace ekf - -#endif // EKF_EKF_WRAPPER_H_ diff --git a/gnc/ekf/launch/ekf.launch b/gnc/ekf/launch/ekf.launch deleted file mode 100644 index 573e1f0308..0000000000 --- a/gnc/ekf/launch/ekf.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/gnc/ekf/nodelet_plugins.xml b/gnc/ekf/nodelet_plugins.xml deleted file mode 100644 index d57eb9774e..0000000000 --- a/gnc/ekf/nodelet_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - This nodelet wraps GNC's implementations of the EKF module. - - - diff --git a/gnc/ekf/package.xml b/gnc/ekf/package.xml deleted file mode 100644 index f65c8d463f..0000000000 --- a/gnc/ekf/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - ekf - 0.0.0 - Calls the GNC simulink auto-generated EKF code, and passes - inputs and outputs to and from ros messages. - - - Apache License, Version 2.0 - - - Astrobee Flight Software - - - Astrobee Flight Software - - catkin - roscpp - sensor_msgs - vpp_msgs - nav_msgs - ff_msgs - nodelet - roscpp - rt_analyze - sensor_msgs - vpp_msgs - nav_msgs - ff_msgs - nodelet - - - - diff --git a/gnc/ekf/readme.md b/gnc/ekf/readme.md deleted file mode 100644 index 33af094847..0000000000 --- a/gnc/ekf/readme.md +++ /dev/null @@ -1,76 +0,0 @@ -\page ekf GNC EKF Wrapper - -The extended kalman filter tracks the robot's pose. - -# Overview - -The EKF is based on the augmented state extended kalman filter by Mourikis et. al. -We localize from IMU readings and visual odometry, together with one of sparse map features, -AR tags, or handrail detections. Only one of these last features can be used at a time. -The filter has three main steps: - -* **Predict**: With every IMU measurement, the EKF forward predicts the robot's motion based on the IMU measurement (linear acceleration and angular velocity). The predict step must -be run a constant rate (currently 62.5 Hz). Every iteration -of the loop must run at this rate, so the performance of the code is critical. -* **Registration**: Camera images cannot be process within one iteration of the predict step. -Hence, we have a registration step when an image is captured. When a registration pulse is -sent to GNC, the EKF state is *augmented*, and we remember the robot's pose when the image -was captured, and capture the covariance to the rest of the state. -* **Update**: When an image is processsed, an observation is used to update the state. - -For more details on how the EKF works, see our paper, *Localization from Visual Landmarks on a Free-Flying Robot. -Brian Coltin, Jesse Fusco, Zack Moratto, Oleg Alexandrov, and Robert Nakamura. IROS 2016*. - -# Inputs - -* IMU Readings, `/hw/imu`: The IMU message. Must be received at a constant rate. -* Sparse Map Features, `/loc/ml/features` and `/loc/ml/registration`: -The features and registration pulses from the sparse map. The features include image coordinates and corresponding -3D feature positions from the map. -* AR Tag Features, `/loc/ar/features` and `/loc/ar/registration`: The same as the -sparse map features, but for AR tags. These are assumed to come from the dock camera. -* Optical Flow Features: `/loc/of/features` and `/loc/of/registration`: These -features are used for visual odometry. They are not associated with a 3D position, but are tracked over time -with a unique associated ID. Inside the EKF wrapper we keep track of these points over time and -send appropriate features to the EKF. -* Handrail Features: `/loc/handrail/features` and `/loc/handrail/registration`: The features for -localization with respect to the handrail. These are point features like with the sparse map features, but also -include a direction of the handrail axis and a boolean indicating whether the position along this axis has been observed. - -# Outputs - -* `/gnc/ekf`, the body state. See the EkfState message documentation for details. -* The body tf2 transform. - -# Services - -* `/gnc/ekf/reset`: Resets the EKF, setting the pose based on the next incoming visual landmarks. -* `/gnc/ekf/initialize_bias`: Observes the accelerometer readings over the next several seconds, -sets the bias, and resets the EKF. This must only be called when the robot is stationary! -* `/gnc/ekf/set_input`: Switches the input mode of the EKF between sparse map features, AR tags, -and handrail features. Only one of these inputs can be used at a time. However, visual -odometry is always active. - -# Threading Model - -For good pose tracking, it is key to eliminate latency in IMU measurements and camera -registrations. That is why we use a multi-threaded ROS node, in which all of the -subscriptions record incoming data in a non-blocking manner. A separate thread loops continuously -and runs the GNC autocode step function whenever a new IMU measurement is received -(informed via a condition variable). This allows us to guarantee that all received -registration pulses and updates will be processed on the next tick. - -Note that each step of the EKF must run within one IMU tick, or serious problems will arise. -All of the ROS subscriptions and threading code is put in `ekf_wrapper.cc`, while the core -functionality is presented in `ekf.cc`. This way the class in `ekf.cc` can be used in -other ways, for example, when processing a bag file. - -# Selecting Visual Odometry Features - -Largely, the EKF wrapper simply passes the inputs directly to the GNC matlab code. The one -exception is the visual odometry features. Here, the wrapper keeps track of which augmentations are -stored in the EKF, and the feature observations during those observations. It chooses which -augmentations to send to the EKF when, and then deletes the sent features and chooses new -augmentations. The goals are to send features spanning as wide as possible a period of time, -to drive the covariance down, and to never send the same observation twice (because this results in overconfidence). - diff --git a/gnc/ekf/src/ekf.cc b/gnc/ekf/src/ekf.cc deleted file mode 100644 index 8fb8ca9db7..0000000000 --- a/gnc/ekf/src/ekf.cc +++ /dev/null @@ -1,733 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -DEFINE_bool(save_inputs_file, false, "Save the inputs to a file."); - -namespace ekf { - -Ekf::Ekf(void) : - reset_ekf_(true), reset_ready_(false), reset_callback_(nullptr), - processing_of_reg_(false), of_inputs_delayed_(false), output_file_(NULL), - vl_camera_id_(0), of_camera_id_(0), dl_camera_id_(0), reset_dock_pose_(true) { - gnc_.cmc_.speed_gain_cmd = 1; // prevent from being invalid when running bags - of_history_size_ = ASE_OF_NUM_AUG; - of_max_features_ = ASE_OF_NUM_FEATURES; - ml_max_features_ = ASE_ML_NUM_FEATURES; - dl_max_features_ = ASE_ML_NUM_FEATURES; - memset(&vis_, 0, sizeof(cvs_landmark_msg)); - memset(®_, 0, sizeof(cvs_registration_pulse)); - memset(&of_, 0, sizeof(cvs_optical_flow_msg)); - memset(&hand_, 0, sizeof(cvs_handrail_msg)); - memset(&imu_, 0, sizeof(imu_msg)); - - - bool output = FLAGS_save_inputs_file; - if (output) { - output_file_ = fopen("ekf_inputs.csv", "w"); - assert(output_file_); - ROS_WARN("Recording EKF inputs. EKF *NOT* running."); - } -} - -Ekf::~Ekf() { - if (output_file_) { - fclose(output_file_); - output_file_ = NULL; - } -} - -void Ekf::ReadParams(config_reader::ConfigReader* config) { - gnc_.ReadParams(config); - - if (!config->GetInt("min_of_observations", &min_of_observations_)) - ROS_FATAL("Unspecified min_of_observations."); - - // get camera transform - Eigen::Vector3d trans; - Eigen::Quaterniond rot; - if (!msg_conversions::config_read_transform(config, "nav_cam_transform", &trans, &rot)) - ROS_FATAL("Unspecified nav_cam_transform."); - nav_cam_to_body_ = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - if (!msg_conversions::config_read_transform(config, "dock_cam_transform", &trans, &rot)) - ROS_FATAL("Unspecified dock_cam_transform."); - dock_cam_to_body_ = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - if (!msg_conversions::config_read_transform(config, "perch_cam_transform", &trans, &rot)) - ROS_FATAL("Unspecified perch_cam_transform."); - perch_cam_to_body_ = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - if (!msg_conversions::config_read_transform(config, "imu_transform", &trans, &rot)) - ROS_FATAL("Unspecified imu_transform."); - imu_to_body_ = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - - std::string imu_filename; - if (!config->GetStr("imu_bias_file", &imu_filename)) { - ROS_FATAL("IMU bias file not specified."); - } - - // Modifying here some initial bias estimates. Also setting gravity subtract - // to happen in the body frame instead of the world frame. Removing in the - // world frame is fragile because vision could possibly rotate the - // body_q_world slightly, making subtraction way off. - auto& p = gnc_.est_->defaultParam; - Eigen::Map gyro_fixed_bias(p->ase_gyro_fixed_bias), - accel_fixed_bias(p->ase_accel_fixed_bias); - std::vector new_bias(3); - FILE* f = fopen(imu_filename.c_str(), "r"); - if (f) { - int ret = fscanf(f, "%g %g %g\n", &new_bias[0], &new_bias[1], &new_bias[2]); - if (ret == 3) - gyro_fixed_bias = Eigen::Vector3f(new_bias[0], new_bias[1], new_bias[2]); - ret = fscanf(f, "%g %g %g\n", &new_bias[0], &new_bias[1], &new_bias[2]); - if (ret == 3) { - accel_fixed_bias = Eigen::Vector3f(new_bias[0], new_bias[1], new_bias[2]); - } - fclose(f); - } else { - ROS_WARN("No bias file found at %s.", imu_filename.c_str()); - } -} - -void Ekf::SetBias(Eigen::Vector3f gyro_bias, Eigen::Vector3f accel_bias) { - Eigen::Map gyro_fixed_bias( - gnc_.est_->defaultParam->ase_gyro_fixed_bias), - accel_fixed_bias(gnc_.est_->defaultParam->ase_accel_fixed_bias); - gyro_fixed_bias = gyro_bias; - accel_fixed_bias = accel_bias; -} - -void Ekf::OpticalFlowUpdate(const ff_msgs::Feature2dArray & of) { - // check that the camera id matches our registration - if (of_camera_id_ != of.camera_id) { - // ROS_DEBUG_THROTTLE(1, "Registered optical flow camera not found."); - return; - } - - // put new features in tree to make it faster - std::map new_features; - for (size_t i = 0; i < of.feature_array.size(); i++) { - new_features.insert(std::pair(of.feature_array[i].id, - {of.feature_array[i].x, of.feature_array[i].y})); - } - - // delete all observations that we didn't see - std::map::iterator of_iter = optical_flow_features_.begin(); - while (of_iter != optical_flow_features_.end()) { - if (new_features.count((*of_iter).first) == 0) { - OFFeature & f = (*of_iter).second; - f.missing_frames++; - } - of_iter++; - } - - // now, add new features to the list of existing features - for (size_t i = 0; i < of.feature_array.size(); i++) { - uint16_t feature_id = of.feature_array[i].id; - float x = of.feature_array[i].x; - float y = of.feature_array[i].y; - // add new feature to list - if (optical_flow_features_.count(feature_id) == 0) { - optical_flow_features_.insert(std::pair( - feature_id, {feature_id, std::vector(), 0})); - } - OFFeature & f = optical_flow_features_[feature_id]; - // add the new observations - f.obs.insert(f.obs.begin(), {x, y}); - optical_flow_augs_feature_counts_[0]++; - } - of_camera_id_ = 0; - - if (deleting_augs_.size() > 0) - return; - if (optical_flow_augs_feature_counts_.size() >= of_history_size_ && - optical_flow_augs_feature_counts_[of_history_size_ - 1] > 3) { - deleting_augs_.push_back(0); - if (optical_flow_augs_feature_counts_[of_history_size_ - 3] < 10) { - deleting_augs_.push_back(of_history_size_ - 3); - deleting_augs_.push_back(of_history_size_ - 2); - deleting_augs_.push_back(of_history_size_ - 1); - } else { - deleting_augs_.push_back(2); - if (optical_flow_augs_feature_counts_[of_history_size_ - 2] < 10) { - deleting_augs_.push_back(of_history_size_ - 2); - deleting_augs_.push_back(of_history_size_ - 1); - } else { - deleting_augs_.push_back(6); - if (optical_flow_augs_feature_counts_[of_history_size_ - 1] < 10) { - deleting_augs_.push_back(of_history_size_ - 1); - } else { - ros::Time now = of.header.stamp; - double age_1 = (now - optical_flow_augs_times_[of_history_size_ - 1]).toSec(); - double age_2 = (now - optical_flow_augs_times_[of_history_size_ - 2]).toSec(); - double age_3 = (now - optical_flow_augs_times_[of_history_size_ - 3]).toSec(); - double age_4 = (now - optical_flow_augs_times_[of_history_size_ - 4]).toSec(); - if (age_1 < 2 * age_2) - deleting_augs_.push_back(of_history_size_ - 2); - else if (age_2 < 2 * age_3) - deleting_augs_.push_back(of_history_size_ - 3); - else if (age_3 < 2 * age_4) - deleting_augs_.push_back(of_history_size_ - 4); - else - deleting_augs_.push_back(of_history_size_ - 5); - } - } - } - } else { - return; - } - - // clear all valid flags - unsigned int index = 0; - memset(of_.cvs_valid_flag, 0, of_history_size_ * of_max_features_); - // we have to wait until the next timestep - if (!processing_of_reg_) { - of_.cvs_timestamp_sec = of.header.stamp.sec; - of_.cvs_timestamp_nsec = of.header.stamp.nsec; - } else { - of_inputs_delayed_ = true; - } - std::map::iterator iter = optical_flow_features_.begin(); - while (iter != optical_flow_features_.end()) { - OFFeature & f = (*iter).second; - if (f.missing_frames > 0) { - // We are no longer using these observations because we can greatly speed up - // computation in the EKF with a sparse block H matrix - for (unsigned int i = 0; i < f.obs.size(); i++) { - unsigned int aug = i + f.missing_frames; - if (aug >= of_history_size_) - break; - // of_.cvs_observations[aug * of_max_features_ * 2 + index] = f.obs[i].x; - // of_.cvs_observations[aug * of_max_features_ * 2 + index + of_max_features_] = f.obs[i].y; - // of_.cvs_valid_flag[aug * of_max_features_ + index] = 1; - if (optical_flow_augs_feature_counts_.size() > aug) - optical_flow_augs_feature_counts_[aug]--; - } - iter = optical_flow_features_.erase(iter); - } else { - // only use oldest features, and choose three oldest frames and newest - if (f.obs.size() >= of_history_size_) { - of_.cvs_observations[0 * of_max_features_ * 2 + index] = f.obs[0].x; - of_.cvs_observations[0 * of_max_features_ * 2 + index + of_max_features_] = f.obs[0].y; - of_.cvs_valid_flag[0 * of_max_features_ + index] = 1; - for (unsigned int aug = of_history_size_ - 3; aug < of_history_size_; aug++) { - of_.cvs_observations[aug * of_max_features_ * 2 + index] = f.obs[aug].x; - of_.cvs_observations[aug * of_max_features_ * 2 + index + of_max_features_] = f.obs[aug].y; - of_.cvs_valid_flag[aug * of_max_features_ + index] = 1; - } - } - iter++; - index++; - } - if (index >= of_max_features_) - break; - } - // just delete any missing features we didn't have space for - while (iter != optical_flow_features_.end()) { - OFFeature & f = (*iter).second; - if (f.missing_frames > 0) { - for (unsigned int i = 0; i < f.obs.size(); i++) { - unsigned int aug = i + f.missing_frames; - if (aug >= of_history_size_) - break; - // of_.cvs_observations[aug * of_max_features_ * 2 + index] = f.obs[i].x; - // of_.cvs_observations[aug * of_max_features_ * 2 + index + of_max_features_] = f.obs[i].y; - // of_.cvs_valid_flag[aug * of_max_features_ + index] = 1; - if (optical_flow_augs_feature_counts_.size() > aug) - optical_flow_augs_feature_counts_[aug]--; - } - iter = optical_flow_features_.erase(iter); - } - iter++; - } -} - -void Ekf::SparseMapUpdate(const ff_msgs::VisualLandmarks & vl) { - VisualLandmarksUpdate(vl); - if (!output_file_ && reset_ekf_ && vl.landmarks.size() >= 5) - ResetPose(nav_cam_to_body_, vl.pose); - cmc_mode_ = ff_msgs::SetEkfInputRequest::MODE_MAP_LANDMARKS; -} - -void Ekf::ResetAR(void) { - reset_dock_pose_ = true; -} - -void Ekf::ResetHR(void) { - reset_handrail_pose_ = true; -} - -bool Ekf::ARTagUpdate(const ff_msgs::VisualLandmarks & vl) { - bool updated = false; - if (reset_dock_pose_) { - if (vl.landmarks.size() < 4) - return false; - geometry_msgs::Pose p; - p.position = msg_conversions::array_to_ros_point(gnc_.kfl_.P_B_ISS_ISS); - p.orientation = msg_conversions::array_to_ros_quat(gnc_.kfl_.quat_ISS2B); - Eigen::Affine3d wTb = msg_conversions::ros_pose_to_eigen_transform(p); - Eigen::Affine3d dTc = msg_conversions::ros_pose_to_eigen_transform(vl.pose); - Eigen::Affine3d bTc = dock_cam_to_body_; - dock_to_world_ = wTb * bTc * dTc.inverse(); - reset_dock_pose_ = false; - updated = true; - } - - ff_msgs::VisualLandmarks vl_mod = vl; - for (unsigned int i = 0; i < vl.landmarks.size(); i++) { - Eigen::Vector3d l(vl.landmarks[i].x, vl.landmarks[i].y, vl.landmarks[i].z); - l = dock_to_world_ * l; - vl_mod.landmarks[i].x = l.x(); - vl_mod.landmarks[i].y = l.y(); - vl_mod.landmarks[i].z = l.z(); - } - VisualLandmarksUpdate(vl_mod); - vl_mod.pose.position = msg_conversions::eigen_to_ros_point( - dock_to_world_ * msg_conversions::ros_point_to_eigen_vector(vl.pose.position)); - Eigen::Quaterniond rot(dock_to_world_.linear()); - vl_mod.pose.orientation = msg_conversions::eigen_to_ros_quat( - rot * msg_conversions::ros_to_eigen_quat(vl.pose.orientation)); - - if (!output_file_ && reset_ekf_ && vl_mod.landmarks.size() >= 4) - ResetPose(dock_cam_to_body_, vl_mod.pose); - cmc_mode_ = ff_msgs::SetEkfInputRequest::MODE_AR_TAGS; - - return updated; -} - -void Ekf::VisualLandmarksUpdate(const ff_msgs::VisualLandmarks & vl) { - // check that the camera frame matches the one we just registered - if (vl_camera_id_ != vl.camera_id) { - ROS_DEBUG_THROTTLE(1, "Registered visual landmark camera %d not found.", vl.camera_id); - return; - } - if (vl.landmarks.size() < 5) - return; - last_estimate_pose_= vl.pose; - // find better way to choose limited landmarks to send? - for (int i = 0; i < std::min(ml_max_features_, static_cast(vl.landmarks.size())); i++) { - vis_.cvs_landmarks[i] = vl.landmarks[i].x; - vis_.cvs_landmarks[ml_max_features_ + i] = vl.landmarks[i].y; - vis_.cvs_landmarks[2 * ml_max_features_ + i] = vl.landmarks[i].z; - vis_.cvs_observations[i] = vl.landmarks[i].u; - vis_.cvs_observations[ml_max_features_ + i] = vl.landmarks[i].v; - vis_.cvs_valid_flag[i] = true; - } - for (int i = vl.landmarks.size(); i < ml_max_features_; i++) - vis_.cvs_valid_flag[i] = false; - - vis_.cvs_timestamp_sec = vl.header.stamp.sec; - vis_.cvs_timestamp_nsec = vl.header.stamp.nsec; -} - -bool Ekf::HRTagUpdate(const ff_msgs::DepthLandmarks & dl) { - bool updated = false; - if (reset_handrail_pose_) { - if (dl.landmarks.size() < 1) - return false; - geometry_msgs::Pose p; - p.position = msg_conversions::array_to_ros_point(gnc_.kfl_.P_B_ISS_ISS); - p.orientation = msg_conversions::array_to_ros_quat(gnc_.kfl_.quat_ISS2B); - - Eigen::Affine3d wTb = msg_conversions::ros_pose_to_eigen_transform(p); - Eigen::Affine3d hTc = msg_conversions::ros_pose_to_eigen_transform(dl.local_pose); - Eigen::Affine3d bTc = perch_cam_to_body_; - handrail_to_world_ = wTb * bTc * hTc; - ROS_WARN(" [EKF] Recalculated handrail_to_world transform."); - reset_handrail_pose_ = false; - updated = true; - } - ff_msgs::DepthLandmarks dl_mod = dl; - for (unsigned int i = 0; i < dl.landmarks.size(); i++) { - Eigen::Vector3d l(dl.landmarks[i].u, dl.landmarks[i].v, dl.landmarks[i].w); - l = handrail_to_world_ * l; - dl_mod.landmarks[i].u = l.x(); - dl_mod.landmarks[i].v = l.y(); - dl_mod.landmarks[i].w = l.z(); - } - HandrailUpdate(dl); - dl_mod.local_pose.position = msg_conversions::eigen_to_ros_point( - handrail_to_world_ * msg_conversions::ros_point_to_eigen_vector(dl.local_pose.position)); - Eigen::Quaterniond rot(handrail_to_world_.linear()); - dl_mod.local_pose.orientation = msg_conversions::eigen_to_ros_quat( - rot * msg_conversions::ros_to_eigen_quat(dl.local_pose.orientation)); - - if (!output_file_ && reset_ekf_ && dl_mod.landmarks.size() >= 1) - ResetPose(perch_cam_to_body_, dl_mod.local_pose); - cmc_mode_ = ff_msgs::SetEkfInputRequest::MODE_HANDRAIL; - - return updated; -} - -void Ekf::HandrailUpdate(const ff_msgs::DepthLandmarks & dl) { - // check that the camera frame matches the one we just registered - if (dl_camera_id_ != dl.camera_id) { - ROS_DEBUG_THROTTLE(1, "Registered depth landmark camera not found."); - return; - } - - for (int i = 0; i < std::min(dl_max_features_, static_cast(dl.landmarks.size())); i++) { - hand_.cvs_observations[i] = dl.landmarks[i].u; - hand_.cvs_observations[dl_max_features_ + i] = dl.landmarks[i].v; - hand_.cvs_observations[2 * dl_max_features_ + i] = dl.landmarks[i].w; - hand_.cvs_valid_flag[i] = true; - } - - for (int i = dl.landmarks.size(); i < dl_max_features_; i++) - hand_.cvs_valid_flag[i] = false; - - hand_.cvs_3d_knowledge_flag = dl.end_seen; - - if (reset_ekf_) { - if (hand_.cvs_handrail_update_global_pose_flag == 0) - hand_.cvs_handrail_update_global_pose_flag = 1; - else - reset_ekf_ = false; - } else { - hand_.cvs_handrail_update_global_pose_flag = 0; - } - - hand_.cvs_handrail_local_pos[0] = dl.local_pose.position.x; - hand_.cvs_handrail_local_pos[1] = dl.local_pose.position.y; - hand_.cvs_handrail_local_pos[2] = dl.local_pose.position.z; - hand_.cvs_handrail_local_quat[0] = dl.local_pose.orientation.x; - hand_.cvs_handrail_local_quat[1] = dl.local_pose.orientation.y; - hand_.cvs_handrail_local_quat[2] = dl.local_pose.orientation.z; - hand_.cvs_handrail_local_quat[3] = dl.local_pose.orientation.w; - - hand_.cvs_timestamp_sec = dl.header.stamp.sec; - hand_.cvs_timestamp_nsec = dl.header.stamp.nsec; -} - -void Ekf::OpticalFlowRegister(const ff_msgs::CameraRegistration & cr) { - processing_of_reg_ = true; - unsigned int erased_aug = 0; - if (of_camera_id_ != 0) { - // ROS_WARN("Failed to get observations on time. Tossing last frame."); - erased_aug = 0; - } else { - if (deleting_augs_.size() > 0) { - erased_aug = deleting_augs_[0]; - deleting_augs_.erase(deleting_augs_.begin()); - } else { - // choose the augmentation to delete - if (optical_flow_augs_feature_counts_.size() < of_history_size_) { - erased_aug = of_history_size_ - 1; - } else if (optical_flow_augs_feature_counts_[of_history_size_ - 1] < 10) { - erased_aug = of_history_size_ - 1; - } else { - erased_aug = 0; - } - } - // delete the features from the deleted augmented state - std::map::iterator of_iter = optical_flow_features_.begin(); - while (of_iter != optical_flow_features_.end()) { - OFFeature & f = (*of_iter).second; - if (f.obs.size() > erased_aug) { - f.obs.erase(f.obs.begin() + erased_aug); - } - if (f.obs.size() == 0) - of_iter = optical_flow_features_.erase(of_iter); - else - of_iter++; - } - - // update arrays of times and counts - if (optical_flow_augs_feature_counts_.size() > of_history_size_) { - optical_flow_augs_feature_counts_.erase(optical_flow_augs_feature_counts_.begin() + erased_aug); - optical_flow_augs_times_.erase(optical_flow_augs_times_.begin() + erased_aug); - } - optical_flow_augs_feature_counts_.insert(optical_flow_augs_feature_counts_.begin(), 0); - optical_flow_augs_times_.insert(optical_flow_augs_times_.begin(), cr.header.stamp); - } - - // output to GNC - reg_.cvs_optical_flow_pulse = of_history_size_ - erased_aug; - of_camera_id_ = cr.camera_id; -} - -void Ekf::SparseMapRegister(const ff_msgs::CameraRegistration & reg) { - VisualLandmarksRegister(reg); -} - -void Ekf::ARTagRegister(const ff_msgs::CameraRegistration & reg) { - VisualLandmarksRegister(reg); -} - -void Ekf::VisualLandmarksRegister(const ff_msgs::CameraRegistration & reg) { - vl_camera_id_ = reg.camera_id; - reg_.cvs_landmark_pulse = true; -} - -void Ekf::HandrailRegister(const ff_msgs::CameraRegistration & reg) { - dl_camera_id_ = reg.camera_id; - reg_.cvs_handrail_pulse = true; -} - -void Ekf::SetSpeedGain(const uint8_t gain) { - gnc_.cmc_.speed_gain_cmd = gain; -} - -void Ekf::SetResetCallback(std::function callback) { - reset_callback_ = callback; -} - -// this saves all the inputs to a file if an output file is specified, -// can be debugged later in matlab -void Ekf::WriteToFile(void) { - // first output timestamp - fprintf(output_file_, "%d, %d, ", gnc_.imu_.imu_timestamp_sec, gnc_.imu_.imu_timestamp_nsec); // 1, 2 - // next all the imu data - fprintf(output_file_, "%g, %g, %g, %g, %g, %g, ", // 3, 4, 5, 6, 7, 8 - gnc_.imu_.imu_omega_B_ECI_sensor[0], gnc_.imu_.imu_omega_B_ECI_sensor[1], gnc_.imu_.imu_omega_B_ECI_sensor[2], - gnc_.imu_.imu_A_B_ECI_sensor[0], gnc_.imu_.imu_A_B_ECI_sensor[1], gnc_.imu_.imu_A_B_ECI_sensor[2]); - - // now the registration pulses - fprintf(output_file_, "%d, %d, ", gnc_.reg_.cvs_landmark_pulse, gnc_.reg_.cvs_optical_flow_pulse); // 9, 10 - // next visual landmarks - fprintf(output_file_, "%d, %d, ", gnc_.vis_.cvs_timestamp_sec, gnc_.vis_.cvs_timestamp_nsec); // 11, 12 - // now the estimated pose - fprintf(output_file_, "%g, %g, %g, ", last_estimate_pose_.position.x, // 13, 14, 15 - last_estimate_pose_.position.y, last_estimate_pose_.position.z); - fprintf(output_file_, "%g, %g, %g, %g, ", last_estimate_pose_.orientation.x, // 16, 17, 18, 19 - last_estimate_pose_.orientation.y, last_estimate_pose_.orientation.z, - last_estimate_pose_.orientation.w); - // now the contents of cvs_landmarks. Stored as x0 x1 x2 ... y0 y1 y2... z0 z1 z2... - for (int i = 0; i < ml_max_features_ * 3; i++) // 20 ~ 169 (50 * 3) - fprintf(output_file_, "%g, ", gnc_.vis_.cvs_landmarks[i]); - // then the observations, again stored as u0 u1 u2... v0 v1 v2... - for (int i = 0; i < ml_max_features_ * 2; i++) // 170 ~ 269 (50 * 2) - fprintf(output_file_, "%g, ", gnc_.vis_.cvs_observations[i]); - // finally the valid flags - for (int i = 0; i < ml_max_features_; i++) // 270 ~ 319 (50) - fprintf(output_file_, "%d, ", gnc_.vis_.cvs_valid_flag[i]); - - // next is the optical flow - fprintf(output_file_, "%d, %d, ", gnc_.of_.cvs_timestamp_sec, gnc_.of_.cvs_timestamp_nsec); // 320, 321 - // now the optical flow observations, stored as (xij is jth observation of feature i) - // x00 x10 x20 x30 ... xn0 - // y00 y10 y20 y30 ... yn0 - // x01 x11 x21 x31 ... xn1 - // ... - for (unsigned int i = 0; i < of_max_features_ * of_history_size_ * 2; i++) // 322 ~ 1921 (50 * 2 * 16) - fprintf(output_file_, "%g, ", gnc_.of_.cvs_observations[i]); - // then valid flags, stored as - // v00 v10 v20 v30 ... vn0 - // v01 v11 v21 v31 ... vn1 - // ... - for (unsigned int i = 0; i < of_max_features_ * of_history_size_; i++) // 1922 ~ 2721 (50 * 16) - fprintf(output_file_, "%d, ", gnc_.of_.cvs_valid_flag[i]); - - // handrail registration pulses - fprintf(output_file_, "%d, ", gnc_.reg_.cvs_handrail_pulse); // 2722 - fprintf(output_file_, "%d, %d, ", gnc_.hand_.cvs_timestamp_sec, gnc_.hand_.cvs_timestamp_nsec); // 2723, 2724 - // now the contents of handrail_landmarks. Stored as u0 u1 u2... v0 v1 v2... w0 w1 w2... - for (int i = 0; i < dl_max_features_ * 3; i++) // 2725 ~ 2874 (50 * 3) - fprintf(output_file_, "%g, ", gnc_.hand_.cvs_observations[i]); - - // finally the valid flags - for (int i = 0; i < dl_max_features_; i++) // 2875 ~ 2924 (50) - fprintf(output_file_, "%d, ", gnc_.hand_.cvs_valid_flag[i]); - - // now the handrail local pose - fprintf(output_file_, "%g, %g, %g, ", gnc_.hand_.cvs_handrail_local_pos[0], // 2925 ~ 2927 (3) - gnc_.hand_.cvs_handrail_local_pos[1], gnc_.hand_.cvs_handrail_local_pos[2]); - fprintf(output_file_, "%g, %g, %g, %g, ", gnc_.hand_.cvs_handrail_local_quat[0], // 2928 ~ 2931 (4) - gnc_.hand_.cvs_handrail_local_quat[1], gnc_.hand_.cvs_handrail_local_quat[2], - gnc_.hand_.cvs_handrail_local_quat[3]); - - // handrail end point detection flag - fprintf(output_file_, "%d, ", gnc_.hand_.cvs_3d_knowledge_flag); // 2932 - // handrail global pose update flag - fprintf(output_file_, "%d, ", gnc_.hand_.cvs_handrail_update_global_pose_flag); // 2933 - - // localization mode cmd - fprintf(output_file_, "%d", gnc_.cmc_.localization_mode_cmd); // 2934 - fprintf(output_file_, "\n"); -} - -void Ekf::PrepareStep(const sensor_msgs::Imu & imu, const geometry_msgs::Quaternion & quat) { - // set IMU values - // set timestamp - imu_.imu_timestamp_sec = imu.header.stamp.sec; - imu_.imu_timestamp_nsec = imu.header.stamp.nsec; - - // set angular vel, ros message is double cast into float - imu_.imu_omega_B_ECI_sensor[0] = static_cast(imu.angular_velocity.x); - imu_.imu_omega_B_ECI_sensor[1] = static_cast(imu.angular_velocity.y); - imu_.imu_omega_B_ECI_sensor[2] = static_cast(imu.angular_velocity.z); - - // set linear accel - imu_.imu_A_B_ECI_sensor[0] = static_cast(imu.linear_acceleration.x); - imu_.imu_A_B_ECI_sensor[1] = static_cast(imu.linear_acceleration.y); - imu_.imu_A_B_ECI_sensor[2] = static_cast(imu.linear_acceleration.z); - - // set validity - imu_.imu_validity_flag = 1; - - // set saturation - imu_.imu_sat_flag = 0; - - // set the ISS2BODY quaternion in preparation for step - this is effectively ignored - // if tun_ase_gravity_removal = true in gnc.config - gnc_.quat_[0] = quat.x; - gnc_.quat_[1] = quat.y; - gnc_.quat_[2] = quat.z; - gnc_.quat_[3] = quat.w; - - // then copy all other values in preparation for step - memcpy(&gnc_.vis_, &vis_, sizeof(cvs_landmark_msg)); - memcpy(&gnc_.reg_, ®_, sizeof(cvs_registration_pulse)); - memcpy(&gnc_.of_, &of_, sizeof(cvs_optical_flow_msg)); - memcpy(&gnc_.hand_, &hand_, sizeof(cvs_handrail_msg)); - memcpy(&gnc_.imu_, &imu_, sizeof(imu_msg)); - gnc_.cmc_.localization_mode_cmd = cmc_mode_; - - // prevent double registrations - reg_.cvs_landmark_pulse = false; - reg_.cvs_optical_flow_pulse = false; - // registration complete, now update next time - if (of_inputs_delayed_) { - of_.cvs_timestamp_sec = imu.header.stamp.sec; - of_.cvs_timestamp_nsec = imu.header.stamp.nsec; - of_inputs_delayed_ = false; - } - processing_of_reg_ = false; - - ApplyReset(); -} - -int Ekf::Step(ff_msgs::EkfState* state) { - if (output_file_) - WriteToFile(); - else - gnc_.Step(); - if (gnc_.kfl_.confidence == 2) - reset_ekf_ = true; - UpdateState(state); - return 1; -} - -void Ekf::UpdateState(ff_msgs::EkfState* state) { - // now copy everything to the output message - state->header.stamp.sec = imu_.imu_timestamp_sec; - state->header.stamp.nsec = imu_.imu_timestamp_nsec; - state->header.frame_id = "world"; - state->child_frame_id = "body"; - state->pose.position = msg_conversions::array_to_ros_point(gnc_.kfl_.P_B_ISS_ISS); - state->velocity = msg_conversions::array_to_ros_vector(gnc_.kfl_.V_B_ISS_ISS); - state->accel = msg_conversions::array_to_ros_vector(gnc_.kfl_.A_B_ISS_ISS); - state->pose.orientation = msg_conversions::array_to_ros_quat(gnc_.kfl_.quat_ISS2B); - state->omega = msg_conversions::array_to_ros_vector(gnc_.kfl_.omega_B_ISS_B); - state->accel_bias = msg_conversions::array_to_ros_vector(gnc_.kfl_.accel_bias); - state->gyro_bias = msg_conversions::array_to_ros_vector(gnc_.kfl_.gyro_bias); - state->confidence = gnc_.kfl_.confidence; - state->aug_state_enum = gnc_.kfl_.aug_state_enum; - state->status = gnc_.kfl_.kfl_status; - state->of_count = gnc_.kfl_.update_OF_tracks_cnt; - state->ml_count = gnc_.kfl_.update_ML_features_cnt; - std::copy(gnc_.kfl_.cov_diag, gnc_.kfl_.cov_diag + 15, state->cov_diag.c_array()); - state->hr_global_pose.position = msg_conversions::array_to_ros_point(gnc_.kfl_.hr_P_hr_ISS_ISS); - state->hr_global_pose.orientation = msg_conversions::array_to_ros_quat(gnc_.kfl_.hr_quat_ISS2hr); - if (state->ml_count > 0) - std::copy(gnc_.kfl_.ml_mahal_distance, gnc_.kfl_.ml_mahal_distance + ml_max_features_, - state->ml_mahal_dists.c_array()); - else - memset(state->ml_mahal_dists.c_array(), 0, ml_max_features_ * sizeof(float)); -} - -void Ekf::Reset(void) { - reset_ekf_ = true; -} - -void Ekf::ResetPose(const Eigen::Affine3d & camera_to_body, geometry_msgs::Pose const& pose) { - reset_camera_to_body_ = camera_to_body; - reset_pose_ = pose; - reset_ready_ = true; -} - -// reset ekf, during step function to prevent race conditions -void Ekf::ApplyReset(void) { - if (!reset_ready_) - return; - - // set the robot's position based on the pose - Eigen::Quaterniond world_q_body(reset_pose_.orientation.w, reset_pose_.orientation.x, - reset_pose_.orientation.y, reset_pose_.orientation.z); - Eigen::Quaterniond camera_to_body_rotation(reset_camera_to_body_.linear()); - Eigen::Vector3d world_r_body(reset_pose_.position.x, reset_pose_.position.y, reset_pose_.position.z); - world_q_body = world_q_body * camera_to_body_rotation.conjugate(); - Eigen::Quaterniond q1(0, reset_camera_to_body_.translation().x(), reset_camera_to_body_.translation().y(), - reset_camera_to_body_.translation().z()); - Eigen::Quaterniond temp = world_q_body * q1 * world_q_body.conjugate(); - world_r_body = world_r_body - Eigen::Vector3d(temp.x(), temp.y(), temp.z()); - Eigen::Vector3d world_r_imu = world_r_body + world_q_body * imu_to_body_.translation(); - auto& quat_ISS2B = gnc_.est_->defaultParam->tun_ase_state_ic_quat_ISS2B; - auto& P_B_ISS_ISS = gnc_.est_->defaultParam->tun_ase_state_ic_P_B_ISS_ISS; - auto& P_EST_ISS_ISS = gnc_.est_->defaultParam->tun_ase_state_ic_P_EST_ISS_ISS; - auto& V_B_ISS_ISS = gnc_.est_->defaultParam->tun_ase_state_ic_V_B_ISS_ISS; - quat_ISS2B[0] = world_q_body.x(); - quat_ISS2B[1] = world_q_body.y(); - quat_ISS2B[2] = world_q_body.z(); - quat_ISS2B[3] = world_q_body.w(); - P_B_ISS_ISS[0] = world_r_body[0]; - P_B_ISS_ISS[1] = world_r_body[1]; - P_B_ISS_ISS[2] = world_r_body[2]; - P_EST_ISS_ISS[0] = world_r_imu[0]; - P_EST_ISS_ISS[1] = world_r_imu[1]; - P_EST_ISS_ISS[2] = world_r_imu[2]; - V_B_ISS_ISS[0] = 0.0; - V_B_ISS_ISS[1] = 0.0; - V_B_ISS_ISS[2] = 0.0; - - ROS_INFO("Reset EKF."); - - // reset the EKF (especially for the covariance) - gnc_.Initialize(); - - // reset optical flow too - optical_flow_features_.clear(); - optical_flow_augs_feature_counts_.clear(); - optical_flow_augs_times_.clear(); - of_camera_id_ = 0; - processing_of_reg_ = false; - of_inputs_delayed_ = false; - - reset_ready_ = false; - reset_ekf_ = false; - - // If we have set the reset callback, call it now. - if (reset_callback_) - reset_callback_(); -} - -} // end namespace ekf diff --git a/gnc/ekf/src/ekf_nodelet.cc b/gnc/ekf/src/ekf_nodelet.cc deleted file mode 100644 index 61a103640d..0000000000 --- a/gnc/ekf/src/ekf_nodelet.cc +++ /dev/null @@ -1,62 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include - -namespace ekf { - -class EkfNodelet : public ff_util::FreeFlyerNodelet { - public: - EkfNodelet() : ff_util::FreeFlyerNodelet(NODE_EKF, true), killed_(false) {} - ~EkfNodelet() { - killed_ = true; - thread_->join(); - } - // This is called when the nodelet is loaded into the nodelet manager - void Initialize(ros::NodeHandle *nh) { - // Bootstrap our environment - ff_common::InitFreeFlyerApplication(getMyArgv()); - gnc_autocode::InitializeAutocode(this); - ekf_.reset( - new ekf::EkfWrapper(this->GetPlatformHandle(true), GetPlatform())); - thread_.reset(new std::thread( - &ekf::EkfWrapper::Run, ekf_.get(), std::ref(killed_))); - } - - private: - std::shared_ptr ekf_; - std::shared_ptr thread_; - std::atomic killed_; -}; - -} // end namespace ekf - -// Declare the plugin -PLUGINLIB_EXPORT_CLASS(ekf::EkfNodelet, nodelet::Nodelet); diff --git a/gnc/ekf/src/ekf_wrapper.cc b/gnc/ekf/src/ekf_wrapper.cc deleted file mode 100644 index dcec848bcd..0000000000 --- a/gnc/ekf/src/ekf_wrapper.cc +++ /dev/null @@ -1,513 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -namespace ekf { - -EkfWrapper::EkfWrapper(ros::NodeHandle* nh, std::string const& platform_name) : - ekf_initialized_(false), imus_dropped_(0), have_imu_(false), - input_mode_(ff_msgs::SetEkfInputRequest::MODE_NONE), nh_(nh), - estimating_bias_(false), disp_features_(false), killed_(false) { - platform_name_ = (platform_name.empty() ? "" : platform_name + "/"); - - config_.AddFile("gnc.config"); - config_.AddFile("cameras.config"); - config_.AddFile("geometry.config"); - ReadParams(); - config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { - config_.CheckFilesUpdated(std::bind(&EkfWrapper::ReadParams, this));}, false, true); - pt_ekf_.Initialize("ekf"); - - // Register to receive a callback when the EKF resets - ekf_.SetResetCallback(std::bind(&EkfWrapper::ResetCallback, this)); - // subscribe to IMU first, then rest once IMU is ready - // this is so localization manager doesn't timeout - imu_sub_ = nh_->subscribe(TOPIC_HARDWARE_IMU, 5, &EkfWrapper::ImuCallBack, this, - ros::TransportHints().tcpNoDelay()); -} - -EkfWrapper::~EkfWrapper() { - killed_ = true; -} - -// wait to start up until the IMU is ready -void EkfWrapper::InitializeEkf(void) { - state_pub_ = nh_->advertise(TOPIC_GNC_EKF, 1); - pose_pub_ = nh_->advertise(TOPIC_LOCALIZATION_POSE, 1); - twist_pub_ = nh_->advertise(TOPIC_LOCALIZATION_TWIST, 1); - feature_pub_ = nh_->advertise(TOPIC_GNC_EKF_FEATURES, 1, - boost::bind(&EkfWrapper::SubscriberCallback, this), - boost::bind(&EkfWrapper::SubscriberCallback, this)); - reset_pub_ = nh_->advertise(TOPIC_GNC_EKF_RESET, 1); - - vl_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_ML_FEATURES, 1, &EkfWrapper::VLVisualLandmarksCallBack, this, - ros::TransportHints().tcpNoDelay()); - vl_reg_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_ML_REGISTRATION, 1, &EkfWrapper::VLRegisterCamera, this, - ros::TransportHints().tcpNoDelay()); - ar_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_AR_FEATURES, 1, &EkfWrapper::ARVisualLandmarksCallBack, this, - ros::TransportHints().tcpNoDelay()); - ar_reg_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_AR_REGISTRATION, 1, &EkfWrapper::ARRegisterCamera, this, - ros::TransportHints().tcpNoDelay()); - of_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_OF_FEATURES, 1, &EkfWrapper::OpticalFlowCallBack, this, - ros::TransportHints().tcpNoDelay()); - of_reg_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_OF_REGISTRATION, 1, &EkfWrapper::RegisterOpticalFlowCamera, this, - ros::TransportHints().tcpNoDelay()); - dl_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_HR_FEATURES, 1, &EkfWrapper::DepthLandmarksCallBack, this, - ros::TransportHints().tcpNoDelay()); - dl_reg_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_HR_REGISTRATION, 1, &EkfWrapper::RegisterDepthCamera, this, - ros::TransportHints().tcpNoDelay()); - truth_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_TRUTH, 1, &EkfWrapper::GroundTruthCallback, this, - ros::TransportHints().tcpNoDelay()); - truth_twist_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_TRUTH_TWIST, 1, &EkfWrapper::GroundTruthTwistCallback, this, - ros::TransportHints().tcpNoDelay()); - flight_mode_sub_ = nh_->subscribe(TOPIC_MOBILITY_FLIGHT_MODE, 1, &EkfWrapper::FlightModeCallback, this); - - reset_srv_ = nh_->advertiseService(SERVICE_GNC_EKF_RESET, &EkfWrapper::ResetService, this); - bias_srv_ = nh_->advertiseService(SERVICE_GNC_EKF_INIT_BIAS, &EkfWrapper::InitializeBiasService, this); - input_mode_srv_ = nh_->advertiseService(SERVICE_GNC_EKF_SET_INPUT, &EkfWrapper::SetInputService, this); - - reset_hr_srv_ = nh_->advertiseService(SERVICE_GNC_EKF_RESET_HR, &EkfWrapper::ResetHandrailService, this); - - ekf_initialized_ = true; -} - -void EkfWrapper::ReadParams(void) { - if (!config_.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; - } - - if (!config_.GetInt("bias_required_observations", &bias_required_observations_)) - ROS_FATAL("Unspecified bias_required_observations."); - - if (!config_.GetStr("imu_bias_file", &bias_file_)) { - ROS_FATAL("IMU bias file not specified."); - } - - ekf_.ReadParams(&config_); -} - -void EkfWrapper::SubscriberCallback() { - disp_features_ = (feature_pub_.getNumSubscribers() > 0); -} - -void EkfWrapper::PublishFeatures(ff_msgs::VisualLandmarks::ConstPtr const& l) { - if (!disp_features_) return; - features_.header = l->header; - features_.height = 1; - features_.width = l->landmarks.size(); - features_.fields.resize(3); - features_.fields[0].name = "x"; - features_.fields[0].offset = 0; - features_.fields[0].datatype = 7; - features_.fields[0].count = 1; - features_.fields[1].name = "y"; - features_.fields[1].offset = 4; - features_.fields[1].datatype = 7; - features_.fields[1].count = 1; - features_.fields[2].name = "z"; - features_.fields[2].offset = 8; - features_.fields[2].datatype = 7; - features_.fields[2].count = 1; - features_.is_bigendian = false; - features_.point_step = 12; - features_.row_step = features_.point_step * features_.width; - features_.is_dense = true; - features_.data.resize(features_.row_step); - for (unsigned int i = 0; i < l->landmarks.size(); i++) { - memcpy(&features_.data[features_.point_step * i + 0], &l->landmarks[i].x, 4); - memcpy(&features_.data[features_.point_step * i + 4], &l->landmarks[i].y, 4); - memcpy(&features_.data[features_.point_step * i + 8], &l->landmarks[i].z, 4); - } - feature_pub_.publish(features_); -} - -void EkfWrapper::PublishFeatures(ff_msgs::DepthLandmarks::ConstPtr const& l) { - if (!disp_features_) return; - features_.header = std_msgs::Header(); - features_.header.stamp = ros::Time::now(); - features_.header.frame_id = platform_name_ + "perch_cam"; - features_.height = 1; - features_.width = l->landmarks.size(); - features_.fields.resize(3); - features_.fields[0].name = "x"; - features_.fields[0].offset = 0; - features_.fields[0].datatype = 7; - features_.fields[0].count = 1; - features_.fields[1].name = "y"; - features_.fields[1].offset = 4; - features_.fields[1].datatype = 7; - features_.fields[1].count = 1; - features_.fields[2].name = "z"; - features_.fields[2].offset = 8; - features_.fields[2].datatype = 7; - features_.fields[2].count = 1; - features_.is_bigendian = false; - features_.point_step = 12; - features_.row_step = features_.point_step * features_.width; - features_.is_dense = true; - features_.data.resize(features_.row_step); - // The features are expressed as image plane coordinated (u,v) and a depth. We - // must use the camera model to convert this to an (x,y,z) coordinate. - static camera::CameraParameters cam_params(&config_, "perch_cam"); - static camera::CameraModel camera_model(Eigen::Vector3d(0, 0, 0), - Eigen::Matrix3d::Identity(), cam_params); - float x, y, z; - for (unsigned int i = 0; i < l->landmarks.size(); i++) { - Eigen::Vector3d p_c = l->landmarks[i].w * camera_model.Ray( - static_cast(l->landmarks[i].u), static_cast(l->landmarks[i].v)); - x = static_cast(p_c[0]); - y = static_cast(p_c[1]); - z = static_cast(p_c[2]); - memcpy(&features_.data[features_.point_step * i + 0], &x, 4); - memcpy(&features_.data[features_.point_step * i + 4], &y, 4); - memcpy(&features_.data[features_.point_step * i + 8], &z, 4); - } - feature_pub_.publish(features_); -} - -void EkfWrapper::ResetCallback() { - static std_msgs::Empty msg; - reset_pub_.publish(msg); -} - -void EkfWrapper::ImuCallBack(sensor_msgs::Imu::ConstPtr const& imu) { - // concurrency protection - std::unique_lock lock(mutex_imu_msg_); - while (have_imu_ && !killed_) - cv_imu_.wait_for(lock, std::chrono::milliseconds(8)); - - // copy IMU data - imu_ = *imu; - - have_imu_ = true; - // now notify the condition variable waiting for IMU that we have it - lock.unlock(); - cv_imu_.notify_all(); - - if (estimating_bias_) - EstimateBias(imu); -} - -void EkfWrapper::EstimateBias(sensor_msgs::Imu::ConstPtr const& imu) { - bias_reset_count_++; - bias_reset_sums_[0] += imu->angular_velocity.x; - bias_reset_sums_[1] += imu->angular_velocity.y; - bias_reset_sums_[2] += imu->angular_velocity.z; - bias_reset_sums_[3] += imu->linear_acceleration.x; - bias_reset_sums_[4] += imu->linear_acceleration.y; - bias_reset_sums_[5] += imu->linear_acceleration.z; - if (bias_reset_count_ >= bias_required_observations_) { - for (int i = 0; i < 6; i++) - bias_reset_sums_[i] = bias_reset_sums_[i] / bias_reset_count_; - - ROS_INFO("Esimated biases: gyro: %g %g %g accel: %g %g %g", - bias_reset_sums_[0], bias_reset_sums_[1], bias_reset_sums_[2], - bias_reset_sums_[3], bias_reset_sums_[4], bias_reset_sums_[5]); - FILE* f = fopen(bias_file_.c_str(), "w"); - if (f) { - fprintf(f, "%g %g %g\n", bias_reset_sums_[0], bias_reset_sums_[1], bias_reset_sums_[2]); - fprintf(f, "%g %g %g\n", bias_reset_sums_[3], bias_reset_sums_[4], bias_reset_sums_[5]); - fclose(f); - } else { - ROS_ERROR("Bias file %s could not be opened.", bias_file_.c_str()); - } - ekf_.SetBias(Eigen::Vector3f(bias_reset_sums_[0], bias_reset_sums_[1], bias_reset_sums_[2]), - Eigen::Vector3f(bias_reset_sums_[3], bias_reset_sums_[4], bias_reset_sums_[5])); - - estimating_bias_ = false; - ekf_.Reset(); - } -} - -void EkfWrapper::OpticalFlowCallBack(ff_msgs::Feature2dArray::ConstPtr const& of) { - std::lock_guard lock(mutex_of_msg_); - ekf_.OpticalFlowUpdate(*of.get()); -} - -void EkfWrapper::VLVisualLandmarksCallBack(ff_msgs::VisualLandmarks::ConstPtr const& vl) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_MAP_LANDMARKS) { - std::lock_guard lock(mutex_vl_msg_); - ekf_.SparseMapUpdate(*vl.get()); - PublishFeatures(vl); - } -} - -void EkfWrapper::ARVisualLandmarksCallBack(ff_msgs::VisualLandmarks::ConstPtr const& vl) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_AR_TAGS) { - std::lock_guard lock(mutex_vl_msg_); - bool updated = ekf_.ARTagUpdate(*vl.get()); - if (updated) { - Eigen::Affine3d t = ekf_.GetDockToWorldTransform(); - geometry_msgs::TransformStamped transform; - transform.header = std_msgs::Header(); - transform.header.stamp = ros::Time::now(); - transform.header.frame_id = "world"; - transform.child_frame_id = "dock/body"; - transform.transform.translation = msg_conversions::eigen_to_ros_vector(t.translation()); - transform.transform.rotation = msg_conversions::eigen_to_ros_quat(Eigen::Quaterniond(t.linear())); - transform_pub_.sendTransform(transform); - } - PublishFeatures(vl); - } -} - -void EkfWrapper::DepthLandmarksCallBack(ff_msgs::DepthLandmarks::ConstPtr const& dl) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_HANDRAIL) { - std::lock_guard lock(mutex_dl_msg_); - bool updated = ekf_.HRTagUpdate(*dl.get()); - if (updated) { - Eigen::Affine3d t = ekf_.GetHandrailToWorldTransform(); - geometry_msgs::TransformStamped transform; - transform.header = std_msgs::Header(); - transform.header.stamp = ros::Time::now(); - transform.header.frame_id = "world"; - transform.child_frame_id = "handrail/body"; - transform.transform.translation = msg_conversions::eigen_to_ros_vector(t.translation()); - transform.transform.rotation = msg_conversions::eigen_to_ros_quat(Eigen::Quaterniond(t.linear())); - transform_pub_.sendTransform(transform); - } - PublishFeatures(dl); - } -} - -void EkfWrapper::RegisterOpticalFlowCamera(ff_msgs::CameraRegistration::ConstPtr const& cr) { - std::lock_guard lock(mutex_of_msg_); - ekf_.OpticalFlowRegister(*cr.get()); -} - -void EkfWrapper::VLRegisterCamera(ff_msgs::CameraRegistration::ConstPtr const& reg) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_MAP_LANDMARKS) { - std::lock_guard lock(mutex_vl_msg_); - ekf_.SparseMapRegister(*reg.get()); - } -} - -void EkfWrapper::ARRegisterCamera(ff_msgs::CameraRegistration::ConstPtr const& reg) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_AR_TAGS) { - std::lock_guard lock(mutex_vl_msg_); - ekf_.ARTagRegister(*reg.get()); - } -} - -void EkfWrapper::RegisterDepthCamera(ff_msgs::CameraRegistration::ConstPtr const& reg) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_HANDRAIL) { - std::lock_guard lock(mutex_vl_msg_); - ekf_.HandrailRegister(*reg.get()); - } -} - -void EkfWrapper::GroundTruthCallback(geometry_msgs::PoseStamped::ConstPtr const& pose) { - // For certain contexts (like MGTF) we want to extract the correct orientation, and pass it to - // GNC, so that Earth's gravity can be extracted out of the linear acceleration. - std::lock_guard lk(mutex_truth_msg_); - assert(pose->header.frame_id == "world"); - quat_ = pose->pose.orientation; - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_TRUTH) { - truth_pose_= *pose; - pose_pub_.publish(pose); - } -} - -void EkfWrapper::GroundTruthTwistCallback(geometry_msgs::TwistStamped::ConstPtr const& twist) { - std::lock_guard lk(mutex_truth_msg_); - assert(twist->header.frame_id == "world"); - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_TRUTH) { - truth_twist_= *twist; - twist_pub_.publish(twist); - } -} - -void EkfWrapper::FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode) { - ekf_.SetSpeedGain(mode->speed); -} - -void EkfWrapper::Run(std::atomic const& killed) { - // Kill the step thread - while (!killed) { - ros::spinOnce(); - Step(); - } - // Make sure the IMU thread also gets killed - killed_ = true; -} - -int EkfWrapper::Step() { - // don't modify anything while we're copying the data - { - // wait until we get an imu reading with the condition variable - std::unique_lock lk(mutex_imu_msg_); - if (!have_imu_) - cv_imu_.wait_for(lk, std::chrono::milliseconds(8)); - if (!have_imu_) { - imus_dropped_++; - // publish a failure if we stop getting imu messages - if (imus_dropped_ > 10 && ekf_initialized_) { - state_.header.stamp = ros::Time::now(); - state_.confidence = 2; // lost - state_pub_.publish(state_); - ekf_.Reset(); - } - return 0; // Changed by Andrew due to 250Hz ctl messages when sim blocks (!) - } - imus_dropped_ = 0; - if (!ekf_initialized_) - InitializeEkf(); - - std::lock(mutex_act_msg_, mutex_vl_msg_, mutex_dl_msg_, mutex_of_msg_, mutex_truth_msg_); - std::lock_guard lock_actMsg(mutex_act_msg_, std::adopt_lock); - std::lock_guard lock_vlMsg(mutex_vl_msg_, std::adopt_lock); - std::lock_guard lock_dlMsg(mutex_dl_msg_, std::adopt_lock); - std::lock_guard lock_ofMsg(mutex_of_msg_, std::adopt_lock); - std::lock_guard lock_truthMsg(mutex_truth_msg_, std::adopt_lock); - // copy everything in EKF, so data structures can be modified for next - // step while current step processes. We pass the ground truth quaternion - // representing the latest ISS2BODY rotation, which is used in certain - // testing contexts to remove the effect of Earth's gravity. - ekf_.PrepareStep(imu_, quat_); - // don't reuse imu reading - have_imu_ = false; - } - cv_imu_.notify_all(); - - int ret = 1; - switch (input_mode_) { - // Don't do anything when localization in turned off - case ff_msgs::SetEkfInputRequest::MODE_NONE: - state_.ml_count = 0; // these could be set from before - state_.of_count = 0; - break; - // In truth mode we don't step the filter forward, but we do copy the pose - // and twist into the EKF message. - case ff_msgs::SetEkfInputRequest::MODE_TRUTH: { - std::lock_guard lk(mutex_truth_msg_); - ros::Time t = ros::Time::now(); - if (fabs((truth_pose_.header.stamp - t).toSec()) < 1 && - fabs((truth_twist_.header.stamp - t).toSec()) < 1) { - state_.header.stamp = t; - state_.pose = truth_pose_.pose; - state_.velocity = truth_twist_.twist.linear; - state_.omega = truth_twist_.twist.angular; - break; - } - ret = 0; - break; - } - // All other pipelines get stepped forward normally - default: - pt_ekf_.Tick(); - ret = ekf_.Step(&state_); - pt_ekf_.Tock(); - break; - } - // Let other elements in the system know that the bias is being estimated - state_.estimating_bias = estimating_bias_; - // Only send the state if the Step() function was successful - if (ret) - PublishState(state_); - return ret; -} - -void EkfWrapper::PublishState(const ff_msgs::EkfState & state) { - // Publish the full EKF state - state_pub_.publish(state); - // Only publish a transform if the confidence is good enough and we have - // actually populated the state (examine the header to check) - if (state.confidence == 0 && !state.header.frame_id.empty()) { - geometry_msgs::TransformStamped transform; - transform.header = state.header; - transform.child_frame_id = platform_name_ + "body"; - transform.transform.translation.x = state.pose.position.x; - transform.transform.translation.y = state.pose.position.y; - transform.transform.translation.z = state.pose.position.z; - transform.transform.rotation = state.pose.orientation; - transform_pub_.sendTransform(transform); - } - // Publish a truthful - if (input_mode_ != ff_msgs::SetEkfInputRequest::MODE_TRUTH) { - geometry_msgs::PoseStamped pose; - pose.header.stamp = state.header.stamp; - pose.pose = state.pose; - pose_pub_.publish(pose); - geometry_msgs::TwistStamped twist; - twist.header.stamp = state.header.stamp; - twist.twist.linear = state.velocity; - twist.twist.angular = state.omega; - twist_pub_.publish(twist); - } -} - -bool EkfWrapper::ResetService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { // NOLINT - ekf_.Reset(); - return true; -} - -// Service to reset the handrail position which is only calculated once. Unused, but -// should be used in the future. -bool EkfWrapper::ResetHandrailService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { - ekf_.ResetHR(); - return true; -} - -bool EkfWrapper::InitializeBiasService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { // NOLINT - bias_reset_count_ = 0; - for (int i = 0; i < 6; i++) - bias_reset_sums_[i] = 0.0; - estimating_bias_ = true; - ROS_INFO("Beginning bias estimation."); - return true; -} - -bool EkfWrapper::SetInputService(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res) { //NOLINT - input_mode_ = req.mode; - switch (input_mode_) { - case ff_msgs::SetEkfInputRequest::MODE_AR_TAGS: - ekf_.ResetAR(); - ROS_INFO("EKF input switched to AR tags."); - break; - case ff_msgs::SetEkfInputRequest::MODE_HANDRAIL: - ekf_.ResetHR(); - ROS_INFO("EKF input switched to handrail."); - break; - case ff_msgs::SetEkfInputRequest::MODE_MAP_LANDMARKS: - ROS_INFO("EKF input switched to mapped landmarks."); - break; - case ff_msgs::SetEkfInputRequest::MODE_NONE: - ROS_INFO("EKF input switched to none."); - break; - case ff_msgs::SetEkfInputRequest::MODE_TRUTH: - ROS_INFO("EKF input switched to ground truth."); - break; - default: - break; - } - return true; -} - -} // end namespace ekf diff --git a/gnc/fam/CMakeLists.txt b/gnc/fam/CMakeLists.txt index b51661889c..ee29170e23 100644 --- a/gnc/fam/CMakeLists.txt +++ b/gnc/fam/CMakeLists.txt @@ -15,17 +15,77 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(fam) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + ff_msgs + ff_hw_msgs + ff_util + config_reader + pmc +) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + catkin_package( - LIBRARIES fam - CATKIN_DEPENDS roscpp sensor_msgs nav_msgs - DEPENDS ff_msgs ff_hw_msgs ff_common + LIBRARIES + fam + CATKIN_DEPENDS + roscpp + nodelet + ff_msgs + ff_hw_msgs + ff_util + config_reader + pmc ) -create_library(TARGET fam - LIBS ${catkin_LIBRARIES} ${EIGEN_LIBRARIES} gnc_autocode msg_conversions ff_common config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - DEPS ff_msgs ff_hw_msgs) +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +# Declare C++ libraries +add_library(fam + src/fam.cc + src/fam_nodelet.cc +) +add_dependencies(fam ${catkin_EXPORTED_TARGETS}) +target_link_libraries(fam ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + -install_launch_files() diff --git a/gnc/fam/include/fam/fam.h b/gnc/fam/include/fam/fam.h index f8681ceb6d..e67cfc4b1b 100644 --- a/gnc/fam/include/fam/fam.h +++ b/gnc/fam/include/fam/fam.h @@ -19,8 +19,6 @@ #ifndef FAM_FAM_H_ #define FAM_FAM_H_ -#include - #include #include #include @@ -28,16 +26,19 @@ #include #include -#include +#include #include +#include #include #include +#include #include namespace fam { + /** * @brief Force Allocation Module implementation using GNC module */ @@ -45,7 +46,7 @@ class Fam { public: explicit Fam(ros::NodeHandle* nh); ~Fam(); - void Step(ex_time_msg* ex_time, cmd_msg* cmd, ctl_msg* ctl); + void Step(); protected: void ReadParams(void); @@ -53,21 +54,22 @@ class Fam { void FlightModeCallback(const ff_msgs::FlightMode::ConstPtr& mode); void InertiaCallback(const geometry_msgs::InertiaStamped::ConstPtr& inertia); - gnc_autocode::GncFamAutocode gnc_; + pmc::Fam fam_; ros::Subscriber ctl_sub_; ros::Subscriber flight_mode_sub_, inertia_sub_; ros::Publisher pmc_pub_; - config_reader::ConfigReader config_; + // config_reader::ConfigReader config_; ff_util::PerfTimer pt_fam_; - ros::Timer config_timer_; + // ros::Timer config_timer_; std::mutex mutex_speed_; - uint8_t speed_; std::mutex mutex_mass_; - geometry_msgs::Vector3 center_of_mass_; + Eigen::Vector3f center_of_mass_; bool inertia_received_; + + pmc::FamInput input_; }; } // end namespace fam diff --git a/gnc/fam/package.xml b/gnc/fam/package.xml index c556cf684f..560fd2f97d 100644 --- a/gnc/fam/package.xml +++ b/gnc/fam/package.xml @@ -16,18 +16,20 @@ catkin roscpp - sensor_msgs - vpp_msgs - nav_msgs - ff_msgs nodelet + ff_msgs + ff_hw_msgs + ff_util + config_reader + pmc roscpp - rt_analyze - sensor_msgs - vpp_msgs - nav_msgs - ff_msgs nodelet + ff_msgs + ff_util + ff_msgs + ff_hw_msgs + config_reader + pmc diff --git a/gnc/fam/src/fam.cc b/gnc/fam/src/fam.cc index de9b651579..7ecad16491 100644 --- a/gnc/fam/src/fam.cc +++ b/gnc/fam/src/fam.cc @@ -18,20 +18,23 @@ #include #include -#include +#include #include +#include + // parameters fam_force_allocation_module_P are set in // matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_data.c namespace fam { Fam::Fam(ros::NodeHandle* nh) : inertia_received_(false) { - config_.AddFile("gnc.config"); - config_.AddFile("geometry.config"); - ReadParams(); - config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { - config_.CheckFilesUpdated(std::bind(&Fam::ReadParams, this));}, false, true); + // config_.AddFile("gnc.config"); + // config_.AddFile("geometry.config"); + // ReadParams(); + // config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { + // config_.CheckFilesUpdated(std::bind(&Fam::ReadParams, this));}, false, true); + pt_fam_.Initialize("fam"); pmc_pub_ = nh->advertise(TOPIC_HARDWARE_PMC_COMMAND, 1); @@ -49,57 +52,39 @@ Fam::Fam(ros::NodeHandle* nh) : inertia_received_(false) { Fam::~Fam() {} void Fam::CtlCallBack(const ff_msgs::FamCommand & c) { - ex_time_msg time; - cmd_msg cmd; - ctl_msg ctl; - - time.timestamp_sec = c.header.stamp.sec; - time.timestamp_nsec = c.header.stamp.nsec; - msg_conversions::ros_to_array_vector(c.wrench.force, ctl.body_force_cmd); - msg_conversions::ros_to_array_vector(c.wrench.torque, ctl.body_torque_cmd); - msg_conversions::ros_to_array_vector(c.accel, ctl.body_accel_cmd); - msg_conversions::ros_to_array_vector(c.alpha, ctl.body_alpha_cmd); - msg_conversions::ros_to_array_vector(c.position_error, ctl.pos_err); - msg_conversions::ros_to_array_vector(c.position_error_integrated, ctl.pos_err_int); - msg_conversions::ros_to_array_vector(c.attitude_error, ctl.att_err); - msg_conversions::ros_to_array_vector(c.attitude_error_integrated, ctl.att_err_int); - ctl.att_err_mag = c.attitude_error_mag; - ctl.ctl_status = c.status; - cmd.cmd_mode = c.control_mode; - - Step(&time, &cmd, &ctl); + input_.body_force_cmd = msg_conversions::ros_to_eigen_vector(c.wrench.force).cast(); + input_.body_torque_cmd = msg_conversions::ros_to_eigen_vector(c.wrench.torque).cast(); + + Step(); } void Fam::FlightModeCallback(const ff_msgs::FlightMode::ConstPtr& mode) { std::lock_guard lock(mutex_speed_); - speed_ = mode->speed; + input_.speed_gain_cmd = mode->speed; } void Fam::InertiaCallback(const geometry_msgs::InertiaStamped::ConstPtr& inertia) { std::lock_guard lock(mutex_mass_); - center_of_mass_ = inertia->inertia.com; + center_of_mass_ = msg_conversions::ros_to_eigen_vector(inertia->inertia.com).cast(); + fam_.UpdateCOM(center_of_mass_); inertia_received_ = true; } -void Fam::Step(ex_time_msg* ex_time, cmd_msg* cmd, ctl_msg* ctl) { - { - std::lock_guard lock(mutex_speed_); - // Overwrite the speed command with the cached value, provided - // through the flight mode message offered by the choreographer - cmd->speed_gain_cmd = speed_; - } - { - std::lock_guard lock(mutex_mass_); - if (!inertia_received_) { - ROS_DEBUG_STREAM_THROTTLE(10, "FAM step waiting for inertia."); - return; - } - msg_conversions::ros_to_array_vector(center_of_mass_, gnc_.cmc_.center_of_mass); +void Fam::Step() { + if (!inertia_received_) { + ROS_DEBUG_STREAM_THROTTLE(10, "FAM step waiting for inertia."); + return; } // Step the FAM simulink code pt_fam_.Tick(); - gnc_.Step(ex_time, cmd, ctl); + uint8_t speed_cmd[2]; + Eigen::Matrix servo_pwm_cmd; + { + std::lock_guard lock_mass(mutex_mass_); + std::lock_guard lock(mutex_speed_); + fam_.Step(input_, speed_cmd, servo_pwm_cmd); + } pt_fam_.Tock(); // Send the PMC command @@ -107,23 +92,22 @@ void Fam::Step(ex_time_msg* ex_time, cmd_msg* cmd, ctl_msg* ctl) { pmc.header.stamp = ros::Time::now(); pmc.header.frame_id = "body"; pmc.goals.resize(2); - pmc.goals[0].motor_speed = gnc_.act_.act_impeller_speed_cmd[0]; - pmc.goals[1].motor_speed = gnc_.act_.act_impeller_speed_cmd[1]; - std::copy(gnc_.act_.act_servo_pwm_cmd, gnc_.act_.act_servo_pwm_cmd + 6, - pmc.goals[0].nozzle_positions.c_array()); - std::copy(gnc_.act_.act_servo_pwm_cmd + 6, gnc_.act_.act_servo_pwm_cmd + 12, - pmc.goals[1].nozzle_positions.c_array()); + pmc.goals[0].motor_speed = speed_cmd[0]; + pmc.goals[1].motor_speed = speed_cmd[1]; + for (int i = 0; i < 6; i++) { + pmc.goals[0].nozzle_positions[i] = (unsigned char)servo_pwm_cmd[i]; + pmc.goals[1].nozzle_positions[i] = (unsigned char)servo_pwm_cmd[6 + i]; + } pmc_pub_.publish(pmc); pt_fam_.Send(); } -void Fam::ReadParams(void) { - if (!config_.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; - } - gnc_.ReadParams(&config_); -} +// void Fam::ReadParams(void) { +// if (!config_.ReadFiles()) { +// ROS_ERROR("Failed to read config files."); +// return; +// } +// } } // end namespace fam diff --git a/gnc/fam/src/fam_nodelet.cc b/gnc/fam/src/fam_nodelet.cc index c1030e67cd..7f3a42856a 100644 --- a/gnc/fam/src/fam_nodelet.cc +++ b/gnc/fam/src/fam_nodelet.cc @@ -24,8 +24,6 @@ #include #include -#include - #include namespace fam { @@ -38,7 +36,6 @@ class FamNodelet : public ff_util::FreeFlyerNodelet { void Initialize(ros::NodeHandle *nh) { // Bootstrap our environment ff_common::InitFreeFlyerApplication(getMyArgv(), false); - gnc_autocode::InitializeAutocode(this); fam_.reset(new fam::Fam(this->GetPlatformHandle(true))); } diff --git a/gnc/gnc_autocode/CMakeLists.txt b/gnc/gnc_autocode/CMakeLists.txt deleted file mode 100644 index a43881d83a..0000000000 --- a/gnc/gnc_autocode/CMakeLists.txt +++ /dev/null @@ -1,99 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -project(gnc_autocode) - -# find gnc directories -find_path ( GNC_ROOT_DIR ctl_controller0_ert_rtw/ctl_controller0.h - PATHS ${CMAKE_SOURCE_DIR}/gnc/matlab/code_generation - HINTS ${CMAKE_SOURCE_DIR}/gnc/matlab/code_generation - NO_CMAKE_FIND_ROOT_PATH -) - -if (GNC_ROOT_DIR) - # blindly setting paths, TODO: confirm these paths exist - set (GNC_ADDITIONAL_DIR ${GNC_ROOT_DIR}/AdditionalIncludes) - set (GNC_CTL_DIR ${GNC_ROOT_DIR}/ctl_controller0_ert_rtw) - set (GNC_EST_DIR ${GNC_ROOT_DIR}/est_estimator_ert_rtw) - set (GNC_FAM_DIR ${GNC_ROOT_DIR}/fam_force_allocation_module_ert_rtw) - set (GNC_SIM_DIR ${GNC_ROOT_DIR}/sim_model_lib0_ert_rtw) - set (GNC_BL1_DIR ${GNC_ROOT_DIR}/bpm_blower_1_propulsion_module_ert_rtw) - set (GNC_BL2_DIR ${GNC_ROOT_DIR}/bpm_blower_2_propulsion_module_ert_rtw) - set (GNC_UTIL_DIR ${GNC_ROOT_DIR}/sharedutils) - set (GNC_SPD_DIR ${GNC_ROOT_DIR}/esc_electronic_speed_controller0_ert_rtw) - set (GNC_CXX_DIR ${GNC_ROOT_DIR}/../cxx_functions) - set (GNC_INCLUDE_DIRS ${GNC_ADDITIONAL_DIR} ${GNC_CTL_DIR} ${GNC_EST_DIR} ${GNC_FAM_DIR}) -else (GNC_ROOT_DIR) - message(ERROR " GNC_ROOT_DIR not found! check PATHS in setGncPaths.cmake") -endif (GNC_ROOT_DIR) - -file(GLOB GNC_UTIL_SOURCES ${GNC_UTIL_DIR}/*.cpp) - -set(GNC_SOURCES - ${GNC_CTL_DIR}/ctl_tunable_funcs.cpp - ${GNC_CTL_DIR}/ctl_controller0.cpp - ${GNC_CTL_DIR}/ctl_controller0_data.cpp - ${GNC_EST_DIR}/est_estimator.cpp - ${GNC_EST_DIR}/est_estimator_data.cpp - ${GNC_EST_DIR}/est_tunable_funcs.cpp - ${GNC_FAM_DIR}/fam_force_allocation_module.cpp - ${GNC_FAM_DIR}/fam_force_allocation_module_data.cpp - ${GNC_FAM_DIR}/fam_tunable_funcs.cpp - ${GNC_SIM_DIR}/sim_model_lib0.cpp - ${GNC_SIM_DIR}/sim_model_lib0_data.cpp - ${GNC_SIM_DIR}/sim_tunable_funcs.cpp - ${GNC_SIM_DIR}/blower_aerodynamics.cpp - ${GNC_BL1_DIR}/blower_aerodynamics.cpp - ${GNC_BL1_DIR}/bpm_blower_1_propulsion_module.cpp - ${GNC_BL1_DIR}/bpm_blower_1_propulsion_module_data.cpp - ${GNC_BL2_DIR}/bpm_blower_2_propulsion_module.cpp - ${GNC_BL2_DIR}/bpm_blower_2_propulsion_module_data.cpp - ${GNC_CXX_DIR}/src/of_residual_and_h.cpp - ${GNC_CXX_DIR}/src/compute_delta_state_and_cov.cpp - ${GNC_CXX_DIR}/src/matrix_multiply.cpp - ${GNC_CXX_DIR}/src/apply_delta_state.cpp - ${GNC_CXX_DIR}/src/ros_log.cpp - ${GNC_CXX_DIR}/src/fault_assert.cpp - ${GNC_CXX_DIR}/src/fault_clear.cpp - ${GNC_UTIL_SOURCES} -) - -set(GNC_INCLUDES - ${GNC_CTL_DIR} - ${GNC_EST_DIR} - ${GNC_FAM_DIR} - ${GNC_SIM_DIR} - ${GNC_BL1_DIR} - ${GNC_BL2_DIR} - ${GNC_SPD_DIR} - ${GNC_UTIL_DIR} - ${GNC_CXX_DIR}/include -) - -STRING(REGEX REPLACE "-Wall" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -STRING(REGEX REPLACE "-Wall" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") -STRING(REGEX REPLACE "-Werror[^ ]*" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -STRING(REGEX REPLACE "-Werror[^ ]*" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -w -DPORTABLE_WORDSIZES -ffast-math -funsafe-math-optimizations") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w -DPORTABLE_WORDSIZES -ffast-math -funsafe-math-optimizations") - -create_library(TARGET gnc_autocode - LIBS ${EIGEN_LIBRARIES} ff_nodelet config_reader msg_conversions - INC ${GNC_INCLUDES} ${EIGEN_INCLUDE_DIRS} - ADD_SRCS ${GNC_SOURCES} -) - diff --git a/gnc/gnc_autocode/include/gnc_autocode/autocode.h b/gnc/gnc_autocode/include/gnc_autocode/autocode.h deleted file mode 100644 index af573cffab..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/autocode.h +++ /dev/null @@ -1,29 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_AUTOCODE_H_ -#define GNC_AUTOCODE_AUTOCODE_H_ - -#include - -// this needs to be called so that we can assert faults from the autocode -namespace gnc_autocode { - void InitializeAutocode(ff_util::FreeFlyerNodelet* nh); -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_AUTOCODE_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/blowers.h b/gnc/gnc_autocode/include/gnc_autocode/blowers.h deleted file mode 100644 index 2763f64505..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/blowers.h +++ /dev/null @@ -1,63 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_BLOWERS_H_ -#define GNC_AUTOCODE_BLOWERS_H_ - -extern "C" { -#include -#include -} - -namespace gnc_autocode { - -struct GncBlowerState{ - float battery_voltage; - float omega_B_ECI_B[3]; - unsigned char impeller_cmd; - float servo_cmd[6]; - float center_of_mass[3]; - float impeller_current; - float servo_current[6]; - float torque_B[3]; - float force_B[3]; - float motor_speed; - float nozzle_theta[6]; - float meas_motor_speed; -}; - -class GncBlowersAutocode { - public: - GncBlowersAutocode(); - ~GncBlowersAutocode(); - virtual void Initialize(); - virtual void Step(); - virtual void SetAngularVelocity(float x, float y, float z); - virtual void SetBatteryVoltage(float voltage); - - // This is just a thin wrapper with a step function - RT_MODEL_bpm_blower_1_propuls_T *blower1_; - RT_MODEL_bpm_blower_2_propuls_T *blower2_; - - // States of our two blowers - GncBlowerState states_[2]; -}; - -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_BLOWERS_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/ctl.h b/gnc/gnc_autocode/include/gnc_autocode/ctl.h deleted file mode 100644 index 39f0e58c8d..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/ctl.h +++ /dev/null @@ -1,54 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_CTL_H_ -#define GNC_AUTOCODE_CTL_H_ - -extern "C" { -#include -} - -namespace config_reader { - class ConfigReader; -} - -namespace gnc_autocode { - -class GncCtlAutocode { - public: - GncCtlAutocode(void); - ~GncCtlAutocode(void); - - virtual void Initialize(void); - virtual void Step(void); - virtual void ReadParams(config_reader::ConfigReader* config); - - RT_MODEL_ctl_controller0_T* controller_; - - // see GN&C_ICD.xlsx in freeflyer_docs for documentation - // inputs - ctl_input_msg ctl_input_; - - // outputs - cmd_msg cmd_; - - ctl_msg ctl_; -}; -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_CTL_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/ekf.h b/gnc/gnc_autocode/include/gnc_autocode/ekf.h deleted file mode 100644 index db70858649..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/ekf.h +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_EKF_H_ -#define GNC_AUTOCODE_EKF_H_ - -extern "C" { -#include -} - -namespace config_reader { - class ConfigReader; -} - -namespace gnc_autocode { - -class GncEkfAutocode { - public: - GncEkfAutocode(void); - ~GncEkfAutocode(void); - - virtual void Initialize(); - virtual void Step(); - virtual void ReadParams(config_reader::ConfigReader* config); - - RT_MODEL_est_estimator_T* est_; - cvs_landmark_msg vis_; - cvs_registration_pulse reg_; - cvs_optical_flow_msg of_; - cvs_handrail_msg hand_; - imu_msg imu_; - real32_T quat_[4]; - // TODO(bcoltin): this needs to be removed by GNC - cmc_msg cmc_; - kfl_msg kfl_; - float* P_; -}; -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_EKF_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/ekf_csv.h b/gnc/gnc_autocode/include/gnc_autocode/ekf_csv.h deleted file mode 100644 index 84a3beb0bb..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/ekf_csv.h +++ /dev/null @@ -1,44 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_EKF_CSV_H_ -#define GNC_AUTOCODE_EKF_CSV_H_ - -#include - -#include -#include - -namespace gnc_autocode { - -class GncEkfCSV : public GncEkfAutocode { - public: - GncEkfCSV(void); - ~GncEkfCSV(void); - - virtual void Initialize(std::string directory); - virtual void Step(); - - FILE* kfl_file_; - protected: - int ReadStepState(void); - void SkipFirstLine(FILE* f); -}; -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_EKF_CSV_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/fam.h b/gnc/gnc_autocode/include/gnc_autocode/fam.h deleted file mode 100644 index e57c442d09..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/fam.h +++ /dev/null @@ -1,51 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_FAM_H_ -#define GNC_AUTOCODE_FAM_H_ - -extern "C" { -#include -} - -namespace config_reader { - class ConfigReader; -} - -namespace gnc_autocode { - -class GncFamAutocode { - public: - GncFamAutocode(void); - ~GncFamAutocode(void); - - virtual void Initialize(); - virtual void Step(ex_time_msg* ex_time, cmd_msg* cmd, ctl_msg* ctl); - virtual void ReadParams(config_reader::ConfigReader* config); - - RT_MODEL_fam_force_allocation_T* fam_; - - ex_time_msg time_; - cmd_msg cmd_; - ctl_msg ctl_; - cmc_msg cmc_; - act_msg act_; -}; -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_FAM_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/sim.h b/gnc/gnc_autocode/include/gnc_autocode/sim.h deleted file mode 100644 index 374f73c3b4..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/sim.h +++ /dev/null @@ -1,62 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_SIM_H_ -#define GNC_AUTOCODE_SIM_H_ - -extern "C" { -#include -} - -namespace config_reader { - class ConfigReader; -} - -namespace gnc_autocode { -class GncSimAutocode { - public: - GncSimAutocode(); - ~GncSimAutocode(); - virtual void Initialize(); - virtual void Step(); - virtual void ReadParams(config_reader::ConfigReader* config); - - // This is just a thin wrapper with a step function - RT_MODEL_sim_model_lib0_T* sim_; - - // state variable - env_msg env_msg_; - - // input variables - act_msg act_msg_; - cmc_msg cmc_in_msg_; - - // output variables - ex_time_msg ex_time_msg_; - cvs_registration_pulse reg_pulse_; - cvs_landmark_msg landmark_msg_; - cvs_landmark_msg ar_tag_msg_; - cvs_optical_flow_msg optical_msg_; - cvs_handrail_msg hand_msg_; - cmc_msg cmc_out_msg_; - imu_msg imu_msg_; - bpm_msg bpm_msg_; -}; -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_SIM_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/sim_csv.h b/gnc/gnc_autocode/include/gnc_autocode/sim_csv.h deleted file mode 100644 index 11ee273288..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/sim_csv.h +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_SIM_CSV_H_ -#define GNC_AUTOCODE_SIM_CSV_H_ - -#include - -#include -#include - -typedef struct { - FILE* act; - FILE* reg; - FILE* landmark; - FILE* optical; - FILE* cmd; - FILE* imu; - FILE* env; -} GncFiles; - -namespace gnc_autocode { -class GncSimCSV : public GncSimAutocode { - public: - GncSimCSV(); - ~GncSimCSV(); - virtual void Initialize(std::string directory); - virtual void Step(); - protected: - void SkipFirstLine(FILE* f); - void LoadGncFiles(std::string directory); - int ReadStepState(void); - - GncFiles f_; - int seconds_, nsec_; - float last_landmark_sec_, last_landmark_nsec_; - float last_of_sec_, last_of_nsec_; -}; -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_SIM_CSV_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/speed_controller.h b/gnc/gnc_autocode/include/gnc_autocode/speed_controller.h deleted file mode 100644 index b27fb456d9..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/speed_controller.h +++ /dev/null @@ -1,26 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_SPEED_CONTROLLER_H_ -#define GNC_AUTOCODE_SPEED_CONTROLLER_H_ - -extern "C" { -#include -} - -#endif // GNC_AUTOCODE_SPEED_CONTROLLER_H_ diff --git a/gnc/gnc_autocode/readme.md b/gnc/gnc_autocode/readme.md deleted file mode 100644 index 16588cf8af..0000000000 --- a/gnc/gnc_autocode/readme.md +++ /dev/null @@ -1,14 +0,0 @@ -\page gncautocode GNC Autocode Wrapper - -A thin wrapper around the C code automatically generated from Matlab and Simulink. - -Each subsystem of GNC has a class wrapping the functionality, which manages -the underlying Simulink structs. Each class calls the proper allocation and -deallocation functions in the constructor and destructor. Then two functions -capture the rest of the functionality: - -* `Initialize`: This initializes the underlying objects. If called more than once, -the objects are reset to their initial state. -* `Step`: This runs the subsystem one step forward in time. It may takes -inputs from the preceding subsystem. - diff --git a/gnc/gnc_autocode/src/autocode.cc b/gnc/gnc_autocode/src/autocode.cc deleted file mode 100644 index 5d46787418..0000000000 --- a/gnc/gnc_autocode/src/autocode.cc +++ /dev/null @@ -1,28 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -namespace gnc_autocode { - -void InitializeAutocode(ff_util::FreeFlyerNodelet* nh) { - set_global_nodelet(nh); -} - -} // namespace gnc_autocode - diff --git a/gnc/gnc_autocode/src/blowers.cc b/gnc/gnc_autocode/src/blowers.cc deleted file mode 100644 index 709d9a7f49..0000000000 --- a/gnc/gnc_autocode/src/blowers.cc +++ /dev/null @@ -1,86 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include - -namespace gnc_autocode { - -GncBlowersAutocode::GncBlowersAutocode(void) { - // allocate blower 1 and check for memory allocation error - blower1_ = bpm_blower_1_propulsion_module(&states_[0].battery_voltage, - states_[0].omega_B_ECI_B, &states_[0].impeller_cmd, states_[0].servo_cmd, states_[0].center_of_mass, - &states_[0].impeller_current, states_[0].servo_current, states_[0].torque_B, - states_[0].force_B, &states_[0].motor_speed, states_[0].nozzle_theta, &states_[0].meas_motor_speed); - assert(blower1_); - assert(rtmGetErrorStatus(blower1_) == NULL); - // allocate blower 2 and check for memory allocation error - blower2_ = bpm_blower_2_propulsion_module(&states_[1].battery_voltage, - states_[1].omega_B_ECI_B, &states_[1].impeller_cmd, states_[1].servo_cmd, states_[1].center_of_mass, - &states_[1].impeller_current, states_[1].servo_current, states_[1].torque_B, - states_[1].force_B, &states_[1].motor_speed, states_[1].nozzle_theta, &states_[1].meas_motor_speed); - assert(blower2_); - assert(rtmGetErrorStatus(blower2_) == NULL); - // Initialize the blowers - Initialize(); -} - -void GncBlowersAutocode::Initialize(void) { - bpm_blower_1_propulsion_module_initialize(blower1_, &states_[0].battery_voltage, - states_[0].omega_B_ECI_B, &states_[0].impeller_cmd, states_[0].servo_cmd, states_[0].center_of_mass, - &states_[0].impeller_current, states_[0].servo_current, states_[0].torque_B, - states_[0].force_B, &states_[0].motor_speed, states_[0].nozzle_theta, &states_[0].meas_motor_speed); - bpm_blower_2_propulsion_module_initialize(blower2_, &states_[1].battery_voltage, - states_[1].omega_B_ECI_B, &states_[1].impeller_cmd, states_[1].servo_cmd, states_[1].center_of_mass, - &states_[1].impeller_current, states_[1].servo_current, states_[1].torque_B, - states_[1].force_B, &states_[1].motor_speed, states_[1].nozzle_theta, &states_[1].meas_motor_speed); -} - -GncBlowersAutocode::~GncBlowersAutocode() { - bpm_blower_1_propulsion_module_terminate(blower1_); - bpm_blower_2_propulsion_module_terminate(blower2_); -} - -void GncBlowersAutocode::Step(void) { - bpm_blower_1_propulsion_module_step(blower1_, states_[0].battery_voltage, - states_[0].omega_B_ECI_B, states_[0].impeller_cmd, states_[0].servo_cmd, states_[0].center_of_mass, - &states_[0].impeller_current, states_[0].servo_current, states_[0].torque_B, - states_[0].force_B, &states_[0].motor_speed, states_[0].nozzle_theta, &states_[0].meas_motor_speed); - bpm_blower_2_propulsion_module_step(blower2_, states_[1].battery_voltage, - states_[1].omega_B_ECI_B, states_[1].impeller_cmd, states_[1].servo_cmd, states_[1].center_of_mass, - &states_[1].impeller_current, states_[1].servo_current, states_[1].torque_B, - states_[1].force_B, &states_[1].motor_speed, states_[1].nozzle_theta, &states_[1].meas_motor_speed); -} - -void GncBlowersAutocode::SetAngularVelocity(float x, float y, float z) { - states_[0].omega_B_ECI_B[0] = x; - states_[0].omega_B_ECI_B[1] = y; - states_[0].omega_B_ECI_B[2] = z; - states_[1].omega_B_ECI_B[0] = x; - states_[1].omega_B_ECI_B[1] = y; - states_[1].omega_B_ECI_B[2] = z; -} - -void GncBlowersAutocode::SetBatteryVoltage(float voltage) { - states_[0].battery_voltage = voltage; - states_[1].battery_voltage = voltage; -} - -} // namespace gnc_autocode - diff --git a/gnc/gnc_autocode/src/constants.cc b/gnc/gnc_autocode/src/constants.cc deleted file mode 100644 index 9f9a6fa0f5..0000000000 --- a/gnc/gnc_autocode/src/constants.cc +++ /dev/null @@ -1,22 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// Missing symbol from the autocode. We only provide the definition here so .. -// everyone needs to link against us. -const char* RT_MEMORY_ALLOCATION_ERROR = "memory allocation error"; - diff --git a/gnc/gnc_autocode/src/ctl.cc b/gnc/gnc_autocode/src/ctl.cc deleted file mode 100644 index ea0072d143..0000000000 --- a/gnc/gnc_autocode/src/ctl.cc +++ /dev/null @@ -1,51 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include - -#include - -namespace gnc_autocode { - -GncCtlAutocode::GncCtlAutocode(void) { - controller_ = ctl_controller0(&ctl_input_, &cmd_, &ctl_); - assert(controller_); - assert(rtmGetErrorStatus(controller_) == NULL); -} - -GncCtlAutocode::~GncCtlAutocode() { - ctl_controller0_terminate(controller_); -} - -void GncCtlAutocode::Step(void) { - ctl_controller0_step(controller_, &ctl_input_, &cmd_, &ctl_); -} - -void GncCtlAutocode::Initialize(void) { - // initialize model - ctl_controller0_initialize(controller_, &ctl_input_, &cmd_, &ctl_); -} - -void GncCtlAutocode::ReadParams(config_reader::ConfigReader* config) { - ctl_ReadParams(config, controller_); -} - -} // end namespace gnc_autocode diff --git a/gnc/gnc_autocode/src/ekf.cc b/gnc/gnc_autocode/src/ekf.cc deleted file mode 100644 index d82b5475b7..0000000000 --- a/gnc/gnc_autocode/src/ekf.cc +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include - -#include - -namespace gnc_autocode { - -GncEkfAutocode::GncEkfAutocode(void) { - int s = 15 + 6 + 6 * ASE_OF_NUM_AUG; - P_ = static_cast(malloc(sizeof(float) * s * s)); - assert(P_); - est_ = est_estimator(&vis_, ®_, &of_, &hand_, &imu_, &cmc_, quat_, &kfl_, P_); - assert(est_); - assert(rtmGetErrorStatus(est_) == NULL); - Initialize(); -} - -GncEkfAutocode::~GncEkfAutocode() { - est_estimator_terminate(est_); - free(P_); -} - -void GncEkfAutocode::Step() { - est_estimator_step(est_, &vis_, ®_, &of_, &hand_, &imu_, &cmc_, quat_, &kfl_, P_); -} - -void GncEkfAutocode::Initialize() { - // initialize model - est_estimator_initialize(est_, &vis_, ®_, &of_, &hand_, &imu_, &cmc_, quat_, &kfl_, P_); -} - -void GncEkfAutocode::ReadParams(config_reader::ConfigReader* config) { - est_ReadParams(config, est_); -} - -} // end namespace gnc_autocode diff --git a/gnc/gnc_autocode/src/ekf_csv.cc b/gnc/gnc_autocode/src/ekf_csv.cc deleted file mode 100644 index da68f1a4bf..0000000000 --- a/gnc/gnc_autocode/src/ekf_csv.cc +++ /dev/null @@ -1,111 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -namespace gnc_autocode { - -GncEkfCSV::GncEkfCSV(void) : GncEkfAutocode() {} - -void GncEkfCSV::Initialize(std::string directory) { - kfl_file_ = fopen((directory + "/out_kfl_msg.csv").c_str(), "r"); - if (kfl_file_ == NULL) { - fprintf(stderr, "Failed to open out_kfl_msg.csv\n"); - exit(1); - } - SkipFirstLine(kfl_file_); -} - -void GncEkfCSV::SkipFirstLine(FILE* f) { - char c; - do { - c = fgetc(f); - } while (c != '\n'); -} - -GncEkfCSV::~GncEkfCSV() { - fclose(kfl_file_); -} - -int GncEkfCSV::ReadStepState(void) { - int ret = fscanf(kfl_file_, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,", - &kfl_.quat_ISS2B[0], &kfl_.quat_ISS2B[1], &kfl_.quat_ISS2B[2], &kfl_.quat_ISS2B[3], - &kfl_.omega_B_ISS_B[0], &kfl_.omega_B_ISS_B[1], &kfl_.omega_B_ISS_B[2], - &kfl_.gyro_bias[0], &kfl_.gyro_bias[1], &kfl_.gyro_bias[2]); - if (ret != 10) { - fprintf(stderr, "Failed to read line part 1 in KFL file.\n"); - return 1; - } - ret = fscanf(kfl_file_, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%hhu,%hhu,", - &kfl_.V_B_ISS_ISS[0], &kfl_.V_B_ISS_ISS[1], &kfl_.V_B_ISS_ISS[2], - &kfl_.A_B_ISS_ISS[0], &kfl_.A_B_ISS_ISS[1], &kfl_.A_B_ISS_ISS[2], - &kfl_.accel_bias[0], &kfl_.accel_bias[1], &kfl_.accel_bias[2], - &kfl_.P_B_ISS_ISS[0], &kfl_.P_B_ISS_ISS[1], &kfl_.P_B_ISS_ISS[2], - &kfl_.confidence, &kfl_.aug_state_enum); - if (ret != 14) { - fprintf(stderr, "Failed to read line part 2 in KFL file.\n"); - return 1; - } - ret = fscanf(kfl_file_, "%g,%g,%g,%g,%g,%g,%g,", - &kfl_.ml_quat_ISS2cam[0], &kfl_.ml_quat_ISS2cam[1], - &kfl_.ml_quat_ISS2cam[2], &kfl_.ml_quat_ISS2cam[3], - &kfl_.ml_P_cam_ISS_ISS[0], &kfl_.ml_P_cam_ISS_ISS[1], &kfl_.ml_P_cam_ISS_ISS[2]); - if (ret != 7) { - fprintf(stderr, "Failed to read line part 3 in KFL file.\n"); - return 1; - } - for (int i = 0; i < 5; i++) { - ret = fscanf(kfl_file_, "%g,%g,%g,%g,", - &kfl_.of_quat_ISS2cam[i], &kfl_.of_quat_ISS2cam[5 + i], - &kfl_.of_quat_ISS2cam[10 + i], &kfl_.of_quat_ISS2cam[15 + i]); - if (ret != 4) { - fprintf(stderr, "Failed to read line part 4 in KFL file.\n"); - return 1; - } - } - for (int i = 0; i < 5; i++) { - ret = fscanf(kfl_file_, "%g,%g,%g,", &kfl_.of_P_cam_ISS_ISS[i], - &kfl_.of_P_cam_ISS_ISS[5 + i], &kfl_.of_P_cam_ISS_ISS[10 + i]); - if (ret != 3) { - fprintf(stderr, "Failed to read line part 5 in KFL file.\n"); - return 1; - } - } - for (int i = 0; i < 51; i++) { - ret = fscanf(kfl_file_, "%lf,", &kfl_.cov_diag[i]); - if (ret != 1) { - fprintf(stderr, "Failed to read line part 6 in KFL file.\n"); - return 1; - } - } - ret = fscanf(kfl_file_, "%hu\n", &kfl_.kfl_status); - if (ret != 1) { - fprintf(stderr, "Failed to read line part 7 in KFL file.\n"); - return 1; - } - return 0; -} - - -void GncEkfCSV::Step(void) { - if (ReadStepState()) - exit(0); -} - -} // namespace gnc_autocode - diff --git a/gnc/gnc_autocode/src/fam.cc b/gnc/gnc_autocode/src/fam.cc deleted file mode 100644 index d9f40ddd6c..0000000000 --- a/gnc/gnc_autocode/src/fam.cc +++ /dev/null @@ -1,52 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include - -#include - -namespace gnc_autocode { - -GncFamAutocode::GncFamAutocode() { - fam_ = fam_force_allocation_module(&time_, &cmd_, &ctl_, &cmc_, &act_); - assert(fam_); - assert(rtmGetErrorStatus(fam_) == NULL); - Initialize(); -} - -GncFamAutocode::~GncFamAutocode() { - fam_force_allocation_module_terminate(fam_); -} - -void GncFamAutocode::Step(ex_time_msg* ex_time, cmd_msg* cmd, ctl_msg* ctl) { - fam_force_allocation_module_step(fam_, ex_time, cmd, ctl, &cmc_, &act_); -} - -void GncFamAutocode::Initialize() { - // initialize model - fam_force_allocation_module_initialize(fam_, &time_, &cmd_, &ctl_, &cmc_, &act_); -} - -void GncFamAutocode::ReadParams(config_reader::ConfigReader* config) { - fam_ReadParams(config, fam_); -} - -} // end namespace gnc_autocode diff --git a/gnc/gnc_autocode/src/sim.cc b/gnc/gnc_autocode/src/sim.cc deleted file mode 100644 index 36a39ea817..0000000000 --- a/gnc/gnc_autocode/src/sim.cc +++ /dev/null @@ -1,149 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include - -#include - -namespace gnc_autocode { - -GncSimAutocode::GncSimAutocode(void) { - // allocate model data - sim_ = sim_model_lib0(&act_msg_, &cmc_in_msg_, &optical_msg_, &hand_msg_, &cmc_out_msg_, &imu_msg_, - &env_msg_, &bpm_msg_, ®_pulse_, &landmark_msg_, &ar_tag_msg_, &ex_time_msg_); - // check for memory allocation error - assert(sim_); - // check status of model registration - assert(rtmGetErrorStatus(sim_) == NULL); - - Initialize(); -} - -void GncSimAutocode::Initialize(void) { - // initialize model - sim_model_lib0_initialize(sim_, &act_msg_, &cmc_in_msg_, &optical_msg_, &hand_msg_, &cmc_out_msg_, &imu_msg_, - &env_msg_, &bpm_msg_, ®_pulse_, &landmark_msg_, &ar_tag_msg_, &ex_time_msg_); -} - -GncSimAutocode::~GncSimAutocode() { - sim_model_lib0_terminate(sim_); -} - -void GncSimAutocode::ReadParams(config_reader::ConfigReader* config) { - sim_ReadParams(config, sim_); -} - -void GncSimAutocode::Step(void) { - // This is copied from the auto code. Copy it again if GNC updates as there - // are magical divisors in here. Yikes! - sim_model_lib0_step0(sim_, &act_msg_, &cmc_in_msg_, &optical_msg_, &hand_msg_, &cmc_out_msg_, &imu_msg_, - &env_msg_, &bpm_msg_, ®_pulse_, &landmark_msg_, &ar_tag_msg_, &ex_time_msg_); - - static bool OverrunFlags[5] = { 0, 0, 0, 0, 0 }; - static bool eventFlags[5] = { 0, 0, 0, 0, 0 }; - static int taskCounter[5] = { 0, 0, 0, 0, 0 }; - int i; - - /* Check base rate for overrun */ - if (OverrunFlags[0]) { - rtmSetErrorStatus(sim_, "Overrun"); - return; - } - - OverrunFlags[0] = true; - - /* - * For a bare-board target (i.e., no operating system), the - * following code checks whether any subrate overruns, - * and also sets the rates that need to run this time step. - */ - for (i = 1; i < 5; i++) { - if (taskCounter[i] == 0) { - if (eventFlags[i]) { - OverrunFlags[0] = false; - OverrunFlags[i] = true; - - /* Sampling too fast */ - rtmSetErrorStatus(sim_, "Overrun"); - return; - } - - eventFlags[i] = true; - } - } - - taskCounter[1]++; - if (taskCounter[1] == 4) { - taskCounter[1]= 0; - } - - taskCounter[2]++; - if (taskCounter[2] == 10) { - taskCounter[2]= 0; - } - - taskCounter[3]++; - if (taskCounter[3] == 13) { - taskCounter[3]= 0; - } - - taskCounter[4]++; - if (taskCounter[4] == 31) { - taskCounter[4]= 0; - } - - /* Indicate task for base rate complete */ - OverrunFlags[0] = false; - - /* Step the model for any subrate */ - for (i = 1; i < 5; i++) { - /* If task "i" is running, don't run any lower priority task */ - if (OverrunFlags[i]) { - return; - } - - if (eventFlags[i]) { - OverrunFlags[i] = true; - - /* Step the model for subrate "i" */ - switch (i) { - case 1 : - sim_model_lib0_step1(sim_); - break; - case 2 : - sim_model_lib0_step2(sim_); - break; - case 3 : - sim_model_lib0_step3(sim_); - break; - case 4 : - sim_model_lib0_step4(sim_); - break; - } - - /* Indicate task complete for sample time "i" */ - OverrunFlags[i] = false; - eventFlags[i] = false; - } - } -} - -} // namespace gnc_autocode - diff --git a/gnc/gnc_autocode/src/sim_csv.cc b/gnc/gnc_autocode/src/sim_csv.cc deleted file mode 100644 index bc34f9b0b8..0000000000 --- a/gnc/gnc_autocode/src/sim_csv.cc +++ /dev/null @@ -1,282 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -namespace gnc_autocode { - -GncSimCSV::GncSimCSV(void) : GncSimAutocode() { - seconds_ = 0; - nsec_ = 0; - last_landmark_sec_ = 0; - last_landmark_nsec_ = 0; - last_of_sec_ = 0; - last_of_nsec_ = 0; -} - -void GncSimCSV::Initialize(std::string directory) { - LoadGncFiles(directory); -} - -GncSimCSV::~GncSimCSV() { - fclose(f_.act); - fclose(f_.reg); - fclose(f_.landmark); - fclose(f_.optical); - fclose(f_.cmd); - fclose(f_.imu); - fclose(f_.env); -} - -void GncSimCSV::SkipFirstLine(FILE* f) { - char c; - do { - c = fgetc(f); - } while (c != '\n'); -} - -void GncSimCSV::LoadGncFiles(std::string directory) { - f_.act = fopen((directory + "/out_act_msg.csv").c_str(), "r"); - if (f_.act == NULL) { - fprintf(stderr, "Failed to open out_act_msg.csv\n"); - exit(1); - } - SkipFirstLine(f_.act); - - f_.reg = fopen((directory + "/out_cvs_reg_pulse.csv").c_str(), "r"); - if (f_.reg == NULL) { - fprintf(stderr, "Failed to open out_cvs_reg_pulse.csv\n"); - exit(1); - } - SkipFirstLine(f_.reg); - - f_.landmark = fopen((directory + "/out_cvs_landmark_msg.csv").c_str(), "r"); - if (f_.landmark == NULL) { - fprintf(stderr, "Failed to open out_cvs_landmark_msg.csv\n"); - exit(1); - } - SkipFirstLine(f_.landmark); - - f_.optical = fopen((directory + "/out_cvs_optflow_msg.csv").c_str(), "r"); - if (f_.optical == NULL) { - fprintf(stderr, "Failed to open out_cvs_optflow_msg.csv\n"); - exit(1); - } - SkipFirstLine(f_.optical); - - f_.cmd = fopen((directory + "/out_cmd_msg.csv").c_str(), "r"); - if (f_.cmd == NULL) { - fprintf(stderr, "Failed to open out_cmd_msg.csv\n"); - exit(1); - } - SkipFirstLine(f_.cmd); - - f_.imu = fopen((directory + "/out_imu_msg.csv").c_str(), "r"); - if (f_.imu == NULL) { - fprintf(stderr, "Failed to open out_imu_msg.csv\n"); - exit(1); - } - SkipFirstLine(f_.imu); - - f_.env = fopen((directory + "/out_env_msg.csv").c_str(), "r"); - if (f_.env == NULL) { - fprintf(stderr, "Failed to open out_env_msg.csv\n"); - exit(1); - } - SkipFirstLine(f_.env); -} - -int GncSimCSV::ReadStepState(void) { - float ignore1 = 0.0, ignore2 = 0.0; - act_msg_.act_timestamp_sec = seconds_; - act_msg_.act_timestamp_nsec = nsec_; - /* - int ret = fscanf(f_.act, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", - &ignore1, &ignore2, - &act_msg_.act_fan_speed_cmd[0], &act_msg_.act_fan_speed_cmd[1], - &act_msg_.act_fan_speed_cmd[2], &act_msg_.act_fan_speed_cmd[3], - &act_msg_.act_fan_speed_cmd[4], &act_msg_.act_fan_speed_cmd[5], - &act_msg_.act_fan_pitch_cmd[0], &act_msg_.act_fan_pitch_cmd[1], - &act_msg_.act_fan_pitch_cmd[2], &act_msg_.act_fan_pitch_cmd[3], - &act_msg_.act_fan_pitch_cmd[4], &act_msg_.act_fan_pitch_cmd[5], - &act_msg_.act_body_force_cmd[0], &act_msg_.act_body_force_cmd[1], - &act_msg_.act_body_force_cmd[2], &act_msg_.act_body_torque_cmd[0], - &act_msg_.act_body_torque_cmd[1], &act_msg_.act_body_torque_cmd[2]); - if (ret != 20) { - return 1; - } - */ - - int ret = fscanf(f_.reg, "%hhu,%hhu\n", ®_pulse_.cvs_landmark_pulse, ®_pulse_.cvs_optical_flow_pulse); - if (ret != 2) { - fprintf(stderr, "Failed to read registration line.\n"); - return 1; - } - - // parse landmarks - ret = fscanf(f_.landmark, "%g,%g,", &ignore1, &ignore2); - if (ret != 2) { - fprintf(stderr, "Failed to read landmark line.\n"); - return 1; - } - // this is how we tell if we got a landmark update - if (last_landmark_sec_ != ignore1 || last_landmark_nsec_ != ignore2) { - last_landmark_sec_ = ignore1; - last_landmark_nsec_ = ignore2; - landmark_msg_.cvs_timestamp_sec = seconds_; - landmark_msg_.cvs_timestamp_nsec = nsec_; - } - for (int i = 0; i < 50; i++) { - ret = fscanf(f_.landmark, "%g,%g,%g,", &landmark_msg_.cvs_landmarks[i], - &landmark_msg_.cvs_landmarks[50 + i], &landmark_msg_.cvs_landmarks[100 + i]); - if (ret != 3) { - fprintf(stderr, "Failed to read landmark line.\n"); - return 1; - } - } - for (int i = 0; i < 50; i++) { - ret = fscanf(f_.landmark, "%g,%g,", - &landmark_msg_.cvs_observations[i], &landmark_msg_.cvs_observations[50 + i]); - if (ret != 2) { - fprintf(stderr, "Failed to read landmark line.\n"); - return 1; - } - } - for (int i = 0; i < 49; i++) { - ret = fscanf(f_.landmark, "%hhu,", &landmark_msg_.cvs_valid_flag[i]); - if (ret != 1) { - fprintf(stderr, "Failed to read landmark line.\n"); - return 1; - } - } - ret = fscanf(f_.landmark, "%hhu\n", &landmark_msg_.cvs_valid_flag[49]); - if (ret != 1) { - fprintf(stderr, "Failed to read landmark line.\n"); - return 1; - } - - // parse optical flow - ret = fscanf(f_.optical, "%g,%g,", &ignore1, &ignore2); - if (ret != 2) { - fprintf(stderr, "Failed to read optical line.\n"); - return 1; - } - // this is how we tell if we got a landmark update - if (last_of_sec_ != ignore1 || last_of_nsec_ != ignore2) { - last_of_sec_ = ignore1; - last_of_nsec_ = ignore2; - optical_msg_.cvs_timestamp_sec = seconds_; - optical_msg_.cvs_timestamp_nsec = nsec_; - } - for (int i = 0; i < 30; i++) { - for (int j = 0; j < 2; j++) { - for (int k = 0; k < 5; k++) { - ret = fscanf(f_.optical, "%g,", &optical_msg_.cvs_observations[30 * 2 * k + 30 * j + i]); - if (ret != 1) { - fprintf(stderr, "Failed to read optical line.\n"); - return 1; - } - } - } - } - for (int i = 0; i < 30; i++) { - for (int j = 0; j < 5; j++) { - ret = fscanf(f_.optical, "%hhu,", &optical_msg_.cvs_valid_flag[30 * j + i]); - if (ret != 1) { - fprintf(stderr, "Failed to read optical line.\n"); - return 1; - } - } - } - for (int i = 0; i < 29; i++) { - ret = fscanf(f_.optical, "%g,", &optical_msg_.cvs_id_tag[i]); - if (ret != 1) { - fprintf(stderr, "Failed to read optical line.\n"); - return 1; - } - } - ret = fscanf(f_.optical, "%g\n", &optical_msg_.cvs_id_tag[29]); - if (ret != 1) { - fprintf(stderr, "Failed to read optical line.\n"); - return 1; - } - - // cmc / cmd isn't used by anything so let's not bother - // ret = fscanf(f_.cmd, "%d,%d,%d,%d,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g," - // "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", - // &s->cmd.cmd_timestamp_sec, &s->cmd.cmd_timestamp_nsec, - // &s->cmd.cmd_mode, &s->cmd.cmd_B_inuse, - // &s->cmd.traj_pos[0], &s->cmd.traj_pos[1], &s->cmd.traj_pos[1], - // &s->cmd.traj_vel[0], &s->cmd.traj_vel[1], &s->cmd.traj_vel[2], - // &s->cmd.traj_accel[0], &s->cmd.traj_accel[1], &s->cmd.traj_accel[2], - // &s->cmd.cmd_pos[0], &s->cmd.cmd_pos[1], &s->cmd.cmd_pos[1], - // &s->cmd.cmd_vel[0], &s->cmd.cmd_vel[1], &s->cmd.cmd_vel[2], - // &s->cmd.cmd_accel[0], &s->cmd.cmd_accel[1], &s->cmd.cmd_accel[2], - // &s->cmd.traj_quat, &s->cmd.traj_omega[0], &s->cmd.traj_omega[1], &s->cmd.traj_omega[2], - // &s->cmd.traj_alpha[0], &s->cmd.traj_alpha[1], &s->cmd.traj_alpha[2], - // &s->cmd.cmd_quat, &s->cmd.cmd_omega[0], &s->cmd.cmd_omega[1], &s->cmd.cmd_omega[2], - // &s->cmd.cmd_alpha[0], &s->cmd.cmd_alpha[1], &s->cmd.cmd_alpha[2]); - // if (ret != 36) - // return 1; - imu_msg_.imu_timestamp_sec = seconds_; - imu_msg_.imu_timestamp_nsec = nsec_; - ret = fscanf(f_.imu, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%hhu,%hhu\n", - &ignore1, &ignore2, - &imu_msg_.imu_A_B_ECI_sensor[0], &imu_msg_.imu_A_B_ECI_sensor[1], &imu_msg_.imu_A_B_ECI_sensor[2], - &imu_msg_.imu_accel_bias[0], &imu_msg_.imu_accel_bias[1], &imu_msg_.imu_accel_bias[2], - &imu_msg_.imu_omega_B_ECI_sensor[0], &imu_msg_.imu_omega_B_ECI_sensor[1], &imu_msg_.imu_omega_B_ECI_sensor[2], - &imu_msg_.imu_gyro_bias[0], &imu_msg_.imu_gyro_bias[1], &imu_msg_.imu_gyro_bias[2], - &imu_msg_.imu_validity_flag, &imu_msg_.imu_sat_flag); - if (ret != 16) { - fprintf(stderr, "Failed to read IMU line.\n"); - return 1; - } - ret = fscanf(f_.env, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", - &env_msg_.P_B_ISS_ISS[0], &env_msg_.P_B_ISS_ISS[1], &env_msg_.P_B_ISS_ISS[2], - &env_msg_.V_B_ISS_ISS[0], &env_msg_.V_B_ISS_ISS[1], &env_msg_.V_B_ISS_ISS[2], - &env_msg_.A_B_ISS_ISS[0], &env_msg_.A_B_ISS_ISS[1], &env_msg_.A_B_ISS_ISS[2], - &env_msg_.A_B_ISS_B[0], &env_msg_.A_B_ISS_B[1], &env_msg_.A_B_ISS_B[2], - &env_msg_.A_B_ECI_B[0], &env_msg_.A_B_ECI_B[1], &env_msg_.A_B_ECI_B[2], - &env_msg_.Q_ISS2B[0], &env_msg_.Q_ISS2B[1], &env_msg_.Q_ISS2B[2], &env_msg_.Q_ISS2B[3], - &env_msg_.omega_B_ISS_B[0], &env_msg_.omega_B_ISS_B[1], &env_msg_.omega_B_ISS_B[2], - &env_msg_.alpha_B_ISS_B[0], &env_msg_.alpha_B_ISS_B[1], &env_msg_.alpha_B_ISS_B[2], - &env_msg_.fan_torques_B[0], &env_msg_.fan_torques_B[1], &env_msg_.fan_torques_B[2], - &env_msg_.fan_forces_B[0], &env_msg_.fan_forces_B[1], &env_msg_.fan_forces_B[2]); - if (ret != 31) { - fprintf(stderr, "Failed to read env line.\n"); - return 1; - } - return 0; -} - - -void GncSimCSV::Step(void) { - if (ReadStepState()) - exit(0); - int rate = 16000000; - - // TODO(bcoltin): load time from gnc autocode - nsec_ += rate; - if (nsec_ >= rate) { - nsec_ -= rate; - seconds_++; - } -} - -} // namespace gnc_autocode - diff --git a/gnc/matlab/.gitignore b/gnc/matlab/.gitignore deleted file mode 100644 index 8e6c57056f..0000000000 --- a/gnc/matlab/.gitignore +++ /dev/null @@ -1,8 +0,0 @@ - -*.tlc -*.mex* -*ile_checksums.ma* -*twmakecfg* -*cxx_functions/Eigen* -ex_*.cpp - diff --git a/gnc/matlab/Equation_Pics/eq_quat2DCM.png b/gnc/matlab/Equation_Pics/eq_quat2DCM.png deleted file mode 100644 index c44af77f76..0000000000 Binary files a/gnc/matlab/Equation_Pics/eq_quat2DCM.png and /dev/null differ diff --git a/gnc/matlab/Equation_Pics/psi_matrix.png b/gnc/matlab/Equation_Pics/psi_matrix.png deleted file mode 100644 index 15e8181d75..0000000000 Binary files a/gnc/matlab/Equation_Pics/psi_matrix.png and /dev/null differ diff --git a/gnc/matlab/Equation_Pics/quat_propogation.png b/gnc/matlab/Equation_Pics/quat_propogation.png deleted file mode 100644 index 64eb971c6b..0000000000 Binary files a/gnc/matlab/Equation_Pics/quat_propogation.png and /dev/null differ diff --git a/gnc/matlab/Equation_Pics/skew_matrix.png b/gnc/matlab/Equation_Pics/skew_matrix.png deleted file mode 100644 index 854e1120a1..0000000000 Binary files a/gnc/matlab/Equation_Pics/skew_matrix.png and /dev/null differ diff --git a/gnc/matlab/Equation_Pics/slow_omega_prop.png b/gnc/matlab/Equation_Pics/slow_omega_prop.png deleted file mode 100644 index 1f136dea07..0000000000 Binary files a/gnc/matlab/Equation_Pics/slow_omega_prop.png and /dev/null differ diff --git a/gnc/matlab/Prototypes/Proto_4/build_Proto4.m b/gnc/matlab/Prototypes/Proto_4/build_Proto4.m deleted file mode 100644 index 080164ac5c..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/build_Proto4.m +++ /dev/null @@ -1,91 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Autocode the freeflyer simulink models -% -%% -run([fileparts(mfilename('fullpath')) '/../../' 'astrobee_set_path']) -% List of subsystems to build -models = {'astrobee/fsw_lib/est_estimator',... - 'astrobee/fsw_lib/ctl_controller', ... - 'astrobee/fsw_lib/fam_force_allocation_module',... - 'astrobee/sim_model_lib',... - 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module', ... - 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module'}; - -% Open the freeflyer model -disp('...Openning Model'); -open('astrobee') - -% Setup parameters for Proto 4 -disp('...Initializing Parameters'); -astrobee_init; -proto4_init; -astrobee_prep; - -% save the current working directory -starting_dir = pwd; - -cd(fullfile(ASTROBEE_ROOT, 'code_generation')); -try - disp('...Removing old generated code'); - if ~isempty(ls('*_rtw')) - rmdir('*_rtw', 's'); % Remove the old generated code - end -catch exception -end -disp('...Building Code'); -for ii = 1:length(models) - disp(['%%%%%%%%%%%% Building ' models{ii} '%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%']); - rtwbuild(models{ii}) - -end -copyfile(fullfile('slprj', 'ert', '_sharedutils'), 'sharedutils') - -%% Generate tunable functions -dirs = ls; -ctl_dir = regexp(dirs, 'ctl_\w+_rtw', 'match'); -est_dir = regexp(dirs, 'est_\w+_rtw', 'match'); -fam_dir = regexp(dirs, 'fam_\w+_rtw', 'match'); -sim_dir = regexp(dirs, 'sim_\w+_rtw', 'match'); - -if ~isempty(ctl_dir) - cd(ctl_dir{1}); - generate_ctl_param_reads; - cd .. -end - -if ~isempty(est_dir) - cd(est_dir{1}); - generate_est_param_reads; - cd .. -end - -if ~isempty(fam_dir) - cd(fam_dir{1}); - generate_fam_param_reads; - cd .. -end - -if ~isempty(sim_dir) - cd(sim_dir{1}); - generate_sim_param_reads; - cd .. -end - -%% Return to the beginning -cd(starting_dir) \ No newline at end of file diff --git a/gnc/matlab/Prototypes/Proto_4/proto4_init.m b/gnc/matlab/Prototypes/Proto_4/proto4_init.m deleted file mode 100644 index 922152ec44..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/proto4_init.m +++ /dev/null @@ -1,76 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Proto 4 INIT -% Init file containing the delta's between proto 4 and flight -% Dependencies: -% astrobee_init has been run first and default parameters are loaded in the current workspace -% -% -%% Debugging -% cvs_noise_on = false; - -astrobee_version = 'p4'; -tunable_init; - -%% Enviroment -% env_granite_lab_flag = boolean(1); %[flag] Indicates if configuration should be for granite lab testing -env_grav_disturb_on = boolean(0); %[flag] Turns on/off the gravity disturbance model (to be used in granite table setup only!) -env_rot_effects_on = boolean(0); %[flag] Turns on/off the rotation effects in the dynamic model -% Moved to gnc.config -%tun_env_drag_disturb_on = boolean(0); %[flag] Turns on/off the drag disturbance model - - -%% Physical Properties - -%% Control Gains -%% Position Loop -% Note, control architecture described in TBD, .75, 6 .001 -% Moved to gnc.config -% ctl_pos_kp = single(4*ones(1,3)); % Note, this is Kp in the traditional sense, actual Kp calculated in prep, 18 -% ctl_pos_ki = single( .1/100*ones(1,3) ); % Note, this is Ki in the traditional sense, actual Ki calculated in prep -ctl_pos_sat_upper = single( 0.5 ); -ctl_pos_sat_lower = single( -0.5 ); - -%.8,4,.01 -%% Velocity Loop -ctl_linear_vel_limit = single( 10000 ); %m/s, 0.5 m/s reads(3.GNC.22), need to rework -%ctl_vel_kd = single( 25*ones(1,3) ); %2.5 - -%% Linear Output -ctl_linear_force_limit = 1*100; - -%% Attitude Loop -% kp = .4, kd=1 -% Moved to gnc.config -% ctl_att_kp = single( 8*.1*ones(1,3)); % Note, this is Kp in the traditional sense, actual Kp calculated in prep -% ctl_att_ki = single( 4*.0001*ones(1,3) ); % Note, this is Ki in the traditional sense, actual Ki calculated in prep - -ctl_att_sat_upper = single( 0.5 ); -ctl_att_sat_lower = single( -0.5 ); - -%% Rate Loop (angular rates) -ctl_omega_limit = single( 10000*pi/180*ones(3,1) ); %rad/sec Still TBD (3.GNC.23) -ctl_omega_kd = single( 3*1*ones(3,1) ); - -%% Earth Parameters (for ground testing only) -ase_earth_rate = single(0*[0, 0, 1]*((360/23.9344699)/3600)*pi/180); % Earth rate, rad/sec, in ECEF -ase_pos_vec = single(0*1.0e+06*[-2.6922, -4.2987, 3.8541]); % N269 Lab position in ECEF (lla2ecef([37.415117, -122.058621, 0])) -ase_gravity_accel = single(0*[0,0,-9.81]); - -ase_accel_fixed_bias = single(0*[-0.0292, -0.0325,0.0349]); -epson_accel_bias_ic = single([0 0 0]); diff --git a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn001.m b/gnc/matlab/Prototypes/Proto_4/run_gantry_scn001.m deleted file mode 100644 index 2819c25cbe..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn001.m +++ /dev/null @@ -1,57 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Gantry scenario 001, straight line forward and backward. -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee_no_noise')) - open('astrobee'); -end - -% Configure the scenario -% Init files -astrobee_init; -proto4_init; -setup_gantry_scn001; - - - -% Dependencies -astrobee_prep; - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -% dlmwrite('gantry_scn001.csv', [out_ctl_msg.body_accel_cmd.time, out_ctl_msg.body_accel_cmd.data, out_ctl_msg.body_alpha_cmd.data]) -% save gantry_scn001_truth out_env_msg - -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - - diff --git a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn002.m b/gnc/matlab/Prototypes/Proto_4/run_gantry_scn002.m deleted file mode 100644 index e8a67e4085..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn002.m +++ /dev/null @@ -1,57 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Gantry scenario 001, straight line forward and backward. -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -% Init files -astrobee_init; -proto4_init; -setup_gantry_scn002; - - - -% Dependencies -astrobee_prep; - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -dlmwrite('gantry_scn002.csv', [out_ctl_msg.body_accel_cmd.time, out_ctl_msg.body_accel_cmd.data, out_ctl_msg.body_alpha_cmd.data]) -save gantry_scn002_truth out_env_msg - -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - - diff --git a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn003.m b/gnc/matlab/Prototypes/Proto_4/run_gantry_scn003.m deleted file mode 100644 index 4c4987ef4f..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn003.m +++ /dev/null @@ -1,57 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Gantry scenario 001, straight line forward and backward. -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -% Init files -astrobee_init; -proto4_init; -setup_gantry_scn003; - - - -% Dependencies -astrobee_prep; - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -dlmwrite('gantry_scn003.csv', [out_ctl_msg.body_accel_cmd.time, out_ctl_msg.body_accel_cmd.data, out_ctl_msg.body_alpha_cmd.data]) -save gantry_scn003_truth out_env_msg - -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - - diff --git a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn004.m b/gnc/matlab/Prototypes/Proto_4/run_gantry_scn004.m deleted file mode 100644 index af26931af4..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn004.m +++ /dev/null @@ -1,57 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Gantry scenario 001, straight line forward and backward. -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -% Init files -astrobee_init; -proto4_init; -setup_gantry_scn004; - - - -% Dependencies -astrobee_prep; - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -dlmwrite('gantry_scn004.csv', [out_ctl_msg.body_accel_cmd.time, out_ctl_msg.body_accel_cmd.data, out_ctl_msg.body_alpha_cmd.data]) -save gantry_scn004_truth out_env_msg - -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - - diff --git a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn005.m b/gnc/matlab/Prototypes/Proto_4/run_gantry_scn005.m deleted file mode 100644 index f2ac4248b4..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_gantry_scn005.m +++ /dev/null @@ -1,57 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Gantry scenario 001, straight line forward and backward. -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -% Init files -astrobee_init; -proto4_init; -setup_gantry_scn005; - - - -% Dependencies -astrobee_prep; - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -dlmwrite('gantry_scn005.csv', [out_ctl_msg.body_accel_cmd.time, out_ctl_msg.body_accel_cmd.data, out_ctl_msg.body_alpha_cmd.data]) -save gantry_scn005_truth out_env_msg - -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - - diff --git a/gnc/matlab/Prototypes/Proto_4/run_p4_scn01.m b/gnc/matlab/Prototypes/Proto_4/run_p4_scn01.m deleted file mode 100644 index 9452c9f0c7..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_p4_scn01.m +++ /dev/null @@ -1,51 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Perching scenario -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -astrobee_init; -proto4_init; -setup_p4_scn01; -tun_debug_ctl_use_truth = uint8(1); - -astrobee_prep; - - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - diff --git a/gnc/matlab/Prototypes/Proto_4/run_p4_scn02.m b/gnc/matlab/Prototypes/Proto_4/run_p4_scn02.m deleted file mode 100644 index a3e0c7506f..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_p4_scn02.m +++ /dev/null @@ -1,50 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Perching scenario -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -astrobee_init; -proto4_init; -setup_p4_scn02; - -astrobee_prep; - - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - diff --git a/gnc/matlab/Prototypes/Proto_4/run_p4_scn03.m b/gnc/matlab/Prototypes/Proto_4/run_p4_scn03.m deleted file mode 100644 index eb8c36d163..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_p4_scn03.m +++ /dev/null @@ -1,50 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Perching scenario -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -astrobee_init; -proto4_init; -setup_p4_scn03; - -astrobee_prep; - - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - diff --git a/gnc/matlab/Prototypes/Proto_4/run_p4_scn101.m b/gnc/matlab/Prototypes/Proto_4/run_p4_scn101.m deleted file mode 100644 index 704cc8801d..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/run_p4_scn101.m +++ /dev/null @@ -1,49 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Perching scenario -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -astrobee_init; -proto4_init; -setup_p4_scn101; - -astrobee_prep; - - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee') -toc -display('Simulation Complete'); -%% Post Process Data -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - diff --git a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn001.m b/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn001.m deleted file mode 100644 index 430bcc77bf..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn001.m +++ /dev/null @@ -1,68 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Configuration file for Gantry Scenario -% TO DO: -% Need to put the localization system into docking mode once we have the -% dock AR targets in the correct location -astrobee_init; -proto4_init; -astrobee_version = 'flight'; -tunable_init; -astrobee_prep; - -tun_debug_ctl_use_truth = true; -astrobee_stop_time = 180; -tun_ase_enable_of = uint8(0); % Disable optical flow - -init_pos = [0 .7 0]; - -[setpoint_out, cmd_times_out] = calc_trapazoidal_waypoints_with_att_end_point([0 .7 0], [4 .7 0], .01, .25, [0 0 0 1],[0 0 0 1], 0, 0, 60); -[setpoint_back, cmd_times_back] = calc_trapazoidal_waypoints_with_att_end_point([4 .7 0], [0 .7 0], .01, .25, [0 0 0 1],[0 0 0 1], 0, 0, 120); -%[mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT filesep 'scenarios' filesep 'scn05.fplan']); -%[pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -mlp_command_times = uint32([0 0; cmd_times_out; cmd_times_back]); -mlp_command_list = single([init_pos 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoint_out; ... - setpoint_back]); - -ini_sim_initial_conditions_init; -tun_ini_P_B_ISS_ISS = single(init_pos); -tun_ase_state_ic_P_B_ISS_ISS = single(tun_ini_P_B_ISS_ISS); -tun_cmd_hard_accel_limit = single(.025); - - - - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 20 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -% Docking mode, 1 -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -% Nominal command mode -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - diff --git a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn002.m b/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn002.m deleted file mode 100644 index 5c52e9f6e2..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn002.m +++ /dev/null @@ -1,76 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Configuration file for Gantry Scenario -% TO DO: -% Need to put the localization system into docking mode once we have the -% dock AR targets in the correct location -astrobee_init; -proto4_init; -astrobee_version = 'flight'; -tunable_init; -astrobee_prep; - -tun_debug_ctl_use_truth = true; -astrobee_stop_time = 300; -tun_ase_enable_of = uint8(0); % Disable optical flow - -init_pos = [0 0 0]; - -[setpoints1, cmd_times1] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [4 0 0], .01, .25, [0 0 0 1],[0 0 0 1], 0, 0, 60); -[setpoints2, cmd_times2] = calc_trapazoidal_waypoints_with_att_end_point([4 0 0], [4 0 0], .01, .25, [0 0 0 1],[0 0 .7071 .7071], 1*pi/180, 10*pi/180, 100); -[setpoints3, cmd_times3] = calc_trapazoidal_waypoints_with_att_end_point([4 0 0], [4 .7 0], .01, .25, [0 0 .7071 .7071],[0 0 .7071 .7071], 1*pi/180, 10*pi/180, 130); -[setpoints4, cmd_times4] = calc_trapazoidal_waypoints_with_att_end_point([4 .7 0], [4 .7 0], .01, .25, [0 0 .7071 .7071] ,[0 0 1 0], 1*pi/180, 10*pi/180, 150); -[setpoints5, cmd_times5] = calc_trapazoidal_waypoints_with_att_end_point([4 .7 0], [0 .7 0], .01, .25, [0 0 1 0],[0 0 1 0], 1*pi/180, 10*pi/180, 180); -[setpoints6, cmd_times6] = calc_trapazoidal_waypoints_with_att_end_point([0 .7 0], [0 .7 0], .01, .25, [0 0 1 0],[0 0 -.7071 .7071], 1*pi/180, 10*pi/180, 220); -[setpoints7, cmd_times7] = calc_trapazoidal_waypoints_with_att_end_point([0 .7 0], [0 0 0], .01, .25, [0 0 -.7071 .7071],[0 0 -.7071 .7071], 1*pi/180, 10*pi/180, 260); -[setpoints8, cmd_times8] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [0 0 -.7071 .7071], [0 0 0 1], 1*pi/180, 10*pi/180, 280); - - -mlp_command_times = uint32([0 0; cmd_times1; cmd_times2; cmd_times3; cmd_times4; cmd_times5; cmd_times6; ... - cmd_times7; cmd_times8]); -mlp_command_list = single([init_pos 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoints1; ... - setpoints2; setpoints3; setpoints4; setpoints5; ... - setpoints6; setpoints7; setpoints8]); - -ini_sim_initial_conditions_init; -tun_ini_P_B_ISS_ISS = single(init_pos); -tun_ase_state_ic_P_B_ISS_ISS = single(tun_ini_P_B_ISS_ISS); -tun_cmd_hard_accel_limit = single(.025); - - - - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 20 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -% Docking mode, 1 -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -% Nominal command mode -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - diff --git a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn003.m b/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn003.m deleted file mode 100644 index 16e922e664..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn003.m +++ /dev/null @@ -1,76 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Configuration file for Gantry Scenario -% TO DO: -% Need to put the localization system into docking mode once we have the -% dock AR targets in the correct location -astrobee_init; -proto4_init; -astrobee_version = 'flight'; -tunable_init; -astrobee_prep; - -tun_debug_ctl_use_truth = true; -astrobee_stop_time = 120; -tun_ase_enable_of = uint8(0); % Disable optical flow - -init_pos = [0 0 0]; - -[setpoints1, cmd_times1] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [0 0 0 1],[.5 0 0 .866], 1*pi/180, 10*pi/180, 30); -[setpoints2, cmd_times2] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [.5 0 0 .866],[-.5 0 0 .866], 1*pi/180, 10*pi/180, 60); -[setpoints3, cmd_times3] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [-.5 0 0 .866],[0 0 0 1], 1*pi/180, 10*pi/180, 90); -[setpoints4, cmd_times4] = calc_trapazoidal_waypoints_with_att_end_point([4 1.2 0], [4 1.2 0], .01, .25, [0 0 .7071 .7071] ,[0 0 1 0], 1*pi/180, 10*pi/180, 150); -[setpoints5, cmd_times5] = calc_trapazoidal_waypoints_with_att_end_point([4 1.2 0], [0 1.2 0], .01, .25, [0 0 1 0],[0 0 1 0], 1*pi/180, 10*pi/180, 180); -[setpoints6, cmd_times6] = calc_trapazoidal_waypoints_with_att_end_point([0 1.2 0], [0 1.2 0], .01, .25, [0 0 1 0],[0 0 -.7071 .7071], 1*pi/180, 10*pi/180, 220); -[setpoints7, cmd_times7] = calc_trapazoidal_waypoints_with_att_end_point([0 1.2 0], [0 0 0], .01, .25, [0 0 -.7071 .7071],[0 0 -.7071 .7071], 1*pi/180, 10*pi/180, 260); -[setpoints8, cmd_times8] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [0 0 -.7071 .7071], [0 0 0 1], 1*pi/180, 10*pi/180, 280); - - -mlp_command_times = uint32([0 0; cmd_times1; cmd_times2; cmd_times3; cmd_times4; cmd_times5; cmd_times6; ... - cmd_times7; cmd_times8]); -mlp_command_list = single([init_pos 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoints1; ... - setpoints2; setpoints3; setpoints4; setpoints5; ... - setpoints6; setpoints7; setpoints8]); - -ini_sim_initial_conditions_init; -tun_ini_P_B_ISS_ISS = single(init_pos); -tun_ase_state_ic_P_B_ISS_ISS = single(tun_ini_P_B_ISS_ISS); -tun_cmd_hard_accel_limit = single(.025); - - - - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 20 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -% Docking mode, 1 -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -% Nominal command mode -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - diff --git a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn004.m b/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn004.m deleted file mode 100644 index b6bf8709dc..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn004.m +++ /dev/null @@ -1,76 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Configuration file for Gantry Scenario -% TO DO: -% Need to put the localization system into docking mode once we have the -% dock AR targets in the correct location -astrobee_init; -proto4_init; -astrobee_version = 'flight'; -tunable_init; -astrobee_prep; - -tun_debug_ctl_use_truth = true; -astrobee_stop_time = 140; -tun_ase_enable_of = uint8(0); % Disable optical flow - -init_pos = [0 0 0]; - -[setpoints1, cmd_times1] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [0 0 0 1],[.7071 0 0 .7071], 1*pi/180, 10*pi/180, 30); -[setpoints2, cmd_times2] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [.7071 0 0 .7071],[1 0 0 0], 1*pi/180, 10*pi/180, 60); -[setpoints3, cmd_times3] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [1 0 0 0],[-.7071 0 0 .7071], 1*pi/180, 10*pi/180, 90); -[setpoints4, cmd_times4] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25,[-.7071 0 0 .7071], [0 0 0 1], 1*pi/180, 10*pi/180, 120); -[setpoints5, cmd_times5] = calc_trapazoidal_waypoints_with_att_end_point([4 1.2 0], [4 1.2 0], .01, .25, [0 0 .7071 .7071] ,[0 0 1 0], 1*pi/180, 10*pi/180, 160); -[setpoints6, cmd_times6] = calc_trapazoidal_waypoints_with_att_end_point([4 1.2 0], [0 1.2 0], .01, .25, [0 0 1 0],[0 0 1 0], 1*pi/180, 10*pi/180, 180); -[setpoints7, cmd_times7] = calc_trapazoidal_waypoints_with_att_end_point([0 1.2 0], [0 1.2 0], .01, .25, [0 0 1 0],[0 0 -.7071 .7071], 1*pi/180, 10*pi/180, 220); -[setpoints8, cmd_times8] = calc_trapazoidal_waypoints_with_att_end_point([0 1.2 0], [0 0 0], .01, .25, [0 0 -.7071 .7071],[0 0 -.7071 .7071], 1*pi/180, 10*pi/180, 260); - - -mlp_command_times = uint32([0 0; cmd_times1; cmd_times2; cmd_times3; cmd_times4; cmd_times5; cmd_times6; ... - cmd_times7; cmd_times8]); -mlp_command_list = single([init_pos 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoints1; ... - setpoints2; setpoints3; setpoints4; setpoints5; ... - setpoints6; setpoints7; setpoints8]); - -ini_sim_initial_conditions_init; -tun_ini_P_B_ISS_ISS = single(init_pos); -tun_ase_state_ic_P_B_ISS_ISS = single(tun_ini_P_B_ISS_ISS); -tun_cmd_hard_accel_limit = single(.025); - - - - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 20 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -% Docking mode, 1 -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -% Nominal command mode -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - diff --git a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn005.m b/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn005.m deleted file mode 100644 index 3ae8357902..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_gantry_scn005.m +++ /dev/null @@ -1,76 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Configuration file for Gantry Scenario -% TO DO: -% Need to put the localization system into docking mode once we have the -% dock AR targets in the correct location -astrobee_init; -proto4_init; -astrobee_version = 'flight'; -tunable_init; -astrobee_prep; - -tun_debug_ctl_use_truth = true; -astrobee_stop_time = 140; -tun_ase_enable_of = uint8(0); % Disable optical flow - -init_pos = [0 0 0]; - -[setpoints1, cmd_times1] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [0 0 0 1],[0 0 1 0], 1*pi/180, 10*pi/180, 30); -[setpoints2, cmd_times2] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [0 0 1 0] ,[-0.2146 0.1274 0.9679 0.0283], 1*pi/180, 10*pi/180, 60); -[setpoints3, cmd_times3] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25, [-0.2146 0.1274 0.9679 0.0283],[0.1274 0.2146 -0.0283 0.9679], 1*pi/180, 10*pi/180, 90); -[setpoints4, cmd_times4] = calc_trapazoidal_waypoints_with_att_end_point([0 0 0], [0 0 0], .01, .25,[0.1274 0.2146 -0.0283 0.9679], [0 0 0 1], 1*pi/180, 10*pi/180, 120); -[setpoints5, cmd_times5] = calc_trapazoidal_waypoints_with_att_end_point([4 1.2 0], [4 1.2 0], .01, .25, [0 0 .7071 .7071] ,[0 0 1 0], 1*pi/180, 10*pi/180, 160); -[setpoints6, cmd_times6] = calc_trapazoidal_waypoints_with_att_end_point([4 1.2 0], [0 1.2 0], .01, .25, [0 0 1 0],[0 0 1 0], 1*pi/180, 10*pi/180, 180); -[setpoints7, cmd_times7] = calc_trapazoidal_waypoints_with_att_end_point([0 1.2 0], [0 1.2 0], .01, .25, [0 0 1 0],[0 0 -.7071 .7071], 1*pi/180, 10*pi/180, 220); -[setpoints8, cmd_times8] = calc_trapazoidal_waypoints_with_att_end_point([0 1.2 0], [0 0 0], .01, .25, [0 0 -.7071 .7071],[0 0 -.7071 .7071], 1*pi/180, 10*pi/180, 260); - - -mlp_command_times = uint32([0 0; cmd_times1; cmd_times2; cmd_times3; cmd_times4; cmd_times5; cmd_times6; ... - cmd_times7; cmd_times8]); -mlp_command_list = single([init_pos 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoints1; ... - setpoints2; setpoints3; setpoints4; setpoints5; ... - setpoints6; setpoints7; setpoints8]); - -ini_sim_initial_conditions_init; -tun_ini_P_B_ISS_ISS = single(init_pos); -tun_ase_state_ic_P_B_ISS_ISS = single(tun_ini_P_B_ISS_ISS); -tun_cmd_hard_accel_limit = single(.025); - - - - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 20 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -% Docking mode, 1 -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -% Nominal command mode -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - diff --git a/gnc/matlab/Prototypes/Proto_4/setup_p4_scn01.m b/gnc/matlab/Prototypes/Proto_4/setup_p4_scn01.m deleted file mode 100644 index 13ce14cff8..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_p4_scn01.m +++ /dev/null @@ -1,60 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%[mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT filesep 'scenarios' filesep 'scn05.fplan']); -%[pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -[setpoints1, cmd_times1] = calc_trapazoidal_waypoints_with_att_end_point([-.8 0 -.4], [0 0 -.4], .01, .25, [0 0 0 1],[0 0 0 1], 0, 0, 60); -[setpoints2, cmd_times2] = calc_trapazoidal_waypoints_with_att_end_point([0 0 -.4], [0 0 -.4], .01, .25, [0 0 0 1],[0 0 1 0], 1*pi/180, 10*pi/180, 100); -[setpoints3, cmd_times3] = calc_trapazoidal_waypoints_with_att_end_point([0 0 -.4], [-.8 0 -.4], .01, .25, [0 0 1 0],[0 0 1 0], 1*pi/180, 10*pi/180, 130); - -ini_sim_initial_conditions_init - - -% mlp_command_times = uint32([0 0; cmd_times1; cmd_times2; cmd_times3]); -% mlp_command_list = single([ini_P_B_ISS_ISS 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... -% setpoints1; ... -% setpoints2; setpoints3]); - -init_pos = [-0.8 0 -.4]; - -mlp_command_times = uint32([0 0; cmd_times1; cmd_times2]); -mlp_command_list = single([init_pos 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoints1; ... - setpoints2]); - - - -%ini_veh_quat = single([0, 0, .7071, .7071]); - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 100 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - - -astrobee_stop_time = 150; diff --git a/gnc/matlab/Prototypes/Proto_4/setup_p4_scn02.m b/gnc/matlab/Prototypes/Proto_4/setup_p4_scn02.m deleted file mode 100644 index 799003572a..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_p4_scn02.m +++ /dev/null @@ -1,47 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%[mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT filesep 'scenarios' filesep 'scn05.fplan']); -%[pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -mlp_command_times = uint32([0 0; 45 0; 100 0]); -mlp_command_list = single([0 0 -.4 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - .8 0 -.4 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;... - .8 0 -.4 .1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]); - -ini_sim_initial_conditions_init -% ini_P_B_ISS_ISS = single([-.8, 0 ,-.4]); -% ase_state_ic_P_B_ISS_ISS = ini_P_B_ISS_ISS; -%ini_veh_quat = single([0, 0, .7071, .7071]); - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 100 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -mlp_mode_cmd_list = uint8([2;2;1]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - - -astrobee_stop_time = 150; \ No newline at end of file diff --git a/gnc/matlab/Prototypes/Proto_4/setup_p4_scn03.m b/gnc/matlab/Prototypes/Proto_4/setup_p4_scn03.m deleted file mode 100644 index 7cbcedfcd5..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_p4_scn03.m +++ /dev/null @@ -1,47 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%[mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT filesep 'scenarios' filesep 'scn05.fplan']); -%[pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -mlp_command_times = uint32([0 0; 45 0; 200 0]); -mlp_command_list = single([0 0 -.8 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - 0 0 -.8 0 0 0 0 0 0 0 0 0 1 0 0 45*pi/180 0 0 0;... - 0 0 -.8 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]); - -ini_sim_initial_conditions_init -% ini_P_B_ISS_ISS = single([-.8, 0 ,-.4]); -% ase_state_ic_P_B_ISS_ISS = ini_P_B_ISS_ISS; -%ini_veh_quat = single([0, 0, .7071, .7071]); - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 100 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - - -astrobee_stop_time = 150; \ No newline at end of file diff --git a/gnc/matlab/Prototypes/Proto_4/setup_p4_scn101.m b/gnc/matlab/Prototypes/Proto_4/setup_p4_scn101.m deleted file mode 100644 index d412e62172..0000000000 --- a/gnc/matlab/Prototypes/Proto_4/setup_p4_scn101.m +++ /dev/null @@ -1,52 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Configuration file for Docking Monte Carlo -% TO DO: -% Need to put the localization system into docking mode once we have the -% dock AR targets in the correct location - -astrobee_stop_time = 116.9120; -tun_ase_enable_of = uint8(0); % Disable optical flow - -[setpoint, cmd_times] = calc_trapazoidal_waypoints([-.15 .57 -.8573], .02, [-.02 0 0], 100); -%[mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT filesep 'scenarios' filesep 'scn05.fplan']); -%[pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -mlp_command_times = uint32([0 0; cmd_times]); -mlp_command_list = single([-.15 .57 -.8573 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoint]); - -ini_sim_initial_conditions_init; -tun_ini_P_B_ISS_ISS = single([-.15 .57 -.8573]); -tun_ase_state_ic_P_B_ISS_ISS = single(tun_ini_P_B_ISS_ISS); -tun_cmd_hard_accel_limit = single(.025); - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 20 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -% Docking mode, 1 -mlp_loc_mode_cmd_list = uint8([1; 1; 1]); %[] Mode Commands to be sent at specified times. - -% Nominal command mode -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; diff --git a/gnc/matlab/astrobee.slx b/gnc/matlab/astrobee.slx deleted file mode 100644 index cd97e2de79..0000000000 Binary files a/gnc/matlab/astrobee.slx and /dev/null differ diff --git a/gnc/matlab/astrobee_configuration.m b/gnc/matlab/astrobee_configuration.m deleted file mode 100644 index cd1232c166..0000000000 --- a/gnc/matlab/astrobee_configuration.m +++ /dev/null @@ -1,478 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%--------------------------------------------------------------------------- -% MATLAB function for configuration set generated on 07-Sep-2016 07:29:09 -% MATLAB version: 8.6.0.267246 (R2015b) -%--------------------------------------------------------------------------- -function cs=astrobee_configuration() - -cs = Simulink.ConfigSet; - -% Original configuration set version: 1.15.1 -if cs.versionCompare('1.15.1') < 0 - error('Simulink:MFileVersionViolation', 'The version of the target configuration set is older than the original configuration set.'); -end - -% % Original environment character encoding: UTF-8 -% if ~strcmpi(get_param(0, 'CharacterEncoding'), 'UTF-8') -% warning('Simulink:EncodingUnMatched', 'The target character encoding (%s) is different from the original (%s).', get_param(0, 'CharacterEncoding'), 'UTF-8'); -% end - -% Do not change the order of the following commands. There are dependencies between the parameters. -cs.set_param('SystemTargetFile', 'ert.tlc'); % System target file -cs.set_param('GenerateComments', 'on'); % Include comments -cs.set_param('SimulinkBlockComments', 'on'); % Simulink block / Stateflow object comments -cs.set_param('ShowEliminatedStatement', 'off'); % Show eliminated blocks -cs.set_param('OperatorAnnotations', 'off'); % Operator annotations -cs.set_param('ForceParamTrailComments', 'off'); % Verbose comments for SimulinkGlobal storage class -cs.set_param('InsertBlockDesc', 'off'); % Simulink block descriptions -cs.set_param('SimulinkDataObjDesc', 'off'); % Simulink data object descriptions -cs.set_param('EnableCustomComments', 'off'); % Custom comments (MPT objects only) -cs.set_param('SFDataObjDesc', 'off'); % Stateflow object descriptions -cs.set_param('ReqsInCode', 'off'); % Requirements in block comments -cs.set_param('IncludeHyperlinkInReport', 'off'); % Code-to-model -cs.set_param('GenerateReport', 'off'); % Create code generation report -cs.set_param('GenerateTraceInfo', 'off'); % Model-to-code -cs.set_param('GenerateTraceReport', 'off'); % Eliminated / virtual blocks -cs.set_param('GenerateTraceReportSl', 'off'); % Traceable Simulink blocks -cs.set_param('GenerateTraceReportSf', 'off'); % Traceable Stateflow objects -cs.set_param('GenerateTraceReportEml', 'off'); % Traceable MATLAB functions -cs.set_param('LaunchReport', 'off'); % Open report automatically -cs.set_param('GenerateCodeReplacementReport', 'off'); % Summarize which blocks triggered code replacements -cs.set_param('GenerateCodeMetricsReport', 'off'); % Static code metrics -cs.set_param('GenerateWebview', 'off'); % Generate model Web view -cs.set_param('OptimizeBlockIOStorage', 'on'); % Signal storage reuse -cs.set_param('ExpressionFolding', 'on'); % Eliminate superfluous local variables (expression folding) -cs.set_param('ParamNamingRule', 'None'); % Parameter naming -cs.set_param('RTWCompilerOptimization', 'off'); % Compiler optimization level -cs.set_param('SignalNamingRule', 'None'); % Signal naming -cs.set_param('UpdateModelReferenceTargets', 'IfOutOfDateOrStructuralChange'); % Rebuild -cs.set_param('GenerateSampleERTMain', 'on'); % Generate an example main program -cs.set_param('ProdHWDeviceType', '32-bit Generic'); % Device vendor -cs.set_param('EnableUserReplacementTypes', 'off'); % Replace data type names in the generated code -cs.set_param('DefaultParameterBehavior', 'Tunable'); % Default parameter behavior -cs.set_param('ERTMultiwordTypeDef', 'System defined'); % Multiword type definitions -cs.set_param('EnableMemcpy', 'on'); % Use memcpy for vector assignment -cs.set_param('GenerateMakefile', 'off'); % Generate makefile -cs.set_param('GlobalDataDefinition', 'Auto'); % Data definition -cs.set_param('GlobalDataReference', 'Auto'); % Data declaration -cs.set_param('SaveFinalState', 'off'); % Final states -cs.set_param('SignalLogging', 'off'); % Signal logging -cs.set_param('TargetUnknown', 'off'); % Target unknown -cs.set_param('EnableConcurrentExecution', 'off'); % EnableConcurrentExecution -cs.set_param('LoadExternalInput', 'off'); % Input -cs.set_param('LoadInitialState', 'off'); % Initial state -cs.set_param('SaveTime', 'off'); % Time -cs.set_param('SaveState', 'off'); % States -cs.set_param('SaveOutput', 'off'); % Output -cs.set_param('LimitDataPoints', 'on'); % Limit data points to last -cs.set_param('DSMLogging', 'off'); % Data stores -cs.set_param('ReturnWorkspaceOutputs', 'off'); % Save simulation output as single object -cs.set_param('SupportNonInlinedSFcns', 'off'); % non-inlined S-functions -cs.set_param('PurelyIntegerCode', 'off'); % floating-point numbers -cs.set_param('SupportNonFinite', 'on'); % non-finite numbers -cs.set_param('MatFileLogging', 'off'); % MAT-file logging -cs.set_param('SuppressErrorStatus', 'off'); % Suppress error status in real-time model data structure -cs.set_param('SupportContinuousTime', 'off'); % continuous time -cs.set_param('IncludeMdlTerminateFcn', 'on'); % Terminate function required -cs.set_param('GRTInterface', 'off'); % Classic call interface -cs.set_param('CombineOutputUpdateFcns', 'on'); % Single output/update function -cs.set_param('ModelReferenceMinAlgLoopOccurrences', 'off'); % Minimize algebraic loop occurrences -cs.set_param('PortableWordSizes', 'on'); % Enable portable word sizes -cs.set_param('SolverType', 'Fixed-step'); % Type -cs.set_param('AutoInsertRateTranBlk', 'off'); % Automatically handle rate transition for data transfer -cs.set_param('ExtMode', 'off'); % Interface - -% The following commands do not have dependencies. -cs.set_param('Description', ''); % Description -cs.set_param('Name', 'Configuration'); % Name - -% Solver pane -cs.set_param('FixedStep', 'astrobee_time_step_size'); % Fixed-step size (fundamental sample time) -cs.set_param('PositivePriorityOrder', 'off'); % Higher priority value indicates higher task priority -cs.set_param('SampleTimeConstraint', 'Unconstrained'); % Periodic sample time constraint -cs.set_param('Solver', 'FixedStepDiscrete'); % Solver -cs.set_param('SolverMode', 'Auto'); % Tasking mode for periodic sample times -cs.set_param('StartTime', '0.0'); % Start time -cs.set_param('StopTime', 'astrobee_stop_time'); % Stop time - -% Data Import/Export pane -cs.set_param('Decimation', '1'); % Decimation -cs.set_param('LoggingIntervals', '[-inf, inf]'); % Logging intervals -cs.set_param('MaxDataPoints', '1000'); % Limit data points to last -cs.set_param('SaveFormat', 'Array'); % Format -cs.set_param('SignalLoggingName', 'logsout'); % Signal logging name -cs.set_param('SignalLoggingSaveFormat', 'Dataset'); % Signal logging format -cs.set_param('StreamToWorkspace', 'off'); % Write streamed signals to workspace -cs.set_param('StreamVariableName', 'streamout'); % Write streamed signals to workspace -cs.set_param('VisualizeSimOutput', 'off'); % Enable live streaming of selected signals to Simulation Data Inspector - -% Optimization pane -cs.set_param('AccelVerboseBuild', 'on'); % Verbose accelerator builds -cs.set_param('BlockReduction', 'on'); % Block reduction -cs.set_param('BooleanDataType', 'on'); % Implement logic signals as Boolean data (vs. double) -cs.set_param('ConditionallyExecuteInputs', 'on'); % Conditional input branch execution -cs.set_param('DefaultUnderspecifiedDataType', 'double'); % Default for underspecified data type -cs.set_param('EfficientFloat2IntCast', 'off'); % Remove code from floating-point to integer conversions that wraps out-of-range values -cs.set_param('EfficientMapNaN2IntZero', 'on'); % Remove code from floating-point to integer conversions with saturation that maps NaN to zero -cs.set_param('InitFltsAndDblsToZero', 'off'); % Use memset to initialize floats and doubles to 0.0 -cs.set_param('LifeSpan', '1'); % Application lifespan (days) -cs.set_param('NoFixptDivByZeroProtection', 'off'); % Remove code that protects against division arithmetic exceptions -cs.set_param('OptimizeModelRefInitCode', 'on'); % Optimize initialization code for model reference -cs.set_param('SimCompilerOptimization', 'off'); % Compiler optimization level -cs.set_param('UseDivisionForNetSlopeComputation', 'off'); % Use division for fixed-point net slope computation -cs.set_param('UseFloatMulNetSlope', 'off'); % Use floating-point multiplication to handle net slope corrections -cs.set_param('UseSpecifiedMinMax', 'off'); % Optimize using the specified minimum and maximum values -cs.set_param('ZeroExternalMemoryAtStartup', 'on'); % Remove root level I/O zero initialization -cs.set_param('ZeroInternalMemoryAtStartup', 'on'); % Remove internal data zero initialization - -% Optimization:Signals and Parameters pane -cs.set_param('BooleansAsBitfields', 'off'); % Pack Boolean data into bitfields -cs.set_param('BufferReuse', 'on'); % Reuse local block outputs -cs.set_param('GlobalBufferReuse', 'on'); % Reuse global block outputs -cs.set_param('GlobalVariableUsage', 'None'); % Optimize global data access -cs.set_param('InlinedParameterPlacement', 'Hierarchical'); % Parameter structure -cs.set_param('LocalBlockOutputs', 'on'); % Enable local block outputs -cs.set_param('MaxStackSize', 'Inherit from target'); % Maximum stack size (bytes) -cs.set_param('MemcpyThreshold', 64); % Memcpy threshold (bytes) -cs.set_param('PassReuseOutputArgsAs', 'Structure reference'); % Pass reusable subsystem outputs as -cs.set_param('RollThreshold', 5); % Loop unrolling threshold -cs.set_param('StrengthReduction', 'off'); % Simplify array indexing - -% Optimization:Stateflow pane -cs.set_param('ActiveStateOutputEnumStorageType', 'Native Integer'); % Base storage type for automatically created enumerations -cs.set_param('DataBitsets', 'off'); % Use bitsets for storing Boolean data -cs.set_param('StateBitsets', 'off'); % Use bitsets for storing state configuration - -% Diagnostics Solver pane -cs.set_param('AlgebraicLoopMsg', 'warning'); % Algebraic loop -cs.set_param('ArtificialAlgebraicLoopMsg', 'warning'); % Minimize algebraic loop -cs.set_param('BlockPriorityViolationMsg', 'warning'); % Block priority violation -cs.set_param('ConsistencyChecking', 'none'); % Solver data inconsistency -cs.set_param('MaxConsecutiveZCsMsg', 'error'); % Consecutive zero crossings violation -cs.set_param('MinStepSizeMsg', 'warning'); % Min step size violation -cs.set_param('ModelReferenceExtraNoncontSigs', 'error'); % Extraneous discrete derivative signals -cs.set_param('SimStateInterfaceChecksumMismatchMsg', 'warning'); % SimState interface checksum mismatch -cs.set_param('SimStateOlderReleaseMsg', 'error'); % SimState object from earlier release -cs.set_param('SolverPrmCheckMsg', 'warning'); % Automatic solver parameter selection -cs.set_param('StateNameClashWarn', 'none'); % State name clash -cs.set_param('TimeAdjustmentMsg', 'none'); % Sample hit time adjusting -cs.set_param('UnknownTsInhSupMsg', 'warning'); % Unspecified inheritability of sample time - -% Diagnostics:Sample Time pane -cs.set_param('InheritedTsInSrcMsg', 'warning'); % Source block specifies -1 sample time -cs.set_param('MultiTaskCondExecSysMsg', 'error'); % Multitask conditionally executed subsystem -cs.set_param('MultiTaskRateTransMsg', 'error'); % Multitask rate transition -cs.set_param('SigSpecEnsureSampleTimeMsg', 'warning'); % Enforce sample times specified by Signal Specification blocks -cs.set_param('SingleTaskRateTransMsg', 'none'); % Single task rate transition -cs.set_param('TasksWithSamePriorityMsg', 'warning'); % Tasks with equal priority - -% Diagnostics:Data Validity pane -cs.set_param('ArrayBoundsChecking', 'none'); % Array bounds exceeded -cs.set_param('AssertControl', 'UseLocalSettings'); % Model Verification block enabling -cs.set_param('CheckMatrixSingularityMsg', 'none'); % Division by singular matrix -cs.set_param('IntegerOverflowMsg', 'warning'); % Wrap on overflow -cs.set_param('IntegerSaturationMsg', 'warning'); % Saturate on overflow -cs.set_param('MultiTaskDSMMsg', 'error'); % Multitask data store -cs.set_param('ParameterDowncastMsg', 'error'); % Detect downcast -cs.set_param('ParameterOverflowMsg', 'error'); % Detect overflow -cs.set_param('ParameterPrecisionLossMsg', 'warning'); % Detect precision loss -cs.set_param('ParameterTunabilityLossMsg', 'warning'); % Detect loss of tunability -cs.set_param('ParameterUnderflowMsg', 'none'); % Detect underflow -cs.set_param('RTPrefix', 'error'); % "rt" prefix for identifiers -cs.set_param('ReadBeforeWriteMsg', 'UseLocalSettings'); % Detect read before write -cs.set_param('SignalInfNanChecking', 'none'); % Inf or NaN block output -cs.set_param('SignalRangeChecking', 'none'); % Simulation range checking -cs.set_param('SignalResolutionControl', 'UseLocalSettings'); % Signal resolution -cs.set_param('UnderSpecifiedDataTypeMsg', 'none'); % Underspecified data types -cs.set_param('UnderspecifiedInitializationDetection', 'Simplified'); % Underspecified initialization detection -cs.set_param('UniqueDataStoreMsg', 'none'); % Duplicate data store names -cs.set_param('WriteAfterReadMsg', 'UseLocalSettings'); % Detect write after read -cs.set_param('WriteAfterWriteMsg', 'UseLocalSettings'); % Detect write after write - -% Diagnostics:Type Conversion pane -cs.set_param('FixptConstOverflowMsg', 'none'); % Detect overflow -cs.set_param('FixptConstPrecisionLossMsg', 'none'); % Detect precision loss -cs.set_param('FixptConstUnderflowMsg', 'none'); % Detect underflow -cs.set_param('Int32ToFloatConvMsg', 'warning'); % 32-bit integer to single precision float conversion -cs.set_param('UnnecessaryDatatypeConvMsg', 'none'); % Unnecessary type conversions -cs.set_param('VectorMatrixConversionMsg', 'none'); % Vector/matrix block input conversion - -% Diagnostics:Connectivity pane -cs.set_param('BusNameAdapt', 'WarnAndRepair'); % Repair bus selections -cs.set_param('BusObjectLabelMismatch', 'error'); % Element name mismatch -cs.set_param('FcnCallInpInsideContextMsg', 'EnableAllAsError'); % Context-dependent inputs -cs.set_param('InvalidFcnCallConnMsg', 'error'); % Invalid function-call connection -cs.set_param('NonBusSignalsTreatedAsBus', 'none'); % Non-bus signals treated as bus signals -cs.set_param('RootOutportRequireBusObject', 'warning'); % Unspecified bus object at root Outport block -cs.set_param('SignalLabelMismatchMsg', 'none'); % Signal label mismatch -cs.set_param('StrictBusMsg', 'ErrorLevel1'); % Mux blocks used to create bus signals -cs.set_param('UnconnectedInputMsg', 'warning'); % Unconnected block input ports -cs.set_param('UnconnectedLineMsg', 'warning'); % Unconnected line -cs.set_param('UnconnectedOutputMsg', 'warning'); % Unconnected block output ports - -% Diagnostics:Compatibility pane -cs.set_param('FrameProcessingCompatibilityMsg', 'warning'); % Block behavior depends on frame status of signal -cs.set_param('SFcnCompatibilityMsg', 'none'); % S-function upgrades needed - -% Diagnostics:Model Referencing pane -cs.set_param('ModelReferenceDataLoggingMessage', 'warning'); % Unsupported data logging -cs.set_param('ModelReferenceIOMismatchMessage', 'none'); % Port and parameter mismatch -cs.set_param('ModelReferenceIOMsg', 'none'); % Invalid root Inport/Outport block connection -cs.set_param('ModelReferenceVersionMismatchMessage', 'none'); % Model block version mismatch - -% Diagnostics:Saving pane -cs.set_param('SaveWithDisabledLinksMsg', 'warning'); % Block diagram contains disabled library links -cs.set_param('SaveWithParameterizedLinksMsg', 'warning'); % Block diagram contains parameterized library links - -% Diagnostics:Stateflow pane -cs.set_param('SFInvalidInputDataAccessInChartInitDiag', 'warning'); % Invalid input data access in chart initialization -cs.set_param('SFNoUnconditionalDefaultTransitionDiag', 'warning'); % No unconditional default transitions -cs.set_param('SFOutputUsedAsStateInMooreChartDiag', 'error'); % Read-before-write to output in Moore chart -cs.set_param('SFTransitionActionBeforeConditionDiag', 'warning'); % Transition action specified before condition action -cs.set_param('SFTransitionOutsideNaturalParentDiag', 'warning'); % Transition outside natural parent -cs.set_param('SFUnconditionalTransitionShadowingDiag', 'warning'); % Transition shadowing -cs.set_param('SFUndirectedBroadcastEventsDiag', 'warning'); % Undirected event broadcasts -cs.set_param('SFUnexpectedBacktrackingDiag', 'warning'); % Unexpected backtracking -cs.set_param('SFUnusedDataAndEventsDiag', 'warning'); % Unused data, events and messages - -% Hardware Implementation pane -cs.set_param('ProdIntDivRoundTo', 'Undefined'); % Signed integer division rounds to -cs.set_param('ProdLargestAtomicFloat', 'None'); % floating-point -cs.set_param('ProdLargestAtomicInteger', 'Char'); % integer - -% Model Referencing pane -cs.set_param('EnableParallelModelReferenceBuilds', 'off'); % Enable parallel model reference builds -cs.set_param('EnableRefExpFcnMdlSchedulingChecks', 'on'); % Enable strict scheduling checks for referenced export-function models -cs.set_param('ModelDependencies', ''); % Model dependencies -cs.set_param('ModelReferenceNumInstancesAllowed', 'Multi'); % Total number of instances allowed per top model -cs.set_param('ModelReferencePassRootInputsByReference', 'on'); % Pass fixed-size scalar root inputs by value for code generation -cs.set_param('ParallelModelReferenceMATLABWorkerInit', 'None'); % MATLAB worker initialization for builds -cs.set_param('PropagateSignalLabelsOutOfModel', 'off'); % Propagate all signal labels out of the model -cs.set_param('PropagateVarSize', 'Infer from blocks in model'); % Propagate sizes of variable-size signals - -% Simulation Target General pane -cs.set_param('SFSimEcho', 'on'); % Echo expressions without semicolons -cs.set_param('SimBuildMode', 'sf_incremental_build'); % Simulation target build mode -cs.set_param('SimCtrlC', 'on'); % Ensure responsiveness -cs.set_param('SimGenImportedTypeDefs', 'off'); % Generate typedefs for imported bus and enumeration types -cs.set_param('SimIntegrity', 'on'); % Ensure memory integrity - -% Simulation Target:Custom Code pane -cs.set_param('SimCustomHeaderCode', ''); % Header file -cs.set_param('SimCustomInitializer', ''); % Initialize function -cs.set_param('SimCustomSourceCode', ''); % Source file -cs.set_param('SimCustomTerminator', ''); % Terminate function -cs.set_param('SimParseCustomCode', 'on'); % Parse custom code symbols -cs.set_param('SimUserIncludeDirs', ''); % Include directories ./../../../../../local/eigen3/Eigen -cs.set_param('SimUserLibraries', ''); % Libraries -cs.set_param('SimUserSources', ''); % Source files - -% Simulation Target:Symbols pane -cs.set_param('SimReservedNameArray', []); % Reserved names - -% Code Generation General pane -cs.set_param('CheckMdlBeforeBuild', 'Warning'); % Check model before generating code -cs.set_param('GenCodeOnly', 'on'); % Generate code only -cs.set_param('IgnoreCustomStorageClasses', 'off'); % Ignore custom storage classes -cs.set_param('IgnoreTestpoints', 'off'); % Ignore test point signals -cs.set_param('ObjectivePriorities', []); % Prioritized objectives -cs.set_param('PackageGeneratedCodeAndArtifacts', 'off'); % Package code and artifacts -cs.set_param('PackageName', ''); % Zip file name -cs.set_param('TargetLang', 'C++'); % Language - -% Code Generation:Comments pane -cs.set_param('MATLABFcnDesc', 'off'); % MATLAB function help text -cs.set_param('MATLABSourceComments', 'off'); % MATLAB source code as comments - -% Code Generation:Symbols pane -cs.set_param('CustomSymbolStrBlkIO', 'rtb_$N$M'); % Local block output variables -cs.set_param('CustomSymbolStrFcn', '$R$N$M$F'); % Subsystem methods -cs.set_param('CustomSymbolStrFcnArg', 'rt$I$N$M'); % Subsystem method arguments -cs.set_param('CustomSymbolStrField', '$N$M'); % Field name of global types -cs.set_param('CustomSymbolStrGlobalVar', '$R$N$M'); % Global variables -cs.set_param('CustomSymbolStrMacro', '$R$N$M'); % Constant macros -cs.set_param('CustomSymbolStrTmpVar', '$N$M'); % Local temporary variables -cs.set_param('CustomSymbolStrType', '$N$R$M_T'); % Global types -cs.set_param('CustomSymbolStrUtil', '$N$C'); % Shared utilities -cs.set_param('DefineNamingRule', 'UpperCase'); % #define naming -cs.set_param('InlinedPrmAccess', 'Literals'); % Generate scalar inlined parameters as -cs.set_param('InternalIdentifier', 'Shortened'); % System-generated identifiers -cs.set_param('MangleLength', 1); % Minimum mangle length -cs.set_param('MaxIdLength', 31); % Maximum identifier length -cs.set_param('ReservedNameArray', []); % Reserved names -cs.set_param('UseSimReservedNames', 'off'); % Use the same reserved names as Simulation Target - -% Code Generation:Custom Code pane -cs.set_param('CustomHeaderCode', ''); % Header file -cs.set_param('CustomInclude', ''); % Include directories -cs.set_param('CustomInitializer', ''); % Initialize function -cs.set_param('CustomLibrary', ''); % Libraries -cs.set_param('CustomSource', ''); % Source files -cs.set_param('CustomSourceCode', ''); % Source file -cs.set_param('CustomTerminator', ''); % Terminate function -cs.set_param('RTWUseSimCustomCode', 'off'); % Use the same custom code settings as Simulation Target - -% Code Generation:Debug pane -cs.set_param('ProfileTLC', 'off'); % Profile TLC -cs.set_param('RTWVerbose', 'on'); % Verbose build -cs.set_param('RetainRTWFile', 'off'); % Retain .rtw file -cs.set_param('TLCAssert', 'off'); % Enable TLC assertion -cs.set_param('TLCCoverage', 'off'); % Start TLC coverage when generating code -cs.set_param('TLCDebug', 'off'); % Start TLC debugger when generating code - -% Code Generation:Interface pane -cs.set_param('CodeInterfacePackaging', 'Reusable function'); % Code interface packaging -cs.set_param('CodeReplacementLibrary', 'None'); % Code replacement library -cs.set_param('CombineSignalStateStructs', 'off'); % Combine signal/state structures -cs.set_param('GenerateASAP2', 'off'); % Interface -cs.set_param('GenerateAllocFcn', 'on'); % Use dynamic memory allocation for model initialization -cs.set_param('MultiInstanceErrorCode', 'Error'); % Multi-instance code error diagnostic -cs.set_param('RootIOFormat', 'Individual arguments'); % Pass root-level I/O as -cs.set_param('SupportAbsoluteTime', 'on'); % absolute time -cs.set_param('SupportComplex', 'on'); % complex numbers -cs.set_param('SupportVariableSizeSignals', 'on'); % variable-size signals -cs.set_param('TargetLangStandard', 'C89/C90 (ANSI)'); % Standard math library -cs.set_param('UtilityFuncGeneration', 'Shared location'); % Shared code placement - -% Code Generation:Verification pane -cs.set_param('CodeExecutionProfileVariable', 'executionProfile'); % Workspace variable -cs.set_param('CodeExecutionProfiling', 'off'); % Measure task execution time -cs.set_param('CodeProfilingInstrumentation', 'off'); % Measure function execution times -cs.set_param('CodeProfilingSaveOptions', 'SummaryOnly'); % Save options -cs.set_param('CreateSILPILBlock', 'None'); % Create block -cs.set_param('SILDebugging', 'off'); % Enable source-level debugging for SIL - -% Code Generation:Code Style pane -cs.set_param('CastingMode', 'Nominal'); % Casting modes -cs.set_param('ConvertIfToSwitch', 'off'); % Convert if-elseif-else patterns to switch-case statements -cs.set_param('EnableSignedLeftShifts', 'on'); % Replace multiplications by powers of two with signed bitwise shifts -cs.set_param('EnableSignedRightShifts', 'on'); % Allow right shifts on signed integers -cs.set_param('IndentSize', '2'); % Indent size -cs.set_param('IndentStyle', 'K&R'); % Indent style -cs.set_param('ParenthesesLevel', 'Nominal'); % Parentheses level -cs.set_param('PreserveExpressionOrder', 'off'); % Preserve operand order in expression -cs.set_param('PreserveExternInFcnDecls', 'on'); % Preserve extern keyword in function declarations -cs.set_param('PreserveIfCondition', 'off'); % Preserve condition expression in if statement -cs.set_param('SuppressUnreachableDefaultCases', 'off'); % Suppress generation of default cases for Stateflow switch statements if unreachable - -% Code Generation:Templates pane -cs.set_param('ERTCustomFileTemplate', 'example_file_process.tlc'); % File customization template -cs.set_param('ERTDataHdrFileTemplate', 'ert_code_template.cgt'); % Header file template -cs.set_param('ERTDataSrcFileTemplate', 'ert_code_template.cgt'); % Source file template -cs.set_param('ERTHdrFileBannerTemplate', 'ert_code_template.cgt'); % Header file template -cs.set_param('ERTSrcFileBannerTemplate', 'ert_code_template.cgt'); % Source file template -cs.set_param('TargetOS', 'BareBoardExample'); % Target operating system - -% Code Generation:Code Placement pane -cs.set_param('ERTFilePackagingFormat', 'Modular'); % File packaging format -cs.set_param('EnableDataOwnership', 'off'); % Use owner from data object for data definition placement -cs.set_param('IncludeFileDelimiter', 'Auto'); % #include file delimiter -cs.set_param('ParamTuneLevel', 10); % Parameter tune level -cs.set_param('SignalDisplayLevel', 10); % Signal display level - -% Code Generation:Memory Sections pane -cs.set_param('MemSecDataConstants', 'Default'); % Constants -cs.set_param('MemSecDataIO', 'Default'); % Inputs/Outputs -cs.set_param('MemSecDataInternal', 'Default'); % Internal data -cs.set_param('MemSecDataParameters', 'Default'); % Parameters -cs.set_param('MemSecFuncExecute', 'Default'); % Execution -cs.set_param('MemSecFuncInitTerm', 'Default'); % Initialize/Terminate -cs.set_param('MemSecFuncSharedUtil', 'Default'); % Shared utility -cs.set_param('MemSecPackage', '--- None ---'); % Package - -% Parameters in List View only -cs.set_param('SolverName', 'FixedStepDiscrete'); % Solver: -cs.set_param('InspectSignalLogs', 'off'); % Record logged workspace data in Simulation Data Inspector -cs.set_param('AdvancedOptControl', ''); % AdvancedOptControl -cs.set_param('BufferReusableBoundary', 'on'); % BufferReusableBoundary -cs.set_param('AllowSymbolicDim', 'off'); % Allow symbolic dimension specification -cs.set_param('IgnoredZcDiagnostic', 'warning'); % IgnoredZcDiagnostic -cs.set_param('InitInArrayFormatMsg', 'warning'); % InitInArrayFormatMsg -cs.set_param('MaskedZcDiagnostic', 'warning'); % MaskedZcDiagnostic -cs.set_param('ModelReferenceSymbolNameMessage', 'warning'); % ModelReferenceSymbolNameMessage -cs.set_param('ProdEqTarget', 'on'); % Test hardware is the same as production hardware -cs.set_param('TargetPreprocMaxBitsSint', 32); % TargetPreprocMaxBitsSint -cs.set_param('TargetPreprocMaxBitsUint', 32); % TargetPreprocMaxBitsUint -cs.set_param('ParallelModelReferenceErrorOnInvalidPool', 'on'); % ParallelModelReferenceErrorOnInvalidPool -cs.set_param('SupportModelReferenceSimTargetCustomCode', 'off'); % SupportModelReferenceSimTargetCustomCode -cs.set_param('SFSimEnableDebug', 'off'); % Allow setting breakpoints during simulation -cs.set_param('Toolchain', 'Automatically locate an installed toolchain'); % Toolchain: -cs.set_param('BuildConfiguration', 'Faster Builds'); % Build configuration: -cs.set_param('PostCodeGenCommand', ''); % Post code generation command -cs.set_param('SaveLog', 'off'); % Save build log -cs.set_param('TLCOptions', ''); % TLC options: -cs.set_param('CommentStyle', 'Auto'); % Comment style -cs.set_param('IncAutoGenComments', 'off'); % IncAutoGenComments -cs.set_param('IncDataTypeInIds', 'off'); % IncDataTypeInIds -cs.set_param('IncHierarchyInIds', 'off'); % IncHierarchyInIds -cs.set_param('InsertPolySpaceComments', 'off'); % Insert Polyspace comments -cs.set_param('PreserveName', 'off'); % PreserveName -cs.set_param('PreserveNameWithParent', 'off'); % PreserveNameWithParent -cs.set_param('CPPClassGenCompliant', 'on'); % CPPClassGenCompliant -cs.set_param('CompOptLevelCompliant', 'on'); % CompOptLevelCompliant -cs.set_param('ConcurrentExecutionCompliant', 'on'); % ConcurrentExecutionCompliant -cs.set_param('ERTCustomFileBanners', 'on'); % ERTCustomFileBanners -cs.set_param('ERTFirstTimeCompliant', 'on'); % ERTFirstTimeCompliant -cs.set_param('GenerateFullHeader', 'on'); % GenerateFullHeader -cs.set_param('InferredTypesCompatibility', 'off'); % InferredTypesCompatibility -cs.set_param('ModelReferenceCompliant', 'on'); % ModelReferenceCompliant -cs.set_param('ModelStepFunctionPrototypeControlCompliant', 'on'); % ModelStepFunctionPrototypeControlCompliant -cs.set_param('MultiwordLength', 2048); % MultiwordLength -cs.set_param('ParMdlRefBuildCompliant', 'on'); % ParMdlRefBuildCompliant -cs.set_param('TargetFcnLib', 'ansi_tfl_table_tmw.mat'); % TargetFcnLib -cs.set_param('TargetLibSuffix', ''); % TargetLibSuffix -cs.set_param('TargetPreCompLibLocation', ''); % TargetPreCompLibLocation -cs.set_param('UseToolchainInfoCompliant', 'on'); % UseToolchainInfoCompliant -cs.set_param('RTWCAPIParams', 'off'); % parameters -cs.set_param('RTWCAPIRootIO', 'off'); % root-level I/O -cs.set_param('RTWCAPISignals', 'off'); % signals -cs.set_param('RTWCAPIStates', 'off'); % states -cs.set_param('RateGroupingCode', 'on'); % RateGroupingCode -cs.set_param('HoistShortCircuitFcnCalls', 'off'); % Hoist function calls inside short circuits -cs.set_param('BooleanTrueId', 'true'); % Boolean true identifier. -cs.set_param('BooleanFalseId', 'false'); % Boolean false identifier. -cs.set_param('MaxIdInt32', 'MAX_int32_T'); % 32-bit integer maximum identifier -cs.set_param('MinIdInt32', 'MIN_int32_T'); % 32-bit integer minimum identifier -cs.set_param('MaxIdUint32', 'MAX_uint32_T'); % 32-bit unsigned integer maximum identifier -cs.set_param('MaxIdInt16', 'MAX_int16_T'); % 16-bit integer maximum identifier -cs.set_param('MinIdInt16', 'MIN_int16_T'); % 16-bit integer minimum identifier -cs.set_param('MaxIdUint16', 'MAX_uint16_T'); % 16-bit unsigned integer maximum identifier -cs.set_param('MaxIdInt8', 'MAX_int8_T'); % 8-bit integer maximum identifier -cs.set_param('MinIdInt8', 'MIN_int8_T'); % 8-bit integer minimum identifier -cs.set_param('MaxIdUint8', 'MAX_uint8_T'); % 8-bit unsigned integer maximum identifier -cs.set_param('TypeLimitIdReplacementHeaderFile', ''); % Type limit identifier replacement header file -cs.set_param('RecordCoverage', 'off'); % Record coverage for this model -cs.set_param('CovModelRefEnable', 'off'); % Record coverage for referenced models -cs.set_param('CovExternalEMLEnable', 'off'); % Record coverage for MATLAB files -cs.set_param('CovEnableCumulative', 'on'); % Enable cumulative data collection -cs.set_param('CovSaveCumulativeToWorkspaceVar', 'on'); % Save cumulative coverage results in workspace variable -cs.set_param('CovCumulativeVarName', 'covCumulativeData'); % Cumulative coverage variable name -cs.set_param('CovSaveSingleToWorkspaceVar', 'on'); % Save last coverage run in workspace variable -cs.set_param('CovSaveName', 'covdata'); % Last coverage run variable name -cs.set_param('CovNameIncrementing', 'off'); % Increment cvdata variable name with each simulation -cs.set_param('CovReportOnPause', 'on'); % Update coverage results on pause -cs.set_param('CovHtmlReporting', 'on'); % Generate coverage report -cs.set_param('CovHTMLOptions', ''); % Coverage report options -cs.set_param('CovCumulativeReport', 'off'); % Include cumulative data in coverage report -cs.set_param('CovCompData', ''); % Additional data to include in coverage report -cs.set_param('CovForceBlockReductionOff', 'on'); % Force block reduction off -cs.set_param('CovUseTimeInterval', 'off'); % Restrict coverage recording interval -cs.set_param('CovBoundaryAbsTol', 1E-05); % Relational boundary coverage absolute tolerance -cs.set_param('CovBoundaryRelTol', 0.01); % Relational boundary coverage relative tolerance -cs.set_param('CovFilter', ''); % Coverage filter filename diff --git a/gnc/matlab/astrobee_control_init.m b/gnc/matlab/astrobee_control_init.m deleted file mode 100644 index 985109e6e3..0000000000 --- a/gnc/matlab/astrobee_control_init.m +++ /dev/null @@ -1,76 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Control sim initialization file -% Configures parameters used in both FSW and Sim simulink models - -ab_verbose = true; %when true, displays output messages -% ab_verbose = false; - -%% Configuration -if(ab_verbose) - disp('Starting Astrobee...'); -end -astrobee_set_path; - -astrobee_stop_time = 5*60; -astrobee_time_step_size = 0.016; % Simulator sample time, .016 -astrobee_fsw_step_size = 0.016; %Used for 1/z functions - -%% init scripts -if(ab_verbose) - disp('...loading init parameters'); -end - - -% physical properties -abp_astrobee_physical_properties_init; - -% Estimator init files -ase_augmented_state_estimator_init; -ctl_controller_init; -fam_force_allocation_module_init; - -% Sim init files -ini_sim_initial_conditions_init; -bpm_blower_propulsion_module_init; -epson_imu_model_init; -env_environment_model_init; -cvs_computer_vision_system_init; -mlp_mid_level_processor_init; - -%% Busses -% Needs to be after the init files as data types are defined in the data -% files -if(ab_verbose) - disp('...loading bus definitions'); -end -bus_defs; -bus_init; - -%% FSW init files, blows away any overlapping values -astrobee_version = 'flight'; - -if(ab_verbose) - disp('...loading tunable parameters'); -end -tunable_init; - -%% -if(ab_verbose) - disp('Astrobee Init Complete'); -end \ No newline at end of file diff --git a/gnc/matlab/astrobee_control_sim.slx b/gnc/matlab/astrobee_control_sim.slx deleted file mode 100644 index bf44b3c8f3..0000000000 Binary files a/gnc/matlab/astrobee_control_sim.slx and /dev/null differ diff --git a/gnc/matlab/astrobee_gen_code.sh b/gnc/matlab/astrobee_gen_code.sh deleted file mode 100755 index 0efb27bd3c..0000000000 --- a/gnc/matlab/astrobee_gen_code.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash - -# This works when the alias for matlab 2016b is setup as matlab-2016b -matlab-2016b -nodisplay -nodesktop -r "astrobee; build_Proto4; exit" diff --git a/gnc/matlab/astrobee_init.m b/gnc/matlab/astrobee_init.m deleted file mode 100644 index 3b8c9da1f2..0000000000 --- a/gnc/matlab/astrobee_init.m +++ /dev/null @@ -1,84 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% master initialization file -% Configures parameters used in both FSW and Sim simulink models - -ab_verbose = true; %when true, displays output messages -% ab_verbose = false; - -%% Configuration -if(ab_verbose) - disp('Starting Astrobee...'); -end -astrobee_set_path; - -astrobee_stop_time = 5*60; -astrobee_time_step_size = 0.016; % Simulator sample time, .016 -astrobee_fsw_step_size = 0.016; %Used for 1/z functions - -%% init scripts -if(ab_verbose) - disp('...loading init parameters'); -end - - -% physical properties -abp_astrobee_physical_properties_init; - -% Estimator init files -ase_augmented_state_estimator_init; -ctl_controller_init; -fam_force_allocation_module_init; - -% Sim init files -ini_sim_initial_conditions_init; -bpm_blower_propulsion_module_init; -epson_imu_model_init; -env_environment_model_init; -cvs_computer_vision_system_init; -mlp_mid_level_processor_init; - -%% Busses -% Needs to be after the init files as data types are defined in the data -% files -if(ab_verbose) - disp('...loading bus definitions'); -end -bus_defs; -bus_init; - -%% FSW init files, blows away any overlapping values -astrobee_version = 'flight'; - -if(ab_verbose) - disp('...loading tunable parameters'); -end -tunable_init; - -%% check if handcoded cpp files have changed, and need to regenerate the mex files -if(ab_verbose) - disp('...compiling cpp functions'); -end -curr_dir = pwd; -cxx_blocks_config(ASTROBEE_ROOT, ase_of_num_aug, ase_of_num_features, ab_verbose); -cd(curr_dir); - -%% -if(ab_verbose) - disp('Astrobee Init Complete'); -end \ No newline at end of file diff --git a/gnc/matlab/astrobee_prep.m b/gnc/matlab/astrobee_prep.m deleted file mode 100644 index a70535192b..0000000000 --- a/gnc/matlab/astrobee_prep.m +++ /dev/null @@ -1,52 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Astrobee master prep file. Configures parameters used in the astrobee simulink model. -% Dependencies: -% astrobee_init.m - -%% Load Configuration Parameters -cset = astrobee_configuration; % Calls mfile that sets Simulink model configuration parameters -astrobee_config_set_ref = Simulink.ConfigSetRef; % Create configuration reference -astrobee_config_set_ref.WSVarName = 'cset'; -astrobee_config_set_ref.Name = 'astrobee_config_set_ref'; - -open_sys = find_system('SearchDepth', 0); % Collect the open simulink diagrams - -% Apply the configuration set to the astrobee model -%end - -% Note: save changes to the config set, DO NOT DO THIS FROM A COMPUTER WITHOUT SIMULINK CODER/ERT -% cset.saveAs('astrobee_configuration.m') - -%% general -% abp_astrobee_physical_properties_prep; %removed since no parameters -if tun_debug_ctl_use_truth - warning('Simulation is using truth data (with no noise injected)') -end - -%% Sim prep -bpm_blower_propulsion_module_prep; -epson_imu_model_prep; -env_environment_model_prep; -cvs_computer_vision_system_prep; -mlp_mid_level_processor_prep; - -%% FSW prep -% ctl_controller_prep; %All values moved to lua config file -fam_force_allocation_module_prep; -ase_augmented_state_estimator_prep; diff --git a/gnc/matlab/astrobee_set_path.m b/gnc/matlab/astrobee_set_path.m deleted file mode 100644 index 23b715d05a..0000000000 --- a/gnc/matlab/astrobee_set_path.m +++ /dev/null @@ -1,25 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% astrobee_set_path -% Sets file paths machine-indepedently - -ASTROBEE_ROOT = fileparts(mfilename('fullpath')); -ASTROBEE_PATHS = genpath(ASTROBEE_ROOT); -ASTROBEE_PATHS = regexprep(ASTROBEE_PATHS,':[^:]*slprj[^:]*',''); -ASTROBEE_PATHS = regexprep(ASTROBEE_PATHS,':[^:]*_rtw[^:]*',''); -addpath(regexprep(ASTROBEE_PATHS,':[^:]*slprj[^:]*','')); diff --git a/gnc/matlab/bus_defs.m b/gnc/matlab/bus_defs.m deleted file mode 100644 index fc0a411abc..0000000000 --- a/gnc/matlab/bus_defs.m +++ /dev/null @@ -1,304 +0,0 @@ -function cellInfo = bus_defs(varargin) -% BUS_DEFS returns a cell array containing bus object information -% -% Optional Input: 'false' will suppress a call to Simulink.Bus.cellToObject -% when the MATLAB file is executed. -% The order of bus element attributes is as follows: -% ElementName, Dimensions, DataType, SampleTime, Complexity, SamplingMode, DimensionsMode, Min, Max, DocUnits, Description - -suppressObject = false; -if nargin == 1 && islogical(varargin{1}) && varargin{1} == false - suppressObject = true; -elseif nargin > 1 - error('Invalid input argument(s) encountered'); -end - -cellInfo = { ... - { ... - 'act_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'act_timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'act_timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'act_impeller_speed_cmd', 2, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'act_servo_pwm_cmd', 12, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'act_nozzle_theta', 12, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'act_predicted_force_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'act_predicted_torque_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'bpm_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'bpm_timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'bpm_timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'bpm_motor_curr', 2, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'bpm_servo_curr', 12, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'bpm_torque_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'bpm_force_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'bpm_motor_speed', 2, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'bpm_nozzle_theta', 12, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'cmc_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'cmc_state_cmd_a', 1, 'Bus: cmc_state_cmd', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cmc_state_cmd_b', 1, 'Bus: cmc_state_cmd', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cmc_mode_cmd', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', sprintf('0 - Idle\n1 - Stopping\n2 - Nominal')}; ... -{'speed_gain_cmd', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'localization_mode_cmd', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', sprintf('0 - Nominal\n1 - Docking\n2 - Perching')}; ... -{'att_kp', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'att_ki', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'omega_kd', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'pos_kp', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'pos_ki', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'vel_kd', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'center_of_mass', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'inertia_matrix', [3 3], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'mass', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'cmc_state_cmd', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'P_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'V_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'A_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'quat_ISS2B', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'omega_B_ISS_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'alpha_B_ISS_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'cmd_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'cmd_timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cmd_timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cmd_mode', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'speed_gain_cmd', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cmd_B_inuse', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_pos', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_vel', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_accel', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_quat', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_omega', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_alpha', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'ctl_input_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'est_quat_ISS2B', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'est_omega_B_ISS_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'est_V_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'est_P_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'est_confidence', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cmd_state_a', 1, 'Bus: cmc_state_cmd', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cmd_state_b', 1, 'Bus: cmc_state_cmd', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'ctl_mode_cmd', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'current_time_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'current_time_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'speed_gain_cmd', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'att_kp', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'att_ki', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'omega_kd', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'pos_kp', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'pos_ki', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'vel_kd', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'inertia_matrix', [3 3], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'mass', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'ctl_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'body_force_cmd', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'body_accel_cmd', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'pos_err', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'pos_err_int', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'body_torque_cmd', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'body_alpha_cmd', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'att_err', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'att_err_mag', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'att_err_int', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'ctl_status', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_error_pos', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_error_att', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_error_vel', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'traj_error_omega', 1, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'cvs_handrail_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'cvs_timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_landmarks', [50 3], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_observations', [50 3], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_valid_flag', 50, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_3d_knowledge_flag', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', sprintf('Flag that allows the estimator to infer knowledge along the handrail.')}; ... -{'cvs_handrail_local_pos', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_handrail_local_quat', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_handrail_update_global_pose_flag', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'cvs_landmark_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'cvs_timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_landmarks', [50 3], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_observations', [50 2], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_valid_flag', 50, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'cvs_optical_flow_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'cvs_timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_observations', [50 2 16], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_valid_flag', [50 16], 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_id_tag', 50, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'cvs_registration_pulse', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'cvs_ar_tag_pulse', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_landmark_pulse', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_optical_flow_pulse', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'cvs_handrail_pulse', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'env_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'P_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('meters'), ''}; ... -{'V_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('m/s'), ''}; ... -{'A_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('m/s/s'), ''}; ... -{'A_B_ISS_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('m/s/s'), ''}; ... -{'A_B_ECI_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'Q_ISS2B', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'omega_B_ISS_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('rad/sec'), ''}; ... -{'omega_B_ECI_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'alpha_B_ISS_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('rad/s/s'), ''}; ... -{'fan_torques_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('Nm'), sprintf('Fan torques in the body frame')}; ... -{'fan_forces_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('N'), sprintf('Fan forces in the body frame')}; ... - } ... - } ... - { ... - 'ex_time_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'imu_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'imu_timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'imu_timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'imu_A_B_ECI_sensor', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('m/s^2'), sprintf('IMU accelerometer measurement')}; ... -{'imu_accel_bias', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'imu_omega_B_ECI_sensor', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('rad/s'), sprintf('IMU angular rate')}; ... -{'imu_gyro_bias', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'imu_validity_flag', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], sprintf('none'), sprintf('flag or status field. Not sure yet.')}; ... -{'imu_sat_flag', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... - { ... - 'kfl_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'quat_ISS2B', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'omega_B_ISS_B', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'gyro_bias', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'V_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'A_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'accel_bias', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'P_B_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'confidence', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'aug_state_enum', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'ml_quat_ISS2cam', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'ml_P_cam_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'of_quat_ISS2cam', [16 4], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', sprintf('Dimensions should equal [ase_of_num_aug 4]')}; ... -{'of_P_cam_ISS_ISS', [16 3], 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', sprintf('Dimensions should equal [ase_of_num_aug 4]')}; ... -{'cov_diag', 117, 'ase_cov_datatype', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'kfl_status', 1, 'uint16', -1, 'real', 'Sample', 'Fixed', [], [], '', sprintf('Bit masked status byte')}; ... -{'update_OF_tracks_cnt', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'update_ML_features_cnt', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'of_mahal_distance', 50, 'double', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'ml_mahal_distance', 50, 'double', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'hr_P_hr_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'hr_quat_ISS2hr', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'P_EST_ISS_ISS', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', sprintf('Position of the estimator frame relative to the ISS frame in the ISS frame')}; ... - } ... - } ... - { ... - 'vis_msg', ... - '', ... - '', ... - 'Auto', ... - '-1', {... -{'vis_timestamp_sec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'vis_timestamp_nsec', 1, 'uint32', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'vis_pos', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'vis_quat', 4, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'vis_lin_vel', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'vis_ang_vel', 3, 'single', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... -{'vis_validity_flag', 1, 'uint8', -1, 'real', 'Sample', 'Fixed', [], [], '', ''}; ... - } ... - } ... -}'; - -if ~suppressObject - % Create bus objects in the MATLAB base workspace - Simulink.Bus.cellToObject(cellInfo) -end diff --git a/gnc/matlab/bus_init.m b/gnc/matlab/bus_init.m deleted file mode 100644 index 379621c0a4..0000000000 --- a/gnc/matlab/bus_init.m +++ /dev/null @@ -1,22 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Defines bus initial conditions for bus creators - -% Bus definitions -bus_cmc_msg_ic = Simulink.Bus.createMATLABStruct('cmc_msg'); -bus_ctl_input_msg_ic = Simulink.Bus.createMATLABStruct('ctl_input_msg'); \ No newline at end of file diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/blower_aerodynamics.cpp b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/blower_aerodynamics.cpp deleted file mode 100644 index 374269695c..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/blower_aerodynamics.cpp +++ /dev/null @@ -1,164 +0,0 @@ -// -// File: blower_aerodynamics.cpp -// -// Code generated for Simulink model 'bpm_blower_1_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:46:28 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "blower_aerodynamics.h" - -// Include model header file for global data -#include "bpm_blower_1_propulsion_module.h" -#include "bpm_blower_1_propulsion_module_private.h" - -// System initialize for atomic system: '/blower_aerodynamics' -void bpm_bl_blower_aerodynamics_Init(DW_blower_aerodynamics_bpm_bl_T *localDW, - P_blower_aerodynamics_bpm_blo_T *localP, real_T rtp_noz_randn_seed) -{ - uint32_T tseed; - int32_T r; - int32_T t; - real_T y1; - int32_T i; - for (i = 0; i < 6; i++) { - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - localDW->DiscreteTimeIntegrator_DSTATE[i] = - localP->DiscreteTimeIntegrator_IC; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(rtp_noz_randn_seed + rtCP_random_noise_rtw_collapsed[i]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * localP->random_noise_StdDev[i] + - localP->random_noise_Mean; - localDW->NextOutput[i] = y1; - localDW->RandSeed[i] = tseed; - - // End of InitializeConditions for RandomNumber: '/random_noise' - } -} - -// Output and update for atomic system: '/blower_aerodynamics' -void bpm_blower__blower_aerodynamics(real32_T rtu_rotor_speed, const real32_T - rtu_nozzle_areas[6], B_blower_aerodynamics_bpm_blo_T *localB, - DW_blower_aerodynamics_bpm_bl_T *localDW, P_blower_aerodynamics_bpm_blo_T - *localP, real32_T rtp_imp_zero_thrust_area, real32_T - rtp_imp_zero_thrust_area_error, real32_T rtp_noise_on_flag, const real32_T - rtp_noz_cd[6], const real32_T rtp_noz_cd_error[6], const real32_T - rtp_imp_cdp_lookup[334], const real32_T rtp_imp_area_lookup[334], real32_T - rtp_imp_diameter, real32_T rtp_const_air_den, real32_T rtp_noz_thrust_sf, - const real_T rtp_noz_thrust_noise_feedback[6]) -{ - real32_T rtb_walking_bias[6]; - real32_T rtb_Product2_i[6]; - real32_T rtb_Cdp; - int32_T i; - for (i = 0; i < 6; i++) { - // Sum: '/Add3' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant8' - // Gain: '/Gain1' - - rtb_Cdp = rtp_noise_on_flag * rtp_noz_cd_error[i] + rtp_noz_cd[i]; - - // Product: '/Product2' - rtb_Product2_i[i] = rtb_Cdp * rtu_nozzle_areas[i]; - - // Sum: '/Add3' - rtb_walking_bias[i] = rtb_Cdp; - } - - // Sum: '/Sum of Elements1' - rtb_Cdp = rtb_Product2_i[0]; - for (i = 0; i < 5; i++) { - rtb_Cdp += rtb_Product2_i[(int32_T)(i + 1)]; - } - - // End of Sum: '/Sum of Elements1' - - // Sum: '/Add' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant6' - // Gain: '/Gain2' - - rtb_Cdp += rtp_noise_on_flag * rtp_imp_zero_thrust_area_error + - rtp_imp_zero_thrust_area; - - // Lookup_n-D: '/bpm_Cdp_lookup' - rtb_Cdp = look1_iflf_binlxpw(rtb_Cdp, rtp_imp_area_lookup, rtp_imp_cdp_lookup, - 333U); - - // Product: '/Product3' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - - rtb_Cdp = rtu_rotor_speed * rtu_rotor_speed * rtb_Cdp * rtp_imp_diameter * - rtp_imp_diameter * rtp_const_air_den; - - // Constant: '/Constant7' - localB->Constant7 = localP->Constant7_Value; - for (i = 0; i < 6; i++) { - // Sum: '/Add1' incorporates: - // Constant: '/Constant4' - // Constant: '/Constant9' - // DataTypeConversion: '/Data Type Conversion' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Product: '/Product1' - // Product: '/Product6' - // Product: '/Product7' - - localB->Add1[i] = localP->Constant4_Value * rtb_walking_bias[i] * - rtb_walking_bias[i] * rtb_Cdp * rtu_nozzle_areas[i] * rtp_noz_thrust_sf + - (real32_T)localDW->DiscreteTimeIntegrator_DSTATE[i]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain3' - // RandomNumber: '/random_noise' - // Sum: '/Sum' - - localDW->DiscreteTimeIntegrator_DSTATE[i] += (localDW->NextOutput[i] - - rtp_noz_thrust_noise_feedback[i] * localDW-> - DiscreteTimeIntegrator_DSTATE[i]) * localP->DiscreteTimeIntegrator_gainval; - - // Update for RandomNumber: '/random_noise' - localDW->NextOutput[i] = rt_nrand_Upu32_Yd_f_pw_snf(&localDW->RandSeed[i]) * - localP->random_noise_StdDev[i] + localP->random_noise_Mean; - } -} - -// Termination for atomic system: '/blower_aerodynamics' -void bpm_bl_blower_aerodynamics_Term(void) -{ -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/blower_aerodynamics.h b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/blower_aerodynamics.h deleted file mode 100644 index 27517bc88e..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/blower_aerodynamics.h +++ /dev/null @@ -1,82 +0,0 @@ -// -// File: blower_aerodynamics.h -// -// Code generated for Simulink model 'bpm_blower_1_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:46:28 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_blower_aerodynamics_h_ -#define RTW_HEADER_blower_aerodynamics_h_ -#include -#ifndef bpm_blower_1_propulsion_module_COMMON_INCLUDES_ -# define bpm_blower_1_propulsion_module_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#endif // bpm_blower_1_propulsion_module_COMMON_INCLUDES_ - -#include "bpm_blower_1_propulsion_module_types.h" -#include "look1_iflf_binlxpw.h" -#include "rt_nonfinite.h" -#include "rt_nrand_Upu32_Yd_f_pw_snf.h" - -// Block signals for system '/blower_aerodynamics' -typedef struct { - real32_T Add1[6]; // '/Add1' - real32_T Constant7; // '/Constant7' -} B_blower_aerodynamics_bpm_blo_T; - -// Block states (auto storage) for system '/blower_aerodynamics' -typedef struct { - real_T DiscreteTimeIntegrator_DSTATE[6];// '/Discrete-Time Integrator' - real_T NextOutput[6]; // '/random_noise' - uint32_T RandSeed[6]; // '/random_noise' -} DW_blower_aerodynamics_bpm_bl_T; - -// Parameters for system: '/blower_aerodynamics' -struct P_blower_aerodynamics_bpm_blo_T_ { - real_T DiscreteTimeIntegrator_gainval;// Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - real_T DiscreteTimeIntegrator_IC; // Expression: 0 - // Referenced by: '/Discrete-Time Integrator' - - real_T random_noise_Mean; // Expression: 0 - // Referenced by: '/random_noise' - - real_T random_noise_StdDev[6]; // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - real32_T Constant4_Value; // Expression: single(2) - // Referenced by: '/Constant4' - - real32_T Constant7_Value; // Expression: single(0) - // Referenced by: '/Constant7' - -}; - -void bpm_bl_blower_aerodynamics_Init(DW_blower_aerodynamics_bpm_bl_T *localDW, - P_blower_aerodynamics_bpm_blo_T *localP, real_T rtp_noz_randn_seed); -void bpm_blower__blower_aerodynamics(real32_T rtu_rotor_speed, const real32_T - rtu_nozzle_areas[6], B_blower_aerodynamics_bpm_blo_T *localB, - DW_blower_aerodynamics_bpm_bl_T *localDW, P_blower_aerodynamics_bpm_blo_T - *localP, real32_T rtp_imp_zero_thrust_area, real32_T - rtp_imp_zero_thrust_area_error, real32_T rtp_noise_on_flag, const real32_T - rtp_noz_cd[6], const real32_T rtp_noz_cd_error[6], const real32_T - rtp_imp_cdp_lookup[334], const real32_T rtp_imp_area_lookup[334], real32_T - rtp_imp_diameter, real32_T rtp_const_air_den, real32_T rtp_noz_thrust_sf, - const real_T rtp_noz_thrust_noise_feedback[6]); - -#endif // RTW_HEADER_blower_aerodynamics_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module.cpp b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module.cpp deleted file mode 100644 index 03b46c8a3c..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module.cpp +++ /dev/null @@ -1,1385 +0,0 @@ -// -// File: bpm_blower_1_propulsion_module.cpp -// -// Code generated for Simulink model 'bpm_blower_1_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:46:28 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "bpm_blower_1_propulsion_module.h" -#include "bpm_blower_1_propulsion_module_private.h" - -// Model step function -void bpm_blower_1_propulsion_module_step(RT_MODEL_bpm_blower_1_propuls_T *const - bpm_blower_1_propulsion_modu_M, real32_T - bpm_blower_1_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - bpm_blower_1_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_1_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_1_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_1_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_1_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_1_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_1_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_1_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_1_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_1_propulsion_modul_Y_meas_motor_speed) -{ - P_bpm_blower_1_propulsion_mod_T *bpm_blower_1_propulsion_modul_P = - ((P_bpm_blower_1_propulsion_mod_T *) - bpm_blower_1_propulsion_modu_M->defaultParam); - B_bpm_blower_1_propulsion_mod_T *bpm_blower_1_propulsion_modul_B = - ((B_bpm_blower_1_propulsion_mod_T *) bpm_blower_1_propulsion_modu_M->blockIO); - DW_bpm_blower_1_propulsion_mo_T *bpm_blower_1_propulsion_modu_DW = - ((DW_bpm_blower_1_propulsion_mo_T *) bpm_blower_1_propulsion_modu_M->dwork); - - // local scratch DWork variables - int32_T ForEach_itr; - real32_T nozzle_moment_arm[18]; - real_T b[18]; - int32_T ibmat; - int32_T itilerow; - real32_T c[18]; - real32_T rtb_current[6]; - real32_T rtb_IntegralGain_p; - real32_T rtb_Sum_j; - real32_T rtb_SignPreIntegrator_j; - real32_T rtb_FilterCoefficient_h; - boolean_T rtb_NotEqual_m; - real32_T rtb_Gain1_ii[6]; - real32_T rtb_IntegralGain[6]; - real32_T rtb_SignDeltaU[6]; - real32_T rtb_FilterCoefficient[6]; - boolean_T rtb_NotEqual[6]; - real32_T rtb_Add1[3]; - real32_T rtb_Assignment[9]; - real32_T rtb_Gain_n; - real32_T rtb_ImpAsg_InsertedFor_rotated_[18]; - real_T rtb_Sum2[3]; - real32_T Backlash1[6]; - real32_T tmp; - int32_T i; - real32_T tmp_0[9]; - real32_T tmp_1[3]; - real32_T tmp_2[9]; - real32_T rtb_Switch_d_0[9]; - real32_T rtb_Assignment_0[9]; - real32_T rtb_SignDeltaU_b; - real32_T rtb_IntegralGain_a; - real32_T rtb_SignDeltaU_1; - int8_T rtb_SignPreIntegrator_g; - const real32_T *rtb_Switch_d_1; - - // Outputs for Atomic SubSystem: '/speed_controller' - // Product: '/Divide' incorporates: - // Constant: '/Constant' - // Inport: '/impeller_cmd' - - rtb_IntegralGain_p = (real32_T)bpm_blower_1_propulsion_modul_U_impeller_cmd * - bpm_blower_1_propulsion_modul_P->bpm_blower_1_propulsion_module_; - - // RateLimiter: '/Rate Limiter' - rtb_FilterCoefficient_h = rtb_IntegralGain_p - - bpm_blower_1_propulsion_modu_DW->PrevY_l; - if (rtb_FilterCoefficient_h > - bpm_blower_1_propulsion_modul_P->RateLimiter_RisingLim) { - rtb_IntegralGain_p = bpm_blower_1_propulsion_modu_DW->PrevY_l + - bpm_blower_1_propulsion_modul_P->RateLimiter_RisingLim; - } else { - if (rtb_FilterCoefficient_h < - bpm_blower_1_propulsion_modul_P->RateLimiter_FallingLim) { - rtb_IntegralGain_p = bpm_blower_1_propulsion_modu_DW->PrevY_l + - bpm_blower_1_propulsion_modul_P->RateLimiter_FallingLim; - } - } - - bpm_blower_1_propulsion_modu_DW->PrevY_l = rtb_IntegralGain_p; - - // End of RateLimiter: '/Rate Limiter' - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteTransferFcn: '/Discrete Transfer Fcn1' - - rtb_IntegralGain_p -= - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE * - bpm_blower_1_propulsion_modul_P->bpm_imp_speed_filt_num / - bpm_blower_1_propulsion_modul_P->bpm_imp_speed_filt_den; - - // Gain: '/Filter Coefficient' incorporates: - // DiscreteIntegrator: '/Filter' - // Gain: '/Derivative Gain' - // Sum: '/SumD' - - rtb_FilterCoefficient_h = (bpm_blower_1_propulsion_modul_P->bpm_imp_ctl_kd * - rtb_IntegralGain_p - bpm_blower_1_propulsion_modu_DW->Filter_DSTATE_o) * - bpm_blower_1_propulsion_modul_P->bpm_imp_ctl_filt_n; - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Integrator' - // Gain: '/Proportional Gain' - - rtb_Sum_j = (bpm_blower_1_propulsion_modul_P->bpm_imp_ctl_kp * - rtb_IntegralGain_p + - bpm_blower_1_propulsion_modu_DW->Integrator_DSTATE_k) + - rtb_FilterCoefficient_h; - - // DeadZone: '/DeadZone' - if (rtb_Sum_j > bpm_blower_1_propulsion_modul_P->bpm_imp_max_voltage) { - rtb_SignPreIntegrator_j = rtb_Sum_j - - bpm_blower_1_propulsion_modul_P->bpm_imp_max_voltage; - } else if (rtb_Sum_j >= - bpm_blower_1_propulsion_modul_P->DiscretePIDController_LowerSatu) { - rtb_SignPreIntegrator_j = 0.0F; - } else { - rtb_SignPreIntegrator_j = rtb_Sum_j - - bpm_blower_1_propulsion_modul_P->DiscretePIDController_LowerSatu; - } - - // End of DeadZone: '/DeadZone' - - // RelationalOperator: '/NotEqual' incorporates: - // Gain: '/ZeroGain' - - rtb_NotEqual_m = (bpm_blower_1_propulsion_modul_P->ZeroGain_Gain * rtb_Sum_j - != rtb_SignPreIntegrator_j); - - // Signum: '/SignDeltaU' - if (rtb_SignPreIntegrator_j < 0.0F) { - rtb_SignPreIntegrator_j = -1.0F; - } else if (rtb_SignPreIntegrator_j > 0.0F) { - rtb_SignPreIntegrator_j = 1.0F; - } else { - if (rtb_SignPreIntegrator_j == 0.0F) { - rtb_SignPreIntegrator_j = 0.0F; - } - } - - // End of Signum: '/SignDeltaU' - - // Gain: '/Integral Gain' - rtb_IntegralGain_p *= bpm_blower_1_propulsion_modul_P->bpm_imp_ctl_ki; - - // Saturate: '/Saturate' - if (rtb_Sum_j > bpm_blower_1_propulsion_modul_P->bpm_imp_max_voltage) { - rtb_Sum_j = bpm_blower_1_propulsion_modul_P->bpm_imp_max_voltage; - } else { - if (rtb_Sum_j < - bpm_blower_1_propulsion_modul_P->DiscretePIDController_LowerSatu) { - rtb_Sum_j = - bpm_blower_1_propulsion_modul_P->DiscretePIDController_LowerSatu; - } - } - - // End of Saturate: '/Saturate' - - // Switch: '/Switch2' incorporates: - // Inport: '/battery_voltage' - // RelationalOperator: '/LowerRelop1' - - if (rtb_Sum_j > bpm_blower_1_propulsion_modul_U_battery_voltage) { - rtb_Sum_j = bpm_blower_1_propulsion_modul_U_battery_voltage; - } else { - // Gain: '/Gain' - rtb_Gain_n = bpm_blower_1_propulsion_modul_P->Gain_Gain * - bpm_blower_1_propulsion_modul_U_battery_voltage; - - // Switch: '/Switch' incorporates: - // RelationalOperator: '/UpperRelop' - - if (rtb_Sum_j < rtb_Gain_n) { - rtb_Sum_j = rtb_Gain_n; - } - - // End of Switch: '/Switch' - } - - // End of Switch: '/Switch2' - - // Signum: '/SignPreIntegrator' - if (rtb_IntegralGain_p < 0.0F) { - rtb_IntegralGain_a = -1.0F; - } else if (rtb_IntegralGain_p > 0.0F) { - rtb_IntegralGain_a = 1.0F; - } else if (rtb_IntegralGain_p == 0.0F) { - rtb_IntegralGain_a = 0.0F; - } else { - rtb_IntegralGain_a = rtb_IntegralGain_p; - } - - // End of Signum: '/SignPreIntegrator' - - // DataTypeConversion: '/DataTypeConv2' - rtb_IntegralGain_a = (real32_T)floor((real_T)rtb_IntegralGain_a); - if (rtIsNaNF(rtb_IntegralGain_a) || rtIsInfF(rtb_IntegralGain_a)) { - rtb_IntegralGain_a = 0.0F; - } else { - rtb_IntegralGain_a = (real32_T)fmod((real_T)rtb_IntegralGain_a, (real_T) - 256.0F); - } - - // DataTypeConversion: '/DataTypeConv1' - if (rtb_SignPreIntegrator_j < 128.0F) { - rtb_SignPreIntegrator_g = (int8_T)rtb_SignPreIntegrator_j; - } else { - rtb_SignPreIntegrator_g = MAX_int8_T; - } - - // End of DataTypeConversion: '/DataTypeConv1' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/DataTypeConv2' - // Logic: '/AND' - // RelationalOperator: '/Equal' - - if (rtb_NotEqual_m && ((rtb_IntegralGain_a < 0.0F ? (int32_T)(int8_T)(int32_T) - -(int32_T)(int8_T)(uint8_T)-rtb_IntegralGain_a : - (int32_T)(int8_T)(uint8_T)rtb_IntegralGain_a) == - (int32_T)rtb_SignPreIntegrator_g)) { - rtb_IntegralGain_p = bpm_blower_1_propulsion_modul_P->Constant_Value; - } - - // End of Switch: '/Switch' - - // Update for DiscreteIntegrator: '/Integrator' - bpm_blower_1_propulsion_modu_DW->Integrator_DSTATE_k += - bpm_blower_1_propulsion_modul_P->Integrator_gainval * rtb_IntegralGain_p; - - // Update for DiscreteIntegrator: '/Filter' - bpm_blower_1_propulsion_modu_DW->Filter_DSTATE_o += - bpm_blower_1_propulsion_modul_P->Filter_gainval * rtb_FilterCoefficient_h; - - // End of Outputs for SubSystem: '/speed_controller' - - // Outputs for Atomic SubSystem: '/dc_motor_model' - // Gain: '/Gain5' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain4' - // Sum: '/Add' - - rtb_FilterCoefficient_h = (rtb_Sum_j - 1.0F / - bpm_blower_1_propulsion_modul_P->bpm_imp_motor_speed_k * - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE) * (1.0F / - bpm_blower_1_propulsion_modul_P->bpm_imp_motor_r); - - // Sum: '/Add1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain1' - // Gain: '/Gain6' - - rtb_IntegralGain_p = bpm_blower_1_propulsion_modul_P->bpm_imp_motor_torque_k * - rtb_FilterCoefficient_h - - bpm_blower_1_propulsion_modul_P->bpm_imp_motor_friction_coeff * - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE; - - // End of Outputs for SubSystem: '/dc_motor_model' - - // Outport: '/impeller_current' - *bpm_blower_1_propulsion_modul_Y_impeller_current = rtb_FilterCoefficient_h; - - // Outputs for Atomic SubSystem: '/servo_model' - // Backlash: '/Backlash1' - rtb_FilterCoefficient_h = - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_backlash_deadband / 2.0F; - - // Gain: '/Gain5' - rtb_SignPreIntegrator_j = 1.0F / - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_r; - for (i = 0; i < 6; i++) { - // Backlash: '/Backlash1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator4' - - if (bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] < - bpm_blower_1_propulsion_modu_DW->PrevY[i] - rtb_FilterCoefficient_h) { - rtb_Sum_j = - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] + - rtb_FilterCoefficient_h; - } else if (bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] - <= bpm_blower_1_propulsion_modu_DW->PrevY[i] + - rtb_FilterCoefficient_h) { - rtb_Sum_j = bpm_blower_1_propulsion_modu_DW->PrevY[i]; - } else { - rtb_Sum_j = - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] - - rtb_FilterCoefficient_h; - } - - // Gain: '/Gain1' - rtb_Gain_n = bpm_blower_1_propulsion_modul_P->bpm_servo_motor_gear_ratio * - rtb_Sum_j; - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant' - // Gain: '/Gain' - // Inport: '/servo_cmd' - // Sum: '/Sum' - - rtb_IntegralGain_a = (bpm_blower_1_propulsion_modul_U_servo_cmd[i] + - bpm_blower_1_propulsion_modul_P->bpm_servo_pwm2angle_bias) - * bpm_blower_1_propulsion_modul_P->bpm_servo_pwm2angle - rtb_Gain_n; - - // Gain: '/Filter Coefficient' incorporates: - // DiscreteIntegrator: '/Filter' - // Gain: '/Derivative Gain' - // Sum: '/SumD' - - tmp = (bpm_blower_1_propulsion_modul_P->bpm_servo_ctl_kd * - rtb_IntegralGain_a - bpm_blower_1_propulsion_modu_DW->Filter_DSTATE[i]) - * bpm_blower_1_propulsion_modul_P->bpm_servo_ctl_filt_n; - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Integrator' - // Gain: '/Proportional Gain' - - rtb_SignDeltaU_b = (bpm_blower_1_propulsion_modul_P->bpm_servo_ctl_kp * - rtb_IntegralGain_a + - bpm_blower_1_propulsion_modu_DW->Integrator_DSTATE[i]) + - tmp; - - // Saturate: '/Saturate' - if (rtb_SignDeltaU_b > - bpm_blower_1_propulsion_modul_P->bpm_servo_max_voltage) { - rtb_SignDeltaU_1 = bpm_blower_1_propulsion_modul_P->bpm_servo_max_voltage; - } else if (rtb_SignDeltaU_b < - bpm_blower_1_propulsion_modul_P->DiscretePIDController_LowerSa_b) - { - rtb_SignDeltaU_1 = - bpm_blower_1_propulsion_modul_P->DiscretePIDController_LowerSa_b; - } else { - rtb_SignDeltaU_1 = rtb_SignDeltaU_b; - } - - // End of Saturate: '/Saturate' - - // Gain: '/Gain5' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - // Gain: '/Gain7' - // Sum: '/Add3' - - rtb_current[i] = (rtb_SignDeltaU_1 - - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_k * - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE - [i]) * rtb_SignPreIntegrator_j; - - // Gain: '/ZeroGain' - rtb_SignDeltaU_1 = bpm_blower_1_propulsion_modul_P->ZeroGain_Gain_e * - rtb_SignDeltaU_b; - - // DeadZone: '/DeadZone' - if (rtb_SignDeltaU_b > - bpm_blower_1_propulsion_modul_P->bpm_servo_max_voltage) { - rtb_SignDeltaU_b -= bpm_blower_1_propulsion_modul_P->bpm_servo_max_voltage; - } else if (rtb_SignDeltaU_b >= - bpm_blower_1_propulsion_modul_P->DiscretePIDController_LowerSa_b) - { - rtb_SignDeltaU_b = 0.0F; - } else { - rtb_SignDeltaU_b -= - bpm_blower_1_propulsion_modul_P->DiscretePIDController_LowerSa_b; - } - - // End of DeadZone: '/DeadZone' - - // RelationalOperator: '/NotEqual' - rtb_NotEqual[i] = (rtb_SignDeltaU_1 != rtb_SignDeltaU_b); - - // Signum: '/SignDeltaU' - if (rtb_SignDeltaU_b < 0.0F) { - rtb_SignDeltaU_b = -1.0F; - } else if (rtb_SignDeltaU_b > 0.0F) { - rtb_SignDeltaU_b = 1.0F; - } else { - if (rtb_SignDeltaU_b == 0.0F) { - rtb_SignDeltaU_b = 0.0F; - } - } - - // End of Signum: '/SignDeltaU' - - // Gain: '/Integral Gain' - rtb_IntegralGain_a *= bpm_blower_1_propulsion_modul_P->bpm_servo_ctl_ki; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator4' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] += - bpm_blower_1_propulsion_modul_P->DiscreteTimeIntegrator4_gainval * - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE[i]; - - // Backlash: '/Backlash1' - Backlash1[i] = rtb_Sum_j; - - // Gain: '/Gain1' - rtb_Gain1_ii[i] = rtb_Gain_n; - - // Sum: '/Sum1' - rtb_IntegralGain[i] = rtb_IntegralGain_a; - - // Gain: '/Filter Coefficient' - rtb_FilterCoefficient[i] = tmp; - - // Sum: '/Sum' - rtb_SignDeltaU[i] = rtb_SignDeltaU_b; - } - - // Gain: '/Gain6' - rtb_SignPreIntegrator_j = 1.0F / - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_gear_box_inertia; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator4' - rtb_FilterCoefficient_h = bpm_blower_1_propulsion_modul_P->bpm_servo_max_theta - / bpm_blower_1_propulsion_modul_P->bpm_servo_motor_gear_ratio; - rtb_Sum_j = (real32_T)bpm_blower_1_propulsion_modul_P->bpm_servo_min_theta / - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_gear_ratio; - rtb_Gain_n = bpm_blower_1_propulsion_modul_P->bpm_servo_max_theta / - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_gear_ratio; - tmp = (real32_T)bpm_blower_1_propulsion_modul_P->bpm_servo_min_theta / - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_gear_ratio; - for (i = 0; i < 6; i++) { - if (bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] >= - rtb_FilterCoefficient_h) { - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] = - rtb_Gain_n; - } else { - if (bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] <= - rtb_Sum_j) { - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] = tmp; - } - } - - // Update for Backlash: '/Backlash1' - bpm_blower_1_propulsion_modu_DW->PrevY[i] = Backlash1[i]; - - // Update for DiscreteIntegrator: '/Integrator' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/DataTypeConv1' - // DataTypeConversion: '/DataTypeConv2' - // Logic: '/AND' - // RelationalOperator: '/Equal' - // Signum: '/SignPreIntegrator' - // Switch: '/Switch' - - if (rtb_IntegralGain[i] < 0.0F) { - rtb_IntegralGain_a = -1.0F; - } else if (rtb_IntegralGain[i] > 0.0F) { - rtb_IntegralGain_a = 1.0F; - } else if (rtb_IntegralGain[i] == 0.0F) { - rtb_IntegralGain_a = 0.0F; - } else { - rtb_IntegralGain_a = rtb_IntegralGain[i]; - } - - rtb_IntegralGain_a = (real32_T)floor((real_T)rtb_IntegralGain_a); - if (rtIsNaNF(rtb_IntegralGain_a) || rtIsInfF(rtb_IntegralGain_a)) { - rtb_IntegralGain_a = 0.0F; - } else { - rtb_IntegralGain_a = (real32_T)fmod((real_T)rtb_IntegralGain_a, (real_T) - 256.0F); - } - - if (rtb_SignDeltaU[i] < 128.0F) { - rtb_SignPreIntegrator_g = (int8_T)rtb_SignDeltaU[i]; - } else { - rtb_SignPreIntegrator_g = MAX_int8_T; - } - - if (rtb_NotEqual[i] && ((rtb_IntegralGain_a < 0.0F ? (int32_T)(int8_T) - (int32_T)-(int32_T)(int8_T)(uint8_T)-rtb_IntegralGain_a : (int32_T) - (int8_T)(uint8_T)rtb_IntegralGain_a) == (int32_T) - rtb_SignPreIntegrator_g)) { - rtb_IntegralGain_a = bpm_blower_1_propulsion_modul_P->Constant_Value_n; - } else { - rtb_IntegralGain_a = rtb_IntegralGain[i]; - } - - bpm_blower_1_propulsion_modu_DW->Integrator_DSTATE[i] += - bpm_blower_1_propulsion_modul_P->Integrator_gainval_f * rtb_IntegralGain_a; - - // End of Update for DiscreteIntegrator: '/Integrator' - - // Update for DiscreteIntegrator: '/Filter' - bpm_blower_1_propulsion_modu_DW->Filter_DSTATE[i] += - bpm_blower_1_propulsion_modul_P->Filter_gainval_n * - rtb_FilterCoefficient[i]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - // Gain: '/Gain11' - // Gain: '/Gain6' - // Gain: '/Gain9' - // Sum: '/Add1' - - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE[i] += - (bpm_blower_1_propulsion_modul_P->bpm_servo_motor_k * rtb_current[i] - - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_friction_coeff * - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE[i]) * - rtb_SignPreIntegrator_j * - bpm_blower_1_propulsion_modul_P->DiscreteTimeIntegrator3_gainval; - } - - // End of Outputs for SubSystem: '/servo_model' - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_SignPreIntegrator_j = - bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_1_propulsion_modul_P->bmp_PM1_impeller_orientation_error[0] + - bpm_blower_1_propulsion_modul_P->abp_pm1_impeller_orientation[0]; - - // DotProduct: '/Dot Product' - rtb_IntegralGain_a = rtb_SignPreIntegrator_j * rtb_SignPreIntegrator_j; - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_Add1[0] = rtb_SignPreIntegrator_j; - rtb_SignPreIntegrator_j = - bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_1_propulsion_modul_P->bmp_PM1_impeller_orientation_error[1] + - bpm_blower_1_propulsion_modul_P->abp_pm1_impeller_orientation[1]; - - // DotProduct: '/Dot Product' - rtb_IntegralGain_a += rtb_SignPreIntegrator_j * rtb_SignPreIntegrator_j; - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_Add1[1] = rtb_SignPreIntegrator_j; - rtb_SignPreIntegrator_j = - bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_1_propulsion_modul_P->bmp_PM1_impeller_orientation_error[2] + - bpm_blower_1_propulsion_modul_P->abp_pm1_impeller_orientation[2]; - - // DotProduct: '/Dot Product' - rtb_IntegralGain_a += rtb_SignPreIntegrator_j * rtb_SignPreIntegrator_j; - - // Sum: '/Add1' - rtb_Add1[2] = rtb_SignPreIntegrator_j; - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_FilterCoefficient_h = (real32_T)sqrt((real_T)rtb_IntegralGain_a); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Product: '/Divide' - - if ((real_T)rtb_FilterCoefficient_h > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - rtb_Add1[0] /= rtb_FilterCoefficient_h; - rtb_Add1[1] /= rtb_FilterCoefficient_h; - rtb_Add1[2] = rtb_SignPreIntegrator_j / rtb_FilterCoefficient_h; - - // End of Outputs for SubSystem: '/Normalize' - } - - // End of If: '/If' - - // Gain: '/Gain1' - rtb_Sum_j = bpm_blower_1_propulsion_modul_P->Gain1_Gain * rtb_IntegralGain_p; - - // Product: '/Product1' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant4' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain1' - // Sum: '/Add3' - - rtb_FilterCoefficient_h = - (bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_1_propulsion_modul_P->bpm_impeller_inertia_error + - bpm_blower_1_propulsion_modul_P->bpm_impeller_inertia) * - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE; - - // Outputs for Enabled SubSystem: '/latch_nozzle_thrust_matricies' incorporates: - // EnablePort: '/Enable' - - // UnitDelay: '/Unit Delay' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant5' - // Constant: '/Constant8' - // Switch: '/Switch' - - if (bpm_blower_1_propulsion_modu_DW->UnitDelay_DSTATE) { - if (bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag > - bpm_blower_1_propulsion_modul_P->Switch_Threshold) { - rtb_Switch_d_1 = - &bpm_blower_1_propulsion_modul_P->bpm_PM1_Q_nozzle2misaligned[0]; - } else { - rtb_Switch_d_1 = &bpm_blower_1_propulsion_modul_P->Constant2_Value[0]; - } - - // Outputs for Iterator SubSystem: '/For Each Subsystem' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr = 0; ForEach_itr < 6; ForEach_itr++) { - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_SignPreIntegrator_j = rtb_Switch_d_1[(int32_T)(18 + ForEach_itr)] * - rtb_Switch_d_1[(int32_T)(18 + ForEach_itr)] * - bpm_blower_1_propulsion_modul_P->CoreSubsys.Gain_Gain - (real32_T) - bpm_blower_1_propulsion_modul_P->CoreSubsys.Constant1_Value; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment[i] = (real32_T) - bpm_blower_1_propulsion_modul_P->CoreSubsys.Constant2_Value[i]; - } - - rtb_Assignment[0] = rtb_SignPreIntegrator_j; - rtb_Assignment[4] = rtb_SignPreIntegrator_j; - rtb_Assignment[8] = rtb_SignPreIntegrator_j; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - - rtb_SignPreIntegrator_j = rtb_Switch_d_1[(int32_T)(18 + ForEach_itr)] * - bpm_blower_1_propulsion_modul_P->CoreSubsys.Gain1_Gain; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_2[0] = (real32_T) - bpm_blower_1_propulsion_modul_P->CoreSubsys.Constant3_Value; - tmp_2[1] = rtb_Switch_d_1[(int32_T)(12 + ForEach_itr)]; - tmp_2[2] = rtb_Switch_d_1[(int32_T)(6 + ForEach_itr)] * - bpm_blower_1_propulsion_modul_P->CoreSubsys.Gain_Gain_a; - tmp_2[3] = rtb_Switch_d_1[(int32_T)(12 + ForEach_itr)] * - bpm_blower_1_propulsion_modul_P->CoreSubsys.Gain1_Gain_m; - tmp_2[4] = (real32_T) - bpm_blower_1_propulsion_modul_P->CoreSubsys.Constant3_Value; - tmp_2[5] = rtb_Switch_d_1[ForEach_itr]; - tmp_2[6] = rtb_Switch_d_1[(int32_T)(ForEach_itr + 6)]; - tmp_2[7] = bpm_blower_1_propulsion_modul_P->CoreSubsys.Gain2_Gain * - rtb_Switch_d_1[ForEach_itr]; - tmp_2[8] = (real32_T) - bpm_blower_1_propulsion_modul_P->CoreSubsys.Constant3_Value; - - // Product: '/Product1' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain2' - - for (i = 0; i < 3; i++) { - rtb_Switch_d_0[i] = rtb_Switch_d_1[(int32_T)((int32_T)(6 * i) + - ForEach_itr)] * rtb_Switch_d_1[ForEach_itr]; - rtb_Switch_d_0[(int32_T)(i + 3)] = rtb_Switch_d_1[(int32_T)((int32_T)(6 * - i) + ForEach_itr)] * rtb_Switch_d_1[(int32_T)(ForEach_itr + 6)]; - rtb_Switch_d_0[(int32_T)(i + 6)] = rtb_Switch_d_1[(int32_T)((int32_T)(6 * - i) + ForEach_itr)] * rtb_Switch_d_1[(int32_T)(ForEach_itr + 12)]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_0[(int32_T)(3 * i)] = (rtb_Assignment[(int32_T)(3 * i)] - - tmp_2[(int32_T)(3 * i)] * rtb_SignPreIntegrator_j) + rtb_Switch_d_0 - [(int32_T)(3 * i)] * - bpm_blower_1_propulsion_modul_P->CoreSubsys.Gain2_Gain_i; - rtb_Assignment_0[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i) + 1)] - tmp_2[(int32_T)((int32_T)(3 * i) + - 1)] * rtb_SignPreIntegrator_j) + rtb_Switch_d_0[(int32_T)((int32_T)(3 * - i) + 1)] * bpm_blower_1_propulsion_modul_P->CoreSubsys.Gain2_Gain_i; - rtb_Assignment_0[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i) + 2)] - tmp_2[(int32_T)((int32_T)(3 * i) + - 2)] * rtb_SignPreIntegrator_j) + rtb_Switch_d_0[(int32_T)((int32_T)(3 * - i) + 2)] * bpm_blower_1_propulsion_modul_P->CoreSubsys.Gain2_Gain_i; - } - - // End of Sum: '/Sum1' - for (i = 0; i < 3; i++) { - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_rotated_vectors_at_inport_0' incorporates: - // Constant: '/Constant3' - // ForEachSliceSelector: '/ImpSel_InsertedFor_unit_vectors_at_outport_0' - // Product: '/Product' - - rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(ForEach_itr + (int32_T)(6 * i))] - = rtb_Assignment_0[(int32_T)(i + 6)] * - bpm_blower_1_propulsion_modul_P->abp_PM1_nozzle_orientations[(int32_T) - (ForEach_itr + 12)] + (rtb_Assignment_0[(int32_T)(i + 3)] * - bpm_blower_1_propulsion_modul_P->abp_PM1_nozzle_orientations[(int32_T) - (ForEach_itr + 6)] + rtb_Assignment_0[i] * - bpm_blower_1_propulsion_modul_P-> - abp_PM1_nozzle_orientations[ForEach_itr]); - } - } - - // End of Outputs for SubSystem: '/For Each Subsystem' - // MATLAB Function 'bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/MATLAB Function': ':1' - // NOTE: if number of nozzles change, must change this hard coded '6' below - // ':1:5' - for (i = 0; i < 3; i++) { - // Sum: '/Sum2' incorporates: - // Constant: '/Constant7' - // Gain: '/Gain2' - // Inport: '/veh_cm' - - rtb_Sum2[i] = (real_T) - bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_1_propulsion_modul_P->abp_P_CG_B_B_error[i] + (real_T) - bpm_blower_1_propulsion_modul_U_veh_cm[i]; - - // MATLAB Function: '/MATLAB Function' - ibmat = (int32_T)(i * 6); - for (itilerow = 0; itilerow < 6; itilerow++) { - b[(int32_T)(ibmat + itilerow)] = rtb_Sum2[i]; - } - } - - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant4' - // Constant: '/Constant5' - // Constant: '/Constant8' - // Gain: '/Gain' - // Sum: '/Sum' - // Switch: '/Switch' - - for (i = 0; i < 18; i++) { - nozzle_moment_arm[i] = - (bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_1_propulsion_modul_P->bpm_PM1_P_nozzle_B_B_error[i] + - bpm_blower_1_propulsion_modul_P->abp_PM1_P_nozzle_B_B[i]) - (real32_T) - b[i]; - } - - // [m] Distance from nozzles to CG - // ':1:7' - // [-] Converts a nozzle thrust into resulting body torques - // ':1:8' - // [-] Converts a nozzle thrust into resulting body forces - for (i = 0; i < 6; i++) { - c[i] = nozzle_moment_arm[(int32_T)(i + 6)] * - rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + 12)] - nozzle_moment_arm - [(int32_T)(i + 12)] * rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + 6)]; - c[(int32_T)(i + 6)] = nozzle_moment_arm[(int32_T)(i + 12)] * - rtb_ImpAsg_InsertedFor_rotated_[i] - rtb_ImpAsg_InsertedFor_rotated_ - [(int32_T)(i + 12)] * nozzle_moment_arm[i]; - c[(int32_T)(i + 12)] = rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + 6)] * - nozzle_moment_arm[i] - nozzle_moment_arm[(int32_T)(i + 6)] * - rtb_ImpAsg_InsertedFor_rotated_[i]; - - // SignalConversion: '/OutportBufferForthrust2force_B' - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2force_B[(int32_T) - (3 * i)] = -rtb_ImpAsg_InsertedFor_rotated_[i]; - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2force_B[(int32_T) - (1 + (int32_T)(3 * i))] = -rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + - 6)]; - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2force_B[(int32_T) - (2 + (int32_T)(3 * i))] = -rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + - 12)]; - - // SignalConversion: '/OutportBufferForthrust2torque_B' - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2torque_B[(int32_T) - (3 * i)] = -c[i]; - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2torque_B[(int32_T) - (1 + (int32_T)(3 * i))] = -c[(int32_T)(i + 6)]; - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2torque_B[(int32_T) - (2 + (int32_T)(3 * i))] = -c[(int32_T)(i + 12)]; - } - } - - // End of UnitDelay: '/Unit Delay' - // End of Outputs for SubSystem: '/latch_nozzle_thrust_matricies' - - // Outputs for Atomic SubSystem: '/calc_nozzle_area' - // Gain: '/Gain12' - rtb_SignPreIntegrator_j = 1.0F / - bpm_blower_1_propulsion_modul_P->abp_nozzle_gear_ratio; - for (i = 0; i < 6; i++) { - rtb_Gain_n = rtb_SignPreIntegrator_j * rtb_Gain1_ii[i]; - - // Product: '/Product2' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant4' - // Constant: '/Constant5' - // Constant: '/Constant6' - // Product: '/Product1' - // Sum: '/Subtract' - // Sum: '/Subtract1' - // Trigonometry: '/Trigonometric Function' - - Backlash1[i] = (bpm_blower_1_propulsion_modul_P->abp_nozzle_intake_height - - (real32_T)cos((real_T)(rtb_Gain_n + - bpm_blower_1_propulsion_modul_P->abp_nozzle_min_open_angle)) * - bpm_blower_1_propulsion_modul_P->abp_nozzle_flap_length) * - bpm_blower_1_propulsion_modul_P->abp_PM1_nozzle_widths[i] * - bpm_blower_1_propulsion_modul_P->abp_nozzle_flap_count; - rtb_Gain1_ii[i] = rtb_Gain_n; - } - - // End of Gain: '/Gain12' - // End of Outputs for SubSystem: '/calc_nozzle_area' - - // Outputs for Atomic SubSystem: '/blower_aerodynamics' - - // DiscreteIntegrator: '/Discrete-Time Integrator' - bpm_blower__blower_aerodynamics - (bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE, Backlash1, - &bpm_blower_1_propulsion_modul_B->blower_aerodynamics, - &bpm_blower_1_propulsion_modu_DW->blower_aerodynamics, - (P_blower_aerodynamics_bpm_blo_T *) - &bpm_blower_1_propulsion_modul_P->blower_aerodynamics, - bpm_blower_1_propulsion_modul_P->abp_pm1_zero_thrust_area, - bpm_blower_1_propulsion_modul_P->bpm_PM1_zero_thrust_area_error, - bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag, - bpm_blower_1_propulsion_modul_P->abp_PM1_discharge_coeff, - bpm_blower_1_propulsion_modul_P->bpm_PM1_nozzle_discharge_coeff_error, - bpm_blower_1_propulsion_modul_P->bpm_lookup_Cdp_data, - bpm_blower_1_propulsion_modul_P->bpm_lookup_totalarea_breakpoints, - bpm_blower_1_propulsion_modul_P->abp_impeller_diameter, - bpm_blower_1_propulsion_modul_P->const_air_density, - bpm_blower_1_propulsion_modul_P->tun_bpm_PM1_thrust_error_sf, - bpm_blower_1_propulsion_modul_P->bpm_PM1_nozzle_noise_feedback_gain); - - // End of Outputs for SubSystem: '/blower_aerodynamics' - - // Outport: '/servo_current' - for (i = 0; i < 6; i++) { - bpm_blower_1_propulsion_modul_Y_servo_current[i] = rtb_current[i]; - } - - // End of Outport: '/servo_current' - - // SignalConversion: '/TmpSignal ConversionAtProductInport1' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Inport: '/omega_B_ECI_B' - // Product: '/Product' - - tmp_0[0] = (real32_T)bpm_blower_1_propulsion_modul_P->Constant3_Value; - tmp_0[1] = bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[2]; - tmp_0[2] = bpm_blower_1_propulsion_modul_P->Gain_Gain_h * - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[1]; - tmp_0[3] = bpm_blower_1_propulsion_modul_P->Gain1_Gain_f * - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[2]; - tmp_0[4] = (real32_T)bpm_blower_1_propulsion_modul_P->Constant3_Value; - tmp_0[5] = bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[0]; - tmp_0[6] = bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[1]; - tmp_0[7] = bpm_blower_1_propulsion_modul_P->Gain2_Gain * - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[0]; - tmp_0[8] = (real32_T)bpm_blower_1_propulsion_modul_P->Constant3_Value; - for (i = 0; i < 3; i++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - tmp_1[i] = 0.0F; - for (ibmat = 0; ibmat < 6; ibmat++) { - tmp_1[i] += - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2torque_B - [(int32_T)((int32_T)(3 * ibmat) + i)] * - bpm_blower_1_propulsion_modul_B->blower_aerodynamics.Add1[ibmat]; - } - - // End of Product: '/Product4' - - // Outport: '/torque_B' incorporates: - // Product: '/Product' - // Product: '/Product' - // Product: '/Product2' - // Sum: '/Add2' - - bpm_blower_1_propulsion_modul_Y_torque_B[i] = (((rtb_Add1[0] * - rtb_FilterCoefficient_h * tmp_0[i] + tmp_0[(int32_T)(i + 3)] * (rtb_Add1[1] - * rtb_FilterCoefficient_h)) + tmp_0[(int32_T)(i + 6)] * (rtb_Add1[2] * - rtb_FilterCoefficient_h)) + rtb_Add1[i] * rtb_Sum_j) + tmp_1[i]; - - // Outport: '/force_B' incorporates: - // Product: '/Product3' - - bpm_blower_1_propulsion_modul_Y_force_B[i] = 0.0F; - for (ibmat = 0; ibmat < 6; ibmat++) { - bpm_blower_1_propulsion_modul_Y_force_B[i] += - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2force_B[(int32_T) - ((int32_T)(3 * ibmat) + i)] * - bpm_blower_1_propulsion_modul_B->blower_aerodynamics.Add1[ibmat]; - } - - // End of Outport: '/force_B' - } - - // Outport: '/motor_speed' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - *bpm_blower_1_propulsion_modul_Y_motor_speed = - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE; - - // Outport: '/nozzle_theta' - for (i = 0; i < 6; i++) { - bpm_blower_1_propulsion_modul_Y_nozzle_theta[i] = rtb_Gain1_ii[i]; - } - - // End of Outport: '/nozzle_theta' - - // DataTypeConversion: '/Data Type Conversion' incorporates: - // Gain: '/Gain1' - // RandomNumber: '/random_noise1' - - rtb_SignPreIntegrator_j = (real32_T)(1.0 / sqrt - (bpm_blower_1_propulsion_modul_P->astrobee_time_step_size) * - bpm_blower_1_propulsion_modu_DW->NextOutput); - - // Quantizer: '/Quantizer' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain3' - // Gain: '/Gain4' - // RandomNumber: '/random_noise' - // Sum: '/Sum1' - - rtb_FilterCoefficient_h = rt_roundf_snf((((real32_T)(1.0 / sqrt - (bpm_blower_1_propulsion_modul_P->astrobee_time_step_size) * - bpm_blower_1_propulsion_modu_DW->NextOutput_e) + - bpm_blower_1_propulsion_modul_P->bpm_sensor_sf * - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE) + - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator1_DSTATE) / - bpm_blower_1_propulsion_modul_P->bpm_sensor_resolution) * - bpm_blower_1_propulsion_modul_P->bpm_sensor_resolution; - - // Outport: '/meas_motor_speed' incorporates: - // Delay: '/Delay2' - - *bpm_blower_1_propulsion_modul_Y_meas_motor_speed = - bpm_blower_1_propulsion_modu_DW->Delay2_DSTATE; - - // Sum: '/Add' - rtb_IntegralGain_p -= - bpm_blower_1_propulsion_modul_B->blower_aerodynamics.Constant7; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - // Gain: '/Gain' - // Gain: '/Gain2' - - if (bpm_blower_1_propulsion_modul_P->tun_bpm_noise_on_flag > - bpm_blower_1_propulsion_modul_P->Switch_Threshold_j) { - rtb_IntegralGain_p *= 1.0F / - bpm_blower_1_propulsion_modul_P->bpm_impeller_inertia; - } else { - rtb_IntegralGain_p *= 1.0F / - (bpm_blower_1_propulsion_modul_P->bpm_impeller_inertia + - bpm_blower_1_propulsion_modul_P->bpm_impeller_inertia_error); - } - - // End of Switch: '/Switch' - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE += - bpm_blower_1_propulsion_modul_P->DiscreteTimeIntegrator_gainval * - rtb_IntegralGain_p; - - // Update for UnitDelay: '/Unit Delay' incorporates: - // Inport: '/veh_cm' - // Logic: '/Logical Operator' - // RelationalOperator: '/FixPt Relational Operator' - // UnitDelay: '/Delay Input1' - - bpm_blower_1_propulsion_modu_DW->UnitDelay_DSTATE = - ((bpm_blower_1_propulsion_modul_U_veh_cm[0] != - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[0]) || - (bpm_blower_1_propulsion_modul_U_veh_cm[1] != - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[1]) || - (bpm_blower_1_propulsion_modul_U_veh_cm[2] != - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[2])); - - // Update for RandomNumber: '/random_noise1' - bpm_blower_1_propulsion_modu_DW->NextOutput = rt_nrand_Upu32_Yd_f_pw_snf - (&bpm_blower_1_propulsion_modu_DW->RandSeed) * - bpm_blower_1_propulsion_modul_P->random_noise1_StdDev + - bpm_blower_1_propulsion_modul_P->random_noise1_Mean; - - // Update for RandomNumber: '/random_noise' - bpm_blower_1_propulsion_modu_DW->NextOutput_e = rt_nrand_Upu32_Yd_f_pw_snf - (&bpm_blower_1_propulsion_modu_DW->RandSeed_j) * - bpm_blower_1_propulsion_modul_P->random_noise_StdDev + - bpm_blower_1_propulsion_modul_P->random_noise_Mean; - - // Saturate: '/Saturation' - if (rtb_FilterCoefficient_h > bpm_blower_1_propulsion_modul_P->bpm_sensor_max) - { - // Update for Delay: '/Delay2' - bpm_blower_1_propulsion_modu_DW->Delay2_DSTATE = - bpm_blower_1_propulsion_modul_P->bpm_sensor_max; - } else if (rtb_FilterCoefficient_h < - bpm_blower_1_propulsion_modul_P->bpm_sensor_min) { - // Update for Delay: '/Delay2' - bpm_blower_1_propulsion_modu_DW->Delay2_DSTATE = - bpm_blower_1_propulsion_modul_P->bpm_sensor_min; - } else { - // Update for Delay: '/Delay2' - bpm_blower_1_propulsion_modu_DW->Delay2_DSTATE = rtb_FilterCoefficient_h; - } - - // End of Saturate: '/Saturation' - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator1_DSTATE += - bpm_blower_1_propulsion_modul_P->DiscreteTimeIntegrator1_gainval * - rtb_SignPreIntegrator_j; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/veh_cm' - - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[0] = - bpm_blower_1_propulsion_modul_U_veh_cm[0]; - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[1] = - bpm_blower_1_propulsion_modul_U_veh_cm[1]; - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[2] = - bpm_blower_1_propulsion_modul_U_veh_cm[2]; -} - -// Model initialize function -void bpm_blower_1_propulsion_module_initialize(RT_MODEL_bpm_blower_1_propuls_T * - const bpm_blower_1_propulsion_modu_M, real32_T - *bpm_blower_1_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - *bpm_blower_1_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_1_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_1_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_1_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_1_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_1_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_1_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_1_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_1_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_1_propulsion_modul_Y_meas_motor_speed) -{ - P_bpm_blower_1_propulsion_mod_T *bpm_blower_1_propulsion_modul_P = - ((P_bpm_blower_1_propulsion_mod_T *) - bpm_blower_1_propulsion_modu_M->defaultParam); - B_bpm_blower_1_propulsion_mod_T *bpm_blower_1_propulsion_modul_B = - ((B_bpm_blower_1_propulsion_mod_T *) bpm_blower_1_propulsion_modu_M->blockIO); - DW_bpm_blower_1_propulsion_mo_T *bpm_blower_1_propulsion_modu_DW = - ((DW_bpm_blower_1_propulsion_mo_T *) bpm_blower_1_propulsion_modu_M->dwork); - - { - uint32_T tseed; - int32_T t; - real32_T y; - int32_T i; - real_T tmp; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE = - bpm_blower_1_propulsion_modul_P->bpm_impeller_init_speed; - - // InitializeConditions for UnitDelay: '/Unit Delay' - bpm_blower_1_propulsion_modu_DW->UnitDelay_DSTATE = - bpm_blower_1_propulsion_modul_P->UnitDelay_InitialCondition; - - // InitializeConditions for RandomNumber: '/random_noise1' - tmp = floor(bpm_blower_1_propulsion_modul_P->bpm_sensor_rand_seed + 1.0); - if (rtIsNaN(tmp) || rtIsInf(tmp)) { - tmp = 0.0; - } else { - tmp = fmod(tmp, 4.294967296E+9); - } - - tseed = tmp < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-tmp : (uint32_T) - tmp; - i = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)i << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)i); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - bpm_blower_1_propulsion_modu_DW->RandSeed = tseed; - bpm_blower_1_propulsion_modu_DW->NextOutput = rt_nrand_Upu32_Yd_f_pw_snf - (&bpm_blower_1_propulsion_modu_DW->RandSeed) * - bpm_blower_1_propulsion_modul_P->random_noise1_StdDev + - bpm_blower_1_propulsion_modul_P->random_noise1_Mean; - - // End of InitializeConditions for RandomNumber: '/random_noise1' - - // InitializeConditions for RandomNumber: '/random_noise' - tmp = floor(bpm_blower_1_propulsion_modul_P->bpm_sensor_rand_seed); - if (rtIsNaN(tmp) || rtIsInf(tmp)) { - tmp = 0.0; - } else { - tmp = fmod(tmp, 4.294967296E+9); - } - - tseed = tmp < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-tmp : (uint32_T) - tmp; - i = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)i << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)i); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - bpm_blower_1_propulsion_modu_DW->RandSeed_j = tseed; - bpm_blower_1_propulsion_modu_DW->NextOutput_e = rt_nrand_Upu32_Yd_f_pw_snf - (&bpm_blower_1_propulsion_modu_DW->RandSeed_j) * - bpm_blower_1_propulsion_modul_P->random_noise_StdDev + - bpm_blower_1_propulsion_modul_P->random_noise_Mean; - - // End of InitializeConditions for RandomNumber: '/random_noise' - - // InitializeConditions for Delay: '/Delay2' - bpm_blower_1_propulsion_modu_DW->Delay2_DSTATE = - bpm_blower_1_propulsion_modul_P->Delay2_InitialCondition; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator1_DSTATE = - bpm_blower_1_propulsion_modul_P->DiscreteTimeIntegrator1_IC; - - // InitializeConditions for UnitDelay: '/Delay Input1' - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[0] = - bpm_blower_1_propulsion_modul_P->DetectChange_vinit; - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[1] = - bpm_blower_1_propulsion_modul_P->DetectChange_vinit; - bpm_blower_1_propulsion_modu_DW->DelayInput1_DSTATE[2] = - bpm_blower_1_propulsion_modul_P->DetectChange_vinit; - - // SystemInitialize for Atomic SubSystem: '/speed_controller' - // InitializeConditions for RateLimiter: '/Rate Limiter' - bpm_blower_1_propulsion_modu_DW->PrevY_l = - bpm_blower_1_propulsion_modul_P->RateLimiter_IC; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - bpm_blower_1_propulsion_modu_DW->Integrator_DSTATE_k = - bpm_blower_1_propulsion_modul_P->Integrator_IC; - - // InitializeConditions for DiscreteIntegrator: '/Filter' - bpm_blower_1_propulsion_modu_DW->Filter_DSTATE_o = - bpm_blower_1_propulsion_modul_P->Filter_IC; - - // End of SystemInitialize for SubSystem: '/speed_controller' - - // SystemInitialize for Atomic SubSystem: '/servo_model' - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator4' - y = (real32_T)bpm_blower_1_propulsion_modul_P->bpm_servo_min_theta / - bpm_blower_1_propulsion_modul_P->bpm_servo_motor_gear_ratio; - for (i = 0; i < 6; i++) { - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] = y; - - // InitializeConditions for Backlash: '/Backlash1' - bpm_blower_1_propulsion_modu_DW->PrevY[i] = - bpm_blower_1_propulsion_modul_P->Backlash1_InitialOutput; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - bpm_blower_1_propulsion_modu_DW->Integrator_DSTATE[i] = - bpm_blower_1_propulsion_modul_P->Integrator_IC_n; - - // InitializeConditions for DiscreteIntegrator: '/Filter' - bpm_blower_1_propulsion_modu_DW->Filter_DSTATE[i] = - bpm_blower_1_propulsion_modul_P->Filter_IC_g; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator3' - bpm_blower_1_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE[i] = - bpm_blower_1_propulsion_modul_P->DiscreteTimeIntegrator3_IC; - } - - // End of InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator4' - // End of SystemInitialize for SubSystem: '/servo_model' - - // SystemInitialize for Enabled SubSystem: '/latch_nozzle_thrust_matricies' - for (i = 0; i < 18; i++) { - // SystemInitialize for Outport: '/thrust2torque_B' - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2torque_B[i] = - bpm_blower_1_propulsion_modul_P->thrust2torque_B_Y0; - - // SystemInitialize for Outport: '/thrust2force_B' - bpm_blower_1_propulsion_modul_B->OutportBufferForthrust2force_B[i] = - bpm_blower_1_propulsion_modul_P->thrust2force_B_Y0; - } - - // End of SystemInitialize for SubSystem: '/latch_nozzle_thrust_matricies' - - // SystemInitialize for Atomic SubSystem: '/blower_aerodynamics' - bpm_bl_blower_aerodynamics_Init - (&bpm_blower_1_propulsion_modu_DW->blower_aerodynamics, - (P_blower_aerodynamics_bpm_blo_T *) - &bpm_blower_1_propulsion_modul_P->blower_aerodynamics, - bpm_blower_1_propulsion_modul_P->bpm_PM1_randn_noise_seed); - - // End of SystemInitialize for SubSystem: '/blower_aerodynamics' - } -} - -// Model terminate function -void bpm_blower_1_propulsion_module_terminate(RT_MODEL_bpm_blower_1_propuls_T - * bpm_blower_1_propulsion_modu_M) -{ - // model code - rt_FREE(bpm_blower_1_propulsion_modu_M->blockIO); - if (bpm_blower_1_propulsion_modu_M->paramIsMalloced) { - rt_FREE(bpm_blower_1_propulsion_modu_M->defaultParam); - } - - rt_FREE(bpm_blower_1_propulsion_modu_M->dwork); - rt_FREE(bpm_blower_1_propulsion_modu_M); -} - -// Model data allocation function -RT_MODEL_bpm_blower_1_propuls_T *bpm_blower_1_propulsion_module(real32_T - *bpm_blower_1_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - *bpm_blower_1_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_1_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_1_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_1_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_1_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_1_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_1_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_1_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_1_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_1_propulsion_modul_Y_meas_motor_speed) -{ - RT_MODEL_bpm_blower_1_propuls_T *bpm_blower_1_propulsion_modu_M; - bpm_blower_1_propulsion_modu_M = (RT_MODEL_bpm_blower_1_propuls_T *) malloc - (sizeof(RT_MODEL_bpm_blower_1_propuls_T)); - if (bpm_blower_1_propulsion_modu_M == NULL) { - return NULL; - } - - (void) memset((char *)bpm_blower_1_propulsion_modu_M, 0, - sizeof(RT_MODEL_bpm_blower_1_propuls_T)); - - // block I/O - { - B_bpm_blower_1_propulsion_mod_T *b = (B_bpm_blower_1_propulsion_mod_T *) - malloc(sizeof(B_bpm_blower_1_propulsion_mod_T)); - rt_VALIDATE_MEMORY(bpm_blower_1_propulsion_modu_M,b); - bpm_blower_1_propulsion_modu_M->blockIO = (b); - } - - // parameters - { - P_bpm_blower_1_propulsion_mod_T *p; - static int_T pSeen = 0; - - // only malloc on multiple model instantiation - if (pSeen == 1 ) { - p = (P_bpm_blower_1_propulsion_mod_T *) malloc(sizeof - (P_bpm_blower_1_propulsion_mod_T)); - rt_VALIDATE_MEMORY(bpm_blower_1_propulsion_modu_M,p); - (void) memcpy(p, &bpm_blower_1_propulsion_modul_P, - sizeof(P_bpm_blower_1_propulsion_mod_T)); - bpm_blower_1_propulsion_modu_M->paramIsMalloced = (true); - } else { - p = &bpm_blower_1_propulsion_modul_P; - bpm_blower_1_propulsion_modu_M->paramIsMalloced = (false); - pSeen = 1; - } - - bpm_blower_1_propulsion_modu_M->defaultParam = (p); - } - - // states (dwork) - { - DW_bpm_blower_1_propulsion_mo_T *dwork = (DW_bpm_blower_1_propulsion_mo_T *) - malloc(sizeof(DW_bpm_blower_1_propulsion_mo_T)); - rt_VALIDATE_MEMORY(bpm_blower_1_propulsion_modu_M,dwork); - bpm_blower_1_propulsion_modu_M->dwork = (dwork); - } - - { - P_bpm_blower_1_propulsion_mod_T *bpm_blower_1_propulsion_modul_P = - ((P_bpm_blower_1_propulsion_mod_T *) - bpm_blower_1_propulsion_modu_M->defaultParam); - B_bpm_blower_1_propulsion_mod_T *bpm_blower_1_propulsion_modul_B = - ((B_bpm_blower_1_propulsion_mod_T *) - bpm_blower_1_propulsion_modu_M->blockIO); - DW_bpm_blower_1_propulsion_mo_T *bpm_blower_1_propulsion_modu_DW = - ((DW_bpm_blower_1_propulsion_mo_T *) bpm_blower_1_propulsion_modu_M->dwork); - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // block I/O - (void) memset(((void *) bpm_blower_1_propulsion_modul_B), 0, - sizeof(B_bpm_blower_1_propulsion_mod_T)); - - // states (dwork) - (void) memset((void *)bpm_blower_1_propulsion_modu_DW, 0, - sizeof(DW_bpm_blower_1_propulsion_mo_T)); - - // external inputs - (void)memset(&bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[0], 0, 3U * - sizeof(real32_T)); - (void)memset(&bpm_blower_1_propulsion_modul_U_veh_cm[0], 0, 3U * sizeof - (real32_T)); - (void)memset(&bpm_blower_1_propulsion_modul_U_servo_cmd[0], 0, 6U * sizeof - (real32_T)); - *bpm_blower_1_propulsion_modul_U_battery_voltage = 0.0F; - *bpm_blower_1_propulsion_modul_U_impeller_cmd = 0U; - - // external outputs - (*bpm_blower_1_propulsion_modul_Y_impeller_current) = 0.0F; - (void) memset(&bpm_blower_1_propulsion_modul_Y_servo_current[0], 0, - 6U*sizeof(real32_T)); - (void) memset(&bpm_blower_1_propulsion_modul_Y_torque_B[0], 0, - 3U*sizeof(real32_T)); - (void) memset(&bpm_blower_1_propulsion_modul_Y_force_B[0], 0, - 3U*sizeof(real32_T)); - (*bpm_blower_1_propulsion_modul_Y_motor_speed) = 0.0F; - (void) memset(&bpm_blower_1_propulsion_modul_Y_nozzle_theta[0], 0, - 6U*sizeof(real32_T)); - (*bpm_blower_1_propulsion_modul_Y_meas_motor_speed) = 0.0F; - } - - return bpm_blower_1_propulsion_modu_M; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module.h b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module.h deleted file mode 100644 index bfabd57af3..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module.h +++ /dev/null @@ -1,604 +0,0 @@ -// -// File: bpm_blower_1_propulsion_module.h -// -// Code generated for Simulink model 'bpm_blower_1_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:46:28 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_bpm_blower_1_propulsion_module_h_ -#define RTW_HEADER_bpm_blower_1_propulsion_module_h_ -#include -#include -#include -#ifndef bpm_blower_1_propulsion_module_COMMON_INCLUDES_ -# define bpm_blower_1_propulsion_module_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#endif // bpm_blower_1_propulsion_module_COMMON_INCLUDES_ - -#include "bpm_blower_1_propulsion_module_types.h" - -// Child system includes -#include "blower_aerodynamics.h" -#include "rt_nonfinite.h" -#include "rt_nrand_Upu32_Yd_f_pw_snf.h" -#include "rt_roundf_snf.h" -#include "rtGetInf.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -# define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -# define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Block signals (auto storage) -typedef struct { - real32_T OutportBufferForthrust2force_B[18];// '/MATLAB Function' - real32_T OutportBufferForthrust2torque_B[18];// '/MATLAB Function' - B_blower_aerodynamics_bpm_blo_T blower_aerodynamics;// '/blower_aerodynamics' -} B_bpm_blower_1_propulsion_mod_T; - -// Block states (auto storage) for system '' -typedef struct { - real_T NextOutput; // '/random_noise1' - real_T NextOutput_e; // '/random_noise' - real32_T DiscreteTimeIntegrator_DSTATE;// '/Discrete-Time Integrator' - real32_T Delay2_DSTATE; // '/Delay2' - real32_T DiscreteTimeIntegrator1_DSTATE;// '/Discrete-Time Integrator1' - real32_T DelayInput1_DSTATE[3]; // '/Delay Input1' - real32_T DiscreteTimeIntegrator4_DSTATE[6];// '/Discrete-Time Integrator4' - real32_T Integrator_DSTATE[6]; // '/Integrator' - real32_T Filter_DSTATE[6]; // '/Filter' - real32_T DiscreteTimeIntegrator3_DSTATE[6];// '/Discrete-Time Integrator3' - real32_T Integrator_DSTATE_k; // '/Integrator' - real32_T Filter_DSTATE_o; // '/Filter' - real32_T PrevY[6]; // '/Backlash1' - real32_T PrevY_l; // '/Rate Limiter' - uint32_T RandSeed; // '/random_noise1' - uint32_T RandSeed_j; // '/random_noise' - boolean_T UnitDelay_DSTATE; // '/Unit Delay' - DW_blower_aerodynamics_bpm_bl_T blower_aerodynamics;// '/blower_aerodynamics' -} DW_bpm_blower_1_propulsion_mo_T; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_bpm_blower_1_pro_T_ { - real_T Constant2_Value[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value; // Expression: 0 - // Referenced by: '/Constant3' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T Gain1_Gain; // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - real32_T Gain_Gain_a; // Computed Parameter: Gain_Gain_a - // Referenced by: '/Gain' - - real32_T Gain1_Gain_m; // Computed Parameter: Gain1_Gain_m - // Referenced by: '/Gain1' - - real32_T Gain2_Gain; // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_i; // Computed Parameter: Gain2_Gain_i - // Referenced by: '/Gain2' - -}; - -// Parameters (auto storage) -struct P_bpm_blower_1_propulsion_mod_T_ { - real_T abp_P_CG_B_B_error[3]; // Variable: abp_P_CG_B_B_error - // Referenced by: '/Constant7' - - real_T astrobee_time_step_size; // Variable: astrobee_time_step_size - // Referenced by: - // '/Gain1' - // '/Gain4' - - real_T bpm_PM1_nozzle_noise_feedback_gain[6];// Variable: bpm_PM1_nozzle_noise_feedback_gain - // Referenced by: '/blower_aerodynamics' - - real_T bpm_PM1_randn_noise_seed; // Variable: bpm_PM1_randn_noise_seed - // Referenced by: '/blower_aerodynamics' - - real_T bpm_sensor_rand_seed; // Variable: bpm_sensor_rand_seed - // Referenced by: - // '/random_noise' - // '/random_noise1' - - real_T bpm_servo_min_theta; // Variable: bpm_servo_min_theta - // Referenced by: '/Discrete-Time Integrator4' - - real32_T abp_PM1_P_nozzle_B_B[18]; // Variable: abp_PM1_P_nozzle_B_B - // Referenced by: '/Constant1' - - real32_T abp_PM1_discharge_coeff[6]; // Variable: abp_PM1_discharge_coeff - // Referenced by: '/blower_aerodynamics' - - real32_T abp_PM1_nozzle_orientations[18];// Variable: abp_PM1_nozzle_orientations - // Referenced by: '/Constant3' - - real32_T abp_PM1_nozzle_widths[6]; // Variable: abp_PM1_nozzle_widths - // Referenced by: '/Constant6' - - real32_T abp_impeller_diameter; // Variable: abp_impeller_diameter - // Referenced by: '/blower_aerodynamics' - - real32_T abp_nozzle_flap_count; // Variable: abp_nozzle_flap_count - // Referenced by: '/Constant1' - - real32_T abp_nozzle_flap_length; // Variable: abp_nozzle_flap_length - // Referenced by: '/Constant4' - - real32_T abp_nozzle_gear_ratio; // Variable: abp_nozzle_gear_ratio - // Referenced by: '/Gain12' - - real32_T abp_nozzle_intake_height; // Variable: abp_nozzle_intake_height - // Referenced by: '/Constant5' - - real32_T abp_nozzle_min_open_angle; // Variable: abp_nozzle_min_open_angle - // Referenced by: '/Constant2' - - real32_T abp_pm1_impeller_orientation[3];// Variable: abp_pm1_impeller_orientation - // Referenced by: '/Constant1' - - real32_T abp_pm1_zero_thrust_area; // Variable: abp_pm1_zero_thrust_area - // Referenced by: '/blower_aerodynamics' - - real32_T bmp_PM1_impeller_orientation_error[3];// Variable: bmp_PM1_impeller_orientation_error - // Referenced by: '/Constant3' - - real32_T bpm_PM1_P_nozzle_B_B_error[18];// Variable: bpm_PM1_P_nozzle_B_B_error - // Referenced by: '/Constant4' - - real32_T bpm_PM1_Q_nozzle2misaligned[24];// Variable: bpm_PM1_Q_nozzle2misaligned - // Referenced by: '/Constant5' - - real32_T bpm_PM1_nozzle_discharge_coeff_error[6];// Variable: bpm_PM1_nozzle_discharge_coeff_error - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_PM1_zero_thrust_area_error;// Variable: bpm_PM1_zero_thrust_area_error - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_imp_ctl_filt_n; // Variable: bpm_imp_ctl_filt_n - // Referenced by: '/Filter Coefficient' - - real32_T bpm_imp_ctl_kd; // Variable: bpm_imp_ctl_kd - // Referenced by: '/Derivative Gain' - - real32_T bpm_imp_ctl_ki; // Variable: bpm_imp_ctl_ki - // Referenced by: '/Integral Gain' - - real32_T bpm_imp_ctl_kp; // Variable: bpm_imp_ctl_kp - // Referenced by: '/Proportional Gain' - - real32_T bpm_imp_max_voltage; // Variable: bpm_imp_max_voltage - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T bpm_imp_motor_friction_coeff;// Variable: bpm_imp_motor_friction_coeff - // Referenced by: '/Gain1' - - real32_T bpm_imp_motor_r; // Variable: bpm_imp_motor_r - // Referenced by: '/Gain5' - - real32_T bpm_imp_motor_speed_k; // Variable: bpm_imp_motor_speed_k - // Referenced by: '/Gain4' - - real32_T bpm_imp_motor_torque_k; // Variable: bpm_imp_motor_torque_k - // Referenced by: '/Gain6' - - real32_T bpm_imp_speed_filt_den; // Variable: bpm_imp_speed_filt_den - // Referenced by: '/Discrete Transfer Fcn1' - - real32_T bpm_imp_speed_filt_num; // Variable: bpm_imp_speed_filt_num - // Referenced by: '/Discrete Transfer Fcn1' - - real32_T bpm_impeller_inertia; // Variable: bpm_impeller_inertia - // Referenced by: - // '/Constant2' - // '/Gain' - // '/Gain2' - - real32_T bpm_impeller_inertia_error; // Variable: bpm_impeller_inertia_error - // Referenced by: - // '/Constant4' - // '/Gain2' - - real32_T bpm_impeller_init_speed; // Variable: bpm_impeller_init_speed - // Referenced by: '/Discrete-Time Integrator' - - real32_T bpm_lookup_Cdp_data[334]; // Variable: bpm_lookup_Cdp_data - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_lookup_totalarea_breakpoints[334];// Variable: bpm_lookup_totalarea_breakpoints - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_sensor_max; // Variable: bpm_sensor_max - // Referenced by: '/Saturation' - - real32_T bpm_sensor_min; // Variable: bpm_sensor_min - // Referenced by: '/Saturation' - - real32_T bpm_sensor_resolution; // Variable: bpm_sensor_resolution - // Referenced by: '/Quantizer' - - real32_T bpm_sensor_sf; // Variable: bpm_sensor_sf - // Referenced by: '/Gain3' - - real32_T bpm_servo_ctl_filt_n; // Variable: bpm_servo_ctl_filt_n - // Referenced by: '/Filter Coefficient' - - real32_T bpm_servo_ctl_kd; // Variable: bpm_servo_ctl_kd - // Referenced by: '/Derivative Gain' - - real32_T bpm_servo_ctl_ki; // Variable: bpm_servo_ctl_ki - // Referenced by: '/Integral Gain' - - real32_T bpm_servo_ctl_kp; // Variable: bpm_servo_ctl_kp - // Referenced by: '/Proportional Gain' - - real32_T bpm_servo_max_theta; // Variable: bpm_servo_max_theta - // Referenced by: '/Discrete-Time Integrator4' - - real32_T bpm_servo_max_voltage; // Variable: bpm_servo_max_voltage - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T bpm_servo_motor_backlash_deadband;// Variable: bpm_servo_motor_backlash_deadband - // Referenced by: '/Backlash1' - - real32_T bpm_servo_motor_friction_coeff;// Variable: bpm_servo_motor_friction_coeff - // Referenced by: '/Gain9' - - real32_T bpm_servo_motor_gear_box_inertia;// Variable: bpm_servo_motor_gear_box_inertia - // Referenced by: '/Gain6' - - real32_T bpm_servo_motor_gear_ratio; // Variable: bpm_servo_motor_gear_ratio - // Referenced by: - // '/Discrete-Time Integrator4' - // '/Gain1' - - real32_T bpm_servo_motor_k; // Variable: bpm_servo_motor_k - // Referenced by: - // '/Gain11' - // '/Gain7' - - real32_T bpm_servo_motor_r; // Variable: bpm_servo_motor_r - // Referenced by: '/Gain5' - - real32_T bpm_servo_pwm2angle; // Variable: bpm_servo_pwm2angle - // Referenced by: '/Gain' - - real32_T bpm_servo_pwm2angle_bias; // Variable: bpm_servo_pwm2angle_bias - // Referenced by: '/Constant' - - real32_T const_air_density; // Variable: const_air_density - // Referenced by: '/blower_aerodynamics' - - real32_T tun_bpm_PM1_thrust_error_sf;// Variable: tun_bpm_PM1_thrust_error_sf - // Referenced by: '/blower_aerodynamics' - - real32_T tun_bpm_noise_on_flag; // Variable: tun_bpm_noise_on_flag - // Referenced by: - // '/blower_aerodynamics' - // '/Gain' - // '/Gain1' - // '/Constant' - // '/Constant8' - // '/Gain' - // '/Gain2' - - real32_T DiscretePIDController_LowerSatu;// Mask Parameter: DiscretePIDController_LowerSatu - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T DiscretePIDController_LowerSa_b;// Mask Parameter: DiscretePIDController_LowerSa_b - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T bpm_blower_1_propulsion_module_;// Mask Parameter: bpm_blower_1_propulsion_module_ - // Referenced by: '/Constant' - - real32_T DetectChange_vinit; // Mask Parameter: DetectChange_vinit - // Referenced by: '/Delay Input1' - - real_T Constant3_Value; // Expression: 0 - // Referenced by: '/Constant3' - - real_T random_noise1_Mean; // Expression: 0 - // Referenced by: '/random_noise1' - - real_T random_noise1_StdDev; // Computed Parameter: random_noise1_StdDev - // Referenced by: '/random_noise1' - - real_T random_noise_Mean; // Expression: 0 - // Referenced by: '/random_noise' - - real_T random_noise_StdDev; // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - real32_T Constant2_Value[24]; // Expression: [zeros(6,3,'single'), ones(6,1,'single')] - // Referenced by: '/Constant2' - - real32_T thrust2torque_B_Y0; // Computed Parameter: thrust2torque_B_Y0 - // Referenced by: '/thrust2torque_B' - - real32_T thrust2force_B_Y0; // Computed Parameter: thrust2force_B_Y0 - // Referenced by: '/thrust2force_B' - - real32_T Switch_Threshold; // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T DiscreteTransferFcn1_InitialSta;// Computed Parameter: DiscreteTransferFcn1_InitialSta - // Referenced by: '/Discrete Transfer Fcn1' - - real32_T RateLimiter_RisingLim; // Computed Parameter: RateLimiter_RisingLim - // Referenced by: '/Rate Limiter' - - real32_T RateLimiter_FallingLim; // Computed Parameter: RateLimiter_FallingLim - // Referenced by: '/Rate Limiter' - - real32_T RateLimiter_IC; // Computed Parameter: RateLimiter_IC - // Referenced by: '/Rate Limiter' - - real32_T Integrator_gainval; // Computed Parameter: Integrator_gainval - // Referenced by: '/Integrator' - - real32_T Integrator_IC; // Computed Parameter: Integrator_IC - // Referenced by: '/Integrator' - - real32_T Filter_gainval; // Computed Parameter: Filter_gainval - // Referenced by: '/Filter' - - real32_T Filter_IC; // Computed Parameter: Filter_IC - // Referenced by: '/Filter' - - real32_T ZeroGain_Gain; // Computed Parameter: ZeroGain_Gain - // Referenced by: '/ZeroGain' - - real32_T Constant_Value; // Computed Parameter: Constant_Value - // Referenced by: '/Constant' - - real32_T DiscreteTimeIntegrator4_gainval;// Computed Parameter: DiscreteTimeIntegrator4_gainval - // Referenced by: '/Discrete-Time Integrator4' - - real32_T Backlash1_InitialOutput; // Computed Parameter: Backlash1_InitialOutput - // Referenced by: '/Backlash1' - - real32_T Integrator_gainval_f; // Computed Parameter: Integrator_gainval_f - // Referenced by: '/Integrator' - - real32_T Integrator_IC_n; // Computed Parameter: Integrator_IC_n - // Referenced by: '/Integrator' - - real32_T Filter_gainval_n; // Computed Parameter: Filter_gainval_n - // Referenced by: '/Filter' - - real32_T Filter_IC_g; // Computed Parameter: Filter_IC_g - // Referenced by: '/Filter' - - real32_T DiscreteTimeIntegrator3_gainval;// Computed Parameter: DiscreteTimeIntegrator3_gainval - // Referenced by: '/Discrete-Time Integrator3' - - real32_T DiscreteTimeIntegrator3_IC; // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator3' - - real32_T ZeroGain_Gain_e; // Computed Parameter: ZeroGain_Gain_e - // Referenced by: '/ZeroGain' - - real32_T Constant_Value_n; // Computed Parameter: Constant_Value_n - // Referenced by: '/Constant' - - real32_T DiscreteTimeIntegrator_gainval;// Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - real32_T Gain1_Gain; // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - real32_T Gain_Gain_h; // Computed Parameter: Gain_Gain_h - // Referenced by: '/Gain' - - real32_T Gain1_Gain_f; // Computed Parameter: Gain1_Gain_f - // Referenced by: '/Gain1' - - real32_T Gain2_Gain; // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - real32_T Delay2_InitialCondition; // Computed Parameter: Delay2_InitialCondition - // Referenced by: '/Delay2' - - real32_T DiscreteTimeIntegrator1_gainval;// Computed Parameter: DiscreteTimeIntegrator1_gainval - // Referenced by: '/Discrete-Time Integrator1' - - real32_T DiscreteTimeIntegrator1_IC; // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator1' - - real32_T Switch_Threshold_j; // Computed Parameter: Switch_Threshold_j - // Referenced by: '/Switch' - - uint32_T Delay2_DelayLength; // Computed Parameter: Delay2_DelayLength - // Referenced by: '/Delay2' - - boolean_T UnitDelay_InitialCondition;// Computed Parameter: UnitDelay_InitialCondition - // Referenced by: '/Unit Delay' - - P_CoreSubsys_bpm_blower_1_pro_T CoreSubsys;// '/CoreSubsys' - P_blower_aerodynamics_bpm_blo_T blower_aerodynamics;// '/blower_aerodynamics' -}; - -// Real-time Model Data Structure -struct tag_RTM_bpm_blower_1_propulsi_T { - const char_T * volatile errorStatus; - B_bpm_blower_1_propulsion_mod_T *blockIO; - P_bpm_blower_1_propulsion_mod_T *defaultParam; - boolean_T paramIsMalloced; - DW_bpm_blower_1_propulsion_mo_T *dwork; -}; - -#ifdef __cplusplus - -extern "C" { - -#endif - -#ifdef __cplusplus - -} -#endif - -// External data declarations for dependent source files -#ifdef __cplusplus - -extern "C" { - -#endif - - extern const char *RT_MEMORY_ALLOCATION_ERROR; - -#ifdef __cplusplus - -} -#endif - -extern P_bpm_blower_1_propulsion_mod_T bpm_blower_1_propulsion_modul_P;// parameters - -#ifdef __cplusplus - -extern "C" { - -#endif - - // Model entry point functions - extern RT_MODEL_bpm_blower_1_propuls_T *bpm_blower_1_propulsion_module - (real32_T *bpm_blower_1_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - *bpm_blower_1_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_1_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_1_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_1_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_1_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_1_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_1_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_1_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_1_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_1_propulsion_modul_Y_meas_motor_speed); - extern void bpm_blower_1_propulsion_module_initialize - (RT_MODEL_bpm_blower_1_propuls_T *const bpm_blower_1_propulsion_modu_M, - real32_T *bpm_blower_1_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - *bpm_blower_1_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_1_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_1_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_1_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_1_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_1_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_1_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_1_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_1_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_1_propulsion_modul_Y_meas_motor_speed); - extern void bpm_blower_1_propulsion_module_step - (RT_MODEL_bpm_blower_1_propuls_T *const bpm_blower_1_propulsion_modu_M, - real32_T bpm_blower_1_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - bpm_blower_1_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_1_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_1_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_1_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_1_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_1_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_1_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_1_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_1_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_1_propulsion_modul_Y_meas_motor_speed); - extern void bpm_blower_1_propulsion_module_terminate - (RT_MODEL_bpm_blower_1_propuls_T * bpm_blower_1_propulsion_modu_M); - -#ifdef __cplusplus - -} -#endif - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Note that this particular code originates from a subsystem build, -// and has its own system numbers different from the parent model. -// Refer to the system hierarchy for this subsystem below, and use the -// MATLAB hilite_system command to trace the generated code back -// to the parent model. For example, -// -// hilite_system('astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module') - opens subsystem astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module -// hilite_system('astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/Kp') - opens and selects block Kp -// -// Here is the system hierarchy for this model -// -// '' : 'astrobee/sim_model_lib/veh_vehicle_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_aerodynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/calc_nozzle_area' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/motor_rotational_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/speed_sensor' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/Detect Change' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/cross_product' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/vector_normalize' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/cross_product/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/cross_product/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/MATLAB Function' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/vector_normalize/No-op' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/vector_normalize/Normalize' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/vector_normalize/vector_magnitude' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/dc_motor_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/speed_controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/speed_controller/Discrete PID Controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/speed_controller/Saturation Dynamic' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/speed_controller/Discrete PID Controller/Clamping circuit' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model/dc_motor_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model/servo_controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model/servo_controller/Discrete PID Controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model/servo_controller/Discrete PID Controller/Clamping circuit' - -#endif // RTW_HEADER_bpm_blower_1_propulsion_module_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_data.cpp b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_data.cpp deleted file mode 100644 index c599d13313..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_data.cpp +++ /dev/null @@ -1,609 +0,0 @@ -// -// File: bpm_blower_1_propulsion_module_data.cpp -// -// Code generated for Simulink model 'bpm_blower_1_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:46:28 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "bpm_blower_1_propulsion_module.h" -#include "bpm_blower_1_propulsion_module_private.h" - -// Block parameters (auto storage) -P_bpm_blower_1_propulsion_mod_T bpm_blower_1_propulsion_modul_P = { - // Variable: abp_P_CG_B_B_error - // Referenced by: '/Constant7' - - { 0.0, 0.0, 0.0 }, - 0.016, // Variable: astrobee_time_step_size - // Referenced by: - // '/Gain1' - // '/Gain4' - - - // Variable: bpm_PM1_nozzle_noise_feedback_gain - // Referenced by: '/blower_aerodynamics' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 254.0, // Variable: bpm_PM1_randn_noise_seed - // Referenced by: '/blower_aerodynamics' - - 1337.0, // Variable: bpm_sensor_rand_seed - // Referenced by: - // '/random_noise' - // '/random_noise1' - - 0.0, // Variable: bpm_servo_min_theta - // Referenced by: '/Discrete-Time Integrator4' - - - // Variable: abp_PM1_P_nozzle_B_B - // Referenced by: '/Constant1' - - { 0.1524F, -0.1524F, 0.0718819946F, -0.0718819946F, -0.067564F, 0.067564F, - 0.101854004F, 0.101854004F, 0.1524F, 0.1524F, 0.101854004F, 0.101854004F, - -0.0396239981F, 0.0396239981F, 0.0718819946F, -0.0718819946F, 0.1524F, - -0.1524F }, - - // Variable: abp_PM1_discharge_coeff - // Referenced by: '/blower_aerodynamics' - - { 0.914971054F, 0.755778253F, 0.940762937F, 0.792109787F, 0.9240188F, - 0.930319786F }, - - // Variable: abp_PM1_nozzle_orientations - // Referenced by: '/Constant3' - - { 1.0F, -1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, -1.0F }, - - // Variable: abp_PM1_nozzle_widths - // Referenced by: '/Constant6' - - { 0.127F, 0.127F, 0.07112F, 0.07112F, 0.07112F, 0.07112F }, - 0.1397F, // Variable: abp_impeller_diameter - // Referenced by: '/blower_aerodynamics' - - 2.0F, // Variable: abp_nozzle_flap_count - // Referenced by: '/Constant1' - - 0.0135966204F, // Variable: abp_nozzle_flap_length - // Referenced by: '/Constant4' - - 0.5F, // Variable: abp_nozzle_gear_ratio - // Referenced by: '/Gain12' - - 0.01309116F, // Variable: abp_nozzle_intake_height - // Referenced by: '/Constant5' - - 0.273667634F, // Variable: abp_nozzle_min_open_angle - // Referenced by: '/Constant2' - - - // Variable: abp_pm1_impeller_orientation - // Referenced by: '/Constant1' - - { 0.0F, 1.0F, 0.0F }, - 0.0044667F, // Variable: abp_pm1_zero_thrust_area - // Referenced by: '/blower_aerodynamics' - - - // Variable: bmp_PM1_impeller_orientation_error - // Referenced by: '/Constant3' - - { 0.0F, 0.0F, 0.0F }, - - // Variable: bpm_PM1_P_nozzle_B_B_error - // Referenced by: '/Constant4' - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - - // Variable: bpm_PM1_Q_nozzle2misaligned - // Referenced by: '/Constant5' - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, -0.0F, - 0.0F, -0.0F, -0.0F, -0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - - // Variable: bpm_PM1_nozzle_discharge_coeff_error - // Referenced by: '/blower_aerodynamics' - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - 0.0F, // Variable: bpm_PM1_zero_thrust_area_error - // Referenced by: '/blower_aerodynamics' - - 1.0F, // Variable: bpm_imp_ctl_filt_n - // Referenced by: '/Filter Coefficient' - - 0.05F, // Variable: bpm_imp_ctl_kd - // Referenced by: '/Derivative Gain' - - 0.1F, // Variable: bpm_imp_ctl_ki - // Referenced by: '/Integral Gain' - - 0.2F, // Variable: bpm_imp_ctl_kp - // Referenced by: '/Proportional Gain' - - 16.6F, // Variable: bpm_imp_max_voltage - // Referenced by: - // '/Saturate' - // '/DeadZone' - - 8.00571161E-6F, // Variable: bpm_imp_motor_friction_coeff - // Referenced by: '/Gain1' - - 1.2F, // Variable: bpm_imp_motor_r - // Referenced by: '/Gain5' - - 39.1651878F, // Variable: bpm_imp_motor_speed_k - // Referenced by: '/Gain4' - - 0.0255F, // Variable: bpm_imp_motor_torque_k - // Referenced by: '/Gain6' - - 1.0F, // Variable: bpm_imp_speed_filt_den - // Referenced by: '/Discrete Transfer Fcn1' - - 1.0F, // Variable: bpm_imp_speed_filt_num - // Referenced by: '/Discrete Transfer Fcn1' - - 0.001F, // Variable: bpm_impeller_inertia - // Referenced by: - // '/Constant2' - // '/Gain' - // '/Gain2' - - 0.0F, // Variable: bpm_impeller_inertia_error - // Referenced by: - // '/Constant4' - // '/Gain2' - - 0.0F, // Variable: bpm_impeller_init_speed - // Referenced by: '/Discrete-Time Integrator' - - - // Variable: bpm_lookup_Cdp_data - // Referenced by: '/blower_aerodynamics' - - { 0.0825F, 0.082625635F, 0.08274699F, 0.0828641355F, 0.082977131F, - 0.083086051F, 0.0831909552F, 0.083291918F, 0.083389F, 0.083482258F, - 0.0835717544F, 0.0836575627F, 0.0837397277F, 0.083818309F, 0.0838933736F, - 0.0839649737F, 0.0840331689F, 0.0840980038F, 0.0841595381F, 0.0842178315F, - 0.0842729211F, 0.084324874F, 0.0843737274F, 0.0844195336F, 0.0844623446F, - 0.0845022127F, 0.0845391676F, 0.0845732689F, 0.0846045539F, 0.0846330673F, - 0.0846588612F, 0.0846819654F, 0.0847024247F, 0.0847202763F, 0.0847355723F, - 0.0847483352F, 0.0847586095F, 0.0847664326F, 0.0847718418F, 0.0847748667F, - 0.0847755447F, 0.084773913F, 0.08477F, 0.0847638398F, 0.0847554579F, - 0.084744893F, 0.0847321674F, 0.0847173184F, 0.0847003683F, 0.0846813396F, - 0.0846602693F, 0.0846371725F, 0.0846120864F, 0.0845850259F, 0.0845560208F, - 0.0845250934F, 0.0844922587F, 0.0844575465F, 0.0844209716F, 0.0843825564F, - 0.0843423232F, 0.0843002871F, 0.0842564702F, 0.084210895F, 0.0841635615F, - 0.0841145F, 0.0840637162F, 0.0840112418F, 0.0839570761F, 0.083901234F, - 0.0838437378F, 0.0837845877F, 0.0837238058F, 0.0836614F, 0.0835973844F, - 0.0835317597F, 0.083464548F, 0.0833957493F, 0.0833253786F, 0.0832534432F, - 0.0831799433F, 0.0831048936F, 0.0830283F, 0.0829501674F, 0.0828705F, - 0.0827893F, 0.0827065781F, 0.0826223418F, 0.0825365856F, 0.0824493095F, - 0.0823605284F, 0.0822702423F, 0.0821784437F, 0.08208514F, 0.0819903314F, - 0.0818940252F, 0.0817962065F, 0.0816968903F, 0.081596069F, 0.0814937353F, - 0.081389904F, 0.0812845528F, 0.0811777F, 0.0810693204F, 0.0809594318F, - 0.0808480158F, 0.0807350799F, 0.0806206167F, 0.0805046186F, 0.0803870782F, - 0.080268F, 0.0801473707F, 0.0800251886F, 0.0799014494F, 0.0797761381F, - 0.0796492621F, 0.0795208F, 0.0793907568F, 0.0792591125F, 0.0791258737F, - 0.0789910182F, 0.0788545534F, 0.0787164569F, 0.0785767287F, 0.0784353614F, - 0.0782923326F, 0.0781476498F, 0.0780012906F, 0.077853255F, 0.0777035281F, - 0.0775521F, 0.0773989633F, 0.0772441104F, 0.0770875216F, 0.0769291893F, - 0.0767691135F, 0.0766072646F, 0.0764436498F, 0.0762782469F, 0.076111041F, - 0.0759420395F, 0.0757712126F, 0.0755985528F, 0.0754240528F, 0.0752477F, - 0.0750694722F, 0.0748893768F, 0.0747073889F, 0.0745235F, 0.0743377F, - 0.0741499662F, 0.0739603F, 0.0737686828F, 0.0735751F, 0.0733795539F, - 0.0731820166F, 0.0729824826F, 0.072780937F, 0.0725773647F, 0.072371766F, - 0.0721641108F, 0.0719544F, 0.071742624F, 0.0715287626F, 0.0713128075F, - 0.0710947439F, 0.0708745569F, 0.0706522539F, 0.0704278F, 0.0702012F, - 0.0699724406F, 0.0697415F, 0.0695083737F, 0.0692730546F, 0.0690355226F, - 0.0687957704F, 0.0685537905F, 0.0683095828F, 0.0680631176F, 0.0678143948F, - 0.0675634F, 0.0673101321F, 0.0670545697F, 0.0667967126F, 0.0665365383F, - 0.0662740618F, 0.0660092533F, 0.0657421201F, 0.0654726326F, 0.0652008057F, - 0.0649266243F, 0.0646500662F, 0.0643711537F, 0.0640898496F, 0.0638061538F, - 0.0635200813F, 0.0632316F, 0.0629407242F, 0.0626474321F, 0.0623517223F, - 0.0620536F, 0.0617530458F, 0.0614500828F, 0.0611446686F, 0.0608368181F, - 0.0605265312F, 0.0602138F, 0.0598986298F, 0.0595810153F, 0.0592609458F, - 0.0589384325F, 0.0586134642F, 0.0582860447F, 0.0579561703F, 0.0576238409F, - 0.0572890751F, 0.0569518507F, 0.0566121712F, 0.0562700555F, 0.0559254885F, - 0.0555784889F, 0.0552290566F, 0.054877162F, 0.0545228682F, 0.0541661382F, - 0.0538069829F, 0.0534454137F, 0.0530814491F, 0.0527150668F, 0.0523463078F, - 0.0519751534F, 0.0516016223F, 0.0512257218F, 0.0508474559F, 0.0504668579F, - 0.0500839F, 0.0496986173F, 0.0493110269F, 0.0489211343F, 0.0485289469F, - 0.0481344871F, 0.0477377549F, 0.04733878F, 0.0469375588F, 0.0465341397F, - 0.0461285152F, 0.0457207039F, 0.0453107134F, 0.0448985882F, 0.0444843248F, - 0.0440679714F, 0.0436495095F, 0.0432290025F, 0.0428064205F, 0.0423818417F, - 0.0419552475F, 0.0415266976F, 0.0410961919F, 0.0406637266F, 0.0402293913F, - 0.0397931822F, 0.0393551365F, 0.038915243F, 0.0384735875F, 0.0380301885F, - 0.0375850387F, 0.0371381938F, 0.0366896801F, 0.0362395607F, 0.0357878208F, - 0.0353345275F, 0.0348797F, 0.0344234072F, 0.0339656323F, 0.033506453F, - 0.033045914F, 0.032584019F, 0.0321208276F, 0.0316563807F, 0.0311907455F, - 0.0307239257F, 0.0302559771F, 0.0297869369F, 0.0293168686F, 0.0288458094F, - 0.0283738412F, 0.0279009566F, 0.0274272151F, 0.0269527063F, 0.0264774524F, - 0.0260014758F, 0.0255248956F, 0.0250477232F, 0.0245700441F, 0.0240918733F, - 0.0236133039F, 0.0231343843F, 0.0226551369F, 0.0221756734F, 0.0216960311F, - 0.0212163F, 0.0207364894F, 0.0202567242F, 0.0197770186F, 0.0192974396F, - 0.0188181363F, 0.0183390342F, 0.0178603418F, 0.0173820443F, 0.0169042759F, - 0.0164270438F, 0.0159504749F, 0.0154745989F, 0.01499952F, 0.0145253353F, - 0.0140520222F, 0.0135798045F, 0.0131086893F, 0.0126387365F, 0.0121700801F, - 0.0117027499F, 0.0112369023F, 0.0107725374F, 0.0103098117F, 0.0098488F, - 0.00938956812F, 0.00893222168F, 0.00847681239F, 0.00802352652F, - 0.00757240178F, 0.007123549F, 0.00667699846F, 0.00623294385F, 0.0057914448F - }, - - // Variable: bpm_lookup_totalarea_breakpoints - // Referenced by: '/blower_aerodynamics' - - { 0.0F, 1.46138646E-5F, 2.92062887E-5F, 4.37784511E-5F, 5.83315159E-5F, - 7.28665837E-5F, 8.7384753E-5F, 0.000101887068F, 0.000116374569F, - 0.000130848246F, 0.000145309066F, 0.000159758F, 0.000174195899F, - 0.000188623759F, 0.000203042364F, 0.000217452631F, 0.000231855331F, - 0.000246251322F, 0.000260641362F, 0.000275026192F, 0.000289406598F, - 0.000303783338F, 0.00031815705F, 0.000332528463F, 0.000346898247F, - 0.000361267099F, 0.00037563566F, 0.000390004541F, 0.000404374383F, - 0.000418745767F, 0.000433119305F, 0.000447495579F, 0.000461875083F, - 0.000476258458F, 0.000490646285F, 0.000505038945F, 0.00051943725F, - 0.000533841376F, 0.000548252079F, 0.000562669651F, 0.00057709479F, - 0.000591527845F, 0.000605969399F, 0.000620419742F, 0.000634879572F, - 0.000649349065F, 0.000663828861F, 0.000678319368F, 0.000692821F, - 0.000707334315F, 0.000721859455F, 0.000736397109F, 0.000750947569F, - 0.0007655113F, 0.000780088594F, 0.000794680149F, 0.000809286F, - 0.000823906856F, 0.000838542939F, 0.000853194739F, 0.000867862604F, - 0.000882546941F, 0.000897248159F, 0.00091196649F, 0.000926702633F, - 0.000941456819F, 0.0009562294F, 0.00097102084F, 0.000985831604F, - 0.00100066199F, 0.00101551227F, 0.00103038305F, 0.0010452749F, - 0.00106018782F, 0.00107512227F, 0.00109007885F, 0.00110505789F, - 0.00112005987F, 0.00113508501F, 0.00115013402F, 0.00116520724F, - 0.0011803048F, 0.00119542738F, 0.00121057557F, 0.00122574961F, 0.00124095F, - 0.00125617709F, 0.00127143157F, 0.00128671364F, 0.001302024F, 0.00131736277F, - 0.00133273099F, 0.00134812866F, 0.00136355648F, 0.00137901504F, - 0.00139450456F, 0.00141002587F, 0.00142557954F, 0.0014411658F, - 0.00145678537F, 0.00147243857F, 0.00148812647F, 0.00150384917F, - 0.00151960761F, 0.00153540191F, 0.00155123312F, 0.0015671018F, - 0.00158300844F, 0.00159895339F, 0.00161493802F, 0.00163096236F, - 0.00164702744F, 0.0016631335F, 0.00167928194F, 0.00169547275F, - 0.00171170721F, 0.00172798557F, 0.00174430886F, 0.0017606779F, - 0.00177709304F, 0.00179355568F, 0.00181006629F, 0.00182662567F, - 0.00184323464F, 0.00185989426F, 0.00187660486F, 0.0018933682F, - 0.00191018439F, 0.00192705484F, 0.00194398011F, 0.00196096138F, - 0.00197799969F, 0.00199509552F, 0.00201225071F, 0.00202946551F, - 0.00204674155F, 0.00206407951F, 0.00208148058F, 0.00209894567F, - 0.00211647665F, 0.00213407329F, 0.00215173839F, 0.00216947193F, - 0.00218727579F, 0.00220515113F, 0.00222309888F, 0.00224112067F, - 0.0022592172F, 0.0022773908F, 0.00229564216F, 0.00231397292F, 0.00233238423F, - 0.00235087774F, 0.00236945506F, 0.00238811737F, 0.00240686629F, - 0.00242570369F, 0.00244463095F, 0.00246364973F, 0.00248276163F, - 0.00250196829F, 0.00252127182F, 0.00254067336F, 0.00256017526F, - 0.00257977913F, 0.00259948708F, 0.00261930074F, 0.00263922219F, - 0.00265925354F, 0.00267939596F, 0.00269965269F, 0.00272002514F, - 0.00274051586F, 0.00276112719F, 0.002781861F, 0.00280271959F, 0.00282370578F, - 0.00284482073F, 0.00286606839F, 0.00288745062F, 0.00290896976F, - 0.00293062883F, 0.00295243063F, 0.00297437701F, 0.00299647171F, - 0.00301871705F, 0.00304111606F, 0.00306367176F, 0.00308638718F, - 0.00310926535F, 0.0031323093F, 0.00315552298F, 0.00317890849F, - 0.00320247072F, 0.00322621223F, 0.00325013627F, 0.00327424728F, - 0.00329854828F, 0.00332304346F, 0.00334773702F, 0.0033726322F, - 0.00339773367F, 0.00342304516F, 0.00344857131F, 0.00347431679F, - 0.00350028556F, 0.00352648227F, 0.00355291134F, 0.00357957813F, - 0.00360648776F, 0.0036336449F, 0.00366105489F, 0.00368872308F, - 0.00371665484F, 0.00374485622F, 0.00377333187F, 0.00380208902F, - 0.00383113371F, 0.00386047084F, 0.00389010808F, 0.00392005173F, - 0.00395030761F, 0.00398088479F, 0.00401178841F, 0.0040430259F, - 0.00407460658F, 0.00410653651F, 0.00413882406F, 0.0041714781F, - 0.00420450559F, 0.00423791679F, 0.00427172F, 0.00430592475F, 0.00434054F, - 0.00437557558F, 0.00441104267F, 0.00444695121F, 0.00448331F, 0.00452013267F, - 0.00455742935F, 0.00459521171F, 0.00463349326F, 0.00467228517F, - 0.00471160142F, 0.00475145364F, 0.00479185674F, 0.00483282562F, - 0.00487437518F, 0.00491652032F, 0.00495927641F, 0.00500265928F, - 0.00504668895F, 0.00509137847F, 0.00513675157F, 0.0051828213F, - 0.00522961235F, 0.00527714146F, 0.00532543147F, 0.00537450658F, - 0.00542438496F, 0.00547509314F, 0.00552665396F, 0.0055790972F, - 0.00563244335F, 0.00568672409F, 0.00574196922F, 0.00579820713F, - 0.00585547043F, 0.00591378892F, 0.00597320078F, 0.00603374047F, - 0.00609544525F, 0.00615835097F, 0.00622250512F, 0.0062879459F, 0.006354718F, - 0.00642287126F, 0.00649245409F, 0.00656351773F, 0.00663611386F, - 0.00671030255F, 0.00678614574F, 0.00686370581F, 0.00694304705F, - 0.00702424115F, 0.00710735889F, 0.00719248503F, 0.0072797006F, - 0.00736908847F, 0.00746074459F, 0.00755477371F, 0.00765126897F, - 0.00775034819F, 0.00785212312F, 0.00795672741F, 0.00806429F, 0.00817495212F, - 0.00828887522F, 0.00840621162F, 0.00852714293F, 0.00865184888F, - 0.00878054276F, 0.00891342945F, 0.00905075204F, 0.00919276662F, - 0.0093397228F, 0.00949195493F, 0.00964974798F, 0.00981347449F, - 0.00998349674F, 0.0101602506F, 0.0103441793F, 0.0105357962F, 0.010735645F, - 0.0109443273F, 0.0111625558F, 0.0113910241F, 0.0116305985F, 0.0118822251F, - 0.0121469265F, 0.0124259172F, 0.0127205F, 0.0130322482F, 0.0133628659F, - 0.0137143685F, 0.0140890917F, 0.0144897308F, 0.0149194878F, 0.01538203F, - 0.0158818662F, 0.0164243504F, 0.0170160942F, 0.0176650118F, 0.0183811728F }, - 10000.0F, // Variable: bpm_sensor_max - // Referenced by: '/Saturation' - - 0.0F, // Variable: bpm_sensor_min - // Referenced by: '/Saturation' - - 0.1F, // Variable: bpm_sensor_resolution - // Referenced by: '/Quantizer' - - 1.0F, // Variable: bpm_sensor_sf - // Referenced by: '/Gain3' - - 1.0F, // Variable: bpm_servo_ctl_filt_n - // Referenced by: '/Filter Coefficient' - - 0.8F, // Variable: bpm_servo_ctl_kd - // Referenced by: '/Derivative Gain' - - 1.5F, // Variable: bpm_servo_ctl_ki - // Referenced by: '/Integral Gain' - - 5.0F, // Variable: bpm_servo_ctl_kp - // Referenced by: '/Proportional Gain' - - 0.560512543F, // Variable: bpm_servo_max_theta - // Referenced by: '/Discrete-Time Integrator4' - - 6.0F, // Variable: bpm_servo_max_voltage - // Referenced by: - // '/Saturate' - // '/DeadZone' - - 0.0174532924F, // Variable: bpm_servo_motor_backlash_deadband - // Referenced by: '/Backlash1' - - 3.0E-7F, // Variable: bpm_servo_motor_friction_coeff - // Referenced by: '/Gain9' - - 2.5E-8F, // Variable: bpm_servo_motor_gear_box_inertia - // Referenced by: '/Gain6' - - 0.01F, // Variable: bpm_servo_motor_gear_ratio - // Referenced by: - // '/Discrete-Time Integrator4' - // '/Gain1' - - 0.00192866661F, // Variable: bpm_servo_motor_k - // Referenced by: - // '/Gain11' - // '/Gain7' - - 4.0F, // Variable: bpm_servo_motor_r - // Referenced by: '/Gain5' - - 0.00219808845F, // Variable: bpm_servo_pwm2angle - // Referenced by: '/Gain' - - 0.0F, // Variable: bpm_servo_pwm2angle_bias - // Referenced by: '/Constant' - - 1.2F, // Variable: const_air_density - // Referenced by: '/blower_aerodynamics' - - 1.25F, // Variable: tun_bpm_PM1_thrust_error_sf - // Referenced by: '/blower_aerodynamics' - - 0.0F, // Variable: tun_bpm_noise_on_flag - // Referenced by: - // '/blower_aerodynamics' - // '/Gain' - // '/Gain1' - // '/Constant' - // '/Constant8' - // '/Gain' - // '/Gain2' - - -16.6F, // Mask Parameter: DiscretePIDController_LowerSatu - // Referenced by: - // '/Saturate' - // '/DeadZone' - - -6.0F, // Mask Parameter: DiscretePIDController_LowerSa_b - // Referenced by: - // '/Saturate' - // '/DeadZone' - - 1.26247478F, // Mask Parameter: bpm_blower_1_propulsion_module_ - // Referenced by: '/Constant' - - 0.0F, // Mask Parameter: DetectChange_vinit - // Referenced by: '/Delay Input1' - - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - 0.0, // Expression: 0 - // Referenced by: '/random_noise1' - - 1.0E-5, // Computed Parameter: random_noise1_StdDev - // Referenced by: '/random_noise1' - - 0.0, // Expression: 0 - // Referenced by: '/random_noise' - - 1.0E-5, // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - - // Expression: [zeros(6,3,'single'), ones(6,1,'single')] - // Referenced by: '/Constant2' - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - 0.0F, // Computed Parameter: thrust2torque_B_Y0 - // Referenced by: '/thrust2torque_B' - - 0.0F, // Computed Parameter: thrust2force_B_Y0 - // Referenced by: '/thrust2force_B' - - 0.0F, // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - -1.0F, // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - 0.0F, // Computed Parameter: DiscreteTransferFcn1_InitialSta - // Referenced by: '/Discrete Transfer Fcn1' - - 0.335103214F, // Computed Parameter: RateLimiter_RisingLim - // Referenced by: '/Rate Limiter' - - -0.335103214F, // Computed Parameter: RateLimiter_FallingLim - // Referenced by: '/Rate Limiter' - - 0.0F, // Computed Parameter: RateLimiter_IC - // Referenced by: '/Rate Limiter' - - 0.016F, // Computed Parameter: Integrator_gainval - // Referenced by: '/Integrator' - - 0.0F, // Computed Parameter: Integrator_IC - // Referenced by: '/Integrator' - - 0.016F, // Computed Parameter: Filter_gainval - // Referenced by: '/Filter' - - 0.0F, // Computed Parameter: Filter_IC - // Referenced by: '/Filter' - - 0.0F, // Computed Parameter: ZeroGain_Gain - // Referenced by: '/ZeroGain' - - 0.0F, // Computed Parameter: Constant_Value - // Referenced by: '/Constant' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator4_gainval - // Referenced by: '/Discrete-Time Integrator4' - - 0.0F, // Computed Parameter: Backlash1_InitialOutput - // Referenced by: '/Backlash1' - - 0.016F, // Computed Parameter: Integrator_gainval_f - // Referenced by: '/Integrator' - - 0.0F, // Computed Parameter: Integrator_IC_n - // Referenced by: '/Integrator' - - 0.016F, // Computed Parameter: Filter_gainval_n - // Referenced by: '/Filter' - - 0.0F, // Computed Parameter: Filter_IC_g - // Referenced by: '/Filter' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator3_gainval - // Referenced by: '/Discrete-Time Integrator3' - - 0.0F, // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator3' - - 0.0F, // Computed Parameter: ZeroGain_Gain_e - // Referenced by: '/ZeroGain' - - 0.0F, // Computed Parameter: Constant_Value_n - // Referenced by: '/Constant' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - -1.0F, // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain_Gain_h - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_f - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - 0.0F, // Computed Parameter: Delay2_InitialCondition - // Referenced by: '/Delay2' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator1_gainval - // Referenced by: '/Discrete-Time Integrator1' - - 0.0F, // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator1' - - 0.0F, // Computed Parameter: Switch_Threshold_j - // Referenced by: '/Switch' - - 1U, // Computed Parameter: Delay2_DelayLength - // Referenced by: '/Delay2' - - 1, // Computed Parameter: UnitDelay_InitialCondition - // Referenced by: '/Unit Delay' - - - // Start of '/CoreSubsys' - { - // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, // Expression: 1 - // Referenced by: '/Constant1' - - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - 2.0F, // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - 2.0F, // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain_Gain_a - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_m - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - 2.0F // Computed Parameter: Gain2_Gain_i - // Referenced by: '/Gain2' - - } - // End of '/CoreSubsys' - , - - // Start of '/blower_aerodynamics' - { - 0.016, // Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - 0.0, // Expression: 0 - // Referenced by: '/Discrete-Time Integrator' - - 0.0, // Expression: 0 - // Referenced by: '/random_noise' - - - // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 2.0F, // Expression: single(2) - // Referenced by: '/Constant4' - - 0.0F // Expression: single(0) - // Referenced by: '/Constant7' - - } - // End of '/blower_aerodynamics' -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_private.h b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_private.h deleted file mode 100644 index a0b874a104..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_private.h +++ /dev/null @@ -1,51 +0,0 @@ -// -// File: bpm_blower_1_propulsion_module_private.h -// -// Code generated for Simulink model 'bpm_blower_1_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:46:28 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_bpm_blower_1_propulsion_module_private_h_ -#define RTW_HEADER_bpm_blower_1_propulsion_module_private_h_ -#include "rtwtypes.h" -#if !defined(rt_VALIDATE_MEMORY) -#define rt_VALIDATE_MEMORY(S, ptr) if(!(ptr)) {\ - rtmSetErrorStatus(bpm_blower_1_propulsion_modu_M, RT_MEMORY_ALLOCATION_ERROR);\ - } -#endif - -#if !defined(rt_FREE) -#if !defined(_WIN32) -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((ptr));\ - (ptr) = (NULL);\ - } -#else - -// Visual and other windows compilers declare free without const -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((void *)(ptr));\ - (ptr) = (NULL);\ - } -#endif -#endif - -extern const real_T rtCP_pooled_15LYgUQHWYtd[6]; - -#define rtCP_random_noise_rtw_collapsed rtCP_pooled_15LYgUQHWYtd // Expression: noz_randn_seed +[1:6] - // Referenced by: '/random_noise' - -#endif // RTW_HEADER_bpm_blower_1_propulsion_module_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_types.h b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_types.h deleted file mode 100644 index 036c3d0958..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/bpm_blower_1_propulsion_module_types.h +++ /dev/null @@ -1,37 +0,0 @@ -// -// File: bpm_blower_1_propulsion_module_types.h -// -// Code generated for Simulink model 'bpm_blower_1_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:46:28 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_bpm_blower_1_propulsion_module_types_h_ -#define RTW_HEADER_bpm_blower_1_propulsion_module_types_h_ -#include "rtwtypes.h" - -// Parameters for system: '/blower_aerodynamics' -typedef struct P_blower_aerodynamics_bpm_blo_T_ P_blower_aerodynamics_bpm_blo_T; - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_bpm_blower_1_pro_T_ P_CoreSubsys_bpm_blower_1_pro_T; - -// Parameters (auto storage) -typedef struct P_bpm_blower_1_propulsion_mod_T_ P_bpm_blower_1_propulsion_mod_T; - -// Forward declaration for rtModel -typedef struct tag_RTM_bpm_blower_1_propulsi_T RT_MODEL_bpm_blower_1_propuls_T; - -#endif // RTW_HEADER_bpm_blower_1_propulsion_module_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/buildInfo.mat b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/buildInfo.mat deleted file mode 100644 index 3e3523ed64..0000000000 Binary files a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/codeInfo.mat b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/codeInfo.mat deleted file mode 100644 index dde8924087..0000000000 Binary files a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/codeInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/defines.txt b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/defines.txt deleted file mode 100644 index 452c7e4215..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/defines.txt +++ /dev/null @@ -1,15 +0,0 @@ -RT_MALLOC -MODEL=bpm_blower_1_propulsion_module -NUMST=1 -NCSTATES=0 -HAVESTDIO -UNIX -ONESTEPFCN=1 -TERMFCN=1 -MAT_FILE=0 -MULTI_INSTANCE_CODE=1 -INTEGER_CODE=0 -MT=0 -CLASSIC_INTERFACE=0 -ALLOCATIONFCN=1 -TID01EQ=0 diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/ert_main.cpp b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/ert_main.cpp deleted file mode 100644 index 5d5a747947..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/ert_main.cpp +++ /dev/null @@ -1,199 +0,0 @@ -// -// File: ert_main.cpp -// -// Code generated for Simulink model 'bpm_blower_1_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:46:28 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include -#include // This ert_main.c example uses printf/fflush -#include "bpm_blower_1_propulsion_module.h" // Model's header file -#include "rtwtypes.h" - -// '/battery_voltage' -static real32_T bpm_blower_1_propulsion_modul_U_battery_voltage; - -// '/omega_B_ECI_B' -static real32_T bpm_blower_1_propulsion_modul_U_omega_B_ECI_B[3]; - -// '/impeller_cmd' -static uint8_T bpm_blower_1_propulsion_modul_U_impeller_cmd; - -// '/servo_cmd' -static real32_T bpm_blower_1_propulsion_modul_U_servo_cmd[6]; - -// '/veh_cm' -static real32_T bpm_blower_1_propulsion_modul_U_veh_cm[3]; - -// '/impeller_current' -static real32_T bpm_blower_1_propulsion_modul_Y_impeller_current; - -// '/servo_current' -static real32_T bpm_blower_1_propulsion_modul_Y_servo_current[6]; - -// '/torque_B' -static real32_T bpm_blower_1_propulsion_modul_Y_torque_B[3]; - -// '/force_B' -static real32_T bpm_blower_1_propulsion_modul_Y_force_B[3]; - -// '/motor_speed' -static real32_T bpm_blower_1_propulsion_modul_Y_motor_speed; - -// '/nozzle_theta' -static real32_T bpm_blower_1_propulsion_modul_Y_nozzle_theta[6]; - -// '/meas_motor_speed' -static real32_T bpm_blower_1_propulsion_modul_Y_meas_motor_speed; -const char *RT_MEMORY_ALLOCATION_ERROR = "memory allocation error"; - -// -// Associating rt_OneStep with a real-time clock or interrupt service routine -// is what makes the generated code "real-time". The function rt_OneStep is -// always associated with the base rate of the model. Subrates are managed -// by the base rate from inside the generated code. Enabling/disabling -// interrupts and floating point context switches are target specific. This -// example code indicates where these should take place relative to executing -// the generated code step function. Overrun behavior should be tailored to -// your application needs. This example simply sets an error status in the -// real-time model and returns from rt_OneStep. -// -void rt_OneStep(RT_MODEL_bpm_blower_1_propuls_T *const - bpm_blower_1_propulsion_modu_M); -void rt_OneStep(RT_MODEL_bpm_blower_1_propuls_T *const - bpm_blower_1_propulsion_modu_M) -{ - static boolean_T OverrunFlag = false; - - // Disable interrupts here - - // Check for overrun - if (OverrunFlag) { - rtmSetErrorStatus(bpm_blower_1_propulsion_modu_M, "Overrun"); - return; - } - - OverrunFlag = true; - - // Save FPU context here (if necessary) - // Re-enable timer or interrupt here - // Set model inputs here - - // Step the model - bpm_blower_1_propulsion_module_step(bpm_blower_1_propulsion_modu_M, - bpm_blower_1_propulsion_modul_U_battery_voltage, - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B, - bpm_blower_1_propulsion_modul_U_impeller_cmd, - bpm_blower_1_propulsion_modul_U_servo_cmd, - bpm_blower_1_propulsion_modul_U_veh_cm, - &bpm_blower_1_propulsion_modul_Y_impeller_current, - bpm_blower_1_propulsion_modul_Y_servo_current, - bpm_blower_1_propulsion_modul_Y_torque_B, - bpm_blower_1_propulsion_modul_Y_force_B, - &bpm_blower_1_propulsion_modul_Y_motor_speed, - bpm_blower_1_propulsion_modul_Y_nozzle_theta, - &bpm_blower_1_propulsion_modul_Y_meas_motor_speed); - - // Get model outputs here - - // Indicate task complete - OverrunFlag = false; - - // Disable interrupts here - // Restore FPU context here (if necessary) - // Enable interrupts here -} - -// -// The example "main" function illustrates what is required by your -// application code to initialize, execute, and terminate the generated code. -// Attaching rt_OneStep to a real-time clock is target specific. This example -// illustrates how you do this relative to initializing the model. -// -int_T main(int_T argc, const char *argv[]) -{ - RT_MODEL_bpm_blower_1_propuls_T *bpm_blower_1_propulsion_modu_M; - - // Unused arguments - (void)(argc); - (void)(argv); - - // Allocate model data - bpm_blower_1_propulsion_modu_M = bpm_blower_1_propulsion_module - (&bpm_blower_1_propulsion_modul_U_battery_voltage, - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B, - &bpm_blower_1_propulsion_modul_U_impeller_cmd, - bpm_blower_1_propulsion_modul_U_servo_cmd, - bpm_blower_1_propulsion_modul_U_veh_cm, - &bpm_blower_1_propulsion_modul_Y_impeller_current, - bpm_blower_1_propulsion_modul_Y_servo_current, - bpm_blower_1_propulsion_modul_Y_torque_B, - bpm_blower_1_propulsion_modul_Y_force_B, - &bpm_blower_1_propulsion_modul_Y_motor_speed, - bpm_blower_1_propulsion_modul_Y_nozzle_theta, - &bpm_blower_1_propulsion_modul_Y_meas_motor_speed); - if (bpm_blower_1_propulsion_modu_M == NULL) { - (void)fprintf(stderr,"Memory allocation error during model " - "registration"); - return(1); - } - - if (rtmGetErrorStatus(bpm_blower_1_propulsion_modu_M) != NULL) { - (void)fprintf(stderr,"Error during model registration: %s\n", - rtmGetErrorStatus(bpm_blower_1_propulsion_modu_M)); - - // Disable rt_OneStep() here - - // Terminate model - bpm_blower_1_propulsion_module_terminate(bpm_blower_1_propulsion_modu_M); - return(1); - } - - // Initialize model - bpm_blower_1_propulsion_module_initialize(bpm_blower_1_propulsion_modu_M, - &bpm_blower_1_propulsion_modul_U_battery_voltage, - bpm_blower_1_propulsion_modul_U_omega_B_ECI_B, - &bpm_blower_1_propulsion_modul_U_impeller_cmd, - bpm_blower_1_propulsion_modul_U_servo_cmd, - bpm_blower_1_propulsion_modul_U_veh_cm, - &bpm_blower_1_propulsion_modul_Y_impeller_current, - bpm_blower_1_propulsion_modul_Y_servo_current, - bpm_blower_1_propulsion_modul_Y_torque_B, - bpm_blower_1_propulsion_modul_Y_force_B, - &bpm_blower_1_propulsion_modul_Y_motor_speed, - bpm_blower_1_propulsion_modul_Y_nozzle_theta, - &bpm_blower_1_propulsion_modul_Y_meas_motor_speed); - - // Attach rt_OneStep to a timer or interrupt service routine with - // period 0.016 seconds (the model's base sample time) here. The - // call syntax for rt_OneStep is - // - // rt_OneStep(bpm_blower_1_propulsion_modu_M); - - printf("Warning: The simulation will run forever. " - "Generated ERT main won't simulate model step behavior. " - "To change this behavior select the 'MAT-file logging' option.\n"); - fflush((NULL)); - while (rtmGetErrorStatus(bpm_blower_1_propulsion_modu_M) == (NULL)) { - // Perform other application tasks here - } - - // Disable rt_OneStep() here - - // Terminate model - bpm_blower_1_propulsion_module_terminate(bpm_blower_1_propulsion_modu_M); - return 0; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/modelsources.txt b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/modelsources.txt deleted file mode 100644 index cbb938843d..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/modelsources.txt +++ /dev/null @@ -1 +0,0 @@ - bpm_blower_1_propulsion_module.cpp blower_aerodynamics.cpp diff --git a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/rtw_proj.tmw b/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/rtw_proj.tmw deleted file mode 100644 index 2afc061909..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_1_propulsion_module_ert_rtw/rtw_proj.tmw +++ /dev/null @@ -1,4 +0,0 @@ -Simulink Coder project for bpm_blower_1_propulsion_module using . MATLAB root = /usr/local/MATLAB/R2016b. SimStruct date: 22-Jul-2016 13:46:14 -This file is generated by Simulink Coder for use by the make utility -to determine when to rebuild objects when the name of the current Simulink Coder project changes. -The rtwinfomat located at: ../slprj/ert/bpm_blower_1_propulsion_module/tmwinternal/binfo.mat diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/blower_aerodynamics.cpp b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/blower_aerodynamics.cpp deleted file mode 100644 index 7580ae70d6..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/blower_aerodynamics.cpp +++ /dev/null @@ -1,164 +0,0 @@ -// -// File: blower_aerodynamics.cpp -// -// Code generated for Simulink model 'bpm_blower_2_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:47:03 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "blower_aerodynamics.h" - -// Include model header file for global data -#include "bpm_blower_2_propulsion_module.h" -#include "bpm_blower_2_propulsion_module_private.h" - -// System initialize for atomic system: '/blower_aerodynamics' -void bpm_bl_blower_aerodynamics_Init(DW_blower_aerodynamics_bpm_bl_T *localDW, - P_blower_aerodynamics_bpm_blo_T *localP, real_T rtp_noz_randn_seed) -{ - uint32_T tseed; - int32_T r; - int32_T t; - real_T y1; - int32_T i; - for (i = 0; i < 6; i++) { - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - localDW->DiscreteTimeIntegrator_DSTATE[i] = - localP->DiscreteTimeIntegrator_IC; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(rtp_noz_randn_seed + rtCP_random_noise_rtw_collapsed[i]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * localP->random_noise_StdDev[i] + - localP->random_noise_Mean; - localDW->NextOutput[i] = y1; - localDW->RandSeed[i] = tseed; - - // End of InitializeConditions for RandomNumber: '/random_noise' - } -} - -// Output and update for atomic system: '/blower_aerodynamics' -void bpm_blower__blower_aerodynamics(real32_T rtu_rotor_speed, const real32_T - rtu_nozzle_areas[6], B_blower_aerodynamics_bpm_blo_T *localB, - DW_blower_aerodynamics_bpm_bl_T *localDW, P_blower_aerodynamics_bpm_blo_T - *localP, real32_T rtp_imp_zero_thrust_area, real32_T - rtp_imp_zero_thrust_area_error, real32_T rtp_noise_on_flag, const real32_T - rtp_noz_cd[6], const real32_T rtp_noz_cd_error[6], const real32_T - rtp_imp_cdp_lookup[334], const real32_T rtp_imp_area_lookup[334], real32_T - rtp_imp_diameter, real32_T rtp_const_air_den, real32_T rtp_noz_thrust_sf, - const real_T rtp_noz_thrust_noise_feedback[6]) -{ - real32_T rtb_walking_bias[6]; - real32_T rtb_Product2_k[6]; - real32_T rtb_Cdp; - int32_T i; - for (i = 0; i < 6; i++) { - // Sum: '/Add3' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant8' - // Gain: '/Gain1' - - rtb_Cdp = rtp_noise_on_flag * rtp_noz_cd_error[i] + rtp_noz_cd[i]; - - // Product: '/Product2' - rtb_Product2_k[i] = rtb_Cdp * rtu_nozzle_areas[i]; - - // Sum: '/Add3' - rtb_walking_bias[i] = rtb_Cdp; - } - - // Sum: '/Sum of Elements1' - rtb_Cdp = rtb_Product2_k[0]; - for (i = 0; i < 5; i++) { - rtb_Cdp += rtb_Product2_k[(int32_T)(i + 1)]; - } - - // End of Sum: '/Sum of Elements1' - - // Sum: '/Add' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant6' - // Gain: '/Gain2' - - rtb_Cdp += rtp_noise_on_flag * rtp_imp_zero_thrust_area_error + - rtp_imp_zero_thrust_area; - - // Lookup_n-D: '/bpm_Cdp_lookup' - rtb_Cdp = look1_iflf_binlxpw(rtb_Cdp, rtp_imp_area_lookup, rtp_imp_cdp_lookup, - 333U); - - // Product: '/Product3' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - - rtb_Cdp = rtu_rotor_speed * rtu_rotor_speed * rtb_Cdp * rtp_imp_diameter * - rtp_imp_diameter * rtp_const_air_den; - - // Constant: '/Constant7' - localB->Constant7 = localP->Constant7_Value; - for (i = 0; i < 6; i++) { - // Sum: '/Add1' incorporates: - // Constant: '/Constant4' - // Constant: '/Constant9' - // DataTypeConversion: '/Data Type Conversion' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Product: '/Product1' - // Product: '/Product6' - // Product: '/Product7' - - localB->Add1[i] = localP->Constant4_Value * rtb_walking_bias[i] * - rtb_walking_bias[i] * rtb_Cdp * rtu_nozzle_areas[i] * rtp_noz_thrust_sf + - (real32_T)localDW->DiscreteTimeIntegrator_DSTATE[i]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain3' - // RandomNumber: '/random_noise' - // Sum: '/Sum' - - localDW->DiscreteTimeIntegrator_DSTATE[i] += (localDW->NextOutput[i] - - rtp_noz_thrust_noise_feedback[i] * localDW-> - DiscreteTimeIntegrator_DSTATE[i]) * localP->DiscreteTimeIntegrator_gainval; - - // Update for RandomNumber: '/random_noise' - localDW->NextOutput[i] = rt_nrand_Upu32_Yd_f_pw_snf(&localDW->RandSeed[i]) * - localP->random_noise_StdDev[i] + localP->random_noise_Mean; - } -} - -// Termination for atomic system: '/blower_aerodynamics' -void bpm_bl_blower_aerodynamics_Term(void) -{ -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/blower_aerodynamics.h b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/blower_aerodynamics.h deleted file mode 100644 index 9a79150e3d..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/blower_aerodynamics.h +++ /dev/null @@ -1,82 +0,0 @@ -// -// File: blower_aerodynamics.h -// -// Code generated for Simulink model 'bpm_blower_2_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:47:03 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_blower_aerodynamics_h_ -#define RTW_HEADER_blower_aerodynamics_h_ -#include -#ifndef bpm_blower_2_propulsion_module_COMMON_INCLUDES_ -# define bpm_blower_2_propulsion_module_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#endif // bpm_blower_2_propulsion_module_COMMON_INCLUDES_ - -#include "bpm_blower_2_propulsion_module_types.h" -#include "look1_iflf_binlxpw.h" -#include "rt_nonfinite.h" -#include "rt_nrand_Upu32_Yd_f_pw_snf.h" - -// Block signals for system '/blower_aerodynamics' -typedef struct { - real32_T Add1[6]; // '/Add1' - real32_T Constant7; // '/Constant7' -} B_blower_aerodynamics_bpm_blo_T; - -// Block states (auto storage) for system '/blower_aerodynamics' -typedef struct { - real_T DiscreteTimeIntegrator_DSTATE[6];// '/Discrete-Time Integrator' - real_T NextOutput[6]; // '/random_noise' - uint32_T RandSeed[6]; // '/random_noise' -} DW_blower_aerodynamics_bpm_bl_T; - -// Parameters for system: '/blower_aerodynamics' -struct P_blower_aerodynamics_bpm_blo_T_ { - real_T DiscreteTimeIntegrator_gainval;// Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - real_T DiscreteTimeIntegrator_IC; // Expression: 0 - // Referenced by: '/Discrete-Time Integrator' - - real_T random_noise_Mean; // Expression: 0 - // Referenced by: '/random_noise' - - real_T random_noise_StdDev[6]; // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - real32_T Constant4_Value; // Expression: single(2) - // Referenced by: '/Constant4' - - real32_T Constant7_Value; // Expression: single(0) - // Referenced by: '/Constant7' - -}; - -void bpm_bl_blower_aerodynamics_Init(DW_blower_aerodynamics_bpm_bl_T *localDW, - P_blower_aerodynamics_bpm_blo_T *localP, real_T rtp_noz_randn_seed); -void bpm_blower__blower_aerodynamics(real32_T rtu_rotor_speed, const real32_T - rtu_nozzle_areas[6], B_blower_aerodynamics_bpm_blo_T *localB, - DW_blower_aerodynamics_bpm_bl_T *localDW, P_blower_aerodynamics_bpm_blo_T - *localP, real32_T rtp_imp_zero_thrust_area, real32_T - rtp_imp_zero_thrust_area_error, real32_T rtp_noise_on_flag, const real32_T - rtp_noz_cd[6], const real32_T rtp_noz_cd_error[6], const real32_T - rtp_imp_cdp_lookup[334], const real32_T rtp_imp_area_lookup[334], real32_T - rtp_imp_diameter, real32_T rtp_const_air_den, real32_T rtp_noz_thrust_sf, - const real_T rtp_noz_thrust_noise_feedback[6]); - -#endif // RTW_HEADER_blower_aerodynamics_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module.cpp b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module.cpp deleted file mode 100644 index 842f6f034a..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module.cpp +++ /dev/null @@ -1,1385 +0,0 @@ -// -// File: bpm_blower_2_propulsion_module.cpp -// -// Code generated for Simulink model 'bpm_blower_2_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:47:03 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "bpm_blower_2_propulsion_module.h" -#include "bpm_blower_2_propulsion_module_private.h" - -// Model step function -void bpm_blower_2_propulsion_module_step(RT_MODEL_bpm_blower_2_propuls_T *const - bpm_blower_2_propulsion_modu_M, real32_T - bpm_blower_2_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - bpm_blower_2_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_2_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_2_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_2_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_2_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_2_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_2_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_2_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_2_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_2_propulsion_modul_Y_meas_motor_speed) -{ - P_bpm_blower_2_propulsion_mod_T *bpm_blower_2_propulsion_modul_P = - ((P_bpm_blower_2_propulsion_mod_T *) - bpm_blower_2_propulsion_modu_M->defaultParam); - B_bpm_blower_2_propulsion_mod_T *bpm_blower_2_propulsion_modul_B = - ((B_bpm_blower_2_propulsion_mod_T *) bpm_blower_2_propulsion_modu_M->blockIO); - DW_bpm_blower_2_propulsion_mo_T *bpm_blower_2_propulsion_modu_DW = - ((DW_bpm_blower_2_propulsion_mo_T *) bpm_blower_2_propulsion_modu_M->dwork); - - // local scratch DWork variables - int32_T ForEach_itr; - real32_T nozzle_moment_arm[18]; - real_T b[18]; - int32_T ibmat; - int32_T itilerow; - real32_T c[18]; - real32_T rtb_current[6]; - real32_T rtb_IntegralGain_b; - real32_T rtb_Sum_l; - real32_T rtb_SignPreIntegrator_a; - real32_T rtb_FilterCoefficient_l; - boolean_T rtb_NotEqual_a; - real32_T rtb_Gain1_d[6]; - real32_T rtb_IntegralGain[6]; - real32_T rtb_SignDeltaU[6]; - real32_T rtb_FilterCoefficient[6]; - boolean_T rtb_NotEqual[6]; - real32_T rtb_Add1[3]; - real32_T rtb_Assignment[9]; - real32_T rtb_Gain_a; - real32_T rtb_ImpAsg_InsertedFor_rotated_[18]; - real_T rtb_Sum2[3]; - real32_T Backlash1[6]; - real32_T tmp; - int32_T i; - real32_T tmp_0[9]; - real32_T tmp_1[3]; - real32_T tmp_2[9]; - real32_T rtb_Switch_b_0[9]; - real32_T rtb_Assignment_0[9]; - real32_T rtb_SignDeltaU_g; - real32_T rtb_IntegralGain_e; - real32_T rtb_SignDeltaU_1; - int8_T rtb_SignPreIntegrator_o; - const real32_T *rtb_Switch_b_1; - - // Outputs for Atomic SubSystem: '/speed_controller' - // Product: '/Divide' incorporates: - // Constant: '/Constant' - // Inport: '/impeller_cmd' - - rtb_IntegralGain_b = (real32_T)bpm_blower_2_propulsion_modul_U_impeller_cmd * - bpm_blower_2_propulsion_modul_P->bpm_blower_2_propulsion_module_; - - // RateLimiter: '/Rate Limiter' - rtb_FilterCoefficient_l = rtb_IntegralGain_b - - bpm_blower_2_propulsion_modu_DW->PrevY_c; - if (rtb_FilterCoefficient_l > - bpm_blower_2_propulsion_modul_P->RateLimiter_RisingLim) { - rtb_IntegralGain_b = bpm_blower_2_propulsion_modu_DW->PrevY_c + - bpm_blower_2_propulsion_modul_P->RateLimiter_RisingLim; - } else { - if (rtb_FilterCoefficient_l < - bpm_blower_2_propulsion_modul_P->RateLimiter_FallingLim) { - rtb_IntegralGain_b = bpm_blower_2_propulsion_modu_DW->PrevY_c + - bpm_blower_2_propulsion_modul_P->RateLimiter_FallingLim; - } - } - - bpm_blower_2_propulsion_modu_DW->PrevY_c = rtb_IntegralGain_b; - - // End of RateLimiter: '/Rate Limiter' - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteTransferFcn: '/Discrete Transfer Fcn1' - - rtb_IntegralGain_b -= - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE * - bpm_blower_2_propulsion_modul_P->bpm_imp_speed_filt_num / - bpm_blower_2_propulsion_modul_P->bpm_imp_speed_filt_den; - - // Gain: '/Filter Coefficient' incorporates: - // DiscreteIntegrator: '/Filter' - // Gain: '/Derivative Gain' - // Sum: '/SumD' - - rtb_FilterCoefficient_l = (bpm_blower_2_propulsion_modul_P->bpm_imp_ctl_kd * - rtb_IntegralGain_b - bpm_blower_2_propulsion_modu_DW->Filter_DSTATE_m) * - bpm_blower_2_propulsion_modul_P->bpm_imp_ctl_filt_n; - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Integrator' - // Gain: '/Proportional Gain' - - rtb_Sum_l = (bpm_blower_2_propulsion_modul_P->bpm_imp_ctl_kp * - rtb_IntegralGain_b + - bpm_blower_2_propulsion_modu_DW->Integrator_DSTATE_c) + - rtb_FilterCoefficient_l; - - // DeadZone: '/DeadZone' - if (rtb_Sum_l > bpm_blower_2_propulsion_modul_P->bpm_imp_max_voltage) { - rtb_SignPreIntegrator_a = rtb_Sum_l - - bpm_blower_2_propulsion_modul_P->bpm_imp_max_voltage; - } else if (rtb_Sum_l >= - bpm_blower_2_propulsion_modul_P->DiscretePIDController_LowerSatu) { - rtb_SignPreIntegrator_a = 0.0F; - } else { - rtb_SignPreIntegrator_a = rtb_Sum_l - - bpm_blower_2_propulsion_modul_P->DiscretePIDController_LowerSatu; - } - - // End of DeadZone: '/DeadZone' - - // RelationalOperator: '/NotEqual' incorporates: - // Gain: '/ZeroGain' - - rtb_NotEqual_a = (bpm_blower_2_propulsion_modul_P->ZeroGain_Gain * rtb_Sum_l - != rtb_SignPreIntegrator_a); - - // Signum: '/SignDeltaU' - if (rtb_SignPreIntegrator_a < 0.0F) { - rtb_SignPreIntegrator_a = -1.0F; - } else if (rtb_SignPreIntegrator_a > 0.0F) { - rtb_SignPreIntegrator_a = 1.0F; - } else { - if (rtb_SignPreIntegrator_a == 0.0F) { - rtb_SignPreIntegrator_a = 0.0F; - } - } - - // End of Signum: '/SignDeltaU' - - // Gain: '/Integral Gain' - rtb_IntegralGain_b *= bpm_blower_2_propulsion_modul_P->bpm_imp_ctl_ki; - - // Saturate: '/Saturate' - if (rtb_Sum_l > bpm_blower_2_propulsion_modul_P->bpm_imp_max_voltage) { - rtb_Sum_l = bpm_blower_2_propulsion_modul_P->bpm_imp_max_voltage; - } else { - if (rtb_Sum_l < - bpm_blower_2_propulsion_modul_P->DiscretePIDController_LowerSatu) { - rtb_Sum_l = - bpm_blower_2_propulsion_modul_P->DiscretePIDController_LowerSatu; - } - } - - // End of Saturate: '/Saturate' - - // Switch: '/Switch2' incorporates: - // Inport: '/battery_voltage' - // RelationalOperator: '/LowerRelop1' - - if (rtb_Sum_l > bpm_blower_2_propulsion_modul_U_battery_voltage) { - rtb_Sum_l = bpm_blower_2_propulsion_modul_U_battery_voltage; - } else { - // Gain: '/Gain' - rtb_Gain_a = bpm_blower_2_propulsion_modul_P->Gain_Gain * - bpm_blower_2_propulsion_modul_U_battery_voltage; - - // Switch: '/Switch' incorporates: - // RelationalOperator: '/UpperRelop' - - if (rtb_Sum_l < rtb_Gain_a) { - rtb_Sum_l = rtb_Gain_a; - } - - // End of Switch: '/Switch' - } - - // End of Switch: '/Switch2' - - // Signum: '/SignPreIntegrator' - if (rtb_IntegralGain_b < 0.0F) { - rtb_IntegralGain_e = -1.0F; - } else if (rtb_IntegralGain_b > 0.0F) { - rtb_IntegralGain_e = 1.0F; - } else if (rtb_IntegralGain_b == 0.0F) { - rtb_IntegralGain_e = 0.0F; - } else { - rtb_IntegralGain_e = rtb_IntegralGain_b; - } - - // End of Signum: '/SignPreIntegrator' - - // DataTypeConversion: '/DataTypeConv2' - rtb_IntegralGain_e = (real32_T)floor((real_T)rtb_IntegralGain_e); - if (rtIsNaNF(rtb_IntegralGain_e) || rtIsInfF(rtb_IntegralGain_e)) { - rtb_IntegralGain_e = 0.0F; - } else { - rtb_IntegralGain_e = (real32_T)fmod((real_T)rtb_IntegralGain_e, (real_T) - 256.0F); - } - - // DataTypeConversion: '/DataTypeConv1' - if (rtb_SignPreIntegrator_a < 128.0F) { - rtb_SignPreIntegrator_o = (int8_T)rtb_SignPreIntegrator_a; - } else { - rtb_SignPreIntegrator_o = MAX_int8_T; - } - - // End of DataTypeConversion: '/DataTypeConv1' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/DataTypeConv2' - // Logic: '/AND' - // RelationalOperator: '/Equal' - - if (rtb_NotEqual_a && ((rtb_IntegralGain_e < 0.0F ? (int32_T)(int8_T)(int32_T) - -(int32_T)(int8_T)(uint8_T)-rtb_IntegralGain_e : - (int32_T)(int8_T)(uint8_T)rtb_IntegralGain_e) == - (int32_T)rtb_SignPreIntegrator_o)) { - rtb_IntegralGain_b = bpm_blower_2_propulsion_modul_P->Constant_Value; - } - - // End of Switch: '/Switch' - - // Update for DiscreteIntegrator: '/Integrator' - bpm_blower_2_propulsion_modu_DW->Integrator_DSTATE_c += - bpm_blower_2_propulsion_modul_P->Integrator_gainval * rtb_IntegralGain_b; - - // Update for DiscreteIntegrator: '/Filter' - bpm_blower_2_propulsion_modu_DW->Filter_DSTATE_m += - bpm_blower_2_propulsion_modul_P->Filter_gainval * rtb_FilterCoefficient_l; - - // End of Outputs for SubSystem: '/speed_controller' - - // Outputs for Atomic SubSystem: '/dc_motor_model' - // Gain: '/Gain5' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain4' - // Sum: '/Add' - - rtb_FilterCoefficient_l = (rtb_Sum_l - 1.0F / - bpm_blower_2_propulsion_modul_P->bpm_imp_motor_speed_k * - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE) * (1.0F / - bpm_blower_2_propulsion_modul_P->bpm_imp_motor_r); - - // Sum: '/Add1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain1' - // Gain: '/Gain6' - - rtb_IntegralGain_b = bpm_blower_2_propulsion_modul_P->bpm_imp_motor_torque_k * - rtb_FilterCoefficient_l - - bpm_blower_2_propulsion_modul_P->bpm_imp_motor_friction_coeff * - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE; - - // End of Outputs for SubSystem: '/dc_motor_model' - - // Outport: '/impeller_current' - *bpm_blower_2_propulsion_modul_Y_impeller_current = rtb_FilterCoefficient_l; - - // Outputs for Atomic SubSystem: '/servo_model' - // Backlash: '/Backlash1' - rtb_FilterCoefficient_l = - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_backlash_deadband / 2.0F; - - // Gain: '/Gain5' - rtb_SignPreIntegrator_a = 1.0F / - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_r; - for (i = 0; i < 6; i++) { - // Backlash: '/Backlash1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator4' - - if (bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] < - bpm_blower_2_propulsion_modu_DW->PrevY[i] - rtb_FilterCoefficient_l) { - rtb_Sum_l = - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] + - rtb_FilterCoefficient_l; - } else if (bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] - <= bpm_blower_2_propulsion_modu_DW->PrevY[i] + - rtb_FilterCoefficient_l) { - rtb_Sum_l = bpm_blower_2_propulsion_modu_DW->PrevY[i]; - } else { - rtb_Sum_l = - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] - - rtb_FilterCoefficient_l; - } - - // Gain: '/Gain1' - rtb_Gain_a = bpm_blower_2_propulsion_modul_P->bpm_servo_motor_gear_ratio * - rtb_Sum_l; - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant' - // Gain: '/Gain' - // Inport: '/servo_cmd' - // Sum: '/Sum' - - rtb_IntegralGain_e = (bpm_blower_2_propulsion_modul_U_servo_cmd[i] + - bpm_blower_2_propulsion_modul_P->bpm_servo_pwm2angle_bias) - * bpm_blower_2_propulsion_modul_P->bpm_servo_pwm2angle - rtb_Gain_a; - - // Gain: '/Filter Coefficient' incorporates: - // DiscreteIntegrator: '/Filter' - // Gain: '/Derivative Gain' - // Sum: '/SumD' - - tmp = (bpm_blower_2_propulsion_modul_P->bpm_servo_ctl_kd * - rtb_IntegralGain_e - bpm_blower_2_propulsion_modu_DW->Filter_DSTATE[i]) - * bpm_blower_2_propulsion_modul_P->bpm_servo_ctl_filt_n; - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Integrator' - // Gain: '/Proportional Gain' - - rtb_SignDeltaU_g = (bpm_blower_2_propulsion_modul_P->bpm_servo_ctl_kp * - rtb_IntegralGain_e + - bpm_blower_2_propulsion_modu_DW->Integrator_DSTATE[i]) + - tmp; - - // Saturate: '/Saturate' - if (rtb_SignDeltaU_g > - bpm_blower_2_propulsion_modul_P->bpm_servo_max_voltage) { - rtb_SignDeltaU_1 = bpm_blower_2_propulsion_modul_P->bpm_servo_max_voltage; - } else if (rtb_SignDeltaU_g < - bpm_blower_2_propulsion_modul_P->DiscretePIDController_LowerSa_d) - { - rtb_SignDeltaU_1 = - bpm_blower_2_propulsion_modul_P->DiscretePIDController_LowerSa_d; - } else { - rtb_SignDeltaU_1 = rtb_SignDeltaU_g; - } - - // End of Saturate: '/Saturate' - - // Gain: '/Gain5' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - // Gain: '/Gain7' - // Sum: '/Add3' - - rtb_current[i] = (rtb_SignDeltaU_1 - - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_k * - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE - [i]) * rtb_SignPreIntegrator_a; - - // Gain: '/ZeroGain' - rtb_SignDeltaU_1 = bpm_blower_2_propulsion_modul_P->ZeroGain_Gain_b * - rtb_SignDeltaU_g; - - // DeadZone: '/DeadZone' - if (rtb_SignDeltaU_g > - bpm_blower_2_propulsion_modul_P->bpm_servo_max_voltage) { - rtb_SignDeltaU_g -= bpm_blower_2_propulsion_modul_P->bpm_servo_max_voltage; - } else if (rtb_SignDeltaU_g >= - bpm_blower_2_propulsion_modul_P->DiscretePIDController_LowerSa_d) - { - rtb_SignDeltaU_g = 0.0F; - } else { - rtb_SignDeltaU_g -= - bpm_blower_2_propulsion_modul_P->DiscretePIDController_LowerSa_d; - } - - // End of DeadZone: '/DeadZone' - - // RelationalOperator: '/NotEqual' - rtb_NotEqual[i] = (rtb_SignDeltaU_1 != rtb_SignDeltaU_g); - - // Signum: '/SignDeltaU' - if (rtb_SignDeltaU_g < 0.0F) { - rtb_SignDeltaU_g = -1.0F; - } else if (rtb_SignDeltaU_g > 0.0F) { - rtb_SignDeltaU_g = 1.0F; - } else { - if (rtb_SignDeltaU_g == 0.0F) { - rtb_SignDeltaU_g = 0.0F; - } - } - - // End of Signum: '/SignDeltaU' - - // Gain: '/Integral Gain' - rtb_IntegralGain_e *= bpm_blower_2_propulsion_modul_P->bpm_servo_ctl_ki; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator4' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] += - bpm_blower_2_propulsion_modul_P->DiscreteTimeIntegrator4_gainval * - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE[i]; - - // Backlash: '/Backlash1' - Backlash1[i] = rtb_Sum_l; - - // Gain: '/Gain1' - rtb_Gain1_d[i] = rtb_Gain_a; - - // Sum: '/Sum1' - rtb_IntegralGain[i] = rtb_IntegralGain_e; - - // Gain: '/Filter Coefficient' - rtb_FilterCoefficient[i] = tmp; - - // Sum: '/Sum' - rtb_SignDeltaU[i] = rtb_SignDeltaU_g; - } - - // Gain: '/Gain6' - rtb_SignPreIntegrator_a = 1.0F / - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_gear_box_inertia; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator4' - rtb_FilterCoefficient_l = bpm_blower_2_propulsion_modul_P->bpm_servo_max_theta - / bpm_blower_2_propulsion_modul_P->bpm_servo_motor_gear_ratio; - rtb_Sum_l = (real32_T)bpm_blower_2_propulsion_modul_P->bpm_servo_min_theta / - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_gear_ratio; - rtb_Gain_a = bpm_blower_2_propulsion_modul_P->bpm_servo_max_theta / - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_gear_ratio; - tmp = (real32_T)bpm_blower_2_propulsion_modul_P->bpm_servo_min_theta / - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_gear_ratio; - for (i = 0; i < 6; i++) { - if (bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] >= - rtb_FilterCoefficient_l) { - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] = - rtb_Gain_a; - } else { - if (bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] <= - rtb_Sum_l) { - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] = tmp; - } - } - - // Update for Backlash: '/Backlash1' - bpm_blower_2_propulsion_modu_DW->PrevY[i] = Backlash1[i]; - - // Update for DiscreteIntegrator: '/Integrator' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/DataTypeConv1' - // DataTypeConversion: '/DataTypeConv2' - // Logic: '/AND' - // RelationalOperator: '/Equal' - // Signum: '/SignPreIntegrator' - // Switch: '/Switch' - - if (rtb_IntegralGain[i] < 0.0F) { - rtb_IntegralGain_e = -1.0F; - } else if (rtb_IntegralGain[i] > 0.0F) { - rtb_IntegralGain_e = 1.0F; - } else if (rtb_IntegralGain[i] == 0.0F) { - rtb_IntegralGain_e = 0.0F; - } else { - rtb_IntegralGain_e = rtb_IntegralGain[i]; - } - - rtb_IntegralGain_e = (real32_T)floor((real_T)rtb_IntegralGain_e); - if (rtIsNaNF(rtb_IntegralGain_e) || rtIsInfF(rtb_IntegralGain_e)) { - rtb_IntegralGain_e = 0.0F; - } else { - rtb_IntegralGain_e = (real32_T)fmod((real_T)rtb_IntegralGain_e, (real_T) - 256.0F); - } - - if (rtb_SignDeltaU[i] < 128.0F) { - rtb_SignPreIntegrator_o = (int8_T)rtb_SignDeltaU[i]; - } else { - rtb_SignPreIntegrator_o = MAX_int8_T; - } - - if (rtb_NotEqual[i] && ((rtb_IntegralGain_e < 0.0F ? (int32_T)(int8_T) - (int32_T)-(int32_T)(int8_T)(uint8_T)-rtb_IntegralGain_e : (int32_T) - (int8_T)(uint8_T)rtb_IntegralGain_e) == (int32_T) - rtb_SignPreIntegrator_o)) { - rtb_IntegralGain_e = bpm_blower_2_propulsion_modul_P->Constant_Value_e; - } else { - rtb_IntegralGain_e = rtb_IntegralGain[i]; - } - - bpm_blower_2_propulsion_modu_DW->Integrator_DSTATE[i] += - bpm_blower_2_propulsion_modul_P->Integrator_gainval_i * rtb_IntegralGain_e; - - // End of Update for DiscreteIntegrator: '/Integrator' - - // Update for DiscreteIntegrator: '/Filter' - bpm_blower_2_propulsion_modu_DW->Filter_DSTATE[i] += - bpm_blower_2_propulsion_modul_P->Filter_gainval_d * - rtb_FilterCoefficient[i]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - // Gain: '/Gain11' - // Gain: '/Gain6' - // Gain: '/Gain9' - // Sum: '/Add1' - - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE[i] += - (bpm_blower_2_propulsion_modul_P->bpm_servo_motor_k * rtb_current[i] - - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_friction_coeff * - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE[i]) * - rtb_SignPreIntegrator_a * - bpm_blower_2_propulsion_modul_P->DiscreteTimeIntegrator3_gainval; - } - - // End of Outputs for SubSystem: '/servo_model' - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_SignPreIntegrator_a = - bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_2_propulsion_modul_P->bmp_PM2_impeller_orientation_error[0] + - bpm_blower_2_propulsion_modul_P->abp_pm2_impeller_orientation[0]; - - // DotProduct: '/Dot Product' - rtb_IntegralGain_e = rtb_SignPreIntegrator_a * rtb_SignPreIntegrator_a; - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_Add1[0] = rtb_SignPreIntegrator_a; - rtb_SignPreIntegrator_a = - bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_2_propulsion_modul_P->bmp_PM2_impeller_orientation_error[1] + - bpm_blower_2_propulsion_modul_P->abp_pm2_impeller_orientation[1]; - - // DotProduct: '/Dot Product' - rtb_IntegralGain_e += rtb_SignPreIntegrator_a * rtb_SignPreIntegrator_a; - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_Add1[1] = rtb_SignPreIntegrator_a; - rtb_SignPreIntegrator_a = - bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_2_propulsion_modul_P->bmp_PM2_impeller_orientation_error[2] + - bpm_blower_2_propulsion_modul_P->abp_pm2_impeller_orientation[2]; - - // DotProduct: '/Dot Product' - rtb_IntegralGain_e += rtb_SignPreIntegrator_a * rtb_SignPreIntegrator_a; - - // Sum: '/Add1' - rtb_Add1[2] = rtb_SignPreIntegrator_a; - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_FilterCoefficient_l = (real32_T)sqrt((real_T)rtb_IntegralGain_e); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Product: '/Divide' - - if ((real_T)rtb_FilterCoefficient_l > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - rtb_Add1[0] /= rtb_FilterCoefficient_l; - rtb_Add1[1] /= rtb_FilterCoefficient_l; - rtb_Add1[2] = rtb_SignPreIntegrator_a / rtb_FilterCoefficient_l; - - // End of Outputs for SubSystem: '/Normalize' - } - - // End of If: '/If' - - // Gain: '/Gain1' - rtb_Sum_l = bpm_blower_2_propulsion_modul_P->Gain1_Gain * rtb_IntegralGain_b; - - // Product: '/Product1' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant4' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain1' - // Sum: '/Add3' - - rtb_FilterCoefficient_l = - (bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_2_propulsion_modul_P->bpm_impeller_inertia_error + - bpm_blower_2_propulsion_modul_P->bpm_impeller_inertia) * - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE; - - // Outputs for Enabled SubSystem: '/latch_nozzle_thrust_matricies' incorporates: - // EnablePort: '/Enable' - - // UnitDelay: '/Unit Delay' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant5' - // Constant: '/Constant8' - // Switch: '/Switch' - - if (bpm_blower_2_propulsion_modu_DW->UnitDelay_DSTATE) { - if (bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag > - bpm_blower_2_propulsion_modul_P->Switch_Threshold) { - rtb_Switch_b_1 = - &bpm_blower_2_propulsion_modul_P->bpm_PM2_Q_nozzle2misaligned[0]; - } else { - rtb_Switch_b_1 = &bpm_blower_2_propulsion_modul_P->Constant2_Value[0]; - } - - // Outputs for Iterator SubSystem: '/For Each Subsystem' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr = 0; ForEach_itr < 6; ForEach_itr++) { - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_SignPreIntegrator_a = rtb_Switch_b_1[(int32_T)(18 + ForEach_itr)] * - rtb_Switch_b_1[(int32_T)(18 + ForEach_itr)] * - bpm_blower_2_propulsion_modul_P->CoreSubsys.Gain_Gain - (real32_T) - bpm_blower_2_propulsion_modul_P->CoreSubsys.Constant1_Value; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment[i] = (real32_T) - bpm_blower_2_propulsion_modul_P->CoreSubsys.Constant2_Value[i]; - } - - rtb_Assignment[0] = rtb_SignPreIntegrator_a; - rtb_Assignment[4] = rtb_SignPreIntegrator_a; - rtb_Assignment[8] = rtb_SignPreIntegrator_a; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - - rtb_SignPreIntegrator_a = rtb_Switch_b_1[(int32_T)(18 + ForEach_itr)] * - bpm_blower_2_propulsion_modul_P->CoreSubsys.Gain1_Gain; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_2[0] = (real32_T) - bpm_blower_2_propulsion_modul_P->CoreSubsys.Constant3_Value; - tmp_2[1] = rtb_Switch_b_1[(int32_T)(12 + ForEach_itr)]; - tmp_2[2] = rtb_Switch_b_1[(int32_T)(6 + ForEach_itr)] * - bpm_blower_2_propulsion_modul_P->CoreSubsys.Gain_Gain_b; - tmp_2[3] = rtb_Switch_b_1[(int32_T)(12 + ForEach_itr)] * - bpm_blower_2_propulsion_modul_P->CoreSubsys.Gain1_Gain_a; - tmp_2[4] = (real32_T) - bpm_blower_2_propulsion_modul_P->CoreSubsys.Constant3_Value; - tmp_2[5] = rtb_Switch_b_1[ForEach_itr]; - tmp_2[6] = rtb_Switch_b_1[(int32_T)(ForEach_itr + 6)]; - tmp_2[7] = bpm_blower_2_propulsion_modul_P->CoreSubsys.Gain2_Gain * - rtb_Switch_b_1[ForEach_itr]; - tmp_2[8] = (real32_T) - bpm_blower_2_propulsion_modul_P->CoreSubsys.Constant3_Value; - - // Product: '/Product1' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain2' - - for (i = 0; i < 3; i++) { - rtb_Switch_b_0[i] = rtb_Switch_b_1[(int32_T)((int32_T)(6 * i) + - ForEach_itr)] * rtb_Switch_b_1[ForEach_itr]; - rtb_Switch_b_0[(int32_T)(i + 3)] = rtb_Switch_b_1[(int32_T)((int32_T)(6 * - i) + ForEach_itr)] * rtb_Switch_b_1[(int32_T)(ForEach_itr + 6)]; - rtb_Switch_b_0[(int32_T)(i + 6)] = rtb_Switch_b_1[(int32_T)((int32_T)(6 * - i) + ForEach_itr)] * rtb_Switch_b_1[(int32_T)(ForEach_itr + 12)]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_0[(int32_T)(3 * i)] = (rtb_Assignment[(int32_T)(3 * i)] - - tmp_2[(int32_T)(3 * i)] * rtb_SignPreIntegrator_a) + rtb_Switch_b_0 - [(int32_T)(3 * i)] * - bpm_blower_2_propulsion_modul_P->CoreSubsys.Gain2_Gain_b; - rtb_Assignment_0[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i) + 1)] - tmp_2[(int32_T)((int32_T)(3 * i) + - 1)] * rtb_SignPreIntegrator_a) + rtb_Switch_b_0[(int32_T)((int32_T)(3 * - i) + 1)] * bpm_blower_2_propulsion_modul_P->CoreSubsys.Gain2_Gain_b; - rtb_Assignment_0[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i) + 2)] - tmp_2[(int32_T)((int32_T)(3 * i) + - 2)] * rtb_SignPreIntegrator_a) + rtb_Switch_b_0[(int32_T)((int32_T)(3 * - i) + 2)] * bpm_blower_2_propulsion_modul_P->CoreSubsys.Gain2_Gain_b; - } - - // End of Sum: '/Sum1' - for (i = 0; i < 3; i++) { - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_rotated_vectors_at_inport_0' incorporates: - // Constant: '/Constant3' - // ForEachSliceSelector: '/ImpSel_InsertedFor_unit_vectors_at_outport_0' - // Product: '/Product' - - rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(ForEach_itr + (int32_T)(6 * i))] - = rtb_Assignment_0[(int32_T)(i + 6)] * - bpm_blower_2_propulsion_modul_P->abp_PM2_nozzle_orientations[(int32_T) - (ForEach_itr + 12)] + (rtb_Assignment_0[(int32_T)(i + 3)] * - bpm_blower_2_propulsion_modul_P->abp_PM2_nozzle_orientations[(int32_T) - (ForEach_itr + 6)] + rtb_Assignment_0[i] * - bpm_blower_2_propulsion_modul_P-> - abp_PM2_nozzle_orientations[ForEach_itr]); - } - } - - // End of Outputs for SubSystem: '/For Each Subsystem' - // MATLAB Function 'bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/MATLAB Function': ':1' - // NOTE: if number of nozzles change, must change this hard coded '6' below - // ':1:5' - for (i = 0; i < 3; i++) { - // Sum: '/Sum2' incorporates: - // Constant: '/Constant7' - // Gain: '/Gain2' - // Inport: '/veh_cm' - - rtb_Sum2[i] = (real_T) - bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_2_propulsion_modul_P->abp_P_CG_B_B_error[i] + (real_T) - bpm_blower_2_propulsion_modul_U_veh_cm[i]; - - // MATLAB Function: '/MATLAB Function' - ibmat = (int32_T)(i * 6); - for (itilerow = 0; itilerow < 6; itilerow++) { - b[(int32_T)(ibmat + itilerow)] = rtb_Sum2[i]; - } - } - - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant4' - // Constant: '/Constant5' - // Constant: '/Constant8' - // Gain: '/Gain' - // Sum: '/Sum' - // Switch: '/Switch' - - for (i = 0; i < 18; i++) { - nozzle_moment_arm[i] = - (bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag * - bpm_blower_2_propulsion_modul_P->bpm_PM2_P_nozzle_B_B_error[i] + - bpm_blower_2_propulsion_modul_P->abp_PM2_P_nozzle_B_B[i]) - (real32_T) - b[i]; - } - - // [m] Distance from nozzles to CG - // ':1:7' - // [-] Converts a nozzle thrust into resulting body torques - // ':1:8' - // [-] Converts a nozzle thrust into resulting body forces - for (i = 0; i < 6; i++) { - c[i] = nozzle_moment_arm[(int32_T)(i + 6)] * - rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + 12)] - nozzle_moment_arm - [(int32_T)(i + 12)] * rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + 6)]; - c[(int32_T)(i + 6)] = nozzle_moment_arm[(int32_T)(i + 12)] * - rtb_ImpAsg_InsertedFor_rotated_[i] - rtb_ImpAsg_InsertedFor_rotated_ - [(int32_T)(i + 12)] * nozzle_moment_arm[i]; - c[(int32_T)(i + 12)] = rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + 6)] * - nozzle_moment_arm[i] - nozzle_moment_arm[(int32_T)(i + 6)] * - rtb_ImpAsg_InsertedFor_rotated_[i]; - - // SignalConversion: '/OutportBufferForthrust2force_B' - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2force_B[(int32_T) - (3 * i)] = -rtb_ImpAsg_InsertedFor_rotated_[i]; - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2force_B[(int32_T) - (1 + (int32_T)(3 * i))] = -rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + - 6)]; - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2force_B[(int32_T) - (2 + (int32_T)(3 * i))] = -rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(i + - 12)]; - - // SignalConversion: '/OutportBufferForthrust2torque_B' - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2torque_B[(int32_T) - (3 * i)] = -c[i]; - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2torque_B[(int32_T) - (1 + (int32_T)(3 * i))] = -c[(int32_T)(i + 6)]; - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2torque_B[(int32_T) - (2 + (int32_T)(3 * i))] = -c[(int32_T)(i + 12)]; - } - } - - // End of UnitDelay: '/Unit Delay' - // End of Outputs for SubSystem: '/latch_nozzle_thrust_matricies' - - // Outputs for Atomic SubSystem: '/calc_nozzle_area' - // Gain: '/Gain12' - rtb_SignPreIntegrator_a = 1.0F / - bpm_blower_2_propulsion_modul_P->abp_nozzle_gear_ratio; - for (i = 0; i < 6; i++) { - rtb_Gain_a = rtb_SignPreIntegrator_a * rtb_Gain1_d[i]; - - // Product: '/Product2' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant4' - // Constant: '/Constant5' - // Constant: '/Constant6' - // Product: '/Product1' - // Sum: '/Subtract' - // Sum: '/Subtract1' - // Trigonometry: '/Trigonometric Function' - - Backlash1[i] = (bpm_blower_2_propulsion_modul_P->abp_nozzle_intake_height - - (real32_T)cos((real_T)(rtb_Gain_a + - bpm_blower_2_propulsion_modul_P->abp_nozzle_min_open_angle)) * - bpm_blower_2_propulsion_modul_P->abp_nozzle_flap_length) * - bpm_blower_2_propulsion_modul_P->abp_PM2_nozzle_widths[i] * - bpm_blower_2_propulsion_modul_P->abp_nozzle_flap_count; - rtb_Gain1_d[i] = rtb_Gain_a; - } - - // End of Gain: '/Gain12' - // End of Outputs for SubSystem: '/calc_nozzle_area' - - // Outputs for Atomic SubSystem: '/blower_aerodynamics' - - // DiscreteIntegrator: '/Discrete-Time Integrator' - bpm_blower__blower_aerodynamics - (bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE, Backlash1, - &bpm_blower_2_propulsion_modul_B->blower_aerodynamics, - &bpm_blower_2_propulsion_modu_DW->blower_aerodynamics, - (P_blower_aerodynamics_bpm_blo_T *) - &bpm_blower_2_propulsion_modul_P->blower_aerodynamics, - bpm_blower_2_propulsion_modul_P->abp_pm2_zero_thrust_area, - bpm_blower_2_propulsion_modul_P->bpm_PM2_zero_thrust_area_error, - bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag, - bpm_blower_2_propulsion_modul_P->abp_PM2_discharge_coeff, - bpm_blower_2_propulsion_modul_P->bpm_PM2_nozzle_discharge_coeff_error, - bpm_blower_2_propulsion_modul_P->bpm_lookup_Cdp_data, - bpm_blower_2_propulsion_modul_P->bpm_lookup_totalarea_breakpoints, - bpm_blower_2_propulsion_modul_P->abp_impeller_diameter, - bpm_blower_2_propulsion_modul_P->const_air_density, - bpm_blower_2_propulsion_modul_P->tun_bpm_PM2_thrust_error_sf, - bpm_blower_2_propulsion_modul_P->bpm_PM2_nozzle_noise_feedback_gain); - - // End of Outputs for SubSystem: '/blower_aerodynamics' - - // Outport: '/servo_current' - for (i = 0; i < 6; i++) { - bpm_blower_2_propulsion_modul_Y_servo_current[i] = rtb_current[i]; - } - - // End of Outport: '/servo_current' - - // SignalConversion: '/TmpSignal ConversionAtProductInport1' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Inport: '/omega_B_ECI_B' - // Product: '/Product' - - tmp_0[0] = (real32_T)bpm_blower_2_propulsion_modul_P->Constant3_Value; - tmp_0[1] = bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[2]; - tmp_0[2] = bpm_blower_2_propulsion_modul_P->Gain_Gain_h * - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[1]; - tmp_0[3] = bpm_blower_2_propulsion_modul_P->Gain1_Gain_e * - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[2]; - tmp_0[4] = (real32_T)bpm_blower_2_propulsion_modul_P->Constant3_Value; - tmp_0[5] = bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[0]; - tmp_0[6] = bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[1]; - tmp_0[7] = bpm_blower_2_propulsion_modul_P->Gain2_Gain * - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[0]; - tmp_0[8] = (real32_T)bpm_blower_2_propulsion_modul_P->Constant3_Value; - for (i = 0; i < 3; i++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - tmp_1[i] = 0.0F; - for (ibmat = 0; ibmat < 6; ibmat++) { - tmp_1[i] += - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2torque_B - [(int32_T)((int32_T)(3 * ibmat) + i)] * - bpm_blower_2_propulsion_modul_B->blower_aerodynamics.Add1[ibmat]; - } - - // End of Product: '/Product4' - - // Outport: '/torque_B' incorporates: - // Product: '/Product' - // Product: '/Product' - // Product: '/Product2' - // Sum: '/Add2' - - bpm_blower_2_propulsion_modul_Y_torque_B[i] = (((rtb_Add1[0] * - rtb_FilterCoefficient_l * tmp_0[i] + tmp_0[(int32_T)(i + 3)] * (rtb_Add1[1] - * rtb_FilterCoefficient_l)) + tmp_0[(int32_T)(i + 6)] * (rtb_Add1[2] * - rtb_FilterCoefficient_l)) + rtb_Add1[i] * rtb_Sum_l) + tmp_1[i]; - - // Outport: '/force_B' incorporates: - // Product: '/Product3' - - bpm_blower_2_propulsion_modul_Y_force_B[i] = 0.0F; - for (ibmat = 0; ibmat < 6; ibmat++) { - bpm_blower_2_propulsion_modul_Y_force_B[i] += - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2force_B[(int32_T) - ((int32_T)(3 * ibmat) + i)] * - bpm_blower_2_propulsion_modul_B->blower_aerodynamics.Add1[ibmat]; - } - - // End of Outport: '/force_B' - } - - // Outport: '/motor_speed' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - *bpm_blower_2_propulsion_modul_Y_motor_speed = - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE; - - // Outport: '/nozzle_theta' - for (i = 0; i < 6; i++) { - bpm_blower_2_propulsion_modul_Y_nozzle_theta[i] = rtb_Gain1_d[i]; - } - - // End of Outport: '/nozzle_theta' - - // DataTypeConversion: '/Data Type Conversion' incorporates: - // Gain: '/Gain1' - // RandomNumber: '/random_noise1' - - rtb_SignPreIntegrator_a = (real32_T)(1.0 / sqrt - (bpm_blower_2_propulsion_modul_P->astrobee_time_step_size) * - bpm_blower_2_propulsion_modu_DW->NextOutput); - - // Quantizer: '/Quantizer' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain3' - // Gain: '/Gain4' - // RandomNumber: '/random_noise' - // Sum: '/Sum1' - - rtb_FilterCoefficient_l = rt_roundf_snf((((real32_T)(1.0 / sqrt - (bpm_blower_2_propulsion_modul_P->astrobee_time_step_size) * - bpm_blower_2_propulsion_modu_DW->NextOutput_p) + - bpm_blower_2_propulsion_modul_P->bpm_sensor_sf * - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE) + - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator1_DSTATE) / - bpm_blower_2_propulsion_modul_P->bpm_sensor_resolution) * - bpm_blower_2_propulsion_modul_P->bpm_sensor_resolution; - - // Outport: '/meas_motor_speed' incorporates: - // Delay: '/Delay2' - - *bpm_blower_2_propulsion_modul_Y_meas_motor_speed = - bpm_blower_2_propulsion_modu_DW->Delay2_DSTATE; - - // Sum: '/Add' - rtb_IntegralGain_b -= - bpm_blower_2_propulsion_modul_B->blower_aerodynamics.Constant7; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - // Gain: '/Gain' - // Gain: '/Gain2' - - if (bpm_blower_2_propulsion_modul_P->tun_bpm_noise_on_flag > - bpm_blower_2_propulsion_modul_P->Switch_Threshold_f) { - rtb_IntegralGain_b *= 1.0F / - bpm_blower_2_propulsion_modul_P->bpm_impeller_inertia; - } else { - rtb_IntegralGain_b *= 1.0F / - (bpm_blower_2_propulsion_modul_P->bpm_impeller_inertia + - bpm_blower_2_propulsion_modul_P->bpm_impeller_inertia_error); - } - - // End of Switch: '/Switch' - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE += - bpm_blower_2_propulsion_modul_P->DiscreteTimeIntegrator_gainval * - rtb_IntegralGain_b; - - // Update for UnitDelay: '/Unit Delay' incorporates: - // Inport: '/veh_cm' - // Logic: '/Logical Operator' - // RelationalOperator: '/FixPt Relational Operator' - // UnitDelay: '/Delay Input1' - - bpm_blower_2_propulsion_modu_DW->UnitDelay_DSTATE = - ((bpm_blower_2_propulsion_modul_U_veh_cm[0] != - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[0]) || - (bpm_blower_2_propulsion_modul_U_veh_cm[1] != - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[1]) || - (bpm_blower_2_propulsion_modul_U_veh_cm[2] != - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[2])); - - // Update for RandomNumber: '/random_noise1' - bpm_blower_2_propulsion_modu_DW->NextOutput = rt_nrand_Upu32_Yd_f_pw_snf - (&bpm_blower_2_propulsion_modu_DW->RandSeed) * - bpm_blower_2_propulsion_modul_P->random_noise1_StdDev + - bpm_blower_2_propulsion_modul_P->random_noise1_Mean; - - // Update for RandomNumber: '/random_noise' - bpm_blower_2_propulsion_modu_DW->NextOutput_p = rt_nrand_Upu32_Yd_f_pw_snf - (&bpm_blower_2_propulsion_modu_DW->RandSeed_o) * - bpm_blower_2_propulsion_modul_P->random_noise_StdDev + - bpm_blower_2_propulsion_modul_P->random_noise_Mean; - - // Saturate: '/Saturation' - if (rtb_FilterCoefficient_l > bpm_blower_2_propulsion_modul_P->bpm_sensor_max) - { - // Update for Delay: '/Delay2' - bpm_blower_2_propulsion_modu_DW->Delay2_DSTATE = - bpm_blower_2_propulsion_modul_P->bpm_sensor_max; - } else if (rtb_FilterCoefficient_l < - bpm_blower_2_propulsion_modul_P->bpm_sensor_min) { - // Update for Delay: '/Delay2' - bpm_blower_2_propulsion_modu_DW->Delay2_DSTATE = - bpm_blower_2_propulsion_modul_P->bpm_sensor_min; - } else { - // Update for Delay: '/Delay2' - bpm_blower_2_propulsion_modu_DW->Delay2_DSTATE = rtb_FilterCoefficient_l; - } - - // End of Saturate: '/Saturation' - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator1_DSTATE += - bpm_blower_2_propulsion_modul_P->DiscreteTimeIntegrator1_gainval * - rtb_SignPreIntegrator_a; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/veh_cm' - - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[0] = - bpm_blower_2_propulsion_modul_U_veh_cm[0]; - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[1] = - bpm_blower_2_propulsion_modul_U_veh_cm[1]; - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[2] = - bpm_blower_2_propulsion_modul_U_veh_cm[2]; -} - -// Model initialize function -void bpm_blower_2_propulsion_module_initialize(RT_MODEL_bpm_blower_2_propuls_T * - const bpm_blower_2_propulsion_modu_M, real32_T - *bpm_blower_2_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - *bpm_blower_2_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_2_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_2_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_2_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_2_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_2_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_2_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_2_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_2_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_2_propulsion_modul_Y_meas_motor_speed) -{ - P_bpm_blower_2_propulsion_mod_T *bpm_blower_2_propulsion_modul_P = - ((P_bpm_blower_2_propulsion_mod_T *) - bpm_blower_2_propulsion_modu_M->defaultParam); - B_bpm_blower_2_propulsion_mod_T *bpm_blower_2_propulsion_modul_B = - ((B_bpm_blower_2_propulsion_mod_T *) bpm_blower_2_propulsion_modu_M->blockIO); - DW_bpm_blower_2_propulsion_mo_T *bpm_blower_2_propulsion_modu_DW = - ((DW_bpm_blower_2_propulsion_mo_T *) bpm_blower_2_propulsion_modu_M->dwork); - - { - uint32_T tseed; - int32_T t; - real32_T y; - int32_T i; - real_T tmp; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator_DSTATE = - bpm_blower_2_propulsion_modul_P->bpm_impeller_init_speed; - - // InitializeConditions for UnitDelay: '/Unit Delay' - bpm_blower_2_propulsion_modu_DW->UnitDelay_DSTATE = - bpm_blower_2_propulsion_modul_P->UnitDelay_InitialCondition; - - // InitializeConditions for RandomNumber: '/random_noise1' - tmp = floor(bpm_blower_2_propulsion_modul_P->bpm_sensor_rand_seed + 1.0); - if (rtIsNaN(tmp) || rtIsInf(tmp)) { - tmp = 0.0; - } else { - tmp = fmod(tmp, 4.294967296E+9); - } - - tseed = tmp < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-tmp : (uint32_T) - tmp; - i = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)i << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)i); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - bpm_blower_2_propulsion_modu_DW->RandSeed = tseed; - bpm_blower_2_propulsion_modu_DW->NextOutput = rt_nrand_Upu32_Yd_f_pw_snf - (&bpm_blower_2_propulsion_modu_DW->RandSeed) * - bpm_blower_2_propulsion_modul_P->random_noise1_StdDev + - bpm_blower_2_propulsion_modul_P->random_noise1_Mean; - - // End of InitializeConditions for RandomNumber: '/random_noise1' - - // InitializeConditions for RandomNumber: '/random_noise' - tmp = floor(bpm_blower_2_propulsion_modul_P->bpm_sensor_rand_seed); - if (rtIsNaN(tmp) || rtIsInf(tmp)) { - tmp = 0.0; - } else { - tmp = fmod(tmp, 4.294967296E+9); - } - - tseed = tmp < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-tmp : (uint32_T) - tmp; - i = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)i << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)i); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - bpm_blower_2_propulsion_modu_DW->RandSeed_o = tseed; - bpm_blower_2_propulsion_modu_DW->NextOutput_p = rt_nrand_Upu32_Yd_f_pw_snf - (&bpm_blower_2_propulsion_modu_DW->RandSeed_o) * - bpm_blower_2_propulsion_modul_P->random_noise_StdDev + - bpm_blower_2_propulsion_modul_P->random_noise_Mean; - - // End of InitializeConditions for RandomNumber: '/random_noise' - - // InitializeConditions for Delay: '/Delay2' - bpm_blower_2_propulsion_modu_DW->Delay2_DSTATE = - bpm_blower_2_propulsion_modul_P->Delay2_InitialCondition; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator1_DSTATE = - bpm_blower_2_propulsion_modul_P->DiscreteTimeIntegrator1_IC; - - // InitializeConditions for UnitDelay: '/Delay Input1' - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[0] = - bpm_blower_2_propulsion_modul_P->DetectChange_vinit; - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[1] = - bpm_blower_2_propulsion_modul_P->DetectChange_vinit; - bpm_blower_2_propulsion_modu_DW->DelayInput1_DSTATE[2] = - bpm_blower_2_propulsion_modul_P->DetectChange_vinit; - - // SystemInitialize for Atomic SubSystem: '/speed_controller' - // InitializeConditions for RateLimiter: '/Rate Limiter' - bpm_blower_2_propulsion_modu_DW->PrevY_c = - bpm_blower_2_propulsion_modul_P->RateLimiter_IC; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - bpm_blower_2_propulsion_modu_DW->Integrator_DSTATE_c = - bpm_blower_2_propulsion_modul_P->Integrator_IC; - - // InitializeConditions for DiscreteIntegrator: '/Filter' - bpm_blower_2_propulsion_modu_DW->Filter_DSTATE_m = - bpm_blower_2_propulsion_modul_P->Filter_IC; - - // End of SystemInitialize for SubSystem: '/speed_controller' - - // SystemInitialize for Atomic SubSystem: '/servo_model' - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator4' - y = (real32_T)bpm_blower_2_propulsion_modul_P->bpm_servo_min_theta / - bpm_blower_2_propulsion_modul_P->bpm_servo_motor_gear_ratio; - for (i = 0; i < 6; i++) { - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator4_DSTATE[i] = y; - - // InitializeConditions for Backlash: '/Backlash1' - bpm_blower_2_propulsion_modu_DW->PrevY[i] = - bpm_blower_2_propulsion_modul_P->Backlash1_InitialOutput; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - bpm_blower_2_propulsion_modu_DW->Integrator_DSTATE[i] = - bpm_blower_2_propulsion_modul_P->Integrator_IC_i; - - // InitializeConditions for DiscreteIntegrator: '/Filter' - bpm_blower_2_propulsion_modu_DW->Filter_DSTATE[i] = - bpm_blower_2_propulsion_modul_P->Filter_IC_d; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator3' - bpm_blower_2_propulsion_modu_DW->DiscreteTimeIntegrator3_DSTATE[i] = - bpm_blower_2_propulsion_modul_P->DiscreteTimeIntegrator3_IC; - } - - // End of InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator4' - // End of SystemInitialize for SubSystem: '/servo_model' - - // SystemInitialize for Enabled SubSystem: '/latch_nozzle_thrust_matricies' - for (i = 0; i < 18; i++) { - // SystemInitialize for Outport: '/thrust2torque_B' - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2torque_B[i] = - bpm_blower_2_propulsion_modul_P->thrust2torque_B_Y0; - - // SystemInitialize for Outport: '/thrust2force_B' - bpm_blower_2_propulsion_modul_B->OutportBufferForthrust2force_B[i] = - bpm_blower_2_propulsion_modul_P->thrust2force_B_Y0; - } - - // End of SystemInitialize for SubSystem: '/latch_nozzle_thrust_matricies' - - // SystemInitialize for Atomic SubSystem: '/blower_aerodynamics' - bpm_bl_blower_aerodynamics_Init - (&bpm_blower_2_propulsion_modu_DW->blower_aerodynamics, - (P_blower_aerodynamics_bpm_blo_T *) - &bpm_blower_2_propulsion_modul_P->blower_aerodynamics, - bpm_blower_2_propulsion_modul_P->bpm_PM2_randn_noise_seed); - - // End of SystemInitialize for SubSystem: '/blower_aerodynamics' - } -} - -// Model terminate function -void bpm_blower_2_propulsion_module_terminate(RT_MODEL_bpm_blower_2_propuls_T - * bpm_blower_2_propulsion_modu_M) -{ - // model code - rt_FREE(bpm_blower_2_propulsion_modu_M->blockIO); - if (bpm_blower_2_propulsion_modu_M->paramIsMalloced) { - rt_FREE(bpm_blower_2_propulsion_modu_M->defaultParam); - } - - rt_FREE(bpm_blower_2_propulsion_modu_M->dwork); - rt_FREE(bpm_blower_2_propulsion_modu_M); -} - -// Model data allocation function -RT_MODEL_bpm_blower_2_propuls_T *bpm_blower_2_propulsion_module(real32_T - *bpm_blower_2_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - *bpm_blower_2_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_2_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_2_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_2_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_2_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_2_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_2_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_2_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_2_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_2_propulsion_modul_Y_meas_motor_speed) -{ - RT_MODEL_bpm_blower_2_propuls_T *bpm_blower_2_propulsion_modu_M; - bpm_blower_2_propulsion_modu_M = (RT_MODEL_bpm_blower_2_propuls_T *) malloc - (sizeof(RT_MODEL_bpm_blower_2_propuls_T)); - if (bpm_blower_2_propulsion_modu_M == NULL) { - return NULL; - } - - (void) memset((char *)bpm_blower_2_propulsion_modu_M, 0, - sizeof(RT_MODEL_bpm_blower_2_propuls_T)); - - // block I/O - { - B_bpm_blower_2_propulsion_mod_T *b = (B_bpm_blower_2_propulsion_mod_T *) - malloc(sizeof(B_bpm_blower_2_propulsion_mod_T)); - rt_VALIDATE_MEMORY(bpm_blower_2_propulsion_modu_M,b); - bpm_blower_2_propulsion_modu_M->blockIO = (b); - } - - // parameters - { - P_bpm_blower_2_propulsion_mod_T *p; - static int_T pSeen = 0; - - // only malloc on multiple model instantiation - if (pSeen == 1 ) { - p = (P_bpm_blower_2_propulsion_mod_T *) malloc(sizeof - (P_bpm_blower_2_propulsion_mod_T)); - rt_VALIDATE_MEMORY(bpm_blower_2_propulsion_modu_M,p); - (void) memcpy(p, &bpm_blower_2_propulsion_modul_P, - sizeof(P_bpm_blower_2_propulsion_mod_T)); - bpm_blower_2_propulsion_modu_M->paramIsMalloced = (true); - } else { - p = &bpm_blower_2_propulsion_modul_P; - bpm_blower_2_propulsion_modu_M->paramIsMalloced = (false); - pSeen = 1; - } - - bpm_blower_2_propulsion_modu_M->defaultParam = (p); - } - - // states (dwork) - { - DW_bpm_blower_2_propulsion_mo_T *dwork = (DW_bpm_blower_2_propulsion_mo_T *) - malloc(sizeof(DW_bpm_blower_2_propulsion_mo_T)); - rt_VALIDATE_MEMORY(bpm_blower_2_propulsion_modu_M,dwork); - bpm_blower_2_propulsion_modu_M->dwork = (dwork); - } - - { - P_bpm_blower_2_propulsion_mod_T *bpm_blower_2_propulsion_modul_P = - ((P_bpm_blower_2_propulsion_mod_T *) - bpm_blower_2_propulsion_modu_M->defaultParam); - B_bpm_blower_2_propulsion_mod_T *bpm_blower_2_propulsion_modul_B = - ((B_bpm_blower_2_propulsion_mod_T *) - bpm_blower_2_propulsion_modu_M->blockIO); - DW_bpm_blower_2_propulsion_mo_T *bpm_blower_2_propulsion_modu_DW = - ((DW_bpm_blower_2_propulsion_mo_T *) bpm_blower_2_propulsion_modu_M->dwork); - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // block I/O - (void) memset(((void *) bpm_blower_2_propulsion_modul_B), 0, - sizeof(B_bpm_blower_2_propulsion_mod_T)); - - // states (dwork) - (void) memset((void *)bpm_blower_2_propulsion_modu_DW, 0, - sizeof(DW_bpm_blower_2_propulsion_mo_T)); - - // external inputs - (void)memset(&bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[0], 0, 3U * - sizeof(real32_T)); - (void)memset(&bpm_blower_2_propulsion_modul_U_servo_cmd[0], 0, 6U * sizeof - (real32_T)); - (void)memset(&bpm_blower_2_propulsion_modul_U_veh_cm[0], 0, 3U * sizeof - (real32_T)); - *bpm_blower_2_propulsion_modul_U_battery_voltage = 0.0F; - *bpm_blower_2_propulsion_modul_U_impeller_cmd = 0U; - - // external outputs - (*bpm_blower_2_propulsion_modul_Y_impeller_current) = 0.0F; - (void) memset(&bpm_blower_2_propulsion_modul_Y_servo_current[0], 0, - 6U*sizeof(real32_T)); - (void) memset(&bpm_blower_2_propulsion_modul_Y_torque_B[0], 0, - 3U*sizeof(real32_T)); - (void) memset(&bpm_blower_2_propulsion_modul_Y_force_B[0], 0, - 3U*sizeof(real32_T)); - (*bpm_blower_2_propulsion_modul_Y_motor_speed) = 0.0F; - (void) memset(&bpm_blower_2_propulsion_modul_Y_nozzle_theta[0], 0, - 6U*sizeof(real32_T)); - (*bpm_blower_2_propulsion_modul_Y_meas_motor_speed) = 0.0F; - } - - return bpm_blower_2_propulsion_modu_M; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module.h b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module.h deleted file mode 100644 index 33f3925c85..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module.h +++ /dev/null @@ -1,604 +0,0 @@ -// -// File: bpm_blower_2_propulsion_module.h -// -// Code generated for Simulink model 'bpm_blower_2_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:47:03 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_bpm_blower_2_propulsion_module_h_ -#define RTW_HEADER_bpm_blower_2_propulsion_module_h_ -#include -#include -#include -#ifndef bpm_blower_2_propulsion_module_COMMON_INCLUDES_ -# define bpm_blower_2_propulsion_module_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#endif // bpm_blower_2_propulsion_module_COMMON_INCLUDES_ - -#include "bpm_blower_2_propulsion_module_types.h" - -// Child system includes -#include "blower_aerodynamics.h" -#include "rt_nonfinite.h" -#include "rt_nrand_Upu32_Yd_f_pw_snf.h" -#include "rt_roundf_snf.h" -#include "rtGetInf.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -# define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -# define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Block signals (auto storage) -typedef struct { - real32_T OutportBufferForthrust2force_B[18];// '/MATLAB Function' - real32_T OutportBufferForthrust2torque_B[18];// '/MATLAB Function' - B_blower_aerodynamics_bpm_blo_T blower_aerodynamics;// '/blower_aerodynamics' -} B_bpm_blower_2_propulsion_mod_T; - -// Block states (auto storage) for system '' -typedef struct { - real_T NextOutput; // '/random_noise1' - real_T NextOutput_p; // '/random_noise' - real32_T DiscreteTimeIntegrator_DSTATE;// '/Discrete-Time Integrator' - real32_T Delay2_DSTATE; // '/Delay2' - real32_T DiscreteTimeIntegrator1_DSTATE;// '/Discrete-Time Integrator1' - real32_T DelayInput1_DSTATE[3]; // '/Delay Input1' - real32_T DiscreteTimeIntegrator4_DSTATE[6];// '/Discrete-Time Integrator4' - real32_T Integrator_DSTATE[6]; // '/Integrator' - real32_T Filter_DSTATE[6]; // '/Filter' - real32_T DiscreteTimeIntegrator3_DSTATE[6];// '/Discrete-Time Integrator3' - real32_T Integrator_DSTATE_c; // '/Integrator' - real32_T Filter_DSTATE_m; // '/Filter' - real32_T PrevY[6]; // '/Backlash1' - real32_T PrevY_c; // '/Rate Limiter' - uint32_T RandSeed; // '/random_noise1' - uint32_T RandSeed_o; // '/random_noise' - boolean_T UnitDelay_DSTATE; // '/Unit Delay' - DW_blower_aerodynamics_bpm_bl_T blower_aerodynamics;// '/blower_aerodynamics' -} DW_bpm_blower_2_propulsion_mo_T; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_bpm_blower_2_pro_T_ { - real_T Constant2_Value[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value; // Expression: 0 - // Referenced by: '/Constant3' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T Gain1_Gain; // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - real32_T Gain_Gain_b; // Computed Parameter: Gain_Gain_b - // Referenced by: '/Gain' - - real32_T Gain1_Gain_a; // Computed Parameter: Gain1_Gain_a - // Referenced by: '/Gain1' - - real32_T Gain2_Gain; // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_b; // Computed Parameter: Gain2_Gain_b - // Referenced by: '/Gain2' - -}; - -// Parameters (auto storage) -struct P_bpm_blower_2_propulsion_mod_T_ { - real_T abp_P_CG_B_B_error[3]; // Variable: abp_P_CG_B_B_error - // Referenced by: '/Constant7' - - real_T astrobee_time_step_size; // Variable: astrobee_time_step_size - // Referenced by: - // '/Gain1' - // '/Gain4' - - real_T bpm_PM2_nozzle_noise_feedback_gain[6];// Variable: bpm_PM2_nozzle_noise_feedback_gain - // Referenced by: '/blower_aerodynamics' - - real_T bpm_PM2_randn_noise_seed; // Variable: bpm_PM2_randn_noise_seed - // Referenced by: '/blower_aerodynamics' - - real_T bpm_sensor_rand_seed; // Variable: bpm_sensor_rand_seed - // Referenced by: - // '/random_noise' - // '/random_noise1' - - real_T bpm_servo_min_theta; // Variable: bpm_servo_min_theta - // Referenced by: '/Discrete-Time Integrator4' - - real32_T abp_PM2_P_nozzle_B_B[18]; // Variable: abp_PM2_P_nozzle_B_B - // Referenced by: '/Constant1' - - real32_T abp_PM2_discharge_coeff[6]; // Variable: abp_PM2_discharge_coeff - // Referenced by: '/blower_aerodynamics' - - real32_T abp_PM2_nozzle_orientations[18];// Variable: abp_PM2_nozzle_orientations - // Referenced by: '/Constant3' - - real32_T abp_PM2_nozzle_widths[6]; // Variable: abp_PM2_nozzle_widths - // Referenced by: '/Constant6' - - real32_T abp_impeller_diameter; // Variable: abp_impeller_diameter - // Referenced by: '/blower_aerodynamics' - - real32_T abp_nozzle_flap_count; // Variable: abp_nozzle_flap_count - // Referenced by: '/Constant1' - - real32_T abp_nozzle_flap_length; // Variable: abp_nozzle_flap_length - // Referenced by: '/Constant4' - - real32_T abp_nozzle_gear_ratio; // Variable: abp_nozzle_gear_ratio - // Referenced by: '/Gain12' - - real32_T abp_nozzle_intake_height; // Variable: abp_nozzle_intake_height - // Referenced by: '/Constant5' - - real32_T abp_nozzle_min_open_angle; // Variable: abp_nozzle_min_open_angle - // Referenced by: '/Constant2' - - real32_T abp_pm2_impeller_orientation[3];// Variable: abp_pm2_impeller_orientation - // Referenced by: '/Constant1' - - real32_T abp_pm2_zero_thrust_area; // Variable: abp_pm2_zero_thrust_area - // Referenced by: '/blower_aerodynamics' - - real32_T bmp_PM2_impeller_orientation_error[3];// Variable: bmp_PM2_impeller_orientation_error - // Referenced by: '/Constant3' - - real32_T bpm_PM2_P_nozzle_B_B_error[18];// Variable: bpm_PM2_P_nozzle_B_B_error - // Referenced by: '/Constant4' - - real32_T bpm_PM2_Q_nozzle2misaligned[24];// Variable: bpm_PM2_Q_nozzle2misaligned - // Referenced by: '/Constant5' - - real32_T bpm_PM2_nozzle_discharge_coeff_error[6];// Variable: bpm_PM2_nozzle_discharge_coeff_error - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_PM2_zero_thrust_area_error;// Variable: bpm_PM2_zero_thrust_area_error - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_imp_ctl_filt_n; // Variable: bpm_imp_ctl_filt_n - // Referenced by: '/Filter Coefficient' - - real32_T bpm_imp_ctl_kd; // Variable: bpm_imp_ctl_kd - // Referenced by: '/Derivative Gain' - - real32_T bpm_imp_ctl_ki; // Variable: bpm_imp_ctl_ki - // Referenced by: '/Integral Gain' - - real32_T bpm_imp_ctl_kp; // Variable: bpm_imp_ctl_kp - // Referenced by: '/Proportional Gain' - - real32_T bpm_imp_max_voltage; // Variable: bpm_imp_max_voltage - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T bpm_imp_motor_friction_coeff;// Variable: bpm_imp_motor_friction_coeff - // Referenced by: '/Gain1' - - real32_T bpm_imp_motor_r; // Variable: bpm_imp_motor_r - // Referenced by: '/Gain5' - - real32_T bpm_imp_motor_speed_k; // Variable: bpm_imp_motor_speed_k - // Referenced by: '/Gain4' - - real32_T bpm_imp_motor_torque_k; // Variable: bpm_imp_motor_torque_k - // Referenced by: '/Gain6' - - real32_T bpm_imp_speed_filt_den; // Variable: bpm_imp_speed_filt_den - // Referenced by: '/Discrete Transfer Fcn1' - - real32_T bpm_imp_speed_filt_num; // Variable: bpm_imp_speed_filt_num - // Referenced by: '/Discrete Transfer Fcn1' - - real32_T bpm_impeller_inertia; // Variable: bpm_impeller_inertia - // Referenced by: - // '/Constant2' - // '/Gain' - // '/Gain2' - - real32_T bpm_impeller_inertia_error; // Variable: bpm_impeller_inertia_error - // Referenced by: - // '/Constant4' - // '/Gain2' - - real32_T bpm_impeller_init_speed; // Variable: bpm_impeller_init_speed - // Referenced by: '/Discrete-Time Integrator' - - real32_T bpm_lookup_Cdp_data[334]; // Variable: bpm_lookup_Cdp_data - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_lookup_totalarea_breakpoints[334];// Variable: bpm_lookup_totalarea_breakpoints - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_sensor_max; // Variable: bpm_sensor_max - // Referenced by: '/Saturation' - - real32_T bpm_sensor_min; // Variable: bpm_sensor_min - // Referenced by: '/Saturation' - - real32_T bpm_sensor_resolution; // Variable: bpm_sensor_resolution - // Referenced by: '/Quantizer' - - real32_T bpm_sensor_sf; // Variable: bpm_sensor_sf - // Referenced by: '/Gain3' - - real32_T bpm_servo_ctl_filt_n; // Variable: bpm_servo_ctl_filt_n - // Referenced by: '/Filter Coefficient' - - real32_T bpm_servo_ctl_kd; // Variable: bpm_servo_ctl_kd - // Referenced by: '/Derivative Gain' - - real32_T bpm_servo_ctl_ki; // Variable: bpm_servo_ctl_ki - // Referenced by: '/Integral Gain' - - real32_T bpm_servo_ctl_kp; // Variable: bpm_servo_ctl_kp - // Referenced by: '/Proportional Gain' - - real32_T bpm_servo_max_theta; // Variable: bpm_servo_max_theta - // Referenced by: '/Discrete-Time Integrator4' - - real32_T bpm_servo_max_voltage; // Variable: bpm_servo_max_voltage - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T bpm_servo_motor_backlash_deadband;// Variable: bpm_servo_motor_backlash_deadband - // Referenced by: '/Backlash1' - - real32_T bpm_servo_motor_friction_coeff;// Variable: bpm_servo_motor_friction_coeff - // Referenced by: '/Gain9' - - real32_T bpm_servo_motor_gear_box_inertia;// Variable: bpm_servo_motor_gear_box_inertia - // Referenced by: '/Gain6' - - real32_T bpm_servo_motor_gear_ratio; // Variable: bpm_servo_motor_gear_ratio - // Referenced by: - // '/Discrete-Time Integrator4' - // '/Gain1' - - real32_T bpm_servo_motor_k; // Variable: bpm_servo_motor_k - // Referenced by: - // '/Gain11' - // '/Gain7' - - real32_T bpm_servo_motor_r; // Variable: bpm_servo_motor_r - // Referenced by: '/Gain5' - - real32_T bpm_servo_pwm2angle; // Variable: bpm_servo_pwm2angle - // Referenced by: '/Gain' - - real32_T bpm_servo_pwm2angle_bias; // Variable: bpm_servo_pwm2angle_bias - // Referenced by: '/Constant' - - real32_T const_air_density; // Variable: const_air_density - // Referenced by: '/blower_aerodynamics' - - real32_T tun_bpm_PM2_thrust_error_sf;// Variable: tun_bpm_PM2_thrust_error_sf - // Referenced by: '/blower_aerodynamics' - - real32_T tun_bpm_noise_on_flag; // Variable: tun_bpm_noise_on_flag - // Referenced by: - // '/blower_aerodynamics' - // '/Gain' - // '/Gain1' - // '/Constant' - // '/Constant8' - // '/Gain' - // '/Gain2' - - real32_T DiscretePIDController_LowerSatu;// Mask Parameter: DiscretePIDController_LowerSatu - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T DiscretePIDController_LowerSa_d;// Mask Parameter: DiscretePIDController_LowerSa_d - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T bpm_blower_2_propulsion_module_;// Mask Parameter: bpm_blower_2_propulsion_module_ - // Referenced by: '/Constant' - - real32_T DetectChange_vinit; // Mask Parameter: DetectChange_vinit - // Referenced by: '/Delay Input1' - - real_T Constant3_Value; // Expression: 0 - // Referenced by: '/Constant3' - - real_T random_noise1_Mean; // Expression: 0 - // Referenced by: '/random_noise1' - - real_T random_noise1_StdDev; // Computed Parameter: random_noise1_StdDev - // Referenced by: '/random_noise1' - - real_T random_noise_Mean; // Expression: 0 - // Referenced by: '/random_noise' - - real_T random_noise_StdDev; // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - real32_T Constant2_Value[24]; // Expression: [zeros(6,3,'single'), ones(6,1,'single')] - // Referenced by: '/Constant2' - - real32_T thrust2torque_B_Y0; // Computed Parameter: thrust2torque_B_Y0 - // Referenced by: '/thrust2torque_B' - - real32_T thrust2force_B_Y0; // Computed Parameter: thrust2force_B_Y0 - // Referenced by: '/thrust2force_B' - - real32_T Switch_Threshold; // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T DiscreteTransferFcn1_InitialSta;// Computed Parameter: DiscreteTransferFcn1_InitialSta - // Referenced by: '/Discrete Transfer Fcn1' - - real32_T RateLimiter_RisingLim; // Computed Parameter: RateLimiter_RisingLim - // Referenced by: '/Rate Limiter' - - real32_T RateLimiter_FallingLim; // Computed Parameter: RateLimiter_FallingLim - // Referenced by: '/Rate Limiter' - - real32_T RateLimiter_IC; // Computed Parameter: RateLimiter_IC - // Referenced by: '/Rate Limiter' - - real32_T Integrator_gainval; // Computed Parameter: Integrator_gainval - // Referenced by: '/Integrator' - - real32_T Integrator_IC; // Computed Parameter: Integrator_IC - // Referenced by: '/Integrator' - - real32_T Filter_gainval; // Computed Parameter: Filter_gainval - // Referenced by: '/Filter' - - real32_T Filter_IC; // Computed Parameter: Filter_IC - // Referenced by: '/Filter' - - real32_T ZeroGain_Gain; // Computed Parameter: ZeroGain_Gain - // Referenced by: '/ZeroGain' - - real32_T Constant_Value; // Computed Parameter: Constant_Value - // Referenced by: '/Constant' - - real32_T DiscreteTimeIntegrator4_gainval;// Computed Parameter: DiscreteTimeIntegrator4_gainval - // Referenced by: '/Discrete-Time Integrator4' - - real32_T Backlash1_InitialOutput; // Computed Parameter: Backlash1_InitialOutput - // Referenced by: '/Backlash1' - - real32_T Integrator_gainval_i; // Computed Parameter: Integrator_gainval_i - // Referenced by: '/Integrator' - - real32_T Integrator_IC_i; // Computed Parameter: Integrator_IC_i - // Referenced by: '/Integrator' - - real32_T Filter_gainval_d; // Computed Parameter: Filter_gainval_d - // Referenced by: '/Filter' - - real32_T Filter_IC_d; // Computed Parameter: Filter_IC_d - // Referenced by: '/Filter' - - real32_T DiscreteTimeIntegrator3_gainval;// Computed Parameter: DiscreteTimeIntegrator3_gainval - // Referenced by: '/Discrete-Time Integrator3' - - real32_T DiscreteTimeIntegrator3_IC; // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator3' - - real32_T ZeroGain_Gain_b; // Computed Parameter: ZeroGain_Gain_b - // Referenced by: '/ZeroGain' - - real32_T Constant_Value_e; // Computed Parameter: Constant_Value_e - // Referenced by: '/Constant' - - real32_T DiscreteTimeIntegrator_gainval;// Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - real32_T Gain1_Gain; // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - real32_T Gain_Gain_h; // Computed Parameter: Gain_Gain_h - // Referenced by: '/Gain' - - real32_T Gain1_Gain_e; // Computed Parameter: Gain1_Gain_e - // Referenced by: '/Gain1' - - real32_T Gain2_Gain; // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - real32_T Delay2_InitialCondition; // Computed Parameter: Delay2_InitialCondition - // Referenced by: '/Delay2' - - real32_T DiscreteTimeIntegrator1_gainval;// Computed Parameter: DiscreteTimeIntegrator1_gainval - // Referenced by: '/Discrete-Time Integrator1' - - real32_T DiscreteTimeIntegrator1_IC; // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator1' - - real32_T Switch_Threshold_f; // Computed Parameter: Switch_Threshold_f - // Referenced by: '/Switch' - - uint32_T Delay2_DelayLength; // Computed Parameter: Delay2_DelayLength - // Referenced by: '/Delay2' - - boolean_T UnitDelay_InitialCondition;// Computed Parameter: UnitDelay_InitialCondition - // Referenced by: '/Unit Delay' - - P_CoreSubsys_bpm_blower_2_pro_T CoreSubsys;// '/CoreSubsys' - P_blower_aerodynamics_bpm_blo_T blower_aerodynamics;// '/blower_aerodynamics' -}; - -// Real-time Model Data Structure -struct tag_RTM_bpm_blower_2_propulsi_T { - const char_T * volatile errorStatus; - B_bpm_blower_2_propulsion_mod_T *blockIO; - P_bpm_blower_2_propulsion_mod_T *defaultParam; - boolean_T paramIsMalloced; - DW_bpm_blower_2_propulsion_mo_T *dwork; -}; - -#ifdef __cplusplus - -extern "C" { - -#endif - -#ifdef __cplusplus - -} -#endif - -// External data declarations for dependent source files -#ifdef __cplusplus - -extern "C" { - -#endif - - extern const char *RT_MEMORY_ALLOCATION_ERROR; - -#ifdef __cplusplus - -} -#endif - -extern P_bpm_blower_2_propulsion_mod_T bpm_blower_2_propulsion_modul_P;// parameters - -#ifdef __cplusplus - -extern "C" { - -#endif - - // Model entry point functions - extern RT_MODEL_bpm_blower_2_propuls_T *bpm_blower_2_propulsion_module - (real32_T *bpm_blower_2_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - *bpm_blower_2_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_2_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_2_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_2_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_2_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_2_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_2_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_2_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_2_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_2_propulsion_modul_Y_meas_motor_speed); - extern void bpm_blower_2_propulsion_module_initialize - (RT_MODEL_bpm_blower_2_propuls_T *const bpm_blower_2_propulsion_modu_M, - real32_T *bpm_blower_2_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - *bpm_blower_2_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_2_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_2_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_2_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_2_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_2_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_2_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_2_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_2_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_2_propulsion_modul_Y_meas_motor_speed); - extern void bpm_blower_2_propulsion_module_step - (RT_MODEL_bpm_blower_2_propuls_T *const bpm_blower_2_propulsion_modu_M, - real32_T bpm_blower_2_propulsion_modul_U_battery_voltage, real32_T - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[3], uint8_T - bpm_blower_2_propulsion_modul_U_impeller_cmd, real32_T - bpm_blower_2_propulsion_modul_U_servo_cmd[6], real32_T - bpm_blower_2_propulsion_modul_U_veh_cm[3], real32_T - *bpm_blower_2_propulsion_modul_Y_impeller_current, real32_T - bpm_blower_2_propulsion_modul_Y_servo_current[6], real32_T - bpm_blower_2_propulsion_modul_Y_torque_B[3], real32_T - bpm_blower_2_propulsion_modul_Y_force_B[3], real32_T - *bpm_blower_2_propulsion_modul_Y_motor_speed, real32_T - bpm_blower_2_propulsion_modul_Y_nozzle_theta[6], real32_T - *bpm_blower_2_propulsion_modul_Y_meas_motor_speed); - extern void bpm_blower_2_propulsion_module_terminate - (RT_MODEL_bpm_blower_2_propuls_T * bpm_blower_2_propulsion_modu_M); - -#ifdef __cplusplus - -} -#endif - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Note that this particular code originates from a subsystem build, -// and has its own system numbers different from the parent model. -// Refer to the system hierarchy for this subsystem below, and use the -// MATLAB hilite_system command to trace the generated code back -// to the parent model. For example, -// -// hilite_system('astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module') - opens subsystem astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module -// hilite_system('astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/Kp') - opens and selects block Kp -// -// Here is the system hierarchy for this model -// -// '' : 'astrobee/sim_model_lib/veh_vehicle_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_aerodynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/calc_nozzle_area' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/motor_rotational_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/speed_sensor' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/Detect Change' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/cross_product' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/vector_normalize' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/cross_product/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/cross_product/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/MATLAB Function' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/vector_normalize/No-op' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/vector_normalize/Normalize' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/vector_normalize/vector_magnitude' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/dc_motor_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/speed_controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/speed_controller/Discrete PID Controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/speed_controller/Saturation Dynamic' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/speed_controller/Discrete PID Controller/Clamping circuit' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model/dc_motor_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model/servo_controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model/servo_controller/Discrete PID Controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model/servo_controller/Discrete PID Controller/Clamping circuit' - -#endif // RTW_HEADER_bpm_blower_2_propulsion_module_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_data.cpp b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_data.cpp deleted file mode 100644 index 787c2bf7d5..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_data.cpp +++ /dev/null @@ -1,608 +0,0 @@ -// -// File: bpm_blower_2_propulsion_module_data.cpp -// -// Code generated for Simulink model 'bpm_blower_2_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:47:03 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "bpm_blower_2_propulsion_module.h" -#include "bpm_blower_2_propulsion_module_private.h" - -// Block parameters (auto storage) -P_bpm_blower_2_propulsion_mod_T bpm_blower_2_propulsion_modul_P = { - // Variable: abp_P_CG_B_B_error - // Referenced by: '/Constant7' - - { 0.0, 0.0, 0.0 }, - 0.016, // Variable: astrobee_time_step_size - // Referenced by: - // '/Gain1' - // '/Gain4' - - - // Variable: bpm_PM2_nozzle_noise_feedback_gain - // Referenced by: '/blower_aerodynamics' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 255.0, // Variable: bpm_PM2_randn_noise_seed - // Referenced by: '/blower_aerodynamics' - - 1337.0, // Variable: bpm_sensor_rand_seed - // Referenced by: - // '/random_noise' - // '/random_noise1' - - 0.0, // Variable: bpm_servo_min_theta - // Referenced by: '/Discrete-Time Integrator4' - - - // Variable: abp_PM2_P_nozzle_B_B - // Referenced by: '/Constant1' - - { -0.1524F, 0.1524F, -0.0718819946F, 0.0718819946F, 0.067564F, -0.067564F, - -0.101854004F, -0.101854004F, -0.1524F, -0.1524F, -0.101854004F, - -0.101854004F, -0.0396239981F, 0.0396239981F, 0.0718819946F, -0.0718819946F, - 0.1524F, -0.1524F }, - - // Variable: abp_PM2_discharge_coeff - // Referenced by: '/blower_aerodynamics' - - { 0.947114F, 0.764468908F, 1.0F, 0.904809415F, 0.936555624F, 0.893794775F }, - - // Variable: abp_PM2_nozzle_orientations - // Referenced by: '/Constant3' - - { -1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, -1.0F, -1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, -1.0F }, - - // Variable: abp_PM2_nozzle_widths - // Referenced by: '/Constant6' - - { 0.127F, 0.127F, 0.07112F, 0.07112F, 0.07112F, 0.07112F }, - 0.1397F, // Variable: abp_impeller_diameter - // Referenced by: '/blower_aerodynamics' - - 2.0F, // Variable: abp_nozzle_flap_count - // Referenced by: '/Constant1' - - 0.0135966204F, // Variable: abp_nozzle_flap_length - // Referenced by: '/Constant4' - - 0.5F, // Variable: abp_nozzle_gear_ratio - // Referenced by: '/Gain12' - - 0.01309116F, // Variable: abp_nozzle_intake_height - // Referenced by: '/Constant5' - - 0.273667634F, // Variable: abp_nozzle_min_open_angle - // Referenced by: '/Constant2' - - - // Variable: abp_pm2_impeller_orientation - // Referenced by: '/Constant1' - - { 0.0F, -1.0F, 0.0F }, - 0.0042273F, // Variable: abp_pm2_zero_thrust_area - // Referenced by: '/blower_aerodynamics' - - - // Variable: bmp_PM2_impeller_orientation_error - // Referenced by: '/Constant3' - - { 0.0F, 0.0F, 0.0F }, - - // Variable: bpm_PM2_P_nozzle_B_B_error - // Referenced by: '/Constant4' - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - - // Variable: bpm_PM2_Q_nozzle2misaligned - // Referenced by: '/Constant5' - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, -0.0F, - -0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - - // Variable: bpm_PM2_nozzle_discharge_coeff_error - // Referenced by: '/blower_aerodynamics' - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - 0.0F, // Variable: bpm_PM2_zero_thrust_area_error - // Referenced by: '/blower_aerodynamics' - - 1.0F, // Variable: bpm_imp_ctl_filt_n - // Referenced by: '/Filter Coefficient' - - 0.05F, // Variable: bpm_imp_ctl_kd - // Referenced by: '/Derivative Gain' - - 0.1F, // Variable: bpm_imp_ctl_ki - // Referenced by: '/Integral Gain' - - 0.2F, // Variable: bpm_imp_ctl_kp - // Referenced by: '/Proportional Gain' - - 16.6F, // Variable: bpm_imp_max_voltage - // Referenced by: - // '/Saturate' - // '/DeadZone' - - 8.00571161E-6F, // Variable: bpm_imp_motor_friction_coeff - // Referenced by: '/Gain1' - - 1.2F, // Variable: bpm_imp_motor_r - // Referenced by: '/Gain5' - - 39.1651878F, // Variable: bpm_imp_motor_speed_k - // Referenced by: '/Gain4' - - 0.0255F, // Variable: bpm_imp_motor_torque_k - // Referenced by: '/Gain6' - - 1.0F, // Variable: bpm_imp_speed_filt_den - // Referenced by: '/Discrete Transfer Fcn1' - - 1.0F, // Variable: bpm_imp_speed_filt_num - // Referenced by: '/Discrete Transfer Fcn1' - - 0.001F, // Variable: bpm_impeller_inertia - // Referenced by: - // '/Constant2' - // '/Gain' - // '/Gain2' - - 0.0F, // Variable: bpm_impeller_inertia_error - // Referenced by: - // '/Constant4' - // '/Gain2' - - 0.0F, // Variable: bpm_impeller_init_speed - // Referenced by: '/Discrete-Time Integrator' - - - // Variable: bpm_lookup_Cdp_data - // Referenced by: '/blower_aerodynamics' - - { 0.0825F, 0.082625635F, 0.08274699F, 0.0828641355F, 0.082977131F, - 0.083086051F, 0.0831909552F, 0.083291918F, 0.083389F, 0.083482258F, - 0.0835717544F, 0.0836575627F, 0.0837397277F, 0.083818309F, 0.0838933736F, - 0.0839649737F, 0.0840331689F, 0.0840980038F, 0.0841595381F, 0.0842178315F, - 0.0842729211F, 0.084324874F, 0.0843737274F, 0.0844195336F, 0.0844623446F, - 0.0845022127F, 0.0845391676F, 0.0845732689F, 0.0846045539F, 0.0846330673F, - 0.0846588612F, 0.0846819654F, 0.0847024247F, 0.0847202763F, 0.0847355723F, - 0.0847483352F, 0.0847586095F, 0.0847664326F, 0.0847718418F, 0.0847748667F, - 0.0847755447F, 0.084773913F, 0.08477F, 0.0847638398F, 0.0847554579F, - 0.084744893F, 0.0847321674F, 0.0847173184F, 0.0847003683F, 0.0846813396F, - 0.0846602693F, 0.0846371725F, 0.0846120864F, 0.0845850259F, 0.0845560208F, - 0.0845250934F, 0.0844922587F, 0.0844575465F, 0.0844209716F, 0.0843825564F, - 0.0843423232F, 0.0843002871F, 0.0842564702F, 0.084210895F, 0.0841635615F, - 0.0841145F, 0.0840637162F, 0.0840112418F, 0.0839570761F, 0.083901234F, - 0.0838437378F, 0.0837845877F, 0.0837238058F, 0.0836614F, 0.0835973844F, - 0.0835317597F, 0.083464548F, 0.0833957493F, 0.0833253786F, 0.0832534432F, - 0.0831799433F, 0.0831048936F, 0.0830283F, 0.0829501674F, 0.0828705F, - 0.0827893F, 0.0827065781F, 0.0826223418F, 0.0825365856F, 0.0824493095F, - 0.0823605284F, 0.0822702423F, 0.0821784437F, 0.08208514F, 0.0819903314F, - 0.0818940252F, 0.0817962065F, 0.0816968903F, 0.081596069F, 0.0814937353F, - 0.081389904F, 0.0812845528F, 0.0811777F, 0.0810693204F, 0.0809594318F, - 0.0808480158F, 0.0807350799F, 0.0806206167F, 0.0805046186F, 0.0803870782F, - 0.080268F, 0.0801473707F, 0.0800251886F, 0.0799014494F, 0.0797761381F, - 0.0796492621F, 0.0795208F, 0.0793907568F, 0.0792591125F, 0.0791258737F, - 0.0789910182F, 0.0788545534F, 0.0787164569F, 0.0785767287F, 0.0784353614F, - 0.0782923326F, 0.0781476498F, 0.0780012906F, 0.077853255F, 0.0777035281F, - 0.0775521F, 0.0773989633F, 0.0772441104F, 0.0770875216F, 0.0769291893F, - 0.0767691135F, 0.0766072646F, 0.0764436498F, 0.0762782469F, 0.076111041F, - 0.0759420395F, 0.0757712126F, 0.0755985528F, 0.0754240528F, 0.0752477F, - 0.0750694722F, 0.0748893768F, 0.0747073889F, 0.0745235F, 0.0743377F, - 0.0741499662F, 0.0739603F, 0.0737686828F, 0.0735751F, 0.0733795539F, - 0.0731820166F, 0.0729824826F, 0.072780937F, 0.0725773647F, 0.072371766F, - 0.0721641108F, 0.0719544F, 0.071742624F, 0.0715287626F, 0.0713128075F, - 0.0710947439F, 0.0708745569F, 0.0706522539F, 0.0704278F, 0.0702012F, - 0.0699724406F, 0.0697415F, 0.0695083737F, 0.0692730546F, 0.0690355226F, - 0.0687957704F, 0.0685537905F, 0.0683095828F, 0.0680631176F, 0.0678143948F, - 0.0675634F, 0.0673101321F, 0.0670545697F, 0.0667967126F, 0.0665365383F, - 0.0662740618F, 0.0660092533F, 0.0657421201F, 0.0654726326F, 0.0652008057F, - 0.0649266243F, 0.0646500662F, 0.0643711537F, 0.0640898496F, 0.0638061538F, - 0.0635200813F, 0.0632316F, 0.0629407242F, 0.0626474321F, 0.0623517223F, - 0.0620536F, 0.0617530458F, 0.0614500828F, 0.0611446686F, 0.0608368181F, - 0.0605265312F, 0.0602138F, 0.0598986298F, 0.0595810153F, 0.0592609458F, - 0.0589384325F, 0.0586134642F, 0.0582860447F, 0.0579561703F, 0.0576238409F, - 0.0572890751F, 0.0569518507F, 0.0566121712F, 0.0562700555F, 0.0559254885F, - 0.0555784889F, 0.0552290566F, 0.054877162F, 0.0545228682F, 0.0541661382F, - 0.0538069829F, 0.0534454137F, 0.0530814491F, 0.0527150668F, 0.0523463078F, - 0.0519751534F, 0.0516016223F, 0.0512257218F, 0.0508474559F, 0.0504668579F, - 0.0500839F, 0.0496986173F, 0.0493110269F, 0.0489211343F, 0.0485289469F, - 0.0481344871F, 0.0477377549F, 0.04733878F, 0.0469375588F, 0.0465341397F, - 0.0461285152F, 0.0457207039F, 0.0453107134F, 0.0448985882F, 0.0444843248F, - 0.0440679714F, 0.0436495095F, 0.0432290025F, 0.0428064205F, 0.0423818417F, - 0.0419552475F, 0.0415266976F, 0.0410961919F, 0.0406637266F, 0.0402293913F, - 0.0397931822F, 0.0393551365F, 0.038915243F, 0.0384735875F, 0.0380301885F, - 0.0375850387F, 0.0371381938F, 0.0366896801F, 0.0362395607F, 0.0357878208F, - 0.0353345275F, 0.0348797F, 0.0344234072F, 0.0339656323F, 0.033506453F, - 0.033045914F, 0.032584019F, 0.0321208276F, 0.0316563807F, 0.0311907455F, - 0.0307239257F, 0.0302559771F, 0.0297869369F, 0.0293168686F, 0.0288458094F, - 0.0283738412F, 0.0279009566F, 0.0274272151F, 0.0269527063F, 0.0264774524F, - 0.0260014758F, 0.0255248956F, 0.0250477232F, 0.0245700441F, 0.0240918733F, - 0.0236133039F, 0.0231343843F, 0.0226551369F, 0.0221756734F, 0.0216960311F, - 0.0212163F, 0.0207364894F, 0.0202567242F, 0.0197770186F, 0.0192974396F, - 0.0188181363F, 0.0183390342F, 0.0178603418F, 0.0173820443F, 0.0169042759F, - 0.0164270438F, 0.0159504749F, 0.0154745989F, 0.01499952F, 0.0145253353F, - 0.0140520222F, 0.0135798045F, 0.0131086893F, 0.0126387365F, 0.0121700801F, - 0.0117027499F, 0.0112369023F, 0.0107725374F, 0.0103098117F, 0.0098488F, - 0.00938956812F, 0.00893222168F, 0.00847681239F, 0.00802352652F, - 0.00757240178F, 0.007123549F, 0.00667699846F, 0.00623294385F, 0.0057914448F - }, - - // Variable: bpm_lookup_totalarea_breakpoints - // Referenced by: '/blower_aerodynamics' - - { 0.0F, 1.46138646E-5F, 2.92062887E-5F, 4.37784511E-5F, 5.83315159E-5F, - 7.28665837E-5F, 8.7384753E-5F, 0.000101887068F, 0.000116374569F, - 0.000130848246F, 0.000145309066F, 0.000159758F, 0.000174195899F, - 0.000188623759F, 0.000203042364F, 0.000217452631F, 0.000231855331F, - 0.000246251322F, 0.000260641362F, 0.000275026192F, 0.000289406598F, - 0.000303783338F, 0.00031815705F, 0.000332528463F, 0.000346898247F, - 0.000361267099F, 0.00037563566F, 0.000390004541F, 0.000404374383F, - 0.000418745767F, 0.000433119305F, 0.000447495579F, 0.000461875083F, - 0.000476258458F, 0.000490646285F, 0.000505038945F, 0.00051943725F, - 0.000533841376F, 0.000548252079F, 0.000562669651F, 0.00057709479F, - 0.000591527845F, 0.000605969399F, 0.000620419742F, 0.000634879572F, - 0.000649349065F, 0.000663828861F, 0.000678319368F, 0.000692821F, - 0.000707334315F, 0.000721859455F, 0.000736397109F, 0.000750947569F, - 0.0007655113F, 0.000780088594F, 0.000794680149F, 0.000809286F, - 0.000823906856F, 0.000838542939F, 0.000853194739F, 0.000867862604F, - 0.000882546941F, 0.000897248159F, 0.00091196649F, 0.000926702633F, - 0.000941456819F, 0.0009562294F, 0.00097102084F, 0.000985831604F, - 0.00100066199F, 0.00101551227F, 0.00103038305F, 0.0010452749F, - 0.00106018782F, 0.00107512227F, 0.00109007885F, 0.00110505789F, - 0.00112005987F, 0.00113508501F, 0.00115013402F, 0.00116520724F, - 0.0011803048F, 0.00119542738F, 0.00121057557F, 0.00122574961F, 0.00124095F, - 0.00125617709F, 0.00127143157F, 0.00128671364F, 0.001302024F, 0.00131736277F, - 0.00133273099F, 0.00134812866F, 0.00136355648F, 0.00137901504F, - 0.00139450456F, 0.00141002587F, 0.00142557954F, 0.0014411658F, - 0.00145678537F, 0.00147243857F, 0.00148812647F, 0.00150384917F, - 0.00151960761F, 0.00153540191F, 0.00155123312F, 0.0015671018F, - 0.00158300844F, 0.00159895339F, 0.00161493802F, 0.00163096236F, - 0.00164702744F, 0.0016631335F, 0.00167928194F, 0.00169547275F, - 0.00171170721F, 0.00172798557F, 0.00174430886F, 0.0017606779F, - 0.00177709304F, 0.00179355568F, 0.00181006629F, 0.00182662567F, - 0.00184323464F, 0.00185989426F, 0.00187660486F, 0.0018933682F, - 0.00191018439F, 0.00192705484F, 0.00194398011F, 0.00196096138F, - 0.00197799969F, 0.00199509552F, 0.00201225071F, 0.00202946551F, - 0.00204674155F, 0.00206407951F, 0.00208148058F, 0.00209894567F, - 0.00211647665F, 0.00213407329F, 0.00215173839F, 0.00216947193F, - 0.00218727579F, 0.00220515113F, 0.00222309888F, 0.00224112067F, - 0.0022592172F, 0.0022773908F, 0.00229564216F, 0.00231397292F, 0.00233238423F, - 0.00235087774F, 0.00236945506F, 0.00238811737F, 0.00240686629F, - 0.00242570369F, 0.00244463095F, 0.00246364973F, 0.00248276163F, - 0.00250196829F, 0.00252127182F, 0.00254067336F, 0.00256017526F, - 0.00257977913F, 0.00259948708F, 0.00261930074F, 0.00263922219F, - 0.00265925354F, 0.00267939596F, 0.00269965269F, 0.00272002514F, - 0.00274051586F, 0.00276112719F, 0.002781861F, 0.00280271959F, 0.00282370578F, - 0.00284482073F, 0.00286606839F, 0.00288745062F, 0.00290896976F, - 0.00293062883F, 0.00295243063F, 0.00297437701F, 0.00299647171F, - 0.00301871705F, 0.00304111606F, 0.00306367176F, 0.00308638718F, - 0.00310926535F, 0.0031323093F, 0.00315552298F, 0.00317890849F, - 0.00320247072F, 0.00322621223F, 0.00325013627F, 0.00327424728F, - 0.00329854828F, 0.00332304346F, 0.00334773702F, 0.0033726322F, - 0.00339773367F, 0.00342304516F, 0.00344857131F, 0.00347431679F, - 0.00350028556F, 0.00352648227F, 0.00355291134F, 0.00357957813F, - 0.00360648776F, 0.0036336449F, 0.00366105489F, 0.00368872308F, - 0.00371665484F, 0.00374485622F, 0.00377333187F, 0.00380208902F, - 0.00383113371F, 0.00386047084F, 0.00389010808F, 0.00392005173F, - 0.00395030761F, 0.00398088479F, 0.00401178841F, 0.0040430259F, - 0.00407460658F, 0.00410653651F, 0.00413882406F, 0.0041714781F, - 0.00420450559F, 0.00423791679F, 0.00427172F, 0.00430592475F, 0.00434054F, - 0.00437557558F, 0.00441104267F, 0.00444695121F, 0.00448331F, 0.00452013267F, - 0.00455742935F, 0.00459521171F, 0.00463349326F, 0.00467228517F, - 0.00471160142F, 0.00475145364F, 0.00479185674F, 0.00483282562F, - 0.00487437518F, 0.00491652032F, 0.00495927641F, 0.00500265928F, - 0.00504668895F, 0.00509137847F, 0.00513675157F, 0.0051828213F, - 0.00522961235F, 0.00527714146F, 0.00532543147F, 0.00537450658F, - 0.00542438496F, 0.00547509314F, 0.00552665396F, 0.0055790972F, - 0.00563244335F, 0.00568672409F, 0.00574196922F, 0.00579820713F, - 0.00585547043F, 0.00591378892F, 0.00597320078F, 0.00603374047F, - 0.00609544525F, 0.00615835097F, 0.00622250512F, 0.0062879459F, 0.006354718F, - 0.00642287126F, 0.00649245409F, 0.00656351773F, 0.00663611386F, - 0.00671030255F, 0.00678614574F, 0.00686370581F, 0.00694304705F, - 0.00702424115F, 0.00710735889F, 0.00719248503F, 0.0072797006F, - 0.00736908847F, 0.00746074459F, 0.00755477371F, 0.00765126897F, - 0.00775034819F, 0.00785212312F, 0.00795672741F, 0.00806429F, 0.00817495212F, - 0.00828887522F, 0.00840621162F, 0.00852714293F, 0.00865184888F, - 0.00878054276F, 0.00891342945F, 0.00905075204F, 0.00919276662F, - 0.0093397228F, 0.00949195493F, 0.00964974798F, 0.00981347449F, - 0.00998349674F, 0.0101602506F, 0.0103441793F, 0.0105357962F, 0.010735645F, - 0.0109443273F, 0.0111625558F, 0.0113910241F, 0.0116305985F, 0.0118822251F, - 0.0121469265F, 0.0124259172F, 0.0127205F, 0.0130322482F, 0.0133628659F, - 0.0137143685F, 0.0140890917F, 0.0144897308F, 0.0149194878F, 0.01538203F, - 0.0158818662F, 0.0164243504F, 0.0170160942F, 0.0176650118F, 0.0183811728F }, - 10000.0F, // Variable: bpm_sensor_max - // Referenced by: '/Saturation' - - 0.0F, // Variable: bpm_sensor_min - // Referenced by: '/Saturation' - - 0.1F, // Variable: bpm_sensor_resolution - // Referenced by: '/Quantizer' - - 1.0F, // Variable: bpm_sensor_sf - // Referenced by: '/Gain3' - - 1.0F, // Variable: bpm_servo_ctl_filt_n - // Referenced by: '/Filter Coefficient' - - 0.8F, // Variable: bpm_servo_ctl_kd - // Referenced by: '/Derivative Gain' - - 1.5F, // Variable: bpm_servo_ctl_ki - // Referenced by: '/Integral Gain' - - 5.0F, // Variable: bpm_servo_ctl_kp - // Referenced by: '/Proportional Gain' - - 0.560512543F, // Variable: bpm_servo_max_theta - // Referenced by: '/Discrete-Time Integrator4' - - 6.0F, // Variable: bpm_servo_max_voltage - // Referenced by: - // '/Saturate' - // '/DeadZone' - - 0.0174532924F, // Variable: bpm_servo_motor_backlash_deadband - // Referenced by: '/Backlash1' - - 3.0E-7F, // Variable: bpm_servo_motor_friction_coeff - // Referenced by: '/Gain9' - - 2.5E-8F, // Variable: bpm_servo_motor_gear_box_inertia - // Referenced by: '/Gain6' - - 0.01F, // Variable: bpm_servo_motor_gear_ratio - // Referenced by: - // '/Discrete-Time Integrator4' - // '/Gain1' - - 0.00192866661F, // Variable: bpm_servo_motor_k - // Referenced by: - // '/Gain11' - // '/Gain7' - - 4.0F, // Variable: bpm_servo_motor_r - // Referenced by: '/Gain5' - - 0.00219808845F, // Variable: bpm_servo_pwm2angle - // Referenced by: '/Gain' - - 0.0F, // Variable: bpm_servo_pwm2angle_bias - // Referenced by: '/Constant' - - 1.2F, // Variable: const_air_density - // Referenced by: '/blower_aerodynamics' - - 1.25F, // Variable: tun_bpm_PM2_thrust_error_sf - // Referenced by: '/blower_aerodynamics' - - 0.0F, // Variable: tun_bpm_noise_on_flag - // Referenced by: - // '/blower_aerodynamics' - // '/Gain' - // '/Gain1' - // '/Constant' - // '/Constant8' - // '/Gain' - // '/Gain2' - - -16.6F, // Mask Parameter: DiscretePIDController_LowerSatu - // Referenced by: - // '/Saturate' - // '/DeadZone' - - -6.0F, // Mask Parameter: DiscretePIDController_LowerSa_d - // Referenced by: - // '/Saturate' - // '/DeadZone' - - 1.26247478F, // Mask Parameter: bpm_blower_2_propulsion_module_ - // Referenced by: '/Constant' - - 0.0F, // Mask Parameter: DetectChange_vinit - // Referenced by: '/Delay Input1' - - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - 0.0, // Expression: 0 - // Referenced by: '/random_noise1' - - 1.0E-5, // Computed Parameter: random_noise1_StdDev - // Referenced by: '/random_noise1' - - 0.0, // Expression: 0 - // Referenced by: '/random_noise' - - 1.0E-5, // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - - // Expression: [zeros(6,3,'single'), ones(6,1,'single')] - // Referenced by: '/Constant2' - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - 0.0F, // Computed Parameter: thrust2torque_B_Y0 - // Referenced by: '/thrust2torque_B' - - 0.0F, // Computed Parameter: thrust2force_B_Y0 - // Referenced by: '/thrust2force_B' - - 0.0F, // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - -1.0F, // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - 0.0F, // Computed Parameter: DiscreteTransferFcn1_InitialSta - // Referenced by: '/Discrete Transfer Fcn1' - - 0.335103214F, // Computed Parameter: RateLimiter_RisingLim - // Referenced by: '/Rate Limiter' - - -0.335103214F, // Computed Parameter: RateLimiter_FallingLim - // Referenced by: '/Rate Limiter' - - 0.0F, // Computed Parameter: RateLimiter_IC - // Referenced by: '/Rate Limiter' - - 0.016F, // Computed Parameter: Integrator_gainval - // Referenced by: '/Integrator' - - 0.0F, // Computed Parameter: Integrator_IC - // Referenced by: '/Integrator' - - 0.016F, // Computed Parameter: Filter_gainval - // Referenced by: '/Filter' - - 0.0F, // Computed Parameter: Filter_IC - // Referenced by: '/Filter' - - 0.0F, // Computed Parameter: ZeroGain_Gain - // Referenced by: '/ZeroGain' - - 0.0F, // Computed Parameter: Constant_Value - // Referenced by: '/Constant' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator4_gainval - // Referenced by: '/Discrete-Time Integrator4' - - 0.0F, // Computed Parameter: Backlash1_InitialOutput - // Referenced by: '/Backlash1' - - 0.016F, // Computed Parameter: Integrator_gainval_i - // Referenced by: '/Integrator' - - 0.0F, // Computed Parameter: Integrator_IC_i - // Referenced by: '/Integrator' - - 0.016F, // Computed Parameter: Filter_gainval_d - // Referenced by: '/Filter' - - 0.0F, // Computed Parameter: Filter_IC_d - // Referenced by: '/Filter' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator3_gainval - // Referenced by: '/Discrete-Time Integrator3' - - 0.0F, // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator3' - - 0.0F, // Computed Parameter: ZeroGain_Gain_b - // Referenced by: '/ZeroGain' - - 0.0F, // Computed Parameter: Constant_Value_e - // Referenced by: '/Constant' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - -1.0F, // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain_Gain_h - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_e - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - 0.0F, // Computed Parameter: Delay2_InitialCondition - // Referenced by: '/Delay2' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator1_gainval - // Referenced by: '/Discrete-Time Integrator1' - - 0.0F, // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator1' - - 0.0F, // Computed Parameter: Switch_Threshold_f - // Referenced by: '/Switch' - - 1U, // Computed Parameter: Delay2_DelayLength - // Referenced by: '/Delay2' - - 1, // Computed Parameter: UnitDelay_InitialCondition - // Referenced by: '/Unit Delay' - - - // Start of '/CoreSubsys' - { - // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, // Expression: 1 - // Referenced by: '/Constant1' - - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - 2.0F, // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - 2.0F, // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain_Gain_b - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_a - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - 2.0F // Computed Parameter: Gain2_Gain_b - // Referenced by: '/Gain2' - - } - // End of '/CoreSubsys' - , - - // Start of '/blower_aerodynamics' - { - 0.016, // Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - 0.0, // Expression: 0 - // Referenced by: '/Discrete-Time Integrator' - - 0.0, // Expression: 0 - // Referenced by: '/random_noise' - - - // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 2.0F, // Expression: single(2) - // Referenced by: '/Constant4' - - 0.0F // Expression: single(0) - // Referenced by: '/Constant7' - - } - // End of '/blower_aerodynamics' -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_private.h b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_private.h deleted file mode 100644 index c55617ff71..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_private.h +++ /dev/null @@ -1,51 +0,0 @@ -// -// File: bpm_blower_2_propulsion_module_private.h -// -// Code generated for Simulink model 'bpm_blower_2_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:47:03 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_bpm_blower_2_propulsion_module_private_h_ -#define RTW_HEADER_bpm_blower_2_propulsion_module_private_h_ -#include "rtwtypes.h" -#if !defined(rt_VALIDATE_MEMORY) -#define rt_VALIDATE_MEMORY(S, ptr) if(!(ptr)) {\ - rtmSetErrorStatus(bpm_blower_2_propulsion_modu_M, RT_MEMORY_ALLOCATION_ERROR);\ - } -#endif - -#if !defined(rt_FREE) -#if !defined(_WIN32) -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((ptr));\ - (ptr) = (NULL);\ - } -#else - -// Visual and other windows compilers declare free without const -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((void *)(ptr));\ - (ptr) = (NULL);\ - } -#endif -#endif - -extern const real_T rtCP_pooled_15LYgUQHWYtd[6]; - -#define rtCP_random_noise_rtw_collapsed rtCP_pooled_15LYgUQHWYtd // Expression: noz_randn_seed +[1:6] - // Referenced by: '/random_noise' - -#endif // RTW_HEADER_bpm_blower_2_propulsion_module_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_types.h b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_types.h deleted file mode 100644 index ff53dd1397..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_types.h +++ /dev/null @@ -1,37 +0,0 @@ -// -// File: bpm_blower_2_propulsion_module_types.h -// -// Code generated for Simulink model 'bpm_blower_2_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:47:03 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_bpm_blower_2_propulsion_module_types_h_ -#define RTW_HEADER_bpm_blower_2_propulsion_module_types_h_ -#include "rtwtypes.h" - -// Parameters for system: '/blower_aerodynamics' -typedef struct P_blower_aerodynamics_bpm_blo_T_ P_blower_aerodynamics_bpm_blo_T; - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_bpm_blower_2_pro_T_ P_CoreSubsys_bpm_blower_2_pro_T; - -// Parameters (auto storage) -typedef struct P_bpm_blower_2_propulsion_mod_T_ P_bpm_blower_2_propulsion_mod_T; - -// Forward declaration for rtModel -typedef struct tag_RTM_bpm_blower_2_propulsi_T RT_MODEL_bpm_blower_2_propuls_T; - -#endif // RTW_HEADER_bpm_blower_2_propulsion_module_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/buildInfo.mat b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/buildInfo.mat deleted file mode 100644 index 83961518d2..0000000000 Binary files a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/codeInfo.mat b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/codeInfo.mat deleted file mode 100644 index 1a9368a4c5..0000000000 Binary files a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/codeInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/defines.txt b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/defines.txt deleted file mode 100644 index 7907a81e43..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/defines.txt +++ /dev/null @@ -1,15 +0,0 @@ -RT_MALLOC -MODEL=bpm_blower_2_propulsion_module -NUMST=1 -NCSTATES=0 -HAVESTDIO -UNIX -ONESTEPFCN=1 -TERMFCN=1 -MAT_FILE=0 -MULTI_INSTANCE_CODE=1 -INTEGER_CODE=0 -MT=0 -CLASSIC_INTERFACE=0 -ALLOCATIONFCN=1 -TID01EQ=0 diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/ert_main.cpp b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/ert_main.cpp deleted file mode 100644 index 4b071ea6a2..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/ert_main.cpp +++ /dev/null @@ -1,199 +0,0 @@ -// -// File: ert_main.cpp -// -// Code generated for Simulink model 'bpm_blower_2_propulsion_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:47:03 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include -#include // This ert_main.c example uses printf/fflush -#include "bpm_blower_2_propulsion_module.h" // Model's header file -#include "rtwtypes.h" - -// '/battery_voltage' -static real32_T bpm_blower_2_propulsion_modul_U_battery_voltage; - -// '/omega_B_ECI_B' -static real32_T bpm_blower_2_propulsion_modul_U_omega_B_ECI_B[3]; - -// '/impeller_cmd' -static uint8_T bpm_blower_2_propulsion_modul_U_impeller_cmd; - -// '/servo_cmd' -static real32_T bpm_blower_2_propulsion_modul_U_servo_cmd[6]; - -// '/veh_cm' -static real32_T bpm_blower_2_propulsion_modul_U_veh_cm[3]; - -// '/impeller_current' -static real32_T bpm_blower_2_propulsion_modul_Y_impeller_current; - -// '/servo_current' -static real32_T bpm_blower_2_propulsion_modul_Y_servo_current[6]; - -// '/torque_B' -static real32_T bpm_blower_2_propulsion_modul_Y_torque_B[3]; - -// '/force_B' -static real32_T bpm_blower_2_propulsion_modul_Y_force_B[3]; - -// '/motor_speed' -static real32_T bpm_blower_2_propulsion_modul_Y_motor_speed; - -// '/nozzle_theta' -static real32_T bpm_blower_2_propulsion_modul_Y_nozzle_theta[6]; - -// '/meas_motor_speed' -static real32_T bpm_blower_2_propulsion_modul_Y_meas_motor_speed; -const char *RT_MEMORY_ALLOCATION_ERROR = "memory allocation error"; - -// -// Associating rt_OneStep with a real-time clock or interrupt service routine -// is what makes the generated code "real-time". The function rt_OneStep is -// always associated with the base rate of the model. Subrates are managed -// by the base rate from inside the generated code. Enabling/disabling -// interrupts and floating point context switches are target specific. This -// example code indicates where these should take place relative to executing -// the generated code step function. Overrun behavior should be tailored to -// your application needs. This example simply sets an error status in the -// real-time model and returns from rt_OneStep. -// -void rt_OneStep(RT_MODEL_bpm_blower_2_propuls_T *const - bpm_blower_2_propulsion_modu_M); -void rt_OneStep(RT_MODEL_bpm_blower_2_propuls_T *const - bpm_blower_2_propulsion_modu_M) -{ - static boolean_T OverrunFlag = false; - - // Disable interrupts here - - // Check for overrun - if (OverrunFlag) { - rtmSetErrorStatus(bpm_blower_2_propulsion_modu_M, "Overrun"); - return; - } - - OverrunFlag = true; - - // Save FPU context here (if necessary) - // Re-enable timer or interrupt here - // Set model inputs here - - // Step the model - bpm_blower_2_propulsion_module_step(bpm_blower_2_propulsion_modu_M, - bpm_blower_2_propulsion_modul_U_battery_voltage, - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B, - bpm_blower_2_propulsion_modul_U_impeller_cmd, - bpm_blower_2_propulsion_modul_U_servo_cmd, - bpm_blower_2_propulsion_modul_U_veh_cm, - &bpm_blower_2_propulsion_modul_Y_impeller_current, - bpm_blower_2_propulsion_modul_Y_servo_current, - bpm_blower_2_propulsion_modul_Y_torque_B, - bpm_blower_2_propulsion_modul_Y_force_B, - &bpm_blower_2_propulsion_modul_Y_motor_speed, - bpm_blower_2_propulsion_modul_Y_nozzle_theta, - &bpm_blower_2_propulsion_modul_Y_meas_motor_speed); - - // Get model outputs here - - // Indicate task complete - OverrunFlag = false; - - // Disable interrupts here - // Restore FPU context here (if necessary) - // Enable interrupts here -} - -// -// The example "main" function illustrates what is required by your -// application code to initialize, execute, and terminate the generated code. -// Attaching rt_OneStep to a real-time clock is target specific. This example -// illustrates how you do this relative to initializing the model. -// -int_T main(int_T argc, const char *argv[]) -{ - RT_MODEL_bpm_blower_2_propuls_T *bpm_blower_2_propulsion_modu_M; - - // Unused arguments - (void)(argc); - (void)(argv); - - // Allocate model data - bpm_blower_2_propulsion_modu_M = bpm_blower_2_propulsion_module - (&bpm_blower_2_propulsion_modul_U_battery_voltage, - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B, - &bpm_blower_2_propulsion_modul_U_impeller_cmd, - bpm_blower_2_propulsion_modul_U_servo_cmd, - bpm_blower_2_propulsion_modul_U_veh_cm, - &bpm_blower_2_propulsion_modul_Y_impeller_current, - bpm_blower_2_propulsion_modul_Y_servo_current, - bpm_blower_2_propulsion_modul_Y_torque_B, - bpm_blower_2_propulsion_modul_Y_force_B, - &bpm_blower_2_propulsion_modul_Y_motor_speed, - bpm_blower_2_propulsion_modul_Y_nozzle_theta, - &bpm_blower_2_propulsion_modul_Y_meas_motor_speed); - if (bpm_blower_2_propulsion_modu_M == NULL) { - (void)fprintf(stderr,"Memory allocation error during model " - "registration"); - return(1); - } - - if (rtmGetErrorStatus(bpm_blower_2_propulsion_modu_M) != NULL) { - (void)fprintf(stderr,"Error during model registration: %s\n", - rtmGetErrorStatus(bpm_blower_2_propulsion_modu_M)); - - // Disable rt_OneStep() here - - // Terminate model - bpm_blower_2_propulsion_module_terminate(bpm_blower_2_propulsion_modu_M); - return(1); - } - - // Initialize model - bpm_blower_2_propulsion_module_initialize(bpm_blower_2_propulsion_modu_M, - &bpm_blower_2_propulsion_modul_U_battery_voltage, - bpm_blower_2_propulsion_modul_U_omega_B_ECI_B, - &bpm_blower_2_propulsion_modul_U_impeller_cmd, - bpm_blower_2_propulsion_modul_U_servo_cmd, - bpm_blower_2_propulsion_modul_U_veh_cm, - &bpm_blower_2_propulsion_modul_Y_impeller_current, - bpm_blower_2_propulsion_modul_Y_servo_current, - bpm_blower_2_propulsion_modul_Y_torque_B, - bpm_blower_2_propulsion_modul_Y_force_B, - &bpm_blower_2_propulsion_modul_Y_motor_speed, - bpm_blower_2_propulsion_modul_Y_nozzle_theta, - &bpm_blower_2_propulsion_modul_Y_meas_motor_speed); - - // Attach rt_OneStep to a timer or interrupt service routine with - // period 0.016 seconds (the model's base sample time) here. The - // call syntax for rt_OneStep is - // - // rt_OneStep(bpm_blower_2_propulsion_modu_M); - - printf("Warning: The simulation will run forever. " - "Generated ERT main won't simulate model step behavior. " - "To change this behavior select the 'MAT-file logging' option.\n"); - fflush((NULL)); - while (rtmGetErrorStatus(bpm_blower_2_propulsion_modu_M) == (NULL)) { - // Perform other application tasks here - } - - // Disable rt_OneStep() here - - // Terminate model - bpm_blower_2_propulsion_module_terminate(bpm_blower_2_propulsion_modu_M); - return 0; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/modelsources.txt b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/modelsources.txt deleted file mode 100644 index dd595f3f67..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/modelsources.txt +++ /dev/null @@ -1 +0,0 @@ - bpm_blower_2_propulsion_module.cpp blower_aerodynamics.cpp diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/rtw_proj.tmw b/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/rtw_proj.tmw deleted file mode 100644 index 3bf0a89ba6..0000000000 --- a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/rtw_proj.tmw +++ /dev/null @@ -1,4 +0,0 @@ -Simulink Coder project for bpm_blower_2_propulsion_module using . MATLAB root = /usr/local/MATLAB/R2016b. SimStruct date: 22-Jul-2016 13:46:14 -This file is generated by Simulink Coder for use by the make utility -to determine when to rebuild objects when the name of the current Simulink Coder project changes. -The rtwinfomat located at: ../slprj/ert/bpm_blower_2_propulsion_module/tmwinternal/binfo.mat diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/buildInfo.mat b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/buildInfo.mat deleted file mode 100644 index 272d38a396..0000000000 Binary files a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/codeInfo.mat b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/codeInfo.mat deleted file mode 100644 index ca14b1d72f..0000000000 Binary files a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/codeInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0.cpp b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0.cpp deleted file mode 100644 index 38fece9f78..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0.cpp +++ /dev/null @@ -1,3368 +0,0 @@ -// -// File: ctl_controller0.cpp -// -// Code generated for Simulink model 'ctl_controller0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:53 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "ctl_controller0.h" -#include "ctl_controller0_private.h" - -const cmd_msg ctl_controller0_rtZcmd_msg = { - 0U, // cmd_timestamp_sec - 0U, // cmd_timestamp_nsec - 0U, // cmd_mode - 0U, // speed_gain_cmd - 0U, // cmd_B_inuse - - { - 0.0F, 0.0F, 0.0F } - , // traj_pos - - { - 0.0F, 0.0F, 0.0F } - , // traj_vel - - { - 0.0F, 0.0F, 0.0F } - , // traj_accel - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // traj_quat - - { - 0.0F, 0.0F, 0.0F } - , // traj_omega - - { - 0.0F, 0.0F, 0.0F } - // traj_alpha -} ; // cmd_msg ground - -const ctl_msg ctl_controller0_rtZctl_msg = { - { - 0.0F, 0.0F, 0.0F } - , // body_force_cmd - - { - 0.0F, 0.0F, 0.0F } - , // body_accel_cmd - - { - 0.0F, 0.0F, 0.0F } - , // pos_err - - { - 0.0F, 0.0F, 0.0F } - , // pos_err_int - - { - 0.0F, 0.0F, 0.0F } - , // body_torque_cmd - - { - 0.0F, 0.0F, 0.0F } - , // body_alpha_cmd - - { - 0.0F, 0.0F, 0.0F } - , // att_err - 0.0F, // att_err_mag - - { - 0.0F, 0.0F, 0.0F } - , // att_err_int - 0U, // ctl_status - 0.0F, // traj_error_pos - 0.0F, // traj_error_att - 0.0F, // traj_error_vel - 0.0F // traj_error_omega -} ; // ctl_msg ground - -const ctl_input_msg ctl_controller0_rtZctl_input_ms = { { 0.0F, 0.0F, 0.0F, 0.0F - }, // est_quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // est_omega_B_ISS_B - { 0.0F, 0.0F, 0.0F }, // est_V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // est_P_B_ISS_ISS - 0U, // est_confidence - { 0U, // timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmd_state_a - { 0U, // timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmd_state_b - 0U, // ctl_mode_cmd - 0U, // current_time_sec - 0U, // current_time_nsec - 0U, // speed_gain_cmd - { 0.0F, 0.0F, 0.0F }, // att_kp - { 0.0F, 0.0F, 0.0F }, // att_ki - { 0.0F, 0.0F, 0.0F }, // omega_kd - { 0.0F, 0.0F, 0.0F }, // pos_kp - { 0.0F, 0.0F, 0.0F }, // pos_ki - { 0.0F, 0.0F, 0.0F }, // vel_kd - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// inertia_matrix - 0.0F // mass -}; - -// -// Output and update for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void ctl_controller0_Normalize(const real32_T rtu_q_in[4], real32_T - rty_positive_scalar_q[4], P_Normalize_ctl_controller0_T *localP) -{ - // Product: '/Product' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - - rty_positive_scalar_q[0] = rtu_q_in[0] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[1] = rtu_q_in[1] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[2] = rtu_q_in[2] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[3] = rtu_q_in[3] * (real32_T)localP->Constant1_Value; -} - -// -// Termination for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void ctl_controller0_Normalize_Term(void) -{ -} - -// -// Output and update for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void ctl_controller0_Normalize_e(const real32_T rtu_Vec[4], real32_T - rtu_Magnitude, real32_T rty_Normalized_Vec[4]) -{ - // Product: '/Divide' - rty_Normalized_Vec[0] = rtu_Vec[0] / rtu_Magnitude; - rty_Normalized_Vec[1] = rtu_Vec[1] / rtu_Magnitude; - rty_Normalized_Vec[2] = rtu_Vec[2] / rtu_Magnitude; - rty_Normalized_Vec[3] = rtu_Vec[3] / rtu_Magnitude; -} - -// -// Termination for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void ctl_controller_Normalize_j_Term(void) -{ -} - -// -// System initialize for iterator system: -// '/For Each Subsystem' -// '/For Each Subsystem1' -// -void ctl_contr_ForEachSubsystem_Init(int32_T NumIters, - DW_ForEachSubsystem_ctl_contr_T localDW[3], P_ForEachSubsystem_ctl_contro_T - *localP) -{ - // local scratch DWork variables - int32_T ForEach_itr; - for (ForEach_itr = 0; ForEach_itr < NumIters; ForEach_itr++) { - // InitializeConditions for DiscreteTransferFcn: '/3 Hz Low Pass' - localDW[ForEach_itr].CoreSubsys.uHzLowPass_states = - localP->CoreSubsys.uHzLowPass_InitialStates; - } -} - -// -// Start for iterator system: -// '/For Each Subsystem' -// '/For Each Subsystem1' -// -void ctl_cont_ForEachSubsystem_Start(int32_T NumIters, - DW_ForEachSubsystem_ctl_contr_T localDW[3]) -{ - // local scratch DWork variables - int32_T ForEach_itr; - for (ForEach_itr = 0; ForEach_itr < NumIters; ForEach_itr++) { - localDW[ForEach_itr].CoreSubsys.uHzLowPass_states = 0.0F; - } -} - -// -// Output and update for iterator system: -// '/For Each Subsystem' -// '/For Each Subsystem1' -// -void ctl_controller_ForEachSubsystem(int32_T NumIters, const real32_T rtu_X[3], - real32_T rty_Y[3], DW_ForEachSubsystem_ctl_contr_T localDW[3], - P_ForEachSubsystem_ctl_contro_T *localP, real32_T rtp_filt_enable) -{ - // local scratch DWork variables - int32_T ForEach_itr; - real32_T rtb_uHzLowPass; - real32_T uHzLowPass_tmp; - - // Outputs for Iterator SubSystem: '/For Each Subsystem' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr = 0; ForEach_itr < NumIters; ForEach_itr++) { - // DiscreteTransferFcn: '/3 Hz Low Pass' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_X_at_outport_0' - - uHzLowPass_tmp = (rtu_X[ForEach_itr] - localP-> - CoreSubsys.uHzLowPass_DenCoef[1] * localDW[ForEach_itr]. - CoreSubsys.uHzLowPass_states) / - localP->CoreSubsys.uHzLowPass_DenCoef[0]; - rtb_uHzLowPass = localP->CoreSubsys.uHzLowPass_NumCoef[0] * uHzLowPass_tmp + - localP->CoreSubsys.uHzLowPass_NumCoef[1] * localDW[ForEach_itr]. - CoreSubsys.uHzLowPass_states; - - // Update for DiscreteTransferFcn: '/3 Hz Low Pass' - localDW[ForEach_itr].CoreSubsys.uHzLowPass_states = uHzLowPass_tmp; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - - if (rtp_filt_enable != 0.0F) { - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_Y_at_inport_0' - rty_Y[ForEach_itr] = rtb_uHzLowPass; - } else { - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_Y_at_inport_0' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_X_at_outport_0' - - rty_Y[ForEach_itr] = rtu_X[ForEach_itr]; - } - - // End of Switch: '/Switch' - } - - // End of Outputs for SubSystem: '/For Each Subsystem' -} - -// -// Termination for iterator system: -// '/For Each Subsystem' -// '/For Each Subsystem1' -// -void ctl_contr_ForEachSubsystem_Term(void) -{ -} - -// Model step function -void ctl_controller0_step(RT_MODEL_ctl_controller0_T *const ctl_controller0_M, - ctl_input_msg *ctl_controller0_U_ctl_input_msg_l, cmd_msg - *ctl_controller0_Y_cmd_msg_f, ctl_msg *ctl_controller0_Y_ctl_msg_n) -{ - P_ctl_controller0_T *ctl_controller0_P = ((P_ctl_controller0_T *) - ctl_controller0_M->defaultParam); - DW_ctl_controller0_T *ctl_controller0_DW = ((DW_ctl_controller0_T *) - ctl_controller0_M->dwork); - - // local block i/o variables - real32_T rtb_ImpAsg_InsertedFor_Y_at_inp[3]; - real32_T rtb_ImpAsg_InsertedFor_Y_at_i_d[3]; - real32_T normA; - real32_T b_s; - int32_T eint; - static const real32_T theta[3] = { 0.425873F, 1.8801527F, 3.92572474F }; - - real32_T rtb_Assignment_h[9]; - real32_T rtb_TSamp[4]; - real32_T rtb_Product[3]; - real32_T rtb_Product5[16]; - real32_T rtb_Product_b[4]; - real32_T rtb_Sqrt; - real32_T rtb_SumofElements; - real32_T rtb_SumofElements1; - boolean_T rtb_LogicalOperator2_c; - uint8_T rtb_Switch3; - real32_T rtb_VectorConcatenate[16]; - real32_T rtb_VectorConcatenate_m[16]; - real32_T rtb_Product1_m[4]; - real32_T rtb_Merge_pa[4]; - real32_T rtb_Divide_f[3]; - uint8_T rtb_Switch12; - boolean_T rtb_LogicalOperator2; - real32_T rtb_Assignment_l[9]; - boolean_T rtb_Compare_j; - real32_T rtb_Switch_b[3]; - boolean_T rtb_Compare_fc; - real32_T rtb_Merge[4]; - real32_T rtb_Sum_f[3]; - real32_T rtb_Merge_j[4]; - real32_T rtb_y[16]; - real32_T rtb_Diff[3]; - uint32_T rtb_Switch_cmd_state_a_timestam; - uint32_T rtb_Switch_cmd_state_a_timest_0; - uint32_T rtb_Switch_cmd_state_b_timestam; - uint32_T rtb_Switch_cmd_state_b_timest_0; - uint8_T rtb_Switch_ctl_mode_cmd; - uint32_T rtb_Switch_current_time_sec; - uint32_T rtb_Switch_current_time_nsec; - uint8_T rtb_Switch_speed_gain_cmd; - real32_T rtb_Switch_vel_kd[3]; - real32_T rtb_Switch_inertia_matrix[9]; - real32_T rtb_Switch_mass; - int32_T i; - real32_T rtb_Product5_0[16]; - real32_T rtb_Assignment_b[12]; - real32_T tmp[12]; - int32_T i_0; - real32_T rtb_VectorConcatenate_p[16]; - real32_T tmp_0[9]; - real32_T rtb_Merge_j_0[9]; - real32_T rtb_Assignment_b_0[9]; - real32_T rtb_Assignment_b_1[3]; - real32_T tmp_1[9]; - real32_T tmp_2[9]; - real32_T tmp_3[9]; - real32_T tmp_4[9]; - real32_T rtb_Product_f; - real32_T rtb_Sum4_p; - real32_T rtb_SumA21_f; - real32_T rtb_Sum4_o_idx_2; - real32_T rtb_Sum4_o_idx_1; - real32_T rtb_Sum4_o_idx_0; - real32_T rtb_Switch_cmd_state_b_alpha_B_; - real32_T rtb_Switch_cmd_state_b_alpha__0; - real32_T rtb_Switch_cmd_state_b_alpha__1; - real32_T rtb_Switch_est_omega_B_ISS_B_id; - real32_T rtb_Switch_omega_kd_idx_0; - real32_T rtb_Switch_est_omega_B_ISS_B__0; - real32_T rtb_Switch_omega_kd_idx_1; - real32_T rtb_Switch_est_omega_B_ISS_B__1; - real32_T rtb_Switch_omega_kd_idx_2; - real32_T rtb_Sum3_k_idx_0; - real32_T rtb_Switch_est_V_B_ISS_ISS_idx_; - real32_T rtb_Switch_est_P_B_ISS_ISS_idx_; - real32_T rtb_Switch_cmd_state_a_A_B_ISS_; - real32_T rtb_Switch_est_V_B_ISS_ISS_id_0; - real32_T rtb_Switch_est_P_B_ISS_ISS_id_0; - real32_T rtb_Switch_cmd_state_a_P_B_ISS_; - real32_T rtb_Switch_cmd_state_a_V_B_ISS_; - real32_T rtb_Switch_cmd_state_a_A_B_IS_0; - real32_T rtb_Switch_est_V_B_ISS_ISS_id_1; - real32_T rtb_Switch_est_P_B_ISS_ISS_id_1; - real32_T rtb_Switch_cmd_state_a_P_B_IS_0; - real32_T rtb_Switch_cmd_state_a_A_B_IS_1; - real32_T rtb_Switch8_idx_0; - real32_T rtb_Sum2_idx_0; - real32_T rtb_Gain1_j_idx_0; - real32_T rtb_Switch_c_idx_0; - real32_T rtb_Switch8_idx_1; - real32_T rtb_Sum2_idx_1; - real32_T rtb_Gain1_j_idx_1; - real32_T rtb_Switch_c_idx_1; - real32_T rtb_Switch8_idx_2; - real32_T rtb_Sum2_idx_2; - real32_T rtb_Gain1_j_idx_2; - real32_T rtb_Switch_c_idx_2; - real32_T rtb_Switch_att_ki_idx_0; - real32_T rtb_Switch_pos_ki_idx_0; - real32_T rtb_Switch_att_ki_idx_1; - real32_T rtb_Switch_pos_ki_idx_1; - real32_T rtb_Switch_att_ki_idx_2; - real32_T rtb_Switch_pos_ki_idx_2; - real32_T rtb_SumA21_idx_0; - real32_T rtb_SumA21_idx_1; - real32_T rtb_SumA21_d_idx_0; - real32_T rtb_SumA21_d_idx_1; - real32_T rtb_Sum3_k_idx_1; - real32_T rtb_Sum3_k_idx_2; - boolean_T exitg1; - boolean_T exitg2; - - // Outputs for Atomic SubSystem: '/ctl_controller' - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Assignment_h[i] = (real32_T)ctl_controller0_P->Constant2_Value[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' incorporates: - // Inport: '/ctl_input_msg' - - rtb_Assignment_h[0] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[3]; - rtb_Assignment_h[4] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[3]; - rtb_Assignment_h[8] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[3]; - - // SampleTimeMath: '/TSamp' incorporates: - // Inport: '/ctl_input_msg' - // - // About '/TSamp': - // y = u * K where K = 1 / ( w * Ts ) - - rtb_TSamp[0] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[0] * - ctl_controller0_P->TSamp_WtEt; - rtb_TSamp[1] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[1] * - ctl_controller0_P->TSamp_WtEt; - rtb_TSamp[2] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[2] * - ctl_controller0_P->TSamp_WtEt; - rtb_TSamp[3] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[3] * - ctl_controller0_P->TSamp_WtEt; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Inport: '/ctl_input_msg' - - rtb_Switch_inertia_matrix[0] = (real32_T)ctl_controller0_P->Constant3_Value; - rtb_Switch_inertia_matrix[1] = - ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[2]; - rtb_Switch_inertia_matrix[2] = ctl_controller0_P->Gain_Gain_p * - ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[1]; - rtb_Switch_inertia_matrix[3] = ctl_controller0_P->Gain1_Gain_e * - ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[2]; - rtb_Switch_inertia_matrix[4] = (real32_T)ctl_controller0_P->Constant3_Value; - rtb_Switch_inertia_matrix[5] = - ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[0]; - rtb_Switch_inertia_matrix[6] = - ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[1]; - rtb_Switch_inertia_matrix[7] = ctl_controller0_P->Gain2_Gain_k * - ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[0]; - rtb_Switch_inertia_matrix[8] = (real32_T)ctl_controller0_P->Constant3_Value; - - // Math: '/Math Function' incorporates: - // Concatenate: '/Matrix Concatenate' - // Gain: '/Gain' - // Gain: '/Gain1' - // Inport: '/ctl_input_msg' - // Sum: '/Sum2' - - for (i = 0; i < 3; i++) { - rtb_Assignment_b[(int32_T)(3 * i)] = rtb_Assignment_h[i] + - rtb_Switch_inertia_matrix[i]; - rtb_Assignment_b[(int32_T)(1 + (int32_T)(3 * i))] = rtb_Assignment_h - [(int32_T)(i + 3)] + rtb_Switch_inertia_matrix[(int32_T)(i + 3)]; - rtb_Assignment_b[(int32_T)(2 + (int32_T)(3 * i))] = rtb_Assignment_h - [(int32_T)(i + 6)] + rtb_Switch_inertia_matrix[(int32_T)(i + 6)]; - rtb_Assignment_b[(int32_T)(9 + i)] = ctl_controller0_P->Gain1_Gain_o * - ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[i]; - } - - // End of Math: '/Math Function' - for (i = 0; i < 4; i++) { - // Gain: '/Gain' incorporates: - // Product: '/Product' - - tmp[(int32_T)(3 * i)] = rtb_Assignment_b[(int32_T)(3 * i)] * - ctl_controller0_P->Gain_Gain_m; - tmp[(int32_T)(1 + (int32_T)(3 * i))] = rtb_Assignment_b[(int32_T)((int32_T) - (3 * i) + 1)] * ctl_controller0_P->Gain_Gain_m; - tmp[(int32_T)(2 + (int32_T)(3 * i))] = rtb_Assignment_b[(int32_T)((int32_T) - (3 * i) + 2)] * ctl_controller0_P->Gain_Gain_m; - - // Sum: '/Diff' incorporates: - // Product: '/Product' - // UnitDelay: '/UD' - - rtb_Product_b[i] = rtb_TSamp[i] - ctl_controller0_DW->UD_DSTATE[i]; - } - - // Product: '/Product' - for (i = 0; i < 3; i++) { - rtb_Product_f = tmp[(int32_T)(i + 9)] * rtb_Product_b[3] + (tmp[(int32_T)(i - + 6)] * rtb_Product_b[2] + (tmp[(int32_T)(i + 3)] * rtb_Product_b[1] + - tmp[i] * rtb_Product_b[0])); - rtb_Product[i] = rtb_Product_f; - } - - // Outputs for Iterator SubSystem: '/For Each Subsystem' - ctl_controller_ForEachSubsystem(3, rtb_Product, - rtb_ImpAsg_InsertedFor_Y_at_i_d, ctl_controller0_DW->ForEachSubsystem, - (P_ForEachSubsystem_ctl_contro_T *)&ctl_controller0_P->ForEachSubsystem, - ctl_controller0_P->tun_truth_q_omega_filt_enable); - - // End of Outputs for SubSystem: '/For Each Subsystem' - - // SampleTimeMath: '/TSamp' incorporates: - // Inport: '/ctl_input_msg' - // - // About '/TSamp': - // y = u * K where K = 1 / ( w * Ts ) - - rtb_Product_f = ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[0] * - ctl_controller0_P->TSamp_WtEt_k; - - // Sum: '/Diff' incorporates: - // UnitDelay: '/UD' - - rtb_Diff[0] = rtb_Product_f - ctl_controller0_DW->UD_DSTATE_e[0]; - - // SampleTimeMath: '/TSamp' incorporates: - // Inport: '/ctl_input_msg' - // - // About '/TSamp': - // y = u * K where K = 1 / ( w * Ts ) - - rtb_Product[0] = rtb_Product_f; - rtb_Product_f = ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[1] * - ctl_controller0_P->TSamp_WtEt_k; - - // Sum: '/Diff' incorporates: - // UnitDelay: '/UD' - - rtb_Diff[1] = rtb_Product_f - ctl_controller0_DW->UD_DSTATE_e[1]; - - // SampleTimeMath: '/TSamp' incorporates: - // Inport: '/ctl_input_msg' - // - // About '/TSamp': - // y = u * K where K = 1 / ( w * Ts ) - - rtb_Product[1] = rtb_Product_f; - rtb_Product_f = ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[2] * - ctl_controller0_P->TSamp_WtEt_k; - - // Sum: '/Diff' incorporates: - // UnitDelay: '/UD' - - rtb_Diff[2] = rtb_Product_f - ctl_controller0_DW->UD_DSTATE_e[2]; - - // Outputs for Iterator SubSystem: '/For Each Subsystem1' - ctl_controller_ForEachSubsystem(3, rtb_Diff, rtb_ImpAsg_InsertedFor_Y_at_inp, - ctl_controller0_DW->ForEachSubsystem1, (P_ForEachSubsystem_ctl_contro_T *) - &ctl_controller0_P->ForEachSubsystem1, - ctl_controller0_P->tun_truth_velocity_filt_enable); - - // End of Outputs for SubSystem: '/For Each Subsystem1' - - // Switch: '/Switch' incorporates: - // BusAssignment: '/Bus Assignment' - // Constant: '/Constant' - // Inport: '/ctl_input_msg' - - if ((int32_T)ctl_controller0_P->tun_debug_ctl_use_truth != 0) { - rtb_Merge_j[0] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[0]; - rtb_Merge_j[1] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[1]; - rtb_Merge_j[2] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[2]; - rtb_Merge_j[3] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[3]; - rtb_Switch12 = ctl_controller0_U_ctl_input_msg_l->est_confidence; - rtb_Switch_cmd_state_a_timestam = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.timestamp_sec; - rtb_Switch_cmd_state_a_timest_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.timestamp_nsec; - rtb_Switch_est_omega_B_ISS_B_id = rtb_ImpAsg_InsertedFor_Y_at_i_d[0]; - rtb_Switch_est_V_B_ISS_ISS_idx_ = rtb_ImpAsg_InsertedFor_Y_at_inp[0]; - rtb_Switch_est_P_B_ISS_ISS_idx_ = - ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[0]; - normA = ctl_controller0_U_ctl_input_msg_l->cmd_state_a.P_B_ISS_ISS[0]; - b_s = ctl_controller0_U_ctl_input_msg_l->cmd_state_a.V_B_ISS_ISS[0]; - rtb_Switch_cmd_state_a_A_B_ISS_ = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.A_B_ISS_ISS[0]; - rtb_Switch_est_omega_B_ISS_B__0 = rtb_ImpAsg_InsertedFor_Y_at_i_d[1]; - rtb_Switch_est_V_B_ISS_ISS_id_0 = rtb_ImpAsg_InsertedFor_Y_at_inp[1]; - rtb_Switch_est_P_B_ISS_ISS_id_0 = - ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[1]; - rtb_Switch_cmd_state_a_P_B_ISS_ = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.P_B_ISS_ISS[1]; - rtb_Switch_cmd_state_a_V_B_ISS_ = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.V_B_ISS_ISS[1]; - rtb_Switch_cmd_state_a_A_B_IS_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.A_B_ISS_ISS[1]; - rtb_Switch_est_omega_B_ISS_B__1 = rtb_ImpAsg_InsertedFor_Y_at_i_d[2]; - rtb_Switch_est_V_B_ISS_ISS_id_1 = rtb_ImpAsg_InsertedFor_Y_at_inp[2]; - rtb_Switch_est_P_B_ISS_ISS_id_1 = - ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[2]; - rtb_Switch_cmd_state_a_P_B_IS_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.P_B_ISS_ISS[2]; - rtb_Sum3_k_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.V_B_ISS_ISS[2]; - rtb_Switch_cmd_state_a_A_B_IS_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.A_B_ISS_ISS[2]; - rtb_Product1_m[0] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.quat_ISS2B[0]; - rtb_Product1_m[1] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.quat_ISS2B[1]; - rtb_Product1_m[2] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.quat_ISS2B[2]; - rtb_Product1_m[3] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.quat_ISS2B[3]; - rtb_Switch_cmd_state_b_timestam = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.timestamp_sec; - rtb_Switch_cmd_state_b_timest_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.timestamp_nsec; - rtb_Switch8_idx_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.omega_B_ISS_B[0]; - rtb_Sum2_idx_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.alpha_B_ISS_B[0]; - rtb_Gain1_j_idx_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.P_B_ISS_ISS[0]; - rtb_Switch_c_idx_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.V_B_ISS_ISS[0]; - rtb_Diff[0] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.A_B_ISS_ISS[0]; - rtb_Switch8_idx_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.omega_B_ISS_B[1]; - rtb_Sum2_idx_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.alpha_B_ISS_B[1]; - rtb_Gain1_j_idx_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.P_B_ISS_ISS[1]; - rtb_Switch_c_idx_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.V_B_ISS_ISS[1]; - rtb_Diff[1] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.A_B_ISS_ISS[1]; - rtb_Switch8_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.omega_B_ISS_B[2]; - rtb_Sum2_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.alpha_B_ISS_B[2]; - rtb_Gain1_j_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.P_B_ISS_ISS[2]; - rtb_Switch_c_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.V_B_ISS_ISS[2]; - rtb_Diff[2] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.A_B_ISS_ISS[2]; - rtb_Merge_pa[0] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.quat_ISS2B - [0]; - rtb_Merge_pa[1] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.quat_ISS2B - [1]; - rtb_Merge_pa[2] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.quat_ISS2B - [2]; - rtb_Merge_pa[3] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.quat_ISS2B - [3]; - rtb_Switch_ctl_mode_cmd = ctl_controller0_U_ctl_input_msg_l->ctl_mode_cmd; - rtb_Switch_current_time_sec = - ctl_controller0_U_ctl_input_msg_l->current_time_sec; - rtb_Switch_current_time_nsec = - ctl_controller0_U_ctl_input_msg_l->current_time_nsec; - rtb_Switch_speed_gain_cmd = - ctl_controller0_U_ctl_input_msg_l->speed_gain_cmd; - rtb_Divide_f[0] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.omega_B_ISS_B[0]; - rtb_Switch_cmd_state_b_alpha__1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.alpha_B_ISS_B[0]; - rtb_Switch_b[0] = ctl_controller0_U_ctl_input_msg_l->att_kp[0]; - rtb_Switch_att_ki_idx_0 = ctl_controller0_U_ctl_input_msg_l->att_ki[0]; - rtb_Switch_omega_kd_idx_0 = ctl_controller0_U_ctl_input_msg_l->omega_kd[0]; - rtb_Sum_f[0] = ctl_controller0_U_ctl_input_msg_l->pos_kp[0]; - rtb_Switch_pos_ki_idx_0 = ctl_controller0_U_ctl_input_msg_l->pos_ki[0]; - rtb_Switch_vel_kd[0] = ctl_controller0_U_ctl_input_msg_l->vel_kd[0]; - rtb_Divide_f[1] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.omega_B_ISS_B[1]; - rtb_Switch_cmd_state_b_alpha__0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.alpha_B_ISS_B[1]; - rtb_Switch_b[1] = ctl_controller0_U_ctl_input_msg_l->att_kp[1]; - rtb_Switch_att_ki_idx_1 = ctl_controller0_U_ctl_input_msg_l->att_ki[1]; - rtb_Switch_omega_kd_idx_1 = ctl_controller0_U_ctl_input_msg_l->omega_kd[1]; - rtb_Sum_f[1] = ctl_controller0_U_ctl_input_msg_l->pos_kp[1]; - rtb_Switch_pos_ki_idx_1 = ctl_controller0_U_ctl_input_msg_l->pos_ki[1]; - rtb_Switch_vel_kd[1] = ctl_controller0_U_ctl_input_msg_l->vel_kd[1]; - rtb_Divide_f[2] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.omega_B_ISS_B[2]; - rtb_Switch_cmd_state_b_alpha_B_ = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.alpha_B_ISS_B[2]; - rtb_Switch_b[2] = ctl_controller0_U_ctl_input_msg_l->att_kp[2]; - rtb_Switch_att_ki_idx_2 = ctl_controller0_U_ctl_input_msg_l->att_ki[2]; - rtb_Switch_omega_kd_idx_2 = ctl_controller0_U_ctl_input_msg_l->omega_kd[2]; - rtb_Sum_f[2] = ctl_controller0_U_ctl_input_msg_l->pos_kp[2]; - rtb_Switch_pos_ki_idx_2 = ctl_controller0_U_ctl_input_msg_l->pos_ki[2]; - rtb_Switch_vel_kd[2] = ctl_controller0_U_ctl_input_msg_l->vel_kd[2]; - for (i = 0; i < 9; i++) { - rtb_Switch_inertia_matrix[i] = - ctl_controller0_U_ctl_input_msg_l->inertia_matrix[i]; - } - - rtb_Switch_mass = ctl_controller0_U_ctl_input_msg_l->mass; - } else { - rtb_Merge_j[0] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[0]; - rtb_Merge_j[1] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[1]; - rtb_Merge_j[2] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[2]; - rtb_Merge_j[3] = ctl_controller0_U_ctl_input_msg_l->est_quat_ISS2B[3]; - rtb_Switch12 = ctl_controller0_U_ctl_input_msg_l->est_confidence; - rtb_Switch_cmd_state_a_timestam = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.timestamp_sec; - rtb_Switch_cmd_state_a_timest_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.timestamp_nsec; - rtb_Switch_est_omega_B_ISS_B_id = - ctl_controller0_U_ctl_input_msg_l->est_omega_B_ISS_B[0]; - rtb_Switch_est_V_B_ISS_ISS_idx_ = - ctl_controller0_U_ctl_input_msg_l->est_V_B_ISS_ISS[0]; - rtb_Switch_est_P_B_ISS_ISS_idx_ = - ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[0]; - normA = ctl_controller0_U_ctl_input_msg_l->cmd_state_a.P_B_ISS_ISS[0]; - b_s = ctl_controller0_U_ctl_input_msg_l->cmd_state_a.V_B_ISS_ISS[0]; - rtb_Switch_cmd_state_a_A_B_ISS_ = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.A_B_ISS_ISS[0]; - rtb_Switch_est_omega_B_ISS_B__0 = - ctl_controller0_U_ctl_input_msg_l->est_omega_B_ISS_B[1]; - rtb_Switch_est_V_B_ISS_ISS_id_0 = - ctl_controller0_U_ctl_input_msg_l->est_V_B_ISS_ISS[1]; - rtb_Switch_est_P_B_ISS_ISS_id_0 = - ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[1]; - rtb_Switch_cmd_state_a_P_B_ISS_ = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.P_B_ISS_ISS[1]; - rtb_Switch_cmd_state_a_V_B_ISS_ = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.V_B_ISS_ISS[1]; - rtb_Switch_cmd_state_a_A_B_IS_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.A_B_ISS_ISS[1]; - rtb_Switch_est_omega_B_ISS_B__1 = - ctl_controller0_U_ctl_input_msg_l->est_omega_B_ISS_B[2]; - rtb_Switch_est_V_B_ISS_ISS_id_1 = - ctl_controller0_U_ctl_input_msg_l->est_V_B_ISS_ISS[2]; - rtb_Switch_est_P_B_ISS_ISS_id_1 = - ctl_controller0_U_ctl_input_msg_l->est_P_B_ISS_ISS[2]; - rtb_Switch_cmd_state_a_P_B_IS_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.P_B_ISS_ISS[2]; - rtb_Sum3_k_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.V_B_ISS_ISS[2]; - rtb_Switch_cmd_state_a_A_B_IS_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.A_B_ISS_ISS[2]; - rtb_Product1_m[0] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.quat_ISS2B[0]; - rtb_Product1_m[1] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.quat_ISS2B[1]; - rtb_Product1_m[2] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.quat_ISS2B[2]; - rtb_Product1_m[3] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.quat_ISS2B[3]; - rtb_Switch_cmd_state_b_timestam = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.timestamp_sec; - rtb_Switch_cmd_state_b_timest_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.timestamp_nsec; - rtb_Switch8_idx_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.omega_B_ISS_B[0]; - rtb_Sum2_idx_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.alpha_B_ISS_B[0]; - rtb_Gain1_j_idx_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.P_B_ISS_ISS[0]; - rtb_Switch_c_idx_0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.V_B_ISS_ISS[0]; - rtb_Diff[0] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.A_B_ISS_ISS[0]; - rtb_Switch8_idx_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.omega_B_ISS_B[1]; - rtb_Sum2_idx_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.alpha_B_ISS_B[1]; - rtb_Gain1_j_idx_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.P_B_ISS_ISS[1]; - rtb_Switch_c_idx_1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.V_B_ISS_ISS[1]; - rtb_Diff[1] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.A_B_ISS_ISS[1]; - rtb_Switch8_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.omega_B_ISS_B[2]; - rtb_Sum2_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_a.alpha_B_ISS_B[2]; - rtb_Gain1_j_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.P_B_ISS_ISS[2]; - rtb_Switch_c_idx_2 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.V_B_ISS_ISS[2]; - rtb_Diff[2] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.A_B_ISS_ISS[2]; - rtb_Merge_pa[0] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.quat_ISS2B - [0]; - rtb_Merge_pa[1] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.quat_ISS2B - [1]; - rtb_Merge_pa[2] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.quat_ISS2B - [2]; - rtb_Merge_pa[3] = ctl_controller0_U_ctl_input_msg_l->cmd_state_b.quat_ISS2B - [3]; - rtb_Switch_ctl_mode_cmd = ctl_controller0_U_ctl_input_msg_l->ctl_mode_cmd; - rtb_Switch_current_time_sec = - ctl_controller0_U_ctl_input_msg_l->current_time_sec; - rtb_Switch_current_time_nsec = - ctl_controller0_U_ctl_input_msg_l->current_time_nsec; - rtb_Switch_speed_gain_cmd = - ctl_controller0_U_ctl_input_msg_l->speed_gain_cmd; - rtb_Divide_f[0] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.omega_B_ISS_B[0]; - rtb_Switch_cmd_state_b_alpha__1 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.alpha_B_ISS_B[0]; - rtb_Switch_b[0] = ctl_controller0_U_ctl_input_msg_l->att_kp[0]; - rtb_Switch_att_ki_idx_0 = ctl_controller0_U_ctl_input_msg_l->att_ki[0]; - rtb_Switch_omega_kd_idx_0 = ctl_controller0_U_ctl_input_msg_l->omega_kd[0]; - rtb_Sum_f[0] = ctl_controller0_U_ctl_input_msg_l->pos_kp[0]; - rtb_Switch_pos_ki_idx_0 = ctl_controller0_U_ctl_input_msg_l->pos_ki[0]; - rtb_Switch_vel_kd[0] = ctl_controller0_U_ctl_input_msg_l->vel_kd[0]; - rtb_Divide_f[1] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.omega_B_ISS_B[1]; - rtb_Switch_cmd_state_b_alpha__0 = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.alpha_B_ISS_B[1]; - rtb_Switch_b[1] = ctl_controller0_U_ctl_input_msg_l->att_kp[1]; - rtb_Switch_att_ki_idx_1 = ctl_controller0_U_ctl_input_msg_l->att_ki[1]; - rtb_Switch_omega_kd_idx_1 = ctl_controller0_U_ctl_input_msg_l->omega_kd[1]; - rtb_Sum_f[1] = ctl_controller0_U_ctl_input_msg_l->pos_kp[1]; - rtb_Switch_pos_ki_idx_1 = ctl_controller0_U_ctl_input_msg_l->pos_ki[1]; - rtb_Switch_vel_kd[1] = ctl_controller0_U_ctl_input_msg_l->vel_kd[1]; - rtb_Divide_f[2] = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.omega_B_ISS_B[2]; - rtb_Switch_cmd_state_b_alpha_B_ = - ctl_controller0_U_ctl_input_msg_l->cmd_state_b.alpha_B_ISS_B[2]; - rtb_Switch_b[2] = ctl_controller0_U_ctl_input_msg_l->att_kp[2]; - rtb_Switch_att_ki_idx_2 = ctl_controller0_U_ctl_input_msg_l->att_ki[2]; - rtb_Switch_omega_kd_idx_2 = ctl_controller0_U_ctl_input_msg_l->omega_kd[2]; - rtb_Sum_f[2] = ctl_controller0_U_ctl_input_msg_l->pos_kp[2]; - rtb_Switch_pos_ki_idx_2 = ctl_controller0_U_ctl_input_msg_l->pos_ki[2]; - rtb_Switch_vel_kd[2] = ctl_controller0_U_ctl_input_msg_l->vel_kd[2]; - for (i = 0; i < 9; i++) { - rtb_Switch_inertia_matrix[i] = - ctl_controller0_U_ctl_input_msg_l->inertia_matrix[i]; - } - - rtb_Switch_mass = ctl_controller0_U_ctl_input_msg_l->mass; - } - - // End of Switch: '/Switch' - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Assignment_h[i] = (real32_T)ctl_controller0_P->Constant2_Value_p[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' incorporates: - // UnitDelay: '/Unit Delay2' - - rtb_Assignment_h[0] = ctl_controller0_DW->UnitDelay2_DSTATE[3]; - - // Gain: '/Gain' incorporates: - // UnitDelay: '/Unit Delay2' - - rtb_Sum4_o_idx_0 = ctl_controller0_P->Gain_Gain_a * - ctl_controller0_DW->UnitDelay2_DSTATE[0]; - - // Assignment: '/Assignment' incorporates: - // UnitDelay: '/Unit Delay2' - - rtb_Assignment_h[4] = ctl_controller0_DW->UnitDelay2_DSTATE[3]; - - // Gain: '/Gain' incorporates: - // UnitDelay: '/Unit Delay2' - - rtb_Sum4_o_idx_1 = ctl_controller0_P->Gain_Gain_a * - ctl_controller0_DW->UnitDelay2_DSTATE[1]; - - // Assignment: '/Assignment' incorporates: - // UnitDelay: '/Unit Delay2' - - rtb_Assignment_h[8] = ctl_controller0_DW->UnitDelay2_DSTATE[3]; - - // Gain: '/Gain' incorporates: - // UnitDelay: '/Unit Delay2' - - rtb_Sum4_o_idx_2 = ctl_controller0_P->Gain_Gain_a * - ctl_controller0_DW->UnitDelay2_DSTATE[2]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Assignment_l[0] = (real32_T)ctl_controller0_P->Constant3_Value_a; - rtb_Assignment_l[1] = rtb_Sum4_o_idx_2; - rtb_Assignment_l[2] = ctl_controller0_P->Gain_Gain_i * rtb_Sum4_o_idx_1; - rtb_Assignment_l[3] = ctl_controller0_P->Gain1_Gain_n * rtb_Sum4_o_idx_2; - rtb_Assignment_l[4] = (real32_T)ctl_controller0_P->Constant3_Value_a; - rtb_Assignment_l[5] = rtb_Sum4_o_idx_0; - rtb_Assignment_l[6] = rtb_Sum4_o_idx_1; - rtb_Assignment_l[7] = ctl_controller0_P->Gain2_Gain_a * rtb_Sum4_o_idx_0; - rtb_Assignment_l[8] = (real32_T)ctl_controller0_P->Constant3_Value_a; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i = 0; i < 3; i++) { - rtb_Product5[(int32_T)(i << 2)] = rtb_Assignment_h[(int32_T)(3 * i)] + - rtb_Assignment_l[(int32_T)(3 * i)]; - rtb_Product5[(int32_T)(1 + (int32_T)(i << 2))] = rtb_Assignment_h[(int32_T) - ((int32_T)(3 * i) + 1)] + rtb_Assignment_l[(int32_T)((int32_T)(3 * i) + 1)]; - rtb_Product5[(int32_T)(2 + (int32_T)(i << 2))] = rtb_Assignment_h[(int32_T) - ((int32_T)(3 * i) + 2)] + rtb_Assignment_l[(int32_T)((int32_T)(3 * i) + 2)]; - } - - rtb_Product5[3] = ctl_controller0_P->Gain1_Gain_j * rtb_Sum4_o_idx_0; - rtb_Product5[7] = ctl_controller0_P->Gain1_Gain_j * rtb_Sum4_o_idx_1; - rtb_Product5[11] = ctl_controller0_P->Gain1_Gain_j * rtb_Sum4_o_idx_2; - - // End of Concatenate: '/Matrix Concatenate' - - // Reshape: '/Reshape1' incorporates: - // UnitDelay: '/Unit Delay2' - - rtb_Product5[12] = rtb_Sum4_o_idx_0; - rtb_Product5[13] = rtb_Sum4_o_idx_1; - rtb_Product5[14] = rtb_Sum4_o_idx_2; - rtb_Product5[15] = ctl_controller0_DW->UnitDelay2_DSTATE[3]; - - // Product: '/Product' - for (i = 0; i < 4; i++) { - rtb_SumofElements = rtb_Product5[(int32_T)(i + 12)] * rtb_Merge_j[3] + - (rtb_Product5[(int32_T)(i + 8)] * rtb_Merge_j[2] + (rtb_Product5[(int32_T) - (i + 4)] * rtb_Merge_j[1] + rtb_Product5[i] * rtb_Merge_j[0])); - rtb_Product_b[i] = rtb_SumofElements; - } - - // End of Product: '/Product' - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product_b[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - ctl_controller0_Normalize(rtb_Product_b, rtb_Merge, - (P_Normalize_ctl_controller0_T *)&ctl_controller0_P->Normalize); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge[0] = rtb_Product_b[0]; - rtb_Merge[1] = rtb_Product_b[1]; - rtb_Merge[2] = rtb_Product_b[2]; - rtb_Merge[3] = rtb_Product_b[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Sqrt = (real32_T)sqrt((real_T)(((rtb_Merge[0] * rtb_Merge[0] + rtb_Merge[1] - * rtb_Merge[1]) + rtb_Merge[2] * rtb_Merge[2]) + rtb_Merge[3] * rtb_Merge[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Sqrt > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - ctl_controller0_Normalize_e(rtb_Merge, rtb_Sqrt, rtb_Product_b); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product_b[3] = rtb_Merge[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/SumA21' incorporates: - // Delay: '/Delay11' - // Gain: '/a(2)(1)' - // Gain: '/s(1)' - - rtb_Sum4_o_idx_2 = (real32_T)(ctl_controller0_P->s1_Gain * (real_T) - rtb_Switch_est_V_B_ISS_ISS_idx_) - (real32_T)(ctl_controller0_P->a21_Gain * - (real_T)ctl_controller0_DW->Delay11_DSTATE[0]); - - // Sum: '/SumB21' incorporates: - // Delay: '/Delay11' - - rtb_Sum4_p = rtb_Sum4_o_idx_2 + ctl_controller0_DW->Delay11_DSTATE[0]; - - // Math: '/Math Function' - rtb_Sum4_p *= rtb_Sum4_p; - - // Sum: '/SumA21' - rtb_SumA21_idx_0 = rtb_Sum4_o_idx_2; - - // Sum: '/SumB21' - rtb_Sum4_o_idx_0 = rtb_Sum4_p; - - // Sum: '/SumA21' incorporates: - // Delay: '/Delay11' - // Gain: '/a(2)(1)' - // Gain: '/s(1)' - - rtb_Sum4_o_idx_2 = (real32_T)(ctl_controller0_P->s1_Gain * (real_T) - rtb_Switch_est_V_B_ISS_ISS_id_0) - (real32_T)(ctl_controller0_P->a21_Gain * - (real_T)ctl_controller0_DW->Delay11_DSTATE[1]); - - // Sum: '/SumB21' incorporates: - // Delay: '/Delay11' - - rtb_Sum4_p = rtb_Sum4_o_idx_2 + ctl_controller0_DW->Delay11_DSTATE[1]; - - // Math: '/Math Function' - rtb_Sum4_p *= rtb_Sum4_p; - - // Sum: '/SumA21' - rtb_SumA21_idx_1 = rtb_Sum4_o_idx_2; - - // Sum: '/SumB21' - rtb_Sum4_o_idx_1 = rtb_Sum4_p; - - // Sum: '/SumA21' incorporates: - // Delay: '/Delay11' - // Gain: '/a(2)(1)' - // Gain: '/s(1)' - - rtb_Sum4_o_idx_2 = (real32_T)(ctl_controller0_P->s1_Gain * (real_T) - rtb_Switch_est_V_B_ISS_ISS_id_1) - (real32_T)(ctl_controller0_P->a21_Gain * - (real_T)ctl_controller0_DW->Delay11_DSTATE[2]); - - // Sum: '/SumB21' incorporates: - // Delay: '/Delay11' - - rtb_Sum4_p = rtb_Sum4_o_idx_2 + ctl_controller0_DW->Delay11_DSTATE[2]; - - // Math: '/Math Function' - rtb_Sum4_p *= rtb_Sum4_p; - - // Sum: '/Sum of Elements' - rtb_SumofElements = (rtb_Sum4_o_idx_0 + rtb_Sum4_o_idx_1) + rtb_Sum4_p; - - // Sum: '/SumA21' incorporates: - // Delay: '/Delay11' - // Gain: '/a(2)(1)' - // Gain: '/s(1)' - - rtb_SumA21_f = (real32_T)(ctl_controller0_P->s1_Gain_b * (real_T) - rtb_Switch_est_omega_B_ISS_B_id) - (real32_T)(ctl_controller0_P->a21_Gain_l * - (real_T)ctl_controller0_DW->Delay11_DSTATE_i[0]); - - // Sum: '/SumB21' incorporates: - // Delay: '/Delay11' - - rtb_SumofElements1 = rtb_SumA21_f + ctl_controller0_DW->Delay11_DSTATE_i[0]; - - // Math: '/Math Function1' - rtb_SumofElements1 *= rtb_SumofElements1; - - // Sum: '/SumA21' - rtb_SumA21_d_idx_0 = rtb_SumA21_f; - - // Sum: '/SumB21' - rtb_Sum3_k_idx_0 = rtb_SumofElements1; - - // Sum: '/SumA21' incorporates: - // Delay: '/Delay11' - // Gain: '/a(2)(1)' - // Gain: '/s(1)' - - rtb_SumA21_f = (real32_T)(ctl_controller0_P->s1_Gain_b * (real_T) - rtb_Switch_est_omega_B_ISS_B__0) - (real32_T)(ctl_controller0_P->a21_Gain_l * - (real_T)ctl_controller0_DW->Delay11_DSTATE_i[1]); - - // Sum: '/SumB21' incorporates: - // Delay: '/Delay11' - - rtb_SumofElements1 = rtb_SumA21_f + ctl_controller0_DW->Delay11_DSTATE_i[1]; - - // Math: '/Math Function1' - rtb_SumofElements1 *= rtb_SumofElements1; - - // Sum: '/SumA21' - rtb_SumA21_d_idx_1 = rtb_SumA21_f; - - // Sum: '/SumB21' - rtb_Sum3_k_idx_1 = rtb_SumofElements1; - - // Sum: '/SumA21' incorporates: - // Delay: '/Delay11' - // Gain: '/a(2)(1)' - // Gain: '/s(1)' - - rtb_SumA21_f = (real32_T)(ctl_controller0_P->s1_Gain_b * (real_T) - rtb_Switch_est_omega_B_ISS_B__1) - (real32_T)(ctl_controller0_P->a21_Gain_l * - (real_T)ctl_controller0_DW->Delay11_DSTATE_i[2]); - - // Sum: '/SumB21' incorporates: - // Delay: '/Delay11' - - rtb_SumofElements1 = rtb_SumA21_f + ctl_controller0_DW->Delay11_DSTATE_i[2]; - - // Math: '/Math Function1' - rtb_SumofElements1 *= rtb_SumofElements1; - - // Sum: '/Sum of Elements1' - rtb_SumofElements1 += rtb_Sum3_k_idx_0 + rtb_Sum3_k_idx_1; - - // Logic: '/Logical Operator1' incorporates: - // Logic: '/Logical Operator' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - - rtb_LogicalOperator2_c = ((rtb_Switch_cmd_state_b_timestam > - rtb_Switch_current_time_sec) || ((rtb_Switch_current_time_sec == - rtb_Switch_cmd_state_b_timestam) && (rtb_Switch_current_time_nsec < - rtb_Switch_cmd_state_b_timest_0))); - - // Switch: '/Switch' - if (rtb_LogicalOperator2_c) { - rtb_Switch_cmd_state_b_timestam = rtb_Switch_cmd_state_a_timestam; - rtb_Switch_cmd_state_b_timest_0 = rtb_Switch_cmd_state_a_timest_0; - rtb_Gain1_j_idx_0 = normA; - rtb_Switch_c_idx_0 = b_s; - rtb_Diff[0] = rtb_Switch_cmd_state_a_A_B_ISS_; - rtb_Gain1_j_idx_1 = rtb_Switch_cmd_state_a_P_B_ISS_; - rtb_Switch_c_idx_1 = rtb_Switch_cmd_state_a_V_B_ISS_; - rtb_Diff[1] = rtb_Switch_cmd_state_a_A_B_IS_0; - rtb_Gain1_j_idx_2 = rtb_Switch_cmd_state_a_P_B_IS_0; - rtb_Switch_c_idx_2 = rtb_Sum3_k_idx_2; - rtb_Diff[2] = rtb_Switch_cmd_state_a_A_B_IS_1; - rtb_Merge_pa[0] = rtb_Product1_m[0]; - rtb_Merge_pa[1] = rtb_Product1_m[1]; - rtb_Merge_pa[2] = rtb_Product1_m[2]; - rtb_Merge_pa[3] = rtb_Product1_m[3]; - rtb_Divide_f[0] = rtb_Switch8_idx_0; - rtb_Switch_cmd_state_b_alpha__1 = rtb_Sum2_idx_0; - rtb_Divide_f[1] = rtb_Switch8_idx_1; - rtb_Switch_cmd_state_b_alpha__0 = rtb_Sum2_idx_1; - rtb_Divide_f[2] = rtb_Switch8_idx_2; - rtb_Switch_cmd_state_b_alpha_B_ = rtb_Sum2_idx_2; - } - - // End of Switch: '/Switch' - - // Logic: '/Logical Operator2' - rtb_LogicalOperator2_c = !rtb_LogicalOperator2_c; - - // Sum: '/Sum' incorporates: - // DataTypeConversion: '/Data Type Conversion4' - // Gain: '/Gain' - // Sum: '/Subtract3' - // Sum: '/Subtract4' - - rtb_Sqrt = (real32_T)(int32_T)((int32_T)rtb_Switch_current_time_nsec - - (int32_T)rtb_Switch_cmd_state_b_timest_0) * ctl_controller0_P->Gain_Gain_h + - (real32_T)(int32_T)((int32_T)rtb_Switch_current_time_sec - (int32_T) - rtb_Switch_cmd_state_b_timestam); - - // Sum: '/Sum1' incorporates: - // Product: '/Product1' - - rtb_Sum3_k_idx_0 = rtb_Diff[0] * rtb_Sqrt + rtb_Switch_c_idx_0; - - // Sum: '/Sum3' incorporates: - // Constant: '/Constant' - // Product: '/Product' - // Product: '/Product2' - - rtb_Gain1_j_idx_0 = (ctl_controller0_P->Constant_Value * rtb_Diff[0] * - rtb_Sqrt * rtb_Sqrt + rtb_Gain1_j_idx_0) + - rtb_Switch_c_idx_0 * rtb_Sqrt; - - // Sum: '/Sum1' incorporates: - // Product: '/Product1' - - rtb_Sum3_k_idx_1 = rtb_Diff[1] * rtb_Sqrt + rtb_Switch_c_idx_1; - - // Sum: '/Sum3' incorporates: - // Constant: '/Constant' - // Product: '/Product' - // Product: '/Product2' - - rtb_Gain1_j_idx_1 = (ctl_controller0_P->Constant_Value * rtb_Diff[1] * - rtb_Sqrt * rtb_Sqrt + rtb_Gain1_j_idx_1) + - rtb_Switch_c_idx_1 * rtb_Sqrt; - rtb_Switch_cmd_state_a_P_B_IS_0 = (ctl_controller0_P->Constant_Value * - rtb_Diff[2] * rtb_Sqrt * rtb_Sqrt + rtb_Gain1_j_idx_2) + rtb_Switch_c_idx_2 * - rtb_Sqrt; - - // Sum: '/Sum1' incorporates: - // Product: '/Product1' - - rtb_Sum3_k_idx_2 = rtb_Diff[2] * rtb_Sqrt + rtb_Switch_c_idx_2; - - // Constant: '/Constant3' - rtb_VectorConcatenate[0] = ctl_controller0_P->Constant3_Value_dz; - - // Gain: '/Gain' - rtb_VectorConcatenate[1] = ctl_controller0_P->Gain_Gain_n * - rtb_Switch_cmd_state_b_alpha_B_; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' - rtb_VectorConcatenate[2] = rtb_Switch_cmd_state_b_alpha__0; - - // Gain: '/Gain1' - rtb_VectorConcatenate[3] = ctl_controller0_P->Gain1_Gain_b * - rtb_Switch_cmd_state_b_alpha__1; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' - rtb_VectorConcatenate[4] = rtb_Switch_cmd_state_b_alpha_B_; - - // Constant: '/Constant2' - rtb_VectorConcatenate[5] = ctl_controller0_P->Constant2_Value_d; - - // Gain: '/Gain2' - rtb_VectorConcatenate[6] = ctl_controller0_P->Gain2_Gain_kx * - rtb_Switch_cmd_state_b_alpha__1; - - // Gain: '/Gain3' - rtb_VectorConcatenate[7] = ctl_controller0_P->Gain3_Gain * - rtb_Switch_cmd_state_b_alpha__0; - - // Gain: '/Gain4' - rtb_VectorConcatenate[8] = ctl_controller0_P->Gain4_Gain * - rtb_Switch_cmd_state_b_alpha__0; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' - rtb_VectorConcatenate[9] = rtb_Switch_cmd_state_b_alpha__1; - - // Constant: '/Constant1' - rtb_VectorConcatenate[10] = ctl_controller0_P->Constant1_Value_l; - - // Gain: '/Gain5' - rtb_VectorConcatenate[11] = ctl_controller0_P->Gain5_Gain * - rtb_Switch_cmd_state_b_alpha_B_; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' - rtb_VectorConcatenate[12] = rtb_Switch_cmd_state_b_alpha__1; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' - rtb_VectorConcatenate[13] = rtb_Switch_cmd_state_b_alpha__0; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' - rtb_VectorConcatenate[14] = rtb_Switch_cmd_state_b_alpha_B_; - - // Constant: '/Constant' - rtb_VectorConcatenate[15] = ctl_controller0_P->Constant_Value_g; - - // Constant: '/Constant3' - rtb_VectorConcatenate_m[0] = ctl_controller0_P->Constant3_Value_f; - - // Gain: '/Gain' - rtb_VectorConcatenate_m[1] = ctl_controller0_P->Gain_Gain_g * rtb_Divide_f[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' - rtb_VectorConcatenate_m[2] = rtb_Divide_f[1]; - - // Gain: '/Gain1' - rtb_VectorConcatenate_m[3] = ctl_controller0_P->Gain1_Gain_ja * rtb_Divide_f[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' - rtb_VectorConcatenate_m[4] = rtb_Divide_f[2]; - - // Constant: '/Constant2' - rtb_VectorConcatenate_m[5] = ctl_controller0_P->Constant2_Value_gq; - - // Gain: '/Gain2' - rtb_VectorConcatenate_m[6] = ctl_controller0_P->Gain2_Gain_c * rtb_Divide_f[0]; - - // Gain: '/Gain3' - rtb_VectorConcatenate_m[7] = ctl_controller0_P->Gain3_Gain_o * rtb_Divide_f[1]; - - // Gain: '/Gain4' - rtb_VectorConcatenate_m[8] = ctl_controller0_P->Gain4_Gain_c * rtb_Divide_f[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' - rtb_VectorConcatenate_m[9] = rtb_Divide_f[0]; - - // Constant: '/Constant1' - rtb_VectorConcatenate_m[10] = ctl_controller0_P->Constant1_Value_gk; - - // Gain: '/Gain5' - rtb_VectorConcatenate_m[11] = ctl_controller0_P->Gain5_Gain_a * rtb_Divide_f[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' - rtb_VectorConcatenate_m[12] = rtb_Divide_f[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' - rtb_VectorConcatenate_m[13] = rtb_Divide_f[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' - rtb_VectorConcatenate_m[14] = rtb_Divide_f[2]; - - // Constant: '/Constant' - rtb_VectorConcatenate_m[15] = ctl_controller0_P->Constant_Value_m; - - // Product: '/Product3' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Product: '/Product' - // Sum: '/Add' - - for (i = 0; i < 16; i++) { - rtb_Product5[i] = (ctl_controller0_P->Constant3_Value_d * - rtb_VectorConcatenate[i] * rtb_Sqrt + - rtb_VectorConcatenate_m[i]) * - ctl_controller0_P->Constant1_Value_g * rtb_Sqrt; - } - - // End of Product: '/Product3' - - // MATLAB Function: '/MATLAB Function' - // MATLAB Function 'cmd_command_shaper/generate_cmd_attitude/MATLAB Function': ':1' - // ':1:4' - normA = 0.0F; - i = 0; - exitg2 = false; - while ((!exitg2) && (i < 4)) { - b_s = (((real32_T)fabs((real_T)rtb_Product5[(int32_T)((int32_T)(i << 2) + 1)]) - + (real32_T)fabs((real_T)rtb_Product5[(int32_T)(i << 2)])) + - (real32_T)fabs((real_T)rtb_Product5[(int32_T)((int32_T)(i << 2) + 2)])) - + (real32_T)fabs((real_T)rtb_Product5[(int32_T)((int32_T)(i << 2) + 3)]); - if (rtIsNaNF(b_s)) { - normA = (rtNaNF); - exitg2 = true; - } else { - if (b_s > normA) { - normA = b_s; - } - - i++; - } - } - - if (normA <= 3.92572474F) { - eint = 0; - exitg1 = false; - while ((!exitg1) && (eint < 3)) { - if (normA <= theta[eint]) { - mglnkfkfmglfjekn_PadeApproximantOfDegree(rtb_Product5, (uint8_T)(int32_T) - ((int32_T)(eint << 1) + 3), rtb_y); - exitg1 = true; - } else { - eint++; - } - } - } else { - b_s = normA / 3.92572474F; - if ((!rtIsInfF(b_s)) && (!rtIsNaNF(b_s))) { - b_s = (real32_T)frexp((real_T)b_s, &eint); - normA = (real32_T)eint; - } else { - normA = 0.0F; - } - - if (b_s == 0.5F) { - normA--; - } - - b_s = rt_powf_snf(2.0F, normA); - for (i = 0; i < 16; i++) { - rtb_Product5_0[i] = rtb_Product5[i] / b_s; - } - - mglnkfkfmglfjekn_PadeApproximantOfDegree(rtb_Product5_0, 7U, rtb_y); - for (eint = 0; eint <= (int32_T)((int32_T)normA - 1); eint++) { - for (i = 0; i < 4; i++) { - for (i_0 = 0; i_0 < 4; i_0++) { - rtb_Product5[(int32_T)(i + (int32_T)(i_0 << 2))] = 0.0F; - rtb_Product5[(int32_T)(i + (int32_T)(i_0 << 2))] += rtb_y[(int32_T) - (i_0 << 2)] * rtb_y[i]; - rtb_Product5[(int32_T)(i + (int32_T)(i_0 << 2))] += rtb_y[(int32_T) - ((int32_T)(i_0 << 2) + 1)] * rtb_y[(int32_T)(i + 4)]; - rtb_Product5[(int32_T)(i + (int32_T)(i_0 << 2))] += rtb_y[(int32_T) - ((int32_T)(i_0 << 2) + 2)] * rtb_y[(int32_T)(i + 8)]; - rtb_Product5[(int32_T)(i + (int32_T)(i_0 << 2))] += rtb_y[(int32_T) - ((int32_T)(i_0 << 2) + 3)] * rtb_y[(int32_T)(i + 12)]; - } - } - - for (i = 0; i < 4; i++) { - rtb_y[(int32_T)(i << 2)] = rtb_Product5[(int32_T)(i << 2)]; - rtb_y[(int32_T)(1 + (int32_T)(i << 2))] = rtb_Product5[(int32_T) - ((int32_T)(i << 2) + 1)]; - rtb_y[(int32_T)(2 + (int32_T)(i << 2))] = rtb_Product5[(int32_T) - ((int32_T)(i << 2) + 2)]; - rtb_y[(int32_T)(3 + (int32_T)(i << 2))] = rtb_Product5[(int32_T) - ((int32_T)(i << 2) + 3)]; - } - } - } - - // End of MATLAB Function: '/MATLAB Function' - - // Product: '/Product6' - normA = rtb_Sqrt * rtb_Sqrt * rtb_Sqrt; - for (i = 0; i < 4; i++) { - for (i_0 = 0; i_0 < 4; i_0++) { - // Product: '/Product5' incorporates: - // Sum: '/Add2' - - rtb_Product5_0[(int32_T)(i_0 + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_p[(int32_T)(i_0 + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product5' incorporates: - // Sum: '/Add2' - - rtb_Product5_0[(int32_T)(i_0 + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)(i << 2)] * rtb_VectorConcatenate[i_0]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_p[(int32_T)(i_0 + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)(i << 2)] * rtb_VectorConcatenate_m[i_0]; - - // Product: '/Product5' incorporates: - // Sum: '/Add2' - - rtb_Product5_0[(int32_T)(i_0 + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate[(int32_T)(i_0 + 4)]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_p[(int32_T)(i_0 + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_m[(int32_T)(i_0 + 4)]; - - // Product: '/Product5' incorporates: - // Sum: '/Add2' - - rtb_Product5_0[(int32_T)(i_0 + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate[(int32_T)(i_0 + 8)]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_p[(int32_T)(i_0 + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_m[(int32_T)(i_0 + 8)]; - - // Product: '/Product5' incorporates: - // Sum: '/Add2' - - rtb_Product5_0[(int32_T)(i_0 + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate[(int32_T)(i_0 + 12)]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_p[(int32_T)(i_0 + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_m[(int32_T)(i_0 + 12)]; - } - } - - // Sum: '/Add1' incorporates: - // Constant: '/Constant2' - // Product: '/Product1' - // Product: '/Product6' - // Sum: '/Add2' - - for (i = 0; i < 4; i++) { - rtb_Product5[(int32_T)(i << 2)] = (rtb_Product5_0[(int32_T)(i << 2)] - - rtb_VectorConcatenate_p[(int32_T)(i << 2)]) * normA / - ctl_controller0_P->Constant2_Value_it + rtb_y[(int32_T)(i << 2)]; - rtb_Product5[(int32_T)(1 + (int32_T)(i << 2))] = (rtb_Product5_0[(int32_T) - ((int32_T)(i << 2) + 1)] - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << - 2) + 1)]) * normA / ctl_controller0_P->Constant2_Value_it + rtb_y[(int32_T) - ((int32_T)(i << 2) + 1)]; - rtb_Product5[(int32_T)(2 + (int32_T)(i << 2))] = (rtb_Product5_0[(int32_T) - ((int32_T)(i << 2) + 2)] - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << - 2) + 2)]) * normA / ctl_controller0_P->Constant2_Value_it + rtb_y[(int32_T) - ((int32_T)(i << 2) + 2)]; - rtb_Product5[(int32_T)(3 + (int32_T)(i << 2))] = (rtb_Product5_0[(int32_T) - ((int32_T)(i << 2) + 3)] - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << - 2) + 3)]) * normA / ctl_controller0_P->Constant2_Value_it + rtb_y[(int32_T) - ((int32_T)(i << 2) + 3)]; - } - - // End of Sum: '/Add1' - - // Product: '/Product1' - for (i = 0; i < 4; i++) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Product5[(int32_T)(i + 12)] * - rtb_Merge_pa[3] + (rtb_Product5[(int32_T)(i + 8)] * rtb_Merge_pa[2] + - (rtb_Product5[(int32_T)(i + 4)] * rtb_Merge_pa[1] + - rtb_Product5[i] * rtb_Merge_pa[0])); - rtb_Product1_m[i] = rtb_Switch_cmd_state_a_A_B_IS_1; - } - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product1_m[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - ctl_controller0_Normalize(rtb_Product1_m, rtb_Merge, - (P_Normalize_ctl_controller0_T *)&ctl_controller0_P->Normalize_eo); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge[0] = rtb_Product1_m[0]; - rtb_Merge[1] = rtb_Product1_m[1]; - rtb_Merge[2] = rtb_Product1_m[2]; - rtb_Merge[3] = rtb_Product1_m[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - normA = (real32_T)sqrt((real_T)(((rtb_Merge[0] * rtb_Merge[0] + rtb_Merge[1] * - rtb_Merge[1]) + rtb_Merge[2] * rtb_Merge[2]) + rtb_Merge[3] * rtb_Merge[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)normA > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - ctl_controller0_Normalize_e(rtb_Merge, normA, rtb_Merge_pa); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge_pa[0] = rtb_Merge[0]; - rtb_Merge_pa[1] = rtb_Merge[1]; - rtb_Merge_pa[2] = rtb_Merge[2]; - rtb_Merge_pa[3] = rtb_Merge[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Switch: '/Switch12' incorporates: - // Constant: '/Constant' - // Constant: '/Constant4' - // RelationalOperator: '/Compare' - - if (rtb_Switch12 != ctl_controller0_P->ase_status_converged) { - rtb_Switch12 = ctl_controller0_P->ctl_idle_mode; - } else { - rtb_Switch12 = rtb_Switch_ctl_mode_cmd; - } - - // End of Switch: '/Switch12' - - // Sum: '/Sum1' incorporates: - // Product: '/Product2' - - rtb_Sum4_p = rtb_Switch_cmd_state_b_alpha__1 * rtb_Sqrt + rtb_Divide_f[0]; - - // BusCreator: '/Bus Creator1' - normA = rtb_Gain1_j_idx_0; - b_s = rtb_Sum3_k_idx_0; - rtb_Switch_cmd_state_a_A_B_ISS_ = rtb_Sum4_p; - - // Sum: '/Sum1' incorporates: - // UnitDelay: '/Unit Delay1' - - rtb_Switch8_idx_0 = ctl_controller0_DW->UnitDelay1_DSTATE[0] - - rtb_Switch_est_P_B_ISS_ISS_idx_; - - // Math: '/Math Function2' - rtb_Switch8_idx_0 *= rtb_Switch8_idx_0; - - // Sum: '/Sum1' - rtb_Divide_f[0] = rtb_Switch8_idx_0; - - // Sum: '/Sum1' incorporates: - // Product: '/Product2' - - rtb_Sum4_o_idx_0 = rtb_Sum4_p; - rtb_Sum4_p = rtb_Switch_cmd_state_b_alpha__0 * rtb_Sqrt + rtb_Divide_f[1]; - - // BusCreator: '/Bus Creator1' - rtb_Switch_cmd_state_a_P_B_ISS_ = rtb_Gain1_j_idx_1; - rtb_Switch_cmd_state_a_V_B_ISS_ = rtb_Sum3_k_idx_1; - rtb_Switch_cmd_state_a_A_B_IS_0 = rtb_Sum4_p; - - // Sum: '/Sum1' incorporates: - // UnitDelay: '/Unit Delay1' - - rtb_Switch8_idx_0 = ctl_controller0_DW->UnitDelay1_DSTATE[1] - - rtb_Switch_est_P_B_ISS_ISS_id_0; - - // Math: '/Math Function2' - rtb_Switch8_idx_0 *= rtb_Switch8_idx_0; - - // Sum: '/Sum1' - rtb_Divide_f[1] = rtb_Switch8_idx_0; - - // Sum: '/Sum1' incorporates: - // Product: '/Product2' - - rtb_Sum4_o_idx_1 = rtb_Sum4_p; - rtb_Sum4_p = rtb_Switch_cmd_state_b_alpha_B_ * rtb_Sqrt + rtb_Divide_f[2]; - - // Sum: '/Sum1' incorporates: - // UnitDelay: '/Unit Delay1' - - rtb_Switch8_idx_0 = ctl_controller0_DW->UnitDelay1_DSTATE[2] - - rtb_Switch_est_P_B_ISS_ISS_id_1; - - // Math: '/Math Function2' - rtb_Switch8_idx_0 *= rtb_Switch8_idx_0; - - // Logic: '/Logical Operator2' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator4' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // S-Function (sfix_udelay): '/Tapped Delay' - - rtb_LogicalOperator2 = (ctl_controller0_DW->TappedDelay_X[0] && - ctl_controller0_DW->TappedDelay_X[1] && ctl_controller0_DW->TappedDelay_X[2] - && ctl_controller0_DW->TappedDelay_X[3] && ((rtb_SumofElements < - ctl_controller0_P->tun_ctl_stopping_vel_thresh) && (rtb_SumofElements1 < - ctl_controller0_P->tun_ctl_stopping_omega_thresh))); - - // Trigonometry: '/Trigonometric Function' - if (rtb_Product_b[3] > 1.0F) { - rtb_SumofElements = 1.0F; - } else if (rtb_Product_b[3] < -1.0F) { - rtb_SumofElements = -1.0F; - } else { - rtb_SumofElements = rtb_Product_b[3]; - } - - // Switch: '/Switch3' incorporates: - // Abs: '/Abs1' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant6' - // Constant: '/Constant' - // Gain: '/Gain2' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator3' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // Sum: '/Sum of Elements2' - // Switch: '/Switch2' - // Trigonometry: '/Trigonometric Function' - - if ((rtb_Switch12 == ctl_controller0_P->ctl_stopped_mode) && (((rtb_Divide_f[0] - + rtb_Divide_f[1]) + rtb_Switch8_idx_0 > - ctl_controller0_P->tun_ctl_stopped_pos_thresh) || ((real32_T)fabs - ((real_T)(ctl_controller0_P->Gain2_Gain_h * (real32_T)acos((real_T) - rtb_SumofElements))) > ctl_controller0_P->tun_ctl_stopped_quat_thresh))) - { - rtb_Switch3 = ctl_controller0_P->ctl_stopping_mode; - } else if (rtb_LogicalOperator2) { - // Switch: '/Switch2' incorporates: - // Constant: '/Constant' - - rtb_Switch3 = ctl_controller0_P->ctl_stopped_mode; - } else { - rtb_Switch3 = rtb_Switch12; - } - - // End of Switch: '/Switch3' - - // Switch: '/Switch8' incorporates: - // Switch: '/Switch9' - // UnitDelay: '/Unit Delay1' - // UnitDelay: '/Unit Delay2' - - if (rtb_LogicalOperator2) { - rtb_Switch8_idx_0 = ctl_controller0_DW->UnitDelay1_DSTATE[0]; - rtb_Switch8_idx_1 = ctl_controller0_DW->UnitDelay1_DSTATE[1]; - rtb_Switch8_idx_2 = ctl_controller0_DW->UnitDelay1_DSTATE[2]; - rtb_Product_b[0] = ctl_controller0_DW->UnitDelay2_DSTATE[0]; - rtb_Product_b[1] = ctl_controller0_DW->UnitDelay2_DSTATE[1]; - rtb_Product_b[2] = ctl_controller0_DW->UnitDelay2_DSTATE[2]; - rtb_Product_b[3] = ctl_controller0_DW->UnitDelay2_DSTATE[3]; - } else { - rtb_Switch8_idx_0 = rtb_Switch_est_P_B_ISS_ISS_idx_; - rtb_Switch8_idx_1 = rtb_Switch_est_P_B_ISS_ISS_id_0; - rtb_Switch8_idx_2 = rtb_Switch_est_P_B_ISS_ISS_id_1; - rtb_Product_b[0] = rtb_Merge_j[0]; - rtb_Product_b[1] = rtb_Merge_j[1]; - rtb_Product_b[2] = rtb_Merge_j[2]; - rtb_Product_b[3] = rtb_Merge_j[3]; - } - - // End of Switch: '/Switch8' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_SumofElements = rtb_Merge_j[3] * rtb_Merge_j[3] * - ctl_controller0_P->Gain_Gain_l - (real32_T) - ctl_controller0_P->Constant1_Value; - for (i = 0; i < 9; i++) { - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment_h[i] = (real32_T)ctl_controller0_P->Constant2_Value_pu[i]; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment_l[i] = (real32_T)ctl_controller0_P->Constant2_Value_k[i]; - } - - // Assignment: '/Assignment' - rtb_Assignment_h[0] = rtb_SumofElements; - rtb_Assignment_h[4] = rtb_SumofElements; - rtb_Assignment_h[8] = rtb_SumofElements; - - // Gain: '/Gain1' - rtb_SumofElements = ctl_controller0_P->Gain1_Gain_ej * rtb_Merge_j[3]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_SumofElements1 = rtb_Merge_j[3] * rtb_Merge_j[3] * - ctl_controller0_P->Gain_Gain_b - (real32_T) - ctl_controller0_P->Constant1_Value_f; - - // Assignment: '/Assignment' - rtb_Assignment_l[0] = rtb_SumofElements1; - rtb_Assignment_l[4] = rtb_SumofElements1; - rtb_Assignment_l[8] = rtb_SumofElements1; - - // Gain: '/Gain1' - rtb_SumofElements1 = ctl_controller0_P->Gain1_Gain_g * rtb_Merge_j[3]; - - // Switch: '/Switch2' incorporates: - // BusCreator: '/Bus Creator1' - // Constant: '/Constant4' - // Constant: '/Constant' - // RelationalOperator: '/Compare' - // Switch: '/Switch10' - - if (rtb_Switch3 <= ctl_controller0_P->CompareToConstant2_const) { - rtb_Switch_c_idx_0 = ctl_controller0_P->Constant4_Value[0]; - rtb_Switch_c_idx_1 = ctl_controller0_P->Constant4_Value[1]; - rtb_Switch_c_idx_2 = ctl_controller0_P->Constant4_Value[2]; - } else if (rtb_LogicalOperator2) { - // Switch: '/Switch10' incorporates: - // Constant: '/Constant2' - - rtb_Switch_c_idx_0 = ctl_controller0_P->Constant2_Value_n[0]; - rtb_Switch_c_idx_1 = ctl_controller0_P->Constant2_Value_n[1]; - rtb_Switch_c_idx_2 = ctl_controller0_P->Constant2_Value_n[2]; - } else { - rtb_Switch_c_idx_0 = rtb_Sum3_k_idx_0; - rtb_Switch_c_idx_1 = rtb_Sum3_k_idx_1; - rtb_Switch_c_idx_2 = rtb_Sum3_k_idx_2; - } - - // End of Switch: '/Switch2' - - // Switch: '/Switch6' incorporates: - // BusCreator: '/Bus Creator1' - - if (rtb_LogicalOperator2) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch8_idx_0; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Gain1_j_idx_0; - } - - // Sum: '/Sum2' - rtb_Sum2_idx_0 = rtb_Switch_cmd_state_a_A_B_IS_1 - - rtb_Switch_est_P_B_ISS_ISS_idx_; - - // Switch: '/Switch6' incorporates: - // BusCreator: '/Bus Creator1' - - if (rtb_LogicalOperator2) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch8_idx_1; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Gain1_j_idx_1; - } - - // Sum: '/Sum2' - rtb_Sum2_idx_1 = rtb_Switch_cmd_state_a_A_B_IS_1 - - rtb_Switch_est_P_B_ISS_ISS_id_0; - - // Switch: '/Switch6' - if (rtb_LogicalOperator2) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch8_idx_2; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch_cmd_state_a_P_B_IS_0; - } - - // Sum: '/Sum2' - rtb_Sum2_idx_2 = rtb_Switch_cmd_state_a_A_B_IS_1 - - rtb_Switch_est_P_B_ISS_ISS_id_1; - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare_j = (rtb_Switch3 <= ctl_controller0_P->CompareToConstant_const); - - // DiscreteIntegrator: '/Discrete-Time Integrator1' - if (rtb_Compare_j || ((int32_T) - ctl_controller0_DW->DiscreteTimeIntegrator1_PrevRes != 0)) - { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] = - ctl_controller0_P->DiscreteTimeIntegrator1_IC[0]; - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] = - ctl_controller0_P->DiscreteTimeIntegrator1_IC[1]; - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] = - ctl_controller0_P->DiscreteTimeIntegrator1_IC[2]; - } - - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] >= - ctl_controller0_P->tun_ctl_pos_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] = - ctl_controller0_P->tun_ctl_pos_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] <= - ctl_controller0_P->tun_ctl_pos_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] = - ctl_controller0_P->tun_ctl_pos_sat_lower; - } - } - - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] >= - ctl_controller0_P->tun_ctl_pos_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] = - ctl_controller0_P->tun_ctl_pos_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] <= - ctl_controller0_P->tun_ctl_pos_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] = - ctl_controller0_P->tun_ctl_pos_sat_lower; - } - } - - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] >= - ctl_controller0_P->tun_ctl_pos_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] = - ctl_controller0_P->tun_ctl_pos_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] <= - ctl_controller0_P->tun_ctl_pos_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] = - ctl_controller0_P->tun_ctl_pos_sat_lower; - } - } - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Product: '/Product' - // RelationalOperator: '/Compare' - // Sum: '/Sum4' - // Switch: '/Switch' - - if (rtb_Switch3 <= ctl_controller0_P->CompareToConstant1_const) { - rtb_Divide_f[0] = ctl_controller0_P->Constant1_Value_k[0]; - rtb_Divide_f[1] = ctl_controller0_P->Constant1_Value_k[1]; - rtb_Divide_f[2] = ctl_controller0_P->Constant1_Value_k[2]; - } else { - if (rtb_Switch_vel_kd[0] != 0.0F) { - // Switch: '/Switch' incorporates: - // Product: '/Divide1' - - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Sum_f[0] / rtb_Switch_vel_kd[0]; - } else { - // Switch: '/Switch' - rtb_Switch_cmd_state_a_A_B_IS_1 = 0.0F; - } - - rtb_Divide_f[0] = rtb_Switch_cmd_state_a_A_B_IS_1 * rtb_Sum2_idx_0 + - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0]; - - // Switch: '/Switch' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Product: '/Product' - // Product: '/Divide1' - // Sum: '/Sum4' - - if (rtb_Switch_vel_kd[1] != 0.0F) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Sum_f[1] / rtb_Switch_vel_kd[1]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = 0.0F; - } - - rtb_Divide_f[1] = rtb_Switch_cmd_state_a_A_B_IS_1 * rtb_Sum2_idx_1 + - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1]; - - // Switch: '/Switch' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Product: '/Product' - // Product: '/Divide1' - // Sum: '/Sum4' - - if (rtb_Switch_vel_kd[2] != 0.0F) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Sum_f[2] / rtb_Switch_vel_kd[2]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = 0.0F; - } - - rtb_Divide_f[2] = rtb_Switch_cmd_state_a_A_B_IS_1 * rtb_Sum2_idx_2 + - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2]; - } - - // End of Switch: '/Switch1' - - // Sum: '/Sum1' - rtb_Switch_c_idx_0 = (rtb_Switch_c_idx_0 + rtb_Divide_f[0]) - - rtb_Switch_est_V_B_ISS_ISS_idx_; - rtb_Switch_c_idx_1 = (rtb_Switch_c_idx_1 + rtb_Divide_f[1]) - - rtb_Switch_est_V_B_ISS_ISS_id_0; - rtb_Switch_c_idx_2 = (rtb_Switch_c_idx_2 + rtb_Divide_f[2]) - - rtb_Switch_est_V_B_ISS_ISS_id_1; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_0[0] = (real32_T)ctl_controller0_P->Constant3_Value_o; - tmp_0[1] = rtb_Merge_j[2]; - tmp_0[2] = ctl_controller0_P->Gain_Gain_ib * rtb_Merge_j[1]; - tmp_0[3] = ctl_controller0_P->Gain1_Gain_k * rtb_Merge_j[2]; - tmp_0[4] = (real32_T)ctl_controller0_P->Constant3_Value_o; - tmp_0[5] = rtb_Merge_j[0]; - tmp_0[6] = rtb_Merge_j[1]; - tmp_0[7] = ctl_controller0_P->Gain2_Gain_b * rtb_Merge_j[0]; - tmp_0[8] = (real32_T)ctl_controller0_P->Constant3_Value_o; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - - for (i = 0; i < 3; i++) { - rtb_Merge_j_0[i] = rtb_Merge_j[i] * rtb_Merge_j[0]; - rtb_Merge_j_0[(int32_T)(i + 3)] = rtb_Merge_j[i] * rtb_Merge_j[1]; - rtb_Merge_j_0[(int32_T)(i + 6)] = rtb_Merge_j[i] * rtb_Merge_j[2]; - } - - // End of Product: '/Product1' - for (i = 0; i < 3; i++) { - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - rtb_Assignment_b_0[(int32_T)(3 * i)] = (rtb_Assignment_h[(int32_T)(3 * i)] - - tmp_0[(int32_T)(3 * i)] * rtb_SumofElements) + rtb_Merge_j_0[(int32_T)(3 * - i)] * ctl_controller0_P->Gain2_Gain_k1; - rtb_Assignment_b_0[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_h - [(int32_T)((int32_T)(3 * i) + 1)] - tmp_0[(int32_T)((int32_T)(3 * i) + 1)] - * rtb_SumofElements) + rtb_Merge_j_0[(int32_T)((int32_T)(3 * i) + 1)] * - ctl_controller0_P->Gain2_Gain_k1; - rtb_Assignment_b_0[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_h - [(int32_T)((int32_T)(3 * i) + 2)] - tmp_0[(int32_T)((int32_T)(3 * i) + 2)] - * rtb_SumofElements) + rtb_Merge_j_0[(int32_T)((int32_T)(3 * i) + 2)] * - ctl_controller0_P->Gain2_Gain_k1; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant5' - // Product: '/Product' - // Switch: '/Switch14' - - if (rtb_LogicalOperator2) { - rtb_Switch_cmd_state_a_A_B_IS_1 = ctl_controller0_P->Constant5_Value[i]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Diff[i]; - } - - rtb_Sum_f[i] = ctl_controller0_P->tun_accel_gain[i] * - rtb_Switch_cmd_state_a_A_B_IS_1; - - // End of Gain: '/Gain' - } - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_1[0] = (real32_T)ctl_controller0_P->Constant3_Value_b; - tmp_1[1] = rtb_Merge_j[2]; - tmp_1[2] = ctl_controller0_P->Gain_Gain_d * rtb_Merge_j[1]; - tmp_1[3] = ctl_controller0_P->Gain1_Gain_ji * rtb_Merge_j[2]; - tmp_1[4] = (real32_T)ctl_controller0_P->Constant3_Value_b; - tmp_1[5] = rtb_Merge_j[0]; - tmp_1[6] = rtb_Merge_j[1]; - tmp_1[7] = ctl_controller0_P->Gain2_Gain_bs * rtb_Merge_j[0]; - tmp_1[8] = (real32_T)ctl_controller0_P->Constant3_Value_b; - for (i = 0; i < 3; i++) { - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - - rtb_Merge_j_0[i] = rtb_Merge_j[i] * rtb_Merge_j[0]; - rtb_Merge_j_0[(int32_T)(i + 3)] = rtb_Merge_j[i] * rtb_Merge_j[1]; - rtb_Merge_j_0[(int32_T)(i + 6)] = rtb_Merge_j[i] * rtb_Merge_j[2]; - - // Product: '/Product' incorporates: - // Product: '/Divide3' - - rtb_Assignment_b_1[i] = rtb_Assignment_b_0[(int32_T)(i + 6)] * rtb_Sum_f[2] - + (rtb_Assignment_b_0[(int32_T)(i + 3)] * rtb_Sum_f[1] + - rtb_Assignment_b_0[i] * rtb_Sum_f[0]); - } - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_h[(int32_T)(3 * i)] = (rtb_Assignment_l[(int32_T)(3 * i)] - - tmp_1[(int32_T)(3 * i)] * rtb_SumofElements1) + rtb_Merge_j_0[(int32_T)(3 * - i)] * ctl_controller0_P->Gain2_Gain_kh; - rtb_Assignment_h[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_l - [(int32_T)((int32_T)(3 * i) + 1)] - tmp_1[(int32_T)((int32_T)(3 * i) + 1)] - * rtb_SumofElements1) + rtb_Merge_j_0[(int32_T)((int32_T)(3 * i) + 1)] * - ctl_controller0_P->Gain2_Gain_kh; - rtb_Assignment_h[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_l - [(int32_T)((int32_T)(3 * i) + 2)] - tmp_1[(int32_T)((int32_T)(3 * i) + 2)] - * rtb_SumofElements1) + rtb_Merge_j_0[(int32_T)((int32_T)(3 * i) + 2)] * - ctl_controller0_P->Gain2_Gain_kh; - } - - // End of Sum: '/Sum1' - - // DotProduct: '/Dot Product' - rtb_SumofElements = 0.0F; - for (i = 0; i < 3; i++) { - // Sum: '/Sum' incorporates: - // Product: '/Divide3' - // Product: '/Product2' - // Product: '/Product3' - // Product: '/Product' - - rtb_SumofElements1 = rtb_Switch_vel_kd[i] * rtb_Switch_mass * - (rtb_Assignment_h[(int32_T)(i + 6)] * rtb_Switch_c_idx_2 + - (rtb_Assignment_h[(int32_T)(i + 3)] * rtb_Switch_c_idx_1 + - rtb_Assignment_h[i] * rtb_Switch_c_idx_0)) + rtb_Switch_mass * - rtb_Assignment_b_1[i]; - - // DotProduct: '/Dot Product' - rtb_SumofElements += rtb_SumofElements1 * rtb_SumofElements1; - - // Sum: '/Sum' incorporates: - // Product: '/Divide3' - - rtb_Sum_f[i] = rtb_SumofElements1; - } - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_SumofElements = (real32_T)sqrt((real_T)rtb_SumofElements); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_SumofElements > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - // Product: '/Divide' - rtb_Switch_c_idx_0 = rtb_Sum_f[0] / rtb_SumofElements; - rtb_Switch_c_idx_1 = rtb_Sum_f[1] / rtb_SumofElements; - rtb_Switch_c_idx_2 = rtb_Sum_f[2] / rtb_SumofElements; - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Switch_c_idx_0 = rtb_Sum_f[0]; - rtb_Switch_c_idx_1 = rtb_Sum_f[1]; - rtb_Switch_c_idx_2 = rtb_Sum_f[2]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant3' - - if ((int32_T)rtb_Switch3 != 0) { - // Switch: '/Switch' incorporates: - // Constant: '/Constant2' - // DotProduct: '/Dot Product' - // Product: '/Product' - // RelationalOperator: '/Relational Operator' - // Sqrt: '/Sqrt' - - if (!((real32_T)sqrt((real_T)((rtb_Sum_f[0] * rtb_Sum_f[0] + rtb_Sum_f[1] * - rtb_Sum_f[1]) + rtb_Sum_f[2] * rtb_Sum_f[2])) < - ctl_controller0_P->tun_ctl_linear_force_limit)) { - rtb_Sum_f[0] = ctl_controller0_P->tun_ctl_linear_force_limit * - rtb_Switch_c_idx_0; - rtb_Sum_f[1] = ctl_controller0_P->tun_ctl_linear_force_limit * - rtb_Switch_c_idx_1; - rtb_Sum_f[2] = ctl_controller0_P->tun_ctl_linear_force_limit * - rtb_Switch_c_idx_2; - } - - // End of Switch: '/Switch' - rtb_Switch_c_idx_0 = rtb_Sum_f[0]; - rtb_Switch_c_idx_1 = rtb_Sum_f[1]; - rtb_Switch_c_idx_2 = rtb_Sum_f[2]; - } else { - rtb_Switch_c_idx_0 = ctl_controller0_P->Constant3_Value_e[0]; - rtb_Switch_c_idx_1 = ctl_controller0_P->Constant3_Value_e[1]; - rtb_Switch_c_idx_2 = ctl_controller0_P->Constant3_Value_e[2]; - } - - // End of Switch: '/Switch' - - // Switch: '/Switch3' incorporates: - // BusCreator: '/Bus Creator1' - // Constant: '/Constant2' - // Constant: '/Constant' - // RelationalOperator: '/Compare' - // Switch: '/Switch11' - - if (rtb_Switch3 <= ctl_controller0_P->CompareToConstant2_const_o) { - rtb_Divide_f[0] = ctl_controller0_P->Constant2_Value_o[0]; - rtb_Divide_f[1] = ctl_controller0_P->Constant2_Value_o[1]; - rtb_Divide_f[2] = ctl_controller0_P->Constant2_Value_o[2]; - } else if (rtb_LogicalOperator2) { - // Switch: '/Switch11' incorporates: - // Constant: '/Constant3' - - rtb_Divide_f[0] = ctl_controller0_P->Constant3_Value_j[0]; - rtb_Divide_f[1] = ctl_controller0_P->Constant3_Value_j[1]; - rtb_Divide_f[2] = ctl_controller0_P->Constant3_Value_j[2]; - } else { - rtb_Divide_f[0] = rtb_Sum4_o_idx_0; - rtb_Divide_f[1] = rtb_Sum4_o_idx_1; - rtb_Divide_f[2] = rtb_Sum4_p; - } - - // End of Switch: '/Switch3' - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Assignment_h[i] = (real32_T)ctl_controller0_P->Constant2_Value_g[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' - rtb_Assignment_h[0] = rtb_Merge_j[3]; - - // Gain: '/Gain' - rtb_Sum_f[0] = ctl_controller0_P->Gain_Gain_pf * rtb_Merge_j[0]; - - // Assignment: '/Assignment' - rtb_Assignment_h[4] = rtb_Merge_j[3]; - - // Gain: '/Gain' - rtb_Sum_f[1] = ctl_controller0_P->Gain_Gain_pf * rtb_Merge_j[1]; - - // Assignment: '/Assignment' - rtb_Assignment_h[8] = rtb_Merge_j[3]; - - // Gain: '/Gain' - rtb_Sum_f[2] = ctl_controller0_P->Gain_Gain_pf * rtb_Merge_j[2]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_2[0] = (real32_T)ctl_controller0_P->Constant3_Value_a5; - tmp_2[1] = rtb_Sum_f[2]; - tmp_2[2] = ctl_controller0_P->Gain_Gain_k * rtb_Sum_f[1]; - tmp_2[3] = ctl_controller0_P->Gain1_Gain_jz * rtb_Sum_f[2]; - tmp_2[4] = (real32_T)ctl_controller0_P->Constant3_Value_a5; - tmp_2[5] = rtb_Sum_f[0]; - tmp_2[6] = rtb_Sum_f[1]; - tmp_2[7] = ctl_controller0_P->Gain2_Gain_n * rtb_Sum_f[0]; - tmp_2[8] = (real32_T)ctl_controller0_P->Constant3_Value_a5; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i = 0; i < 3; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = rtb_Assignment_h[(int32_T)(3 * i)] - + tmp_2[(int32_T)(3 * i)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = rtb_Assignment_h - [(int32_T)((int32_T)(3 * i) + 1)] + tmp_2[(int32_T)((int32_T)(3 * i) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = rtb_Assignment_h - [(int32_T)((int32_T)(3 * i) + 2)] + tmp_2[(int32_T)((int32_T)(3 * i) + 2)]; - } - - rtb_VectorConcatenate[3] = ctl_controller0_P->Gain1_Gain_od * rtb_Sum_f[0]; - rtb_VectorConcatenate[7] = ctl_controller0_P->Gain1_Gain_od * rtb_Sum_f[1]; - rtb_VectorConcatenate[11] = ctl_controller0_P->Gain1_Gain_od * rtb_Sum_f[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Reshape: '/Reshape1' - rtb_VectorConcatenate[12] = rtb_Sum_f[0]; - rtb_VectorConcatenate[13] = rtb_Sum_f[1]; - rtb_VectorConcatenate[14] = rtb_Sum_f[2]; - rtb_VectorConcatenate[15] = rtb_Merge_j[3]; - - // Product: '/Product' incorporates: - // BusCreator: '/Bus Creator1' - // Switch: '/Switch7' - - if (rtb_LogicalOperator2) { - rtb_SumofElements = rtb_Product_b[0]; - rtb_SumofElements1 = rtb_Product_b[1]; - rtb_Sqrt = rtb_Product_b[2]; - rtb_Gain1_j_idx_2 = rtb_Product_b[3]; - } else { - rtb_SumofElements = rtb_Merge_pa[0]; - rtb_SumofElements1 = rtb_Merge_pa[1]; - rtb_Sqrt = rtb_Merge_pa[2]; - rtb_Gain1_j_idx_2 = rtb_Merge_pa[3]; - } - - for (i = 0; i < 4; i++) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_VectorConcatenate[(int32_T)(i + 12)] * - rtb_Gain1_j_idx_2 + (rtb_VectorConcatenate[(int32_T)(i + 8)] * rtb_Sqrt + - (rtb_VectorConcatenate[(int32_T)(i + 4)] * rtb_SumofElements1 + - rtb_VectorConcatenate[i] * rtb_SumofElements)); - rtb_Product1_m[i] = rtb_Switch_cmd_state_a_A_B_IS_1; - } - - // End of Product: '/Product' - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product1_m[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - ctl_controller0_Normalize(rtb_Product1_m, rtb_Merge, - (P_Normalize_ctl_controller0_T *)&ctl_controller0_P->Normalize_l); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge[0] = rtb_Product1_m[0]; - rtb_Merge[1] = rtb_Product1_m[1]; - rtb_Merge[2] = rtb_Product1_m[2]; - rtb_Merge[3] = rtb_Product1_m[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_SumofElements = (real32_T)sqrt((real_T)(((rtb_Merge[0] * rtb_Merge[0] + - rtb_Merge[1] * rtb_Merge[1]) + rtb_Merge[2] * rtb_Merge[2]) + rtb_Merge[3] * - rtb_Merge[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_SumofElements > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - ctl_controller0_Normalize_e(rtb_Merge, rtb_SumofElements, rtb_Product1_m); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product1_m[0] = rtb_Merge[0]; - rtb_Product1_m[1] = rtb_Merge[1]; - rtb_Product1_m[2] = rtb_Merge[2]; - rtb_Product1_m[3] = rtb_Merge[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare_fc = (rtb_Switch3 <= ctl_controller0_P->CompareToConstant_const_f); - - // DiscreteIntegrator: '/Discrete-Time Integrator' - if (rtb_Compare_fc || ((int32_T) - ctl_controller0_DW->DiscreteTimeIntegrator_PrevRese != - 0)) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] = - ctl_controller0_P->DiscreteTimeIntegrator_IC[0]; - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] = - ctl_controller0_P->DiscreteTimeIntegrator_IC[1]; - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] = - ctl_controller0_P->DiscreteTimeIntegrator_IC[2]; - } - - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] >= - ctl_controller0_P->tun_ctl_att_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] = - ctl_controller0_P->tun_ctl_att_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] <= - ctl_controller0_P->tun_ctl_att_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] = - ctl_controller0_P->tun_ctl_att_sat_lower; - } - } - - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] >= - ctl_controller0_P->tun_ctl_att_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] = - ctl_controller0_P->tun_ctl_att_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] <= - ctl_controller0_P->tun_ctl_att_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] = - ctl_controller0_P->tun_ctl_att_sat_lower; - } - } - - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] >= - ctl_controller0_P->tun_ctl_att_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] = - ctl_controller0_P->tun_ctl_att_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] <= - ctl_controller0_P->tun_ctl_att_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] = - ctl_controller0_P->tun_ctl_att_sat_lower; - } - } - - // Switch: '/Switch2' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Product: '/Product' - // RelationalOperator: '/Compare' - // Sum: '/Sum3' - // Switch: '/Switch' - - if (rtb_Switch3 <= ctl_controller0_P->CompareToConstant1_const_m) { - rtb_Switch_b[0] = ctl_controller0_P->Constant1_Value_o[0]; - rtb_Switch_b[1] = ctl_controller0_P->Constant1_Value_o[1]; - rtb_Switch_b[2] = ctl_controller0_P->Constant1_Value_o[2]; - } else { - if (rtb_Switch_omega_kd_idx_0 != 0.0F) { - // Sum: '/Sum3' incorporates: - // Product: '/Product' - // Product: '/Divide1' - // Switch: '/Switch' - - rtb_SumofElements = rtb_Switch_b[0] / rtb_Switch_omega_kd_idx_0; - } else { - // Sum: '/Sum3' incorporates: - // Switch: '/Switch' - - rtb_SumofElements = 0.0F; - } - - rtb_Switch_b[0] = rtb_Product1_m[0] * rtb_SumofElements + - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0]; - - // Sum: '/Sum3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Product: '/Product' - // Product: '/Divide1' - // Switch: '/Switch' - - if (rtb_Switch_omega_kd_idx_1 != 0.0F) { - rtb_SumofElements = rtb_Switch_b[1] / rtb_Switch_omega_kd_idx_1; - } else { - rtb_SumofElements = 0.0F; - } - - rtb_Switch_b[1] = rtb_Product1_m[1] * rtb_SumofElements + - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1]; - - // Sum: '/Sum3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Product: '/Product' - // Product: '/Divide1' - // Switch: '/Switch' - - if (rtb_Switch_omega_kd_idx_2 != 0.0F) { - rtb_SumofElements = rtb_Switch_b[2] / rtb_Switch_omega_kd_idx_2; - } else { - rtb_SumofElements = 0.0F; - } - - rtb_Switch_b[2] = rtb_Product1_m[2] * rtb_SumofElements + - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2]; - } - - // End of Switch: '/Switch2' - - // Product: '/Product3' incorporates: - // Product: '/Product4' - // Sum: '/Sum' - - rtb_Sum_f[0] = ((rtb_Divide_f[0] + rtb_Switch_b[0]) - - rtb_Switch_est_omega_B_ISS_B_id) * (rtb_Switch_omega_kd_idx_0 * - rtb_Switch_inertia_matrix[0]); - rtb_Sum_f[1] = ((rtb_Divide_f[1] + rtb_Switch_b[1]) - - rtb_Switch_est_omega_B_ISS_B__0) * (rtb_Switch_omega_kd_idx_1 * - rtb_Switch_inertia_matrix[4]); - rtb_Sum_f[2] = ((rtb_Divide_f[2] + rtb_Switch_b[2]) - - rtb_Switch_est_omega_B_ISS_B__1) * (rtb_Switch_omega_kd_idx_2 * - rtb_Switch_inertia_matrix[8]); - - // Product: '/Product2' - for (i = 0; i < 3; i++) { - rtb_Divide_f[i] = rtb_Switch_inertia_matrix[(int32_T)(i + 6)] * - rtb_Switch_est_omega_B_ISS_B__1 + (rtb_Switch_inertia_matrix[(int32_T)(i + - 3)] * rtb_Switch_est_omega_B_ISS_B__0 + rtb_Switch_inertia_matrix[i] * - rtb_Switch_est_omega_B_ISS_B_id); - } - - // End of Product: '/Product2' - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant4' - - if ((int32_T)rtb_Switch3 != 0) { - // Product: '/Divide3' incorporates: - // Constant: '/Constant1' - // Gain: '/Gain' - // Switch: '/Switch1' - - if (rtb_LogicalOperator2) { - rtb_Switch_cmd_state_a_A_B_IS_1 = ctl_controller0_P->Constant1_Value_a[0]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch_cmd_state_b_alpha__1; - } - - rtb_SumofElements = ctl_controller0_P->tun_alpha_gain[0] * - rtb_Switch_cmd_state_a_A_B_IS_1; - if (rtb_LogicalOperator2) { - rtb_Switch_cmd_state_a_A_B_IS_1 = ctl_controller0_P->Constant1_Value_a[1]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch_cmd_state_b_alpha__0; - } - - rtb_SumofElements1 = ctl_controller0_P->tun_alpha_gain[1] * - rtb_Switch_cmd_state_a_A_B_IS_1; - if (rtb_LogicalOperator2) { - rtb_Switch_cmd_state_a_A_B_IS_1 = ctl_controller0_P->Constant1_Value_a[2]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch_cmd_state_b_alpha_B_; - } - - rtb_Switch_cmd_state_a_A_B_IS_1 *= ctl_controller0_P->tun_alpha_gain[2]; - - // SignalConversion: '/TmpSignal ConversionAtProductInport1' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Product: '/Product' - - tmp_3[0] = (real32_T)ctl_controller0_P->Constant3_Value_c; - tmp_3[1] = rtb_Divide_f[2]; - tmp_3[2] = ctl_controller0_P->Gain_Gain * rtb_Divide_f[1]; - tmp_3[3] = ctl_controller0_P->Gain1_Gain * rtb_Divide_f[2]; - tmp_3[4] = (real32_T)ctl_controller0_P->Constant3_Value_c; - tmp_3[5] = rtb_Divide_f[0]; - tmp_3[6] = rtb_Divide_f[1]; - tmp_3[7] = ctl_controller0_P->Gain2_Gain * rtb_Divide_f[0]; - tmp_3[8] = (real32_T)ctl_controller0_P->Constant3_Value_c; - - // Product: '/Product' incorporates: - // Product: '/Divide3' - // Sum: '/Sum1' - - for (i = 0; i < 3; i++) { - rtb_Switch_b[i] = (((rtb_Switch_inertia_matrix[(int32_T)(i + 3)] * - rtb_SumofElements1 + rtb_Switch_inertia_matrix[i] * - rtb_SumofElements) + rtb_Switch_inertia_matrix - [(int32_T)(i + 6)] * rtb_Switch_cmd_state_a_A_B_IS_1) - + rtb_Sum_f[i]) - (tmp_3[(int32_T)(i + 6)] * - rtb_Switch_est_omega_B_ISS_B__1 + (tmp_3[(int32_T)(i + 3)] * - rtb_Switch_est_omega_B_ISS_B__0 + tmp_3[i] * - rtb_Switch_est_omega_B_ISS_B_id)); - } - } else { - rtb_Switch_b[0] = ctl_controller0_P->Constant4_Value_f[0]; - rtb_Switch_b[1] = ctl_controller0_P->Constant4_Value_f[1]; - rtb_Switch_b[2] = ctl_controller0_P->Constant4_Value_f[2]; - } - - // End of Switch: '/Switch1' - - // Sum: '/Sum1' - rtb_Gain1_j_idx_0 -= rtb_Switch_est_P_B_ISS_ISS_idx_; - rtb_Gain1_j_idx_1 -= rtb_Switch_est_P_B_ISS_ISS_id_0; - rtb_Switch_est_P_B_ISS_ISS_idx_ = rtb_Switch_cmd_state_a_P_B_IS_0 - - rtb_Switch_est_P_B_ISS_ISS_id_1; - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Assignment_h[i] = (real32_T)ctl_controller0_P->Constant2_Value_i[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' - rtb_Assignment_h[0] = rtb_Merge_j[3]; - - // Gain: '/Gain' - rtb_Divide_f[0] = ctl_controller0_P->Gain_Gain_is * rtb_Merge_j[0]; - - // Assignment: '/Assignment' - rtb_Assignment_h[4] = rtb_Merge_j[3]; - - // Gain: '/Gain' - rtb_Divide_f[1] = ctl_controller0_P->Gain_Gain_is * rtb_Merge_j[1]; - - // Assignment: '/Assignment' - rtb_Assignment_h[8] = rtb_Merge_j[3]; - - // Gain: '/Gain' - rtb_Divide_f[2] = ctl_controller0_P->Gain_Gain_is * rtb_Merge_j[2]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_4[0] = (real32_T)ctl_controller0_P->Constant3_Value_n; - tmp_4[1] = rtb_Divide_f[2]; - tmp_4[2] = ctl_controller0_P->Gain_Gain_m2 * rtb_Divide_f[1]; - tmp_4[3] = ctl_controller0_P->Gain1_Gain_bs * rtb_Divide_f[2]; - tmp_4[4] = (real32_T)ctl_controller0_P->Constant3_Value_n; - tmp_4[5] = rtb_Divide_f[0]; - tmp_4[6] = rtb_Divide_f[1]; - tmp_4[7] = ctl_controller0_P->Gain2_Gain_o * rtb_Divide_f[0]; - tmp_4[8] = (real32_T)ctl_controller0_P->Constant3_Value_n; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i = 0; i < 3; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = rtb_Assignment_h[(int32_T)(3 * i)] - + tmp_4[(int32_T)(3 * i)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = rtb_Assignment_h - [(int32_T)((int32_T)(3 * i) + 1)] + tmp_4[(int32_T)((int32_T)(3 * i) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = rtb_Assignment_h - [(int32_T)((int32_T)(3 * i) + 2)] + tmp_4[(int32_T)((int32_T)(3 * i) + 2)]; - } - - rtb_VectorConcatenate[3] = ctl_controller0_P->Gain1_Gain_ed * rtb_Divide_f[0]; - rtb_VectorConcatenate[7] = ctl_controller0_P->Gain1_Gain_ed * rtb_Divide_f[1]; - rtb_VectorConcatenate[11] = ctl_controller0_P->Gain1_Gain_ed * rtb_Divide_f[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Reshape: '/Reshape1' - rtb_VectorConcatenate[12] = rtb_Divide_f[0]; - rtb_VectorConcatenate[13] = rtb_Divide_f[1]; - rtb_VectorConcatenate[14] = rtb_Divide_f[2]; - rtb_VectorConcatenate[15] = rtb_Merge_j[3]; - - // Product: '/Product' - for (i = 0; i < 4; i++) { - rtb_Switch_est_P_B_ISS_ISS_id_0 = rtb_VectorConcatenate[(int32_T)(i + 12)] * - rtb_Merge_pa[3] + (rtb_VectorConcatenate[(int32_T)(i + 8)] * rtb_Merge_pa - [2] + (rtb_VectorConcatenate[(int32_T)(i + 4)] * - rtb_Merge_pa[1] + rtb_VectorConcatenate[i] * rtb_Merge_pa[0])); - rtb_Merge[i] = rtb_Switch_est_P_B_ISS_ISS_id_0; - } - - // End of Product: '/Product' - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Merge[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - ctl_controller0_Normalize(rtb_Merge, rtb_Merge_j, - (P_Normalize_ctl_controller0_T *)&ctl_controller0_P->Normalize_k); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge_j[0] = rtb_Merge[0]; - rtb_Merge_j[1] = rtb_Merge[1]; - rtb_Merge_j[2] = rtb_Merge[2]; - rtb_Merge_j[3] = rtb_Merge[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_SumofElements = (real32_T)sqrt((real_T)(((rtb_Merge_j[0] * rtb_Merge_j[0] - + rtb_Merge_j[1] * rtb_Merge_j[1]) + rtb_Merge_j[2] * rtb_Merge_j[2]) + - rtb_Merge_j[3] * rtb_Merge_j[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_SumofElements > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - ctl_controller0_Normalize_e(rtb_Merge_j, rtb_SumofElements, rtb_Merge); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge[3] = rtb_Merge_j[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/Sum3' - rtb_Sum3_k_idx_0 -= rtb_Switch_est_V_B_ISS_ISS_idx_; - - // DiscreteIntegrator: '/Discrete-Time Integrator' - rtb_Switch_est_V_B_ISS_ISS_idx_ = - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0]; - - // Sum: '/Sum4' - rtb_Sum4_o_idx_0 -= rtb_Switch_est_omega_B_ISS_B_id; - - // DiscreteIntegrator: '/Discrete-Time Integrator1' - rtb_Switch_est_omega_B_ISS_B_id = - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0]; - - // Sum: '/Sum3' - rtb_Sum3_k_idx_1 -= rtb_Switch_est_V_B_ISS_ISS_id_0; - - // DiscreteIntegrator: '/Discrete-Time Integrator' - rtb_Switch_est_V_B_ISS_ISS_id_0 = - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1]; - - // Sum: '/Sum4' - rtb_Sum4_o_idx_1 -= rtb_Switch_est_omega_B_ISS_B__0; - - // DiscreteIntegrator: '/Discrete-Time Integrator1' - rtb_Switch_est_omega_B_ISS_B__0 = - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1]; - - // Sum: '/Sum3' - rtb_SumofElements1 = rtb_Sum3_k_idx_2 - rtb_Switch_est_V_B_ISS_ISS_id_1; - - // Sum: '/Sum4' - rtb_Switch_est_omega_B_ISS_B__1 = rtb_Sum4_p - rtb_Switch_est_omega_B_ISS_B__1; - - // BusCreator: '/bus_creator' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_Switch_est_V_B_ISS_ISS_id_1 = - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2]; - rtb_Switch_est_P_B_ISS_ISS_id_0 = - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2]; - - // Update for UnitDelay: '/UD' - ctl_controller0_DW->UD_DSTATE[0] = rtb_TSamp[0]; - ctl_controller0_DW->UD_DSTATE[1] = rtb_TSamp[1]; - ctl_controller0_DW->UD_DSTATE[2] = rtb_TSamp[2]; - ctl_controller0_DW->UD_DSTATE[3] = rtb_TSamp[3]; - - // Update for UnitDelay: '/UD' - ctl_controller0_DW->UD_DSTATE_e[0] = rtb_Product[0]; - ctl_controller0_DW->UD_DSTATE_e[1] = rtb_Product[1]; - ctl_controller0_DW->UD_DSTATE_e[2] = rtb_Product_f; - - // Update for UnitDelay: '/Unit Delay2' - ctl_controller0_DW->UnitDelay2_DSTATE[0] = rtb_Product_b[0]; - ctl_controller0_DW->UnitDelay2_DSTATE[1] = rtb_Product_b[1]; - ctl_controller0_DW->UnitDelay2_DSTATE[2] = rtb_Product_b[2]; - ctl_controller0_DW->UnitDelay2_DSTATE[3] = rtb_Product_b[3]; - - // Update for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE[0] = rtb_SumA21_idx_0; - - // Update for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE_i[0] = rtb_SumA21_d_idx_0; - - // Update for UnitDelay: '/Unit Delay1' - ctl_controller0_DW->UnitDelay1_DSTATE[0] = rtb_Switch8_idx_0; - - // Update for S-Function (sfix_udelay): '/Tapped Delay' - ctl_controller0_DW->TappedDelay_X[0] = ctl_controller0_DW->TappedDelay_X[1]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // Product: '/Product1' - // Product: '/Divide1' - // Switch: '/Switch' - - if (rtb_Switch_vel_kd[0] != 0.0F) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch_pos_ki_idx_0 / - rtb_Switch_vel_kd[0]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = 0.0F; - } - - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] += rtb_Sum2_idx_0 * - rtb_Switch_cmd_state_a_A_B_IS_1 * - ctl_controller0_P->DiscreteTimeIntegrator1_gainval; - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] >= - ctl_controller0_P->tun_ctl_pos_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] = - ctl_controller0_P->tun_ctl_pos_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] <= - ctl_controller0_P->tun_ctl_pos_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] = - ctl_controller0_P->tun_ctl_pos_sat_lower; - } - } - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // Product: '/Product1' - // Product: '/Divide1' - // Switch: '/Switch' - - if (rtb_Switch_omega_kd_idx_0 != 0.0F) { - rtb_SumofElements = rtb_Switch_att_ki_idx_0 / rtb_Switch_omega_kd_idx_0; - } else { - rtb_SumofElements = 0.0F; - } - - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] += rtb_Product1_m[0] * - rtb_SumofElements * ctl_controller0_P->DiscreteTimeIntegrator_gainval; - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] >= - ctl_controller0_P->tun_ctl_att_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] = - ctl_controller0_P->tun_ctl_att_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] <= - ctl_controller0_P->tun_ctl_att_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] = - ctl_controller0_P->tun_ctl_att_sat_lower; - } - } - - // End of Outputs for SubSystem: '/ctl_controller' - - // Outport: '/cmd_msg' - ctl_controller0_Y_cmd_msg_f->traj_pos[0] = normA; - ctl_controller0_Y_cmd_msg_f->traj_vel[0] = b_s; - ctl_controller0_Y_cmd_msg_f->traj_accel[0] = rtb_Diff[0]; - - // Outputs for Atomic SubSystem: '/ctl_controller' - // Update for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE[1] = rtb_SumA21_idx_1; - - // Update for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE_i[1] = rtb_SumA21_d_idx_1; - - // Update for UnitDelay: '/Unit Delay1' - ctl_controller0_DW->UnitDelay1_DSTATE[1] = rtb_Switch8_idx_1; - - // Update for S-Function (sfix_udelay): '/Tapped Delay' - ctl_controller0_DW->TappedDelay_X[1] = ctl_controller0_DW->TappedDelay_X[2]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // Product: '/Product1' - // Product: '/Divide1' - // Switch: '/Switch' - - if (rtb_Switch_vel_kd[1] != 0.0F) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch_pos_ki_idx_1 / - rtb_Switch_vel_kd[1]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = 0.0F; - } - - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] += rtb_Sum2_idx_1 * - rtb_Switch_cmd_state_a_A_B_IS_1 * - ctl_controller0_P->DiscreteTimeIntegrator1_gainval; - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] >= - ctl_controller0_P->tun_ctl_pos_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] = - ctl_controller0_P->tun_ctl_pos_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] <= - ctl_controller0_P->tun_ctl_pos_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] = - ctl_controller0_P->tun_ctl_pos_sat_lower; - } - } - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // Product: '/Product1' - // Product: '/Divide1' - // Switch: '/Switch' - - if (rtb_Switch_omega_kd_idx_1 != 0.0F) { - rtb_SumofElements = rtb_Switch_att_ki_idx_1 / rtb_Switch_omega_kd_idx_1; - } else { - rtb_SumofElements = 0.0F; - } - - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] += rtb_Product1_m[1] * - rtb_SumofElements * ctl_controller0_P->DiscreteTimeIntegrator_gainval; - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] >= - ctl_controller0_P->tun_ctl_att_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] = - ctl_controller0_P->tun_ctl_att_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] <= - ctl_controller0_P->tun_ctl_att_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] = - ctl_controller0_P->tun_ctl_att_sat_lower; - } - } - - // End of Outputs for SubSystem: '/ctl_controller' - - // Outport: '/cmd_msg' - ctl_controller0_Y_cmd_msg_f->traj_pos[1] = rtb_Switch_cmd_state_a_P_B_ISS_; - ctl_controller0_Y_cmd_msg_f->traj_vel[1] = rtb_Switch_cmd_state_a_V_B_ISS_; - ctl_controller0_Y_cmd_msg_f->traj_accel[1] = rtb_Diff[1]; - - // Outputs for Atomic SubSystem: '/ctl_controller' - // Update for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE[2] = rtb_Sum4_o_idx_2; - - // Update for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE_i[2] = rtb_SumA21_f; - - // Update for UnitDelay: '/Unit Delay1' - ctl_controller0_DW->UnitDelay1_DSTATE[2] = rtb_Switch8_idx_2; - - // Update for S-Function (sfix_udelay): '/Tapped Delay' - ctl_controller0_DW->TappedDelay_X[2] = ctl_controller0_DW->TappedDelay_X[3]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // Product: '/Product1' - // Product: '/Divide1' - // Switch: '/Switch' - - if (rtb_Switch_vel_kd[2] != 0.0F) { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Switch_pos_ki_idx_2 / - rtb_Switch_vel_kd[2]; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = 0.0F; - } - - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] += rtb_Sum2_idx_2 * - rtb_Switch_cmd_state_a_A_B_IS_1 * - ctl_controller0_P->DiscreteTimeIntegrator1_gainval; - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] >= - ctl_controller0_P->tun_ctl_pos_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] = - ctl_controller0_P->tun_ctl_pos_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] <= - ctl_controller0_P->tun_ctl_pos_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] = - ctl_controller0_P->tun_ctl_pos_sat_lower; - } - } - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // Product: '/Product1' - // Product: '/Divide1' - // Switch: '/Switch' - - if (rtb_Switch_omega_kd_idx_2 != 0.0F) { - rtb_SumofElements = rtb_Switch_att_ki_idx_2 / rtb_Switch_omega_kd_idx_2; - } else { - rtb_SumofElements = 0.0F; - } - - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] += rtb_Product1_m[2] * - rtb_SumofElements * ctl_controller0_P->DiscreteTimeIntegrator_gainval; - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] >= - ctl_controller0_P->tun_ctl_att_sat_upper) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] = - ctl_controller0_P->tun_ctl_att_sat_upper; - } else { - if (ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] <= - ctl_controller0_P->tun_ctl_att_sat_lower) { - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] = - ctl_controller0_P->tun_ctl_att_sat_lower; - } - } - - // End of Outputs for SubSystem: '/ctl_controller' - - // Outport: '/cmd_msg' incorporates: - // BusCreator: '/Bus Creator1' - - ctl_controller0_Y_cmd_msg_f->traj_pos[2] = rtb_Switch_cmd_state_a_P_B_IS_0; - - // Outputs for Atomic SubSystem: '/ctl_controller' - ctl_controller0_Y_cmd_msg_f->traj_vel[2] = rtb_Sum3_k_idx_2; - - // End of Outputs for SubSystem: '/ctl_controller' - ctl_controller0_Y_cmd_msg_f->traj_accel[2] = rtb_Diff[2]; - - // Outputs for Atomic SubSystem: '/ctl_controller' - // Update for S-Function (sfix_udelay): '/Tapped Delay' incorporates: - // Constant: '/Constant' - // RelationalOperator: '/Compare' - - ctl_controller0_DW->TappedDelay_X[3] = (rtb_Switch12 == - ctl_controller0_P->ctl_stopping_mode); - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' - ctl_controller0_DW->DiscreteTimeIntegrator1_PrevRes = (int8_T)rtb_Compare_j; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - ctl_controller0_DW->DiscreteTimeIntegrator_PrevRese = (int8_T)rtb_Compare_fc; - - // End of Outputs for SubSystem: '/ctl_controller' - - // Outport: '/cmd_msg' incorporates: - // BusCreator: '/Bus Creator1' - // DataTypeConversion: '/Data Type Conversion' - - ctl_controller0_Y_cmd_msg_f->cmd_timestamp_sec = - rtb_Switch_cmd_state_b_timestam; - ctl_controller0_Y_cmd_msg_f->cmd_timestamp_nsec = - rtb_Switch_cmd_state_b_timest_0; - ctl_controller0_Y_cmd_msg_f->cmd_mode = rtb_Switch_ctl_mode_cmd; - ctl_controller0_Y_cmd_msg_f->speed_gain_cmd = rtb_Switch_speed_gain_cmd; - - // Outputs for Atomic SubSystem: '/ctl_controller' - ctl_controller0_Y_cmd_msg_f->cmd_B_inuse = (uint8_T)rtb_LogicalOperator2_c; - ctl_controller0_Y_cmd_msg_f->traj_quat[0] = rtb_Merge_pa[0]; - ctl_controller0_Y_cmd_msg_f->traj_quat[1] = rtb_Merge_pa[1]; - ctl_controller0_Y_cmd_msg_f->traj_quat[2] = rtb_Merge_pa[2]; - ctl_controller0_Y_cmd_msg_f->traj_quat[3] = rtb_Merge_pa[3]; - - // Product: '/Divide' incorporates: - // Outport: '/ctl_msg' - - rt_mldivide_U1f3x3_U2f_XeZWzB4d(rtb_Switch_inertia_matrix, rtb_Sum_f, - ctl_controller0_Y_ctl_msg_n->body_alpha_cmd); - - // Trigonometry: '/Trigonometric Function' - if (rtb_Product1_m[3] > 1.0F) { - rtb_Switch_cmd_state_a_A_B_IS_1 = 1.0F; - } else if (rtb_Product1_m[3] < -1.0F) { - rtb_Switch_cmd_state_a_A_B_IS_1 = -1.0F; - } else { - rtb_Switch_cmd_state_a_A_B_IS_1 = rtb_Product1_m[3]; - } - - // Outport: '/ctl_msg' incorporates: - // BusCreator: '/bus_creator' - // Gain: '/Gain2' - // Trigonometry: '/Trigonometric Function' - - ctl_controller0_Y_ctl_msg_n->att_err_mag = ctl_controller0_P->Gain2_Gain_j * - (real32_T)acos((real_T)rtb_Switch_cmd_state_a_A_B_IS_1); - ctl_controller0_Y_ctl_msg_n->ctl_status = rtb_Switch3; - - // End of Outputs for SubSystem: '/ctl_controller' - - // Outport: '/cmd_msg' - ctl_controller0_Y_cmd_msg_f->traj_omega[0] = rtb_Switch_cmd_state_a_A_B_ISS_; - ctl_controller0_Y_cmd_msg_f->traj_alpha[0] = rtb_Switch_cmd_state_b_alpha__1; - - // Outputs for Atomic SubSystem: '/ctl_controller' - // Outport: '/ctl_msg' incorporates: - // BusCreator: '/bus_creator' - // Product: '/Divide' - - ctl_controller0_Y_ctl_msg_n->body_force_cmd[0] = rtb_Switch_c_idx_0; - ctl_controller0_Y_ctl_msg_n->body_accel_cmd[0] = 1.0F / rtb_Switch_mass * - rtb_Switch_c_idx_0; - ctl_controller0_Y_ctl_msg_n->pos_err[0] = rtb_Sum2_idx_0; - - // End of Outputs for SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->pos_err_int[0] = rtb_Switch_est_omega_B_ISS_B_id; - - // Outputs for Atomic SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->body_torque_cmd[0] = rtb_Switch_b[0]; - ctl_controller0_Y_ctl_msg_n->att_err[0] = rtb_Product1_m[0]; - - // End of Outputs for SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->att_err_int[0] = rtb_Switch_est_V_B_ISS_ISS_idx_; - - // Outport: '/cmd_msg' - ctl_controller0_Y_cmd_msg_f->traj_omega[1] = rtb_Switch_cmd_state_a_A_B_IS_0; - ctl_controller0_Y_cmd_msg_f->traj_alpha[1] = rtb_Switch_cmd_state_b_alpha__0; - - // Outputs for Atomic SubSystem: '/ctl_controller' - // Outport: '/ctl_msg' incorporates: - // BusCreator: '/bus_creator' - // Product: '/Divide' - - ctl_controller0_Y_ctl_msg_n->body_force_cmd[1] = rtb_Switch_c_idx_1; - ctl_controller0_Y_ctl_msg_n->body_accel_cmd[1] = 1.0F / rtb_Switch_mass * - rtb_Switch_c_idx_1; - ctl_controller0_Y_ctl_msg_n->pos_err[1] = rtb_Sum2_idx_1; - - // End of Outputs for SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->pos_err_int[1] = rtb_Switch_est_omega_B_ISS_B__0; - - // Outputs for Atomic SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->body_torque_cmd[1] = rtb_Switch_b[1]; - ctl_controller0_Y_ctl_msg_n->att_err[1] = rtb_Product1_m[1]; - - // End of Outputs for SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->att_err_int[1] = rtb_Switch_est_V_B_ISS_ISS_id_0; - - // Outport: '/cmd_msg' - ctl_controller0_Y_cmd_msg_f->traj_omega[2] = rtb_Sum4_p; - ctl_controller0_Y_cmd_msg_f->traj_alpha[2] = rtb_Switch_cmd_state_b_alpha_B_; - - // Outputs for Atomic SubSystem: '/ctl_controller' - // Outport: '/ctl_msg' incorporates: - // BusCreator: '/bus_creator' - // DotProduct: '/Dot Product' - // Product: '/Divide' - // Sqrt: '/Sqrt' - - ctl_controller0_Y_ctl_msg_n->body_force_cmd[2] = rtb_Switch_c_idx_2; - ctl_controller0_Y_ctl_msg_n->body_accel_cmd[2] = 1.0F / rtb_Switch_mass * - rtb_Switch_c_idx_2; - ctl_controller0_Y_ctl_msg_n->pos_err[2] = rtb_Sum2_idx_2; - - // End of Outputs for SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->pos_err_int[2] = rtb_Switch_est_V_B_ISS_ISS_id_1; - - // Outputs for Atomic SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->body_torque_cmd[2] = rtb_Switch_b[2]; - ctl_controller0_Y_ctl_msg_n->att_err[2] = rtb_Product1_m[2]; - - // End of Outputs for SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->att_err_int[2] = rtb_Switch_est_P_B_ISS_ISS_id_0; - - // Outputs for Atomic SubSystem: '/ctl_controller' - ctl_controller0_Y_ctl_msg_n->traj_error_pos = (real32_T)sqrt((real_T) - ((rtb_Gain1_j_idx_0 * rtb_Gain1_j_idx_0 + rtb_Gain1_j_idx_1 * - rtb_Gain1_j_idx_1) + rtb_Switch_est_P_B_ISS_ISS_idx_ * - rtb_Switch_est_P_B_ISS_ISS_idx_)); - - // Trigonometry: '/Trigonometric Function' - if (rtb_Merge[3] > 1.0F) { - rtb_Switch_est_P_B_ISS_ISS_id_0 = 1.0F; - } else if (rtb_Merge[3] < -1.0F) { - rtb_Switch_est_P_B_ISS_ISS_id_0 = -1.0F; - } else { - rtb_Switch_est_P_B_ISS_ISS_id_0 = rtb_Merge[3]; - } - - // Outport: '/ctl_msg' incorporates: - // Abs: '/Abs' - // DotProduct: '/Dot Product' - // DotProduct: '/Dot Product' - // Gain: '/Gain' - // Sqrt: '/Sqrt' - // Sqrt: '/Sqrt' - // Trigonometry: '/Trigonometric Function' - - ctl_controller0_Y_ctl_msg_n->traj_error_att = ctl_controller0_P->Gain_Gain_hg * - (real32_T)fabs((real_T)(real32_T)acos((real_T) - rtb_Switch_est_P_B_ISS_ISS_id_0)); - ctl_controller0_Y_ctl_msg_n->traj_error_vel = (real32_T)sqrt((real_T) - ((rtb_Sum3_k_idx_0 * rtb_Sum3_k_idx_0 + rtb_Sum3_k_idx_1 * rtb_Sum3_k_idx_1) - + rtb_SumofElements1 * rtb_SumofElements1)); - ctl_controller0_Y_ctl_msg_n->traj_error_omega = (real32_T)sqrt((real_T) - ((rtb_Sum4_o_idx_0 * rtb_Sum4_o_idx_0 + rtb_Sum4_o_idx_1 * rtb_Sum4_o_idx_1) - + rtb_Switch_est_omega_B_ISS_B__1 * rtb_Switch_est_omega_B_ISS_B__1)); - - // End of Outputs for SubSystem: '/ctl_controller' -} - -// Model initialize function -void ctl_controller0_initialize(RT_MODEL_ctl_controller0_T *const - ctl_controller0_M, ctl_input_msg *ctl_controller0_U_ctl_input_msg_l, cmd_msg - *ctl_controller0_Y_cmd_msg_f, ctl_msg *ctl_controller0_Y_ctl_msg_n) -{ - P_ctl_controller0_T *ctl_controller0_P = ((P_ctl_controller0_T *) - ctl_controller0_M->defaultParam); - DW_ctl_controller0_T *ctl_controller0_DW = ((DW_ctl_controller0_T *) - ctl_controller0_M->dwork); - - // Start for Atomic SubSystem: '/ctl_controller' - - // Start for Iterator SubSystem: '/For Each Subsystem' - ctl_cont_ForEachSubsystem_Start(3, ctl_controller0_DW->ForEachSubsystem); - - // End of Start for SubSystem: '/For Each Subsystem' - - // Start for Iterator SubSystem: '/For Each Subsystem1' - ctl_cont_ForEachSubsystem_Start(3, ctl_controller0_DW->ForEachSubsystem1); - - // End of Start for SubSystem: '/For Each Subsystem1' - - // End of Start for SubSystem: '/ctl_controller' - - // SystemInitialize for Atomic SubSystem: '/ctl_controller' - // InitializeConditions for UnitDelay: '/UD' - ctl_controller0_DW->UD_DSTATE[0] = - ctl_controller0_P->DiscreteDerivative_ICPrevScaled; - ctl_controller0_DW->UD_DSTATE[1] = - ctl_controller0_P->DiscreteDerivative_ICPrevScaled; - ctl_controller0_DW->UD_DSTATE[2] = - ctl_controller0_P->DiscreteDerivative_ICPrevScaled; - ctl_controller0_DW->UD_DSTATE[3] = - ctl_controller0_P->DiscreteDerivative_ICPrevScaled; - - // InitializeConditions for UnitDelay: '/UD' - ctl_controller0_DW->UD_DSTATE_e[0] = - ctl_controller0_P->DiscreteDerivative_ICPrevScal_k; - ctl_controller0_DW->UD_DSTATE_e[1] = - ctl_controller0_P->DiscreteDerivative_ICPrevScal_k; - ctl_controller0_DW->UD_DSTATE_e[2] = - ctl_controller0_P->DiscreteDerivative_ICPrevScal_k; - - // InitializeConditions for UnitDelay: '/Unit Delay2' - ctl_controller0_DW->UnitDelay2_DSTATE[0] = - ctl_controller0_P->UnitDelay2_InitialCondition; - ctl_controller0_DW->UnitDelay2_DSTATE[1] = - ctl_controller0_P->UnitDelay2_InitialCondition; - ctl_controller0_DW->UnitDelay2_DSTATE[2] = - ctl_controller0_P->UnitDelay2_InitialCondition; - ctl_controller0_DW->UnitDelay2_DSTATE[3] = - ctl_controller0_P->UnitDelay2_InitialCondition; - - // InitializeConditions for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE[0] = - ctl_controller0_P->Delay11_InitialCondition; - - // InitializeConditions for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE_i[0] = - ctl_controller0_P->Delay11_InitialCondition_k; - - // InitializeConditions for UnitDelay: '/Unit Delay1' - ctl_controller0_DW->UnitDelay1_DSTATE[0] = - ctl_controller0_P->UnitDelay1_InitialCondition; - - // InitializeConditions for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE[1] = - ctl_controller0_P->Delay11_InitialCondition; - - // InitializeConditions for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE_i[1] = - ctl_controller0_P->Delay11_InitialCondition_k; - - // InitializeConditions for UnitDelay: '/Unit Delay1' - ctl_controller0_DW->UnitDelay1_DSTATE[1] = - ctl_controller0_P->UnitDelay1_InitialCondition; - - // InitializeConditions for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE[2] = - ctl_controller0_P->Delay11_InitialCondition; - - // InitializeConditions for Delay: '/Delay11' - ctl_controller0_DW->Delay11_DSTATE_i[2] = - ctl_controller0_P->Delay11_InitialCondition_k; - - // InitializeConditions for UnitDelay: '/Unit Delay1' - ctl_controller0_DW->UnitDelay1_DSTATE[2] = - ctl_controller0_P->UnitDelay1_InitialCondition; - - // InitializeConditions for S-Function (sfix_udelay): '/Tapped Delay' - ctl_controller0_DW->TappedDelay_X[0] = ((int32_T) - ctl_controller0_P->TappedDelay_vinit != 0); - ctl_controller0_DW->TappedDelay_X[1] = ((int32_T) - ctl_controller0_P->TappedDelay_vinit != 0); - ctl_controller0_DW->TappedDelay_X[2] = ((int32_T) - ctl_controller0_P->TappedDelay_vinit != 0); - ctl_controller0_DW->TappedDelay_X[3] = ((int32_T) - ctl_controller0_P->TappedDelay_vinit != 0); - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - ctl_controller0_DW->DiscreteTimeIntegrator1_PrevRes = 0; - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[0] = - ctl_controller0_P->DiscreteTimeIntegrator1_IC[0]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[0] = - ctl_controller0_P->DiscreteTimeIntegrator_IC[0]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[1] = - ctl_controller0_P->DiscreteTimeIntegrator1_IC[1]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[1] = - ctl_controller0_P->DiscreteTimeIntegrator_IC[1]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - ctl_controller0_DW->DiscreteTimeIntegrator1_DSTATE[2] = - ctl_controller0_P->DiscreteTimeIntegrator1_IC[2]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - ctl_controller0_DW->DiscreteTimeIntegrator_DSTATE[2] = - ctl_controller0_P->DiscreteTimeIntegrator_IC[2]; - ctl_controller0_DW->DiscreteTimeIntegrator_PrevRese = 0; - - // SystemInitialize for Iterator SubSystem: '/For Each Subsystem' - ctl_contr_ForEachSubsystem_Init(3, ctl_controller0_DW->ForEachSubsystem, - (P_ForEachSubsystem_ctl_contro_T *)&ctl_controller0_P->ForEachSubsystem); - - // End of SystemInitialize for SubSystem: '/For Each Subsystem' - - // SystemInitialize for Iterator SubSystem: '/For Each Subsystem1' - ctl_contr_ForEachSubsystem_Init(3, ctl_controller0_DW->ForEachSubsystem1, - (P_ForEachSubsystem_ctl_contro_T *)&ctl_controller0_P->ForEachSubsystem1); - - // End of SystemInitialize for SubSystem: '/For Each Subsystem1' - - // End of SystemInitialize for SubSystem: '/ctl_controller' -} - -// Model terminate function -void ctl_controller0_terminate(RT_MODEL_ctl_controller0_T * ctl_controller0_M) -{ - // model code - if (ctl_controller0_M->paramIsMalloced) { - rt_FREE(ctl_controller0_M->defaultParam); - } - - rt_FREE(ctl_controller0_M->dwork); - rt_FREE(ctl_controller0_M); -} - -// Model data allocation function -RT_MODEL_ctl_controller0_T *ctl_controller0(ctl_input_msg - *ctl_controller0_U_ctl_input_msg_l, cmd_msg *ctl_controller0_Y_cmd_msg_f, - ctl_msg *ctl_controller0_Y_ctl_msg_n) -{ - RT_MODEL_ctl_controller0_T *ctl_controller0_M; - ctl_controller0_M = (RT_MODEL_ctl_controller0_T *) malloc(sizeof - (RT_MODEL_ctl_controller0_T)); - if (ctl_controller0_M == NULL) { - return NULL; - } - - (void) memset((char *)ctl_controller0_M, 0, - sizeof(RT_MODEL_ctl_controller0_T)); - - // parameters - { - P_ctl_controller0_T *p; - static int_T pSeen = 0; - - // only malloc on multiple model instantiation - if (pSeen == 1 ) { - p = (P_ctl_controller0_T *) malloc(sizeof(P_ctl_controller0_T)); - rt_VALIDATE_MEMORY(ctl_controller0_M,p); - (void) memcpy(p, &ctl_controller0_P, - sizeof(P_ctl_controller0_T)); - ctl_controller0_M->paramIsMalloced = (true); - } else { - p = &ctl_controller0_P; - ctl_controller0_M->paramIsMalloced = (false); - pSeen = 1; - } - - ctl_controller0_M->defaultParam = (p); - } - - // states (dwork) - { - DW_ctl_controller0_T *dwork = (DW_ctl_controller0_T *) malloc(sizeof - (DW_ctl_controller0_T)); - rt_VALIDATE_MEMORY(ctl_controller0_M,dwork); - ctl_controller0_M->dwork = (dwork); - } - - { - P_ctl_controller0_T *ctl_controller0_P = ((P_ctl_controller0_T *) - ctl_controller0_M->defaultParam); - DW_ctl_controller0_T *ctl_controller0_DW = ((DW_ctl_controller0_T *) - ctl_controller0_M->dwork); - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // states (dwork) - (void) memset((void *)ctl_controller0_DW, 0, - sizeof(DW_ctl_controller0_T)); - - // external inputs - *ctl_controller0_U_ctl_input_msg_l = ctl_controller0_rtZctl_input_ms; - - // external outputs - (*ctl_controller0_Y_cmd_msg_f) = ctl_controller0_rtZcmd_msg; - (*ctl_controller0_Y_ctl_msg_n) = ctl_controller0_rtZctl_msg; - } - - return ctl_controller0_M; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0.h b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0.h deleted file mode 100644 index 43774004d0..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0.h +++ /dev/null @@ -1,709 +0,0 @@ -// -// File: ctl_controller0.h -// -// Code generated for Simulink model 'ctl_controller0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:53 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_ctl_controller0_h_ -#define RTW_HEADER_ctl_controller0_h_ -#include -#include -#include -#ifndef ctl_controller0_COMMON_INCLUDES_ -# define ctl_controller0_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#endif // ctl_controller0_COMMON_INCLUDES_ - -#include "ctl_controller0_types.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "mglnkfkfmglfjekn_PadeApproximantOfDegree.h" -#include "rt_mldivide_U1f3x3_U2f_XeZWzB4d.h" -#include "rt_powf_snf.h" -#include "rtGetInf.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -# define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -# define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Block states (auto storage) for system '/CoreSubsys' -typedef struct { - real32_T uHzLowPass_states; // '/3 Hz Low Pass' -} DW_CoreSubsys_ctl_controller0_T; - -// Block states (auto storage) for system '/For Each Subsystem' -typedef struct { - DW_CoreSubsys_ctl_controller0_T CoreSubsys;// '/CoreSubsys' -} DW_ForEachSubsystem_ctl_contr_T; - -// Block states (auto storage) for system '' -typedef struct { - real32_T UD_DSTATE[4]; // '/UD' - real32_T UD_DSTATE_e[3]; // '/UD' - real32_T UnitDelay2_DSTATE[4]; // '/Unit Delay2' - real32_T Delay11_DSTATE[3]; // '/Delay11' - real32_T Delay11_DSTATE_i[3]; // '/Delay11' - real32_T UnitDelay1_DSTATE[3]; // '/Unit Delay1' - real32_T DiscreteTimeIntegrator1_DSTATE[3];// '/Discrete-Time Integrator1' - real32_T DiscreteTimeIntegrator_DSTATE[3];// '/Discrete-Time Integrator' - boolean_T TappedDelay_X[4]; // '/Tapped Delay' - int8_T DiscreteTimeIntegrator1_PrevRes;// '/Discrete-Time Integrator1' - int8_T DiscreteTimeIntegrator_PrevRese;// '/Discrete-Time Integrator' - DW_ForEachSubsystem_ctl_contr_T ForEachSubsystem1[3];// '/For Each Subsystem1' - DW_ForEachSubsystem_ctl_contr_T ForEachSubsystem[3];// '/For Each Subsystem' -} DW_ctl_controller0_T; - -// Parameters for system: '/Normalize' -struct P_Normalize_ctl_controller0_T_ { - real_T Constant1_Value; // Expression: -1 - // Referenced by: '/Constant1' - -}; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_ctl_controller0_T_ { - real32_T uHzLowPass_NumCoef[2]; // Expression: [omega*T/(2+omega*T) omega*T/(2+omega*T)] - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_DenCoef[2]; // Expression: [1 (omega*T-2)/(2+omega*T)] - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_InitialStates; // Computed Parameter: uHzLowPass_InitialStates - // Referenced by: '/3 Hz Low Pass' - -}; - -// Parameters for system: '/For Each Subsystem' -struct P_ForEachSubsystem_ctl_contro_T_ { - P_CoreSubsys_ctl_controller0_T CoreSubsys;// '/CoreSubsys' -}; - -// Parameters (auto storage) -struct P_ctl_controller0_T_ { - real32_T tun_accel_gain[3]; // Variable: tun_accel_gain - // Referenced by: '/Gain' - - real32_T tun_alpha_gain[3]; // Variable: tun_alpha_gain - // Referenced by: '/Gain' - - real32_T tun_ctl_att_sat_lower; // Variable: tun_ctl_att_sat_lower - // Referenced by: '/Discrete-Time Integrator' - - real32_T tun_ctl_att_sat_upper; // Variable: tun_ctl_att_sat_upper - // Referenced by: '/Discrete-Time Integrator' - - real32_T tun_ctl_linear_force_limit; // Variable: tun_ctl_linear_force_limit - // Referenced by: '/Constant2' - - real32_T tun_ctl_pos_sat_lower; // Variable: tun_ctl_pos_sat_lower - // Referenced by: '/Discrete-Time Integrator1' - - real32_T tun_ctl_pos_sat_upper; // Variable: tun_ctl_pos_sat_upper - // Referenced by: '/Discrete-Time Integrator1' - - real32_T tun_ctl_stopped_pos_thresh; // Variable: tun_ctl_stopped_pos_thresh - // Referenced by: '/Constant' - - real32_T tun_ctl_stopped_quat_thresh;// Variable: tun_ctl_stopped_quat_thresh - // Referenced by: '/Constant' - - real32_T tun_ctl_stopping_omega_thresh;// Variable: tun_ctl_stopping_omega_thresh - // Referenced by: '/Constant' - - real32_T tun_ctl_stopping_vel_thresh;// Variable: tun_ctl_stopping_vel_thresh - // Referenced by: '/Constant' - - real32_T tun_truth_q_omega_filt_enable;// Variable: tun_truth_q_omega_filt_enable - // Referenced by: '/For Each Subsystem' - - real32_T tun_truth_velocity_filt_enable;// Variable: tun_truth_velocity_filt_enable - // Referenced by: '/For Each Subsystem1' - - uint8_T ase_status_converged; // Variable: ase_status_converged - // Referenced by: '/Constant' - - uint8_T ctl_idle_mode; // Variable: ctl_idle_mode - // Referenced by: '/Constant4' - - uint8_T ctl_stopped_mode; // Variable: ctl_stopped_mode - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T ctl_stopping_mode; // Variable: ctl_stopping_mode - // Referenced by: - // '/Constant6' - // '/Constant' - - uint8_T tun_debug_ctl_use_truth; // Variable: tun_debug_ctl_use_truth - // Referenced by: '/Constant' - - real32_T DiscreteDerivative_ICPrevScaled;// Mask Parameter: DiscreteDerivative_ICPrevScaled - // Referenced by: '/UD' - - real32_T DiscreteDerivative_ICPrevScal_k;// Mask Parameter: DiscreteDerivative_ICPrevScal_k - // Referenced by: '/UD' - - uint8_T CompareToConstant2_const; // Mask Parameter: CompareToConstant2_const - // Referenced by: '/Constant' - - uint8_T CompareToConstant1_const; // Mask Parameter: CompareToConstant1_const - // Referenced by: '/Constant' - - uint8_T CompareToConstant_const; // Mask Parameter: CompareToConstant_const - // Referenced by: '/Constant' - - uint8_T CompareToConstant2_const_o; // Mask Parameter: CompareToConstant2_const_o - // Referenced by: '/Constant' - - uint8_T CompareToConstant1_const_m; // Mask Parameter: CompareToConstant1_const_m - // Referenced by: '/Constant' - - uint8_T CompareToConstant_const_f; // Mask Parameter: CompareToConstant_const_f - // Referenced by: '/Constant' - - uint8_T TappedDelay_vinit; // Mask Parameter: TappedDelay_vinit - // Referenced by: '/Tapped Delay' - - real_T Constant2_Value[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_p[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_a; // Expression: 0 - // Referenced by: '/Constant3' - - real_T s1_Gain; // Expression: 0.0031317642291927056 - // Referenced by: '/s(1)' - - real_T a21_Gain; // Expression: -0.993736471541614597 - // Referenced by: '/a(2)(1)' - - real_T s1_Gain_b; // Expression: 0.0031317642291927056 - // Referenced by: '/s(1)' - - real_T a21_Gain_l; // Expression: -0.993736471541614597 - // Referenced by: '/a(2)(1)' - - real_T Constant2_Value_pu[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_o; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_k[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_f; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_b; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_g[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_a5; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant3_Value_c; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_i[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_n; // Expression: 0 - // Referenced by: '/Constant3' - - real32_T Gain2_Gain; // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - real32_T Gain1_Gain; // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T Gain_Gain_p; // Computed Parameter: Gain_Gain_p - // Referenced by: '/Gain' - - real32_T Gain1_Gain_e; // Computed Parameter: Gain1_Gain_e - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_k; // Computed Parameter: Gain2_Gain_k - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_o; // Computed Parameter: Gain1_Gain_o - // Referenced by: '/Gain1' - - real32_T Gain_Gain_m; // Computed Parameter: Gain_Gain_m - // Referenced by: '/Gain' - - real32_T TSamp_WtEt; // Computed Parameter: TSamp_WtEt - // Referenced by: '/TSamp' - - real32_T TSamp_WtEt_k; // Computed Parameter: TSamp_WtEt_k - // Referenced by: '/TSamp' - - real32_T UnitDelay2_InitialCondition;// Computed Parameter: UnitDelay2_InitialCondition - // Referenced by: '/Unit Delay2' - - real32_T Gain_Gain_a; // Computed Parameter: Gain_Gain_a - // Referenced by: '/Gain' - - real32_T Gain_Gain_i; // Computed Parameter: Gain_Gain_i - // Referenced by: '/Gain' - - real32_T Gain1_Gain_n; // Computed Parameter: Gain1_Gain_n - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_a; // Computed Parameter: Gain2_Gain_a - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_j; // Computed Parameter: Gain1_Gain_j - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_h; // Computed Parameter: Gain2_Gain_h - // Referenced by: '/Gain2' - - real32_T Delay11_InitialCondition; // Computed Parameter: Delay11_InitialCondition - // Referenced by: '/Delay11' - - real32_T Delay11_InitialCondition_k; // Computed Parameter: Delay11_InitialCondition_k - // Referenced by: '/Delay11' - - real32_T Constant_Value; // Expression: single(0.5) - // Referenced by: '/Constant' - - real32_T Gain_Gain_h; // Expression: single(1e-9) - // Referenced by: '/Gain' - - real32_T Constant1_Value_g; // Computed Parameter: Constant1_Value_g - // Referenced by: '/Constant1' - - real32_T Constant3_Value_d; // Computed Parameter: Constant3_Value_d - // Referenced by: '/Constant3' - - real32_T Constant3_Value_dz; // Computed Parameter: Constant3_Value_dz - // Referenced by: '/Constant3' - - real32_T Gain_Gain_n; // Computed Parameter: Gain_Gain_n - // Referenced by: '/Gain' - - real32_T Gain1_Gain_b; // Computed Parameter: Gain1_Gain_b - // Referenced by: '/Gain1' - - real32_T Constant2_Value_d; // Computed Parameter: Constant2_Value_d - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_kx; // Computed Parameter: Gain2_Gain_kx - // Referenced by: '/Gain2' - - real32_T Gain3_Gain; // Computed Parameter: Gain3_Gain - // Referenced by: '/Gain3' - - real32_T Gain4_Gain; // Computed Parameter: Gain4_Gain - // Referenced by: '/Gain4' - - real32_T Constant1_Value_l; // Computed Parameter: Constant1_Value_l - // Referenced by: '/Constant1' - - real32_T Gain5_Gain; // Computed Parameter: Gain5_Gain - // Referenced by: '/Gain5' - - real32_T Constant_Value_g; // Computed Parameter: Constant_Value_g - // Referenced by: '/Constant' - - real32_T Constant3_Value_f; // Computed Parameter: Constant3_Value_f - // Referenced by: '/Constant3' - - real32_T Gain_Gain_g; // Computed Parameter: Gain_Gain_g - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ja; // Computed Parameter: Gain1_Gain_ja - // Referenced by: '/Gain1' - - real32_T Constant2_Value_gq; // Computed Parameter: Constant2_Value_gq - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_c; // Computed Parameter: Gain2_Gain_c - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_o; // Computed Parameter: Gain3_Gain_o - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_c; // Computed Parameter: Gain4_Gain_c - // Referenced by: '/Gain4' - - real32_T Constant1_Value_gk; // Computed Parameter: Constant1_Value_gk - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_a; // Computed Parameter: Gain5_Gain_a - // Referenced by: '/Gain5' - - real32_T Constant_Value_m; // Computed Parameter: Constant_Value_m - // Referenced by: '/Constant' - - real32_T Constant2_Value_it; // Computed Parameter: Constant2_Value_it - // Referenced by: '/Constant2' - - real32_T UnitDelay1_InitialCondition;// Computed Parameter: UnitDelay1_InitialCondition - // Referenced by: '/Unit Delay1' - - real32_T Constant1_Value_a[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant1' - - real32_T Constant2_Value_n[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant2' - - real32_T Constant3_Value_j[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant3' - - real32_T Constant5_Value[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant5' - - real32_T Gain_Gain_l; // Computed Parameter: Gain_Gain_l - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ej; // Computed Parameter: Gain1_Gain_ej - // Referenced by: '/Gain1' - - real32_T Gain_Gain_ib; // Computed Parameter: Gain_Gain_ib - // Referenced by: '/Gain' - - real32_T Gain1_Gain_k; // Computed Parameter: Gain1_Gain_k - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_b; // Computed Parameter: Gain2_Gain_b - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_k1; // Computed Parameter: Gain2_Gain_k1 - // Referenced by: '/Gain2' - - real32_T Gain_Gain_b; // Computed Parameter: Gain_Gain_b - // Referenced by: '/Gain' - - real32_T Gain1_Gain_g; // Computed Parameter: Gain1_Gain_g - // Referenced by: '/Gain1' - - real32_T Gain_Gain_d; // Computed Parameter: Gain_Gain_d - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ji; // Computed Parameter: Gain1_Gain_ji - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_bs; // Computed Parameter: Gain2_Gain_bs - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_kh; // Computed Parameter: Gain2_Gain_kh - // Referenced by: '/Gain2' - - real32_T Constant4_Value[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant4' - - real32_T Constant1_Value_k[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant1' - - real32_T DiscreteTimeIntegrator1_gainval;// Computed Parameter: DiscreteTimeIntegrator1_gainval - // Referenced by: '/Discrete-Time Integrator1' - - real32_T DiscreteTimeIntegrator1_IC[3];// Expression: single([0 0 0]) - // Referenced by: '/Discrete-Time Integrator1' - - real32_T Constant3_Value_e[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant3' - - real32_T Constant2_Value_o[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant2' - - real32_T Constant1_Value_o[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant1' - - real32_T Gain_Gain_pf; // Computed Parameter: Gain_Gain_pf - // Referenced by: '/Gain' - - real32_T Gain_Gain_k; // Computed Parameter: Gain_Gain_k - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jz; // Computed Parameter: Gain1_Gain_jz - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_n; // Computed Parameter: Gain2_Gain_n - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_od; // Computed Parameter: Gain1_Gain_od - // Referenced by: '/Gain1' - - real32_T DiscreteTimeIntegrator_gainval;// Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - real32_T DiscreteTimeIntegrator_IC[3];// Expression: single([0 0 0]) - // Referenced by: '/Discrete-Time Integrator' - - real32_T Constant4_Value_f[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant4' - - real32_T Gain2_Gain_j; // Computed Parameter: Gain2_Gain_j - // Referenced by: '/Gain2' - - real32_T Gain_Gain_is; // Computed Parameter: Gain_Gain_is - // Referenced by: '/Gain' - - real32_T Gain_Gain_m2; // Computed Parameter: Gain_Gain_m2 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_bs; // Computed Parameter: Gain1_Gain_bs - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_o; // Computed Parameter: Gain2_Gain_o - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_ed; // Computed Parameter: Gain1_Gain_ed - // Referenced by: '/Gain1' - - real32_T Gain_Gain_hg; // Computed Parameter: Gain_Gain_hg - // Referenced by: '/Gain' - - uint32_T Delay11_DelayLength; // Computed Parameter: Delay11_DelayLength - // Referenced by: '/Delay11' - - uint32_T Delay11_DelayLength_g; // Computed Parameter: Delay11_DelayLength_g - // Referenced by: '/Delay11' - - P_ForEachSubsystem_ctl_contro_T ForEachSubsystem1;// '/For Each Subsystem1' - P_ForEachSubsystem_ctl_contro_T ForEachSubsystem;// '/For Each Subsystem' - P_Normalize_ctl_controller0_T Normalize_k;// '/Normalize' - P_Normalize_ctl_controller0_T Normalize_eo;// '/Normalize' - P_Normalize_ctl_controller0_T Normalize_l;// '/Normalize' - P_Normalize_ctl_controller0_T Normalize;// '/Normalize' -}; - -// Real-time Model Data Structure -struct tag_RTM_ctl_controller0_T { - const char_T * volatile errorStatus; - P_ctl_controller0_T *defaultParam; - boolean_T paramIsMalloced; - DW_ctl_controller0_T *dwork; -}; - -#ifdef __cplusplus - -extern "C" { - -#endif - -#ifdef __cplusplus - -} -#endif - -// External data declarations for dependent source files -extern const ctl_input_msg ctl_controller0_rtZctl_input_msg;// ctl_input_msg ground -extern const cmd_msg ctl_controller0_rtZcmd_msg;// cmd_msg ground -extern const ctl_msg ctl_controller0_rtZctl_msg;// ctl_msg ground - -#ifdef __cplusplus - -extern "C" { - -#endif - - extern const char *RT_MEMORY_ALLOCATION_ERROR; - -#ifdef __cplusplus - -} -#endif - -extern P_ctl_controller0_T ctl_controller0_P;// parameters - -#ifdef __cplusplus - -extern "C" { - -#endif - - // Model entry point functions - extern RT_MODEL_ctl_controller0_T *ctl_controller0(ctl_input_msg - *ctl_controller0_U_ctl_input_msg_l, cmd_msg *ctl_controller0_Y_cmd_msg_f, - ctl_msg *ctl_controller0_Y_ctl_msg_n); - extern void ctl_controller0_initialize(RT_MODEL_ctl_controller0_T *const - ctl_controller0_M, ctl_input_msg *ctl_controller0_U_ctl_input_msg_l, cmd_msg - *ctl_controller0_Y_cmd_msg_f, ctl_msg *ctl_controller0_Y_ctl_msg_n); - extern void ctl_controller0_step(RT_MODEL_ctl_controller0_T *const - ctl_controller0_M, ctl_input_msg *ctl_controller0_U_ctl_input_msg_l, cmd_msg - *ctl_controller0_Y_cmd_msg_f, ctl_msg *ctl_controller0_Y_ctl_msg_n); - extern void ctl_controller0_terminate(RT_MODEL_ctl_controller0_T - * ctl_controller0_M); - -#ifdef __cplusplus - -} -#endif - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Note that this particular code originates from a subsystem build, -// and has its own system numbers different from the parent model. -// Refer to the system hierarchy for this subsystem below, and use the -// MATLAB hilite_system command to trace the generated code back -// to the parent model. For example, -// -// hilite_system('astrobee/fsw_lib/ctl_controller') - opens subsystem astrobee/fsw_lib/ctl_controller -// hilite_system('astrobee/fsw_lib/ctl_controller/Kp') - opens and selects block Kp -// -// Here is the system hierarchy for this model -// -// '' : 'astrobee/fsw_lib' -// '' : 'astrobee/fsw_lib/ctl_controller' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Compare To Constant1' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Compare To Constant2' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Compare To Constant3' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Compare To Constant4' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Compare To Constant5' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Compare To Constant6' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Compare To Constant7' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Filter' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/Filter1' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/Quaternion Inverse' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/Quaternion_Multiplication' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cex_control_executive/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/Compare To Constant' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/Compare To Constant1' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/Compare To Constant2' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B1' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/safe_divide' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/safe_divide2' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/saturate_vector_mag' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B1/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B1/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B1/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B1/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/rotate_vec_A2B1/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/saturate_vector_mag/vector_magnitude1' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/saturate_vector_mag/vector_normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/saturate_vector_mag/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/saturate_vector_mag/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/linear control/saturate_vector_mag/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/Compare To Constant' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/Compare To Constant1' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/Compare To Constant2' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/cross_product' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/safe_divide' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/safe_divide1' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/cross_product/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/cross_product/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/Quaternion Inverse' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/Quaternion_Multiplication' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/clc_closed_loop_controller_lib/rotational control/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/cmd_selector' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_path' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/MATLAB Function' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/create_omega_matrix' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/create_omega_matrix1' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/generate_cmd_attitude/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/vector_magnitude' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/vector_magnitude1' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/vector_magnitude2' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/Quaternion Inverse' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/Quaternion_Multiplication' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/ctl_controller/cmd_command_shaper/traj_errors/quaternion_error1/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/Compute Rates From Quat' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/Discrete Derivative' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/For Each Subsystem' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/For Each Subsystem1' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/Compute Rates From Quat/Discrete Derivative' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/Compute Rates From Quat/Quaternion Xi' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/Compute Rates From Quat/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/Compute Rates From Quat/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/ctl_controller/debug_derive_rates/Compute Rates From Quat/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' - -#endif // RTW_HEADER_ctl_controller0_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_data.cpp b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_data.cpp deleted file mode 100644 index 5f139deb23..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_data.cpp +++ /dev/null @@ -1,541 +0,0 @@ -// -// File: ctl_controller0_data.cpp -// -// Code generated for Simulink model 'ctl_controller0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:53 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "ctl_controller0.h" -#include "ctl_controller0_private.h" - -// Block parameters (auto storage) -P_ctl_controller0_T ctl_controller0_P = { - // Variable: tun_accel_gain - // Referenced by: '/Gain' - - { 1.0F, 1.0F, 1.0F }, - - // Variable: tun_alpha_gain - // Referenced by: '/Gain' - - { 1.0F, 1.0F, 1.0F }, - -0.5F, // Variable: tun_ctl_att_sat_lower - // Referenced by: '/Discrete-Time Integrator' - - 0.5F, // Variable: tun_ctl_att_sat_upper - // Referenced by: '/Discrete-Time Integrator' - - 100.0F, // Variable: tun_ctl_linear_force_limit - // Referenced by: '/Constant2' - - -0.1F, // Variable: tun_ctl_pos_sat_lower - // Referenced by: '/Discrete-Time Integrator1' - - 0.1F, // Variable: tun_ctl_pos_sat_upper - // Referenced by: '/Discrete-Time Integrator1' - - 0.1F, // Variable: tun_ctl_stopped_pos_thresh - // Referenced by: '/Constant' - - 0.174533F, // Variable: tun_ctl_stopped_quat_thresh - // Referenced by: '/Constant' - - 0.0004F, // Variable: tun_ctl_stopping_omega_thresh - // Referenced by: '/Constant' - - 0.0004F, // Variable: tun_ctl_stopping_vel_thresh - // Referenced by: '/Constant' - - 0.0F, // Variable: tun_truth_q_omega_filt_enable - // Referenced by: '/For Each Subsystem' - - 0.0F, // Variable: tun_truth_velocity_filt_enable - // Referenced by: '/For Each Subsystem1' - - 0U, // Variable: ase_status_converged - // Referenced by: '/Constant' - - 0U, // Variable: ctl_idle_mode - // Referenced by: '/Constant4' - - 3U, // Variable: ctl_stopped_mode - // Referenced by: - // '/Constant' - // '/Constant' - - 1U, // Variable: ctl_stopping_mode - // Referenced by: - // '/Constant6' - // '/Constant' - - 0U, // Variable: tun_debug_ctl_use_truth - // Referenced by: '/Constant' - - 0.0F, // Mask Parameter: DiscreteDerivative_ICPrevScaled - // Referenced by: '/UD' - - 0.0F, // Mask Parameter: DiscreteDerivative_ICPrevScal_k - // Referenced by: '/UD' - - 1U, // Mask Parameter: CompareToConstant2_const - // Referenced by: '/Constant' - - 1U, // Mask Parameter: CompareToConstant1_const - // Referenced by: '/Constant' - - 1U, // Mask Parameter: CompareToConstant_const - // Referenced by: '/Constant' - - 1U, // Mask Parameter: CompareToConstant2_const_o - // Referenced by: '/Constant' - - 1U, // Mask Parameter: CompareToConstant1_const_m - // Referenced by: '/Constant' - - 1U, // Mask Parameter: CompareToConstant_const_f - // Referenced by: '/Constant' - - 0U, // Mask Parameter: TappedDelay_vinit - // Referenced by: '/Tapped Delay' - - - // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - - // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - 0.0031317642291927056, // Expression: 0.0031317642291927056 - // Referenced by: '/s(1)' - - -0.9937364715416146, // Expression: -0.993736471541614597 - // Referenced by: '/a(2)(1)' - - 0.0031317642291927056, // Expression: 0.0031317642291927056 - // Referenced by: '/s(1)' - - -0.9937364715416146, // Expression: -0.993736471541614597 - // Referenced by: '/a(2)(1)' - - - // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, // Expression: 1 - // Referenced by: '/Constant1' - - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - - // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, // Expression: 1 - // Referenced by: '/Constant1' - - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - - // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - - // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, // Expression: 0 - // Referenced by: '/Constant3' - - -1.0F, // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - -1.0F, // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain_Gain_p - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_e - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain_k - // Referenced by: '/Gain2' - - -1.0F, // Computed Parameter: Gain1_Gain_o - // Referenced by: '/Gain1' - - 2.0F, // Computed Parameter: Gain_Gain_m - // Referenced by: '/Gain' - - 62.5F, // Computed Parameter: TSamp_WtEt - // Referenced by: '/TSamp' - - 62.5F, // Computed Parameter: TSamp_WtEt_k - // Referenced by: '/TSamp' - - 0.0F, // Computed Parameter: UnitDelay2_InitialCondition - // Referenced by: '/Unit Delay2' - - -1.0F, // Computed Parameter: Gain_Gain_a - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain_Gain_i - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_n - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain_a - // Referenced by: '/Gain2' - - -1.0F, // Computed Parameter: Gain1_Gain_j - // Referenced by: '/Gain1' - - 2.0F, // Computed Parameter: Gain2_Gain_h - // Referenced by: '/Gain2' - - 0.0F, // Computed Parameter: Delay11_InitialCondition - // Referenced by: '/Delay11' - - 0.0F, // Computed Parameter: Delay11_InitialCondition_k - // Referenced by: '/Delay11' - - 0.5F, // Expression: single(0.5) - // Referenced by: '/Constant' - - 1.0E-9F, // Expression: single(1e-9) - // Referenced by: '/Gain' - - 0.5F, // Computed Parameter: Constant1_Value_g - // Referenced by: '/Constant1' - - 0.5F, // Computed Parameter: Constant3_Value_d - // Referenced by: '/Constant3' - - 0.0F, // Computed Parameter: Constant3_Value_dz - // Referenced by: '/Constant3' - - -1.0F, // Computed Parameter: Gain_Gain_n - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_b - // Referenced by: '/Gain1' - - 0.0F, // Computed Parameter: Constant2_Value_d - // Referenced by: '/Constant2' - - -1.0F, // Computed Parameter: Gain2_Gain_kx - // Referenced by: '/Gain2' - - -1.0F, // Computed Parameter: Gain3_Gain - // Referenced by: '/Gain3' - - -1.0F, // Computed Parameter: Gain4_Gain - // Referenced by: '/Gain4' - - 0.0F, // Computed Parameter: Constant1_Value_l - // Referenced by: '/Constant1' - - -1.0F, // Computed Parameter: Gain5_Gain - // Referenced by: '/Gain5' - - 0.0F, // Computed Parameter: Constant_Value_g - // Referenced by: '/Constant' - - 0.0F, // Computed Parameter: Constant3_Value_f - // Referenced by: '/Constant3' - - -1.0F, // Computed Parameter: Gain_Gain_g - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_ja - // Referenced by: '/Gain1' - - 0.0F, // Computed Parameter: Constant2_Value_gq - // Referenced by: '/Constant2' - - -1.0F, // Computed Parameter: Gain2_Gain_c - // Referenced by: '/Gain2' - - -1.0F, // Computed Parameter: Gain3_Gain_o - // Referenced by: '/Gain3' - - -1.0F, // Computed Parameter: Gain4_Gain_c - // Referenced by: '/Gain4' - - 0.0F, // Computed Parameter: Constant1_Value_gk - // Referenced by: '/Constant1' - - -1.0F, // Computed Parameter: Gain5_Gain_a - // Referenced by: '/Gain5' - - 0.0F, // Computed Parameter: Constant_Value_m - // Referenced by: '/Constant' - - 48.0F, // Computed Parameter: Constant2_Value_it - // Referenced by: '/Constant2' - - 0.0F, // Computed Parameter: UnitDelay1_InitialCondition - // Referenced by: '/Unit Delay1' - - - // Expression: single([0 0 0]) - // Referenced by: '/Constant1' - - { 0.0F, 0.0F, 0.0F }, - - // Expression: single([0 0 0]) - // Referenced by: '/Constant2' - - { 0.0F, 0.0F, 0.0F }, - - // Expression: single([0 0 0]) - // Referenced by: '/Constant3' - - { 0.0F, 0.0F, 0.0F }, - - // Expression: single([0 0 0]) - // Referenced by: '/Constant5' - - { 0.0F, 0.0F, 0.0F }, - 2.0F, // Computed Parameter: Gain_Gain_l - // Referenced by: '/Gain' - - 2.0F, // Computed Parameter: Gain1_Gain_ej - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain_Gain_ib - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_k - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain_b - // Referenced by: '/Gain2' - - 2.0F, // Computed Parameter: Gain2_Gain_k1 - // Referenced by: '/Gain2' - - 2.0F, // Computed Parameter: Gain_Gain_b - // Referenced by: '/Gain' - - 2.0F, // Computed Parameter: Gain1_Gain_g - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain_Gain_d - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_ji - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain_bs - // Referenced by: '/Gain2' - - 2.0F, // Computed Parameter: Gain2_Gain_kh - // Referenced by: '/Gain2' - - - // Expression: single([0 0 0]) - // Referenced by: '/Constant4' - - { 0.0F, 0.0F, 0.0F }, - - // Expression: single([0 0 0]) - // Referenced by: '/Constant1' - - { 0.0F, 0.0F, 0.0F }, - 0.016F, // Computed Parameter: DiscreteTimeIntegrator1_gainval - // Referenced by: '/Discrete-Time Integrator1' - - - // Expression: single([0 0 0]) - // Referenced by: '/Discrete-Time Integrator1' - - { 0.0F, 0.0F, 0.0F }, - - // Expression: single([0 0 0]) - // Referenced by: '/Constant3' - - { 0.0F, 0.0F, 0.0F }, - - // Expression: single([0 0 0]) - // Referenced by: '/Constant2' - - { 0.0F, 0.0F, 0.0F }, - - // Expression: single([0 0 0]) - // Referenced by: '/Constant1' - - { 0.0F, 0.0F, 0.0F }, - -1.0F, // Computed Parameter: Gain_Gain_pf - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain_Gain_k - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_jz - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain_n - // Referenced by: '/Gain2' - - -1.0F, // Computed Parameter: Gain1_Gain_od - // Referenced by: '/Gain1' - - 0.016F, // Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - - // Expression: single([0 0 0]) - // Referenced by: '/Discrete-Time Integrator' - - { 0.0F, 0.0F, 0.0F }, - - // Expression: single([0 0 0]) - // Referenced by: '/Constant4' - - { 0.0F, 0.0F, 0.0F }, - 2.0F, // Computed Parameter: Gain2_Gain_j - // Referenced by: '/Gain2' - - -1.0F, // Computed Parameter: Gain_Gain_is - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain_Gain_m2 - // Referenced by: '/Gain' - - -1.0F, // Computed Parameter: Gain1_Gain_bs - // Referenced by: '/Gain1' - - -1.0F, // Computed Parameter: Gain2_Gain_o - // Referenced by: '/Gain2' - - -1.0F, // Computed Parameter: Gain1_Gain_ed - // Referenced by: '/Gain1' - - 2.0F, // Computed Parameter: Gain_Gain_hg - // Referenced by: '/Gain' - - 1U, // Computed Parameter: Delay11_DelayLength - // Referenced by: '/Delay11' - - 1U, // Computed Parameter: Delay11_DelayLength_g - // Referenced by: '/Delay11' - - - // Start of '/For Each Subsystem1' - { - // Start of '/CoreSubsys' - { - // Expression: [omega*T/(2+omega*T) omega*T/(2+omega*T)] - // Referenced by: '/3 Hz Low Pass' - - { 0.00209001475F, 0.00209001475F }, - - // Expression: [1 (omega*T-2)/(2+omega*T)] - // Referenced by: '/3 Hz Low Pass' - - { 1.0F, -0.99582F }, - 0.0F // Computed Parameter: uHzLowPass_InitialStates - // Referenced by: '/3 Hz Low Pass' - - } - // End of '/CoreSubsys' - } - // End of '/For Each Subsystem1' - , - - // Start of '/For Each Subsystem' - { - // Start of '/CoreSubsys' - { - // Expression: [omega*T/(2+omega*T) omega*T/(2+omega*T)] - // Referenced by: '/3 Hz Low Pass' - - { 0.00209001475F, 0.00209001475F }, - - // Expression: [1 (omega*T-2)/(2+omega*T)] - // Referenced by: '/3 Hz Low Pass' - - { 1.0F, -0.99582F }, - 0.0F // Computed Parameter: uHzLowPass_InitialStates - // Referenced by: '/3 Hz Low Pass' - - } - // End of '/CoreSubsys' - } - // End of '/For Each Subsystem' - , - - // Start of '/Normalize' - { - -1.0 // Expression: -1 - // Referenced by: '/Constant1' - - } - // End of '/Normalize' - , - - // Start of '/Normalize' - { - -1.0 // Expression: -1 - // Referenced by: '/Constant1' - - } - // End of '/Normalize' - , - - // Start of '/Normalize' - { - -1.0 // Expression: -1 - // Referenced by: '/Constant1' - - } - // End of '/Normalize' - , - - // Start of '/Normalize' - { - -1.0 // Expression: -1 - // Referenced by: '/Constant1' - - } - // End of '/Normalize' -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_private.h b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_private.h deleted file mode 100644 index d819c4a4d1..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_private.h +++ /dev/null @@ -1,60 +0,0 @@ -// -// File: ctl_controller0_private.h -// -// Code generated for Simulink model 'ctl_controller0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:53 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_ctl_controller0_private_h_ -#define RTW_HEADER_ctl_controller0_private_h_ -#include "rtwtypes.h" -#include "ctl_controller0.h" -#if !defined(rt_VALIDATE_MEMORY) -#define rt_VALIDATE_MEMORY(S, ptr) if(!(ptr)) {\ - rtmSetErrorStatus(ctl_controller0_M, RT_MEMORY_ALLOCATION_ERROR);\ - } -#endif - -#if !defined(rt_FREE) -#if !defined(_WIN32) -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((ptr));\ - (ptr) = (NULL);\ - } -#else - -// Visual and other windows compilers declare free without const -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((void *)(ptr));\ - (ptr) = (NULL);\ - } -#endif -#endif - -void ctl_controller0_Normalize(const real32_T rtu_q_in[4], real32_T - rty_positive_scalar_q[4], P_Normalize_ctl_controller0_T *localP); -void ctl_controller0_Normalize_e(const real32_T rtu_Vec[4], real32_T - rtu_Magnitude, real32_T rty_Normalized_Vec[4]); -void ctl_contr_ForEachSubsystem_Init(int32_T NumIters, - DW_ForEachSubsystem_ctl_contr_T localDW[3], P_ForEachSubsystem_ctl_contro_T - *localP); -void ctl_cont_ForEachSubsystem_Start(int32_T NumIters, - DW_ForEachSubsystem_ctl_contr_T localDW[3]); -void ctl_controller_ForEachSubsystem(int32_T NumIters, const real32_T rtu_X[3], - real32_T rty_Y[3], DW_ForEachSubsystem_ctl_contr_T localDW[3], - P_ForEachSubsystem_ctl_contro_T *localP, real32_T rtp_filt_enable); - -#endif // RTW_HEADER_ctl_controller0_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_types.h b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_types.h deleted file mode 100644 index f842705c36..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_types.h +++ /dev/null @@ -1,133 +0,0 @@ -// -// File: ctl_controller0_types.h -// -// Code generated for Simulink model 'ctl_controller0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:53 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_ctl_controller0_types_h_ -#define RTW_HEADER_ctl_controller0_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_cmc_state_cmd_ -#define DEFINED_TYPEDEF_FOR_cmc_state_cmd_ - -typedef struct { - uint32_T timestamp_sec; - uint32_T timestamp_nsec; - real32_T P_B_ISS_ISS[3]; - real32_T V_B_ISS_ISS[3]; - real32_T A_B_ISS_ISS[3]; - real32_T quat_ISS2B[4]; - real32_T omega_B_ISS_B[3]; - real32_T alpha_B_ISS_B[3]; -} cmc_state_cmd; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ctl_input_msg_ -#define DEFINED_TYPEDEF_FOR_ctl_input_msg_ - -typedef struct { - real32_T est_quat_ISS2B[4]; - real32_T est_omega_B_ISS_B[3]; - real32_T est_V_B_ISS_ISS[3]; - real32_T est_P_B_ISS_ISS[3]; - uint8_T est_confidence; - cmc_state_cmd cmd_state_a; - cmc_state_cmd cmd_state_b; - uint8_T ctl_mode_cmd; - uint32_T current_time_sec; - uint32_T current_time_nsec; - uint8_T speed_gain_cmd; - real32_T att_kp[3]; - real32_T att_ki[3]; - real32_T omega_kd[3]; - real32_T pos_kp[3]; - real32_T pos_ki[3]; - real32_T vel_kd[3]; - real32_T inertia_matrix[9]; - real32_T mass; -} ctl_input_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmd_msg_ -#define DEFINED_TYPEDEF_FOR_cmd_msg_ - -typedef struct { - uint32_T cmd_timestamp_sec; - uint32_T cmd_timestamp_nsec; - uint8_T cmd_mode; - uint8_T speed_gain_cmd; - uint8_T cmd_B_inuse; - real32_T traj_pos[3]; - real32_T traj_vel[3]; - real32_T traj_accel[3]; - real32_T traj_quat[4]; - real32_T traj_omega[3]; - real32_T traj_alpha[3]; -} cmd_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ctl_msg_ -#define DEFINED_TYPEDEF_FOR_ctl_msg_ - -typedef struct { - real32_T body_force_cmd[3]; - real32_T body_accel_cmd[3]; - real32_T pos_err[3]; - real32_T pos_err_int[3]; - real32_T body_torque_cmd[3]; - real32_T body_alpha_cmd[3]; - real32_T att_err[3]; - real32_T att_err_mag; - real32_T att_err_int[3]; - uint8_T ctl_status; - real32_T traj_error_pos; - real32_T traj_error_att; - real32_T traj_error_vel; - real32_T traj_error_omega; -} ctl_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ex_time_msg_ -#define DEFINED_TYPEDEF_FOR_ex_time_msg_ - -typedef struct { - uint32_T timestamp_sec; - uint32_T timestamp_nsec; -} ex_time_msg; - -#endif - -// Parameters for system: '/Normalize' -typedef struct P_Normalize_ctl_controller0_T_ P_Normalize_ctl_controller0_T; - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_ctl_controller0_T_ P_CoreSubsys_ctl_controller0_T; - -// Parameters for system: '/For Each Subsystem' -typedef struct P_ForEachSubsystem_ctl_contro_T_ P_ForEachSubsystem_ctl_contro_T; - -// Parameters (auto storage) -typedef struct P_ctl_controller0_T_ P_ctl_controller0_T; - -// Forward declaration for rtModel -typedef struct tag_RTM_ctl_controller0_T RT_MODEL_ctl_controller0_T; - -#endif // RTW_HEADER_ctl_controller0_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_tunable_funcs.cpp b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_tunable_funcs.cpp deleted file mode 100644 index f05153be26..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_tunable_funcs.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// Autogenerated file, see write_tunable_param_functions for details - -#include "ctl_tunable_funcs.h" -#include -#include -#include - -// Do config setup and add includes -void ctl_ReadParams(config_reader::ConfigReader* config, RT_MODEL_ctl_controller0_T* ctl) { - - auto & p = ctl->defaultParam; - - if (!msg_conversions::config_read_array(config, "tun_accel_gain", 3 , p->tun_accel_gain)) - ROS_FATAL("Unspecified tun_accel_gain."); - if (!msg_conversions::config_read_array(config, "tun_alpha_gain", 3 , p->tun_alpha_gain)) - ROS_FATAL("Unspecified tun_alpha_gain."); - if (!config->GetReal("tun_ctl_att_sat_lower", &p->tun_ctl_att_sat_lower)) - ROS_FATAL("Unspecified tun_ctl_att_sat_lower."); - if (!config->GetReal("tun_ctl_att_sat_upper", &p->tun_ctl_att_sat_upper)) - ROS_FATAL("Unspecified tun_ctl_att_sat_upper."); - if (!config->GetReal("tun_ctl_linear_force_limit", &p->tun_ctl_linear_force_limit)) - ROS_FATAL("Unspecified tun_ctl_linear_force_limit."); - if (!config->GetReal("tun_ctl_pos_sat_lower", &p->tun_ctl_pos_sat_lower)) - ROS_FATAL("Unspecified tun_ctl_pos_sat_lower."); - if (!config->GetReal("tun_ctl_pos_sat_upper", &p->tun_ctl_pos_sat_upper)) - ROS_FATAL("Unspecified tun_ctl_pos_sat_upper."); - if (!config->GetReal("tun_ctl_stopped_pos_thresh", &p->tun_ctl_stopped_pos_thresh)) - ROS_FATAL("Unspecified tun_ctl_stopped_pos_thresh."); - if (!config->GetReal("tun_ctl_stopped_quat_thresh", &p->tun_ctl_stopped_quat_thresh)) - ROS_FATAL("Unspecified tun_ctl_stopped_quat_thresh."); - if (!config->GetReal("tun_ctl_stopping_omega_thresh", &p->tun_ctl_stopping_omega_thresh)) - ROS_FATAL("Unspecified tun_ctl_stopping_omega_thresh."); - if (!config->GetReal("tun_ctl_stopping_vel_thresh", &p->tun_ctl_stopping_vel_thresh)) - ROS_FATAL("Unspecified tun_ctl_stopping_vel_thresh."); - if (!config->GetReal("tun_truth_q_omega_filt_enable", &p->tun_truth_q_omega_filt_enable)) - ROS_FATAL("Unspecified tun_truth_q_omega_filt_enable."); - if (!config->GetReal("tun_truth_velocity_filt_enable", &p->tun_truth_velocity_filt_enable)) - ROS_FATAL("Unspecified tun_truth_velocity_filt_enable."); - bool debug_ctl_use_truth; - if (!config->GetBool("tun_debug_ctl_use_truth", &debug_ctl_use_truth)) - ROS_FATAL("Unspecified tun_debug_ctl_use_truth."); - p->tun_debug_ctl_use_truth = debug_ctl_use_truth; -} diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_tunable_funcs.h b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_tunable_funcs.h deleted file mode 100644 index 79e30bd748..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_tunable_funcs.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef READ_PARAMS_CPP -#define READ_PARAMS_CPP - -namespace config_reader { - class ConfigReader; -} - -#include "ctl_controller0.h" -void ctl_ReadParams(config_reader::ConfigReader* config, RT_MODEL_ctl_controller0_T* ctl); - -#endif \ No newline at end of file diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/defines.txt b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/defines.txt deleted file mode 100644 index d9c3df7629..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/defines.txt +++ /dev/null @@ -1,15 +0,0 @@ -RT_MALLOC -MODEL=ctl_controller0 -NUMST=1 -NCSTATES=0 -HAVESTDIO -UNIX -ONESTEPFCN=1 -TERMFCN=1 -MAT_FILE=0 -MULTI_INSTANCE_CODE=1 -INTEGER_CODE=0 -MT=0 -CLASSIC_INTERFACE=0 -ALLOCATIONFCN=1 -TID01EQ=0 diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ert_main.cpp b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ert_main.cpp deleted file mode 100644 index cc25a25ba0..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ert_main.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// -// File: ert_main.cpp -// -// Code generated for Simulink model 'ctl_controller0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:53 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include -#include // This ert_main.c example uses printf/fflush -#include "ctl_controller0.h" // Model's header file -#include "rtwtypes.h" - -// '/ctl_input_msg' -static ctl_input_msg ctl_controller0_U_ctl_input_msg_l; - -// '/cmd_msg' -static cmd_msg ctl_controller0_Y_cmd_msg_f; - -// '/ctl_msg' -static ctl_msg ctl_controller0_Y_ctl_msg_n; -const char *RT_MEMORY_ALLOCATION_ERROR = "memory allocation error"; - -// -// Associating rt_OneStep with a real-time clock or interrupt service routine -// is what makes the generated code "real-time". The function rt_OneStep is -// always associated with the base rate of the model. Subrates are managed -// by the base rate from inside the generated code. Enabling/disabling -// interrupts and floating point context switches are target specific. This -// example code indicates where these should take place relative to executing -// the generated code step function. Overrun behavior should be tailored to -// your application needs. This example simply sets an error status in the -// real-time model and returns from rt_OneStep. -// -void rt_OneStep(RT_MODEL_ctl_controller0_T *const ctl_controller0_M); -void rt_OneStep(RT_MODEL_ctl_controller0_T *const ctl_controller0_M) -{ - static boolean_T OverrunFlag = false; - - // Disable interrupts here - - // Check for overrun - if (OverrunFlag) { - rtmSetErrorStatus(ctl_controller0_M, "Overrun"); - return; - } - - OverrunFlag = true; - - // Save FPU context here (if necessary) - // Re-enable timer or interrupt here - // Set model inputs here - - // Step the model - ctl_controller0_step(ctl_controller0_M, &ctl_controller0_U_ctl_input_msg_l, - &ctl_controller0_Y_cmd_msg_f, - &ctl_controller0_Y_ctl_msg_n); - - // Get model outputs here - - // Indicate task complete - OverrunFlag = false; - - // Disable interrupts here - // Restore FPU context here (if necessary) - // Enable interrupts here -} - -// -// The example "main" function illustrates what is required by your -// application code to initialize, execute, and terminate the generated code. -// Attaching rt_OneStep to a real-time clock is target specific. This example -// illustrates how you do this relative to initializing the model. -// -int_T main(int_T argc, const char *argv[]) -{ - RT_MODEL_ctl_controller0_T *ctl_controller0_M; - - // Unused arguments - (void)(argc); - (void)(argv); - - // Allocate model data - ctl_controller0_M = ctl_controller0(&ctl_controller0_U_ctl_input_msg_l, - &ctl_controller0_Y_cmd_msg_f, &ctl_controller0_Y_ctl_msg_n); - if (ctl_controller0_M == NULL) { - (void)fprintf(stderr,"Memory allocation error during model " - "registration"); - return(1); - } - - if (rtmGetErrorStatus(ctl_controller0_M) != NULL) { - (void)fprintf(stderr,"Error during model registration: %s\n", - rtmGetErrorStatus(ctl_controller0_M)); - - // Disable rt_OneStep() here - - // Terminate model - ctl_controller0_terminate(ctl_controller0_M); - return(1); - } - - // Initialize model - ctl_controller0_initialize(ctl_controller0_M, - &ctl_controller0_U_ctl_input_msg_l, &ctl_controller0_Y_cmd_msg_f, - &ctl_controller0_Y_ctl_msg_n); - - // Attach rt_OneStep to a timer or interrupt service routine with - // period 0.016 seconds (the model's base sample time) here. The - // call syntax for rt_OneStep is - // - // rt_OneStep(ctl_controller0_M); - - printf("Warning: The simulation will run forever. " - "Generated ERT main won't simulate model step behavior. " - "To change this behavior select the 'MAT-file logging' option.\n"); - fflush((NULL)); - while (rtmGetErrorStatus(ctl_controller0_M) == (NULL)) { - // Perform other application tasks here - } - - // Disable rt_OneStep() here - - // Terminate model - ctl_controller0_terminate(ctl_controller0_M); - return 0; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/modelsources.txt b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/modelsources.txt deleted file mode 100644 index 5bf1dbea94..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/modelsources.txt +++ /dev/null @@ -1 +0,0 @@ - ctl_controller0.cpp diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/rtw_proj.tmw b/gnc/matlab/code_generation/ctl_controller0_ert_rtw/rtw_proj.tmw deleted file mode 100644 index 5c7e108d7c..0000000000 --- a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/rtw_proj.tmw +++ /dev/null @@ -1,4 +0,0 @@ -Simulink Coder project for ctl_controller0 using . MATLAB root = /usr/local/MATLAB/R2016b. SimStruct date: 22-Jul-2016 13:46:14 -This file is generated by Simulink Coder for use by the make utility -to determine when to rebuild objects when the name of the current Simulink Coder project changes. -The rtwinfomat located at: ../slprj/ert/ctl_controller0/tmwinternal/binfo.mat diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/buildInfo.mat b/gnc/matlab/code_generation/est_estimator_ert_rtw/buildInfo.mat deleted file mode 100644 index 621d550a2a..0000000000 Binary files a/gnc/matlab/code_generation/est_estimator_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/codeInfo.mat b/gnc/matlab/code_generation/est_estimator_ert_rtw/codeInfo.mat deleted file mode 100644 index 359f7838b5..0000000000 Binary files a/gnc/matlab/code_generation/est_estimator_ert_rtw/codeInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/defines.txt b/gnc/matlab/code_generation/est_estimator_ert_rtw/defines.txt deleted file mode 100644 index 3d2edf20c6..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/defines.txt +++ /dev/null @@ -1,15 +0,0 @@ -RT_MALLOC -MODEL=est_estimator -NUMST=1 -NCSTATES=0 -HAVESTDIO -UNIX -ONESTEPFCN=1 -TERMFCN=1 -MAT_FILE=0 -MULTI_INSTANCE_CODE=1 -INTEGER_CODE=0 -MT=0 -CLASSIC_INTERFACE=0 -ALLOCATIONFCN=1 -TID01EQ=0 diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/ert_main.cpp b/gnc/matlab/code_generation/est_estimator_ert_rtw/ert_main.cpp deleted file mode 100644 index 10e2381df9..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/ert_main.cpp +++ /dev/null @@ -1,165 +0,0 @@ -// -// File: ert_main.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include -#include // This ert_main.c example uses printf/fflush -#include "est_estimator.h" // Model's header file -#include "rtwtypes.h" - -// '/landmark_msg' -static cvs_landmark_msg est_estimator_U_landmark_msg; - -// '/Vision Registration' -static cvs_registration_pulse est_estimator_U_VisionRegistration; - -// '/optical_flow_msg' -static cvs_optical_flow_msg est_estimator_U_cvs_optical_flow_msg_n; - -// '/handrail_msg' -static cvs_handrail_msg est_estimator_U_handrail_msg; - -// '/imu_msg' -static imu_msg est_estimator_U_imu_msg_c; - -// '/cmc_msg' -static cmc_msg est_estimator_U_cmc_msg_o; - -// '/true_q_ISS2B' -static real32_T est_estimator_U_Q_ISS2B[4]; - -// '/kfl_msg' -static kfl_msg est_estimator_Y_kfl_msg_h; - -// '/P_out' -static ase_cov_datatype est_estimator_Y_P_out[13689]; -const char *RT_MEMORY_ALLOCATION_ERROR = "memory allocation error"; - -// -// Associating rt_OneStep with a real-time clock or interrupt service routine -// is what makes the generated code "real-time". The function rt_OneStep is -// always associated with the base rate of the model. Subrates are managed -// by the base rate from inside the generated code. Enabling/disabling -// interrupts and floating point context switches are target specific. This -// example code indicates where these should take place relative to executing -// the generated code step function. Overrun behavior should be tailored to -// your application needs. This example simply sets an error status in the -// real-time model and returns from rt_OneStep. -// -void rt_OneStep(RT_MODEL_est_estimator_T *const est_estimator_M); -void rt_OneStep(RT_MODEL_est_estimator_T *const est_estimator_M) -{ - static boolean_T OverrunFlag = false; - - // Disable interrupts here - - // Check for overrun - if (OverrunFlag) { - rtmSetErrorStatus(est_estimator_M, "Overrun"); - return; - } - - OverrunFlag = true; - - // Save FPU context here (if necessary) - // Re-enable timer or interrupt here - // Set model inputs here - - // Step the model - est_estimator_step(est_estimator_M, &est_estimator_U_landmark_msg, - &est_estimator_U_VisionRegistration, - &est_estimator_U_cvs_optical_flow_msg_n, - &est_estimator_U_handrail_msg, &est_estimator_U_imu_msg_c, - &est_estimator_U_cmc_msg_o, est_estimator_U_Q_ISS2B, - &est_estimator_Y_kfl_msg_h, est_estimator_Y_P_out); - - // Get model outputs here - - // Indicate task complete - OverrunFlag = false; - - // Disable interrupts here - // Restore FPU context here (if necessary) - // Enable interrupts here -} - -// -// The example "main" function illustrates what is required by your -// application code to initialize, execute, and terminate the generated code. -// Attaching rt_OneStep to a real-time clock is target specific. This example -// illustrates how you do this relative to initializing the model. -// -int_T main(int_T argc, const char *argv[]) -{ - RT_MODEL_est_estimator_T *est_estimator_M; - - // Unused arguments - (void)(argc); - (void)(argv); - - // Allocate model data - est_estimator_M = est_estimator(&est_estimator_U_landmark_msg, - &est_estimator_U_VisionRegistration, &est_estimator_U_cvs_optical_flow_msg_n, - &est_estimator_U_handrail_msg, &est_estimator_U_imu_msg_c, - &est_estimator_U_cmc_msg_o, est_estimator_U_Q_ISS2B, - &est_estimator_Y_kfl_msg_h, est_estimator_Y_P_out); - if (est_estimator_M == NULL) { - (void)fprintf(stderr,"Memory allocation error during model " - "registration"); - return(1); - } - - if (rtmGetErrorStatus(est_estimator_M) != NULL) { - (void)fprintf(stderr,"Error during model registration: %s\n", - rtmGetErrorStatus(est_estimator_M)); - - // Disable rt_OneStep() here - - // Terminate model - est_estimator_terminate(est_estimator_M); - return(1); - } - - // Initialize model - est_estimator_initialize(est_estimator_M, &est_estimator_U_landmark_msg, - &est_estimator_U_VisionRegistration, &est_estimator_U_cvs_optical_flow_msg_n, - &est_estimator_U_handrail_msg, &est_estimator_U_imu_msg_c, - &est_estimator_U_cmc_msg_o, est_estimator_U_Q_ISS2B, - &est_estimator_Y_kfl_msg_h, est_estimator_Y_P_out); - - // Attach rt_OneStep to a timer or interrupt service routine with - // period 0.016 seconds (the model's base sample time) here. The - // call syntax for rt_OneStep is - // - // rt_OneStep(est_estimator_M); - - printf("Warning: The simulation will run forever. " - "Generated ERT main won't simulate model step behavior. " - "To change this behavior select the 'MAT-file logging' option.\n"); - fflush((NULL)); - while (rtmGetErrorStatus(est_estimator_M) == (NULL)) { - // Perform other application tasks here - } - - // Disable rt_OneStep() here - - // Terminate model - est_estimator_terminate(est_estimator_M); - return 0; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.cpp b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.cpp deleted file mode 100644 index 3a6d27ad71..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.cpp +++ /dev/null @@ -1,9448 +0,0 @@ -// -// File: est_estimator.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "est_estimator.h" -#include "est_estimator_private.h" - -const kfl_msg est_estimator_rtZkfl_msg = { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F - } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS -} ; // kfl_msg ground - -const cmc_msg est_estimator_rtZcmc_msg = { { 0U,// timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmc_state_cmd_a - { 0U, // timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmc_state_cmd_b - 0U, // cmc_mode_cmd - 0U, // speed_gain_cmd - 0U, // localization_mode_cmd - { 0.0F, 0.0F, 0.0F }, // att_kp - { 0.0F, 0.0F, 0.0F }, // att_ki - { 0.0F, 0.0F, 0.0F }, // omega_kd - { 0.0F, 0.0F, 0.0F }, // pos_kp - { 0.0F, 0.0F, 0.0F }, // pos_ki - { 0.0F, 0.0F, 0.0F }, // vel_kd - { 0.0F, 0.0F, 0.0F }, // center_of_mass - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// inertia_matrix - 0.0F // mass -}; - -const cvs_handrail_msg est_estimator_rtZcvs_handrail_m = { 0U,// cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// cvs_landmarks - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// cvs_observations - { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U },// cvs_valid_flag - 0U, // cvs_3d_knowledge_flag - { 0.0F, 0.0F, 0.0F }, // cvs_handrail_local_pos - { 0.0F, 0.0F, 0.0F, 0.0F }, // cvs_handrail_local_quat - 0U // cvs_handrail_update_global_pose_flag -}; - -const cvs_landmark_msg est_estimator_rtZcvs_landmark_m = { 0U,// cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// cvs_landmarks - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// cvs_observations - { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U }// cvs_valid_flag -}; - -const cvs_optical_flow_msg est_estimator_rtZcvs_optical_fl = { 0U,// cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F }, // cvs_observations - { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U }, // cvs_valid_flag - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }// cvs_id_tag -}; - -const cvs_registration_pulse est_estimator_rtZcvs_registrati = { 0U,// cvs_ar_tag_pulse - 0U, // cvs_landmark_pulse - 0U, // cvs_optical_flow_pulse - 0U // cvs_handrail_pulse -}; - -const imu_msg est_estimator_rtZimu_msg = { 0U,// imu_timestamp_sec - 0U, // imu_timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // imu_A_B_ECI_sensor - { 0.0F, 0.0F, 0.0F }, // imu_accel_bias - { 0.0F, 0.0F, 0.0F }, // imu_omega_B_ECI_sensor - { 0.0F, 0.0F, 0.0F }, // imu_gyro_bias - 0U, // imu_validity_flag - 0U // imu_sat_flag -}; - -// -// System initialize for atomic system: -// '/MATLAB Function1' -// '/MATLAB Function1' -// -void est_estima_MATLABFunction1_Init(DW_MATLABFunction1_est_estima_T *localDW) -{ - localDW->prev_impeller_speed = -1.0F; - - // No number will be equal to NaN, this will trigger the coefficients to update on the first tick - localDW->y = 0.0F; - localDW->den[0] = 0.0F; - localDW->num[0] = localDW->den[0]; - localDW->den[1] = 0.0F; - localDW->num[1] = localDW->den[1]; - localDW->den[2] = 1.0F; - localDW->num[2] = localDW->den[2]; -} - -// -// Start for atomic system: -// '/MATLAB Function1' -// '/MATLAB Function1' -// -void est_estim_MATLABFunction1_Start(B_MATLABFunction1_est_estimat_T *localB) -{ - localB->num_out[0] = 0.0F; - localB->den_out[0] = 0.0F; - localB->num_out[1] = 0.0F; - localB->den_out[1] = 0.0F; - localB->num_out[2] = 0.0F; - localB->den_out[2] = 0.0F; -} - -// -// Output and update for atomic system: -// '/MATLAB Function1' -// '/MATLAB Function1' -// -void est_estimator_MATLABFunction1(real_T rtu_Ts, real32_T rtu_impeller_speed, - B_MATLABFunction1_est_estimat_T *localB, DW_MATLABFunction1_est_estima_T - *localDW) -{ - real_T Ws; - real_T Nyquist_W; - real32_T Aliased_W[4]; - real32_T Omega_n; - boolean_T b_x[4]; - int32_T idx; - int32_T b_ii; - int32_T ii_data_idx_0; - int32_T ii_sizes_idx_1; - boolean_T exitg1; - - // MATLAB Function 'est_estimator/filter_prep/imu_prep/filter/MATLAB Function1': ':1' - // Calculate notch filter coefficients, note only executes when the impeller speeds change - // Setup to only run when the impeller_speed changes - // update_flag = (prev_impeller_speed ~= impeller_speed); - if (localDW->prev_impeller_speed != rtu_impeller_speed) { - // ':1:22' - // Filter Bandwidth - // ':1:25' - // Sample frequency, Hz - // ':1:26' - Ws = 1.0 / rtu_Ts * 6.2831853071795862; - - // Sample frequency, Rad/sec - // ':1:27' - Nyquist_W = Ws / 2.0; - - // Nyquist rate, rad/sec - // ':1:29' - // Look for the possible aliased frequncies - // Find the actual aliased frequency - // ':1:32' - Omega_n = (real32_T)fabs((real_T)((real32_T)(Ws * 0.0) - rtu_impeller_speed)); - b_x[0] = ((Omega_n > 0.0F) && ((real_T)Omega_n <= Nyquist_W)); - Aliased_W[0] = Omega_n; - Omega_n = (real32_T)fabs((real_T)((real32_T)Ws - rtu_impeller_speed)); - b_x[1] = ((Omega_n > 0.0F) && ((real_T)Omega_n <= Nyquist_W)); - Aliased_W[1] = Omega_n; - Omega_n = (real32_T)fabs((real_T)((real32_T)(Ws * 2.0) - rtu_impeller_speed)); - b_x[2] = ((Omega_n > 0.0F) && ((real_T)Omega_n <= Nyquist_W)); - Aliased_W[2] = Omega_n; - Omega_n = (real32_T)fabs((real_T)((real32_T)(Ws * 3.0) - rtu_impeller_speed)); - b_x[3] = ((Omega_n > 0.0F) && ((real_T)Omega_n <= Nyquist_W)); - Aliased_W[3] = Omega_n; - idx = 0; - ii_sizes_idx_1 = 1; - b_ii = 1; - exitg1 = false; - while ((!exitg1) && (b_ii < 5)) { - if (b_x[(int32_T)(b_ii - 1)]) { - idx = 1; - ii_data_idx_0 = b_ii; - exitg1 = true; - } else { - b_ii++; - } - } - - if (idx == 0) { - ii_sizes_idx_1 = 0; - } - - if (!(ii_sizes_idx_1 == 0)) { - // ':1:34' - // ':1:35' - localDW->y = Aliased_W[(int32_T)(ii_data_idx_0 - 1)]; - - // Impeller frequency - // ':1:36' - Omega_n = 3.14159274F * localDW->y / (real32_T)Nyquist_W; - - // Normalized frequency - // ':1:37' - Ws = 39.478417604357432 / Nyquist_W; - - // Normalized bandwidth - // Calc coefficients of filter, from: - // http://onlinelibrary.wiley.com/store/10.1002/mop.23439/asset/23439_ftp.pdf?v=1&t=iyyqfya1&s=b26e8f569108cf78a04dbd5e7edb44f70b9bc3d1 - // ':1:41' - // ':1:42' - Ws = (1.0 - tan(Ws / 2.0)) / (tan(Ws / 2.0) + 1.0); - - // ':1:43' - localDW->num[0] = (real32_T)(1.0 + Ws) / 2.0F; - localDW->num[1] = 2.0F * -(real32_T)cos((real_T)Omega_n) * (real32_T)(1.0 - + Ws) / 2.0F; - localDW->num[2] = (real32_T)(Ws + 1.0) / 2.0F; - - // ':1:44' - localDW->den[0] = 1.0F; - localDW->den[1] = (real32_T)(1.0 + Ws) * -(real32_T)cos((real_T)Omega_n); - localDW->den[2] = (real32_T)Ws; - } else { - // ':1:47' - localDW->y = 0.0F; - - // ':1:48' - // ':1:49' - localDW->num[0] = 0.0F; - localDW->den[0] = 0.0F; - localDW->num[1] = 0.0F; - localDW->den[1] = 0.0F; - localDW->num[2] = 1.0F; - localDW->den[2] = 1.0F; - } - } - - // ':1:54' - localDW->prev_impeller_speed = rtu_impeller_speed; - - // ':1:55' - // ':1:56' - // ':1:57' - localB->num_out[0] = localDW->num[0]; - localB->den_out[0] = localDW->den[0]; - localB->num_out[1] = localDW->num[1]; - localB->den_out[1] = localDW->den[1]; - localB->num_out[2] = localDW->num[2]; - localB->den_out[2] = localDW->den[2]; -} - -// -// Termination for atomic system: -// '/MATLAB Function1' -// '/MATLAB Function1' -// -void est_estima_MATLABFunction1_Term(void) -{ -} - -// -// Output and update for atomic system: -// '/MATLAB Function' -// '/MATLAB Function' -// '/MATLAB Function' -// -void est_estimator_MATLABFunction(const real32_T rtu_u[16], - B_MATLABFunction_est_estimato_T *localB) -{ - real32_T normA; - real32_T b_s; - int32_T b_j; - int32_T eint; - static const real32_T theta[3] = { 0.425873F, 1.8801527F, 3.92572474F }; - - real32_T rtu_u_0[16]; - int32_T i; - boolean_T exitg1; - boolean_T exitg2; - - // MATLAB Function 'first_order_quaternion_propogation/MATLAB Function': ':1' - // ':1:4' - normA = 0.0F; - b_j = 0; - exitg2 = false; - while ((!exitg2) && (b_j < 4)) { - b_s = (((real32_T)fabs((real_T)rtu_u[(int32_T)((int32_T)(b_j << 2) + 1)]) + - (real32_T)fabs((real_T)rtu_u[(int32_T)(b_j << 2)])) + (real32_T)fabs - ((real_T)rtu_u[(int32_T)((int32_T)(b_j << 2) + 2)])) + (real32_T)fabs - ((real_T)rtu_u[(int32_T)((int32_T)(b_j << 2) + 3)]); - if (rtIsNaNF(b_s)) { - normA = (rtNaNF); - exitg2 = true; - } else { - if (b_s > normA) { - normA = b_s; - } - - b_j++; - } - } - - if (normA <= 3.92572474F) { - b_j = 0; - exitg1 = false; - while ((!exitg1) && (b_j < 3)) { - if (normA <= theta[b_j]) { - mglnkfkfmglfjekn_PadeApproximantOfDegree(rtu_u, (uint8_T)(int32_T) - ((int32_T)(b_j << 1) + 3), localB->y); - exitg1 = true; - } else { - b_j++; - } - } - } else { - b_s = normA / 3.92572474F; - if ((!rtIsInfF(b_s)) && (!rtIsNaNF(b_s))) { - b_s = (real32_T)frexp((real_T)b_s, &eint); - normA = (real32_T)eint; - } else { - normA = 0.0F; - } - - if (b_s == 0.5F) { - normA--; - } - - b_s = rt_powf_snf(2.0F, normA); - for (eint = 0; eint < 16; eint++) { - rtu_u_0[eint] = rtu_u[eint] / b_s; - } - - mglnkfkfmglfjekn_PadeApproximantOfDegree(rtu_u_0, 7U, localB->y); - for (b_j = 0; b_j <= (int32_T)((int32_T)normA - 1); b_j++) { - for (eint = 0; eint < 4; eint++) { - for (i = 0; i < 4; i++) { - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] = 0.0F; - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] += localB->y[(int32_T)(i << - 2)] * localB->y[eint]; - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] += localB->y[(int32_T) - ((int32_T)(i << 2) + 1)] * localB->y[(int32_T)(eint + 4)]; - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] += localB->y[(int32_T) - ((int32_T)(i << 2) + 2)] * localB->y[(int32_T)(eint + 8)]; - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] += localB->y[(int32_T) - ((int32_T)(i << 2) + 3)] * localB->y[(int32_T)(eint + 12)]; - } - } - - for (eint = 0; eint < 4; eint++) { - localB->y[(int32_T)(eint << 2)] = rtu_u_0[(int32_T)(eint << 2)]; - localB->y[(int32_T)(1 + (int32_T)(eint << 2))] = rtu_u_0[(int32_T) - ((int32_T)(eint << 2) + 1)]; - localB->y[(int32_T)(2 + (int32_T)(eint << 2))] = rtu_u_0[(int32_T) - ((int32_T)(eint << 2) + 2)]; - localB->y[(int32_T)(3 + (int32_T)(eint << 2))] = rtu_u_0[(int32_T) - ((int32_T)(eint << 2) + 3)]; - } - } - } -} - -// -// Termination for atomic system: -// '/MATLAB Function' -// '/MATLAB Function' -// '/MATLAB Function' -// -void est_estimat_MATLABFunction_Term(void) -{ -} - -// -// Output and update for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void est_estimator_Normalize(const real32_T rtu_q_in[4], real32_T - rty_positive_scalar_q[4], P_Normalize_est_estimator_T *localP) -{ - // Product: '/Product' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - - rty_positive_scalar_q[0] = rtu_q_in[0] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[1] = rtu_q_in[1] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[2] = rtu_q_in[2] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[3] = rtu_q_in[3] * (real32_T)localP->Constant1_Value; -} - -// -// Termination for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void est_estimator_Normalize_Term(void) -{ -} - -// -// Output and update for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void est_estimator_Normalize_p(const real32_T rtu_Vec[4], real32_T rtu_Magnitude, - real32_T rty_Normalized_Vec[4]) -{ - // Product: '/Divide' - rty_Normalized_Vec[0] = rtu_Vec[0] / rtu_Magnitude; - rty_Normalized_Vec[1] = rtu_Vec[1] / rtu_Magnitude; - rty_Normalized_Vec[2] = rtu_Vec[2] / rtu_Magnitude; - rty_Normalized_Vec[3] = rtu_Vec[3] / rtu_Magnitude; -} - -// -// Termination for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void est_estimator_Normalize_h_Term(void) -{ -} - -// -// Output and update for action system: -// '/If Action Subsystem1' -// '/If Action Subsystem1' -// -void est_estimato_IfActionSubsystem1(const kfl_msg *rtu_state_in, const - ase_cov_datatype rtu_P_in[13689], const real_T rtu_aug_velocity[3], const - real_T rtu_aug_velocity_p[3], const real_T rtu_aug_velocity_l[48], const - real_T rtu_aug_velocity_o[48], kfl_msg *rty_state_out, ase_cov_datatype - rty_P_out[13689], real_T rty_aug_velocity_out[3], real_T - rty_aug_velocity_out_p[3], real_T rty_aug_velocity_out_l[48], real_T - rty_aug_velocity_out_o[48]) -{ - int32_T i; - - // SignalConversion: '/Signal Conversion' - *rty_state_out = *rtu_state_in; - - // Inport: '/P_in' - for (i = 0; i < 13689; i++) { - rty_P_out[i] = rtu_P_in[i]; - } - - // End of Inport: '/P_in' - - // SignalConversion: '/Signal Conversion1' - rty_aug_velocity_out[0] = rtu_aug_velocity[0]; - rty_aug_velocity_out[1] = rtu_aug_velocity[1]; - rty_aug_velocity_out[2] = rtu_aug_velocity[2]; - - // SignalConversion: '/Signal Conversion1' - rty_aug_velocity_out_p[0] = rtu_aug_velocity_p[0]; - rty_aug_velocity_out_p[1] = rtu_aug_velocity_p[1]; - rty_aug_velocity_out_p[2] = rtu_aug_velocity_p[2]; - - // SignalConversion: '/Signal Conversion1' - for (i = 0; i < 48; i++) { - rty_aug_velocity_out_l[i] = rtu_aug_velocity_l[i]; - } - - // End of SignalConversion: '/Signal Conversion1' - - // SignalConversion: '/Signal Conversion1' - for (i = 0; i < 48; i++) { - rty_aug_velocity_out_o[i] = rtu_aug_velocity_o[i]; - } - - // End of SignalConversion: '/Signal Conversion1' -} - -// -// Termination for action system: -// '/If Action Subsystem1' -// '/If Action Subsystem1' -// -void est_est_IfActionSubsystem1_Term(void) -{ -} - -// Model step function -void est_estimator_step(RT_MODEL_est_estimator_T *const est_estimator_M, - cvs_landmark_msg *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]) -{ - P_est_estimator_T *est_estimator_P = ((P_est_estimator_T *) - est_estimator_M->defaultParam); - B_est_estimator_T *est_estimator_B = ((B_est_estimator_T *) - est_estimator_M->blockIO); - DW_est_estimator_T *est_estimator_DW = ((DW_est_estimator_T *) - est_estimator_M->dwork); - - // local block i/o variables - real32_T rtb_ex_matrix_multiply1[180]; - real32_T rtb_ex_matrix_multiply3[225]; - real32_T rtb_ex_of_residual_and_h_o2[96]; - real32_T rtb_ex_of_residual_and_h_o6[50]; - real32_T rtb_ex_compute_delta_state_and_[117]; - real32_T rtb_ex_apply_delta_state_o1[4]; - real32_T rtb_ex_apply_delta_state_o2[3]; - real32_T rtb_ex_apply_delta_state_o3[3]; - real32_T rtb_ex_apply_delta_state_o4[3]; - real32_T rtb_ex_apply_delta_state_o5[3]; - real32_T rtb_ex_apply_delta_state_o6[4]; - real32_T rtb_ex_apply_delta_state_o7[3]; - real32_T rtb_ex_apply_delta_state_o9[64]; - real32_T rtb_ex_apply_delta_state_o10[48]; - real32_T rtb_ex_compute_delta_state_an_p[117]; - real32_T rtb_ex_apply_delta_state_o1_d[4]; - real32_T rtb_ex_apply_delta_state_o2_i[3]; - real32_T rtb_ex_apply_delta_state_o3_e[3]; - real32_T rtb_ex_apply_delta_state_o4_g[3]; - real32_T rtb_ex_apply_delta_state_o5_i[3]; - real32_T rtb_ex_apply_delta_state_o6_d[4]; - real32_T rtb_ex_apply_delta_state_o7_p[3]; - real32_T rtb_ex_apply_delta_state_o9_c[64]; - real32_T rtb_ex_apply_delta_state_o10_i[48]; - real32_T rtb_ex_matrix_multiply5[225]; - real32_T rtb_MathFunction_eu[225]; - int32_T rtb_ex_of_residual_and_h_o1; - int32_T rtb_ex_compute_delta_state_an_g; - int32_T rtb_ex_compute_delta_state_a_pg; - uint32_T rtb_ex_of_residual_and_h_o4; - uint16_T rtb_ex_apply_delta_state_o8; - uint16_T rtb_ex_apply_delta_state_o8_j; - uint8_T rtb_ex_of_residual_and_h_o5; - - // local scratch DWork variables - int32_T ForEach_itr; - int32_T ForEach_itr_g; - real32_T accel[3]; - real32_T S[9]; - real32_T C[9]; - static const int8_T b[9] = { -1, 0, 0, 0, -1, 0, 0, 0, -1 }; - - static const int8_T c[45] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0 }; - - static const real32_T theta[3] = { 0.425873F, 1.8801527F, 3.92572474F }; - - static const int8_T d[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - boolean_T d_0[12]; - real32_T x[12]; - boolean_T b_0[17]; - int32_T ar; - static const int8_T Aug_Indx[102] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, - 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, - 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, - 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, - 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102 }; - - real_T kept_augmentations[15]; - real_T of_in_prange[90]; - real32_T M[126]; - static const int8_T g[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - real_T camera_tf_camera1[256]; - real32_T global_tf_camera1[16]; - real32_T A[96]; - real32_T b_1[32]; - real_T num_augs; - real_T aug_ind; - real_T ind0; - int32_T b_m; - static const int8_T b_2[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - int32_T handrail_knowledge_dims; - static const int8_T t[12] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0 }; - - static const int8_T v[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - static const int8_T c_a[9] = { -1, 0, 0, 0, -1, 0, 0, 0, -1 }; - - real_T camera_ml_tf_global[12]; - real_T next_ml_tf_global[12]; - real32_T temp[6]; - int32_T num_original; - boolean_T empty_non_axis_sizes; - int32_T nx; - static const int8_T g_0[12] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0 }; - - uint32_T rtb_BitwiseOperator; - real32_T rtb_P_B_ISS_ISS[3]; - real_T rtb_UnitDelay18[50]; - real_T rtb_UnitDelay19[50]; - real32_T rtb_UnitDelay25[4]; - real32_T rtb_Sum1_k3[3]; - boolean_T rtb_Compare; - boolean_T rtb_FixPtRelationalOperator_n; - real32_T rtb_VectorConcatenate[16]; - real32_T rtb_VectorConcatenate_hq[16]; - real32_T rtb_Assignment_b[9]; - real32_T rtb_Sum_k1; - uint8_T rtb_Saturation_n; - real32_T rtb_ImpAsg_InsertedFor_Out1_a_d[3]; - real32_T rtb_Product1[4]; - real32_T rtb_Assignment_n[9]; - real32_T rtb_ImpAsg_InsertedFor_Out1_at_[3]; - boolean_T rtb_LogicalOperator; - boolean_T rtb_of_update_flag; - real32_T rtb_Q[144]; - real32_T rtb_state_trans[225]; - real_T rtb_ml_vel_aug[3]; - real_T rtb_ml_omega_aug[3]; - real32_T rtb_Merge_o[4]; - real32_T rtb_Sum2[3]; - int32_T s12_iter; - real32_T rtb_VectorConcatenate_c[16]; - real32_T rtb_VectorConcatenate_p[16]; - real32_T rtb_Add_e[3]; - real32_T rtb_VectorConcatenate_m[16]; - real32_T rtb_VectorConcatenate_i[16]; - uint8_T rtb_Switch_m; - real32_T rtb_global_points[200]; - real32_T rtb_camera_tf_global[256]; - int32_T rtb_valid_out[800]; - real32_T rtb_Gain1_f; - real32_T rtb_Assignment_ef[9]; - real32_T rtb_Sum_l; - uint8_T rtb_numFeatures_f; - real32_T rtb_Switch1; - uint8_T rtb_BusCreator2_update_ML_featu; - uint32_T rtb_BusAssignment_aug_state_enu; - real32_T rtb_Product2[16]; - boolean_T rtb_Compare_ic[50]; - real32_T rtb_Sum_b[225]; - real_T rtb_of_vel_aug[48]; - real_T rtb_of_omega_aug[48]; - kfl_msg rtb_Merge2; - ase_cov_datatype rtb_Assignment6[90]; - ase_cov_datatype rtb_Product2_b[90]; - ase_cov_datatype rtb_Product3[576]; - kfl_msg rtb_BusAssignment_a; - real32_T rtb_Product_j[6]; - real32_T rtb_H_out[702]; - real32_T rtb_R_mat[36]; - real32_T rtb_hr_global_landmarks[150]; - real32_T rtb_Assignment1[180]; - real32_T rtb_MathFunction2[180]; - real32_T rtb_ex_matrix_multiply4[225]; - real_T ml_vel_aug[3]; - real_T ml_omega_aug[3]; - uint8_T numFeatures; - real32_T hr_P_hr_ISS_ISS[3]; - real_T of_vel_aug[48]; - real_T of_omega_aug[48]; - real32_T UnitDelay_DSTATE_n_V_B_ISS_ISS[3]; - real32_T UnitDelay_DSTATE_n_accel_bias[3]; - real32_T UnitDelay_DSTATE_n_ml_quat_ISS2[4]; - real32_T UnitDelay_DSTATE_n_ml_P_cam_ISS[3]; - real32_T UnitDelay_DSTATE_n_of_quat_ISS2[64]; - real32_T UnitDelay_DSTATE_n_of_P_cam_ISS[48]; - ase_cov_datatype UnitDelay_DSTATE_n_cov_diag[117]; - uint16_T UnitDelay_DSTATE_n_kfl_status; - real_T UnitDelay_DSTATE_n_of_mahal_dis[50]; - real_T UnitDelay_DSTATE_n_ml_mahal_dis[50]; - real32_T UnitDelay_DSTATE_n_P_EST_ISS_IS[3]; - uint32_T UnitDelay_DSTATE_aug_state_enum; - int32_T i; - real_T rtb_ml_vel_aug_0[3]; - real32_T rtb_Compare_2[4]; - real_T tmp[3]; - real32_T rtb_Switch1_0[2]; - real32_T tmp_0[2]; - real32_T rtb_Switch1_1[2]; - real32_T H[4]; - int32_T rtb_valid_out_0[16]; - uint32_T tmp_1[17]; - real32_T tmp_2[117]; - real32_T rtb_VectorConcatenate_l[16]; - real32_T rtb_Product1_0[9]; - real32_T rtb_Assignment_o[9]; - real32_T tmp_3[9]; - real32_T rtb_Assignment_l[9]; - real32_T rtb_Product1_1[12]; - real32_T rtb_Product1_2[12]; - int32_T br; - real32_T tmp_4[9]; - real32_T tmp_5[9]; - int32_T i_0; - real32_T tmp_6[9]; - real32_T h[6]; - real32_T tmp_7[9]; - real32_T temp_0[6]; - real32_T temp_1[6]; - real32_T temp_2[6]; - real32_T tmp_8[12]; - real32_T v_0[18]; - real32_T S_0[18]; - real_T tmp_9; - real32_T tmp_a[9]; - real32_T tmp_b[9]; - ase_cov_datatype Switch1[90]; - real32_T tmp_c[9]; - real32_T tmp_d[9]; - real32_T rtb_Merge2_0[60]; - real_T rtb_VectorConcatenate_o[4]; - real32_T tmp_e[9]; - real32_T rtb_BusAssignment_h[48]; - real32_T M_0[126]; - real32_T M_1[126]; - real32_T M_2[540]; - real32_T tmp_f[9]; - real32_T rtb_Merge2_1[9]; - int8_T tmp_g[6]; - int8_T b_data[12]; - int8_T c_data[12]; - int8_T valid_indx_mat_data[102]; - int8_T c_data_0[17]; - int8_T f_data[90]; - int8_T rot_indices_data[63]; - int8_T c_data_1[63]; - real32_T C_data[63]; - int16_T h_data[1600]; - int32_T x_sizes[3]; - real32_T a_data[96]; - real32_T c_a_data[189]; - real32_T newerr_data[200]; - real32_T r_data[150]; - real32_T landmark_error_rail_frame_data[150]; - real_T r_vec_data[150]; - real32_T H_data[900]; - real32_T T_H_data[900]; - int16_T p_data[150]; - real32_T d_result_data[200]; - real32_T y_data[150]; - real32_T b_a_data[900]; - real32_T camera_landmarks_data[150]; - real32_T z_est_data[100]; - real32_T r_data_0[100]; - real32_T next_landmarks_data[150]; - real32_T omega_error_data[100]; - real_T r_vec_data_0[100]; - real32_T H_data_0[600]; - int32_T H_sizes[2]; - boolean_T invalid_vec_data[50]; - boolean_T invalid_data[100]; - int32_T q1_sizes[2]; - real32_T T_H_data_0[600]; - int32_T T_H_sizes[2]; - int8_T e_data[50]; - real32_T varargin_1_data[150]; - real32_T y_data_0[100]; - boolean_T g_result_data[100]; - real32_T a_data_0[600]; - int32_T of_measured_in_sizes[3]; - real32_T A_data[96]; - int8_T valid_indx_mat_data_0[102]; - real32_T b_data_0[32]; - boolean_T d_1; - int32_T C_sizes_idx_1; - int32_T O_sizes_idx_0; - int32_T z_est_sizes_idx_1; - int8_T c_sz_idx_0; - int16_T tmp_h; - real32_T UnitDelay_DSTATE_n_P_B_ISS_ISS_; - real32_T UnitDelay_DSTATE_n_A_B_ISS_ISS_; - real32_T UnitDelay_DSTATE_n_P_B_ISS_IS_0; - real32_T UnitDelay_DSTATE_n_hr_P_hr_ISS_; - real32_T UnitDelay_DSTATE_n_hr_P_hr_IS_0; - real32_T UnitDelay_DSTATE_n_hr_P_hr_IS_1; - real32_T UnitDelay_DSTATE_n_hr_quat_ISS2; - real32_T UnitDelay_DSTATE_n_hr_quat_IS_0; - real32_T UnitDelay_DSTATE_n_hr_quat_IS_1; - real32_T UnitDelay_DSTATE_n_hr_quat_IS_2; - real32_T hr_quat_ISS2hr_idx_0; - real32_T hr_quat_ISS2hr_idx_1; - real32_T hr_quat_ISS2hr_idx_2; - real32_T hr_quat_ISS2hr_idx_3; - boolean_T exitg1; - boolean_T exitg2; - - // Outputs for Atomic SubSystem: '/est_estimator' - // UnitDelay: '/Unit Delay20' - rtb_ml_vel_aug[0] = est_estimator_DW->UnitDelay20_DSTATE[0]; - - // UnitDelay: '/Unit Delay21' - rtb_ml_omega_aug[0] = est_estimator_DW->UnitDelay21_DSTATE[0]; - - // UnitDelay: '/Unit Delay20' - rtb_ml_vel_aug[1] = est_estimator_DW->UnitDelay20_DSTATE[1]; - - // UnitDelay: '/Unit Delay21' - rtb_ml_omega_aug[1] = est_estimator_DW->UnitDelay21_DSTATE[1]; - - // UnitDelay: '/Unit Delay20' - rtb_ml_vel_aug[2] = est_estimator_DW->UnitDelay20_DSTATE[2]; - - // UnitDelay: '/Unit Delay21' - rtb_ml_omega_aug[2] = est_estimator_DW->UnitDelay21_DSTATE[2]; - - // UnitDelay: '/Unit Delay22' - memcpy(&rtb_of_vel_aug[0], &est_estimator_DW->UnitDelay22_DSTATE[0], (uint32_T) - (48U * sizeof(real_T))); - - // UnitDelay: '/Unit Delay23' - memcpy(&rtb_of_omega_aug[0], &est_estimator_DW->UnitDelay23_DSTATE[0], - (uint32_T)(48U * sizeof(real_T))); - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - - memcpy(&est_estimator_B->Assignment[0], &est_estimator_P->Constant2_Value_ad[0], - (uint32_T)(13689U * sizeof(real32_T))); - - // SignalConversion: '/TmpSignal ConversionAtAssignmentInport2' incorporates: - // Constant: '/Constant4' - // Constant: '/Constant5' - // Gain: '/Gain' - - for (i = 0; i < 15; i++) { - tmp_2[i] = est_estimator_P->tun_ase_state_ic_cov_diag[i]; - } - - for (i = 0; i < 102; i++) { - tmp_2[(int32_T)(i + 15)] = est_estimator_P->tun_aug_ic_cov * - est_estimator_P->Constant5_Value_a[i]; - } - - // Assignment: '/Assignment' incorporates: - // SignalConversion: '/TmpSignal ConversionAtAssignmentInport2' - - for (i = 0; i < 117; i++) { - est_estimator_B->Assignment[(int32_T)(118 * i)] = tmp_2[i]; - } - - // Delay: '/Delay' - if ((int32_T)est_estimator_DW->icLoad != 0) { - memcpy(&est_estimator_DW->Delay_DSTATE[0], &est_estimator_B->Assignment[0], - (uint32_T)(13689U * sizeof(ase_cov_datatype))); - } - - // UnitDelay: '/Unit Delay18' - memcpy(&rtb_UnitDelay18[0], &est_estimator_DW->UnitDelay18_DSTATE[0], - (uint32_T)(50U * sizeof(real_T))); - - // UnitDelay: '/Unit Delay19' - memcpy(&rtb_UnitDelay19[0], &est_estimator_DW->UnitDelay19_DSTATE[0], - (uint32_T)(50U * sizeof(real_T))); - - // UnitDelay: '/Unit Delay25' - rtb_UnitDelay25[0] = est_estimator_DW->UnitDelay25_DSTATE[0]; - rtb_UnitDelay25[1] = est_estimator_DW->UnitDelay25_DSTATE[1]; - rtb_UnitDelay25[2] = est_estimator_DW->UnitDelay25_DSTATE[2]; - rtb_UnitDelay25[3] = est_estimator_DW->UnitDelay25_DSTATE[3]; - - // BusCreator: '/Bus Creator2' incorporates: - // UnitDelay: '/Unit Delay16' - // UnitDelay: '/Unit Delay17' - - numFeatures = est_estimator_DW->UnitDelay16_DSTATE; - rtb_BusCreator2_update_ML_featu = est_estimator_DW->UnitDelay17_DSTATE; - - // Sum: '/Sum of Elements2' incorporates: - // Inport: '/handrail_msg' - - UnitDelay_DSTATE_aug_state_enum = (uint32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[0]; - for (ar = 0; ar < 49; ar++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[(int32_T)(ar + 1)]; - } - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - // Sum: '/Sum of Elements2' - - rtb_Compare = ((uint8_T)UnitDelay_DSTATE_aug_state_enum >= - est_estimator_P->Constant_Value_oq); - - // Logic: '/Logical Operator8' incorporates: - // Inport: '/handrail_msg' - - empty_non_axis_sizes = ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag - [0] != 0); - for (handrail_knowledge_dims = 0; handrail_knowledge_dims < 49; - handrail_knowledge_dims++) { - empty_non_axis_sizes = (empty_non_axis_sizes || ((int32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[(int32_T) - (handrail_knowledge_dims + 1)] != 0)); - } - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // Inport: '/cmc_msg' - // SignalConversion: '/Signal Conversion' - // UnitDelay: '/Delay Input1' - - rtb_FixPtRelationalOperator_n = - (est_estimator_U_cmc_msg_o->localization_mode_cmd != - est_estimator_DW->DelayInput1_DSTATE_k); - - // Switch: '/Switch' incorporates: - // S-Function (sfix_bitop): '/FixPt Bitwise Operator3' - // S-Function (sfix_bitop): '/FixPt Bitwise Operator4' - // S-Function (sfix_bitop): '/FixPt Bitwise Operator5' - // UnitDelay: '/Unit Delay9' - - if (rtb_FixPtRelationalOperator_n) { - rtb_BitwiseOperator = (uint32_T)~(uint32_T)((uint32_T) - ~est_estimator_DW->UnitDelay9_DSTATE | - est_estimator_P->FixPtBitwiseOperator3_BitMask); - } else { - rtb_BitwiseOperator = est_estimator_DW->UnitDelay9_DSTATE; - } - - // End of Switch: '/Switch' - - // BusAssignment: '/Bus Assignment' - rtb_BusAssignment_aug_state_enu = rtb_BitwiseOperator; - - // Constant: '/Constant3' - rtb_VectorConcatenate[0] = est_estimator_P->Constant3_Value_c4; - - // BusAssignment: '/Bus Assignment' incorporates: - // UnitDelay: '/Unit Delay24' - - rtb_Add_e[0] = est_estimator_DW->UnitDelay24_DSTATE[0]; - rtb_Add_e[1] = est_estimator_DW->UnitDelay24_DSTATE[1]; - rtb_Add_e[2] = est_estimator_DW->UnitDelay24_DSTATE[2]; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[1] = est_estimator_P->Gain_Gain_n1 * (real32_T) - est_estimator_P->Constant_Value_l[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[2] = (real32_T)est_estimator_P->Constant_Value_l[1]; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[3] = est_estimator_P->Gain1_Gain_au * (real32_T) - est_estimator_P->Constant_Value_l[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[4] = (real32_T)est_estimator_P->Constant_Value_l[2]; - - // Constant: '/Constant2' - rtb_VectorConcatenate[5] = est_estimator_P->Constant2_Value_am; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[6] = est_estimator_P->Gain2_Gain_a5 * (real32_T) - est_estimator_P->Constant_Value_l[0]; - - // Gain: '/Gain3' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[7] = est_estimator_P->Gain3_Gain_m * (real32_T) - est_estimator_P->Constant_Value_l[1]; - - // Gain: '/Gain4' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[8] = est_estimator_P->Gain4_Gain_h * (real32_T) - est_estimator_P->Constant_Value_l[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[9] = (real32_T)est_estimator_P->Constant_Value_l[0]; - - // Constant: '/Constant1' - rtb_VectorConcatenate[10] = est_estimator_P->Constant1_Value_p; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[11] = est_estimator_P->Gain5_Gain_a * (real32_T) - est_estimator_P->Constant_Value_l[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[12] = (real32_T)est_estimator_P->Constant_Value_l[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[13] = (real32_T)est_estimator_P->Constant_Value_l[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[14] = (real32_T)est_estimator_P->Constant_Value_l[2]; - - // Constant: '/Constant' - rtb_VectorConcatenate[15] = est_estimator_P->Constant_Value_d; - - // Constant: '/Constant3' - rtb_VectorConcatenate_hq[0] = est_estimator_P->Constant3_Value_dg; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_k1 = est_estimator_P->tun_abp_quat_body2imu[3] * - est_estimator_P->tun_abp_quat_body2imu[3] * est_estimator_P->Gain_Gain_g0 - - (real32_T)est_estimator_P->Constant1_Value_mb; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_a[i]; - } - - rtb_Assignment_b[0] = rtb_Sum_k1; - rtb_Assignment_b[4] = rtb_Sum_k1; - rtb_Assignment_b[8] = rtb_Sum_k1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_jx * - est_estimator_P->tun_abp_quat_body2imu[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Product1_0[0] = (real32_T)est_estimator_P->Constant3_Value_k; - rtb_Product1_0[1] = est_estimator_P->tun_abp_quat_body2imu[2]; - rtb_Product1_0[2] = est_estimator_P->Gain_Gain_e * - est_estimator_P->tun_abp_quat_body2imu[1]; - rtb_Product1_0[3] = est_estimator_P->Gain1_Gain_nf * - est_estimator_P->tun_abp_quat_body2imu[2]; - rtb_Product1_0[4] = (real32_T)est_estimator_P->Constant3_Value_k; - rtb_Product1_0[5] = est_estimator_P->tun_abp_quat_body2imu[0]; - rtb_Product1_0[6] = est_estimator_P->tun_abp_quat_body2imu[1]; - rtb_Product1_0[7] = est_estimator_P->Gain2_Gain_h5 * - est_estimator_P->tun_abp_quat_body2imu[0]; - rtb_Product1_0[8] = (real32_T)est_estimator_P->Constant3_Value_k; - - // Math: '/Math Function' incorporates: - // Constant: '/Constant' - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Assignment_o[i] = est_estimator_P->tun_abp_quat_body2imu[0] * - est_estimator_P->tun_abp_quat_body2imu[i]; - rtb_Assignment_o[(int32_T)(i + 3)] = est_estimator_P->tun_abp_quat_body2imu - [1] * est_estimator_P->tun_abp_quat_body2imu[i]; - rtb_Assignment_o[(int32_T)(i + 6)] = est_estimator_P->tun_abp_quat_body2imu - [2] * est_estimator_P->tun_abp_quat_body2imu[i]; - } - - // End of Math: '/Math Function' - for (i = 0; i < 3; i++) { - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - rtb_Assignment_n[(int32_T)(3 * i)] = (rtb_Assignment_b[i] - rtb_Sum_k1 * - rtb_Product1_0[i]) + rtb_Assignment_o[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_gg; - rtb_Assignment_n[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 3)] - rtb_Product1_0[(int32_T)(i + 3)] * rtb_Sum_k1) + - rtb_Assignment_o[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_gg; - rtb_Assignment_n[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 6)] - rtb_Product1_0[(int32_T)(i + 6)] * rtb_Sum_k1) + - rtb_Assignment_o[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_gg; - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant1' - // Inport: '/imu_msg' - // Product: '/Product' - - rtb_P_B_ISS_ISS[i] = est_estimator_U_imu_msg_c->imu_omega_B_ECI_sensor[i] - - est_estimator_P->ase_gyro_fixed_bias[i]; - } - - // Product: '/Product' - for (i = 0; i < 3; i++) { - rtb_Sum1_k3[i] = rtb_Assignment_n[(int32_T)(i + 6)] * rtb_P_B_ISS_ISS[2] + - (rtb_Assignment_n[(int32_T)(i + 3)] * rtb_P_B_ISS_ISS[1] + - rtb_Assignment_n[i] * rtb_P_B_ISS_ISS[0]); - } - - // Saturate: '/Saturation' incorporates: - // Inport: '/cmc_msg' - // SignalConversion: '/Signal Conversion' - - if (est_estimator_U_cmc_msg_o->speed_gain_cmd > - est_estimator_P->fam_impeller_speeds_cnt) { - rtb_Saturation_n = est_estimator_P->fam_impeller_speeds_cnt; - } else if (est_estimator_U_cmc_msg_o->speed_gain_cmd < - est_estimator_P->Saturation_LowerSat_n) { - rtb_Saturation_n = est_estimator_P->Saturation_LowerSat_n; - } else { - rtb_Saturation_n = est_estimator_U_cmc_msg_o->speed_gain_cmd; - } - - // End of Saturate: '/Saturation' - - // Outputs for Iterator SubSystem: '/filter' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr_g = 0; ForEach_itr_g < 3; ForEach_itr_g++) { - // MATLAB Function: '/MATLAB Function1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant2' - // Selector: '/Selector' - - est_estimator_MATLABFunction1(est_estimator_P->astrobee_fsw_step_size, - est_estimator_P->fam_impeller_speeds[(int32_T)((int32_T)rtb_Saturation_n - - 1)], &est_estimator_B->CoreSubsys[ForEach_itr_g].sf_MATLABFunction1, - &est_estimator_DW->CoreSubsys[ForEach_itr_g].sf_MATLABFunction1); - - // DiscreteTransferFcn: '/Discrete Transfer Fcn' incorporates: - // DiscreteTransferFcn: '/3 Hz Low Pass' - - rtb_Sum_k1 = (est_estimator_P->CoreSubsys.uHzLowPass_NumCoef * - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states - - est_estimator_B->CoreSubsys[ForEach_itr_g]. - sf_MATLABFunction1.den_out[1] * est_estimator_DW-> - CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0]) - - est_estimator_B->CoreSubsys[ForEach_itr_g].sf_MATLABFunction1.den_out[2] * - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1]; - rtb_Sum_l = (est_estimator_B->CoreSubsys[ForEach_itr_g]. - sf_MATLABFunction1.num_out[0] * rtb_Sum_k1 + - est_estimator_B->CoreSubsys[ForEach_itr_g]. - sf_MATLABFunction1.num_out[1] * est_estimator_DW-> - CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0]) + - est_estimator_B->CoreSubsys[ForEach_itr_g].sf_MATLABFunction1.num_out[2] * - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1]; - - // Update for DiscreteTransferFcn: '/3 Hz Low Pass' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_In1_at_outport_0' - - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states = - (rtb_Sum1_k3[ForEach_itr_g] - - est_estimator_P->CoreSubsys.uHzLowPass_DenCoef[1] * - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states) / - est_estimator_P->CoreSubsys.uHzLowPass_DenCoef[0]; - - // Update for DiscreteTransferFcn: '/Discrete Transfer Fcn' - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1] = - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0]; - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0] = - rtb_Sum_k1; - - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_Out1_at_inport_0' - rtb_ImpAsg_InsertedFor_Out1_a_d[ForEach_itr_g] = rtb_Sum_l; - } - - // End of Outputs for SubSystem: '/filter' - - // Sum: '/Sum' incorporates: - // UnitDelay: '/Unit Delay3' - - rtb_ImpAsg_InsertedFor_Out1_a_d[0] -= est_estimator_DW->UnitDelay3_DSTATE[0]; - rtb_ImpAsg_InsertedFor_Out1_a_d[1] -= est_estimator_DW->UnitDelay3_DSTATE[1]; - hr_quat_ISS2hr_idx_0 = rtb_ImpAsg_InsertedFor_Out1_a_d[2] - - est_estimator_DW->UnitDelay3_DSTATE[2]; - - // Gain: '/Gain' - rtb_VectorConcatenate_hq[1] = est_estimator_P->Gain_Gain_co * - hr_quat_ISS2hr_idx_0; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' - rtb_VectorConcatenate_hq[2] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - - // Gain: '/Gain1' - rtb_VectorConcatenate_hq[3] = est_estimator_P->Gain1_Gain_cz * - rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' - rtb_VectorConcatenate_hq[4] = hr_quat_ISS2hr_idx_0; - - // Constant: '/Constant2' - rtb_VectorConcatenate_hq[5] = est_estimator_P->Constant2_Value_c; - - // Gain: '/Gain2' - rtb_VectorConcatenate_hq[6] = est_estimator_P->Gain2_Gain_g4 * - rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - - // Gain: '/Gain3' - rtb_VectorConcatenate_hq[7] = est_estimator_P->Gain3_Gain_fu * - rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - - // Gain: '/Gain4' - rtb_VectorConcatenate_hq[8] = est_estimator_P->Gain4_Gain_g * - rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' - rtb_VectorConcatenate_hq[9] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - - // Constant: '/Constant1' - rtb_VectorConcatenate_hq[10] = est_estimator_P->Constant1_Value_o; - - // Gain: '/Gain5' - rtb_VectorConcatenate_hq[11] = est_estimator_P->Gain5_Gain_jr * - hr_quat_ISS2hr_idx_0; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' - rtb_VectorConcatenate_hq[12] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' - rtb_VectorConcatenate_hq[13] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' - rtb_VectorConcatenate_hq[14] = hr_quat_ISS2hr_idx_0; - - // Constant: '/Constant' - rtb_VectorConcatenate_hq[15] = est_estimator_P->Constant_Value_lw; - - // Product: '/Product2' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Constant: '/Constant' - // Product: '/Product' - // Sum: '/Add' - - for (i = 0; i < 16; i++) { - rtb_Product2[i] = (est_estimator_P->Constant3_Value_dc * - rtb_VectorConcatenate[i] * (real32_T) - est_estimator_P->ase_ts + rtb_VectorConcatenate_hq[i]) * - est_estimator_P->Constant1_Value_i3 * (real32_T)est_estimator_P->ase_ts; - } - - // End of Product: '/Product2' - - // MATLAB Function: '/MATLAB Function' - est_estimator_MATLABFunction(rtb_Product2, &est_estimator_B->sf_MATLABFunction); - - // Product: '/Product5' incorporates: - // Constant: '/Constant' - - hr_quat_ISS2hr_idx_1 = (real32_T)est_estimator_P->ase_ts * (real32_T) - est_estimator_P->ase_ts * (real32_T)est_estimator_P->ase_ts; - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 4; b_m++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_hq[(int32_T)(i << 2)] * rtb_VectorConcatenate[b_m]; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)(i << 2)] * rtb_VectorConcatenate_hq[b_m]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate[(int32_T)(b_m + 4)]; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_hq[(int32_T)(b_m + 4)]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate[(int32_T)(b_m + 8)]; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_hq[(int32_T)(b_m + 8)]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate[(int32_T)(b_m + 12)]; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_hq[(int32_T)(b_m + 12)]; - } - } - - // Sum: '/Add1' incorporates: - // Constant: '/Constant2' - // Product: '/Product1' - // Product: '/Product5' - // Sum: '/Add2' - - for (i = 0; i < 4; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = (rtb_Product2[(int32_T)(i << 2)] - - rtb_VectorConcatenate_l[(int32_T)(i << 2)]) * hr_quat_ISS2hr_idx_1 / - est_estimator_P->Constant2_Value_j0 + est_estimator_B-> - sf_MATLABFunction.y[(int32_T)(i << 2)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = (rtb_Product2 - [(int32_T)((int32_T)(i << 2) + 1)] - rtb_VectorConcatenate_l[(int32_T) - ((int32_T)(i << 2) + 1)]) * hr_quat_ISS2hr_idx_1 / - est_estimator_P->Constant2_Value_j0 + est_estimator_B-> - sf_MATLABFunction.y[(int32_T)((int32_T)(i << 2) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = (rtb_Product2 - [(int32_T)((int32_T)(i << 2) + 2)] - rtb_VectorConcatenate_l[(int32_T) - ((int32_T)(i << 2) + 2)]) * hr_quat_ISS2hr_idx_1 / - est_estimator_P->Constant2_Value_j0 + est_estimator_B-> - sf_MATLABFunction.y[(int32_T)((int32_T)(i << 2) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(i << 2))] = (rtb_Product2 - [(int32_T)((int32_T)(i << 2) + 3)] - rtb_VectorConcatenate_l[(int32_T) - ((int32_T)(i << 2) + 3)]) * hr_quat_ISS2hr_idx_1 / - est_estimator_P->Constant2_Value_j0 + est_estimator_B-> - sf_MATLABFunction.y[(int32_T)((int32_T)(i << 2) + 3)]; - } - - // End of Sum: '/Add1' - - // Product: '/Product1' incorporates: - // UnitDelay: '/Unit Delay1' - - for (i = 0; i < 4; i++) { - rtb_Sum_k1 = rtb_VectorConcatenate[(int32_T)(i + 12)] * - est_estimator_DW->UnitDelay1_DSTATE[3] + (rtb_VectorConcatenate[(int32_T) - (i + 8)] * est_estimator_DW->UnitDelay1_DSTATE[2] + - (rtb_VectorConcatenate[(int32_T)(i + 4)] * - est_estimator_DW->UnitDelay1_DSTATE[1] + rtb_VectorConcatenate[i] * - est_estimator_DW->UnitDelay1_DSTATE[0])); - rtb_Product1[i] = rtb_Sum_k1; - } - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product1[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize(rtb_Product1, rtb_Merge_o, - (P_Normalize_est_estimator_T *)&est_estimator_P->Normalize); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge_o[0] = rtb_Product1[0]; - rtb_Merge_o[1] = rtb_Product1[1]; - rtb_Merge_o[2] = rtb_Product1[2]; - rtb_Merge_o[3] = rtb_Product1[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Sum_k1 = (real32_T)sqrt((real_T)(((rtb_Merge_o[0] * rtb_Merge_o[0] + - rtb_Merge_o[1] * rtb_Merge_o[1]) + rtb_Merge_o[2] * rtb_Merge_o[2]) + - rtb_Merge_o[3] * rtb_Merge_o[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Sum_k1 > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize_p(rtb_Merge_o, rtb_Sum_k1, rtb_Product1); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product1[0] = rtb_Merge_o[0]; - rtb_Product1[1] = rtb_Merge_o[1]; - rtb_Product1[2] = rtb_Merge_o[2]; - rtb_Product1[3] = rtb_Merge_o[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_k1 = rtb_Product1[3] * rtb_Product1[3] * est_estimator_P->Gain_Gain_c0 - - (real32_T)est_estimator_P->Constant1_Value_c; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_n[i] = (real32_T)est_estimator_P->Constant2_Value_ja[i]; - } - - rtb_Assignment_n[0] = rtb_Sum_k1; - rtb_Assignment_n[4] = rtb_Sum_k1; - rtb_Assignment_n[8] = rtb_Sum_k1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_il * rtb_Product1[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Assignment_ef[0] = (real32_T)est_estimator_P->Constant3_Value_c; - rtb_Assignment_ef[1] = rtb_Product1[2]; - rtb_Assignment_ef[2] = est_estimator_P->Gain_Gain_lj * rtb_Product1[1]; - rtb_Assignment_ef[3] = est_estimator_P->Gain1_Gain_lb * rtb_Product1[2]; - rtb_Assignment_ef[4] = (real32_T)est_estimator_P->Constant3_Value_c; - rtb_Assignment_ef[5] = rtb_Product1[0]; - rtb_Assignment_ef[6] = rtb_Product1[1]; - rtb_Assignment_ef[7] = est_estimator_P->Gain2_Gain_m4 * rtb_Product1[0]; - rtb_Assignment_ef[8] = (real32_T)est_estimator_P->Constant3_Value_c; - - // Math: '/Math Function' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product' - // Product: '/Product1' - // Sum: '/Sum1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = rtb_Product1[0] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 3)] = rtb_Product1[1] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 6)] = rtb_Product1[2] * rtb_Product1[i]; - } - - for (i = 0; i < 3; i++) { - rtb_Assignment_b[(int32_T)(3 * i)] = (rtb_Assignment_n[i] - rtb_Sum_k1 * - rtb_Assignment_ef[i]) + rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_j; - rtb_Assignment_b[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_n - [(int32_T)(i + 3)] - rtb_Assignment_ef[(int32_T)(i + 3)] * rtb_Sum_k1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_j; - rtb_Assignment_b[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_n - [(int32_T)(i + 6)] - rtb_Assignment_ef[(int32_T)(i + 6)] * rtb_Sum_k1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_j; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_k1 = est_estimator_P->tun_abp_quat_body2imu[3] * - est_estimator_P->tun_abp_quat_body2imu[3] * est_estimator_P->Gain_Gain_p - - (real32_T)est_estimator_P->Constant1_Value_l; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_n[i] = (real32_T)est_estimator_P->Constant2_Value_bl[i]; - } - - rtb_Assignment_n[0] = rtb_Sum_k1; - rtb_Assignment_n[4] = rtb_Sum_k1; - rtb_Assignment_n[8] = rtb_Sum_k1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant2' - - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_fe * - est_estimator_P->tun_abp_quat_body2imu[3]; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant4' - // Constant: '/Constant6' - // Product: '/Product' - - if (est_estimator_P->tun_ase_gravity_removal > - est_estimator_P->Switch_Threshold) { - // Gain: '/Gain1' incorporates: - // Inport: '/true_q_ISS2B' - - rtb_Gain1_f = est_estimator_P->Gain1_Gain_d * est_estimator_U_Q_ISS2B[3]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Inport: '/true_q_ISS2B' - // Math: '/Math Function' - - rtb_Sum_l = est_estimator_U_Q_ISS2B[3] * est_estimator_U_Q_ISS2B[3] * - est_estimator_P->Gain_Gain_l - (real32_T) - est_estimator_P->Constant1_Value_k; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_ef[i] = (real32_T)est_estimator_P->Constant2_Value_p[i]; - } - - rtb_Assignment_ef[0] = rtb_Sum_l; - rtb_Assignment_ef[4] = rtb_Sum_l; - rtb_Assignment_ef[8] = rtb_Sum_l; - - // End of Assignment: '/Assignment' - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Inport: '/true_q_ISS2B' - - rtb_Assignment_l[0] = (real32_T)est_estimator_P->Constant3_Value_i; - rtb_Assignment_l[1] = est_estimator_U_Q_ISS2B[2]; - rtb_Assignment_l[2] = est_estimator_P->Gain_Gain * est_estimator_U_Q_ISS2B[1]; - rtb_Assignment_l[3] = est_estimator_P->Gain1_Gain * est_estimator_U_Q_ISS2B - [2]; - rtb_Assignment_l[4] = (real32_T)est_estimator_P->Constant3_Value_i; - rtb_Assignment_l[5] = est_estimator_U_Q_ISS2B[0]; - rtb_Assignment_l[6] = est_estimator_U_Q_ISS2B[1]; - rtb_Assignment_l[7] = est_estimator_P->Gain2_Gain_e * - est_estimator_U_Q_ISS2B[0]; - rtb_Assignment_l[8] = (real32_T)est_estimator_P->Constant3_Value_i; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Inport: '/true_q_ISS2B' - // Math: '/Math Function1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = est_estimator_U_Q_ISS2B[i] * est_estimator_U_Q_ISS2B[0]; - rtb_Product1_0[(int32_T)(i + 3)] = est_estimator_U_Q_ISS2B[i] * - est_estimator_U_Q_ISS2B[1]; - rtb_Product1_0[(int32_T)(i + 6)] = est_estimator_U_Q_ISS2B[i] * - est_estimator_U_Q_ISS2B[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_o[(int32_T)(3 * i)] = (rtb_Assignment_ef[(int32_T)(3 * i)] - - rtb_Assignment_l[(int32_T)(3 * i)] * rtb_Gain1_f) + rtb_Product1_0 - [(int32_T)(3 * i)] * est_estimator_P->Gain2_Gain; - rtb_Assignment_o[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_ef - [(int32_T)((int32_T)(3 * i) + 1)] - rtb_Assignment_l[(int32_T)((int32_T) - (3 * i) + 1)] * rtb_Gain1_f) + rtb_Product1_0[(int32_T)((int32_T)(3 * i) - + 1)] * est_estimator_P->Gain2_Gain; - rtb_Assignment_o[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_ef - [(int32_T)((int32_T)(3 * i) + 2)] - rtb_Assignment_l[(int32_T)((int32_T) - (3 * i) + 2)] * rtb_Gain1_f) + rtb_Product1_0[(int32_T)((int32_T)(3 * i) - + 2)] * est_estimator_P->Gain2_Gain; - } - - // End of Sum: '/Sum1' - for (i = 0; i < 3; i++) { - rtb_Sum1_k3[i] = rtb_Assignment_o[(int32_T)(i + 6)] * - est_estimator_P->tun_ase_gravity_accel[2] + (rtb_Assignment_o[(int32_T) - (i + 3)] * est_estimator_P->tun_ase_gravity_accel[1] + - rtb_Assignment_o[i] * est_estimator_P->tun_ase_gravity_accel[0]); - } - } else { - rtb_Sum1_k3[0] = est_estimator_P->Constant6_Value[0]; - rtb_Sum1_k3[1] = est_estimator_P->Constant6_Value[1]; - rtb_Sum1_k3[2] = est_estimator_P->Constant6_Value[2]; - } - - // End of Switch: '/Switch' - - // Product: '/Product' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_3[0] = (real32_T)est_estimator_P->Constant3_Value_o; - tmp_3[1] = est_estimator_P->tun_abp_quat_body2imu[2]; - tmp_3[2] = est_estimator_P->Gain_Gain_az * - est_estimator_P->tun_abp_quat_body2imu[1]; - tmp_3[3] = est_estimator_P->Gain1_Gain_ln * - est_estimator_P->tun_abp_quat_body2imu[2]; - tmp_3[4] = (real32_T)est_estimator_P->Constant3_Value_o; - tmp_3[5] = est_estimator_P->tun_abp_quat_body2imu[0]; - tmp_3[6] = est_estimator_P->tun_abp_quat_body2imu[1]; - tmp_3[7] = est_estimator_P->Gain2_Gain_c * - est_estimator_P->tun_abp_quat_body2imu[0]; - tmp_3[8] = (real32_T)est_estimator_P->Constant3_Value_o; - - // Math: '/Math Function' incorporates: - // Constant: '/Constant2' - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = est_estimator_P->tun_abp_quat_body2imu[0] * - est_estimator_P->tun_abp_quat_body2imu[i]; - rtb_Product1_0[(int32_T)(i + 3)] = est_estimator_P->tun_abp_quat_body2imu[1] - * est_estimator_P->tun_abp_quat_body2imu[i]; - rtb_Product1_0[(int32_T)(i + 6)] = est_estimator_P->tun_abp_quat_body2imu[2] - * est_estimator_P->tun_abp_quat_body2imu[i]; - } - - // End of Math: '/Math Function' - for (i = 0; i < 3; i++) { - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - rtb_Assignment_l[(int32_T)(3 * i)] = (rtb_Assignment_n[i] - rtb_Sum_k1 * - tmp_3[i]) + rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_i5; - rtb_Assignment_l[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_n - [(int32_T)(i + 3)] - tmp_3[(int32_T)(i + 3)] * rtb_Sum_k1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_i5; - rtb_Assignment_l[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_n - [(int32_T)(i + 6)] - tmp_3[(int32_T)(i + 6)] * rtb_Sum_k1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_i5; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant5' - // Inport: '/imu_msg' - // Product: '/Product' - - rtb_P_B_ISS_ISS[i] = est_estimator_U_imu_msg_c->imu_A_B_ECI_sensor[i] - - est_estimator_P->ase_accel_fixed_bias[i]; - } - - // Sum: '/Sum2' incorporates: - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Sum2[i] = ((rtb_Assignment_l[(int32_T)(i + 3)] * rtb_P_B_ISS_ISS[1] + - rtb_Assignment_l[i] * rtb_P_B_ISS_ISS[0]) + - rtb_Assignment_l[(int32_T)(i + 6)] * rtb_P_B_ISS_ISS[2]) - - rtb_Sum1_k3[i]; - } - - // End of Sum: '/Sum2' - - // Outputs for Iterator SubSystem: '/filter_with_HP_filter' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr = 0; ForEach_itr < 3; ForEach_itr++) { - // MATLAB Function: '/MATLAB Function1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant2' - // Selector: '/Selector' - - est_estimator_MATLABFunction1(est_estimator_P->astrobee_fsw_step_size, - est_estimator_P->fam_impeller_speeds[(int32_T)((int32_T)rtb_Saturation_n - - 1)], &est_estimator_B->CoreSubsys_l[ForEach_itr].sf_MATLABFunction1, - &est_estimator_DW->CoreSubsys_l[ForEach_itr].sf_MATLABFunction1); - - // DiscreteTransferFcn: '/Discrete Transfer Fcn' incorporates: - // DiscreteTransferFcn: '/3 Hz Low Pass' - - rtb_Sum_k1 = (est_estimator_P->CoreSubsys_l.uHzLowPass_NumCoef * - est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states - - est_estimator_B->CoreSubsys_l[ForEach_itr]. - sf_MATLABFunction1.den_out[1] * est_estimator_DW-> - CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0]) - - est_estimator_B->CoreSubsys_l[ForEach_itr].sf_MATLABFunction1.den_out[2] * - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1]; - rtb_Switch1 = (est_estimator_B->CoreSubsys_l[ForEach_itr]. - sf_MATLABFunction1.num_out[0] * rtb_Sum_k1 + - est_estimator_B->CoreSubsys_l[ForEach_itr]. - sf_MATLABFunction1.num_out[1] * - est_estimator_DW->CoreSubsys_l[ForEach_itr]. - DiscreteTransferFcn_states[0]) + - est_estimator_B->CoreSubsys_l[ForEach_itr].sf_MATLABFunction1.num_out[2] * - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1]; - - // DiscreteTransferFcn: '/High Pass Filter' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_In1_at_outport_0' - - rtb_Sum_l = (rtb_Sum2[ForEach_itr] - est_estimator_P->tun_grav_hp_den[1] * - est_estimator_DW->CoreSubsys_l[ForEach_itr]. - HighPassFilter_states) / est_estimator_P->tun_grav_hp_den[0]; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant1' - // DiscreteTransferFcn: '/High Pass Filter' - // ForEachSliceSelector: '/ImpSel_InsertedFor_In1_at_outport_0' - - if ((int32_T)est_estimator_P->tun_grav_hp_enable_f != 0) { - hr_quat_ISS2hr_idx_1 = est_estimator_P->tun_grav_hp_num[0] * rtb_Sum_l + - est_estimator_P->tun_grav_hp_num[1] * est_estimator_DW-> - CoreSubsys_l[ForEach_itr].HighPassFilter_states; - } else { - hr_quat_ISS2hr_idx_1 = rtb_Sum2[ForEach_itr]; - } - - // End of Switch: '/Switch' - - // Update for DiscreteTransferFcn: '/3 Hz Low Pass' - est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states = - (hr_quat_ISS2hr_idx_1 - est_estimator_P->CoreSubsys_l.uHzLowPass_DenCoef[1] - * est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states) / - est_estimator_P->CoreSubsys_l.uHzLowPass_DenCoef[0]; - - // Update for DiscreteTransferFcn: '/Discrete Transfer Fcn' - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1] = - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0]; - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0] = - rtb_Sum_k1; - - // Update for DiscreteTransferFcn: '/High Pass Filter' - est_estimator_DW->CoreSubsys_l[ForEach_itr].HighPassFilter_states = - rtb_Sum_l; - - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_Out1_at_inport_0' - rtb_ImpAsg_InsertedFor_Out1_at_[ForEach_itr] = rtb_Switch1; - } - - // End of Outputs for SubSystem: '/filter_with_HP_filter' - - // Sum: '/Sum1' incorporates: - // UnitDelay: '/Unit Delay6' - - rtb_ImpAsg_InsertedFor_Out1_at_[0] -= est_estimator_DW->UnitDelay6_DSTATE[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] -= est_estimator_DW->UnitDelay6_DSTATE[1]; - rtb_Sum_l = rtb_ImpAsg_InsertedFor_Out1_at_[2] - - est_estimator_DW->UnitDelay6_DSTATE[2]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = rtb_Sum_l; - for (i = 0; i < 3; i++) { - // Product: '/Product' - rtb_Sum_k1 = rtb_Assignment_b[(int32_T)(i + 6)] * rtb_Sum_l + - (rtb_Assignment_b[(int32_T)(i + 3)] * rtb_ImpAsg_InsertedFor_Out1_at_[1] + - rtb_Assignment_b[i] * rtb_ImpAsg_InsertedFor_Out1_at_[0]); - - // Sum: '/Sum5' incorporates: - // Gain: '/Gain1' - // UnitDelay: '/Unit Delay4' - - rtb_Gain1_f = (real32_T)est_estimator_P->ase_ts * rtb_Sum_k1 + - est_estimator_DW->UnitDelay4_DSTATE[i]; - - // Sum: '/Sum2' incorporates: - // Gain: '/Gain' - // UnitDelay: '/Unit Delay26' - - rtb_P_B_ISS_ISS[i] = (real32_T)est_estimator_P->ase_ts * rtb_Gain1_f + - est_estimator_DW->UnitDelay26_DSTATE[i]; - - // Product: '/Product' - rtb_Sum1_k3[i] = rtb_Sum_k1; - - // Sum: '/Sum5' - rtb_Sum2[i] = rtb_Gain1_f; - } - - // S-Function (sfix_bitop): '/Bitwise Operator' incorporates: - // BusAssignment: '/Bus Assignment' - - rtb_BitwiseOperator &= est_estimator_P->BitwiseOperator_BitMask; - - // Sum: '/Sum of Elements1' incorporates: - // Inport: '/landmark_msg' - - UnitDelay_DSTATE_aug_state_enum = (uint32_T) - est_estimator_U_landmark_msg->cvs_valid_flag[0]; - for (ar = 0; ar < 49; ar++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T) - est_estimator_U_landmark_msg->cvs_valid_flag[(int32_T)(ar + 1)]; - } - - rtb_Switch_m = (uint8_T)UnitDelay_DSTATE_aug_state_enum; - - // End of Sum: '/Sum of Elements1' - - // Logic: '/Logical Operator1' incorporates: - // Inport: '/landmark_msg' - - rtb_LogicalOperator = ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[0] - != 0); - for (handrail_knowledge_dims = 0; handrail_knowledge_dims < 49; - handrail_knowledge_dims++) { - rtb_LogicalOperator = (rtb_LogicalOperator || ((int32_T) - est_estimator_U_landmark_msg->cvs_valid_flag[(int32_T) - (handrail_knowledge_dims + 1)] != 0)); - } - - // Logic: '/Logical Operator' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Inport: '/cmc_msg' - // Inport: '/landmark_msg' - // Logic: '/Logical Operator4' - // Logic: '/Logical Operator5' - // Logic: '/Logical Operator6' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator6' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/FixPt Relational Operator' - // RelationalOperator: '/FixPt Relational Operator' - // SignalConversion: '/Signal Conversion' - // UnitDelay: '/Delay Input1' - // UnitDelay: '/Delay Input1' - - rtb_LogicalOperator = ((((est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_map) && (rtb_Switch_m >= - est_estimator_P->Constant_Value_lf)) || ((rtb_Switch_m >= - est_estimator_P->Constant_Value_js) && - (est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_docking))) && - (((est_estimator_U_landmark_msg->cvs_timestamp_sec != - est_estimator_DW->DelayInput1_DSTATE_o) || - (est_estimator_U_landmark_msg->cvs_timestamp_nsec != - est_estimator_DW->DelayInput1_DSTATE_n)) && rtb_LogicalOperator) && - (rtb_BitwiseOperator != 0U)); - - // Logic: '/Logical Operator4' incorporates: - // Inport: '/optical_flow_msg' - // RelationalOperator: '/FixPt Relational Operator' - // RelationalOperator: '/FixPt Relational Operator' - // UnitDelay: '/Delay Input1' - // UnitDelay: '/Delay Input1' - - rtb_of_update_flag = - ((est_estimator_U_cvs_optical_flow_msg_n->cvs_timestamp_sec != - est_estimator_DW->DelayInput1_DSTATE_b) || - (est_estimator_U_cvs_optical_flow_msg_n->cvs_timestamp_nsec != - est_estimator_DW->DelayInput1_DSTATE_d)); - - // Outputs for Enabled SubSystem: '/Enabled Row-Wise SUM' incorporates: - // EnablePort: '/Enable' - - for (nx = 0; nx < 50; nx++) { - if (rtb_of_update_flag) { - // Sum: '/Sum of Elements3' incorporates: - // Inport: '/optical_flow_msg' - - UnitDelay_DSTATE_aug_state_enum = (uint32_T) - est_estimator_U_cvs_optical_flow_msg_n->cvs_valid_flag[nx]; - for (i = 0; i < 15; i++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T) - est_estimator_U_cvs_optical_flow_msg_n->cvs_valid_flag[(int32_T) - ((int32_T)((int32_T)(i + 1) * 50) + nx)]; - } - - est_estimator_B->SumofElements3[nx] = (uint8_T) - UnitDelay_DSTATE_aug_state_enum; - - // End of Sum: '/Sum of Elements3' - } - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare_ic[nx] = (est_estimator_B->SumofElements3[nx] >= - est_estimator_P->CompareToConstant7_const); - } - - // End of Outputs for SubSystem: '/Enabled Row-Wise SUM' - - // Sum: '/Sum of Elements' - UnitDelay_DSTATE_aug_state_enum = (uint32_T)rtb_Compare_ic[0]; - for (ar = 0; ar < 49; ar++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T)rtb_Compare_ic[(int32_T)(ar + 1)]; - } - - // MATLAB Function: '/MATLAB Function' - // MATLAB Function 'predictor/Covariance Propogation/MATLAB Function': ':1' - // ':1:27' - // ':1:24' - // ':1:23' - // ':1:7' - // ':1:4' - S[0] = 0.0F; - S[3] = -rtb_Product1[2]; - S[6] = rtb_Product1[1]; - S[1] = rtb_Product1[2]; - S[4] = 0.0F; - S[7] = -rtb_Product1[0]; - S[2] = -rtb_Product1[1]; - S[5] = rtb_Product1[0]; - S[8] = 0.0F; - - // ':1:7' - // ':1:8' - // ':1:9' - rtb_Sum_k1 = rtb_Product1[3]; - rtb_Gain1_f = rtb_Product1[3]; - for (i = 0; i < 3; i++) { - rtb_Product1_1[(int32_T)(3 * i)] = rtb_Sum_k1 * (real32_T)d[i] + S[i]; - rtb_Product1_2[(int32_T)(i << 2)] = (real32_T)d[(int32_T)(3 * i)] * - rtb_Gain1_f - S[(int32_T)(3 * i)]; - rtb_Product1_1[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)d[(int32_T)(i + - 3)] * rtb_Sum_k1 + S[(int32_T)(i + 3)]; - rtb_Product1_2[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)d[(int32_T) - ((int32_T)(3 * i) + 1)] * rtb_Gain1_f - S[(int32_T)((int32_T)(3 * i) + 1)]; - rtb_Product1_1[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)d[(int32_T)(i + - 6)] * rtb_Sum_k1 + S[(int32_T)(i + 6)]; - rtb_Product1_2[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)d[(int32_T) - ((int32_T)(3 * i) + 2)] * rtb_Gain1_f - S[(int32_T)((int32_T)(3 * i) + 2)]; - rtb_Product1_1[(int32_T)(9 + i)] = -rtb_Product1[i]; - rtb_Product1_2[(int32_T)(3 + (int32_T)(i << 2))] = -rtb_Product1[i]; - } - - // ':1:11' - // ':1:15' - // ':1:16' - // ':1:23' - // state_trans_1 = single(eye(15) + F_E*0.01); % Consider using expm - // ':1:32' - tmp_4[0] = 0.0F; - tmp_4[3] = -hr_quat_ISS2hr_idx_0; - tmp_4[6] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - tmp_4[1] = hr_quat_ISS2hr_idx_0; - tmp_4[4] = 0.0F; - tmp_4[7] = -rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - tmp_4[2] = -rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - tmp_4[5] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - tmp_4[8] = 0.0F; - for (br = 0; br < 3; br++) { - accel[br] = (real32_T)fabs((real_T)rtb_ImpAsg_InsertedFor_Out1_at_[br]); - for (i = 0; i < 3; i++) { - C[(int32_T)(br + (int32_T)(3 * i))] = 0.0F; - C[(int32_T)(br + (int32_T)(3 * i))] += rtb_Product1_2[(int32_T)(i << 2)] * - rtb_Product1_1[br]; - C[(int32_T)(br + (int32_T)(3 * i))] += rtb_Product1_2[(int32_T)((int32_T) - (i << 2) + 1)] * rtb_Product1_1[(int32_T)(br + 3)]; - C[(int32_T)(br + (int32_T)(3 * i))] += rtb_Product1_2[(int32_T)((int32_T) - (i << 2) + 2)] * rtb_Product1_1[(int32_T)(br + 6)]; - C[(int32_T)(br + (int32_T)(3 * i))] += rtb_Product1_2[(int32_T)((int32_T) - (i << 2) + 3)] * rtb_Product1_1[(int32_T)(br + 9)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * br))] = -C[(int32_T)((int32_T) - (3 * i) + br)]; - } - } - - tmp_5[0] = 0.0F; - tmp_5[3] = -accel[2]; - tmp_5[6] = accel[1]; - tmp_5[1] = accel[2]; - tmp_5[4] = 0.0F; - tmp_5[7] = -accel[0]; - tmp_5[2] = -accel[1]; - tmp_5[5] = accel[0]; - tmp_5[8] = 0.0F; - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m < 3; b_m++) { - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_5[(int32_T)(3 * - b_m)] * rtb_Assignment_b[i]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_5[(int32_T) - ((int32_T)(3 * b_m) + 1)] * rtb_Assignment_b[(int32_T)(i + 3)]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_5[(int32_T) - ((int32_T)(3 * b_m) + 2)] * rtb_Assignment_b[(int32_T)(i + 6)]; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * i))] = -tmp_4[(int32_T) - ((int32_T)(3 * i) + b_m)]; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * (int32_T)(i + 3)))] = - (real32_T)b[(int32_T)((int32_T)(3 * i) + b_m)]; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * (int32_T)(i + 6)))] = 0.0F; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * (int32_T)(i + 9)))] = 0.0F; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * (int32_T)(i + 12)))] = 0.0F; - } - } - - for (i = 0; i < 15; i++) { - rtb_state_trans[(int32_T)(3 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(4 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(5 + (int32_T)(15 * i))] = 0.0F; - } - - for (i = 0; i < 3; i++) { - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * i))] = rtb_Product1_0[(int32_T) - (3 * i)]; - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * (int32_T)(i + 3)))] = 0.0F; - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * (int32_T)(i + 6)))] = 0.0F; - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * (int32_T)(i + 9)))] = -C[i]; - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * (int32_T)(i + 12)))] = 0.0F; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * i))] = rtb_Product1_0[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * (int32_T)(i + 3)))] = 0.0F; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * (int32_T)(i + 6)))] = 0.0F; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * (int32_T)(i + 9)))] = -C - [(int32_T)(i + 3)]; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * (int32_T)(i + 12)))] = 0.0F; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * i))] = rtb_Product1_0[(int32_T) - ((int32_T)(3 * i) + 2)]; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * (int32_T)(i + 3)))] = 0.0F; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * (int32_T)(i + 6)))] = 0.0F; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * (int32_T)(i + 9)))] = -C - [(int32_T)(i + 6)]; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * (int32_T)(i + 12)))] = 0.0F; - } - - for (i = 0; i < 15; i++) { - rtb_state_trans[(int32_T)(9 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(12 + (int32_T)(15 * i))] = (real32_T)c[(int32_T)(3 - * i)]; - rtb_state_trans[(int32_T)(10 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(13 + (int32_T)(15 * i))] = (real32_T)c[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_state_trans[(int32_T)(11 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(14 + (int32_T)(15 * i))] = (real32_T)c[(int32_T) - ((int32_T)(3 * i) + 2)]; - for (b_m = 0; b_m < 15; b_m++) { - rtb_ex_matrix_multiply4[(int32_T)(b_m + (int32_T)(15 * i))] = - rtb_state_trans[(int32_T)((int32_T)(15 * i) + b_m)] * (real32_T) - est_estimator_P->ase_ts; - } - } - - rtb_Sum_k1 = 0.0F; - handrail_knowledge_dims = 0; - exitg2 = false; - while ((!exitg2) && (handrail_knowledge_dims < 15)) { - rtb_Gain1_f = 0.0F; - for (num_original = 0; num_original < 15; num_original++) { - rtb_Gain1_f += (real32_T)fabs((real_T)rtb_ex_matrix_multiply4[(int32_T) - ((int32_T)(15 * handrail_knowledge_dims) + num_original)]); - } - - if (rtIsNaNF(rtb_Gain1_f)) { - rtb_Sum_k1 = (rtNaNF); - exitg2 = true; - } else { - if (rtb_Gain1_f > rtb_Sum_k1) { - rtb_Sum_k1 = rtb_Gain1_f; - } - - handrail_knowledge_dims++; - } - } - - if (rtb_Sum_k1 <= 3.92572474F) { - num_original = 0; - exitg1 = false; - while ((!exitg1) && (num_original < 3)) { - if (rtb_Sum_k1 <= theta[num_original]) { - hdbaohdbkngdbimo_PadeApproximantOfDegree(rtb_ex_matrix_multiply4, - (uint8_T)(int32_T)((int32_T)(num_original << 1) + 3), rtb_state_trans); - exitg1 = true; - } else { - num_original++; - } - } - } else { - rtb_Gain1_f = rtb_Sum_k1 / 3.92572474F; - if ((!rtIsInfF(rtb_Gain1_f)) && (!rtIsNaNF(rtb_Gain1_f))) { - rtb_Gain1_f = (real32_T)frexp((real_T)rtb_Gain1_f, &s12_iter); - rtb_Sum_k1 = (real32_T)s12_iter; - } else { - rtb_Sum_k1 = 0.0F; - } - - if (rtb_Gain1_f == 0.5F) { - rtb_Sum_k1--; - } - - rtb_Gain1_f = rt_powf_snf(2.0F, rtb_Sum_k1); - for (i = 0; i < 225; i++) { - rtb_Sum_b[i] = rtb_ex_matrix_multiply4[i] / rtb_Gain1_f; - } - - hdbaohdbkngdbimo_PadeApproximantOfDegree(rtb_Sum_b, 7U, rtb_state_trans); - for (num_original = 0; num_original <= (int32_T)((int32_T)rtb_Sum_k1 - 1); - num_original++) { - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_ex_matrix_multiply4[(int32_T)(i + (int32_T)(15 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - rtb_ex_matrix_multiply4[(int32_T)(i + (int32_T)(15 * b_m))] += - rtb_state_trans[(int32_T)((int32_T)(15 * i_0) + i)] * - rtb_state_trans[(int32_T)((int32_T)(15 * b_m) + i_0)]; - } - } - } - - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * i))] = - rtb_ex_matrix_multiply4[(int32_T)((int32_T)(15 * i) + b_m)]; - } - } - } - } - - // End of MATLAB Function: '/MATLAB Function' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - // Paramaterize Ts - rtb_Sum_k1 = rtb_Product1[3] * rtb_Product1[3] * est_estimator_P->Gain_Gain_fm - - (real32_T)est_estimator_P->Constant1_Value_e; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_bd[i]; - } - - rtb_Assignment_b[0] = rtb_Sum_k1; - rtb_Assignment_b[4] = rtb_Sum_k1; - rtb_Assignment_b[8] = rtb_Sum_k1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_l4 * rtb_Product1[3]; - - // Assignment: '/Assignment1' incorporates: - // Constant: '/Constant5' - - memcpy(&rtb_Assignment1[0], &est_estimator_P->Constant5_Value_p[0], (uint32_T) - (180U * sizeof(real32_T))); - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_6[0] = (real32_T)est_estimator_P->Constant3_Value_kc; - tmp_6[1] = rtb_Product1[2]; - tmp_6[2] = est_estimator_P->Gain_Gain_k * rtb_Product1[1]; - tmp_6[3] = est_estimator_P->Gain1_Gain_j5 * rtb_Product1[2]; - tmp_6[4] = (real32_T)est_estimator_P->Constant3_Value_kc; - tmp_6[5] = rtb_Product1[0]; - tmp_6[6] = rtb_Product1[1]; - tmp_6[7] = est_estimator_P->Gain2_Gain_i5w * rtb_Product1[0]; - tmp_6[8] = (real32_T)est_estimator_P->Constant3_Value_kc; - - // Math: '/Math Function1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = rtb_Product1[0] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 3)] = rtb_Product1[1] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 6)] = rtb_Product1[2] * rtb_Product1[i]; - } - - // End of Math: '/Math Function1' - - // Assignment: '/Assignment1' incorporates: - // Gain: '/Gain2' - // Gain: '/Gain2' - // Product: '/Product' - // Sum: '/Sum1' - - for (i = 0; i < 3; i++) { - rtb_Assignment1[(int32_T)(6 + (int32_T)(15 * (int32_T)(6 + i)))] = - ((rtb_Assignment_b[i] - rtb_Sum_k1 * tmp_6[i]) + rtb_Product1_0[(int32_T) - (3 * i)] * est_estimator_P->Gain2_Gain_kj) * - est_estimator_P->Gain2_Gain_cx; - rtb_Assignment1[(int32_T)(7 + (int32_T)(15 * (int32_T)(6 + i)))] = - ((rtb_Assignment_b[(int32_T)(i + 3)] - tmp_6[(int32_T)(i + 3)] * - rtb_Sum_k1) + rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_kj) * est_estimator_P->Gain2_Gain_cx; - rtb_Assignment1[(int32_T)(8 + (int32_T)(15 * (int32_T)(6 + i)))] = - ((rtb_Assignment_b[(int32_T)(i + 6)] - tmp_6[(int32_T)(i + 6)] * - rtb_Sum_k1) + rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_kj) * est_estimator_P->Gain2_Gain_cx; - } - - // MATLAB Function: '/diag' incorporates: - // Constant: '/Constant9' - - // MATLAB Function 'predictor/Covariance Propogation/diag': ':1' - // ':1:3' - x[0] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - x[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - x[2] = hr_quat_ISS2hr_idx_0; - x[3] = 0.0F; - x[4] = 0.0F; - x[5] = 0.0F; - x[6] = 0.0F; - x[7] = 0.0F; - x[8] = 0.0F; - x[9] = 0.0F; - x[10] = 0.0F; - x[11] = 0.0F; - br = 0; - for (num_original = 0; num_original < 12; num_original++) { - rtb_Product1_1[num_original] = est_estimator_P->tun_ase_Q_imu[num_original]; - d_1 = ((real32_T)fabs((real_T)x[num_original]) >= - est_estimator_P->tun_ase_max_gyro); - if (d_1) { - br++; - } - - d_0[num_original] = d_1; - } - - handrail_knowledge_dims = br; - br = 0; - for (num_original = 0; num_original < 12; num_original++) { - if (d_0[num_original]) { - b_data[br] = (int8_T)(int32_T)(num_original + 1); - br++; - } - } - - // ':1:3' - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - rtb_Product1_1[(int32_T)((int32_T)b_data[i] - 1)] = - est_estimator_P->tun_ase_q_saturated_gyro; - } - - // ':1:4' - x[0] = 0.0F; - x[1] = 0.0F; - x[2] = 0.0F; - x[3] = 0.0F; - x[4] = 0.0F; - x[5] = 0.0F; - x[6] = rtb_ImpAsg_InsertedFor_Out1_at_[0]; - x[7] = rtb_ImpAsg_InsertedFor_Out1_at_[1]; - x[8] = rtb_Sum_l; - x[9] = 0.0F; - x[10] = 0.0F; - x[11] = 0.0F; - num_original = 0; - for (br = 0; br < 12; br++) { - d_1 = ((real32_T)fabs((real_T)x[br]) >= est_estimator_P->tun_ase_max_accel); - if (d_1) { - num_original++; - } - - d_0[br] = d_1; - } - - handrail_knowledge_dims = num_original; - num_original = 0; - for (nx = 0; nx < 12; nx++) { - if (d_0[nx]) { - c_data[num_original] = (int8_T)(int32_T)(nx + 1); - num_original++; - } - } - - // ':1:4' - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - rtb_Product1_1[(int32_T)((int32_T)c_data[i] - 1)] = - est_estimator_P->tun_ase_q_saturated_accel; - } - - // ':1:5' - memset(&rtb_Q[0], 0, (uint32_T)(144U * sizeof(real32_T))); - for (num_original = 0; num_original < 12; num_original++) { - rtb_Q[(int32_T)(num_original + (int32_T)(12 * num_original))] = - rtb_Product1_1[num_original]; - } - - // End of MATLAB Function: '/diag' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply1' - matrix_multiply((real32_T*)&rtb_Assignment1[0], 15, 12, (real32_T*)&rtb_Q[0], - 12, 12, &rtb_ex_matrix_multiply1[0]); - - // Math: '/Math Function2' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 12; b_m++) { - rtb_MathFunction2[(int32_T)(b_m + (int32_T)(12 * i))] = rtb_Assignment1 - [(int32_T)((int32_T)(15 * b_m) + i)]; - } - } - - // End of Math: '/Math Function2' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply3' - matrix_multiply((real32_T*)&rtb_ex_matrix_multiply1[0], 15, 12, (real32_T*) - &rtb_MathFunction2[0], 12, 15, &rtb_ex_matrix_multiply3[0]); - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply' - matrix_multiply((real32_T*)&rtb_state_trans[0], 15, 15, (real32_T*) - &rtb_ex_matrix_multiply3[0], 15, 15, &rtb_ex_matrix_multiply5 - [0]); - - // Math: '/Math Function5' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_ex_matrix_multiply4[(int32_T)(b_m + (int32_T)(15 * i))] = - rtb_state_trans[(int32_T)((int32_T)(15 * b_m) + i)]; - } - } - - // End of Math: '/Math Function5' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply2' - matrix_multiply((real32_T*)&rtb_ex_matrix_multiply5[0], 15, 15, (real32_T*) - &rtb_ex_matrix_multiply4[0], 15, 15, &rtb_MathFunction_eu[0]); - - // Sum: '/Sum' - for (i = 0; i < 225; i++) { - rtb_Sum_b[i] = rtb_MathFunction_eu[i] + rtb_ex_matrix_multiply3[i]; - } - - // End of Sum: '/Sum' - - // Gain: '/Gain3' - num_augs = 0.5 * est_estimator_P->ase_ts; - - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Delay: '/Delay' - - // MATLAB Function 'est_estimator/filter_prep/kfl_system_prep/MATLAB Function': ':1' - // ':1:4' - memcpy(&est_estimator_B->Assignment[0], &est_estimator_DW->Delay_DSTATE[0], - (uint32_T)(13689U * sizeof(real32_T))); - if (rtb_FixPtRelationalOperator_n) { - // ':1:6' - // Quat covariance is 1:3, Pos covariance is 13:15 - // ':1:8' - for (i = 0; i < 117; i++) { - est_estimator_B->Assignment[(int32_T)(117 * i)] = 0.0F; - est_estimator_B->Assignment[(int32_T)(1 + (int32_T)(117 * i))] = 0.0F; - est_estimator_B->Assignment[(int32_T)(2 + (int32_T)(117 * i))] = 0.0F; - } - - // ':1:9' - for (i = 0; i < 3; i++) { - memset(&est_estimator_B->Assignment[(int32_T)(i * 117)], 0, (uint32_T) - (117U * sizeof(real32_T))); - } - - // ':1:10' - for (i = 0; i < 9; i++) { - S[i] = 0.0F; - } - - S[0] = est_estimator_P->tun_ic_cov_quat; - S[4] = est_estimator_P->tun_ic_cov_quat; - S[8] = est_estimator_P->tun_ic_cov_quat; - for (i = 0; i < 3; i++) { - est_estimator_B->Assignment[(int32_T)(117 * i)] = S[(int32_T)(3 * i)]; - est_estimator_B->Assignment[(int32_T)(1 + (int32_T)(117 * i))] = S - [(int32_T)((int32_T)(3 * i) + 1)]; - est_estimator_B->Assignment[(int32_T)(2 + (int32_T)(117 * i))] = S - [(int32_T)((int32_T)(3 * i) + 2)]; - } - - // ':1:12' - for (i = 0; i < 117; i++) { - est_estimator_B->Assignment[(int32_T)(12 + (int32_T)(117 * i))] = 0.0F; - est_estimator_B->Assignment[(int32_T)(13 + (int32_T)(117 * i))] = 0.0F; - est_estimator_B->Assignment[(int32_T)(14 + (int32_T)(117 * i))] = 0.0F; - } - - // ':1:13' - for (i = 0; i < 3; i++) { - memset(&est_estimator_B->Assignment[(int32_T)((int32_T)(i * 117) + 1404)], - 0, (uint32_T)(117U * sizeof(real32_T))); - } - - // ':1:14' - for (i = 0; i < 9; i++) { - S[i] = 0.0F; - } - - S[0] = est_estimator_P->tun_ic_cov_pos; - S[4] = est_estimator_P->tun_ic_cov_pos; - S[8] = est_estimator_P->tun_ic_cov_pos; - for (i = 0; i < 3; i++) { - est_estimator_B->Assignment[(int32_T)(12 + (int32_T)(117 * (int32_T)(12 + - i)))] = S[(int32_T)(3 * i)]; - est_estimator_B->Assignment[(int32_T)(13 + (int32_T)(117 * (int32_T)(12 + - i)))] = S[(int32_T)((int32_T)(3 * i) + 1)]; - est_estimator_B->Assignment[(int32_T)(14 + (int32_T)(117 * (int32_T)(12 + - i)))] = S[(int32_T)((int32_T)(3 * i) + 2)]; - } - } - - // End of MATLAB Function: '/MATLAB Function' - - // Selector: '/Selector' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_MathFunction_eu[(int32_T)(b_m + (int32_T)(15 * i))] = - est_estimator_B->Assignment[(int32_T)((int32_T)(117 * i) + b_m)]; - } - } - - // End of Selector: '/Selector' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply4' - matrix_multiply((real32_T*)&rtb_state_trans[0], 15, 15, (real32_T*) - &rtb_MathFunction_eu[0], 15, 15, &rtb_ex_matrix_multiply4[0]); - - // Math: '/Math Function' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_MathFunction_eu[(int32_T)(b_m + (int32_T)(15 * i))] = rtb_state_trans - [(int32_T)((int32_T)(15 * b_m) + i)]; - } - } - - // End of Math: '/Math Function' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply5' - matrix_multiply((real32_T*)&rtb_ex_matrix_multiply4[0], 15, 15, (real32_T*) - &rtb_MathFunction_eu[0], 15, 15, &rtb_ex_matrix_multiply5[0]); - - // Sum: '/Sum6' incorporates: - // Gain: '/Gain3' - - for (i = 0; i < 225; i++) { - est_estimator_B->MatrixConcatenate[i] = (real32_T)num_augs * rtb_Sum_b[i] + - rtb_ex_matrix_multiply5[i]; - } - - // End of Sum: '/Sum6' - - // MATLAB Function: '/MATLAB Function2' - // MATLAB Function 'predictor/Covariance Propogation/MATLAB Function2': ':1' - // ':1:7' - // ':1:4' - for (i = 0; i < 102; i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->P_IC[(int32_T)(b_m + (int32_T)(15 * i))] = - est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)(15 + i) * 117) - + b_m)]; - } - } - - // Extract the cross coupled part of the camera and IMU covariance - // Creates a matrix where each column is the indices for each augmentation - // ':1:7' - // Number of columns is number of OF augmentations + ML augmentations - // Collect the indices of the valid current augmentations - // Form a vector with the valid indices corresponding to the current - // number of augmentations - // ':1:12' - mglfbimobiechdbi_bitget(rtb_BusAssignment_aug_state_enu, tmp_1); - br = 0; - for (num_original = 0; num_original < 17; num_original++) { - rtb_FixPtRelationalOperator_n = (tmp_1[num_original] != 0U); - if (rtb_FixPtRelationalOperator_n) { - br++; - } - - b_0[num_original] = rtb_FixPtRelationalOperator_n; - } - - handrail_knowledge_dims = br; - br = 0; - for (num_original = 0; num_original < 17; num_original++) { - if (b_0[num_original]) { - c_data_0[br] = (int8_T)(int32_T)(num_original + 1); - br++; - } - } - - // ':1:12' - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - for (b_m = 0; b_m < 6; b_m++) { - valid_indx_mat_data[(int32_T)(b_m + (int32_T)(6 * i))] = Aug_Indx[(int32_T) - ((int32_T)((int32_T)((int32_T)c_data_0[i] - 1) * 6) + b_m)]; - } - } - - // ':1:13' - // Do the covariance propagation - C_sizes_idx_1 = (int32_T)(int8_T)(int32_T)(6 * handrail_knowledge_dims); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->Selector1[(int32_T)(b_m + (int32_T)(15 * i))] = 0.0F; - } - } - - if ((int32_T)(6 * handrail_knowledge_dims) != 0) { - num_original = (int32_T)((int32_T)((int32_T)(6 * handrail_knowledge_dims) - - 1) * 15); - for (br = 0; br <= num_original; br += 15) { - for (nx = (int32_T)(br + 1); nx <= (int32_T)(br + 15); nx++) { - est_estimator_B->Selector1[(int32_T)(nx - 1)] = 0.0F; - } - } - - br = 0; - for (nx = 0; nx <= num_original; nx += 15) { - ar = -1; - for (i = br; (int32_T)(i + 1) <= (int32_T)(br + 15); i++) { - for (b_m = 0; b_m <= (int32_T)(handrail_knowledge_dims - 1); b_m++) { - for (i_0 = 0; i_0 < 6; i_0++) { - valid_indx_mat_data_0[(int32_T)(i_0 + (int32_T)(6 * b_m))] = - valid_indx_mat_data[(int32_T)((int32_T)(6 * b_m) + i_0)]; - } - } - - if (est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)((int32_T) - valid_indx_mat_data_0[div_nzp_s32_floor(i, 15)] + 14) * 117) + i % - 15)] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = nx; (int32_T)(O_sizes_idx_0 + 1) <= (int32_T)(nx - + 15); O_sizes_idx_0++) { - s12_iter++; - for (b_m = 0; b_m <= (int32_T)(handrail_knowledge_dims - 1); b_m++) - { - for (i_0 = 0; i_0 < 6; i_0++) { - valid_indx_mat_data_0[(int32_T)(i_0 + (int32_T)(6 * b_m))] = - valid_indx_mat_data[(int32_T)((int32_T)(6 * b_m) + i_0)]; - } - } - - est_estimator_B->Selector1[O_sizes_idx_0] += - est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)((int32_T) - valid_indx_mat_data_0[div_nzp_s32_floor(i, 15)] + 14) * 117) + i % - 15)] * rtb_state_trans[s12_iter]; - } - } - - ar += 15; - } - - br += 15; - } - } - - // ':1:16' - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - for (b_m = 0; b_m < 6; b_m++) { - valid_indx_mat_data_0[(int32_T)(b_m + (int32_T)(6 * i))] = - valid_indx_mat_data[(int32_T)((int32_T)(6 * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->P_IC[(int32_T)(b_m + (int32_T)(15 * (int32_T)((int32_T) - valid_indx_mat_data_0[i] - 1)))] = est_estimator_B->Selector1[(int32_T) - ((int32_T)(15 * i) + b_m)]; - } - } - - // End of MATLAB Function: '/MATLAB Function2' - - // Selector: '/Selector1' - // Aug_Indx(Aug_State_Flag) - // Do the multiplication with current valid indices - // P(:, D+1:size(P, 2)) = Phi * P(1:D, D+1:size(P, 2)); - for (i = 0; i < 102; i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->Selector1[(int32_T)(b_m + (int32_T)(15 * i))] = - est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)(15 + i) * 117) - + b_m)]; - } - } - - // End of Selector: '/Selector1' - - // Switch: '/Switch' - if (rtb_BusAssignment_aug_state_enu != 0U) { - memcpy(&est_estimator_B->MatrixConcatenate[225], &est_estimator_B->P_IC[0], - (uint32_T)(1530U * sizeof(real32_T))); - } else { - memcpy(&est_estimator_B->MatrixConcatenate[225], &est_estimator_B-> - Selector1[0], (uint32_T)(1530U * sizeof(real32_T))); - } - - // End of Switch: '/Switch' - - // Switch: '/Switch1' incorporates: - // Math: '/Math Function3' - // Math: '/Math Function4' - - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 102; b_m++) { - if (rtb_BusAssignment_aug_state_enu != 0U) { - est_estimator_B->MatrixConcatenate2[(int32_T)(b_m + (int32_T)(i * 102))] - = est_estimator_B->P_IC[(int32_T)((int32_T)(15 * b_m) + i)]; - } else { - est_estimator_B->MatrixConcatenate2[(int32_T)(b_m + (int32_T)(i * 102))] - = est_estimator_B->Selector1[(int32_T)((int32_T)(15 * b_m) + i)]; - } - } - } - - // End of Switch: '/Switch1' - - // Selector: '/Selector2' - for (i = 0; i < 102; i++) { - memcpy(&est_estimator_B->MatrixConcatenate2[(int32_T)((int32_T)(i * 102) + - 1530)], &est_estimator_B->Assignment[(int32_T)((int32_T)(i * 117) + - 1770)], (uint32_T)(102U * sizeof(real32_T))); - } - - // End of Selector: '/Selector2' - - // Concatenate: '/Matrix Concatenate1' - for (i = 0; i < 117; i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->Assignment[(int32_T)(b_m + (int32_T)(117 * i))] = - est_estimator_B->MatrixConcatenate[(int32_T)((int32_T)(15 * i) + b_m)]; - } - - memcpy(&est_estimator_B->Assignment[(int32_T)((int32_T)(i * 117) + 15)], - &est_estimator_B->MatrixConcatenate2[(int32_T)(i * 102)], (uint32_T) - (102U * sizeof(real32_T))); - } - - // End of Concatenate: '/Matrix Concatenate1' - - // If: '/If' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Inport: '/cmc_msg' - // Inport: '/handrail_msg' - // Inport: '/P_in' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // Logic: '/Logical Operator5' - // Logic: '/Logical Operator7' - // Logic: '/Logical Operator8' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/FixPt Relational Operator' - // RelationalOperator: '/FixPt Relational Operator' - // S-Function (sfix_bitop): '/Bitwise Operator1' - // SignalConversion: '/Signal Conversion' - // Sum: '/Sum of Elements' - // Switch: '/Switch3' - // UnitDelay: '/Delay Input1' - // UnitDelay: '/Delay Input1' - - if ((rtb_Compare && (((est_estimator_U_handrail_msg->cvs_timestamp_sec != - est_estimator_DW->DelayInput1_DSTATE) || - (est_estimator_U_handrail_msg->cvs_timestamp_nsec != - est_estimator_DW->DelayInput1_DSTATE_h)) && - empty_non_axis_sizes) && (rtb_BitwiseOperator != 0U) && - (est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_perching)) || rtb_LogicalOperator) { - c_sz_idx_0 = 0; - } else if (((uint32_T)(rtb_BusAssignment_aug_state_enu & - est_estimator_P->BitwiseOperator1_BitMask_l) == - est_estimator_P->ase_aug_state_bitmask) && rtb_of_update_flag && - ((int32_T)est_estimator_P->tun_ase_enable_of != 0) && ((uint8_T) - UnitDelay_DSTATE_aug_state_enum >= - est_estimator_P->Constant_Value_e)) { - c_sz_idx_0 = 1; - } else { - c_sz_idx_0 = 2; - } - - switch (c_sz_idx_0) { - case 0: - // Outputs for IfAction SubSystem: '/Absolute_Update' incorporates: - // ActionPort: '/Action Port' - - // Outputs for Iterator SubSystem: '/ML Update' incorporates: - // WhileIterator: '/While Iterator' - - // InitializeConditions for UnitDelay: '/Unit Delay1' incorporates: - // UnitDelay: '/Unit Delay1' - - for (i = 0; i < 13689; i++) { - est_estimator_B->Switch1[i] = est_estimator_P->UnitDelay1_InitialCondition; - } - - // End of InitializeConditions for UnitDelay: '/Unit Delay1' - - // InitializeConditions for UnitDelay: '/Unit Delay' incorporates: - // UnitDelay: '/Unit Delay' - - rtb_Merge_o[0] = est_estimator_P->UnitDelay_InitialCondition.quat_ISS2B[0]; - rtb_Merge_o[1] = est_estimator_P->UnitDelay_InitialCondition.quat_ISS2B[1]; - rtb_Merge_o[2] = est_estimator_P->UnitDelay_InitialCondition.quat_ISS2B[2]; - rtb_Merge_o[3] = est_estimator_P->UnitDelay_InitialCondition.quat_ISS2B[3]; - rtb_ImpAsg_InsertedFor_Out1_at_[0] = - est_estimator_P->UnitDelay_InitialCondition.omega_B_ISS_B[0]; - accel[0] = est_estimator_P->UnitDelay_InitialCondition.gyro_bias[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = - est_estimator_P->UnitDelay_InitialCondition.V_B_ISS_ISS[0]; - rtb_Sum_k1 = est_estimator_P->UnitDelay_InitialCondition.A_B_ISS_ISS[0]; - UnitDelay_DSTATE_n_accel_bias[0] = - est_estimator_P->UnitDelay_InitialCondition.accel_bias[0]; - rtb_Sum_l = est_estimator_P->UnitDelay_InitialCondition.P_B_ISS_ISS[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] = - est_estimator_P->UnitDelay_InitialCondition.omega_B_ISS_B[1]; - accel[1] = est_estimator_P->UnitDelay_InitialCondition.gyro_bias[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = - est_estimator_P->UnitDelay_InitialCondition.V_B_ISS_ISS[1]; - rtb_Gain1_f = est_estimator_P->UnitDelay_InitialCondition.A_B_ISS_ISS[1]; - UnitDelay_DSTATE_n_accel_bias[1] = - est_estimator_P->UnitDelay_InitialCondition.accel_bias[1]; - UnitDelay_DSTATE_n_P_B_ISS_ISS_ = - est_estimator_P->UnitDelay_InitialCondition.P_B_ISS_ISS[1]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = - est_estimator_P->UnitDelay_InitialCondition.omega_B_ISS_B[2]; - accel[2] = est_estimator_P->UnitDelay_InitialCondition.gyro_bias[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = - est_estimator_P->UnitDelay_InitialCondition.V_B_ISS_ISS[2]; - UnitDelay_DSTATE_n_A_B_ISS_ISS_ = - est_estimator_P->UnitDelay_InitialCondition.A_B_ISS_ISS[2]; - UnitDelay_DSTATE_n_accel_bias[2] = - est_estimator_P->UnitDelay_InitialCondition.accel_bias[2]; - UnitDelay_DSTATE_n_P_B_ISS_IS_0 = - est_estimator_P->UnitDelay_InitialCondition.P_B_ISS_ISS[2]; - rtb_Saturation_n = est_estimator_P->UnitDelay_InitialCondition.confidence; - rtb_BitwiseOperator = - est_estimator_P->UnitDelay_InitialCondition.aug_state_enum; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = - est_estimator_P->UnitDelay_InitialCondition.ml_quat_ISS2cam[0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = - est_estimator_P->UnitDelay_InitialCondition.ml_quat_ISS2cam[1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = - est_estimator_P->UnitDelay_InitialCondition.ml_quat_ISS2cam[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = - est_estimator_P->UnitDelay_InitialCondition.ml_quat_ISS2cam[3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = - est_estimator_P->UnitDelay_InitialCondition.ml_P_cam_ISS_ISS[0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = - est_estimator_P->UnitDelay_InitialCondition.ml_P_cam_ISS_ISS[1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = - est_estimator_P->UnitDelay_InitialCondition.ml_P_cam_ISS_ISS[2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &est_estimator_P->UnitDelay_InitialCondition.of_quat_ISS2cam[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &est_estimator_P->UnitDelay_InitialCondition.of_P_cam_ISS_ISS[0], - (uint32_T)(48U * sizeof(real32_T))); - memcpy(&UnitDelay_DSTATE_n_cov_diag[0], - &est_estimator_P->UnitDelay_InitialCondition.cov_diag[0], (uint32_T) - (117U * sizeof(ase_cov_datatype))); - UnitDelay_DSTATE_n_kfl_status = - est_estimator_P->UnitDelay_InitialCondition.kfl_status; - rtb_Switch_m = - est_estimator_P->UnitDelay_InitialCondition.update_OF_tracks_cnt; - rtb_numFeatures_f = - est_estimator_P->UnitDelay_InitialCondition.update_ML_features_cnt; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], - &est_estimator_P->UnitDelay_InitialCondition.of_mahal_distance[0], - (uint32_T)(50U * sizeof(real_T))); - memcpy(&UnitDelay_DSTATE_n_ml_mahal_dis[0], - &est_estimator_P->UnitDelay_InitialCondition.ml_mahal_distance[0], - (uint32_T)(50U * sizeof(real_T))); - UnitDelay_DSTATE_n_hr_P_hr_ISS_ = - est_estimator_P->UnitDelay_InitialCondition.hr_P_hr_ISS_ISS[0]; - UnitDelay_DSTATE_n_hr_P_hr_IS_0 = - est_estimator_P->UnitDelay_InitialCondition.hr_P_hr_ISS_ISS[1]; - UnitDelay_DSTATE_n_hr_P_hr_IS_1 = - est_estimator_P->UnitDelay_InitialCondition.hr_P_hr_ISS_ISS[2]; - UnitDelay_DSTATE_n_hr_quat_ISS2 = - est_estimator_P->UnitDelay_InitialCondition.hr_quat_ISS2hr[0]; - UnitDelay_DSTATE_n_hr_quat_IS_0 = - est_estimator_P->UnitDelay_InitialCondition.hr_quat_ISS2hr[1]; - UnitDelay_DSTATE_n_hr_quat_IS_1 = - est_estimator_P->UnitDelay_InitialCondition.hr_quat_ISS2hr[2]; - UnitDelay_DSTATE_n_hr_quat_IS_2 = - est_estimator_P->UnitDelay_InitialCondition.hr_quat_ISS2hr[3]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = - est_estimator_P->UnitDelay_InitialCondition.P_EST_ISS_ISS[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = - est_estimator_P->UnitDelay_InitialCondition.P_EST_ISS_ISS[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = - est_estimator_P->UnitDelay_InitialCondition.P_EST_ISS_ISS[2]; - if (1 > est_estimator_P->Switch3_Threshold) { - memcpy(&est_estimator_B->Assignment[0], &est_estimator_B->Switch1[0], - (uint32_T)(13689U * sizeof(real32_T))); - } - - // Switch: '/Switch2' incorporates: - // BusAssignment: '/Bus Assignment' - // BusAssignment: '/Bus Assignment' - // BusCreator: '/Bus Creator2' - // Switch: '/Switch3' - // UnitDelay: '/Unit Delay10' - // UnitDelay: '/Unit Delay11' - // UnitDelay: '/Unit Delay12' - // UnitDelay: '/Unit Delay13' - // UnitDelay: '/Unit Delay14' - // UnitDelay: '/Unit Delay15' - // UnitDelay: '/Unit Delay3' - // UnitDelay: '/Unit Delay6' - // UnitDelay: '/Unit Delay7' - // UnitDelay: '/Unit Delay8' - - if (!(1 > est_estimator_P->Switch2_Threshold)) { - rtb_Merge_o[0] = rtb_Product1[0]; - rtb_Merge_o[1] = rtb_Product1[1]; - rtb_Merge_o[2] = rtb_Product1[2]; - rtb_Merge_o[3] = rtb_Product1[3]; - rtb_ImpAsg_InsertedFor_Out1_at_[0] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - accel[0] = est_estimator_DW->UnitDelay3_DSTATE[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = rtb_Sum2[0]; - rtb_Sum_k1 = rtb_Sum1_k3[0]; - UnitDelay_DSTATE_n_accel_bias[0] = est_estimator_DW->UnitDelay6_DSTATE[0]; - rtb_Sum_l = est_estimator_DW->UnitDelay7_DSTATE[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - accel[1] = est_estimator_DW->UnitDelay3_DSTATE[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = rtb_Sum2[1]; - rtb_Gain1_f = rtb_Sum1_k3[1]; - UnitDelay_DSTATE_n_accel_bias[1] = est_estimator_DW->UnitDelay6_DSTATE[1]; - UnitDelay_DSTATE_n_P_B_ISS_ISS_ = est_estimator_DW->UnitDelay7_DSTATE[1]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = hr_quat_ISS2hr_idx_0; - accel[2] = est_estimator_DW->UnitDelay3_DSTATE[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = rtb_Sum2[2]; - UnitDelay_DSTATE_n_A_B_ISS_ISS_ = rtb_Sum1_k3[2]; - UnitDelay_DSTATE_n_accel_bias[2] = est_estimator_DW->UnitDelay6_DSTATE[2]; - UnitDelay_DSTATE_n_P_B_ISS_IS_0 = est_estimator_DW->UnitDelay7_DSTATE[2]; - rtb_Saturation_n = est_estimator_DW->UnitDelay8_DSTATE; - rtb_BitwiseOperator = rtb_BusAssignment_aug_state_enu; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = est_estimator_DW->UnitDelay10_DSTATE - [0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = est_estimator_DW->UnitDelay10_DSTATE - [1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = est_estimator_DW->UnitDelay10_DSTATE - [2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = est_estimator_DW->UnitDelay10_DSTATE - [3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = est_estimator_DW->UnitDelay11_DSTATE - [0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = est_estimator_DW->UnitDelay11_DSTATE - [1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = est_estimator_DW->UnitDelay11_DSTATE - [2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &est_estimator_DW->UnitDelay12_DSTATE[0], (uint32_T)(sizeof - (real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &est_estimator_DW->UnitDelay13_DSTATE[0], (uint32_T)(48U * sizeof - (real32_T))); - memcpy(&UnitDelay_DSTATE_n_cov_diag[0], - &est_estimator_DW->UnitDelay14_DSTATE[0], (uint32_T)(117U * sizeof - (ase_cov_datatype))); - UnitDelay_DSTATE_n_kfl_status = est_estimator_DW->UnitDelay15_DSTATE; - rtb_Switch_m = numFeatures; - rtb_numFeatures_f = rtb_BusCreator2_update_ML_featu; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], &rtb_UnitDelay18[0], (uint32_T) - (50U * sizeof(real_T))); - memcpy(&UnitDelay_DSTATE_n_ml_mahal_dis[0], &rtb_UnitDelay19[0], (uint32_T) - (50U * sizeof(real_T))); - UnitDelay_DSTATE_n_hr_P_hr_ISS_ = rtb_Add_e[0]; - UnitDelay_DSTATE_n_hr_P_hr_IS_0 = rtb_Add_e[1]; - UnitDelay_DSTATE_n_hr_P_hr_IS_1 = rtb_Add_e[2]; - UnitDelay_DSTATE_n_hr_quat_ISS2 = rtb_UnitDelay25[0]; - UnitDelay_DSTATE_n_hr_quat_IS_0 = rtb_UnitDelay25[1]; - UnitDelay_DSTATE_n_hr_quat_IS_1 = rtb_UnitDelay25[2]; - UnitDelay_DSTATE_n_hr_quat_IS_2 = rtb_UnitDelay25[3]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = rtb_P_B_ISS_ISS[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = rtb_P_B_ISS_ISS[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = rtb_P_B_ISS_ISS[2]; - } - - // End of Switch: '/Switch2' - - // If: '/IF' - if ((int32_T)est_estimator_U_cmc_msg_o->localization_mode_cmd == 2) { - // Outputs for IfAction SubSystem: '/HR_Compute_Residual_and_H1' incorporates: - // ActionPort: '/Action Port' - - // MATLAB Function: '/Compute Global positions of Handrail Features' - // MATLAB Function 'camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1/Compute Global positions of Handrail Features': ':1' - // ':1:4' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - if (!est_estimator_DW->hr_P_hr_ISS_ISS_pers_not_empty) { - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - S[0] = 0.0F; - S[3] = -UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - S[6] = UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - S[1] = UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - S[4] = 0.0F; - S[7] = -UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - S[2] = -UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - S[5] = UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - S[8] = 0.0F; - hr_quat_ISS2hr_idx_0 = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - rtb_Switch1 = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - for (i = 0; i < 3; i++) { - x[(int32_T)(3 * i)] = hr_quat_ISS2hr_idx_0 * (real32_T)b_2[i] + S[i]; - rtb_Product1_1[(int32_T)(i << 2)] = (real32_T)b_2[(int32_T)(3 * i)] * - rtb_Switch1 - S[(int32_T)(3 * i)]; - x[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 3)] * - hr_quat_ISS2hr_idx_0 + S[(int32_T)(i + 3)]; - rtb_Product1_1[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 1)] * rtb_Switch1 - S[(int32_T) - ((int32_T)(3 * i) + 1)]; - x[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 6)] * - hr_quat_ISS2hr_idx_0 + S[(int32_T)(i + 6)]; - rtb_Product1_1[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 2)] * rtb_Switch1 - S[(int32_T) - ((int32_T)(3 * i) + 2)]; - x[(int32_T)(9 + i)] = -UnitDelay_DSTATE_n_ml_quat_ISS2[i]; - rtb_Product1_1[(int32_T)(3 + (int32_T)(i << 2))] = - -UnitDelay_DSTATE_n_ml_quat_ISS2[i]; - } - - for (i = 0; i < 3; i++) { - hr_quat_ISS2hr_idx_1 = 0.0F; - for (b_m = 0; b_m < 3; b_m++) { - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)(i << 2)] * x[b_m]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 1)] * x[(int32_T)(b_m - + 3)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 2)] * x[(int32_T)(b_m - + 6)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 3)] * x[(int32_T)(b_m - + 9)]; - hr_quat_ISS2hr_idx_1 += rtb_Assignment_b[(int32_T)((int32_T)(3 * b_m) - + i)] * est_estimator_U_handrail_msg->cvs_handrail_local_pos[b_m]; - } - - est_estimator_DW->hr_P_hr_ISS_ISS_pers[i] = - UnitDelay_DSTATE_n_ml_P_cam_ISS[i] + hr_quat_ISS2hr_idx_1; - } - - est_estimator_DW->hr_P_hr_ISS_ISS_pers_not_empty = true; - } - - if (!est_estimator_DW->hr_quat_ISS2hr_pers_not_empty) { - iecjopppiecjmgln_quatmult(UnitDelay_DSTATE_n_ml_quat_ISS2, - est_estimator_U_handrail_msg->cvs_handrail_local_quat, - est_estimator_DW->hr_quat_ISS2hr_pers); - est_estimator_DW->hr_quat_ISS2hr_pers_not_empty = true; - } - - if ((int32_T) - est_estimator_U_handrail_msg->cvs_handrail_update_global_pose_flag == - 1) { - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - S[0] = 0.0F; - S[3] = -UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - S[6] = UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - S[1] = UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - S[4] = 0.0F; - S[7] = -UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - S[2] = -UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - S[5] = UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - S[8] = 0.0F; - hr_quat_ISS2hr_idx_0 = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - rtb_Switch1 = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - for (i = 0; i < 3; i++) { - x[(int32_T)(3 * i)] = hr_quat_ISS2hr_idx_0 * (real32_T)b_2[i] + S[i]; - rtb_Product1_1[(int32_T)(i << 2)] = (real32_T)b_2[(int32_T)(3 * i)] * - rtb_Switch1 - S[(int32_T)(3 * i)]; - x[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 3)] * - hr_quat_ISS2hr_idx_0 + S[(int32_T)(i + 3)]; - rtb_Product1_1[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 1)] * rtb_Switch1 - S[(int32_T) - ((int32_T)(3 * i) + 1)]; - x[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 6)] * - hr_quat_ISS2hr_idx_0 + S[(int32_T)(i + 6)]; - rtb_Product1_1[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 2)] * rtb_Switch1 - S[(int32_T) - ((int32_T)(3 * i) + 2)]; - x[(int32_T)(9 + i)] = -UnitDelay_DSTATE_n_ml_quat_ISS2[i]; - rtb_Product1_1[(int32_T)(3 + (int32_T)(i << 2))] = - -UnitDelay_DSTATE_n_ml_quat_ISS2[i]; - } - - for (i = 0; i < 3; i++) { - hr_quat_ISS2hr_idx_1 = 0.0F; - for (b_m = 0; b_m < 3; b_m++) { - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)(i << 2)] * x[b_m]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 1)] * x[(int32_T)(b_m - + 3)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 2)] * x[(int32_T)(b_m - + 6)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 3)] * x[(int32_T)(b_m - + 9)]; - hr_quat_ISS2hr_idx_1 += rtb_Assignment_b[(int32_T)((int32_T)(3 * b_m) - + i)] * est_estimator_U_handrail_msg->cvs_handrail_local_pos[b_m]; - } - - est_estimator_DW->hr_P_hr_ISS_ISS_pers[i] = - UnitDelay_DSTATE_n_ml_P_cam_ISS[i] + hr_quat_ISS2hr_idx_1; - } - - iecjopppiecjmgln_quatmult(UnitDelay_DSTATE_n_ml_quat_ISS2, - est_estimator_U_handrail_msg->cvs_handrail_local_quat, - est_estimator_DW->hr_quat_ISS2hr_pers); - } - - memset(&rtb_hr_global_landmarks[0], 0, (uint32_T)(150U * sizeof(real32_T))); - num_original = 0; - while ((num_original < 50) && (!((int32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[num_original] == 0))) - { - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - S[0] = 0.0F; - S[3] = -est_estimator_U_handrail_msg->cvs_handrail_local_quat[2]; - S[6] = est_estimator_U_handrail_msg->cvs_handrail_local_quat[1]; - S[1] = est_estimator_U_handrail_msg->cvs_handrail_local_quat[2]; - S[4] = 0.0F; - S[7] = -est_estimator_U_handrail_msg->cvs_handrail_local_quat[0]; - S[2] = -est_estimator_U_handrail_msg->cvs_handrail_local_quat[1]; - S[5] = est_estimator_U_handrail_msg->cvs_handrail_local_quat[0]; - S[8] = 0.0F; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - C[0] = 0.0F; - C[3] = -est_estimator_DW->hr_quat_ISS2hr_pers[2]; - C[6] = est_estimator_DW->hr_quat_ISS2hr_pers[1]; - C[1] = est_estimator_DW->hr_quat_ISS2hr_pers[2]; - C[4] = 0.0F; - C[7] = -est_estimator_DW->hr_quat_ISS2hr_pers[0]; - C[2] = -est_estimator_DW->hr_quat_ISS2hr_pers[1]; - C[5] = est_estimator_DW->hr_quat_ISS2hr_pers[0]; - C[8] = 0.0F; - hr_quat_ISS2hr_idx_1 = est_estimator_DW->hr_quat_ISS2hr_pers[3]; - hr_quat_ISS2hr_idx_0 = est_estimator_DW->hr_quat_ISS2hr_pers[3]; - rtb_Switch1 = est_estimator_U_handrail_msg->cvs_handrail_local_quat[3]; - hr_quat_ISS2hr_idx_2 = - est_estimator_U_handrail_msg->cvs_handrail_local_quat[3]; - for (i = 0; i < 3; i++) { - x[(int32_T)(3 * i)] = hr_quat_ISS2hr_idx_1 * (real32_T)b_2[i] + C[i]; - tmp_8[(int32_T)(i << 2)] = (real32_T)b_2[(int32_T)(3 * i)] * - hr_quat_ISS2hr_idx_0 - C[(int32_T)(3 * i)]; - rtb_Product1_1[(int32_T)(3 * i)] = rtb_Switch1 * (real32_T)b_2[i] + - S[i]; - rtb_Product1_2[(int32_T)(i << 2)] = (real32_T)b_2[(int32_T)(3 * i)] * - hr_quat_ISS2hr_idx_2 - S[(int32_T)(3 * i)]; - x[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 3)] * - hr_quat_ISS2hr_idx_1 + C[(int32_T)(i + 3)]; - tmp_8[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b_2[(int32_T) - ((int32_T)(3 * i) + 1)] * hr_quat_ISS2hr_idx_0 - C[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_Product1_1[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b_2 - [(int32_T)(i + 3)] * rtb_Switch1 + S[(int32_T)(i + 3)]; - rtb_Product1_2[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 1)] * hr_quat_ISS2hr_idx_2 - S - [(int32_T)((int32_T)(3 * i) + 1)]; - x[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 6)] * - hr_quat_ISS2hr_idx_1 + C[(int32_T)(i + 6)]; - tmp_8[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b_2[(int32_T) - ((int32_T)(3 * i) + 2)] * hr_quat_ISS2hr_idx_0 - C[(int32_T) - ((int32_T)(3 * i) + 2)]; - rtb_Product1_1[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b_2 - [(int32_T)(i + 6)] * rtb_Switch1 + S[(int32_T)(i + 6)]; - rtb_Product1_2[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 2)] * hr_quat_ISS2hr_idx_2 - S - [(int32_T)((int32_T)(3 * i) + 2)]; - x[(int32_T)(9 + i)] = -est_estimator_DW->hr_quat_ISS2hr_pers[i]; - tmp_8[(int32_T)(3 + (int32_T)(i << 2))] = - -est_estimator_DW->hr_quat_ISS2hr_pers[i]; - rtb_Product1_1[(int32_T)(9 + i)] = - -est_estimator_U_handrail_msg->cvs_handrail_local_quat[i]; - rtb_Product1_2[(int32_T)(3 + (int32_T)(i << 2))] = - -est_estimator_U_handrail_msg->cvs_handrail_local_quat[i]; - } - - for (i = 0; i < 3; i++) { - rtb_P_B_ISS_ISS[i] = est_estimator_U_handrail_msg->cvs_observations - [(int32_T)((int32_T)(50 * i) + num_original)] - - est_estimator_U_handrail_msg->cvs_handrail_local_pos[i]; - for (b_m = 0; b_m < 3; b_m++) { - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += rtb_Product1_2 - [(int32_T)(b_m << 2)] * rtb_Product1_1[i]; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_8 - [(int32_T)(i << 2)] * x[b_m]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += rtb_Product1_2 - [(int32_T)((int32_T)(b_m << 2) + 1)] * rtb_Product1_1[(int32_T)(i - + 3)]; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_8 - [(int32_T)((int32_T)(i << 2) + 1)] * x[(int32_T)(b_m + 3)]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += rtb_Product1_2 - [(int32_T)((int32_T)(b_m << 2) + 2)] * rtb_Product1_1[(int32_T)(i - + 6)]; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_8 - [(int32_T)((int32_T)(i << 2) + 2)] * x[(int32_T)(b_m + 6)]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += rtb_Product1_2 - [(int32_T)((int32_T)(b_m << 2) + 3)] * rtb_Product1_1[(int32_T)(i - + 9)]; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_8 - [(int32_T)((int32_T)(i << 2) + 3)] * x[(int32_T)(b_m + 9)]; - } - } - - for (i = 0; i < 3; i++) { - rtb_Sum1_k3[i] = rtb_Product1_0[(int32_T)(i + 6)] * rtb_P_B_ISS_ISS[2] - + (rtb_Product1_0[(int32_T)(i + 3)] * rtb_P_B_ISS_ISS[1] + - rtb_Product1_0[i] * rtb_P_B_ISS_ISS[0]); - } - - for (i = 0; i < 3; i++) { - rtb_hr_global_landmarks[(int32_T)(num_original + (int32_T)(50 * i))] = - ((rtb_Assignment_o[(int32_T)(i + 3)] * rtb_Sum1_k3[1] + - rtb_Assignment_o[i] * rtb_Sum1_k3[0]) + rtb_Assignment_o[(int32_T) - (i + 6)] * rtb_Sum1_k3[2]) + est_estimator_DW-> - hr_P_hr_ISS_ISS_pers[i]; - } - - num_original++; - } - - // MATLAB Function: '/Compute Residual and H' - // ':1:4' - // MATLAB Function 'camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1/Compute Residual and H': ':1' - // ':1:3' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - if ((int32_T)est_estimator_U_handrail_msg->cvs_3d_knowledge_flag == 1) { - handrail_knowledge_dims = 3; - } else { - handrail_knowledge_dims = 2; - } - - // Get tf matrix that converts from global position (world frame) - // to local position (camera frame) - for (i = 0; i < 12; i++) { - camera_ml_tf_global[i] = (real_T)t[i]; - } - - fkfcbaiengdjgdje_quaternion_to_rotation(UnitDelay_DSTATE_n_ml_quat_ISS2, - rtb_Product1_0); - for (i = 0; i < 3; i++) { - C[(int32_T)(3 * i)] = (real32_T)-(real_T)rtb_Product1_0[(int32_T)(3 * i)]; - camera_ml_tf_global[(int32_T)(3 * i)] = (real_T)rtb_Product1_0[(int32_T) - (3 * i)]; - C[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)-(real_T)rtb_Product1_0 - [(int32_T)((int32_T)(3 * i) + 1)]; - camera_ml_tf_global[(int32_T)(1 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)]; - C[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)-(real_T)rtb_Product1_0 - [(int32_T)((int32_T)(3 * i) + 2)]; - camera_ml_tf_global[(int32_T)(2 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 3; i++) { - camera_ml_tf_global[(int32_T)(9 + i)] = (real_T)((C[(int32_T)(i + 3)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] + C[i] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[0]) + C[(int32_T)(i + 6)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[2]); - } - - // Remove the invalid features from the observations and landmarks - // Observations from the camera frame - // Landmarks from the world frame - // Convert global positions of the landmarks to the local positions - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[nx] != 0) { - br++; - } - } - - C_sizes_idx_1 = br; - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[nx] != 0) { - e_data[br] = (int8_T)(int32_T)(nx + 1); - br++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = rtb_hr_global_landmarks[(int32_T) - ((int32_T)e_data[i] - 1)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - rtb_hr_global_landmarks[(int32_T)((int32_T)e_data[i] + 49)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - rtb_hr_global_landmarks[(int32_T)((int32_T)e_data[i] + 99)]; - } - - if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else { - num_original = 0; - if (C_sizes_idx_1 > 0) { - num_original = C_sizes_idx_1; - } - } - - empty_non_axis_sizes = (num_original == 0); - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - br = 3; - } else { - br = 0; - } - - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - nx = 1; - } else { - nx = 0; - } - - ar = (int32_T)(br + nx); - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - rtb_global_points[(int32_T)(b_m + (int32_T)(ar * i))] = - varargin_1_data[(int32_T)((int32_T)(br * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - rtb_global_points[(int32_T)((int32_T)(b_m + br) + (int32_T)(ar * i))] = - 1.0F; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)(ar * b_m)] * (real32_T) - camera_ml_tf_global[i]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 1)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 3)]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 2)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 6)]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 3)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 9)]; - } - } - - // Landmarks in the camera frame - // Create rotation matrix to convert between bases - // R rotation matrix that rotates from the camera frame into the handrail frame - // with the axis_body vector as the z axis unit vector - fkfcbaiengdjgdje_quaternion_to_rotation - (est_estimator_U_handrail_msg->cvs_handrail_local_quat, S); - - // R = eye(3); - num_original = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[nx] != 0) { - num_original++; - } - } - - C_sizes_idx_1 = num_original; - num_original = 0; - for (ar = 0; ar < 50; ar++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[ar] != 0) { - e_data[num_original] = (int8_T)(int32_T)(ar + 1); - num_original++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = - est_estimator_U_handrail_msg->cvs_observations[(int32_T)((int32_T) - e_data[i] - 1)] - camera_landmarks_data[(int32_T)(3 * i)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - est_estimator_U_handrail_msg->cvs_observations[(int32_T)((int32_T) - e_data[i] + 49)] - camera_landmarks_data[(int32_T)((int32_T)(3 * i) + - 1)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - est_estimator_U_handrail_msg->cvs_observations[(int32_T)((int32_T) - e_data[i] + 99)] - camera_landmarks_data[(int32_T)((int32_T)(3 * i) + - 2)]; - } - - if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else { - num_original = 0; - if (C_sizes_idx_1 > 0) { - num_original = C_sizes_idx_1; - } - } - - empty_non_axis_sizes = (num_original == 0); - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - br = 3; - } else { - br = 0; - } - - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - nx = 1; - } else { - nx = 0; - } - - ar = (int32_T)(br + nx); - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - d_result_data[(int32_T)(b_m + (int32_T)(ar * i))] = varargin_1_data - [(int32_T)((int32_T)(br * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - d_result_data[(int32_T)((int32_T)(b_m + br) + (int32_T)(ar * i))] = - 1.0F; - } - } - - for (i = 0; i < 3; i++) { - global_tf_camera1[(int32_T)(i << 2)] = S[(int32_T)(3 * i)]; - global_tf_camera1[(int32_T)(1 + (int32_T)(i << 2))] = S[(int32_T) - ((int32_T)(3 * i) + 1)]; - global_tf_camera1[(int32_T)(2 + (int32_T)(i << 2))] = S[(int32_T) - ((int32_T)(3 * i) + 2)]; - global_tf_camera1[(int32_T)(12 + i)] = 0.0F; - } - - global_tf_camera1[3] = 0.0F; - global_tf_camera1[7] = 0.0F; - global_tf_camera1[11] = 0.0F; - global_tf_camera1[15] = 1.0F; - if (ar == 1) { - z_est_sizes_idx_1 = num_original; - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] = 0.0F; - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] += - global_tf_camera1[i] * d_result_data[b_m]; - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] += - global_tf_camera1[(int32_T)(i + 4)] * d_result_data[(int32_T)(1 + - b_m)]; - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] += - global_tf_camera1[(int32_T)(i + 8)] * d_result_data[(int32_T)(2 + - b_m)]; - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] += - global_tf_camera1[(int32_T)(i + 12)] * d_result_data[(int32_T)(3 + - b_m)]; - } - } - } else { - z_est_sizes_idx_1 = (int32_T)(int8_T)num_original; - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - newerr_data[(int32_T)(i << 2)] = 0.0F; - newerr_data[(int32_T)(1 + (int32_T)(i << 2))] = 0.0F; - newerr_data[(int32_T)(2 + (int32_T)(i << 2))] = 0.0F; - newerr_data[(int32_T)(3 + (int32_T)(i << 2))] = 0.0F; - } - - if (num_original != 0) { - num_original = (int32_T)((int32_T)(num_original - 1) << 2); - for (br = 0; br <= num_original; br += 4) { - for (nx = (int32_T)(br + 1); nx <= (int32_T)(br + 4); nx++) { - newerr_data[(int32_T)(nx - 1)] = 0.0F; - } - } - - br = 0; - for (nx = 0; nx <= num_original; nx += 4) { - ar = 0; - for (i = br; (int32_T)(i + 1) <= (int32_T)(br + 4); i++) { - if (d_result_data[i] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = nx; (int32_T)(O_sizes_idx_0 + 1) <= - (int32_T)(nx + 4); O_sizes_idx_0++) { - s12_iter++; - newerr_data[O_sizes_idx_0] += global_tf_camera1[(int32_T) - (s12_iter - 1)] * d_result_data[i]; - } - } - - ar += 4; - } - - br += 4; - } - } - } - - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - nx++; - } - } - - C_sizes_idx_1 = nx; - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(handrail_knowledge_dims - 1); b_m++) { - est_estimator_B->x_data[(int32_T)(b_m + (int32_T) - (handrail_knowledge_dims * i))] = newerr_data[(int32_T)((int32_T)(i << - 2) + b_m)]; - } - } - - nx = (int32_T)(handrail_knowledge_dims * z_est_sizes_idx_1); - numFeatures = (uint8_T)(int32_T)(C_sizes_idx_1 * handrail_knowledge_dims); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - r_data[br] = est_estimator_B->x_data[br]; - } - - for (i = 0; i < 12; i++) { - next_ml_tf_global[i] = (real_T)t[i]; - } - - // move to camera frame and with half a frame rotation - // TODO: we should use the velocity and omega from the time of the - // registration pulse - // camera_angle = eulers_to_quat(cam_omega(1), cam_omega(2), cam_omega(3)); - rtb_ml_vel_aug_0[0] = rtb_ml_vel_aug[0]; - rtb_ml_vel_aug_0[1] = rtb_ml_vel_aug[1]; - rtb_ml_vel_aug_0[2] = rtb_ml_vel_aug[2]; - gdjmmglnmgdjlfkf_quat_rotation_vec(rtb_ml_vel_aug_0, - est_estimator_P->tun_abp_q_body2perchcam, tmp); - kngldbimhdbaimgd_quat_propagate_step(UnitDelay_DSTATE_n_ml_quat_ISS2, tmp, - est_estimator_P->tun_ase_ml_forward_projection_time, - rtb_VectorConcatenate_o); - ecjedbaiaiekohln_quaternion_to_rotation(rtb_VectorConcatenate_o, (real_T *) - &next_ml_tf_global[0]); - for (i = 0; i < 3; i++) { - C[(int32_T)(3 * i)] = (real32_T)-camera_ml_tf_global[(int32_T)(3 * i)]; - C[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)-camera_ml_tf_global - [(int32_T)((int32_T)(3 * i) + 1)]; - C[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)-camera_ml_tf_global - [(int32_T)((int32_T)(3 * i) + 2)]; - rtb_P_B_ISS_ISS[i] = est_estimator_P->tun_ase_ml_forward_projection_time - * (real32_T)rtb_ml_omega_aug[i] + UnitDelay_DSTATE_n_ml_P_cam_ISS[i]; - } - - for (i = 0; i < 3; i++) { - next_ml_tf_global[(int32_T)(9 + i)] = (real_T)((C[(int32_T)(i + 3)] * - rtb_P_B_ISS_ISS[1] + C[i] * rtb_P_B_ISS_ISS[0]) + C[(int32_T)(i + 6)] * - rtb_P_B_ISS_ISS[2]); - } - - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - nx++; - } - } - - C_sizes_idx_1 = nx; - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - e_data[nx] = (int8_T)(int32_T)(br + 1); - nx++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = rtb_hr_global_landmarks[(int32_T) - ((int32_T)e_data[i] - 1)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - rtb_hr_global_landmarks[(int32_T)((int32_T)e_data[i] + 49)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - rtb_hr_global_landmarks[(int32_T)((int32_T)e_data[i] + 99)]; - } - - if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else { - num_original = 0; - if (C_sizes_idx_1 > 0) { - num_original = C_sizes_idx_1; - } - } - - rtb_Compare = (num_original == 0); - if (rtb_Compare || (!(C_sizes_idx_1 == 0))) { - br = 3; - } else { - br = 0; - } - - if (rtb_Compare || (!(C_sizes_idx_1 == 0))) { - nx = 1; - } else { - nx = 0; - } - - // Landmarks in the camera frame - // z_next = next_landmarks; - // Find the difference between the current landmark locations and the - // distorted landmark locations due to time error, and rotate into the - // handrail frame - ar = (int32_T)(br + nx); - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - rtb_global_points[(int32_T)(b_m + (int32_T)(ar * i))] = - varargin_1_data[(int32_T)((int32_T)(br * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - rtb_global_points[(int32_T)((int32_T)(b_m + br) + (int32_T)(ar * i))] = - 1.0F; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] = - camera_landmarks_data[(int32_T)((int32_T)(3 * b_m) + i)] - - (((rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 1)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 3)] + rtb_global_points[(int32_T) - (ar * b_m)] * (real32_T)next_ml_tf_global[i]) + - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 2)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 6)]) + rtb_global_points[(int32_T) - ((int32_T)(ar * b_m) + 3)] * (real32_T)next_ml_tf_global[(int32_T) - (i + 9)]); - } - } - - c_sz_idx_0 = (int8_T)num_original; - z_est_sizes_idx_1 = (int32_T)(int8_T)num_original; - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - y_data[(int32_T)(3 * i)] = 0.0F; - y_data[(int32_T)(1 + (int32_T)(3 * i))] = 0.0F; - y_data[(int32_T)(2 + (int32_T)(3 * i))] = 0.0F; - } - - if (num_original != 0) { - num_original = (int32_T)((int32_T)(num_original - 1) * 3); - for (br = 0; br <= num_original; br += 3) { - for (nx = (int32_T)(br + 1); nx <= (int32_T)(br + 3); nx++) { - y_data[(int32_T)(nx - 1)] = 0.0F; - } - } - - br = 0; - for (nx = 0; nx <= num_original; nx += 3) { - ar = 0; - for (i = br; (int32_T)(i + 1) <= (int32_T)(br + 3); i++) { - if (next_landmarks_data[i] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = nx; (int32_T)(O_sizes_idx_0 + 1) <= (int32_T) - (nx + 3); O_sizes_idx_0++) { - s12_iter++; - y_data[O_sizes_idx_0] += S[(int32_T)(s12_iter - 1)] * - next_landmarks_data[i]; - } - } - - ar += 3; - } - - br += 3; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(z_est_sizes_idx_1 - 1); b_m++) { - landmark_error_rail_frame_data[(int32_T)(b_m + (int32_T)((int32_T) - c_sz_idx_0 * i))] = y_data[(int32_T)((int32_T)(3 * b_m) + i)]; - } - } - - nx = 0; - for (ar = 0; ar < 50; ar++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[ar] != 0) { - nx++; - } - } - - C_sizes_idx_1 = nx; - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(z_est_sizes_idx_1 - 1); b_m++) { - est_estimator_B->x_data[(int32_T)(b_m + (int32_T)((int32_T)c_sz_idx_0 * - i))] = landmark_error_rail_frame_data[(int32_T)((int32_T)((int32_T) - c_sz_idx_0 * i) + b_m)]; - } - } - - nx = (int32_T)((int32_T)c_sz_idx_0 * handrail_knowledge_dims); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - varargin_1_data[br] = est_estimator_B->x_data[br]; - } - - nohlcjekmohddjmg_abs(varargin_1_data, (int32_T)(uint8_T)(int32_T) - (C_sizes_idx_1 * handrail_knowledge_dims), - next_landmarks_data, &i); - - // compute Jacobian - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[nx] != 0) { - br++; - } - } - - z_est_sizes_idx_1 = (int32_T)(br * handrail_knowledge_dims); - C_sizes_idx_1 = (int32_T)(br * handrail_knowledge_dims); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - r_vec_data[i] = 0.0; - } - - num_original = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - num_original++; - } - } - - H_sizes[0] = (int32_T)(handrail_knowledge_dims * num_original); - H_sizes[1] = 6; - C_sizes_idx_1 = (int32_T)((int32_T)(handrail_knowledge_dims * num_original) - * 6); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - H_data[i] = 0.0F; - } - - num_original = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - num_original++; - } - } - - br = num_original; - for (num_original = 0; num_original <= (int32_T)(br - 1); num_original++) - { - // temp = 1.0 ./ camera_landmarks(3, i) * [eye(2, 2) -z_est(:, i)]; - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // rotate to new coordinate system - nx = (int32_T)((int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims) - handrail_knowledge_dims); - if ((int32_T)(nx + 1) > (int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims)) { - nx = 0; - } - - tmp_7[0] = 0.0F; - tmp_7[3] = -camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) - + 2)]; - tmp_7[6] = camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + - 1)]; - tmp_7[1] = camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + - 2)]; - tmp_7[4] = 0.0F; - tmp_7[7] = -camera_landmarks_data[(int32_T)(3 * num_original)]; - tmp_7[2] = -camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) - + 1)]; - tmp_7[5] = camera_landmarks_data[(int32_T)(3 * num_original)]; - tmp_7[8] = 0.0F; - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m < 3; b_m++) { - C[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - C[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_7[(int32_T)(3 * b_m)] * - (real32_T)v[i]; - C[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_7[(int32_T)((int32_T)(3 * - b_m) + 1)] * (real32_T)v[(int32_T)(i + 3)]; - C[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_7[(int32_T)((int32_T)(3 * - b_m) + 2)] * (real32_T)v[(int32_T)(i + 6)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] = (real32_T) - ((camera_ml_tf_global[(int32_T)((int32_T)(3 * b_m) + 1)] * (real_T) - c_a[(int32_T)(i + 3)] + camera_ml_tf_global[(int32_T)(3 * b_m)] * - (real_T)c_a[i]) + camera_ml_tf_global[(int32_T)((int32_T)(3 * - b_m) + 2)] * (real_T)c_a[(int32_T)(i + 6)]); - } - } - - for (i = 0; i < 3; i++) { - v_0[(int32_T)(3 * i)] = C[(int32_T)(3 * i)]; - v_0[(int32_T)(3 * (int32_T)(i + 3))] = rtb_Assignment_b[(int32_T)(3 * - i)]; - v_0[(int32_T)(1 + (int32_T)(3 * i))] = C[(int32_T)((int32_T)(3 * i) + - 1)]; - v_0[(int32_T)(1 + (int32_T)(3 * (int32_T)(i + 3)))] = - rtb_Assignment_b[(int32_T)((int32_T)(3 * i) + 1)]; - v_0[(int32_T)(2 + (int32_T)(3 * i))] = C[(int32_T)((int32_T)(3 * i) + - 2)]; - v_0[(int32_T)(2 + (int32_T)(3 * (int32_T)(i + 3)))] = - rtb_Assignment_b[(int32_T)((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m < 6; b_m++) { - S_0[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - S_0[(int32_T)(i + (int32_T)(3 * b_m))] += v_0[(int32_T)(3 * b_m)] * - S[i]; - S_0[(int32_T)(i + (int32_T)(3 * b_m))] += v_0[(int32_T)((int32_T)(3 * - b_m) + 1)] * S[(int32_T)(i + 3)]; - S_0[(int32_T)(i + (int32_T)(3 * b_m))] += v_0[(int32_T)((int32_T)(3 * - b_m) + 2)] * S[(int32_T)(i + 6)]; - } - } - - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m <= (int32_T)(handrail_knowledge_dims - 1); b_m++) { - H_data[(int32_T)((int32_T)(nx + b_m) + (int32_T)(H_sizes[0] * i))] = - S_0[(int32_T)((int32_T)(3 * i) + b_m)]; - } - } - - // drop last row, since we don't have knowledge along handrail axis - // make the confidence depend on mapped landmarked error which is function - // of distance - // [??] Should the camera_landmarks distance be the RSS, not just Z? - // r_vec is measurment noise - nx = (int32_T)((int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims) - handrail_knowledge_dims); - ar = (int32_T)((int32_T)(1 + num_original) * handrail_knowledge_dims); - if ((int32_T)(nx + 1) > ar) { - nx = 0; - ar = 0; - } - - i = (int32_T)((int32_T)((int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims) - handrail_knowledge_dims) + 1); - s12_iter = (int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims); - if (i > s12_iter) { - i = 1; - s12_iter = 0; - } - - O_sizes_idx_0 = (int32_T)((int32_T)(int16_T)(int32_T)((int32_T)(int16_T) - s12_iter - (int32_T)(int16_T)i) + 1); - C_sizes_idx_1 = (int32_T)(int16_T)(int32_T)((int32_T)(int16_T)s12_iter - - (int32_T)(int16_T)i); - for (b_m = 0; b_m <= C_sizes_idx_1; b_m++) { - p_data[b_m] = (int16_T)(int32_T)((int32_T)(int16_T)(int32_T)((int32_T) - (int16_T)i + b_m) - 1); - } - - hr_quat_ISS2hr_idx_1 = est_estimator_P->tun_ase_hr_distance_r * - (real32_T)fabs((real_T)camera_landmarks_data[2]) + - est_estimator_P->tun_ase_hr_r_mag; - C_sizes_idx_1 = (int32_T)(ar - nx); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[i] = next_landmarks_data[(int32_T)(nx + i)] + - hr_quat_ISS2hr_idx_1; - } - - djmgjecbcbiengln_power(varargin_1_data, (int32_T)(ar - nx), - rtb_hr_global_landmarks, &i); - for (i = 0; i <= (int32_T)(O_sizes_idx_0 - 1); i++) { - r_vec_data[(int32_T)p_data[i]] = (real_T)rtb_hr_global_landmarks[i]; - } - } - - // Compress the r and H - jmohiecblfcjnohl_qr(H_data, H_sizes, est_estimator_B->q1_data, q1_sizes, - T_H_data, T_H_sizes); - C_sizes_idx_1 = q1_sizes[0]; - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - H_data[(int32_T)(b_m + (int32_T)(C_sizes_idx_1 * i))] = - est_estimator_B->q1_data[(int32_T)((int32_T)(q1_sizes[0] * i) + b_m)]; - } - } - - num_original = q1_sizes[0]; - br = q1_sizes[0]; - for (i = 0; i <= (int32_T)(br - 1); i++) { - for (b_m = 0; b_m < 6; b_m++) { - b_a_data[(int32_T)(b_m + (int32_T)(6 * i))] = H_data[(int32_T) - ((int32_T)(C_sizes_idx_1 * b_m) + i)]; - } - } - - if ((q1_sizes[0] == 1) || ((int32_T)numFeatures == 1)) { - for (i = 0; i < 6; i++) { - rtb_Product_j[i] = 0.0F; - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - rtb_Product_j[i] += b_a_data[(int32_T)((int32_T)(6 * b_m) + i)] * - r_data[b_m]; - } - } - } else { - for (handrail_knowledge_dims = 0; handrail_knowledge_dims < 6; - handrail_knowledge_dims++) { - rtb_Product_j[handrail_knowledge_dims] = 0.0F; - } - - handrail_knowledge_dims = 0; - for (num_original = 0; (int32_T)(num_original + 1) <= C_sizes_idx_1; - num_original++) { - if (r_data[num_original] != 0.0F) { - br = handrail_knowledge_dims; - for (nx = 0; nx < 6; nx++) { - br++; - rtb_Product_j[nx] += b_a_data[(int32_T)(br - 1)] * - r_data[num_original]; - } - } - - handrail_knowledge_dims += 6; - } - } - - jekfopppngdbhlng_diag(r_vec_data, z_est_sizes_idx_1, - est_estimator_B->tmp_data, H_sizes); - num_original = H_sizes[1]; - for (i = 0; i < 6; i++) { - br = H_sizes[1]; - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - b_a_data[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 <= (int32_T)(C_sizes_idx_1 - 1); i_0++) { - b_a_data[(int32_T)(i + (int32_T)(6 * b_m))] += H_data[(int32_T) - ((int32_T)(C_sizes_idx_1 * i) + i_0)] * (real32_T) - est_estimator_B->tmp_data[(int32_T)((int32_T)(H_sizes[0] * b_m) + - i_0)]; - } - } - } - - if ((H_sizes[1] == 1) || (q1_sizes[0] == 1)) { - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_R_mat[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 <= (int32_T)(num_original - 1); i_0++) { - rtb_R_mat[(int32_T)(i + (int32_T)(6 * b_m))] += b_a_data[(int32_T) - ((int32_T)(6 * i_0) + i)] * H_data[(int32_T)((int32_T) - (C_sizes_idx_1 * b_m) + i_0)]; - } - } - } - } else { - memset(&rtb_R_mat[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (handrail_knowledge_dims = 0; handrail_knowledge_dims <= 31; - handrail_knowledge_dims += 6) { - for (br = handrail_knowledge_dims; (int32_T)(br + 1) <= (int32_T) - (handrail_knowledge_dims + 6); br++) { - rtb_R_mat[br] = 0.0F; - } - } - - handrail_knowledge_dims = 0; - for (br = 0; br <= 31; br += 6) { - nx = 0; - ar = (int32_T)(handrail_knowledge_dims + num_original); - for (i = handrail_knowledge_dims; (int32_T)(i + 1) <= ar; i++) { - if (est_estimator_B->q1_data[(int32_T)(i % q1_sizes[0] + (int32_T) - (q1_sizes[0] * div_nzp_s32_floor(i, q1_sizes[0])))] != 0.0F) { - s12_iter = nx; - for (O_sizes_idx_0 = br; (int32_T)(O_sizes_idx_0 + 1) <= (int32_T) - (br + 6); O_sizes_idx_0++) { - s12_iter++; - rtb_R_mat[O_sizes_idx_0] += est_estimator_B->q1_data[(int32_T)(i - % q1_sizes[0] + (int32_T)(q1_sizes[0] * div_nzp_s32_floor(i, - q1_sizes[0])))] * b_a_data[(int32_T)(s12_iter - 1)]; - } - } - - nx += 6; - } - - handrail_knowledge_dims += num_original; - } - } - - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Residual and H' - - // Convert H to be in terms of the entire state vector - // ':1:3' - est_estimator_B->error_out = 0; - for (i = 0; i < 6; i++) { - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Residual and H' - - est_estimator_B->r_out[i] = rtb_Product_j[i]; - - // MATLAB Function: '/Compute Residual and H' - for (b_m = 0; b_m < 6; b_m++) { - // SignalConversion: '/Signal Conversion' - est_estimator_B->H_out[(int32_T)(b_m + (int32_T)(6 * i))] = T_H_data - [(int32_T)((int32_T)(T_H_sizes[0] * i) + b_m)]; - } - } - - // MATLAB Function: '/Compute Residual and H' - for (i = 0; i < 111; i++) { - for (b_m = 0; b_m < 6; b_m++) { - // SignalConversion: '/Signal Conversion' - est_estimator_B->H_out[(int32_T)(b_m + (int32_T)(6 * (int32_T)(i + 6)))] - = 0.0F; - } - } - - // SignalConversion: '/Signal Conversion' - memcpy(&est_estimator_B->R_mat[0], &rtb_R_mat[0], (uint32_T)(36U * sizeof - (real32_T))); - - // Sum: '/Sum' - UnitDelay_DSTATE_aug_state_enum = (uint32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[0]; - for (ar = 0; ar < 49; ar++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[(int32_T)(ar + 1)]; - } - - // SignalConversion: '/Signal Conversion' incorporates: - // Sum: '/Sum' - - numFeatures = (uint8_T)UnitDelay_DSTATE_aug_state_enum; - - // SignalConversion: '/Signal Conversion' incorporates: - // Constant: '/Constant' - - memcpy(&rtb_UnitDelay18[0], &est_estimator_P->Constant_Value[0], (uint32_T) - (50U * sizeof(real_T))); - - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Global positions of Handrail Features' - - hr_P_hr_ISS_ISS[0] = est_estimator_DW->hr_P_hr_ISS_ISS_pers[0]; - hr_P_hr_ISS_ISS[1] = est_estimator_DW->hr_P_hr_ISS_ISS_pers[1]; - hr_P_hr_ISS_ISS[2] = est_estimator_DW->hr_P_hr_ISS_ISS_pers[2]; - - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Global positions of Handrail Features' - - hr_quat_ISS2hr_idx_0 = est_estimator_DW->hr_quat_ISS2hr_pers[0]; - hr_quat_ISS2hr_idx_1 = est_estimator_DW->hr_quat_ISS2hr_pers[1]; - hr_quat_ISS2hr_idx_2 = est_estimator_DW->hr_quat_ISS2hr_pers[2]; - hr_quat_ISS2hr_idx_3 = est_estimator_DW->hr_quat_ISS2hr_pers[3]; - - // End of Outputs for SubSystem: '/HR_Compute_Residual_and_H1' - } else { - // Outputs for IfAction SubSystem: '/ML_Compute_Residual_and_H' incorporates: - // ActionPort: '/Action Port' - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare = (est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_map); - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant3' - - if (rtb_Compare) { - rtb_Switch1 = est_estimator_P->tun_ase_navcam_inv_focal_length; - } else { - rtb_Switch1 = est_estimator_P->tun_ase_dockcam_inv_focal_length; - } - - // End of Switch: '/Switch1' - - // MATLAB Function: '/Compute Residual and H' incorporates: - // Inport: '/landmark_msg' - - // MATLAB Function 'camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H/Compute Residual and H': ':1' - // ':1:3' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - br = 0; - for (num_original = 0; num_original < 50; num_original++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[num_original] - != 0) { - br++; - } - } - - C_sizes_idx_1 = br; - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[nx] != 0) { - e_data[br] = (int8_T)(int32_T)(nx + 1); - br++; - } - } - - O_sizes_idx_0 = C_sizes_idx_1; - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - r_data_0[i] = est_estimator_U_landmark_msg->cvs_observations[(int32_T) - ((int32_T)e_data[i] - 1)] * rtb_Switch1; - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - r_data_0[(int32_T)(i + C_sizes_idx_1)] = - est_estimator_U_landmark_msg->cvs_observations[(int32_T)((int32_T) - e_data[i] + 49)] * rtb_Switch1; - } - - for (i = 0; i < 12; i++) { - camera_ml_tf_global[i] = (real_T)g_0[i]; - } - - fkfcbaiengdjgdje_quaternion_to_rotation(UnitDelay_DSTATE_n_ml_quat_ISS2, - rtb_Product1_0); - for (i = 0; i < 3; i++) { - C[(int32_T)(3 * i)] = (real32_T)-(real_T)rtb_Product1_0[(int32_T)(3 * i)]; - camera_ml_tf_global[(int32_T)(3 * i)] = (real_T)rtb_Product1_0[(int32_T) - (3 * i)]; - C[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)-(real_T)rtb_Product1_0 - [(int32_T)((int32_T)(3 * i) + 1)]; - camera_ml_tf_global[(int32_T)(1 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)]; - C[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)-(real_T)rtb_Product1_0 - [(int32_T)((int32_T)(3 * i) + 2)]; - camera_ml_tf_global[(int32_T)(2 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 3; i++) { - camera_ml_tf_global[(int32_T)(9 + i)] = (real_T)((C[(int32_T)(i + 3)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] + C[i] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[0]) + C[(int32_T)(i + 6)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[2]); - } - - num_original = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[nx] != 0) { - num_original++; - } - } - - C_sizes_idx_1 = num_original; - num_original = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[nx] != 0) { - e_data[num_original] = (int8_T)(int32_T)(nx + 1); - num_original++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] - 1)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] + 49)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] + 99)]; - } - - if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else { - num_original = 0; - if (C_sizes_idx_1 > 0) { - num_original = C_sizes_idx_1; - } - } - - empty_non_axis_sizes = (num_original == 0); - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - br = 3; - } else { - br = 0; - } - - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - nx = 1; - } else { - nx = 0; - } - - ar = (int32_T)(br + nx); - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - rtb_global_points[(int32_T)(b_m + (int32_T)(ar * i))] = - varargin_1_data[(int32_T)((int32_T)(br * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - rtb_global_points[(int32_T)((int32_T)(b_m + br) + (int32_T)(ar * i))] = - 1.0F; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)(ar * b_m)] * (real32_T) - camera_ml_tf_global[i]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 1)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 3)]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 2)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 6)]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 3)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 9)]; - } - } - - // Landmarks in the camera frame - handrail_knowledge_dims = 0; - z_est_sizes_idx_1 = num_original; - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - z_est_data[(int32_T)(i << 1)] = camera_landmarks_data[(int32_T)(3 * i)] / - camera_landmarks_data[(int32_T)((int32_T)(3 * i) + 2)]; - z_est_data[(int32_T)(1 + (int32_T)(i << 1))] = camera_landmarks_data - [(int32_T)((int32_T)(3 * i) + 1)] / camera_landmarks_data[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i <= (int32_T)(O_sizes_idx_0 - 1); i++) { - omega_error_data[(int32_T)(i << 1)] = r_data_0[i] - z_est_data[(int32_T) - (i << 1)]; - omega_error_data[(int32_T)(1 + (int32_T)(i << 1))] = r_data_0[(int32_T) - (i + O_sizes_idx_0)] - z_est_data[(int32_T)((int32_T)(i << 1) + 1)]; - } - - nx = (int32_T)(O_sizes_idx_0 << 1); - s12_iter = (int32_T)(int8_T)(int32_T)(O_sizes_idx_0 << 1); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - r_data_0[br] = omega_error_data[br]; - } - - // forward propagate the saved state based on velocity to add an error - // term for the timing of the registration pulse - for (i = 0; i < 12; i++) { - next_ml_tf_global[i] = (real_T)g_0[i]; - } - - // move to camera frame and with half a frame rotation - // TODO: we should use the velocity and omega from the time of the - // registration pulse - rtb_ml_vel_aug_0[0] = rtb_ml_vel_aug[0]; - rtb_ml_vel_aug_0[1] = rtb_ml_vel_aug[1]; - rtb_ml_vel_aug_0[2] = rtb_ml_vel_aug[2]; - - // Switch: '/Switch' - if (rtb_Compare) { - // MATLAB Function: '/Compute Residual and H' incorporates: - // Constant: '/Constant' - - rtb_Compare_2[0] = est_estimator_P->tun_abp_q_body2navcam[0]; - rtb_Compare_2[1] = est_estimator_P->tun_abp_q_body2navcam[1]; - rtb_Compare_2[2] = est_estimator_P->tun_abp_q_body2navcam[2]; - rtb_Compare_2[3] = est_estimator_P->tun_abp_q_body2navcam[3]; - } else { - // MATLAB Function: '/Compute Residual and H' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - - rtb_Compare_2[0] = est_estimator_P->tun_abp_q_body2dockcam[0]; - rtb_Compare_2[1] = est_estimator_P->tun_abp_q_body2dockcam[1]; - rtb_Compare_2[2] = est_estimator_P->tun_abp_q_body2dockcam[2]; - rtb_Compare_2[3] = est_estimator_P->tun_abp_q_body2dockcam[3]; - } - - // End of Switch: '/Switch' - - // MATLAB Function: '/Compute Residual and H' incorporates: - // Constant: '/Constant' - // Inport: '/landmark_msg' - // RelationalOperator: '/Compare' - - gdjmmglnmgdjlfkf_quat_rotation_vec(rtb_ml_vel_aug_0, rtb_Compare_2, tmp); - baieimopcbaiaaai_eulers_to_quat - (est_estimator_P->tun_ase_ml_forward_projection_time * (real32_T)tmp[0], - est_estimator_P->tun_ase_ml_forward_projection_time * (real32_T)tmp[1], - est_estimator_P->tun_ase_ml_forward_projection_time * (real32_T)tmp[2], - rtb_Product1); - iecjopppiecjmgln_quatmult(rtb_Product1, UnitDelay_DSTATE_n_ml_quat_ISS2, - rtb_Compare_2); - fkfcbaiengdjgdje_quaternion_to_rotation(rtb_Compare_2, rtb_Product1_0); - for (i = 0; i < 3; i++) { - next_ml_tf_global[(int32_T)(3 * i)] = (real_T)rtb_Product1_0[(int32_T)(3 - * i)]; - C[(int32_T)(3 * i)] = (real32_T)-camera_ml_tf_global[(int32_T)(3 * i)]; - next_ml_tf_global[(int32_T)(1 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)]; - C[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)-camera_ml_tf_global - [(int32_T)((int32_T)(3 * i) + 1)]; - next_ml_tf_global[(int32_T)(2 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)]; - C[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)-camera_ml_tf_global - [(int32_T)((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 3; i++) { - next_ml_tf_global[(int32_T)(9 + i)] = (real_T)(((C[(int32_T)(i + 3)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] + C[i] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[0]) + C[(int32_T)(i + 6)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[2]) + - est_estimator_P->tun_ase_ml_forward_projection_time * (real32_T) - rtb_ml_omega_aug[i]); - } - - nx = 0; - for (ar = 0; ar < 50; ar++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[ar] != 0) { - nx++; - } - } - - C_sizes_idx_1 = nx; - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[br] != 0) { - e_data[nx] = (int8_T)(int32_T)(br + 1); - nx++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] - 1)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] + 49)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] + 99)]; - } - - if (!(C_sizes_idx_1 == 0)) { - nx = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - nx = C_sizes_idx_1; - } else { - nx = 0; - if (C_sizes_idx_1 > 0) { - nx = C_sizes_idx_1; - } - } - - empty_non_axis_sizes = (nx == 0); - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - num_original = 3; - } else { - num_original = 0; - } - - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - br = 1; - } else { - br = 0; - } - - ar = (int32_T)(num_original + br); - for (i = 0; i <= (int32_T)(nx - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - rtb_global_points[(int32_T)(b_m + (int32_T)(ar * i))] = - varargin_1_data[(int32_T)((int32_T)(num_original * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(nx - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - rtb_global_points[(int32_T)((int32_T)(b_m + num_original) + (int32_T) - (ar * i))] = 1.0F; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)(ar * b_m)] * (real32_T) - next_ml_tf_global[i]; - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 1)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 3)]; - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 2)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 6)]; - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 3)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 9)]; - } - } - - // Landmarks in the camera frame - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - omega_error_data[(int32_T)(i << 1)] = z_est_data[(int32_T)(i << 1)] - - next_landmarks_data[(int32_T)(3 * i)] / next_landmarks_data[(int32_T) - ((int32_T)(3 * i) + 2)]; - omega_error_data[(int32_T)(1 + (int32_T)(i << 1))] = z_est_data[(int32_T) - ((int32_T)(i << 1) + 1)] - next_landmarks_data[(int32_T)((int32_T)(3 * - i) + 1)] / next_landmarks_data[(int32_T)((int32_T)(3 * i) + 2)]; - } - - nx = (int32_T)(z_est_sizes_idx_1 << 1); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - y_data_0[br] = omega_error_data[br]; - } - - iecjbieccbailfcb_abs(y_data_0, (int32_T)(int8_T)(int32_T)(O_sizes_idx_0 << - 1), omega_error_data, &i); - - // compute Jacobian - z_est_sizes_idx_1 = (int32_T)(O_sizes_idx_0 << 1); - C_sizes_idx_1 = (int32_T)(O_sizes_idx_0 << 1); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - r_vec_data_0[i] = 0.0; - } - - H_sizes[0] = (int32_T)(O_sizes_idx_0 << 1); - H_sizes[1] = 6; - C_sizes_idx_1 = (int32_T)((int32_T)(O_sizes_idx_0 << 1) * 6); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - H_data_0[i] = 0.0F; - } - - for (num_original = 0; num_original <= (int32_T)(O_sizes_idx_0 - 1); - num_original++) { - hr_quat_ISS2hr_idx_0 = 1.0F / camera_landmarks_data[(int32_T)((int32_T) - (3 * num_original) + 2)]; - h[0] = 1.0F; - h[1] = 0.0F; - h[4] = -z_est_data[(int32_T)(num_original << 1)]; - h[2] = 0.0F; - h[3] = 1.0F; - h[5] = -z_est_data[(int32_T)((int32_T)(num_original << 1) + 1)]; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - tmp_7[0] = 0.0F; - tmp_7[3] = -camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) - + 2)]; - tmp_7[6] = camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + - 1)]; - tmp_7[1] = camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + - 2)]; - tmp_7[4] = 0.0F; - tmp_7[7] = -camera_landmarks_data[(int32_T)(3 * num_original)]; - tmp_7[2] = -camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) - + 1)]; - tmp_7[5] = camera_landmarks_data[(int32_T)(3 * num_original)]; - tmp_7[8] = 0.0F; - for (i = 0; i < 3; i++) { - hr_quat_ISS2hr_idx_1 = h[(int32_T)(i << 1)] * hr_quat_ISS2hr_idx_0; - temp_0[(int32_T)(i << 1)] = -hr_quat_ISS2hr_idx_1; - temp[(int32_T)(i << 1)] = hr_quat_ISS2hr_idx_1; - hr_quat_ISS2hr_idx_1 = h[(int32_T)((int32_T)(i << 1) + 1)] * - hr_quat_ISS2hr_idx_0; - temp_0[(int32_T)(1 + (int32_T)(i << 1))] = -hr_quat_ISS2hr_idx_1; - temp[(int32_T)(1 + (int32_T)(i << 1))] = hr_quat_ISS2hr_idx_1; - } - - for (i = 0; i < 2; i++) { - for (b_m = 0; b_m < 3; b_m++) { - temp_1[(int32_T)(i + (int32_T)(b_m << 1))] = 0.0F; - temp_2[(int32_T)(i + (int32_T)(b_m << 1))] = 0.0F; - temp_1[(int32_T)(i + (int32_T)(b_m << 1))] += tmp_7[(int32_T)(3 * - b_m)] * temp[i]; - temp_2[(int32_T)(i + (int32_T)(b_m << 1))] += (real32_T) - camera_ml_tf_global[(int32_T)(3 * b_m)] * temp_0[i]; - temp_1[(int32_T)(i + (int32_T)(b_m << 1))] += tmp_7[(int32_T) - ((int32_T)(3 * b_m) + 1)] * temp[(int32_T)(i + 2)]; - temp_2[(int32_T)(i + (int32_T)(b_m << 1))] += (real32_T) - camera_ml_tf_global[(int32_T)((int32_T)(3 * b_m) + 1)] * temp_0 - [(int32_T)(i + 2)]; - temp_1[(int32_T)(i + (int32_T)(b_m << 1))] += tmp_7[(int32_T) - ((int32_T)(3 * b_m) + 2)] * temp[(int32_T)(i + 4)]; - temp_2[(int32_T)(i + (int32_T)(b_m << 1))] += (real32_T) - camera_ml_tf_global[(int32_T)((int32_T)(3 * b_m) + 2)] * temp_0 - [(int32_T)(i + 4)]; - } - } - - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - for (b_m = 0; b_m < 3; b_m++) { - H_data_0[(int32_T)(i + (int32_T)(H_sizes[0] * b_m))] = temp_1[(int32_T) - (b_m << 1)]; - H_data_0[(int32_T)((int32_T)(i + (int32_T)(H_sizes[0] * b_m)) + 1)] = - temp_1[(int32_T)((int32_T)(b_m << 1) + 1)]; - } - - for (b_m = 0; b_m < 3; b_m++) { - H_data_0[(int32_T)(i + (int32_T)(H_sizes[0] * (int32_T)(b_m + 3)))] = - temp_2[(int32_T)(b_m << 1)]; - H_data_0[(int32_T)((int32_T)(i + (int32_T)(H_sizes[0] * (int32_T)(b_m - + 3))) + 1)] = temp_2[(int32_T)((int32_T)(b_m << 1) + 1)]; - } - - // account for distortion model - // if ru > 1e-5 - // distorted = O(i, :) * tan(ru * ase_distortion) / d2 / ru; - // radius = norm(distorted); - // rd = radius * ase_inv_focal_length; - // t = tan(ase_distortion * rd); - // f1 = f1 * 1 / (d2 * radius) * (distorted(2)^2 * t / rd + ase_distortion * distorted(1)^2 * (1 + t^2)); - // f2 = f2 * 1 / (d2 * radius) * (distorted(1)^2 * t / rd + ase_distortion * distorted(2)^2 * (1 + t^2)); - // end - if (rtb_Compare) { - // make the confidence depend on mapped landmarked error which is function - // of distance - hr_quat_ISS2hr_idx_0 = est_estimator_P->tun_ase_map_error / - camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + 2)] + - est_estimator_P->tun_ase_vis_r_mag; - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - rtb_Switch1_1[0] = (hr_quat_ISS2hr_idx_0 + omega_error_data[i]) * - rtb_Switch1; - rtb_Switch1_1[1] = (omega_error_data[(int32_T)(1 + i)] + - hr_quat_ISS2hr_idx_0) * rtb_Switch1; - nohdcbaibiecnohl_power(rtb_Switch1_1, tmp_0); - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - r_vec_data_0[i] = (real_T)tmp_0[0]; - r_vec_data_0[(int32_T)(1 + i)] = (real_T)tmp_0[1]; - } else { - // AR update - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - rtb_Switch1_0[0] = (est_estimator_P->tun_ase_dock_r_mag + - omega_error_data[i]) * rtb_Switch1; - rtb_Switch1_0[1] = (omega_error_data[(int32_T)(1 + i)] + - est_estimator_P->tun_ase_dock_r_mag) * rtb_Switch1; - nohdcbaibiecnohl_power(rtb_Switch1_0, tmp_0); - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - r_vec_data_0[i] = (real_T)tmp_0[0]; - r_vec_data_0[(int32_T)(1 + i)] = (real_T)tmp_0[1]; - } - } - - // r_out = zeros(size(L, 1),1); - for (i = 0; i < 50; i++) { - rtb_UnitDelay18[i] = (rtNaN); - } - - // only ignore observations via Malanobis check in the following - // conditions: - // 1. The estimator is converged - // 2. The estiamtor is performing a ML update - // 3. The mahalanobis check hasn't previously rejected the maximum number - // of consecutive frame allowed to be rejected set via tun_max_mahal_reject_frames - if ((rtb_Saturation_n == est_estimator_P->ase_status_converged) && - rtb_Compare && (est_estimator_DW->num_consecutive_rejected_meas < - (real_T)est_estimator_P->tun_max_mahal_reject_frames)) - { - // S = H*P_in(16:21, 16:21)*H'; - // for i=1:size(S, 1) - // S(i, i) = S(i, i) + r_vec(i); - // end - // S_inv = pinv(S); - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[br] != 0) { - nx++; - } - } - - br = (int32_T)(((real_T)nx * 2.0 + 1.0) / 2.0); - for (nx = 0; nx <= (int32_T)(br - 1); nx++) { - ar = (int32_T)(nx << 1); - for (i = 0; i < 6; i++) { - x[(int32_T)(i << 1)] = H_data_0[(int32_T)((int32_T)(H_sizes[0] * i) - + ar)]; - x[(int32_T)(1 + (int32_T)(i << 1))] = H_data_0[(int32_T)((int32_T) - ((int32_T)(H_sizes[0] * i) + ar) + 1)]; - } - - for (i = 0; i < 2; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_Product1_1[(int32_T)(i + (int32_T)(b_m << 1))] = 0.0F; - for (i_0 = 0; i_0 < 6; i_0++) { - rtb_Product1_1[(int32_T)(i + (int32_T)(b_m << 1))] += - est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T) - ((int32_T)(15 + b_m) * 117) + i_0) + 15)] * x[(int32_T) - ((int32_T)(i_0 << 1) + i)]; - } - - rtb_Product1_2[(int32_T)(b_m + (int32_T)(6 * i))] = H_data_0 - [(int32_T)((int32_T)(i + ar) + (int32_T)(H_sizes[0] * b_m))]; - } - } - - for (i = 0; i < 2; i++) { - for (b_m = 0; b_m < 2; b_m++) { - rtb_Product1[(int32_T)(i + (int32_T)(b_m << 1))] = 0.0F; - for (i_0 = 0; i_0 < 6; i_0++) { - rtb_Product1[(int32_T)(i + (int32_T)(b_m << 1))] += - rtb_Product1_1[(int32_T)((int32_T)(i_0 << 1) + i)] * - rtb_Product1_2[(int32_T)((int32_T)(6 * b_m) + i_0)]; - } - } - } - - H[0] = rtb_Product1[0] + (real32_T)r_vec_data_0[ar]; - H[1] = rtb_Product1[1]; - H[2] = rtb_Product1[2]; - H[3] = (real32_T)r_vec_data_0[(int32_T)(ar + 1)] + rtb_Product1[3]; - iekfiecjknopophd_pinv(H, rtb_Product1); - rtb_UnitDelay18[(int32_T)((int32_T)(((real_T)(int32_T)(ar + 1) + 1.0) / - 2.0) - 1)] = (real_T)(real32_T)sqrt((real_T)((r_data_0[(int32_T)(1 + - ar)] * rtb_Product1[1] + r_data_0[ar] * rtb_Product1[0]) * - r_data_0[ar] + (r_data_0[(int32_T)(1 + ar)] * rtb_Product1[3] + - r_data_0[ar] * rtb_Product1[2]) * r_data_0[(int32_T) - (1 + ar)])); - } - - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[br] != 0) { - nx++; - } - } - - if (1 > nx) { - C_sizes_idx_1 = 0; - } else { - C_sizes_idx_1 = nx; - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - invalid_vec_data[i] = (rtb_UnitDelay18[i] > (real_T) - est_estimator_P->tun_ase_mahal_distance_max); - } - - // toss if nothing is within std. dev. of expected, ransac prob. failed - // if sum(mahal_dists < 3) == 0 - // invalid_vec(:) = true; - // end - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[nx] != 0) { - br++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - g_result_data[(int32_T)(i << 1)] = invalid_vec_data[i]; - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - g_result_data[(int32_T)(1 + (int32_T)(i << 1))] = invalid_vec_data[i]; - } - - nx = (int32_T)(C_sizes_idx_1 << 1); - c_sz_idx_0 = (int8_T)(int32_T)(br << 1); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - invalid_data[br] = g_result_data[br]; - } - - mgdbbiekfknonglf_nullAssignment(r_data_0, &s12_iter, invalid_data, - (int32_T)c_sz_idx_0); - imohcjmoimopimoh_nullAssignment(H_data_0, H_sizes, invalid_data, - (int32_T)c_sz_idx_0); - ophlcjmgkfcbmohl_nullAssignment(r_vec_data_0, &z_est_sizes_idx_1, - invalid_data, (int32_T)c_sz_idx_0); - } - - i = (int32_T)rt_roundd_snf((real_T)s12_iter / 2.0); - if (i < 256) { - if (i >= 0) { - numFeatures = (uint8_T)i; - } else { - numFeatures = 0U; - } - } else { - numFeatures = MAX_uint8_T; - } - - if (rtb_Compare) { - rtb_Switch1 = est_estimator_P->tun_ase_min_ml_meas; - - // Sparse map update - } else { - rtb_Switch1 = est_estimator_P->tun_ase_min_ar_meas; - - // AR update - } - - // toss if not enough measurements, (Mahalanobis check removed all valid features) - // features - if (((real_T)s12_iter / 2.0 < (real_T)rtb_Switch1) || ((real_T)s12_iter < - (real_T)(int8_T)(int32_T)(O_sizes_idx_0 << 1) / 2.0)) { - numFeatures = 0U; - handrail_knowledge_dims = 1; - for (i = 0; i < 6; i++) { - rtb_Product_j[i] = 1.0F; - } - - ngdjjecbgdbaglfc_eye(rtb_H_out); - biekcjmgdbaadbim_eye(rtb_R_mat); - est_estimator_DW->num_consecutive_rejected_meas++; - } else { - est_estimator_DW->num_consecutive_rejected_meas = 0.0; - - // Compress the r and H - jmglopphppphkfkf_qr(H_data_0, H_sizes, est_estimator_B->q1_data_c, - q1_sizes, T_H_data_0, T_H_sizes); - C_sizes_idx_1 = q1_sizes[0]; - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - H_data_0[(int32_T)(b_m + (int32_T)(C_sizes_idx_1 * i))] = - est_estimator_B->q1_data_c[(int32_T)((int32_T)(q1_sizes[0] * i) + - b_m)]; - } - } - - num_original = q1_sizes[0]; - br = q1_sizes[0]; - for (i = 0; i <= (int32_T)(br - 1); i++) { - for (b_m = 0; b_m < 6; b_m++) { - a_data_0[(int32_T)(b_m + (int32_T)(6 * i))] = H_data_0[(int32_T) - ((int32_T)(C_sizes_idx_1 * b_m) + i)]; - } - } - - if ((q1_sizes[0] == 1) || (s12_iter == 1)) { - for (i = 0; i < 6; i++) { - rtb_Product_j[i] = 0.0F; - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - rtb_Product_j[i] += a_data_0[(int32_T)((int32_T)(6 * b_m) + i)] * - r_data_0[b_m]; - } - } - } else { - for (nx = 0; nx < 6; nx++) { - rtb_Product_j[nx] = 0.0F; - } - - ar = 0; - for (i = 0; (int32_T)(i + 1) <= C_sizes_idx_1; i++) { - if (r_data_0[i] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = 0; O_sizes_idx_0 < 6; O_sizes_idx_0++) { - s12_iter++; - rtb_Product_j[O_sizes_idx_0] += a_data_0[(int32_T)(s12_iter - 1)] - * r_data_0[i]; - } - } - - ar += 6; - } - } - - moppbaaafkfkimgd_diag(r_vec_data_0, z_est_sizes_idx_1, - est_estimator_B->tmp_data_m, H_sizes); - num_original = H_sizes[1]; - for (i = 0; i < 6; i++) { - br = H_sizes[1]; - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - a_data_0[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 <= (int32_T)(C_sizes_idx_1 - 1); i_0++) { - a_data_0[(int32_T)(i + (int32_T)(6 * b_m))] += H_data_0[(int32_T) - ((int32_T)(C_sizes_idx_1 * i) + i_0)] * (real32_T) - est_estimator_B->tmp_data_m[(int32_T)((int32_T)(H_sizes[0] * b_m) - + i_0)]; - } - } - } - - if ((H_sizes[1] == 1) || (q1_sizes[0] == 1)) { - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_R_mat[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 <= (int32_T)(num_original - 1); i_0++) { - rtb_R_mat[(int32_T)(i + (int32_T)(6 * b_m))] += a_data_0 - [(int32_T)((int32_T)(6 * i_0) + i)] * H_data_0[(int32_T) - ((int32_T)(C_sizes_idx_1 * b_m) + i_0)]; - } - } - } - } else { - memset(&rtb_R_mat[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (br = 0; br <= 31; br += 6) { - for (nx = br; (int32_T)(nx + 1) <= (int32_T)(br + 6); nx++) { - rtb_R_mat[nx] = 0.0F; - } - } - - br = 0; - for (nx = 0; nx <= 31; nx += 6) { - ar = 0; - b_m = (int32_T)(br + num_original); - for (i = br; (int32_T)(i + 1) <= b_m; i++) { - if (est_estimator_B->q1_data_c[(int32_T)(i % q1_sizes[0] + - (int32_T)(q1_sizes[0] * div_nzp_s32_floor(i, q1_sizes[0])))] - != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = nx; (int32_T)(O_sizes_idx_0 + 1) <= - (int32_T)(nx + 6); O_sizes_idx_0++) { - s12_iter++; - rtb_R_mat[O_sizes_idx_0] += est_estimator_B->q1_data_c - [(int32_T)(i % q1_sizes[0] + (int32_T)(q1_sizes[0] * - div_nzp_s32_floor(i, q1_sizes[0])))] * a_data_0[(int32_T) - (s12_iter - 1)]; - } - } - - ar += 6; - } - - br += num_original; - } - } - - // Convert H to be in terms of the entire state vector - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_H_out[(int32_T)(b_m + (int32_T)(6 * i))] = T_H_data_0[(int32_T) - ((int32_T)(T_H_sizes[0] * i) + b_m)]; - } - } - - for (i = 0; i < 111; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_H_out[(int32_T)(b_m + (int32_T)(6 * (int32_T)(i + 6)))] = 0.0F; - } - } - } - - // ':1:3' - for (i = 0; i < 6; i++) { - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Residual and H' - - est_estimator_B->r_out[i] = rtb_Product_j[i]; - } - - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Residual and H' - - est_estimator_B->error_out = handrail_knowledge_dims; - - // SignalConversion: '/Signal Conversion' - memcpy(&est_estimator_B->H_out[0], &rtb_H_out[0], (uint32_T)(702U * sizeof - (real32_T))); - - // SignalConversion: '/Signal Conversion' - memcpy(&est_estimator_B->R_mat[0], &rtb_R_mat[0], (uint32_T)(36U * sizeof - (real32_T))); - - // SignalConversion: '/Signal Conversion' - hr_P_hr_ISS_ISS[0] = UnitDelay_DSTATE_n_hr_P_hr_ISS_; - hr_P_hr_ISS_ISS[1] = UnitDelay_DSTATE_n_hr_P_hr_IS_0; - hr_P_hr_ISS_ISS[2] = UnitDelay_DSTATE_n_hr_P_hr_IS_1; - - // SignalConversion: '/Signal Conversion' - hr_quat_ISS2hr_idx_0 = UnitDelay_DSTATE_n_hr_quat_ISS2; - hr_quat_ISS2hr_idx_1 = UnitDelay_DSTATE_n_hr_quat_IS_0; - hr_quat_ISS2hr_idx_2 = UnitDelay_DSTATE_n_hr_quat_IS_1; - hr_quat_ISS2hr_idx_3 = UnitDelay_DSTATE_n_hr_quat_IS_2; - - // End of Outputs for SubSystem: '/ML_Compute_Residual_and_H' - } - - // End of If: '/IF' - - // Product: '/Product' - for (i = 0; i < 6; i++) { - rtb_Product_j[i] = est_estimator_B->r_out[i] * est_estimator_B->r_out[i]; - } - - // End of Product: '/Product' - - // Sum: '/Sum of Elements' - rtb_Switch1 = rtb_Product_j[0]; - for (ar = 0; ar < 5; ar++) { - rtb_Switch1 += rtb_Product_j[(int32_T)(ar + 1)]; - } - - // End of Sum: '/Sum of Elements' - - // Sqrt: '/Sqrt' - rtb_Switch1 = (real32_T)sqrt((real_T)rtb_Switch1); - - // S-Function (ex_compute_delta_state_and_cov): '/ex_compute_delta_state_and_cov' - rtb_ex_compute_delta_state_a_pg = compute_delta_state_and_cov((real32_T*) - &est_estimator_B->r_out[0], est_estimator_B->error_out, (real32_T*) - &est_estimator_B->H_out[0], 6, 117, est_estimator_P->Constant_Value_h, - (real32_T*)&est_estimator_B->R_mat[0], (real32_T*) - &est_estimator_B->Assignment[0], &rtb_ex_compute_delta_state_an_p[0], - &est_estimator_B->Switch1[0]); - - // S-Function (ex_apply_delta_state): '/ex_apply_delta_state' - apply_delta_state((real32_T*)&rtb_ex_compute_delta_state_an_p[0], 117, - est_estimator_P->Constant_Value_ln, (real32_T*) - &rtb_Merge_o[0], (real32_T*)&accel[0], (real32_T*) - &UnitDelay_DSTATE_n_V_B_ISS_ISS[0], (real32_T*) - &UnitDelay_DSTATE_n_accel_bias[0], (real32_T*) - &UnitDelay_DSTATE_n_P_EST_ISS_IS[0], (real32_T*) - &UnitDelay_DSTATE_n_ml_quat_ISS2[0], (real32_T*) - &UnitDelay_DSTATE_n_ml_P_cam_ISS[0], - UnitDelay_DSTATE_n_kfl_status, (real32_T*) - &UnitDelay_DSTATE_n_of_quat_ISS2[0], (real32_T*) - &UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &rtb_ex_apply_delta_state_o1_d[0], - &rtb_ex_apply_delta_state_o2_i[0], - &rtb_ex_apply_delta_state_o3_e[0], - &rtb_ex_apply_delta_state_o4_g[0], - &rtb_ex_apply_delta_state_o5_i[0], - &rtb_ex_apply_delta_state_o6_d[0], - &rtb_ex_apply_delta_state_o7_p[0], - &rtb_ex_apply_delta_state_o8_j, - &rtb_ex_apply_delta_state_o9_c[0], - &rtb_ex_apply_delta_state_o10_i[0]); - - // Logic: '/Logical Operator1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant' - // Product: '/Divide' - // RelationalOperator: '/Relational Operator1' - // Sum: '/Add' - // UnitDelay: '/Unit Delay2' - - empty_non_axis_sizes = ((est_estimator_P->ase_minumum_resid_thresh > (real_T) - ((est_estimator_P->UnitDelay2_InitialCondition - rtb_Switch1) / - rtb_Switch1)) || (rtb_ex_compute_delta_state_a_pg != 0)); - - // Switch: '/Switch' incorporates: - // BusAssignment: '/Bus Assignment1' - // BusAssignment: '/Bus Assignment1' - // Constant: '/Constant2' - // Constant: '/Constant3' - - if (empty_non_axis_sizes) { - numFeatures = rtb_numFeatures_f; - memcpy(&rtb_UnitDelay18[0], &UnitDelay_DSTATE_n_ml_mahal_dis[0], (uint32_T) - (50U * sizeof(real_T))); - hr_P_hr_ISS_ISS[0] = UnitDelay_DSTATE_n_hr_P_hr_ISS_; - hr_P_hr_ISS_ISS[1] = UnitDelay_DSTATE_n_hr_P_hr_IS_0; - hr_P_hr_ISS_ISS[2] = UnitDelay_DSTATE_n_hr_P_hr_IS_1; - hr_quat_ISS2hr_idx_0 = UnitDelay_DSTATE_n_hr_quat_ISS2; - hr_quat_ISS2hr_idx_1 = UnitDelay_DSTATE_n_hr_quat_IS_0; - hr_quat_ISS2hr_idx_2 = UnitDelay_DSTATE_n_hr_quat_IS_1; - hr_quat_ISS2hr_idx_3 = UnitDelay_DSTATE_n_hr_quat_IS_2; - } else { - rtb_Merge_o[0] = rtb_ex_apply_delta_state_o1_d[0]; - rtb_Merge_o[1] = rtb_ex_apply_delta_state_o1_d[1]; - rtb_Merge_o[2] = rtb_ex_apply_delta_state_o1_d[2]; - rtb_Merge_o[3] = rtb_ex_apply_delta_state_o1_d[3]; - accel[0] = rtb_ex_apply_delta_state_o2_i[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = rtb_ex_apply_delta_state_o3_e[0]; - UnitDelay_DSTATE_n_accel_bias[0] = rtb_ex_apply_delta_state_o4_g[0]; - accel[1] = rtb_ex_apply_delta_state_o2_i[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = rtb_ex_apply_delta_state_o3_e[1]; - UnitDelay_DSTATE_n_accel_bias[1] = rtb_ex_apply_delta_state_o4_g[1]; - accel[2] = rtb_ex_apply_delta_state_o2_i[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = rtb_ex_apply_delta_state_o3_e[2]; - UnitDelay_DSTATE_n_accel_bias[2] = rtb_ex_apply_delta_state_o4_g[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = rtb_ex_apply_delta_state_o6_d[0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = rtb_ex_apply_delta_state_o6_d[1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = rtb_ex_apply_delta_state_o6_d[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = rtb_ex_apply_delta_state_o6_d[3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = rtb_ex_apply_delta_state_o7_p[0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = rtb_ex_apply_delta_state_o7_p[1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = rtb_ex_apply_delta_state_o7_p[2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &rtb_ex_apply_delta_state_o9_c[0], (uint32_T)(sizeof(real32_T) << - 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &rtb_ex_apply_delta_state_o10_i[0], (uint32_T)(48U * sizeof - (real32_T))); - UnitDelay_DSTATE_n_kfl_status = rtb_ex_apply_delta_state_o8_j; - rtb_Switch_m = est_estimator_P->Constant2_Value_n4; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], - &est_estimator_P->Constant3_Value[0], (uint32_T)(50U * sizeof - (real_T))); - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = rtb_ex_apply_delta_state_o5_i[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = rtb_ex_apply_delta_state_o5_i[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = rtb_ex_apply_delta_state_o5_i[2]; - } - - // End of Switch: '/Switch' - - // Switch: '/Switch1' - for (i = 0; i < 13689; i++) { - rtb_Switch1 = est_estimator_B->Switch1[i]; - if (empty_non_axis_sizes) { - rtb_Switch1 = est_estimator_B->Assignment[i]; - } - - est_estimator_B->Switch1[i] = rtb_Switch1; - } - - // End of Switch: '/Switch1' - - // SignalConversion: '/OutportBufferForstate_out' incorporates: - // SignalConversion: '/Signal Conversion' - - rtb_Merge2.quat_ISS2B[0] = rtb_Merge_o[0]; - rtb_Merge2.quat_ISS2B[1] = rtb_Merge_o[1]; - rtb_Merge2.quat_ISS2B[2] = rtb_Merge_o[2]; - rtb_Merge2.quat_ISS2B[3] = rtb_Merge_o[3]; - rtb_Merge2.omega_B_ISS_B[0] = rtb_ImpAsg_InsertedFor_Out1_at_[0]; - rtb_Merge2.gyro_bias[0] = accel[0]; - rtb_Merge2.V_B_ISS_ISS[0] = UnitDelay_DSTATE_n_V_B_ISS_ISS[0]; - rtb_Merge2.A_B_ISS_ISS[0] = rtb_Sum_k1; - rtb_Merge2.accel_bias[0] = UnitDelay_DSTATE_n_accel_bias[0]; - rtb_Merge2.P_B_ISS_ISS[0] = rtb_Sum_l; - rtb_Merge2.omega_B_ISS_B[1] = rtb_ImpAsg_InsertedFor_Out1_at_[1]; - rtb_Merge2.gyro_bias[1] = accel[1]; - rtb_Merge2.V_B_ISS_ISS[1] = UnitDelay_DSTATE_n_V_B_ISS_ISS[1]; - rtb_Merge2.A_B_ISS_ISS[1] = rtb_Gain1_f; - rtb_Merge2.accel_bias[1] = UnitDelay_DSTATE_n_accel_bias[1]; - rtb_Merge2.P_B_ISS_ISS[1] = UnitDelay_DSTATE_n_P_B_ISS_ISS_; - rtb_Merge2.omega_B_ISS_B[2] = rtb_ImpAsg_InsertedFor_Out1_at_[2]; - rtb_Merge2.gyro_bias[2] = accel[2]; - rtb_Merge2.V_B_ISS_ISS[2] = UnitDelay_DSTATE_n_V_B_ISS_ISS[2]; - rtb_Merge2.A_B_ISS_ISS[2] = UnitDelay_DSTATE_n_A_B_ISS_ISS_; - rtb_Merge2.accel_bias[2] = UnitDelay_DSTATE_n_accel_bias[2]; - rtb_Merge2.P_B_ISS_ISS[2] = UnitDelay_DSTATE_n_P_B_ISS_IS_0; - rtb_Merge2.confidence = rtb_Saturation_n; - rtb_Merge2.aug_state_enum = rtb_BitwiseOperator; - rtb_Merge2.ml_quat_ISS2cam[0] = UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - rtb_Merge2.ml_quat_ISS2cam[1] = UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - rtb_Merge2.ml_quat_ISS2cam[2] = UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - rtb_Merge2.ml_quat_ISS2cam[3] = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - rtb_Merge2.ml_P_cam_ISS_ISS[0] = UnitDelay_DSTATE_n_ml_P_cam_ISS[0]; - rtb_Merge2.ml_P_cam_ISS_ISS[1] = UnitDelay_DSTATE_n_ml_P_cam_ISS[1]; - rtb_Merge2.ml_P_cam_ISS_ISS[2] = UnitDelay_DSTATE_n_ml_P_cam_ISS[2]; - memcpy(&rtb_Merge2.of_quat_ISS2cam[0], &UnitDelay_DSTATE_n_of_quat_ISS2[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&rtb_Merge2.of_P_cam_ISS_ISS[0], &UnitDelay_DSTATE_n_of_P_cam_ISS[0], - (uint32_T)(48U * sizeof(real32_T))); - memcpy(&rtb_Merge2.cov_diag[0], &UnitDelay_DSTATE_n_cov_diag[0], (uint32_T) - (117U * sizeof(ase_cov_datatype))); - rtb_Merge2.kfl_status = UnitDelay_DSTATE_n_kfl_status; - rtb_Merge2.update_OF_tracks_cnt = rtb_Switch_m; - rtb_Merge2.update_ML_features_cnt = numFeatures; - memcpy(&rtb_Merge2.of_mahal_distance[0], &UnitDelay_DSTATE_n_of_mahal_dis[0], - (uint32_T)(50U * sizeof(real_T))); - memcpy(&rtb_Merge2.ml_mahal_distance[0], &rtb_UnitDelay18[0], (uint32_T)(50U - * sizeof(real_T))); - rtb_Merge2.hr_P_hr_ISS_ISS[0] = hr_P_hr_ISS_ISS[0]; - rtb_Merge2.hr_P_hr_ISS_ISS[1] = hr_P_hr_ISS_ISS[1]; - rtb_Merge2.hr_P_hr_ISS_ISS[2] = hr_P_hr_ISS_ISS[2]; - rtb_Merge2.hr_quat_ISS2hr[0] = hr_quat_ISS2hr_idx_0; - rtb_Merge2.hr_quat_ISS2hr[1] = hr_quat_ISS2hr_idx_1; - rtb_Merge2.hr_quat_ISS2hr[2] = hr_quat_ISS2hr_idx_2; - rtb_Merge2.hr_quat_ISS2hr[3] = hr_quat_ISS2hr_idx_3; - rtb_Merge2.P_EST_ISS_ISS[0] = UnitDelay_DSTATE_n_P_EST_ISS_IS[0]; - rtb_Merge2.P_EST_ISS_ISS[1] = UnitDelay_DSTATE_n_P_EST_ISS_IS[1]; - rtb_Merge2.P_EST_ISS_ISS[2] = UnitDelay_DSTATE_n_P_EST_ISS_IS[2]; - - // End of Outputs for SubSystem: '/ML Update' - // End of Outputs for SubSystem: '/Absolute_Update' - break; - - case 1: - // Outputs for IfAction SubSystem: '/Optical_Flow_Update' incorporates: - // ActionPort: '/Action Port' - - // Outputs for Iterator SubSystem: '/OF Update' incorporates: - // WhileIterator: '/While Iterator' - - // InitializeConditions for UnitDelay: '/Unit Delay' incorporates: - // UnitDelay: '/Unit Delay' - - rtb_Merge_o[0] = est_estimator_P->UnitDelay_InitialCondition_p.quat_ISS2B[0]; - rtb_Merge_o[1] = est_estimator_P->UnitDelay_InitialCondition_p.quat_ISS2B[1]; - rtb_Merge_o[2] = est_estimator_P->UnitDelay_InitialCondition_p.quat_ISS2B[2]; - rtb_Merge_o[3] = est_estimator_P->UnitDelay_InitialCondition_p.quat_ISS2B[3]; - rtb_ImpAsg_InsertedFor_Out1_at_[0] = - est_estimator_P->UnitDelay_InitialCondition_p.omega_B_ISS_B[0]; - accel[0] = est_estimator_P->UnitDelay_InitialCondition_p.gyro_bias[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = - est_estimator_P->UnitDelay_InitialCondition_p.V_B_ISS_ISS[0]; - rtb_Sum_k1 = est_estimator_P->UnitDelay_InitialCondition_p.A_B_ISS_ISS[0]; - UnitDelay_DSTATE_n_accel_bias[0] = - est_estimator_P->UnitDelay_InitialCondition_p.accel_bias[0]; - rtb_Sum_l = est_estimator_P->UnitDelay_InitialCondition_p.P_B_ISS_ISS[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] = - est_estimator_P->UnitDelay_InitialCondition_p.omega_B_ISS_B[1]; - accel[1] = est_estimator_P->UnitDelay_InitialCondition_p.gyro_bias[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = - est_estimator_P->UnitDelay_InitialCondition_p.V_B_ISS_ISS[1]; - rtb_Gain1_f = est_estimator_P->UnitDelay_InitialCondition_p.A_B_ISS_ISS[1]; - UnitDelay_DSTATE_n_accel_bias[1] = - est_estimator_P->UnitDelay_InitialCondition_p.accel_bias[1]; - UnitDelay_DSTATE_n_P_B_ISS_ISS_ = - est_estimator_P->UnitDelay_InitialCondition_p.P_B_ISS_ISS[1]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = - est_estimator_P->UnitDelay_InitialCondition_p.omega_B_ISS_B[2]; - accel[2] = est_estimator_P->UnitDelay_InitialCondition_p.gyro_bias[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = - est_estimator_P->UnitDelay_InitialCondition_p.V_B_ISS_ISS[2]; - UnitDelay_DSTATE_n_A_B_ISS_ISS_ = - est_estimator_P->UnitDelay_InitialCondition_p.A_B_ISS_ISS[2]; - UnitDelay_DSTATE_n_accel_bias[2] = - est_estimator_P->UnitDelay_InitialCondition_p.accel_bias[2]; - UnitDelay_DSTATE_n_P_B_ISS_IS_0 = - est_estimator_P->UnitDelay_InitialCondition_p.P_B_ISS_ISS[2]; - rtb_Switch_m = est_estimator_P->UnitDelay_InitialCondition_p.confidence; - UnitDelay_DSTATE_aug_state_enum = - est_estimator_P->UnitDelay_InitialCondition_p.aug_state_enum; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_quat_ISS2cam[0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_quat_ISS2cam[1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_quat_ISS2cam[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_quat_ISS2cam[3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_P_cam_ISS_ISS[0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_P_cam_ISS_ISS[1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_P_cam_ISS_ISS[2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &est_estimator_P->UnitDelay_InitialCondition_p.of_quat_ISS2cam[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &est_estimator_P->UnitDelay_InitialCondition_p.of_P_cam_ISS_ISS[0], - (uint32_T)(48U * sizeof(real32_T))); - memcpy(&UnitDelay_DSTATE_n_cov_diag[0], - &est_estimator_P->UnitDelay_InitialCondition_p.cov_diag[0], (uint32_T) - (117U * sizeof(ase_cov_datatype))); - UnitDelay_DSTATE_n_kfl_status = - est_estimator_P->UnitDelay_InitialCondition_p.kfl_status; - rtb_numFeatures_f = - est_estimator_P->UnitDelay_InitialCondition_p.update_OF_tracks_cnt; - rtb_Saturation_n = - est_estimator_P->UnitDelay_InitialCondition_p.update_ML_features_cnt; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], - &est_estimator_P->UnitDelay_InitialCondition_p.of_mahal_distance[0], - (uint32_T)(50U * sizeof(real_T))); - memcpy(&UnitDelay_DSTATE_n_ml_mahal_dis[0], - &est_estimator_P->UnitDelay_InitialCondition_p.ml_mahal_distance[0], - (uint32_T)(50U * sizeof(real_T))); - UnitDelay_DSTATE_n_hr_P_hr_ISS_ = - est_estimator_P->UnitDelay_InitialCondition_p.hr_P_hr_ISS_ISS[0]; - UnitDelay_DSTATE_n_hr_P_hr_IS_0 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_P_hr_ISS_ISS[1]; - UnitDelay_DSTATE_n_hr_P_hr_IS_1 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_P_hr_ISS_ISS[2]; - UnitDelay_DSTATE_n_hr_quat_ISS2 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_quat_ISS2hr[0]; - UnitDelay_DSTATE_n_hr_quat_IS_0 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_quat_ISS2hr[1]; - UnitDelay_DSTATE_n_hr_quat_IS_1 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_quat_ISS2hr[2]; - UnitDelay_DSTATE_n_hr_quat_IS_2 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_quat_ISS2hr[3]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = - est_estimator_P->UnitDelay_InitialCondition_p.P_EST_ISS_ISS[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = - est_estimator_P->UnitDelay_InitialCondition_p.P_EST_ISS_ISS[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = - est_estimator_P->UnitDelay_InitialCondition_p.P_EST_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay1' incorporates: - // UnitDelay: '/Unit Delay1' - - for (i = 0; i < 13689; i++) { - est_estimator_B->Switch1[i] = - est_estimator_P->UnitDelay1_InitialCondition_m; - } - - // End of InitializeConditions for UnitDelay: '/Unit Delay1' - - // Switch: '/Switch2' incorporates: - // BusAssignment: '/Bus Assignment' - // BusAssignment: '/Bus Assignment' - // BusCreator: '/Bus Creator2' - // UnitDelay: '/Unit Delay10' - // UnitDelay: '/Unit Delay11' - // UnitDelay: '/Unit Delay12' - // UnitDelay: '/Unit Delay13' - // UnitDelay: '/Unit Delay14' - // UnitDelay: '/Unit Delay15' - // UnitDelay: '/Unit Delay3' - // UnitDelay: '/Unit Delay6' - // UnitDelay: '/Unit Delay7' - // UnitDelay: '/Unit Delay8' - - if (!(1 > est_estimator_P->Switch2_Threshold_b)) { - rtb_Merge_o[0] = rtb_Product1[0]; - rtb_Merge_o[1] = rtb_Product1[1]; - rtb_Merge_o[2] = rtb_Product1[2]; - rtb_Merge_o[3] = rtb_Product1[3]; - rtb_ImpAsg_InsertedFor_Out1_at_[0] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - accel[0] = est_estimator_DW->UnitDelay3_DSTATE[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = rtb_Sum2[0]; - rtb_Sum_k1 = rtb_Sum1_k3[0]; - UnitDelay_DSTATE_n_accel_bias[0] = est_estimator_DW->UnitDelay6_DSTATE[0]; - rtb_Sum_l = est_estimator_DW->UnitDelay7_DSTATE[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - accel[1] = est_estimator_DW->UnitDelay3_DSTATE[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = rtb_Sum2[1]; - rtb_Gain1_f = rtb_Sum1_k3[1]; - UnitDelay_DSTATE_n_accel_bias[1] = est_estimator_DW->UnitDelay6_DSTATE[1]; - UnitDelay_DSTATE_n_P_B_ISS_ISS_ = est_estimator_DW->UnitDelay7_DSTATE[1]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = hr_quat_ISS2hr_idx_0; - accel[2] = est_estimator_DW->UnitDelay3_DSTATE[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = rtb_Sum2[2]; - UnitDelay_DSTATE_n_A_B_ISS_ISS_ = rtb_Sum1_k3[2]; - UnitDelay_DSTATE_n_accel_bias[2] = est_estimator_DW->UnitDelay6_DSTATE[2]; - UnitDelay_DSTATE_n_P_B_ISS_IS_0 = est_estimator_DW->UnitDelay7_DSTATE[2]; - rtb_Switch_m = est_estimator_DW->UnitDelay8_DSTATE; - UnitDelay_DSTATE_aug_state_enum = rtb_BusAssignment_aug_state_enu; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = est_estimator_DW->UnitDelay10_DSTATE - [0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = est_estimator_DW->UnitDelay10_DSTATE - [1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = est_estimator_DW->UnitDelay10_DSTATE - [2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = est_estimator_DW->UnitDelay10_DSTATE - [3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = est_estimator_DW->UnitDelay11_DSTATE - [0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = est_estimator_DW->UnitDelay11_DSTATE - [1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = est_estimator_DW->UnitDelay11_DSTATE - [2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &est_estimator_DW->UnitDelay12_DSTATE[0], (uint32_T)(sizeof - (real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &est_estimator_DW->UnitDelay13_DSTATE[0], (uint32_T)(48U * sizeof - (real32_T))); - memcpy(&UnitDelay_DSTATE_n_cov_diag[0], - &est_estimator_DW->UnitDelay14_DSTATE[0], (uint32_T)(117U * sizeof - (ase_cov_datatype))); - UnitDelay_DSTATE_n_kfl_status = est_estimator_DW->UnitDelay15_DSTATE; - rtb_numFeatures_f = numFeatures; - rtb_Saturation_n = rtb_BusCreator2_update_ML_featu; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], &rtb_UnitDelay18[0], (uint32_T) - (50U * sizeof(real_T))); - memcpy(&UnitDelay_DSTATE_n_ml_mahal_dis[0], &rtb_UnitDelay19[0], (uint32_T) - (50U * sizeof(real_T))); - UnitDelay_DSTATE_n_hr_P_hr_ISS_ = rtb_Add_e[0]; - UnitDelay_DSTATE_n_hr_P_hr_IS_0 = rtb_Add_e[1]; - UnitDelay_DSTATE_n_hr_P_hr_IS_1 = rtb_Add_e[2]; - UnitDelay_DSTATE_n_hr_quat_ISS2 = rtb_UnitDelay25[0]; - UnitDelay_DSTATE_n_hr_quat_IS_0 = rtb_UnitDelay25[1]; - UnitDelay_DSTATE_n_hr_quat_IS_1 = rtb_UnitDelay25[2]; - UnitDelay_DSTATE_n_hr_quat_IS_2 = rtb_UnitDelay25[3]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = rtb_P_B_ISS_ISS[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = rtb_P_B_ISS_ISS[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = rtb_P_B_ISS_ISS[2]; - } - - // End of Switch: '/Switch2' - - // MATLAB Function: '/compute_of_global_points' incorporates: - // Inport: '/optical_flow_msg' - - // MATLAB Function 'camera_update/Optical_Flow_Update/OF Update/compute_of_global_points': ':1' - // Code is taken from Zack Morrato's prototyped Optical Flow Kalman Filter. - // This code computes the residual and measurment matrices for the optical - // flow measurments. - // - // ':1:8' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Code is taken from Zack Morrato's prototyped Optical Flow Kalman Filter. - // This code computes the residual and measurment matrices for the optical - // flow measurments. - // for some reason matlab doesn't like creating matrix sizes based on "unbounded" parameters - // if ase_of_num_aug ~= 5 - // error('eml_compute_of_global_points is optimized for four augmented states.'); - // end - // ase_of_num_aug = 4; - memset(&est_estimator_B->of_measured[0], 0, (uint32_T)(1600U * sizeof - (real32_T))); - for (i = 0; i < 200; i++) { - rtb_global_points[i] = 1.0F; - } - - memset(&rtb_valid_out[0], 0, (uint32_T)(800U * sizeof(int32_T))); - for (i = 0; i < 1600; i++) { - est_estimator_B->of_measured_in[i] = - est_estimator_U_cvs_optical_flow_msg_n->cvs_observations[i] * - est_estimator_P->tun_ase_vocam_inv_focal_length; - } - - // Convert measurements to normalized units - // Note for some reason of_measured and valid can not match? Why? - // Select out only features that are seen by three or more cameras - fkngdjekgdjepphl_sum(est_estimator_U_cvs_optical_flow_msg_n->cvs_valid_flag, - rtb_UnitDelay18); - br = 0; - for (nx = 0; nx < 50; nx++) { - rtb_Compare = (rtb_UnitDelay18[nx] >= 3.0); - if (rtb_Compare) { - br++; - } - - rtb_Compare_ic[nx] = rtb_Compare; - } - - handrail_knowledge_dims = br; - num_original = 0; - for (br = 0; br < 50; br++) { - if (rtb_Compare_ic[br]) { - num_original++; - } - } - - C_sizes_idx_1 = num_original; - num_original = 0; - for (br = 0; br < 50; br++) { - if (rtb_Compare_ic[br]) { - e_data[num_original] = (int8_T)(int32_T)(br + 1); - num_original++; - } - } - - for (i = 0; i < 16; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - rtb_valid_out[(int32_T)(b_m + (int32_T)(50 * i))] = (int32_T) - est_estimator_U_cvs_optical_flow_msg_n->cvs_valid_flag[(int32_T) - ((int32_T)((int32_T)(50 * i) + (int32_T)e_data[b_m]) - 1)]; - } - } - - // reshape the measurements so the order is by feature, x measure then y - // measure, with every camera observation one after the other. - num_original = (int32_T)(handrail_knowledge_dims << 5); - nx = 0; - for (br = 0; br < 50; br++) { - if (rtb_Compare_ic[br]) { - nx++; - } - } - - C_sizes_idx_1 = nx; - nx = 0; - for (br = 0; br < 50; br++) { - if (rtb_Compare_ic[br]) { - e_data[nx] = (int8_T)(int32_T)(br + 1); - nx++; - } - } - - of_measured_in_sizes[0] = C_sizes_idx_1; - of_measured_in_sizes[1] = 2; - of_measured_in_sizes[2] = 16; - for (i = 0; i < 16; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - est_estimator_B->of_measured_in_data[(int32_T)(b_m + (int32_T)((int32_T) - (C_sizes_idx_1 << 1) * i))] = est_estimator_B->of_measured_in[(int32_T) - ((int32_T)((int32_T)(100 * i) + (int32_T)e_data[b_m]) - 1)]; - } - - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - est_estimator_B->of_measured_in_data[(int32_T)((int32_T)(b_m + - C_sizes_idx_1) + (int32_T)((int32_T)(C_sizes_idx_1 << 1) * i))] = - est_estimator_B->of_measured_in[(int32_T)((int32_T)((int32_T)(100 * i) - + (int32_T)e_data[b_m]) + 49)]; - } - } - - aaiepphlecjelnoh_permute(est_estimator_B->of_measured_in_data, - of_measured_in_sizes, est_estimator_B->of_measured_in, x_sizes); - nx = (int32_T)(x_sizes[1] << 5); - tmp_h = (int16_T)(int32_T)(handrail_knowledge_dims << 4); - ar = (int32_T)(int16_T)(int32_T)(handrail_knowledge_dims << 4); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - est_estimator_B->of_measured_in_data[br] = est_estimator_B-> - of_measured_in[br]; - } - - for (i = 0; i <= (int32_T)(ar - 1); i++) { - est_estimator_B->b_x_data[(int32_T)(i << 1)] = - est_estimator_B->of_measured_in_data[i]; - est_estimator_B->b_x_data[(int32_T)(1 + (int32_T)(i << 1))] = - est_estimator_B->of_measured_in_data[(int32_T)(i + (int32_T)tmp_h)]; - } - - nx = (int32_T)((int32_T)tmp_h << 1); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - est_estimator_B->of_measured_in_data[br] = est_estimator_B->b_x_data[br]; - } - - if (1 > num_original) { - num_original = 0; - } - - tmp_h = (int16_T)(int32_T)((int32_T)(int16_T)num_original - 1); - br = (int32_T)((int32_T)tmp_h + 1); - C_sizes_idx_1 = (int32_T)tmp_h; - for (i = 0; i <= C_sizes_idx_1; i++) { - h_data[i] = (int16_T)i; - } - - for (i = 0; i <= (int32_T)(br - 1); i++) { - est_estimator_B->of_measured[(int32_T)h_data[i]] = - est_estimator_B->of_measured_in_data[i]; - } - - // Extract the 4x4 transforms which convert points from global frame to camera - // frame. I'll be stacking them vertically so they can all be evaluated - // simultaneously. - imohknglphlfpphd_repmat(rtb_camera_tf_global); - for (num_original = 0; num_original < 16; num_original++) { - br = (int32_T)((int32_T)((int32_T)(1 + num_original) << 2) - 3); - nx = (int32_T)((int32_T)((int32_T)(1 + num_original) << 2) - 1); - if (nx < br) { - z_est_sizes_idx_1 = 0; - } else { - z_est_sizes_idx_1 = (int32_T)((int32_T)(nx - br) + 1); - C_sizes_idx_1 = (int32_T)(nx - br); - for (i = 0; i <= C_sizes_idx_1; i++) { - rot_indices_data[i] = (int8_T)(int32_T)(br + i); - } - } - - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - c_data_1[i] = (int8_T)(int32_T)((int32_T)rot_indices_data[i] - 1); - } - - H[0] = UnitDelay_DSTATE_n_of_quat_ISS2[num_original]; - H[1] = UnitDelay_DSTATE_n_of_quat_ISS2[(int32_T)(num_original + 16)]; - H[2] = UnitDelay_DSTATE_n_of_quat_ISS2[(int32_T)(num_original + 32)]; - H[3] = UnitDelay_DSTATE_n_of_quat_ISS2[(int32_T)(num_original + 48)]; - fkfcbaiengdjgdje_quaternion_to_rotation(H, rtb_Product1_0); - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(z_est_sizes_idx_1 - 1); b_m++) { - rtb_camera_tf_global[(int32_T)((int32_T)c_data_1[b_m] + (int32_T)(i << - 6))] = rtb_Product1_0[(int32_T)((int32_T)(z_est_sizes_idx_1 * i) + - b_m)]; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(z_est_sizes_idx_1 - 1); b_m++) { - c_a_data[(int32_T)(b_m + (int32_T)(z_est_sizes_idx_1 * i))] = - -rtb_camera_tf_global[(int32_T)((int32_T)((int32_T)(i << 6) + - (int32_T)rot_indices_data[b_m]) - 1)]; - } - } - - br = (int32_T)(int8_T)z_est_sizes_idx_1; - b_m = (int32_T)(int8_T)z_est_sizes_idx_1; - for (i = 0; i <= (int32_T)(br - 1); i++) { - C_data[i] = 0.0F; - } - - if (z_est_sizes_idx_1 != 0) { - br = 0; - while ((z_est_sizes_idx_1 > 0) && (br <= 0)) { - for (nx = 1; nx <= z_est_sizes_idx_1; nx++) { - C_data[(int32_T)(nx - 1)] = 0.0F; - } - - br = z_est_sizes_idx_1; - } - - br = 3; - nx = 0; - while ((z_est_sizes_idx_1 > 0) && (nx <= 0)) { - ar = 0; - for (i = (int32_T)(br - 3); (int32_T)(i + 1) <= br; i++) { - if (UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)((int32_T)(i << 4) + - num_original)] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = 0; (int32_T)(O_sizes_idx_0 + 1) <= - z_est_sizes_idx_1; O_sizes_idx_0++) { - s12_iter++; - C_data[O_sizes_idx_0] += UnitDelay_DSTATE_n_of_P_cam_ISS - [(int32_T)((int32_T)(i << 4) + num_original)] * c_a_data - [(int32_T)(s12_iter - 1)]; - } - } - - ar += z_est_sizes_idx_1; - } - - br += 3; - nx = z_est_sizes_idx_1; - } - } - - for (i = 0; i <= (int32_T)(b_m - 1); i++) { - rtb_camera_tf_global[(int32_T)((int32_T)rot_indices_data[i] + 191)] = - C_data[i]; - } - } - - // Generate the transforms from camera 1 into all the other cameras. This is - // used for point triangulation. - hdbihlngknopkfcj_repmat(camera_tf_camera1); - for (i = 0; i < 4; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = rtb_camera_tf_global[(int32_T) - (i << 6)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)(i << 6) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)(i << 6) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(i << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)(i << 6) + 3)]; - } - - biecdbieglngohlf_pinv(rtb_VectorConcatenate, global_tf_camera1); - - // Used to be inv - for (num_original = 0; num_original < 16; num_original++) { - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 2) - 4); - for (b_m = 0; b_m < 4; b_m++) { - rtb_VectorConcatenate[(int32_T)(b_m << 2)] = rtb_camera_tf_global - [(int32_T)((int32_T)(b_m << 6) + i)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(b_m << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)((int32_T)(b_m << 6) + i) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(b_m << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)((int32_T)(b_m << 6) + i) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(b_m << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)((int32_T)(b_m << 6) + i) + 3)]; - } - - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 2) - 4); - for (b_m = 0; b_m < 4; b_m++) { - for (i_0 = 0; i_0 < 4; i_0++) { - camera_tf_camera1[(int32_T)((int32_T)(b_m + i) + (int32_T)(i_0 << 6))] - = (real_T)(((global_tf_camera1[(int32_T)((int32_T)(i_0 << 2) + 1)] * - rtb_VectorConcatenate[(int32_T)(b_m + 4)] + - global_tf_camera1[(int32_T)(i_0 << 2)] * - rtb_VectorConcatenate[b_m]) + global_tf_camera1 - [(int32_T)((int32_T)(i_0 << 2) + 2)] * - rtb_VectorConcatenate[(int32_T)(b_m + 8)]) + - global_tf_camera1[(int32_T)((int32_T)(i_0 << 2) + 3)] * - rtb_VectorConcatenate[(int32_T)(b_m + 12)]); - } - } - } - - // Triangulate all of the points - // for use interanlly, allocate outside for speed - memset(&A[0], 0, (uint32_T)(96U * sizeof(real32_T))); - memset(&b_1[0], 0, (uint32_T)(sizeof(real32_T) << 5U)); - for (num_original = 0; (int32_T)(num_original + 1) <= - handrail_knowledge_dims; num_original++) { - // Solving for location of the point in the 3D world - // Forming A and b matrices, used in a Ax=b to solve for alpha, beta, rho, - // an inverse depth parameterization of the points location in the 1st - // camera. - for (i = 0; i < 16; i++) { - rtb_valid_out_0[i] = rtb_valid_out[(int32_T)((int32_T)(50 * i) + - num_original)]; - } - - num_augs = baaanophjmgddbie_sum(rtb_valid_out_0); - aug_ind = 1.0; - for (nx = 0; nx <= (int32_T)((int32_T)num_augs - 1); nx++) { - while (!(rtb_valid_out[(int32_T)((int32_T)((int32_T)((int32_T)aug_ind - - 1) * 50) + num_original)] != 0)) { - aug_ind++; - } - - ind0 = 4.0 * aug_ind - 4.0; - tmp_9 = rt_roundd_snf(((real_T)(int32_T)(num_original + 1) - 1.0) * 2.0 * - 16.0 + 2.0 * aug_ind); - if (tmp_9 < 2.147483648E+9) { - if (tmp_9 >= -2.147483648E+9) { - i = (int32_T)tmp_9; - } else { - i = MIN_int32_T; - } - } else { - i = MAX_int32_T; - } - - br = (int32_T)(i - 2); - tmp_9 = (1.0 + (real_T)nx) * 2.0; - A[(int32_T)((int32_T)(tmp_9 + -1.0) - 1)] = (real32_T)camera_tf_camera1 - [(int32_T)((int32_T)(ind0 + 1.0) - 1)] - (real32_T)camera_tf_camera1 - [(int32_T)((int32_T)(ind0 + 3.0) - 1)] * est_estimator_B-> - of_measured[br]; - A[(int32_T)((int32_T)(tmp_9 + -1.0) + 31)] = (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 1.0) + 63)] - (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 3.0) + 63)] * - est_estimator_B->of_measured[br]; - A[(int32_T)((int32_T)(tmp_9 + -1.0) + 63)] = (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 1.0) + 191)] - (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 3.0) + 191)] * - est_estimator_B->of_measured[br]; - A[(int32_T)((int32_T)tmp_9 - 1)] = (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 2.0) - 1)] - (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 3.0) - 1)] * est_estimator_B->of_measured[(int32_T) - (1 + br)]; - A[(int32_T)((int32_T)tmp_9 + 31)] = (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 2.0) + 63)] - (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 3.0) + 63)] * est_estimator_B->of_measured[(int32_T) - (1 + br)]; - A[(int32_T)((int32_T)tmp_9 + 63)] = (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 2.0) + 191)] - (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 3.0) + 191)] * est_estimator_B->of_measured[(int32_T) - (1 + br)]; - tmp_9 = (1.0 + (real_T)nx) * 2.0; - b_1[(int32_T)((int32_T)(tmp_9 + -1.0) - 1)] = (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 3.0) + 127)] * - est_estimator_B->of_measured[br] - (real32_T)camera_tf_camera1 - [(int32_T)((int32_T)(ind0 + 1.0) + 127)]; - b_1[(int32_T)((int32_T)tmp_9 - 1)] = (real32_T)camera_tf_camera1 - [(int32_T)((int32_T)(ind0 + 3.0) + 127)] * - est_estimator_B->of_measured[(int32_T)(1 + br)] - (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 2.0) + 127)]; - aug_ind++; - } - - // let's weight the oldest augmentation more heavily - tmp_9 = 2.0 * num_augs; - aug_ind = 2.0 * num_augs; - for (i = 0; i < 3; i++) { - rtb_Product_j[(int32_T)(i << 1)] = A[(int32_T)((int32_T)((int32_T)(tmp_9 - + -1.0) + (int32_T)(i << 5)) - 1)] * 5.0F; - rtb_Product_j[(int32_T)(1 + (int32_T)(i << 1))] = A[(int32_T)((int32_T) - ((int32_T)(i << 5) + (int32_T)tmp_9) - 1)] * 5.0F; - } - - for (i = 0; i < 3; i++) { - A[(int32_T)((int32_T)((int32_T)(aug_ind + -1.0) + (int32_T)(i << 5)) - 1)] - = rtb_Product_j[(int32_T)(i << 1)]; - A[(int32_T)((int32_T)((int32_T)aug_ind + (int32_T)(i << 5)) - 1)] = - rtb_Product_j[(int32_T)((int32_T)(i << 1) + 1)]; - } - - tmp_9 = 2.0 * num_augs; - aug_ind = 2.0 * num_augs; - hr_quat_ISS2hr_idx_0 = b_1[(int32_T)((int32_T)tmp_9 - 1)] * 5.0F; - b_1[(int32_T)((int32_T)(aug_ind + -1.0) - 1)] = b_1[(int32_T)((int32_T) - (tmp_9 + -1.0) - 1)] * 5.0F; - b_1[(int32_T)((int32_T)aug_ind - 1)] = hr_quat_ISS2hr_idx_0; - aug_ind = num_augs * 2.0; - num_augs *= 2.0; - if (1.0 > num_augs) { - br = 0; - } else { - br = (int32_T)num_augs; - } - - if (1.0 > aug_ind) { - C_sizes_idx_1 = 0; - } else { - C_sizes_idx_1 = (int32_T)aug_ind; - } - - q1_sizes[0] = C_sizes_idx_1; - q1_sizes[1] = 3; - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - A_data[(int32_T)(b_m + (int32_T)(C_sizes_idx_1 * i))] = A[(int32_T) - ((int32_T)(i << 5) + b_m)]; - } - } - - glfcngdjgdjmmglf_pinv(A_data, q1_sizes, a_data, T_H_sizes); - if ((T_H_sizes[1] == 1) || (br == 1)) { - for (i = 0; i <= (int32_T)(br - 1); i++) { - b_data_0[i] = b_1[i]; - } - - for (i = 0; i < 3; i++) { - hr_P_hr_ISS_ISS[i] = 0.0F; - C_sizes_idx_1 = T_H_sizes[1]; - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - hr_P_hr_ISS_ISS[i] += a_data[(int32_T)((int32_T)(T_H_sizes[0] * b_m) - + i)] * b_data_0[b_m]; - } - } - } else { - hr_P_hr_ISS_ISS[0] = 0.0F; - hr_P_hr_ISS_ISS[1] = 0.0F; - hr_P_hr_ISS_ISS[2] = 0.0F; - ar = 0; - for (i = 0; (int32_T)(i + 1) <= T_H_sizes[1]; i++) { - if (b_1[i] != 0.0F) { - s12_iter = (int32_T)(ar + 1); - hr_P_hr_ISS_ISS[0] += a_data[(int32_T)(s12_iter - 1)] * b_1[i]; - s12_iter++; - hr_P_hr_ISS_ISS[1] += a_data[(int32_T)(s12_iter - 1)] * b_1[i]; - s12_iter++; - hr_P_hr_ISS_ISS[2] += a_data[(int32_T)(s12_iter - 1)] * b_1[i]; - } - - ar += 3; - } - } - - // solve Ax = B with guass newton - if ((real_T)hr_P_hr_ISS_ISS[2] < 1.0E-5) { - // This is a point that is truly in our face because we have no - // parallax on it. I only see this happen when using perfect camera - // positions and the robot is stationary (our noise usually makes it so - // that inverse depth is non zero). - hr_P_hr_ISS_ISS[2] = 1.0E-5F; - } - - // Converting the inverse depth parameterization into the global coordinate - // frame. - hr_quat_ISS2hr_idx_0 = hr_P_hr_ISS_ISS[0] / hr_P_hr_ISS_ISS[2]; - rtb_Switch1 = hr_P_hr_ISS_ISS[1] / hr_P_hr_ISS_ISS[2]; - hr_quat_ISS2hr_idx_1 = 1.0F / hr_P_hr_ISS_ISS[2]; - for (i = 0; i < 3; i++) { - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] = 0.0F; - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] += - global_tf_camera1[i] * hr_quat_ISS2hr_idx_0; - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] += - global_tf_camera1[(int32_T)(i + 4)] * rtb_Switch1; - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] += - global_tf_camera1[(int32_T)(i + 8)] * hr_quat_ISS2hr_idx_1; - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] += - global_tf_camera1[(int32_T)(i + 12)]; - } - } - - // End of MATLAB Function: '/compute_of_global_points' - - // Switch: '/Switch3' - // ':1:8' - rtb_Compare = (1 > est_estimator_P->Switch3_Threshold_d); - for (i = 0; i < 13689; i++) { - rtb_Switch1 = est_estimator_B->Switch1[i]; - if (!rtb_Compare) { - rtb_Switch1 = est_estimator_B->Assignment[i]; - } - - est_estimator_B->Switch1[i] = rtb_Switch1; - } - - // End of Switch: '/Switch3' - - // S-Function (ex_of_residual_and_h): '/ex_of_residual_and_h' - rtb_ex_of_residual_and_h_o1 = of_residual_and_h((real32_T*) - &est_estimator_B->of_measured[0], (real32_T*)&rtb_global_points[0], - (real32_T*)&rtb_camera_tf_global[0], (int32_T*)&rtb_valid_out[0], - handrail_knowledge_dims, ASE_OF_NUM_AUG, ASE_OF_NUM_FEATURES, - est_estimator_P->tun_ase_mahal_distance_max, - est_estimator_P->tun_ase_of_r_mag, - est_estimator_P->tun_ase_navcam_inv_focal_length, - est_estimator_P->tun_ase_navcam_distortion, (real32_T*) - &est_estimator_B->Switch1[0], &rtb_ex_of_residual_and_h_o2[0], - &est_estimator_B->ex_of_residual_and_h_o3[0], &rtb_ex_of_residual_and_h_o4, - &rtb_ex_of_residual_and_h_o5, &rtb_ex_of_residual_and_h_o6[0], - &est_estimator_B->ex_of_residual_and_h_o7[0]); - - // Product: '/Product' - for (i = 0; i < 96; i++) { - A[i] = rtb_ex_of_residual_and_h_o2[i] * rtb_ex_of_residual_and_h_o2[i]; - } - - // End of Product: '/Product' - - // Sum: '/Sum of Elements' - rtb_Switch1 = A[0]; - for (ar = 0; ar < 95; ar++) { - rtb_Switch1 += A[(int32_T)(ar + 1)]; - } - - // End of Sum: '/Sum of Elements' - - // Sqrt: '/Sqrt' - rtb_Switch1 = (real32_T)sqrt((real_T)rtb_Switch1); - - // S-Function (ex_compute_delta_state_and_cov): '/ex_compute_delta_state_and_cov' - rtb_ex_compute_delta_state_an_g = compute_delta_state_and_cov((real32_T*) - &rtb_ex_of_residual_and_h_o2[0], rtb_ex_of_residual_and_h_o1, (real32_T*) - &est_estimator_B->ex_of_residual_and_h_o3[0], 96, 117, - rtb_ex_of_residual_and_h_o4, (real32_T*) - &est_estimator_B->ex_of_residual_and_h_o7[0], (real32_T*) - &est_estimator_B->Switch1[0], &rtb_ex_compute_delta_state_and_[0], - &est_estimator_B->ex_compute_delta_state_an_e[0]); - - // S-Function (ex_apply_delta_state): '/ex_apply_delta_state' - apply_delta_state((real32_T*)&rtb_ex_compute_delta_state_and_[0], 117, - est_estimator_P->Constant_Value_c, (real32_T*) - &rtb_Merge_o[0], (real32_T*)&accel[0], (real32_T*) - &UnitDelay_DSTATE_n_V_B_ISS_ISS[0], (real32_T*) - &UnitDelay_DSTATE_n_accel_bias[0], (real32_T*) - &UnitDelay_DSTATE_n_P_EST_ISS_IS[0], (real32_T*) - &UnitDelay_DSTATE_n_ml_quat_ISS2[0], (real32_T*) - &UnitDelay_DSTATE_n_ml_P_cam_ISS[0], - UnitDelay_DSTATE_n_kfl_status, (real32_T*) - &UnitDelay_DSTATE_n_of_quat_ISS2[0], (real32_T*) - &UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &rtb_ex_apply_delta_state_o1[0], - &rtb_ex_apply_delta_state_o2[0], - &rtb_ex_apply_delta_state_o3[0], - &rtb_ex_apply_delta_state_o4[0], - &rtb_ex_apply_delta_state_o5[0], - &rtb_ex_apply_delta_state_o6[0], - &rtb_ex_apply_delta_state_o7[0], - &rtb_ex_apply_delta_state_o8, - &rtb_ex_apply_delta_state_o9[0], - &rtb_ex_apply_delta_state_o10[0]); - - // Logic: '/Logical Operator1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Product: '/Divide' - // RelationalOperator: '/Relational Operator1' - // Sum: '/Add' - // UnitDelay: '/Unit Delay2' - - rtb_Compare = ((est_estimator_P->ase_minumum_resid_thresh > (real_T) - ((est_estimator_P->UnitDelay2_InitialCondition_f - - rtb_Switch1) / rtb_Switch1)) || - (rtb_ex_compute_delta_state_an_g != 0) || - (rtb_ex_of_residual_and_h_o1 != 0)); - - // Switch: '/Switch' incorporates: - // BusAssignment: '/Bus Assignment1' - // BusAssignment: '/Bus Assignment1' - // Constant: '/Constant' - // Constant: '/Constant2' - // DataTypeConversion: '/Data Type Conversion' - - if (!rtb_Compare) { - rtb_Merge_o[0] = rtb_ex_apply_delta_state_o1[0]; - rtb_Merge_o[1] = rtb_ex_apply_delta_state_o1[1]; - rtb_Merge_o[2] = rtb_ex_apply_delta_state_o1[2]; - rtb_Merge_o[3] = rtb_ex_apply_delta_state_o1[3]; - accel[0] = rtb_ex_apply_delta_state_o2[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = rtb_ex_apply_delta_state_o3[0]; - UnitDelay_DSTATE_n_accel_bias[0] = rtb_ex_apply_delta_state_o4[0]; - accel[1] = rtb_ex_apply_delta_state_o2[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = rtb_ex_apply_delta_state_o3[1]; - UnitDelay_DSTATE_n_accel_bias[1] = rtb_ex_apply_delta_state_o4[1]; - accel[2] = rtb_ex_apply_delta_state_o2[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = rtb_ex_apply_delta_state_o3[2]; - UnitDelay_DSTATE_n_accel_bias[2] = rtb_ex_apply_delta_state_o4[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = rtb_ex_apply_delta_state_o6[0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = rtb_ex_apply_delta_state_o6[1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = rtb_ex_apply_delta_state_o6[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = rtb_ex_apply_delta_state_o6[3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = rtb_ex_apply_delta_state_o7[0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = rtb_ex_apply_delta_state_o7[1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = rtb_ex_apply_delta_state_o7[2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], &rtb_ex_apply_delta_state_o9[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], &rtb_ex_apply_delta_state_o10 - [0], (uint32_T)(48U * sizeof(real32_T))); - UnitDelay_DSTATE_n_kfl_status = rtb_ex_apply_delta_state_o8; - rtb_numFeatures_f = rtb_ex_of_residual_and_h_o5; - rtb_Saturation_n = est_estimator_P->Constant_Value_dd; - for (i = 0; i < 50; i++) { - UnitDelay_DSTATE_n_of_mahal_dis[i] = (real_T) - rtb_ex_of_residual_and_h_o6[i]; - UnitDelay_DSTATE_n_ml_mahal_dis[i] = est_estimator_P->Constant2_Value[i]; - } - - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = rtb_ex_apply_delta_state_o5[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = rtb_ex_apply_delta_state_o5[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = rtb_ex_apply_delta_state_o5[2]; - } - - // End of Switch: '/Switch' - - // Switch: '/Switch1' - for (i = 0; i < 13689; i++) { - rtb_Switch1 = est_estimator_B->Switch1[i]; - if (!rtb_Compare) { - rtb_Switch1 = est_estimator_B->ex_compute_delta_state_an_e[i]; - } - - est_estimator_B->Switch1[i] = rtb_Switch1; - } - - // End of Switch: '/Switch1' - - // SignalConversion: '/OutportBufferForstate_out' incorporates: - // SignalConversion: '/Signal Conversion' - - rtb_Merge2.quat_ISS2B[0] = rtb_Merge_o[0]; - rtb_Merge2.quat_ISS2B[1] = rtb_Merge_o[1]; - rtb_Merge2.quat_ISS2B[2] = rtb_Merge_o[2]; - rtb_Merge2.quat_ISS2B[3] = rtb_Merge_o[3]; - rtb_Merge2.omega_B_ISS_B[0] = rtb_ImpAsg_InsertedFor_Out1_at_[0]; - rtb_Merge2.gyro_bias[0] = accel[0]; - rtb_Merge2.V_B_ISS_ISS[0] = UnitDelay_DSTATE_n_V_B_ISS_ISS[0]; - rtb_Merge2.A_B_ISS_ISS[0] = rtb_Sum_k1; - rtb_Merge2.accel_bias[0] = UnitDelay_DSTATE_n_accel_bias[0]; - rtb_Merge2.P_B_ISS_ISS[0] = rtb_Sum_l; - rtb_Merge2.omega_B_ISS_B[1] = rtb_ImpAsg_InsertedFor_Out1_at_[1]; - rtb_Merge2.gyro_bias[1] = accel[1]; - rtb_Merge2.V_B_ISS_ISS[1] = UnitDelay_DSTATE_n_V_B_ISS_ISS[1]; - rtb_Merge2.A_B_ISS_ISS[1] = rtb_Gain1_f; - rtb_Merge2.accel_bias[1] = UnitDelay_DSTATE_n_accel_bias[1]; - rtb_Merge2.P_B_ISS_ISS[1] = UnitDelay_DSTATE_n_P_B_ISS_ISS_; - rtb_Merge2.omega_B_ISS_B[2] = rtb_ImpAsg_InsertedFor_Out1_at_[2]; - rtb_Merge2.gyro_bias[2] = accel[2]; - rtb_Merge2.V_B_ISS_ISS[2] = UnitDelay_DSTATE_n_V_B_ISS_ISS[2]; - rtb_Merge2.A_B_ISS_ISS[2] = UnitDelay_DSTATE_n_A_B_ISS_ISS_; - rtb_Merge2.accel_bias[2] = UnitDelay_DSTATE_n_accel_bias[2]; - rtb_Merge2.P_B_ISS_ISS[2] = UnitDelay_DSTATE_n_P_B_ISS_IS_0; - rtb_Merge2.confidence = rtb_Switch_m; - rtb_Merge2.aug_state_enum = UnitDelay_DSTATE_aug_state_enum; - rtb_Merge2.ml_quat_ISS2cam[0] = UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - rtb_Merge2.ml_quat_ISS2cam[1] = UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - rtb_Merge2.ml_quat_ISS2cam[2] = UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - rtb_Merge2.ml_quat_ISS2cam[3] = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - rtb_Merge2.ml_P_cam_ISS_ISS[0] = UnitDelay_DSTATE_n_ml_P_cam_ISS[0]; - rtb_Merge2.ml_P_cam_ISS_ISS[1] = UnitDelay_DSTATE_n_ml_P_cam_ISS[1]; - rtb_Merge2.ml_P_cam_ISS_ISS[2] = UnitDelay_DSTATE_n_ml_P_cam_ISS[2]; - memcpy(&rtb_Merge2.of_quat_ISS2cam[0], &UnitDelay_DSTATE_n_of_quat_ISS2[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&rtb_Merge2.of_P_cam_ISS_ISS[0], &UnitDelay_DSTATE_n_of_P_cam_ISS[0], - (uint32_T)(48U * sizeof(real32_T))); - memcpy(&rtb_Merge2.cov_diag[0], &UnitDelay_DSTATE_n_cov_diag[0], (uint32_T) - (117U * sizeof(ase_cov_datatype))); - rtb_Merge2.kfl_status = UnitDelay_DSTATE_n_kfl_status; - rtb_Merge2.update_OF_tracks_cnt = rtb_numFeatures_f; - rtb_Merge2.update_ML_features_cnt = rtb_Saturation_n; - memcpy(&rtb_Merge2.of_mahal_distance[0], &UnitDelay_DSTATE_n_of_mahal_dis[0], - (uint32_T)(50U * sizeof(real_T))); - memcpy(&rtb_Merge2.ml_mahal_distance[0], &UnitDelay_DSTATE_n_ml_mahal_dis[0], - (uint32_T)(50U * sizeof(real_T))); - rtb_Merge2.hr_P_hr_ISS_ISS[0] = UnitDelay_DSTATE_n_hr_P_hr_ISS_; - rtb_Merge2.hr_P_hr_ISS_ISS[1] = UnitDelay_DSTATE_n_hr_P_hr_IS_0; - rtb_Merge2.hr_P_hr_ISS_ISS[2] = UnitDelay_DSTATE_n_hr_P_hr_IS_1; - rtb_Merge2.hr_quat_ISS2hr[0] = UnitDelay_DSTATE_n_hr_quat_ISS2; - rtb_Merge2.hr_quat_ISS2hr[1] = UnitDelay_DSTATE_n_hr_quat_IS_0; - rtb_Merge2.hr_quat_ISS2hr[2] = UnitDelay_DSTATE_n_hr_quat_IS_1; - rtb_Merge2.hr_quat_ISS2hr[3] = UnitDelay_DSTATE_n_hr_quat_IS_2; - rtb_Merge2.P_EST_ISS_ISS[0] = UnitDelay_DSTATE_n_P_EST_ISS_IS[0]; - rtb_Merge2.P_EST_ISS_ISS[1] = UnitDelay_DSTATE_n_P_EST_ISS_IS[1]; - rtb_Merge2.P_EST_ISS_ISS[2] = UnitDelay_DSTATE_n_P_EST_ISS_IS[2]; - - // End of Outputs for SubSystem: '/OF Update' - // End of Outputs for SubSystem: '/Optical_Flow_Update' - break; - - case 2: - // Outputs for IfAction SubSystem: '/If Action Subsystem1' incorporates: - // ActionPort: '/Action Port' - - // SignalConversion: '/Signal Conversion' incorporates: - // BusAssignment: '/Bus Assignment' - // BusAssignment: '/Bus Assignment1' - // BusCreator: '/Bus Creator2' - // Constant: '/Constant' - // Constant: '/Constant1' - // S-Function (sfix_bitop): '/Bitwise Operator' - // UnitDelay: '/Unit Delay10' - // UnitDelay: '/Unit Delay11' - // UnitDelay: '/Unit Delay12' - // UnitDelay: '/Unit Delay13' - // UnitDelay: '/Unit Delay14' - // UnitDelay: '/Unit Delay15' - // UnitDelay: '/Unit Delay3' - // UnitDelay: '/Unit Delay6' - // UnitDelay: '/Unit Delay7' - // UnitDelay: '/Unit Delay8' - - rtb_Merge2.quat_ISS2B[0] = rtb_Product1[0]; - rtb_Merge2.quat_ISS2B[1] = rtb_Product1[1]; - rtb_Merge2.quat_ISS2B[2] = rtb_Product1[2]; - rtb_Merge2.quat_ISS2B[3] = rtb_Product1[3]; - rtb_Merge2.omega_B_ISS_B[0] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - rtb_Merge2.gyro_bias[0] = est_estimator_DW->UnitDelay3_DSTATE[0]; - rtb_Merge2.V_B_ISS_ISS[0] = rtb_Sum2[0]; - rtb_Merge2.A_B_ISS_ISS[0] = rtb_Sum1_k3[0]; - rtb_Merge2.accel_bias[0] = est_estimator_DW->UnitDelay6_DSTATE[0]; - rtb_Merge2.P_B_ISS_ISS[0] = est_estimator_DW->UnitDelay7_DSTATE[0]; - rtb_Merge2.omega_B_ISS_B[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - rtb_Merge2.gyro_bias[1] = est_estimator_DW->UnitDelay3_DSTATE[1]; - rtb_Merge2.V_B_ISS_ISS[1] = rtb_Sum2[1]; - rtb_Merge2.A_B_ISS_ISS[1] = rtb_Sum1_k3[1]; - rtb_Merge2.accel_bias[1] = est_estimator_DW->UnitDelay6_DSTATE[1]; - rtb_Merge2.P_B_ISS_ISS[1] = est_estimator_DW->UnitDelay7_DSTATE[1]; - rtb_Merge2.omega_B_ISS_B[2] = hr_quat_ISS2hr_idx_0; - rtb_Merge2.gyro_bias[2] = est_estimator_DW->UnitDelay3_DSTATE[2]; - rtb_Merge2.V_B_ISS_ISS[2] = rtb_Sum2[2]; - rtb_Merge2.A_B_ISS_ISS[2] = rtb_Sum1_k3[2]; - rtb_Merge2.accel_bias[2] = est_estimator_DW->UnitDelay6_DSTATE[2]; - rtb_Merge2.P_B_ISS_ISS[2] = est_estimator_DW->UnitDelay7_DSTATE[2]; - rtb_Merge2.confidence = est_estimator_DW->UnitDelay8_DSTATE; - rtb_Merge2.aug_state_enum = rtb_BusAssignment_aug_state_enu; - rtb_Merge2.ml_quat_ISS2cam[0] = est_estimator_DW->UnitDelay10_DSTATE[0]; - rtb_Merge2.ml_quat_ISS2cam[1] = est_estimator_DW->UnitDelay10_DSTATE[1]; - rtb_Merge2.ml_quat_ISS2cam[2] = est_estimator_DW->UnitDelay10_DSTATE[2]; - rtb_Merge2.ml_quat_ISS2cam[3] = est_estimator_DW->UnitDelay10_DSTATE[3]; - rtb_Merge2.ml_P_cam_ISS_ISS[0] = est_estimator_DW->UnitDelay11_DSTATE[0]; - rtb_Merge2.ml_P_cam_ISS_ISS[1] = est_estimator_DW->UnitDelay11_DSTATE[1]; - rtb_Merge2.ml_P_cam_ISS_ISS[2] = est_estimator_DW->UnitDelay11_DSTATE[2]; - memcpy(&rtb_Merge2.of_quat_ISS2cam[0], &est_estimator_DW-> - UnitDelay12_DSTATE[0], (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&rtb_Merge2.of_P_cam_ISS_ISS[0], - &est_estimator_DW->UnitDelay13_DSTATE[0], (uint32_T)(48U * sizeof - (real32_T))); - memcpy(&rtb_Merge2.cov_diag[0], &est_estimator_DW->UnitDelay14_DSTATE[0], - (uint32_T)(117U * sizeof(ase_cov_datatype))); - rtb_Merge2.kfl_status = (uint16_T)(int32_T)((int32_T) - est_estimator_DW->UnitDelay15_DSTATE & (int32_T) - est_estimator_P->BitwiseOperator_BitMask_c); - rtb_Merge2.update_OF_tracks_cnt = est_estimator_P->Constant_Value_oa; - rtb_Merge2.update_ML_features_cnt = est_estimator_P->Constant1_Value_f; - memcpy(&rtb_Merge2.of_mahal_distance[0], &rtb_UnitDelay18[0], (uint32_T)(50U - * sizeof(real_T))); - memcpy(&rtb_Merge2.ml_mahal_distance[0], &rtb_UnitDelay19[0], (uint32_T)(50U - * sizeof(real_T))); - rtb_Merge2.hr_P_hr_ISS_ISS[0] = rtb_Add_e[0]; - rtb_Merge2.hr_P_hr_ISS_ISS[1] = rtb_Add_e[1]; - rtb_Merge2.hr_P_hr_ISS_ISS[2] = rtb_Add_e[2]; - rtb_Merge2.hr_quat_ISS2hr[0] = rtb_UnitDelay25[0]; - rtb_Merge2.hr_quat_ISS2hr[1] = rtb_UnitDelay25[1]; - rtb_Merge2.hr_quat_ISS2hr[2] = rtb_UnitDelay25[2]; - rtb_Merge2.hr_quat_ISS2hr[3] = rtb_UnitDelay25[3]; - rtb_Merge2.P_EST_ISS_ISS[0] = rtb_P_B_ISS_ISS[0]; - rtb_Merge2.P_EST_ISS_ISS[1] = rtb_P_B_ISS_ISS[1]; - rtb_Merge2.P_EST_ISS_ISS[2] = rtb_P_B_ISS_ISS[2]; - memcpy(&est_estimator_B->Switch1[0], &est_estimator_B->Assignment[0], - (uint32_T)(13689U * sizeof(real32_T))); - - // End of Outputs for SubSystem: '/If Action Subsystem1' - break; - } - - // End of If: '/If' - - // If: '/If' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Inport: '/Vision Registration' - // Inport: '/cmc_msg' - // Inport: '/P_in' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // S-Function (sfix_bitop): '/Bitwise Operator1' - // SignalConversion: '/Signal Conversion' - - if (((est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_perching) && ((int32_T) - est_estimator_U_VisionRegistration->cvs_handrail_pulse != 0)) || - (((int32_T)est_estimator_U_VisionRegistration->cvs_landmark_pulse != 0) && - ((est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_map) || - (est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_docking)))) { - // Outputs for IfAction SubSystem: '/If Action Subsystem2' incorporates: - // ActionPort: '/Action Port' - - rtb_VectorConcatenate_c[0] = est_estimator_P->Constant3_Value_oe; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant3' - - rtb_VectorConcatenate_c[1] = est_estimator_P->Gain_Gain_d * - est_estimator_P->Constant5_Value[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[2] = est_estimator_P->Constant5_Value[1]; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[3] = est_estimator_P->Gain1_Gain_n * - est_estimator_P->Constant5_Value[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[4] = est_estimator_P->Constant5_Value[2]; - rtb_VectorConcatenate_c[5] = est_estimator_P->Constant2_Value_m2; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant2' - - rtb_VectorConcatenate_c[6] = est_estimator_P->Gain2_Gain_k * - est_estimator_P->Constant5_Value[0]; - - // Gain: '/Gain3' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[7] = est_estimator_P->Gain3_Gain_f * - est_estimator_P->Constant5_Value[1]; - - // Gain: '/Gain4' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[8] = est_estimator_P->Gain4_Gain_b * - est_estimator_P->Constant5_Value[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[9] = est_estimator_P->Constant5_Value[0]; - rtb_VectorConcatenate_c[10] = est_estimator_P->Constant1_Value_i; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant1' - - rtb_VectorConcatenate_c[11] = est_estimator_P->Gain5_Gain_f * - est_estimator_P->Constant5_Value[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[12] = est_estimator_P->Constant5_Value[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[13] = est_estimator_P->Constant5_Value[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[14] = est_estimator_P->Constant5_Value[2]; - rtb_VectorConcatenate_c[15] = est_estimator_P->Constant_Value_m; - rtb_VectorConcatenate_p[0] = est_estimator_P->Constant3_Value_op; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - - rtb_Add_e[0] = est_estimator_P->Gain1_Gain_o * rtb_Merge2.omega_B_ISS_B[0]; - rtb_Add_e[1] = est_estimator_P->Gain1_Gain_o * rtb_Merge2.omega_B_ISS_B[1]; - rtb_Add_e[2] = est_estimator_P->Gain1_Gain_o * rtb_Merge2.omega_B_ISS_B[2]; - - // Gain: '/Gain' - rtb_VectorConcatenate_p[1] = est_estimator_P->Gain_Gain_ng * rtb_Add_e[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' - rtb_VectorConcatenate_p[2] = rtb_Add_e[1]; - - // Gain: '/Gain1' - rtb_VectorConcatenate_p[3] = est_estimator_P->Gain1_Gain_a * rtb_Add_e[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' - rtb_VectorConcatenate_p[4] = rtb_Add_e[2]; - rtb_VectorConcatenate_p[5] = est_estimator_P->Constant2_Value_o; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant2' - - rtb_VectorConcatenate_p[6] = est_estimator_P->Gain2_Gain_b * rtb_Add_e[0]; - - // Gain: '/Gain3' - rtb_VectorConcatenate_p[7] = est_estimator_P->Gain3_Gain_o * rtb_Add_e[1]; - - // Gain: '/Gain4' - rtb_VectorConcatenate_p[8] = est_estimator_P->Gain4_Gain_i * rtb_Add_e[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' - rtb_VectorConcatenate_p[9] = rtb_Add_e[0]; - rtb_VectorConcatenate_p[10] = est_estimator_P->Constant1_Value_b; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant1' - - rtb_VectorConcatenate_p[11] = est_estimator_P->Gain5_Gain_c * rtb_Add_e[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' - rtb_VectorConcatenate_p[12] = rtb_Add_e[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' - rtb_VectorConcatenate_p[13] = rtb_Add_e[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' - rtb_VectorConcatenate_p[14] = rtb_Add_e[2]; - rtb_VectorConcatenate_p[15] = est_estimator_P->Constant_Value_px; - - // Product: '/Product2' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant3' - // Constant: '/Constant' - // Product: '/Product' - // Sum: '/Add' - - for (i = 0; i < 16; i++) { - rtb_VectorConcatenate[i] = (est_estimator_P->Constant3_Value_da * - rtb_VectorConcatenate_c[i] * - est_estimator_P->tun_ase_ml_forward_projection_time + - rtb_VectorConcatenate_p[i]) * est_estimator_P->Constant1_Value_a * - est_estimator_P->tun_ase_ml_forward_projection_time; - } - - // End of Product: '/Product2' - - // MATLAB Function: '/MATLAB Function' - est_estimator_MATLABFunction(rtb_VectorConcatenate, - &est_estimator_B->sf_MATLABFunction_l0); - - // Product: '/Product5' incorporates: - // Constant: '/Constant' - - hr_quat_ISS2hr_idx_1 = est_estimator_P->tun_ase_ml_forward_projection_time * - est_estimator_P->tun_ase_ml_forward_projection_time * - est_estimator_P->tun_ase_ml_forward_projection_time; - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 4; b_m++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_p[(int32_T)(i << 2)] * - rtb_VectorConcatenate_c[b_m]; - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_c[(int32_T)(i << 2)] * - rtb_VectorConcatenate_p[b_m]; - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_c[(int32_T)(b_m + 4)]; - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_p[(int32_T)(b_m + 4)]; - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_c[(int32_T)(b_m + 8)]; - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_p[(int32_T)(b_m + 8)]; - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_c[(int32_T)(b_m + 12)]; - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_p[(int32_T)(b_m + 12)]; - } - } - - // Sum: '/Add1' incorporates: - // Constant: '/Constant2' - // Product: '/Product1' - // Product: '/Product3' - // Product: '/Product4' - // Product: '/Product5' - // Sum: '/Add2' - - for (i = 0; i < 4; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = (global_tf_camera1[(int32_T)(i << - 2)] - rtb_VectorConcatenate_hq[(int32_T)(i << 2)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_mp + - est_estimator_B->sf_MATLABFunction_l0.y[(int32_T)(i << 2)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = - (global_tf_camera1[(int32_T)((int32_T)(i << 2) + 1)] - - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 1)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_mp + - est_estimator_B->sf_MATLABFunction_l0.y[(int32_T)((int32_T)(i << 2) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = - (global_tf_camera1[(int32_T)((int32_T)(i << 2) + 2)] - - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 2)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_mp + - est_estimator_B->sf_MATLABFunction_l0.y[(int32_T)((int32_T)(i << 2) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(i << 2))] = - (global_tf_camera1[(int32_T)((int32_T)(i << 2) + 3)] - - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 3)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_mp + - est_estimator_B->sf_MATLABFunction_l0.y[(int32_T)((int32_T)(i << 2) + 3)]; - } - - // End of Sum: '/Add1' - - // Product: '/Product1' - for (i = 0; i < 4; i++) { - rtb_Sum_k1 = rtb_VectorConcatenate[(int32_T)(i + 12)] * - rtb_Merge2.quat_ISS2B[3] + (rtb_VectorConcatenate[(int32_T)(i + 8)] * - rtb_Merge2.quat_ISS2B[2] + (rtb_VectorConcatenate[(int32_T)(i + 4)] * - rtb_Merge2.quat_ISS2B[1] + rtb_VectorConcatenate[i] * - rtb_Merge2.quat_ISS2B[0])); - rtb_Product1[i] = rtb_Sum_k1; - } - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product1[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize(rtb_Product1, rtb_UnitDelay25, - (P_Normalize_est_estimator_T *)&est_estimator_P->Normalize_i); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_UnitDelay25[0] = rtb_Product1[0]; - rtb_UnitDelay25[1] = rtb_Product1[1]; - rtb_UnitDelay25[2] = rtb_Product1[2]; - rtb_UnitDelay25[3] = rtb_Product1[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Switch1 = (real32_T)sqrt((real_T)(((rtb_UnitDelay25[0] * - rtb_UnitDelay25[0] + rtb_UnitDelay25[1] * rtb_UnitDelay25[1]) + - rtb_UnitDelay25[2] * rtb_UnitDelay25[2]) + rtb_UnitDelay25[3] * - rtb_UnitDelay25[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Switch1 > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize_p(rtb_UnitDelay25, rtb_Switch1, rtb_Product1); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product1[0] = rtb_UnitDelay25[0]; - rtb_Product1[1] = rtb_UnitDelay25[1]; - rtb_Product1[2] = rtb_UnitDelay25[2]; - rtb_Product1[3] = rtb_UnitDelay25[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Switch1 = rtb_Product1[3] * rtb_Product1[3] * - est_estimator_P->Gain_Gain_c - (real32_T) - est_estimator_P->Constant1_Value_m; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_m[i]; - } - - rtb_Assignment_b[0] = rtb_Switch1; - rtb_Assignment_b[4] = rtb_Switch1; - rtb_Assignment_b[8] = rtb_Switch1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Switch1 = est_estimator_P->Gain1_Gain_nq * rtb_Product1[3]; - - // MultiPortSwitch: '/Multiport Switch1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant4' - - switch ((int32_T)est_estimator_U_cmc_msg_o->localization_mode_cmd) { - case 0: - rtb_Add_e[0] = est_estimator_P->tun_abp_p_navcam_imu_est[0]; - rtb_Add_e[1] = est_estimator_P->tun_abp_p_navcam_imu_est[1]; - rtb_Add_e[2] = est_estimator_P->tun_abp_p_navcam_imu_est[2]; - break; - - case 1: - rtb_Add_e[0] = est_estimator_P->tun_abp_p_dockcam_imu_est[0]; - rtb_Add_e[1] = est_estimator_P->tun_abp_p_dockcam_imu_est[1]; - rtb_Add_e[2] = est_estimator_P->tun_abp_p_dockcam_imu_est[2]; - break; - - default: - rtb_Add_e[0] = est_estimator_P->tun_abp_p_perchcam_imu_est[0]; - rtb_Add_e[1] = est_estimator_P->tun_abp_p_perchcam_imu_est[1]; - rtb_Add_e[2] = est_estimator_P->tun_abp_p_perchcam_imu_est[2]; - break; - } - - // End of MultiPortSwitch: '/Multiport Switch1' - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_a[0] = (real32_T)est_estimator_P->Constant3_Value_m; - tmp_a[1] = rtb_Product1[2]; - tmp_a[2] = est_estimator_P->Gain_Gain_fr * rtb_Product1[1]; - tmp_a[3] = est_estimator_P->Gain1_Gain_c * rtb_Product1[2]; - tmp_a[4] = (real32_T)est_estimator_P->Constant3_Value_m; - tmp_a[5] = rtb_Product1[0]; - tmp_a[6] = rtb_Product1[1]; - tmp_a[7] = est_estimator_P->Gain2_Gain_hc * rtb_Product1[0]; - tmp_a[8] = (real32_T)est_estimator_P->Constant3_Value_m; - - // Math: '/Math Function' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = rtb_Product1[0] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 3)] = rtb_Product1[1] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 6)] = rtb_Product1[2] * rtb_Product1[i]; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_n[(int32_T)(3 * i)] = (rtb_Assignment_b[i] - rtb_Switch1 * - tmp_a[i]) + rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_g; - rtb_Assignment_n[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 3)] - tmp_a[(int32_T)(i + 3)] * rtb_Switch1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_g; - rtb_Assignment_n[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 6)] - tmp_a[(int32_T)(i + 6)] * rtb_Switch1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_g; - } - - // End of Sum: '/Sum1' - - // Product: '/Product' - for (i = 0; i < 3; i++) { - rtb_ImpAsg_InsertedFor_Out1_a_d[i] = rtb_Assignment_n[(int32_T)(i + 6)] * - rtb_Add_e[2] + (rtb_Assignment_n[(int32_T)(i + 3)] * rtb_Add_e[1] + - rtb_Assignment_n[i] * rtb_Add_e[0]); - } - - // MultiPortSwitch: '/Multiport Switch' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Constant: '/Constant8' - - switch ((int32_T)est_estimator_U_cmc_msg_o->localization_mode_cmd) { - case 0: - rtb_UnitDelay25[0] = est_estimator_P->tun_abp_q_body2navcam[0]; - rtb_UnitDelay25[1] = est_estimator_P->tun_abp_q_body2navcam[1]; - rtb_UnitDelay25[2] = est_estimator_P->tun_abp_q_body2navcam[2]; - rtb_UnitDelay25[3] = est_estimator_P->tun_abp_q_body2navcam[3]; - break; - - case 1: - rtb_UnitDelay25[0] = est_estimator_P->tun_abp_q_body2dockcam[0]; - rtb_UnitDelay25[1] = est_estimator_P->tun_abp_q_body2dockcam[1]; - rtb_UnitDelay25[2] = est_estimator_P->tun_abp_q_body2dockcam[2]; - rtb_UnitDelay25[3] = est_estimator_P->tun_abp_q_body2dockcam[3]; - break; - - default: - rtb_UnitDelay25[0] = est_estimator_P->tun_abp_q_body2perchcam[0]; - rtb_UnitDelay25[1] = est_estimator_P->tun_abp_q_body2perchcam[1]; - rtb_UnitDelay25[2] = est_estimator_P->tun_abp_q_body2perchcam[2]; - rtb_UnitDelay25[3] = est_estimator_P->tun_abp_q_body2perchcam[3]; - break; - } - - // End of MultiPortSwitch: '/Multiport Switch' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Switch1 = rtb_UnitDelay25[3] * rtb_UnitDelay25[3] * - est_estimator_P->Gain_Gain_cu - (real32_T) - est_estimator_P->Constant1_Value_n; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_b[i]; - } - - rtb_Assignment_b[0] = rtb_Switch1; - rtb_Assignment_b[4] = rtb_Switch1; - rtb_Assignment_b[8] = rtb_Switch1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Switch1 = est_estimator_P->Gain1_Gain_l * rtb_UnitDelay25[3]; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant3' - - memcpy(&rtb_Assignment6[0], &est_estimator_P->Constant3_Value_l2[0], - (uint32_T)(90U * sizeof(ase_cov_datatype))); - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Merge2_1[0] = (real32_T)est_estimator_P->Constant3_Value_d; - rtb_Merge2_1[1] = rtb_UnitDelay25[2]; - rtb_Merge2_1[2] = est_estimator_P->Gain_Gain_ne * rtb_UnitDelay25[1]; - rtb_Merge2_1[3] = est_estimator_P->Gain1_Gain_j4 * rtb_UnitDelay25[2]; - rtb_Merge2_1[4] = (real32_T)est_estimator_P->Constant3_Value_d; - rtb_Merge2_1[5] = rtb_UnitDelay25[0]; - rtb_Merge2_1[6] = rtb_UnitDelay25[1]; - rtb_Merge2_1[7] = est_estimator_P->Gain2_Gain_n * rtb_UnitDelay25[0]; - rtb_Merge2_1[8] = (real32_T)est_estimator_P->Constant3_Value_d; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i = 0; i < 3; i++) { - S[i] = rtb_UnitDelay25[i] * rtb_UnitDelay25[0]; - S[(int32_T)(i + 3)] = rtb_UnitDelay25[i] * rtb_UnitDelay25[1]; - S[(int32_T)(i + 6)] = rtb_UnitDelay25[i] * rtb_UnitDelay25[2]; - } - - // End of Product: '/Product1' - - // SignalConversion: '/TmpSignal ConversionAtAssignment6Inport2' incorporates: - // Assignment: '/Assignment6' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_b[0] = (real32_T)est_estimator_P->Constant3_Value_pv; - tmp_b[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[2]; - tmp_b[2] = est_estimator_P->Gain_Gain_fx * rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - tmp_b[3] = est_estimator_P->Gain1_Gain_ik * rtb_ImpAsg_InsertedFor_Out1_a_d - [2]; - tmp_b[4] = (real32_T)est_estimator_P->Constant3_Value_pv; - tmp_b[5] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - tmp_b[6] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - tmp_b[7] = est_estimator_P->Gain2_Gain_m * rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - tmp_b[8] = (real32_T)est_estimator_P->Constant3_Value_pv; - for (i = 0; i < 3; i++) { - // Assignment: '/Assignment' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Sum: '/Sum1' - - rtb_Assignment6[(int32_T)(6 * i)] = (rtb_Assignment_b[(int32_T)(3 * i)] - - rtb_Merge2_1[(int32_T)(3 * i)] * rtb_Switch1) + S[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_p0; - rtb_Assignment6[(int32_T)(1 + (int32_T)(6 * i))] = (rtb_Assignment_b - [(int32_T)((int32_T)(3 * i) + 1)] - rtb_Merge2_1[(int32_T)((int32_T)(3 * - i) + 1)] * rtb_Switch1) + S[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_p0; - rtb_Assignment6[(int32_T)(2 + (int32_T)(6 * i))] = (rtb_Assignment_b - [(int32_T)((int32_T)(3 * i) + 2)] - rtb_Merge2_1[(int32_T)((int32_T)(3 * - i) + 2)] * rtb_Switch1) + S[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_p0; - - // Assignment: '/Assignment6' - rtb_Assignment6[(int32_T)(3 + (int32_T)(6 * i))] = tmp_b[(int32_T)(3 * i)]; - rtb_Assignment6[(int32_T)(4 + (int32_T)(6 * i))] = tmp_b[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_Assignment6[(int32_T)(5 + (int32_T)(6 * i))] = tmp_b[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - // Assignment: '/Assignment1' - memcpy(&est_estimator_B->Assignment[0], &est_estimator_B->Switch1[0], - (uint32_T)(13689U * sizeof(real32_T))); - - // Product: '/Product1' incorporates: - // Math: '/Math Function1' - // Selector: '/Selector1' - - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 6; b_m++) { - Switch1[(int32_T)(i + (int32_T)(15 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - Switch1[(int32_T)(i + (int32_T)(15 * b_m))] += - est_estimator_B->Switch1[(int32_T)((int32_T)(117 * i_0) + i)] * - rtb_Assignment6[(int32_T)((int32_T)(6 * i_0) + b_m)]; - } - } - } - - for (i = 0; i < 6; i++) { - // Assignment: '/Assignment1' incorporates: - // Product: '/Product1' - - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(i + (int32_T)(117 * - (int32_T)(15 + b_m))) + 15)] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(i + (int32_T)(117 * - (int32_T)(15 + b_m))) + 15)] = est_estimator_B->Assignment[(int32_T) - ((int32_T)((int32_T)((int32_T)(15 + b_m) * 117) + i) + 15)] + - rtb_Assignment6[(int32_T)((int32_T)(6 * i_0) + i)] * Switch1 - [(int32_T)((int32_T)(15 * b_m) + i_0)]; - } - } - - // Product: '/Product2' incorporates: - // Selector: '/Selector2' - - for (b_m = 0; b_m < 15; b_m++) { - rtb_Product2_b[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - rtb_Product2_b[(int32_T)(i + (int32_T)(6 * b_m))] += rtb_Assignment6 - [(int32_T)((int32_T)(6 * i_0) + i)] * est_estimator_B->Switch1 - [(int32_T)((int32_T)(117 * b_m) + i_0)]; - } - } - - // End of Product: '/Product2' - } - - // Assignment: '/Assignment2' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(b_m + (int32_T)(117 * i)) - + 15)] = rtb_Product2_b[(int32_T)((int32_T)(6 * i) + b_m)]; - } - } - - // End of Assignment: '/Assignment2' - for (i = 0; i < 6; i++) { - // Assignment: '/Assignment3' incorporates: - // Math: '/Math Function2' - - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->Assignment[(int32_T)(b_m + (int32_T)(117 * (int32_T)(15 - + i)))] = rtb_Product2_b[(int32_T)((int32_T)(6 * b_m) + i)]; - } - - // End of Assignment: '/Assignment3' - - // Product: '/Product3' incorporates: - // Selector: '/Selector4' - - for (b_m = 0; b_m < 96; b_m++) { - rtb_Product3[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - rtb_Product3[(int32_T)(i + (int32_T)(6 * b_m))] += - est_estimator_B->Switch1[(int32_T)((int32_T)((int32_T)(21 + b_m) * - 117) + i_0)] * rtb_Assignment6[(int32_T)((int32_T)(6 * i_0) + i)]; - } - } - - // End of Product: '/Product3' - } - - // Assignment: '/Assignment4' - for (i = 0; i < 96; i++) { - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(b_m + (int32_T)(117 * - (int32_T)(21 + i))) + 15)] = rtb_Product3[(int32_T)((int32_T)(6 * i) + - b_m)]; - } - } - - // End of Assignment: '/Assignment4' - - // Assignment: '/Assignment5' incorporates: - // Math: '/Math Function3' - - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 96; b_m++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(b_m + (int32_T)(117 * - (int32_T)(15 + i))) + 21)] = rtb_Product3[(int32_T)((int32_T)(6 * b_m) - + i)]; - } - } - - // End of Assignment: '/Assignment5' - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_jy[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' - rtb_Assignment_b[0] = rtb_Product1[3]; - rtb_Assignment_b[4] = rtb_Product1[3]; - rtb_Assignment_b[8] = rtb_Product1[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_c[0] = (real32_T)est_estimator_P->Constant3_Value_pi; - tmp_c[1] = rtb_Product1[2]; - tmp_c[2] = est_estimator_P->Gain_Gain_j * rtb_Product1[1]; - tmp_c[3] = est_estimator_P->Gain1_Gain_g * rtb_Product1[2]; - tmp_c[4] = (real32_T)est_estimator_P->Constant3_Value_pi; - tmp_c[5] = rtb_Product1[0]; - tmp_c[6] = rtb_Product1[1]; - tmp_c[7] = est_estimator_P->Gain2_Gain_eo * rtb_Product1[0]; - tmp_c[8] = (real32_T)est_estimator_P->Constant3_Value_pi; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i = 0; i < 3; i++) { - rtb_VectorConcatenate_c[(int32_T)(i << 2)] = rtb_Assignment_b[(int32_T)(3 * - i)] + tmp_c[(int32_T)(3 * i)]; - rtb_VectorConcatenate_c[(int32_T)(1 + (int32_T)(i << 2))] = - rtb_Assignment_b[(int32_T)((int32_T)(3 * i) + 1)] + tmp_c[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_VectorConcatenate_c[(int32_T)(2 + (int32_T)(i << 2))] = - rtb_Assignment_b[(int32_T)((int32_T)(3 * i) + 2)] + tmp_c[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - rtb_VectorConcatenate_c[3] = est_estimator_P->Gain1_Gain_ei * rtb_Product1[0]; - rtb_VectorConcatenate_c[7] = est_estimator_P->Gain1_Gain_ei * rtb_Product1[1]; - rtb_VectorConcatenate_c[11] = est_estimator_P->Gain1_Gain_ei * rtb_Product1 - [2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Reshape: '/Reshape1' - rtb_VectorConcatenate_c[12] = rtb_Product1[0]; - rtb_VectorConcatenate_c[13] = rtb_Product1[1]; - rtb_VectorConcatenate_c[14] = rtb_Product1[2]; - rtb_VectorConcatenate_c[15] = rtb_Product1[3]; - - // BusAssignment: '/Bus Assignment' incorporates: - // Gain: '/Gain' - // Product: '/Product' - // S-Function (sfix_bitop): '/Bitwise Operator2' - // Sum: '/Add' - // Sum: '/Add1' - - rtb_BusAssignment_a = rtb_Merge2; - rtb_BusAssignment_a.aug_state_enum = (uint32_T)(rtb_Merge2.aug_state_enum | - est_estimator_P->BitwiseOperator2_BitMask); - rtb_BusAssignment_a.ml_P_cam_ISS_ISS[0] = (rtb_Merge2.P_EST_ISS_ISS[0] - - est_estimator_P->tun_ase_ml_forward_projection_time * - rtb_Merge2.V_B_ISS_ISS[0]) + rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - rtb_BusAssignment_a.ml_P_cam_ISS_ISS[1] = (rtb_Merge2.P_EST_ISS_ISS[1] - - est_estimator_P->tun_ase_ml_forward_projection_time * - rtb_Merge2.V_B_ISS_ISS[1]) + rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - rtb_BusAssignment_a.ml_P_cam_ISS_ISS[2] = (rtb_Merge2.P_EST_ISS_ISS[2] - - est_estimator_P->tun_ase_ml_forward_projection_time * - rtb_Merge2.V_B_ISS_ISS[2]) + rtb_ImpAsg_InsertedFor_Out1_a_d[2]; - for (i = 0; i < 4; i++) { - rtb_BusAssignment_a.ml_quat_ISS2cam[i] = 0.0F; - rtb_BusAssignment_a.ml_quat_ISS2cam[i] += rtb_VectorConcatenate_c[i] * - rtb_UnitDelay25[0]; - rtb_BusAssignment_a.ml_quat_ISS2cam[i] += rtb_VectorConcatenate_c[(int32_T) - (i + 4)] * rtb_UnitDelay25[1]; - rtb_BusAssignment_a.ml_quat_ISS2cam[i] += rtb_VectorConcatenate_c[(int32_T) - (i + 8)] * rtb_UnitDelay25[2]; - rtb_BusAssignment_a.ml_quat_ISS2cam[i] += rtb_VectorConcatenate_c[(int32_T) - (i + 12)] * rtb_UnitDelay25[3]; - } - - // End of BusAssignment: '/Bus Assignment' - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - ml_vel_aug[0] = (real_T)rtb_Merge2.V_B_ISS_ISS[0]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion2' - - ml_omega_aug[0] = (real_T)rtb_Merge2.omega_B_ISS_B[0]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - ml_vel_aug[1] = (real_T)rtb_Merge2.V_B_ISS_ISS[1]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion2' - - ml_omega_aug[1] = (real_T)rtb_Merge2.omega_B_ISS_B[1]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - ml_vel_aug[2] = (real_T)rtb_Merge2.V_B_ISS_ISS[2]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion2' - - ml_omega_aug[2] = (real_T)rtb_Merge2.omega_B_ISS_B[2]; - - // SignalConversion: '/Signal Conversion1' - memcpy(&of_vel_aug[0], &rtb_of_vel_aug[0], (uint32_T)(48U * sizeof(real_T))); - - // SignalConversion: '/Signal Conversion1' - memcpy(&of_omega_aug[0], &rtb_of_omega_aug[0], (uint32_T)(48U * sizeof - (real_T))); - - // End of Outputs for SubSystem: '/If Action Subsystem2' - } else if ((int32_T)((int32_T)rtb_Merge2.kfl_status & (int32_T) - est_estimator_P->BitwiseOperator1_BitMask) != (int32_T) - est_estimator_P->Constant_Value_o) { - // Outputs for IfAction SubSystem: '/If Action Subsystem4' incorporates: - // ActionPort: '/Action Port' - - memcpy(&est_estimator_B->Assignment[0], &est_estimator_B->Switch1[0], - (uint32_T)(13689U * sizeof(real32_T))); - - // SignalConversion: '/Signal Conversion' incorporates: - // Inport: '/P_in' - - rtb_BusAssignment_a = rtb_Merge2; - - // SignalConversion: '/Signal Conversion2' - ml_vel_aug[0] = rtb_ml_vel_aug[0]; - - // SignalConversion: '/Signal Conversion2' - ml_omega_aug[0] = rtb_ml_omega_aug[0]; - - // SignalConversion: '/Signal Conversion2' - ml_vel_aug[1] = rtb_ml_vel_aug[1]; - - // SignalConversion: '/Signal Conversion2' - ml_omega_aug[1] = rtb_ml_omega_aug[1]; - - // SignalConversion: '/Signal Conversion2' - ml_vel_aug[2] = rtb_ml_vel_aug[2]; - - // SignalConversion: '/Signal Conversion2' - ml_omega_aug[2] = rtb_ml_omega_aug[2]; - - // SignalConversion: '/Signal Conversion2' - memcpy(&of_vel_aug[0], &rtb_of_vel_aug[0], (uint32_T)(48U * sizeof(real_T))); - - // SignalConversion: '/Signal Conversion2' - memcpy(&of_omega_aug[0], &rtb_of_omega_aug[0], (uint32_T)(48U * sizeof - (real_T))); - - // End of Outputs for SubSystem: '/If Action Subsystem4' - } else { - // Outputs for IfAction SubSystem: '/If Action Subsystem1' incorporates: - // ActionPort: '/Action Port' - - est_estimato_IfActionSubsystem1(&rtb_Merge2, est_estimator_B->Switch1, - rtb_ml_vel_aug, rtb_ml_omega_aug, rtb_of_vel_aug, rtb_of_omega_aug, - &rtb_BusAssignment_a, est_estimator_B->Assignment, ml_vel_aug, - ml_omega_aug, of_vel_aug, of_omega_aug); - - // End of Outputs for SubSystem: '/If Action Subsystem1' - } - - // End of If: '/If' - - // If: '/If' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Inport: '/Vision Registration' - // Logic: '/Logical Operator' - // RelationalOperator: '/Compare' - - if ((est_estimator_U_VisionRegistration->cvs_optical_flow_pulse > - est_estimator_P->CompareToConstant_const) && ((int32_T) - est_estimator_P->tun_ase_enable_of != 0)) { - // Outputs for IfAction SubSystem: '/If Action Subsystem2' incorporates: - // ActionPort: '/Action Port' - - rtb_VectorConcatenate_m[0] = est_estimator_P->Constant3_Value_cc; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[1] = est_estimator_P->Gain_Gain_h * (real32_T) - est_estimator_P->Constant_Value_p[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[2] = (real32_T)est_estimator_P->Constant_Value_p[1]; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[3] = est_estimator_P->Gain1_Gain_b * (real32_T) - est_estimator_P->Constant_Value_p[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[4] = (real32_T)est_estimator_P->Constant_Value_p[2]; - rtb_VectorConcatenate_m[5] = est_estimator_P->Constant2_Value_pd; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant' - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[6] = est_estimator_P->Gain2_Gain_h * (real32_T) - est_estimator_P->Constant_Value_p[0]; - - // Gain: '/Gain3' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[7] = est_estimator_P->Gain3_Gain * (real32_T) - est_estimator_P->Constant_Value_p[1]; - - // Gain: '/Gain4' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[8] = est_estimator_P->Gain4_Gain * (real32_T) - est_estimator_P->Constant_Value_p[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[9] = (real32_T)est_estimator_P->Constant_Value_p[0]; - rtb_VectorConcatenate_m[10] = est_estimator_P->Constant1_Value_j; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[11] = est_estimator_P->Gain5_Gain * (real32_T) - est_estimator_P->Constant_Value_p[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[12] = (real32_T)est_estimator_P->Constant_Value_p[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[13] = (real32_T)est_estimator_P->Constant_Value_p[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[14] = (real32_T)est_estimator_P->Constant_Value_p[2]; - rtb_VectorConcatenate_m[15] = est_estimator_P->Constant_Value_f; - rtb_VectorConcatenate_i[0] = est_estimator_P->Constant3_Value_iq; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - - rtb_Add_e[0] = est_estimator_P->Gain1_Gain_j * - rtb_BusAssignment_a.omega_B_ISS_B[0]; - rtb_Add_e[1] = est_estimator_P->Gain1_Gain_j * - rtb_BusAssignment_a.omega_B_ISS_B[1]; - rtb_Add_e[2] = est_estimator_P->Gain1_Gain_j * - rtb_BusAssignment_a.omega_B_ISS_B[2]; - - // Gain: '/Gain' - rtb_VectorConcatenate_i[1] = est_estimator_P->Gain_Gain_n * rtb_Add_e[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' - rtb_VectorConcatenate_i[2] = rtb_Add_e[1]; - - // Gain: '/Gain1' - rtb_VectorConcatenate_i[3] = est_estimator_P->Gain1_Gain_f * rtb_Add_e[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' - rtb_VectorConcatenate_i[4] = rtb_Add_e[2]; - rtb_VectorConcatenate_i[5] = est_estimator_P->Constant2_Value_n; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant2' - - rtb_VectorConcatenate_i[6] = est_estimator_P->Gain2_Gain_a * rtb_Add_e[0]; - - // Gain: '/Gain3' - rtb_VectorConcatenate_i[7] = est_estimator_P->Gain3_Gain_l * rtb_Add_e[1]; - - // Gain: '/Gain4' - rtb_VectorConcatenate_i[8] = est_estimator_P->Gain4_Gain_o * rtb_Add_e[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' - rtb_VectorConcatenate_i[9] = rtb_Add_e[0]; - rtb_VectorConcatenate_i[10] = est_estimator_P->Constant1_Value_nk; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant1' - - rtb_VectorConcatenate_i[11] = est_estimator_P->Gain5_Gain_j * rtb_Add_e[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' - rtb_VectorConcatenate_i[12] = rtb_Add_e[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' - rtb_VectorConcatenate_i[13] = rtb_Add_e[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' - rtb_VectorConcatenate_i[14] = rtb_Add_e[2]; - rtb_VectorConcatenate_i[15] = est_estimator_P->Constant_Value_j; - - // Product: '/Product2' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant3' - // Constant: '/Constant' - // Product: '/Product' - // Sum: '/Add' - - for (i = 0; i < 16; i++) { - rtb_VectorConcatenate_c[i] = (est_estimator_P->Constant3_Value_f * - rtb_VectorConcatenate_m[i] * - est_estimator_P->tun_ase_ml_forward_projection_time + - rtb_VectorConcatenate_i[i]) * est_estimator_P->Constant1_Value_cn * - est_estimator_P->tun_ase_ml_forward_projection_time; - } - - // End of Product: '/Product2' - - // MATLAB Function: '/MATLAB Function' - est_estimator_MATLABFunction(rtb_VectorConcatenate_c, - &est_estimator_B->sf_MATLABFunction_o); - - // Product: '/Product5' incorporates: - // Constant: '/Constant' - - hr_quat_ISS2hr_idx_1 = est_estimator_P->tun_ase_ml_forward_projection_time * - est_estimator_P->tun_ase_ml_forward_projection_time * - est_estimator_P->tun_ase_ml_forward_projection_time; - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 4; b_m++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_i[(int32_T)(i << 2)] * - rtb_VectorConcatenate_m[b_m]; - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)(i << 2)] * - rtb_VectorConcatenate_i[b_m]; - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_i[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_m[(int32_T)(b_m + 4)]; - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_i[(int32_T)(b_m + 4)]; - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_i[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_m[(int32_T)(b_m + 8)]; - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_i[(int32_T)(b_m + 8)]; - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_i[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_m[(int32_T)(b_m + 12)]; - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_i[(int32_T)(b_m + 12)]; - } - } - - // Sum: '/Add1' incorporates: - // Constant: '/Constant2' - // Product: '/Product1' - // Product: '/Product3' - // Product: '/Product4' - // Product: '/Product5' - // Sum: '/Add2' - - for (i = 0; i < 4; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = (rtb_VectorConcatenate_c - [(int32_T)(i << 2)] - rtb_VectorConcatenate_p[(int32_T)(i << 2)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_p3 + - est_estimator_B->sf_MATLABFunction_o.y[(int32_T)(i << 2)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = - (rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 1)] - - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 1)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_p3 + - est_estimator_B->sf_MATLABFunction_o.y[(int32_T)((int32_T)(i << 2) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = - (rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 2)] - - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 2)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_p3 + - est_estimator_B->sf_MATLABFunction_o.y[(int32_T)((int32_T)(i << 2) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(i << 2))] = - (rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 3)] - - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 3)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_p3 + - est_estimator_B->sf_MATLABFunction_o.y[(int32_T)((int32_T)(i << 2) + 3)]; - } - - // End of Sum: '/Add1' - - // Product: '/Product1' - for (i = 0; i < 4; i++) { - rtb_Sum_k1 = rtb_VectorConcatenate[(int32_T)(i + 12)] * - rtb_BusAssignment_a.quat_ISS2B[3] + (rtb_VectorConcatenate[(int32_T)(i + - 8)] * rtb_BusAssignment_a.quat_ISS2B[2] + (rtb_VectorConcatenate - [(int32_T)(i + 4)] * rtb_BusAssignment_a.quat_ISS2B[1] + - rtb_VectorConcatenate[i] * rtb_BusAssignment_a.quat_ISS2B[0])); - rtb_Product1[i] = rtb_Sum_k1; - } - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product1[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize(rtb_Product1, rtb_UnitDelay25, - (P_Normalize_est_estimator_T *)&est_estimator_P->Normalize_h); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_UnitDelay25[0] = rtb_Product1[0]; - rtb_UnitDelay25[1] = rtb_Product1[1]; - rtb_UnitDelay25[2] = rtb_Product1[2]; - rtb_UnitDelay25[3] = rtb_Product1[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Switch1 = (real32_T)sqrt((real_T)(((rtb_UnitDelay25[0] * - rtb_UnitDelay25[0] + rtb_UnitDelay25[1] * rtb_UnitDelay25[1]) + - rtb_UnitDelay25[2] * rtb_UnitDelay25[2]) + rtb_UnitDelay25[3] * - rtb_UnitDelay25[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Switch1 > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize_p(rtb_UnitDelay25, rtb_Switch1, rtb_Product1); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product1[0] = rtb_UnitDelay25[0]; - rtb_Product1[1] = rtb_UnitDelay25[1]; - rtb_Product1[2] = rtb_UnitDelay25[2]; - rtb_Product1[3] = rtb_UnitDelay25[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Switch1 = rtb_Product1[3] * rtb_Product1[3] * - est_estimator_P->Gain_Gain_a - (real32_T)est_estimator_P->Constant1_Value; - for (i = 0; i < 9; i++) { - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_j[i]; - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - rtb_Assignment_n[i] = (real32_T)est_estimator_P->Constant2_Value_l[i]; - } - - // Assignment: '/Assignment' - rtb_Assignment_b[0] = rtb_Switch1; - rtb_Assignment_b[4] = rtb_Switch1; - rtb_Assignment_b[8] = rtb_Switch1; - - // Gain: '/Gain1' - rtb_Switch1 = est_estimator_P->Gain1_Gain_e * rtb_Product1[3]; - - // Assignment: '/Assignment' - rtb_Assignment_n[0] = rtb_Product1[3]; - rtb_Assignment_n[4] = rtb_Product1[3]; - rtb_Assignment_n[8] = rtb_Product1[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_d[0] = (real32_T)est_estimator_P->Constant3_Value_p; - tmp_d[1] = rtb_Product1[2]; - tmp_d[2] = est_estimator_P->Gain_Gain_g * rtb_Product1[1]; - tmp_d[3] = est_estimator_P->Gain1_Gain_jc * rtb_Product1[2]; - tmp_d[4] = (real32_T)est_estimator_P->Constant3_Value_p; - tmp_d[5] = rtb_Product1[0]; - tmp_d[6] = rtb_Product1[1]; - tmp_d[7] = est_estimator_P->Gain2_Gain_o * rtb_Product1[0]; - tmp_d[8] = (real32_T)est_estimator_P->Constant3_Value_p; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i = 0; i < 3; i++) { - rtb_VectorConcatenate_m[(int32_T)(i << 2)] = rtb_Assignment_n[(int32_T)(3 * - i)] + tmp_d[(int32_T)(3 * i)]; - rtb_VectorConcatenate_m[(int32_T)(1 + (int32_T)(i << 2))] = - rtb_Assignment_n[(int32_T)((int32_T)(3 * i) + 1)] + tmp_d[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_VectorConcatenate_m[(int32_T)(2 + (int32_T)(i << 2))] = - rtb_Assignment_n[(int32_T)((int32_T)(3 * i) + 2)] + tmp_d[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - rtb_VectorConcatenate_m[3] = est_estimator_P->Gain1_Gain_i * rtb_Product1[0]; - rtb_VectorConcatenate_m[7] = est_estimator_P->Gain1_Gain_i * rtb_Product1[1]; - rtb_VectorConcatenate_m[11] = est_estimator_P->Gain1_Gain_i * rtb_Product1[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Reshape: '/Reshape1' - rtb_VectorConcatenate_m[12] = rtb_Product1[0]; - rtb_VectorConcatenate_m[13] = rtb_Product1[1]; - rtb_VectorConcatenate_m[14] = rtb_Product1[2]; - rtb_VectorConcatenate_m[15] = rtb_Product1[3]; - - // MATLAB Function: '/MATLAB Function' - // MATLAB Function 'state_manager/Optical Flow Registration Manager/If Action Subsystem2/MATLAB Function': ':1' - // From Zack's matlab filter. This function add loads up the most recent - // camera attitude and position, as well as setting the aug_state_byte - // ':1:5' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // This function add loads up the most recent - // camera attitude and position, as well as setting the aug_state_byte - // Cast this back to double to not cause issues with other integers - // retain the values between function calls - if ((!est_estimator_DW->aug_velocity_not_empty) || - (!est_estimator_DW->aug_velocity_mag_not_empty) || - (!est_estimator_DW->aug_omega_not_empty) || - (!est_estimator_DW->aug_omega_mag_not_empty)) { - // Initialize values to zero and the same data type as state_in.V_B_ISS_ISS - memset(&est_estimator_DW->aug_velocity[0], 0, (uint32_T)(48U * sizeof - (real32_T))); - est_estimator_DW->aug_velocity_not_empty = true; - memset(&est_estimator_DW->aug_velocity_mag[0], 0, (uint32_T)(sizeof - (real32_T) << 4U)); - est_estimator_DW->aug_velocity_mag_not_empty = true; - memset(&est_estimator_DW->aug_omega[0], 0, (uint32_T)(48U * sizeof - (real32_T))); - est_estimator_DW->aug_omega_not_empty = true; - memset(&est_estimator_DW->aug_omega_mag[0], 0, (uint32_T)(sizeof(real32_T) - << 4U)); - est_estimator_DW->aug_omega_mag_not_empty = true; - } - - memcpy(&est_estimator_B->Switch1[0], &est_estimator_B->Assignment[0], - (uint32_T)(13689U * sizeof(real32_T))); - rtb_Merge2 = rtb_BusAssignment_a; - - // Constants/variables I'll need later - // Augment flag - // [MSB Oldest_OF_Aug ... Newest_OF_Aug Valid_ML_Augment(LSB)] - // Mask off just the OF bits - // Extract out the bits associated with OF - // Shift all the OF bits 1 bit more significant - // Set the newest OF bit as valid - // Mask off the unused bits, remove rollover - rtb_Merge2.aug_state_enum = (uint32_T)((uint32_T)((uint32_T)((uint32_T) - ((uint32_T)(rtb_BusAssignment_a.aug_state_enum & 131070U) << 1U) | 2U) & - 131070U) | (uint32_T)((int32_T)(uint32_T) - (rtb_BusAssignment_a.aug_state_enum & 1U) != 0)); - - // Restore the value of the ML aug bit - // new_jf = bitset(bitshift(bitand(jf, bin2dec('111110')), 1), 2) - // Augment the state vector - // Ive arranged the state vector so it is: - // [IMU location, ML Camera Location, Newest OF Camera Location .... Oldest OF Camera Loc] - UnitDelay_DSTATE_aug_state_enum = (uint32_T)(16U - (uint32_T) - est_estimator_U_VisionRegistration->cvs_optical_flow_pulse); - if (UnitDelay_DSTATE_aug_state_enum > 16U) { - UnitDelay_DSTATE_aug_state_enum = 0U; - } - - numFeatures = (uint8_T)(int32_T)((int32_T)UnitDelay_DSTATE_aug_state_enum - + 1); - - // lowest number replaces most recent - memset(&of_in_prange[0], 0, (uint32_T)(90U * sizeof(real_T))); - num_augs = 1.0; - for (num_original = 0; num_original < 15; num_original++) { - if (num_augs == (real_T)numFeatures) { - num_augs++; - } - - kept_augmentations[num_original] = num_augs; - br = (int32_T)((int32_T)(6 * num_original) + 1); - nx = (int32_T)((int32_T)(1 + num_original) * 6); - if (br > nx) { - br = 1; - nx = 0; - } - - handrail_knowledge_dims = (int32_T)((int32_T)(nx - br) + 1); - C_sizes_idx_1 = (int32_T)(nx - br); - for (i = 0; i <= C_sizes_idx_1; i++) { - f_data[i] = (int8_T)(int32_T)((int32_T)(int8_T)(int32_T)(br + i) - 1); - } - - tmp_9 = (num_augs - 1.0) * 6.0 + 21.0; - for (i = 0; i < 6; i++) { - tmp_g[i] = (int8_T)(int32_T)(1 + i); - } - - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - of_in_prange[(int32_T)f_data[i]] = tmp_9 + (real_T)tmp_g[i]; - } - - num_augs++; - } - - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_Merge2_0[(int32_T)(b_m + (int32_T)(15 * i))] = - rtb_Merge2.of_quat_ISS2cam[(int32_T)((int32_T)((int32_T)(i << 4) + - (int32_T)kept_augmentations[b_m]) - 1)]; - } - } - - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_Merge2.of_quat_ISS2cam[(int32_T)((int32_T)(b_m + (int32_T)(i << 4)) - + 1)] = rtb_Merge2_0[(int32_T)((int32_T)(15 * i) + b_m)]; - } - - // DataTypeConversion: '/Data Type Conversion1' incorporates: - // Constant: '/Constant2' - // Product: '/Product' - - rtb_VectorConcatenate_o[i] = (real_T)(((rtb_VectorConcatenate_m[(int32_T) - (i + 4)] * est_estimator_P->tun_abp_q_body2navcam[1] + - rtb_VectorConcatenate_m[i] * est_estimator_P->tun_abp_q_body2navcam[0]) - + rtb_VectorConcatenate_m[(int32_T)(i + 8)] * - est_estimator_P->tun_abp_q_body2navcam[2]) + rtb_VectorConcatenate_m - [(int32_T)(i + 12)] * est_estimator_P->tun_abp_q_body2navcam[3]); - rtb_Merge2.of_quat_ISS2cam[(int32_T)(i << 4)] = (real32_T) - rtb_VectorConcatenate_o[i]; - } - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_e[0] = (real32_T)est_estimator_P->Constant3_Value_l; - tmp_e[1] = rtb_Product1[2]; - tmp_e[2] = est_estimator_P->Gain_Gain_f * rtb_Product1[1]; - tmp_e[3] = est_estimator_P->Gain1_Gain_k * rtb_Product1[2]; - tmp_e[4] = (real32_T)est_estimator_P->Constant3_Value_l; - tmp_e[5] = rtb_Product1[0]; - tmp_e[6] = rtb_Product1[1]; - tmp_e[7] = est_estimator_P->Gain2_Gain_i * rtb_Product1[0]; - tmp_e[8] = (real32_T)est_estimator_P->Constant3_Value_l; - - // Math: '/Math Function' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = rtb_Product1[0] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 3)] = rtb_Product1[1] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 6)] = rtb_Product1[2] * rtb_Product1[i]; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_n[(int32_T)(3 * i)] = (rtb_Assignment_b[i] - rtb_Switch1 * - tmp_e[i]) + rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_p; - rtb_Assignment_n[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 3)] - tmp_e[(int32_T)(i + 3)] * rtb_Switch1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_p; - rtb_Assignment_n[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 6)] - tmp_e[(int32_T)(i + 6)] * rtb_Switch1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_p; - } - - // End of Sum: '/Sum1' - for (i = 0; i < 3; i++) { - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Data Type Conversion2' - // Gain: '/Gain' - // Product: '/Product' - // Sum: '/Add' - // Sum: '/Add1' - - UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)(i << 4)] = ((rtb_Assignment_n - [(int32_T)(i + 3)] * est_estimator_P->tun_abp_p_navcam_imu_est[1] + - rtb_Assignment_n[i] * est_estimator_P->tun_abp_p_navcam_imu_est[0]) + - rtb_Assignment_n[(int32_T)(i + 6)] * - est_estimator_P->tun_abp_p_navcam_imu_est[2]) + - (rtb_BusAssignment_a.P_EST_ISS_ISS[i] - - est_estimator_P->tun_ase_ml_forward_projection_time * - rtb_BusAssignment_a.V_B_ISS_ISS[i]); - for (b_m = 0; b_m < 15; b_m++) { - UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)((int32_T)(b_m + (int32_T)(i << - 4)) + 1)] = rtb_Merge2.of_P_cam_ISS_ISS[(int32_T)((int32_T)((int32_T) - (i << 4) + (int32_T)kept_augmentations[b_m]) - 1)]; - } - } - - // MATLAB Function: '/MATLAB Function' - // Augment velocities - for (i = 0; i < 3; i++) { - memcpy(&rtb_Merge2.of_P_cam_ISS_ISS[(int32_T)(i << 4)], - &UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)(i << 4)], (uint32_T) - (sizeof(real32_T) << 4U)); - rtb_BusAssignment_h[(int32_T)(i << 4)] = rtb_BusAssignment_a.V_B_ISS_ISS[i]; - for (b_m = 0; b_m < 15; b_m++) { - rtb_BusAssignment_h[(int32_T)((int32_T)(b_m + (int32_T)(i << 4)) + 1)] = - est_estimator_DW->aug_velocity[(int32_T)((int32_T)((int32_T)(i << 4) + - (int32_T)kept_augmentations[b_m]) - 1)]; - } - } - - for (i = 0; i < 3; i++) { - memcpy(&est_estimator_DW->aug_velocity[(int32_T)(i << 4)], - &rtb_BusAssignment_h[(int32_T)(i << 4)], (uint32_T)(sizeof(real32_T) - << 4U)); - } - - rtb_VectorConcatenate[0] = aimgbiekknohhdjm_norm - (rtb_BusAssignment_a.V_B_ISS_ISS); - for (i = 0; i < 15; i++) { - rtb_VectorConcatenate[(int32_T)(i + 1)] = - est_estimator_DW->aug_velocity_mag[(int32_T)((int32_T) - kept_augmentations[i] - 1)]; - } - - memcpy(&est_estimator_DW->aug_velocity_mag[0], &rtb_VectorConcatenate[0], - (uint32_T)(sizeof(real32_T) << 4U)); - for (i = 0; i < 3; i++) { - UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)(i << 4)] = - rtb_BusAssignment_a.omega_B_ISS_B[i]; - for (b_m = 0; b_m < 15; b_m++) { - UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)((int32_T)(b_m + (int32_T)(i << - 4)) + 1)] = est_estimator_DW->aug_omega[(int32_T)((int32_T)((int32_T) - (i << 4) + (int32_T)kept_augmentations[b_m]) - 1)]; - } - } - - for (i = 0; i < 3; i++) { - memcpy(&est_estimator_DW->aug_omega[(int32_T)(i << 4)], - &UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)(i << 4)], (uint32_T) - (sizeof(real32_T) << 4U)); - } - - rtb_VectorConcatenate[0] = aimgbiekknohhdjm_norm - (rtb_BusAssignment_a.omega_B_ISS_B); - for (i = 0; i < 15; i++) { - rtb_VectorConcatenate[(int32_T)(i + 1)] = est_estimator_DW->aug_omega_mag - [(int32_T)((int32_T)kept_augmentations[i] - 1)]; - } - - memcpy(&est_estimator_DW->aug_omega_mag[0], &rtb_VectorConcatenate[0], - (uint32_T)(sizeof(real32_T) << 4U)); - - // Augmenting the covariance matrix - // Move covariances down the stack: - // P = [A B C] then P+1 = [A 0 B] - // [D E F] [0 0 0] - // [G H J] [D 0 E] - // Section D - for (i = 0; i < 21; i++) { - for (b_m = 0; b_m < 90; b_m++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(b_m + (int32_T)(117 * i)) + - 27)] = est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)(117 * - i) + (int32_T)of_in_prange[b_m]) - 1)]; - } - } - - // Section B - for (i = 0; i < 90; i++) { - for (b_m = 0; b_m < 21; b_m++) { - est_estimator_B->Switch1_c[(int32_T)(b_m + (int32_T)(21 * i))] = - est_estimator_B->Switch1[(int32_T)((int32_T)((int32_T)((int32_T) - of_in_prange[i] - 1) * 117) + b_m)]; - } - } - - for (i = 0; i < 90; i++) { - memcpy(&est_estimator_B->Switch1[(int32_T)((int32_T)(i * 117) + 3159)], - &est_estimator_B->Switch1_c[(int32_T)(i * 21)], (uint32_T)(21U * - sizeof(real32_T))); - } - - // Section E - for (i = 0; i < 90; i++) { - for (b_m = 0; b_m < 90; b_m++) { - est_estimator_B->Switch1_k[(int32_T)(b_m + (int32_T)(90 * i))] = - est_estimator_B->Switch1[(int32_T)((int32_T)((int32_T)((int32_T) - ((int32_T)of_in_prange[i] - 1) * 117) + (int32_T)of_in_prange[b_m]) - - 1)]; - } - } - - for (i = 0; i < 90; i++) { - memcpy(&est_estimator_B->Switch1[(int32_T)((int32_T)(i * 117) + 3186)], - &est_estimator_B->Switch1_k[(int32_T)(i * 90)], (uint32_T)(90U * - sizeof(real32_T))); - } - - // Now actually place an augmentation of IMU into OF - // M here is actually just the left part of J as defined in equation 24 in the - // Visinav paper by Mourikis '09 + space for the ML augmentation. - fkfcbaiengdjgdje_quaternion_to_rotation(rtb_BusAssignment_a.quat_ISS2B, - rtb_Product1_0); - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - fkfcbaiengdjgdje_quaternion_to_rotation - (est_estimator_P->tun_abp_q_body2navcam, rtb_Assignment_o); - for (i = 0; i < 3; i++) { - M[(int32_T)(6 * i)] = rtb_Assignment_o[(int32_T)(3 * i)]; - M[(int32_T)(1 + (int32_T)(6 * i))] = rtb_Assignment_o[(int32_T)((int32_T) - (3 * i) + 1)]; - M[(int32_T)(2 + (int32_T)(6 * i))] = rtb_Assignment_o[(int32_T)((int32_T) - (3 * i) + 2)]; - rtb_Add_e[i] = rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->tun_abp_p_navcam_imu_est[2] + (rtb_Product1_0[(int32_T) - ((int32_T)(3 * i) + 1)] * est_estimator_P->tun_abp_p_navcam_imu_est[1] + - rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->tun_abp_p_navcam_imu_est[0]); - } - - for (i = 0; i < 12; i++) { - M[(int32_T)(6 * (int32_T)(i + 3))] = 0.0F; - M[(int32_T)(1 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - M[(int32_T)(2 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - } - - for (i = 0; i < 6; i++) { - M[(int32_T)(6 * (int32_T)(i + 15))] = 0.0F; - M[(int32_T)(1 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - M[(int32_T)(2 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - } - - M[3] = 0.0F; - M[9] = -rtb_Add_e[2]; - M[15] = rtb_Add_e[1]; - M[4] = rtb_Add_e[2]; - M[10] = 0.0F; - M[16] = -rtb_Add_e[0]; - M[5] = -rtb_Add_e[1]; - M[11] = rtb_Add_e[0]; - M[17] = 0.0F; - for (i = 0; i < 9; i++) { - M[(int32_T)(3 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - M[(int32_T)(4 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - M[(int32_T)(5 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - } - - for (i = 0; i < 3; i++) { - M[(int32_T)(3 + (int32_T)(6 * (int32_T)(i + 12)))] = (real32_T)g[(int32_T) - (3 * i)]; - M[(int32_T)(4 + (int32_T)(6 * (int32_T)(i + 12)))] = (real32_T)g[(int32_T) - ((int32_T)(3 * i) + 1)]; - M[(int32_T)(5 + (int32_T)(6 * (int32_T)(i + 12)))] = (real32_T)g[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 6; i++) { - M[(int32_T)(3 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - M[(int32_T)(4 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - M[(int32_T)(5 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - } - - // J = [eye(21, size(P, 2)); M zeros(6, size(P, 2) - size(M, 2)); zeros(size(P, 1) - 27, 27) eye(size(P, 1) - 27, size(P, 2) - 27)]; - // P = J * P * J'; - // P = [A 0 C] then P+1 = [A AMt C ] - // [0 0 0] [MA MAMt MC] - // [G 0 J] [G GMt J ] - // Center section - // Top left sections - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 21; b_m++) { - M_0[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 21; i_0++) { - M_0[(int32_T)(i + (int32_T)(6 * b_m))] += M[(int32_T)((int32_T)(6 * - i_0) + i)] * est_estimator_B->Switch1[(int32_T)((int32_T)(117 * b_m) - + i_0)]; - } - } - - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(i + (int32_T)(117 * - (int32_T)(21 + b_m))) + 21)] = 0.0F; - for (i_0 = 0; i_0 < 21; i_0++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(i + (int32_T)(117 * - (int32_T)(21 + b_m))) + 21)] = est_estimator_B->Switch1[(int32_T) - ((int32_T)((int32_T)((int32_T)(21 + b_m) * 117) + i) + 21)] + M_0 - [(int32_T)((int32_T)(6 * i_0) + i)] * M[(int32_T)((int32_T)(6 * i_0) - + b_m)]; - } - } - - for (b_m = 0; b_m < 21; b_m++) { - M_1[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 21; i_0++) { - M_1[(int32_T)(i + (int32_T)(6 * b_m))] += M[(int32_T)((int32_T)(6 * - i_0) + i)] * est_estimator_B->Switch1[(int32_T)((int32_T)(117 * b_m) - + i_0)]; - } - } - } - - for (i = 0; i < 21; i++) { - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(b_m + (int32_T)(117 * i)) + - 21)] = M_1[(int32_T)((int32_T)(6 * i) + b_m)]; - } - } - - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 21; b_m++) { - M_0[(int32_T)(b_m + (int32_T)(21 * i))] = est_estimator_B->Switch1 - [(int32_T)((int32_T)((int32_T)(117 * b_m) + i) + 21)]; - } - } - - for (i = 0; i < 6; i++) { - memcpy(&est_estimator_B->Switch1[(int32_T)((int32_T)(i * 117) + 2457)], - &M_0[(int32_T)(i * 21)], (uint32_T)(21U * sizeof(real32_T))); - } - - // Bottom right sections - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 90; b_m++) { - M_2[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 21; i_0++) { - M_2[(int32_T)(i + (int32_T)(6 * b_m))] += est_estimator_B->Switch1 - [(int32_T)((int32_T)((int32_T)(27 + b_m) * 117) + i_0)] * M[(int32_T) - ((int32_T)(6 * i_0) + i)]; - } - } - } - - for (i = 0; i < 90; i++) { - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(b_m + (int32_T)(117 * - (int32_T)(27 + i))) + 21)] = M_2[(int32_T)((int32_T)(6 * i) + b_m)]; - } - } - - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 90; b_m++) { - M_2[(int32_T)(b_m + (int32_T)(90 * i))] = est_estimator_B->Switch1 - [(int32_T)((int32_T)((int32_T)((int32_T)(27 + b_m) * 117) + i) + 21)]; - } - } - - for (i = 0; i < 6; i++) { - memcpy(&est_estimator_B->Switch1[(int32_T)((int32_T)(i * 117) + 2484)], - &M_2[(int32_T)(i * 90)], (uint32_T)(90U * sizeof(real32_T))); - } - - // SignalConversion: '/Signal Conversion1' - // ':1:5' - rtb_ml_vel_aug[0] = ml_vel_aug[0]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_omega_aug[0] = ml_omega_aug[0]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_vel_aug[1] = ml_vel_aug[1]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_omega_aug[1] = ml_omega_aug[1]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_vel_aug[2] = ml_vel_aug[2]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_omega_aug[2] = ml_omega_aug[2]; - for (i = 0; i < 48; i++) { - // SignalConversion: '/Signal Conversion1' incorporates: - // MATLAB Function: '/MATLAB Function' - - rtb_of_vel_aug[i] = (real_T)est_estimator_DW->aug_velocity[i]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // MATLAB Function: '/MATLAB Function' - - rtb_of_omega_aug[i] = (real_T)est_estimator_DW->aug_omega[i]; - } - - // End of Outputs for SubSystem: '/If Action Subsystem2' - } else { - // Outputs for IfAction SubSystem: '/If Action Subsystem1' incorporates: - // ActionPort: '/Action Port' - - est_estimato_IfActionSubsystem1(&rtb_BusAssignment_a, - est_estimator_B->Assignment, ml_vel_aug, ml_omega_aug, of_vel_aug, - of_omega_aug, &rtb_Merge2, est_estimator_B->Switch1, rtb_ml_vel_aug, - rtb_ml_omega_aug, rtb_of_vel_aug, rtb_of_omega_aug); - - // End of Outputs for SubSystem: '/If Action Subsystem1' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // Math: '/Math Function' - // SignalConversion: '/TmpSignal ConversionAtMath FunctionInport1' - // Sum: '/Sum of Elements' - - rtb_Switch1 = (real32_T)sqrt((real_T)((est_estimator_B->Switch1[1416] * - est_estimator_B->Switch1[1416] + est_estimator_B->Switch1[1534] * - est_estimator_B->Switch1[1534]) + est_estimator_B->Switch1[1652] * - est_estimator_B->Switch1[1652])); - - // Switch: '/Switch2' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant3' - // S-Function (sfix_bitop): '/Bitwise Operator' - // Sum: '/Sum' - // UnitDelay: '/Unit Delay' - - if ((int32_T)((int32_T)rtb_Merge2.kfl_status & (int32_T) - est_estimator_P->BitwiseOperator_BitMask_l) != 0) { - num_augs = est_estimator_P->Constant2_Value_h; - } else { - num_augs = est_estimator_DW->UnitDelay_DSTATE_j + - est_estimator_P->Constant3_Value_p3; - } - - // End of Switch: '/Switch2' - - // Saturate: '/Saturation' - if (num_augs > (real_T)est_estimator_P->tun_ase_acquired_ticks) { - num_augs = (real_T)est_estimator_P->tun_ase_acquired_ticks; - } else { - if (num_augs < est_estimator_P->Saturation_LowerSat) { - num_augs = est_estimator_P->Saturation_LowerSat; - } - } - - // End of Saturate: '/Saturation' - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare = (num_augs >= (real_T)est_estimator_P->tun_ase_acquired_ticks); - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // Logic: '/Logical Operator4' - // Logic: '/Logical Operator5' - // Logic: '/Logical Operator6' - // Logic: '/Logical Operator7' - // Logic: '/Logical Operator8' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // Switch: '/Switch' - // Switch: '/Switch3' - - if ((rtb_Switch1 < est_estimator_P->tun_ase_converged_thresh) && - ((rtb_Merge2.confidence == est_estimator_P->ase_status_diverged) || - ((rtb_Merge2.confidence == est_estimator_P->ase_status_acquiring) && - (!rtb_Compare)))) { - rtb_Switch_m = est_estimator_P->ase_status_converged; - } else if (((rtb_Switch1 > est_estimator_P->tun_ase_diverged_thresh) || - (!((!rtIsNaNF(rtb_Switch1)) && (!rtIsInfF(rtb_Switch1))))) && - ((rtb_Merge2.confidence == est_estimator_P->ase_status_converged) || - (rtb_Merge2.confidence == est_estimator_P->ase_status_acquiring))) - { - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - - rtb_Switch_m = est_estimator_P->ase_status_diverged; - } else if ((rtb_Merge2.confidence == est_estimator_P->ase_status_converged) && - rtb_Compare) { - // Switch: '/Switch3' incorporates: - // Constant: '/Constant4' - // Switch: '/Switch' - - rtb_Switch_m = est_estimator_P->ase_status_acquiring; - } else { - // Switch: '/Switch' incorporates: - // Switch: '/Switch3' - - rtb_Switch_m = rtb_Merge2.confidence; - } - - // End of Switch: '/Switch1' - - // BusAssignment: '/Bus Assignment' - UnitDelay_DSTATE_n_cov_diag[0] = est_estimator_B->Switch1[0]; - UnitDelay_DSTATE_n_cov_diag[1] = est_estimator_B->Switch1[118]; - UnitDelay_DSTATE_n_cov_diag[2] = est_estimator_B->Switch1[236]; - UnitDelay_DSTATE_n_cov_diag[3] = est_estimator_B->Switch1[354]; - UnitDelay_DSTATE_n_cov_diag[4] = est_estimator_B->Switch1[472]; - UnitDelay_DSTATE_n_cov_diag[5] = est_estimator_B->Switch1[590]; - UnitDelay_DSTATE_n_cov_diag[6] = est_estimator_B->Switch1[708]; - UnitDelay_DSTATE_n_cov_diag[7] = est_estimator_B->Switch1[826]; - UnitDelay_DSTATE_n_cov_diag[8] = est_estimator_B->Switch1[944]; - UnitDelay_DSTATE_n_cov_diag[9] = est_estimator_B->Switch1[1062]; - UnitDelay_DSTATE_n_cov_diag[10] = est_estimator_B->Switch1[1180]; - UnitDelay_DSTATE_n_cov_diag[11] = est_estimator_B->Switch1[1298]; - UnitDelay_DSTATE_n_cov_diag[12] = est_estimator_B->Switch1[1416]; - UnitDelay_DSTATE_n_cov_diag[13] = est_estimator_B->Switch1[1534]; - UnitDelay_DSTATE_n_cov_diag[14] = est_estimator_B->Switch1[1652]; - UnitDelay_DSTATE_n_cov_diag[15] = est_estimator_B->Switch1[1770]; - UnitDelay_DSTATE_n_cov_diag[16] = est_estimator_B->Switch1[1888]; - UnitDelay_DSTATE_n_cov_diag[17] = est_estimator_B->Switch1[2006]; - UnitDelay_DSTATE_n_cov_diag[18] = est_estimator_B->Switch1[2124]; - UnitDelay_DSTATE_n_cov_diag[19] = est_estimator_B->Switch1[2242]; - UnitDelay_DSTATE_n_cov_diag[20] = est_estimator_B->Switch1[2360]; - UnitDelay_DSTATE_n_cov_diag[21] = est_estimator_B->Switch1[2478]; - UnitDelay_DSTATE_n_cov_diag[22] = est_estimator_B->Switch1[2596]; - UnitDelay_DSTATE_n_cov_diag[23] = est_estimator_B->Switch1[2714]; - UnitDelay_DSTATE_n_cov_diag[24] = est_estimator_B->Switch1[2832]; - UnitDelay_DSTATE_n_cov_diag[25] = est_estimator_B->Switch1[2950]; - UnitDelay_DSTATE_n_cov_diag[26] = est_estimator_B->Switch1[3068]; - UnitDelay_DSTATE_n_cov_diag[27] = est_estimator_B->Switch1[3186]; - UnitDelay_DSTATE_n_cov_diag[28] = est_estimator_B->Switch1[3304]; - UnitDelay_DSTATE_n_cov_diag[29] = est_estimator_B->Switch1[3422]; - UnitDelay_DSTATE_n_cov_diag[30] = est_estimator_B->Switch1[3540]; - UnitDelay_DSTATE_n_cov_diag[31] = est_estimator_B->Switch1[3658]; - UnitDelay_DSTATE_n_cov_diag[32] = est_estimator_B->Switch1[3776]; - UnitDelay_DSTATE_n_cov_diag[33] = est_estimator_B->Switch1[3894]; - UnitDelay_DSTATE_n_cov_diag[34] = est_estimator_B->Switch1[4012]; - UnitDelay_DSTATE_n_cov_diag[35] = est_estimator_B->Switch1[4130]; - UnitDelay_DSTATE_n_cov_diag[36] = est_estimator_B->Switch1[4248]; - UnitDelay_DSTATE_n_cov_diag[37] = est_estimator_B->Switch1[4366]; - UnitDelay_DSTATE_n_cov_diag[38] = est_estimator_B->Switch1[4484]; - UnitDelay_DSTATE_n_cov_diag[39] = est_estimator_B->Switch1[4602]; - UnitDelay_DSTATE_n_cov_diag[40] = est_estimator_B->Switch1[4720]; - UnitDelay_DSTATE_n_cov_diag[41] = est_estimator_B->Switch1[4838]; - UnitDelay_DSTATE_n_cov_diag[42] = est_estimator_B->Switch1[4956]; - UnitDelay_DSTATE_n_cov_diag[43] = est_estimator_B->Switch1[5074]; - UnitDelay_DSTATE_n_cov_diag[44] = est_estimator_B->Switch1[5192]; - UnitDelay_DSTATE_n_cov_diag[45] = est_estimator_B->Switch1[5310]; - UnitDelay_DSTATE_n_cov_diag[46] = est_estimator_B->Switch1[5428]; - UnitDelay_DSTATE_n_cov_diag[47] = est_estimator_B->Switch1[5546]; - UnitDelay_DSTATE_n_cov_diag[48] = est_estimator_B->Switch1[5664]; - UnitDelay_DSTATE_n_cov_diag[49] = est_estimator_B->Switch1[5782]; - UnitDelay_DSTATE_n_cov_diag[50] = est_estimator_B->Switch1[5900]; - UnitDelay_DSTATE_n_cov_diag[51] = est_estimator_B->Switch1[6018]; - UnitDelay_DSTATE_n_cov_diag[52] = est_estimator_B->Switch1[6136]; - UnitDelay_DSTATE_n_cov_diag[53] = est_estimator_B->Switch1[6254]; - UnitDelay_DSTATE_n_cov_diag[54] = est_estimator_B->Switch1[6372]; - UnitDelay_DSTATE_n_cov_diag[55] = est_estimator_B->Switch1[6490]; - UnitDelay_DSTATE_n_cov_diag[56] = est_estimator_B->Switch1[6608]; - UnitDelay_DSTATE_n_cov_diag[57] = est_estimator_B->Switch1[6726]; - UnitDelay_DSTATE_n_cov_diag[58] = est_estimator_B->Switch1[6844]; - UnitDelay_DSTATE_n_cov_diag[59] = est_estimator_B->Switch1[6962]; - UnitDelay_DSTATE_n_cov_diag[60] = est_estimator_B->Switch1[7080]; - UnitDelay_DSTATE_n_cov_diag[61] = est_estimator_B->Switch1[7198]; - UnitDelay_DSTATE_n_cov_diag[62] = est_estimator_B->Switch1[7316]; - UnitDelay_DSTATE_n_cov_diag[63] = est_estimator_B->Switch1[7434]; - UnitDelay_DSTATE_n_cov_diag[64] = est_estimator_B->Switch1[7552]; - UnitDelay_DSTATE_n_cov_diag[65] = est_estimator_B->Switch1[7670]; - UnitDelay_DSTATE_n_cov_diag[66] = est_estimator_B->Switch1[7788]; - UnitDelay_DSTATE_n_cov_diag[67] = est_estimator_B->Switch1[7906]; - UnitDelay_DSTATE_n_cov_diag[68] = est_estimator_B->Switch1[8024]; - UnitDelay_DSTATE_n_cov_diag[69] = est_estimator_B->Switch1[8142]; - UnitDelay_DSTATE_n_cov_diag[70] = est_estimator_B->Switch1[8260]; - UnitDelay_DSTATE_n_cov_diag[71] = est_estimator_B->Switch1[8378]; - UnitDelay_DSTATE_n_cov_diag[72] = est_estimator_B->Switch1[8496]; - UnitDelay_DSTATE_n_cov_diag[73] = est_estimator_B->Switch1[8614]; - UnitDelay_DSTATE_n_cov_diag[74] = est_estimator_B->Switch1[8732]; - UnitDelay_DSTATE_n_cov_diag[75] = est_estimator_B->Switch1[8850]; - UnitDelay_DSTATE_n_cov_diag[76] = est_estimator_B->Switch1[8968]; - UnitDelay_DSTATE_n_cov_diag[77] = est_estimator_B->Switch1[9086]; - UnitDelay_DSTATE_n_cov_diag[78] = est_estimator_B->Switch1[9204]; - UnitDelay_DSTATE_n_cov_diag[79] = est_estimator_B->Switch1[9322]; - UnitDelay_DSTATE_n_cov_diag[80] = est_estimator_B->Switch1[9440]; - UnitDelay_DSTATE_n_cov_diag[81] = est_estimator_B->Switch1[9558]; - UnitDelay_DSTATE_n_cov_diag[82] = est_estimator_B->Switch1[9676]; - UnitDelay_DSTATE_n_cov_diag[83] = est_estimator_B->Switch1[9794]; - UnitDelay_DSTATE_n_cov_diag[84] = est_estimator_B->Switch1[9912]; - UnitDelay_DSTATE_n_cov_diag[85] = est_estimator_B->Switch1[10030]; - UnitDelay_DSTATE_n_cov_diag[86] = est_estimator_B->Switch1[10148]; - UnitDelay_DSTATE_n_cov_diag[87] = est_estimator_B->Switch1[10266]; - UnitDelay_DSTATE_n_cov_diag[88] = est_estimator_B->Switch1[10384]; - UnitDelay_DSTATE_n_cov_diag[89] = est_estimator_B->Switch1[10502]; - UnitDelay_DSTATE_n_cov_diag[90] = est_estimator_B->Switch1[10620]; - UnitDelay_DSTATE_n_cov_diag[91] = est_estimator_B->Switch1[10738]; - UnitDelay_DSTATE_n_cov_diag[92] = est_estimator_B->Switch1[10856]; - UnitDelay_DSTATE_n_cov_diag[93] = est_estimator_B->Switch1[10974]; - UnitDelay_DSTATE_n_cov_diag[94] = est_estimator_B->Switch1[11092]; - UnitDelay_DSTATE_n_cov_diag[95] = est_estimator_B->Switch1[11210]; - UnitDelay_DSTATE_n_cov_diag[96] = est_estimator_B->Switch1[11328]; - UnitDelay_DSTATE_n_cov_diag[97] = est_estimator_B->Switch1[11446]; - UnitDelay_DSTATE_n_cov_diag[98] = est_estimator_B->Switch1[11564]; - UnitDelay_DSTATE_n_cov_diag[99] = est_estimator_B->Switch1[11682]; - UnitDelay_DSTATE_n_cov_diag[100] = est_estimator_B->Switch1[11800]; - UnitDelay_DSTATE_n_cov_diag[101] = est_estimator_B->Switch1[11918]; - UnitDelay_DSTATE_n_cov_diag[102] = est_estimator_B->Switch1[12036]; - UnitDelay_DSTATE_n_cov_diag[103] = est_estimator_B->Switch1[12154]; - UnitDelay_DSTATE_n_cov_diag[104] = est_estimator_B->Switch1[12272]; - UnitDelay_DSTATE_n_cov_diag[105] = est_estimator_B->Switch1[12390]; - UnitDelay_DSTATE_n_cov_diag[106] = est_estimator_B->Switch1[12508]; - UnitDelay_DSTATE_n_cov_diag[107] = est_estimator_B->Switch1[12626]; - UnitDelay_DSTATE_n_cov_diag[108] = est_estimator_B->Switch1[12744]; - UnitDelay_DSTATE_n_cov_diag[109] = est_estimator_B->Switch1[12862]; - UnitDelay_DSTATE_n_cov_diag[110] = est_estimator_B->Switch1[12980]; - UnitDelay_DSTATE_n_cov_diag[111] = est_estimator_B->Switch1[13098]; - UnitDelay_DSTATE_n_cov_diag[112] = est_estimator_B->Switch1[13216]; - UnitDelay_DSTATE_n_cov_diag[113] = est_estimator_B->Switch1[13334]; - UnitDelay_DSTATE_n_cov_diag[114] = est_estimator_B->Switch1[13452]; - UnitDelay_DSTATE_n_cov_diag[115] = est_estimator_B->Switch1[13570]; - UnitDelay_DSTATE_n_cov_diag[116] = est_estimator_B->Switch1[13688]; - - // Sum: '/Sum' incorporates: - // BusAssignment: '/Bus Assignment' - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Switch1 = rtb_Merge2.quat_ISS2B[3] * rtb_Merge2.quat_ISS2B[3] * - est_estimator_P->Gain_Gain_h3 - (real32_T) - est_estimator_P->Constant1_Value_ec; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_d[i]; - } - - rtb_Assignment_b[0] = rtb_Switch1; - rtb_Assignment_b[4] = rtb_Switch1; - rtb_Assignment_b[8] = rtb_Switch1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // BusAssignment: '/Bus Assignment' - - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_jb * rtb_Merge2.quat_ISS2B[3]; - - // Product: '/Product' incorporates: - // BusAssignment: '/Bus Assignment' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_f[0] = (real32_T)est_estimator_P->Constant3_Value_ol; - tmp_f[1] = rtb_Merge2.quat_ISS2B[2]; - tmp_f[2] = est_estimator_P->Gain_Gain_j2 * rtb_Merge2.quat_ISS2B[1]; - tmp_f[3] = est_estimator_P->Gain1_Gain_jp * rtb_Merge2.quat_ISS2B[2]; - tmp_f[4] = (real32_T)est_estimator_P->Constant3_Value_ol; - tmp_f[5] = rtb_Merge2.quat_ISS2B[0]; - tmp_f[6] = rtb_Merge2.quat_ISS2B[1]; - tmp_f[7] = est_estimator_P->Gain2_Gain_ko * rtb_Merge2.quat_ISS2B[0]; - tmp_f[8] = (real32_T)est_estimator_P->Constant3_Value_ol; - - // Math: '/Math Function' incorporates: - // BusAssignment: '/Bus Assignment' - // Gain: '/Gain2' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Merge2_1[i] = rtb_Merge2.quat_ISS2B[0] * rtb_Merge2.quat_ISS2B[i]; - rtb_Merge2_1[(int32_T)(i + 3)] = rtb_Merge2.quat_ISS2B[1] * - rtb_Merge2.quat_ISS2B[i]; - rtb_Merge2_1[(int32_T)(i + 6)] = rtb_Merge2.quat_ISS2B[2] * - rtb_Merge2.quat_ISS2B[i]; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_n[(int32_T)(3 * i)] = (rtb_Assignment_b[i] - rtb_Sum_k1 * - tmp_f[i]) + rtb_Merge2_1[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_a2; - rtb_Assignment_n[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 3)] - tmp_f[(int32_T)(i + 3)] * rtb_Sum_k1) + rtb_Merge2_1 - [(int32_T)((int32_T)(3 * i) + 1)] * est_estimator_P->Gain2_Gain_a2; - rtb_Assignment_n[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 6)] - tmp_f[(int32_T)(i + 6)] * rtb_Sum_k1) + rtb_Merge2_1 - [(int32_T)((int32_T)(3 * i) + 2)] * est_estimator_P->Gain2_Gain_a2; - } - - // End of Sum: '/Sum1' - for (i = 0; i < 3; i++) { - // Sum: '/Sum1' incorporates: - // BusAssignment: '/Bus Assignment' - // Constant: '/Constant5' - // Product: '/Product' - - rtb_Sum1_k3[i] = rtb_Merge2.P_EST_ISS_ISS[i] - ((rtb_Assignment_n[(int32_T) - (i + 3)] * est_estimator_P->tun_abp_p_imu_body_body[1] + - rtb_Assignment_n[i] * est_estimator_P->tun_abp_p_imu_body_body[0]) + - rtb_Assignment_n[(int32_T)(i + 6)] * - est_estimator_P->tun_abp_p_imu_body_body[2]); - - // Update for UnitDelay: '/Unit Delay20' - est_estimator_DW->UnitDelay20_DSTATE[i] = rtb_ml_vel_aug[i]; - - // Update for UnitDelay: '/Unit Delay21' - est_estimator_DW->UnitDelay21_DSTATE[i] = rtb_ml_omega_aug[i]; - } - - // Update for UnitDelay: '/Unit Delay22' - memcpy(&est_estimator_DW->UnitDelay22_DSTATE[0], &rtb_of_vel_aug[0], (uint32_T) - (48U * sizeof(real_T))); - - // Update for UnitDelay: '/Unit Delay23' - memcpy(&est_estimator_DW->UnitDelay23_DSTATE[0], &rtb_of_omega_aug[0], - (uint32_T)(48U * sizeof(real_T))); - - // Update for Delay: '/Delay' - est_estimator_DW->icLoad = 0U; - memcpy(&est_estimator_DW->Delay_DSTATE[0], &est_estimator_B->Switch1[0], - (uint32_T)(13689U * sizeof(ase_cov_datatype))); - - // Update for UnitDelay: '/Unit Delay1' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay1_DSTATE[0] = rtb_Merge2.quat_ISS2B[0]; - est_estimator_DW->UnitDelay1_DSTATE[1] = rtb_Merge2.quat_ISS2B[1]; - est_estimator_DW->UnitDelay1_DSTATE[2] = rtb_Merge2.quat_ISS2B[2]; - est_estimator_DW->UnitDelay1_DSTATE[3] = rtb_Merge2.quat_ISS2B[3]; - - // Update for UnitDelay: '/Unit Delay3' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay3_DSTATE[0] = rtb_Merge2.gyro_bias[0]; - - // Update for UnitDelay: '/Unit Delay4' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay4_DSTATE[0] = rtb_Merge2.V_B_ISS_ISS[0]; - - // Update for UnitDelay: '/Unit Delay6' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay6_DSTATE[0] = rtb_Merge2.accel_bias[0]; - - // Update for UnitDelay: '/Unit Delay7' incorporates: - // BusAssignment: '/Bus Assignment1' - - est_estimator_DW->UnitDelay7_DSTATE[0] = rtb_Sum1_k3[0]; - - // Update for UnitDelay: '/Unit Delay3' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay3_DSTATE[1] = rtb_Merge2.gyro_bias[1]; - - // Update for UnitDelay: '/Unit Delay4' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay4_DSTATE[1] = rtb_Merge2.V_B_ISS_ISS[1]; - - // Update for UnitDelay: '/Unit Delay6' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay6_DSTATE[1] = rtb_Merge2.accel_bias[1]; - - // Update for UnitDelay: '/Unit Delay7' incorporates: - // BusAssignment: '/Bus Assignment1' - - est_estimator_DW->UnitDelay7_DSTATE[1] = rtb_Sum1_k3[1]; - - // Update for UnitDelay: '/Unit Delay3' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay3_DSTATE[2] = rtb_Merge2.gyro_bias[2]; - - // Update for UnitDelay: '/Unit Delay4' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay4_DSTATE[2] = rtb_Merge2.V_B_ISS_ISS[2]; - - // Update for UnitDelay: '/Unit Delay6' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay6_DSTATE[2] = rtb_Merge2.accel_bias[2]; - - // Update for UnitDelay: '/Unit Delay7' incorporates: - // BusAssignment: '/Bus Assignment1' - - est_estimator_DW->UnitDelay7_DSTATE[2] = rtb_Sum1_k3[2]; - - // Update for UnitDelay: '/Unit Delay8' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay8_DSTATE = rtb_Switch_m; - - // Update for UnitDelay: '/Unit Delay9' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay9_DSTATE = rtb_Merge2.aug_state_enum; - - // Update for UnitDelay: '/Unit Delay10' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay10_DSTATE[0] = rtb_Merge2.ml_quat_ISS2cam[0]; - est_estimator_DW->UnitDelay10_DSTATE[1] = rtb_Merge2.ml_quat_ISS2cam[1]; - est_estimator_DW->UnitDelay10_DSTATE[2] = rtb_Merge2.ml_quat_ISS2cam[2]; - est_estimator_DW->UnitDelay10_DSTATE[3] = rtb_Merge2.ml_quat_ISS2cam[3]; - - // Update for UnitDelay: '/Unit Delay11' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay11_DSTATE[0] = rtb_Merge2.ml_P_cam_ISS_ISS[0]; - est_estimator_DW->UnitDelay11_DSTATE[1] = rtb_Merge2.ml_P_cam_ISS_ISS[1]; - est_estimator_DW->UnitDelay11_DSTATE[2] = rtb_Merge2.ml_P_cam_ISS_ISS[2]; - - // Update for UnitDelay: '/Unit Delay12' incorporates: - // BusAssignment: '/Bus Assignment' - - memcpy(&est_estimator_DW->UnitDelay12_DSTATE[0], &rtb_Merge2.of_quat_ISS2cam[0], - (uint32_T)(sizeof(real32_T) << 6U)); - - // Update for UnitDelay: '/Unit Delay13' incorporates: - // BusAssignment: '/Bus Assignment' - - memcpy(&est_estimator_DW->UnitDelay13_DSTATE[0], &rtb_Merge2.of_P_cam_ISS_ISS - [0], (uint32_T)(48U * sizeof(real32_T))); - - // Update for UnitDelay: '/Unit Delay14' incorporates: - // BusAssignment: '/Bus Assignment1' - - memcpy(&est_estimator_DW->UnitDelay14_DSTATE[0], &UnitDelay_DSTATE_n_cov_diag - [0], (uint32_T)(117U * sizeof(ase_cov_datatype))); - - // Update for UnitDelay: '/Unit Delay15' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay15_DSTATE = rtb_Merge2.kfl_status; - - // Update for UnitDelay: '/Unit Delay16' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay16_DSTATE = rtb_Merge2.update_OF_tracks_cnt; - - // Update for UnitDelay: '/Unit Delay17' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay17_DSTATE = rtb_Merge2.update_ML_features_cnt; - - // Update for UnitDelay: '/Unit Delay18' incorporates: - // BusAssignment: '/Bus Assignment' - - memcpy(&est_estimator_DW->UnitDelay18_DSTATE[0], - &rtb_Merge2.of_mahal_distance[0], (uint32_T)(50U * sizeof(real_T))); - - // Update for UnitDelay: '/Unit Delay19' incorporates: - // BusAssignment: '/Bus Assignment' - - memcpy(&est_estimator_DW->UnitDelay19_DSTATE[0], - &rtb_Merge2.ml_mahal_distance[0], (uint32_T)(50U * sizeof(real_T))); - - // Update for UnitDelay: '/Unit Delay24' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay24_DSTATE[0] = rtb_Merge2.hr_P_hr_ISS_ISS[0]; - est_estimator_DW->UnitDelay24_DSTATE[1] = rtb_Merge2.hr_P_hr_ISS_ISS[1]; - est_estimator_DW->UnitDelay24_DSTATE[2] = rtb_Merge2.hr_P_hr_ISS_ISS[2]; - - // Update for UnitDelay: '/Unit Delay25' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay25_DSTATE[0] = rtb_Merge2.hr_quat_ISS2hr[0]; - est_estimator_DW->UnitDelay25_DSTATE[1] = rtb_Merge2.hr_quat_ISS2hr[1]; - est_estimator_DW->UnitDelay25_DSTATE[2] = rtb_Merge2.hr_quat_ISS2hr[2]; - est_estimator_DW->UnitDelay25_DSTATE[3] = rtb_Merge2.hr_quat_ISS2hr[3]; - - // Update for UnitDelay: '/Unit Delay26' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay26_DSTATE[0] = rtb_Merge2.P_EST_ISS_ISS[0]; - est_estimator_DW->UnitDelay26_DSTATE[1] = rtb_Merge2.P_EST_ISS_ISS[1]; - est_estimator_DW->UnitDelay26_DSTATE[2] = rtb_Merge2.P_EST_ISS_ISS[2]; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/handrail_msg' - - est_estimator_DW->DelayInput1_DSTATE = - est_estimator_U_handrail_msg->cvs_timestamp_sec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/handrail_msg' - - est_estimator_DW->DelayInput1_DSTATE_h = - est_estimator_U_handrail_msg->cvs_timestamp_nsec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Inport: '/cmc_msg' - // SignalConversion: '/Signal Conversion' - - est_estimator_DW->DelayInput1_DSTATE_k = - est_estimator_U_cmc_msg_o->localization_mode_cmd; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/landmark_msg' - - est_estimator_DW->DelayInput1_DSTATE_o = - est_estimator_U_landmark_msg->cvs_timestamp_sec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/landmark_msg' - - est_estimator_DW->DelayInput1_DSTATE_n = - est_estimator_U_landmark_msg->cvs_timestamp_nsec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/optical_flow_msg' - - est_estimator_DW->DelayInput1_DSTATE_b = - est_estimator_U_cvs_optical_flow_msg_n->cvs_timestamp_sec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/optical_flow_msg' - - est_estimator_DW->DelayInput1_DSTATE_d = - est_estimator_U_cvs_optical_flow_msg_n->cvs_timestamp_nsec; - - // Update for UnitDelay: '/Unit Delay' - est_estimator_DW->UnitDelay_DSTATE_j = num_augs; - - // Outport: '/kfl_msg' incorporates: - // BusAssignment: '/Bus Assignment' - // BusAssignment: '/Bus Assignment1' - - est_estimator_Y_kfl_msg_h->quat_ISS2B[0] = rtb_Merge2.quat_ISS2B[0]; - est_estimator_Y_kfl_msg_h->quat_ISS2B[1] = rtb_Merge2.quat_ISS2B[1]; - est_estimator_Y_kfl_msg_h->quat_ISS2B[2] = rtb_Merge2.quat_ISS2B[2]; - est_estimator_Y_kfl_msg_h->quat_ISS2B[3] = rtb_Merge2.quat_ISS2B[3]; - est_estimator_Y_kfl_msg_h->omega_B_ISS_B[0] = rtb_Merge2.omega_B_ISS_B[0]; - est_estimator_Y_kfl_msg_h->gyro_bias[0] = rtb_Merge2.gyro_bias[0]; - est_estimator_Y_kfl_msg_h->V_B_ISS_ISS[0] = rtb_Merge2.V_B_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->A_B_ISS_ISS[0] = rtb_Merge2.A_B_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->accel_bias[0] = rtb_Merge2.accel_bias[0]; - est_estimator_Y_kfl_msg_h->P_B_ISS_ISS[0] = rtb_Sum1_k3[0]; - est_estimator_Y_kfl_msg_h->omega_B_ISS_B[1] = rtb_Merge2.omega_B_ISS_B[1]; - est_estimator_Y_kfl_msg_h->gyro_bias[1] = rtb_Merge2.gyro_bias[1]; - est_estimator_Y_kfl_msg_h->V_B_ISS_ISS[1] = rtb_Merge2.V_B_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->A_B_ISS_ISS[1] = rtb_Merge2.A_B_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->accel_bias[1] = rtb_Merge2.accel_bias[1]; - est_estimator_Y_kfl_msg_h->P_B_ISS_ISS[1] = rtb_Sum1_k3[1]; - est_estimator_Y_kfl_msg_h->omega_B_ISS_B[2] = rtb_Merge2.omega_B_ISS_B[2]; - est_estimator_Y_kfl_msg_h->gyro_bias[2] = rtb_Merge2.gyro_bias[2]; - est_estimator_Y_kfl_msg_h->V_B_ISS_ISS[2] = rtb_Merge2.V_B_ISS_ISS[2]; - est_estimator_Y_kfl_msg_h->A_B_ISS_ISS[2] = rtb_Merge2.A_B_ISS_ISS[2]; - est_estimator_Y_kfl_msg_h->accel_bias[2] = rtb_Merge2.accel_bias[2]; - est_estimator_Y_kfl_msg_h->P_B_ISS_ISS[2] = rtb_Sum1_k3[2]; - est_estimator_Y_kfl_msg_h->confidence = rtb_Switch_m; - est_estimator_Y_kfl_msg_h->aug_state_enum = rtb_Merge2.aug_state_enum; - est_estimator_Y_kfl_msg_h->ml_quat_ISS2cam[0] = rtb_Merge2.ml_quat_ISS2cam[0]; - est_estimator_Y_kfl_msg_h->ml_quat_ISS2cam[1] = rtb_Merge2.ml_quat_ISS2cam[1]; - est_estimator_Y_kfl_msg_h->ml_quat_ISS2cam[2] = rtb_Merge2.ml_quat_ISS2cam[2]; - est_estimator_Y_kfl_msg_h->ml_quat_ISS2cam[3] = rtb_Merge2.ml_quat_ISS2cam[3]; - est_estimator_Y_kfl_msg_h->ml_P_cam_ISS_ISS[0] = rtb_Merge2.ml_P_cam_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->ml_P_cam_ISS_ISS[1] = rtb_Merge2.ml_P_cam_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->ml_P_cam_ISS_ISS[2] = rtb_Merge2.ml_P_cam_ISS_ISS[2]; - memcpy(&est_estimator_Y_kfl_msg_h->of_quat_ISS2cam[0], - &rtb_Merge2.of_quat_ISS2cam[0], (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&est_estimator_Y_kfl_msg_h->of_P_cam_ISS_ISS[0], - &rtb_Merge2.of_P_cam_ISS_ISS[0], (uint32_T)(48U * sizeof(real32_T))); - memcpy(&est_estimator_Y_kfl_msg_h->cov_diag[0], &UnitDelay_DSTATE_n_cov_diag[0], - (uint32_T)(117U * sizeof(ase_cov_datatype))); - est_estimator_Y_kfl_msg_h->kfl_status = rtb_Merge2.kfl_status; - est_estimator_Y_kfl_msg_h->update_OF_tracks_cnt = - rtb_Merge2.update_OF_tracks_cnt; - est_estimator_Y_kfl_msg_h->update_ML_features_cnt = - rtb_Merge2.update_ML_features_cnt; - memcpy(&est_estimator_Y_kfl_msg_h->of_mahal_distance[0], - &rtb_Merge2.of_mahal_distance[0], (uint32_T)(50U * sizeof(real_T))); - memcpy(&est_estimator_Y_kfl_msg_h->ml_mahal_distance[0], - &rtb_Merge2.ml_mahal_distance[0], (uint32_T)(50U * sizeof(real_T))); - est_estimator_Y_kfl_msg_h->hr_P_hr_ISS_ISS[0] = rtb_Merge2.hr_P_hr_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->hr_P_hr_ISS_ISS[1] = rtb_Merge2.hr_P_hr_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->hr_P_hr_ISS_ISS[2] = rtb_Merge2.hr_P_hr_ISS_ISS[2]; - est_estimator_Y_kfl_msg_h->hr_quat_ISS2hr[0] = rtb_Merge2.hr_quat_ISS2hr[0]; - est_estimator_Y_kfl_msg_h->hr_quat_ISS2hr[1] = rtb_Merge2.hr_quat_ISS2hr[1]; - est_estimator_Y_kfl_msg_h->hr_quat_ISS2hr[2] = rtb_Merge2.hr_quat_ISS2hr[2]; - est_estimator_Y_kfl_msg_h->hr_quat_ISS2hr[3] = rtb_Merge2.hr_quat_ISS2hr[3]; - est_estimator_Y_kfl_msg_h->P_EST_ISS_ISS[0] = rtb_Merge2.P_EST_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->P_EST_ISS_ISS[1] = rtb_Merge2.P_EST_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->P_EST_ISS_ISS[2] = rtb_Merge2.P_EST_ISS_ISS[2]; - - // End of Outputs for SubSystem: '/est_estimator' - - // Outport: '/P_out' - memcpy(&est_estimator_Y_P_out[0], &est_estimator_B->Switch1[0], (uint32_T) - (13689U * sizeof(ase_cov_datatype))); -} - -// Model initialize function -void est_estimator_initialize(RT_MODEL_est_estimator_T *const est_estimator_M, - cvs_landmark_msg *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]) -{ - P_est_estimator_T *est_estimator_P = ((P_est_estimator_T *) - est_estimator_M->defaultParam); - B_est_estimator_T *est_estimator_B = ((B_est_estimator_T *) - est_estimator_M->blockIO); - DW_est_estimator_T *est_estimator_DW = ((DW_est_estimator_T *) - est_estimator_M->dwork); - - { - // local scratch DWork variables - int32_T ForEach_itr; - int32_T ForEach_itr_g; - int32_T i; - - // Start for Atomic SubSystem: '/est_estimator' - // Start for Iterator SubSystem: '/filter' - for (ForEach_itr_g = 0; ForEach_itr_g < 3; ForEach_itr_g++) { - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states = 0.0F; - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0] = - 0.0F; - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1] = - 0.0F; - - // Start for MATLAB Function: '/MATLAB Function1' - est_estim_MATLABFunction1_Start(&est_estimator_B->CoreSubsys[ForEach_itr_g] - .sf_MATLABFunction1); - } - - // End of Start for SubSystem: '/filter' - - // Start for Iterator SubSystem: '/filter_with_HP_filter' - for (ForEach_itr = 0; ForEach_itr < 3; ForEach_itr++) { - est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states = 0.0F; - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0] = - 0.0F; - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1] = - 0.0F; - est_estimator_DW->CoreSubsys_l[ForEach_itr].HighPassFilter_states = 0.0F; - - // Start for MATLAB Function: '/MATLAB Function1' - est_estim_MATLABFunction1_Start(&est_estimator_B->CoreSubsys_l[ForEach_itr] - .sf_MATLABFunction1); - } - - // End of Start for SubSystem: '/filter_with_HP_filter' - // End of Start for SubSystem: '/est_estimator' - - // SystemInitialize for Atomic SubSystem: '/est_estimator' - // InitializeConditions for UnitDelay: '/Unit Delay20' - est_estimator_DW->UnitDelay20_DSTATE[0] = - est_estimator_P->UnitDelay20_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay21' - est_estimator_DW->UnitDelay21_DSTATE[0] = - est_estimator_P->UnitDelay21_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay20' - est_estimator_DW->UnitDelay20_DSTATE[1] = - est_estimator_P->UnitDelay20_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay21' - est_estimator_DW->UnitDelay21_DSTATE[1] = - est_estimator_P->UnitDelay21_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay20' - est_estimator_DW->UnitDelay20_DSTATE[2] = - est_estimator_P->UnitDelay20_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay21' - est_estimator_DW->UnitDelay21_DSTATE[2] = - est_estimator_P->UnitDelay21_InitialCondition; - for (i = 0; i < 48; i++) { - // InitializeConditions for UnitDelay: '/Unit Delay22' - est_estimator_DW->UnitDelay22_DSTATE[i] = - est_estimator_P->UnitDelay22_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay23' - est_estimator_DW->UnitDelay23_DSTATE[i] = - est_estimator_P->UnitDelay23_InitialCondition; - } - - // InitializeConditions for Delay: '/Delay' - est_estimator_DW->icLoad = 1U; - - // InitializeConditions for UnitDelay: '/Unit Delay1' - est_estimator_DW->UnitDelay1_DSTATE[0] = - est_estimator_P->tun_ase_state_ic_quat_ISS2B[0]; - est_estimator_DW->UnitDelay1_DSTATE[1] = - est_estimator_P->tun_ase_state_ic_quat_ISS2B[1]; - est_estimator_DW->UnitDelay1_DSTATE[2] = - est_estimator_P->tun_ase_state_ic_quat_ISS2B[2]; - est_estimator_DW->UnitDelay1_DSTATE[3] = - est_estimator_P->tun_ase_state_ic_quat_ISS2B[3]; - - // InitializeConditions for UnitDelay: '/Unit Delay3' - est_estimator_DW->UnitDelay3_DSTATE[0] = - est_estimator_P->ase_state_ic_gyro_bias[0]; - - // InitializeConditions for UnitDelay: '/Unit Delay4' - est_estimator_DW->UnitDelay4_DSTATE[0] = - est_estimator_P->tun_ase_state_ic_V_B_ISS_ISS[0]; - - // InitializeConditions for UnitDelay: '/Unit Delay6' - est_estimator_DW->UnitDelay6_DSTATE[0] = - est_estimator_P->ase_state_ic_accel_bias[0]; - - // InitializeConditions for UnitDelay: '/Unit Delay7' - est_estimator_DW->UnitDelay7_DSTATE[0] = - est_estimator_P->tun_ase_state_ic_P_B_ISS_ISS[0]; - - // InitializeConditions for UnitDelay: '/Unit Delay3' - est_estimator_DW->UnitDelay3_DSTATE[1] = - est_estimator_P->ase_state_ic_gyro_bias[1]; - - // InitializeConditions for UnitDelay: '/Unit Delay4' - est_estimator_DW->UnitDelay4_DSTATE[1] = - est_estimator_P->tun_ase_state_ic_V_B_ISS_ISS[1]; - - // InitializeConditions for UnitDelay: '/Unit Delay6' - est_estimator_DW->UnitDelay6_DSTATE[1] = - est_estimator_P->ase_state_ic_accel_bias[1]; - - // InitializeConditions for UnitDelay: '/Unit Delay7' - est_estimator_DW->UnitDelay7_DSTATE[1] = - est_estimator_P->tun_ase_state_ic_P_B_ISS_ISS[1]; - - // InitializeConditions for UnitDelay: '/Unit Delay3' - est_estimator_DW->UnitDelay3_DSTATE[2] = - est_estimator_P->ase_state_ic_gyro_bias[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay4' - est_estimator_DW->UnitDelay4_DSTATE[2] = - est_estimator_P->tun_ase_state_ic_V_B_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay6' - est_estimator_DW->UnitDelay6_DSTATE[2] = - est_estimator_P->ase_state_ic_accel_bias[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay7' - est_estimator_DW->UnitDelay7_DSTATE[2] = - est_estimator_P->tun_ase_state_ic_P_B_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay8' - est_estimator_DW->UnitDelay8_DSTATE = - est_estimator_P->ase_state_ic_confidence; - - // InitializeConditions for UnitDelay: '/Unit Delay9' - est_estimator_DW->UnitDelay9_DSTATE = (uint32_T) - est_estimator_P->ase_state_ic_aug_state_enum; - - // InitializeConditions for UnitDelay: '/Unit Delay10' - est_estimator_DW->UnitDelay10_DSTATE[0] = - est_estimator_P->ase_state_ic_ml_quat_ISS2cam[0]; - est_estimator_DW->UnitDelay10_DSTATE[1] = - est_estimator_P->ase_state_ic_ml_quat_ISS2cam[1]; - est_estimator_DW->UnitDelay10_DSTATE[2] = - est_estimator_P->ase_state_ic_ml_quat_ISS2cam[2]; - est_estimator_DW->UnitDelay10_DSTATE[3] = - est_estimator_P->ase_state_ic_ml_quat_ISS2cam[3]; - - // InitializeConditions for UnitDelay: '/Unit Delay11' - est_estimator_DW->UnitDelay11_DSTATE[0] = - est_estimator_P->ase_state_ic_ml_P_cam_ISS_ISS[0]; - est_estimator_DW->UnitDelay11_DSTATE[1] = - est_estimator_P->ase_state_ic_ml_P_cam_ISS_ISS[1]; - est_estimator_DW->UnitDelay11_DSTATE[2] = - est_estimator_P->ase_state_ic_ml_P_cam_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay12' - memcpy(&est_estimator_DW->UnitDelay12_DSTATE[0], - &est_estimator_P->ase_state_ic_of_quat_ISS2cam[0], (uint32_T)(sizeof - (real32_T) << 6U)); - - // InitializeConditions for UnitDelay: '/Unit Delay13' - memcpy(&est_estimator_DW->UnitDelay13_DSTATE[0], - &est_estimator_P->ase_state_ic_of_P_cam_ISS_ISS[0], (uint32_T)(48U * - sizeof(real32_T))); - - // InitializeConditions for UnitDelay: '/Unit Delay14' - memcpy(&est_estimator_DW->UnitDelay14_DSTATE[0], - &est_estimator_P->ase_state_ic_cov_diag[0], (uint32_T)(117U * sizeof - (ase_cov_datatype))); - - // InitializeConditions for UnitDelay: '/Unit Delay15' - est_estimator_DW->UnitDelay15_DSTATE = (uint16_T) - est_estimator_P->ase_state_ic_status; - - // InitializeConditions for UnitDelay: '/Unit Delay16' - est_estimator_DW->UnitDelay16_DSTATE = - est_estimator_P->UnitDelay16_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay17' - est_estimator_DW->UnitDelay17_DSTATE = - est_estimator_P->UnitDelay17_InitialCondition; - for (i = 0; i < 50; i++) { - // InitializeConditions for UnitDelay: '/Unit Delay18' - est_estimator_DW->UnitDelay18_DSTATE[i] = - est_estimator_P->UnitDelay18_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay19' - est_estimator_DW->UnitDelay19_DSTATE[i] = - est_estimator_P->UnitDelay19_InitialCondition; - } - - // InitializeConditions for UnitDelay: '/Unit Delay24' - est_estimator_DW->UnitDelay24_DSTATE[0] = - est_estimator_P->UnitDelay24_InitialCondition; - est_estimator_DW->UnitDelay24_DSTATE[1] = - est_estimator_P->UnitDelay24_InitialCondition; - est_estimator_DW->UnitDelay24_DSTATE[2] = - est_estimator_P->UnitDelay24_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay25' - est_estimator_DW->UnitDelay25_DSTATE[0] = - est_estimator_P->UnitDelay25_InitialCondition; - est_estimator_DW->UnitDelay25_DSTATE[1] = - est_estimator_P->UnitDelay25_InitialCondition; - est_estimator_DW->UnitDelay25_DSTATE[2] = - est_estimator_P->UnitDelay25_InitialCondition; - est_estimator_DW->UnitDelay25_DSTATE[3] = - est_estimator_P->UnitDelay25_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay26' - est_estimator_DW->UnitDelay26_DSTATE[0] = - est_estimator_P->tun_ase_state_ic_P_EST_ISS_ISS[0]; - est_estimator_DW->UnitDelay26_DSTATE[1] = - est_estimator_P->tun_ase_state_ic_P_EST_ISS_ISS[1]; - est_estimator_DW->UnitDelay26_DSTATE[2] = - est_estimator_P->tun_ase_state_ic_P_EST_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE = est_estimator_P->DetectChange6_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_h = - est_estimator_P->DetectChange7_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_k = - est_estimator_P->DetectChange_vinit_i; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_o = est_estimator_P->DetectChange_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_n = - est_estimator_P->DetectChange1_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_b = - est_estimator_P->DetectChange4_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_d = - est_estimator_P->DetectChange5_vinit; - - // InitializeConditions for UnitDelay: '/Unit Delay' - est_estimator_DW->UnitDelay_DSTATE_j = - est_estimator_P->UnitDelay_InitialCondition_h; - - // SystemInitialize for Iterator SubSystem: '/filter' - for (ForEach_itr_g = 0; ForEach_itr_g < 3; ForEach_itr_g++) { - // InitializeConditions for DiscreteTransferFcn: '/3 Hz Low Pass' - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states = - est_estimator_P->CoreSubsys.uHzLowPass_InitialStates; - - // InitializeConditions for DiscreteTransferFcn: '/Discrete Transfer Fcn' - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0] = - est_estimator_P->CoreSubsys.DiscreteTransferFcn_InitialStat; - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1] = - est_estimator_P->CoreSubsys.DiscreteTransferFcn_InitialStat; - - // SystemInitialize for MATLAB Function: '/MATLAB Function1' - est_estima_MATLABFunction1_Init(&est_estimator_DW-> - CoreSubsys[ForEach_itr_g].sf_MATLABFunction1); - } - - // End of SystemInitialize for SubSystem: '/filter' - - // SystemInitialize for Iterator SubSystem: '/filter_with_HP_filter' - for (ForEach_itr = 0; ForEach_itr < 3; ForEach_itr++) { - // InitializeConditions for DiscreteTransferFcn: '/3 Hz Low Pass' - est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states = - est_estimator_P->CoreSubsys_l.uHzLowPass_InitialStates; - - // InitializeConditions for DiscreteTransferFcn: '/Discrete Transfer Fcn' - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0] = - est_estimator_P->CoreSubsys_l.DiscreteTransferFcn_InitialStat; - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1] = - est_estimator_P->CoreSubsys_l.DiscreteTransferFcn_InitialStat; - - // InitializeConditions for DiscreteTransferFcn: '/High Pass Filter' - est_estimator_DW->CoreSubsys_l[ForEach_itr].HighPassFilter_states = - est_estimator_P->CoreSubsys_l.HighPassFilter_InitialStates; - - // SystemInitialize for MATLAB Function: '/MATLAB Function1' - est_estima_MATLABFunction1_Init(&est_estimator_DW-> - CoreSubsys_l[ForEach_itr].sf_MATLABFunction1); - } - - // End of SystemInitialize for SubSystem: '/filter_with_HP_filter' - - // SystemInitialize for Enabled SubSystem: '/Enabled Row-Wise SUM' - // SystemInitialize for Outport: '/Out1' - for (i = 0; i < 50; i++) { - est_estimator_B->SumofElements3[i] = est_estimator_P->Out1_Y0; - } - - // End of SystemInitialize for Outport: '/Out1' - // End of SystemInitialize for SubSystem: '/Enabled Row-Wise SUM' - - // SystemInitialize for IfAction SubSystem: '/Absolute_Update' - // SystemInitialize for Iterator SubSystem: '/ML Update' - // SystemInitialize for IfAction SubSystem: '/HR_Compute_Residual_and_H1' - // SystemInitialize for MATLAB Function: '/Compute Global positions of Handrail Features' - est_estimator_DW->hr_P_hr_ISS_ISS_pers_not_empty = false; - est_estimator_DW->hr_quat_ISS2hr_pers_not_empty = false; - - // End of SystemInitialize for SubSystem: '/HR_Compute_Residual_and_H1' - - // SystemInitialize for IfAction SubSystem: '/ML_Compute_Residual_and_H' - // SystemInitialize for MATLAB Function: '/Compute Residual and H' - est_estimator_DW->num_consecutive_rejected_meas = 0.0; - - // End of SystemInitialize for SubSystem: '/ML_Compute_Residual_and_H' - - // SystemInitialize for Merge: '/Merge' - for (i = 0; i < 6; i++) { - est_estimator_B->r_out[i] = est_estimator_P->Merge_1_InitialOutput; - } - - // End of SystemInitialize for Merge: '/Merge' - - // SystemInitialize for Merge: '/Merge' - est_estimator_B->error_out = est_estimator_P->Merge_2_InitialOutput; - - // SystemInitialize for Merge: '/Merge' - for (i = 0; i < 702; i++) { - est_estimator_B->H_out[i] = est_estimator_P->Merge_3_InitialOutput; - } - - // End of SystemInitialize for Merge: '/Merge' - - // SystemInitialize for Merge: '/Merge' - for (i = 0; i < 36; i++) { - est_estimator_B->R_mat[i] = est_estimator_P->Merge_4_InitialOutput; - } - - // End of SystemInitialize for Merge: '/Merge' - // End of SystemInitialize for SubSystem: '/ML Update' - // End of SystemInitialize for SubSystem: '/Absolute_Update' - - // SystemInitialize for IfAction SubSystem: '/If Action Subsystem2' - // SystemInitialize for MATLAB Function: '/MATLAB Function' - est_estimator_DW->aug_velocity_not_empty = false; - est_estimator_DW->aug_velocity_mag_not_empty = false; - est_estimator_DW->aug_omega_not_empty = false; - est_estimator_DW->aug_omega_mag_not_empty = false; - - // End of SystemInitialize for SubSystem: '/If Action Subsystem2' - // End of SystemInitialize for SubSystem: '/est_estimator' - } -} - -// Model terminate function -void est_estimator_terminate(RT_MODEL_est_estimator_T * est_estimator_M) -{ - // model code - rt_FREE(est_estimator_M->blockIO); - if (est_estimator_M->paramIsMalloced) { - rt_FREE(est_estimator_M->defaultParam); - } - - rt_FREE(est_estimator_M->dwork); - rt_FREE(est_estimator_M); -} - -// Model data allocation function -RT_MODEL_est_estimator_T *est_estimator(cvs_landmark_msg - *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]) -{ - RT_MODEL_est_estimator_T *est_estimator_M; - est_estimator_M = (RT_MODEL_est_estimator_T *) malloc(sizeof - (RT_MODEL_est_estimator_T)); - if (est_estimator_M == NULL) { - return NULL; - } - - (void) memset((char *)est_estimator_M, 0, - sizeof(RT_MODEL_est_estimator_T)); - - // block I/O - { - B_est_estimator_T *b = (B_est_estimator_T *) malloc(sizeof(B_est_estimator_T)); - rt_VALIDATE_MEMORY(est_estimator_M,b); - est_estimator_M->blockIO = (b); - } - - // parameters - { - P_est_estimator_T *p; - static int_T pSeen = 0; - - // only malloc on multiple model instantiation - if (pSeen == 1 ) { - p = (P_est_estimator_T *) malloc(sizeof(P_est_estimator_T)); - rt_VALIDATE_MEMORY(est_estimator_M,p); - (void) memcpy(p, &est_estimator_P, - sizeof(P_est_estimator_T)); - est_estimator_M->paramIsMalloced = (true); - } else { - p = &est_estimator_P; - est_estimator_M->paramIsMalloced = (false); - pSeen = 1; - } - - est_estimator_M->defaultParam = (p); - } - - // states (dwork) - { - DW_est_estimator_T *dwork = (DW_est_estimator_T *) malloc(sizeof - (DW_est_estimator_T)); - rt_VALIDATE_MEMORY(est_estimator_M,dwork); - est_estimator_M->dwork = (dwork); - } - - { - P_est_estimator_T *est_estimator_P = ((P_est_estimator_T *) - est_estimator_M->defaultParam); - B_est_estimator_T *est_estimator_B = ((B_est_estimator_T *) - est_estimator_M->blockIO); - DW_est_estimator_T *est_estimator_DW = ((DW_est_estimator_T *) - est_estimator_M->dwork); - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // non-finite (run-time) assignments - est_estimator_P->UnitDelay2_InitialCondition = rtInfF; - est_estimator_P->UnitDelay2_InitialCondition_f = rtInfF; - - // block I/O - (void) memset(((void *) est_estimator_B), 0, - sizeof(B_est_estimator_T)); - - // states (dwork) - (void) memset((void *)est_estimator_DW, 0, - sizeof(DW_est_estimator_T)); - - // external inputs - (void)memset(&est_estimator_U_Q_ISS2B[0], 0, sizeof(real32_T) << 2U); - *est_estimator_U_landmark_msg = est_estimator_rtZcvs_landmark_m; - *est_estimator_U_VisionRegistration = est_estimator_rtZcvs_registrati; - *est_estimator_U_cvs_optical_flow_msg_n = est_estimator_rtZcvs_optical_fl; - *est_estimator_U_handrail_msg = est_estimator_rtZcvs_handrail_m; - *est_estimator_U_imu_msg_c = est_estimator_rtZimu_msg; - *est_estimator_U_cmc_msg_o = est_estimator_rtZcmc_msg; - - // external outputs - (*est_estimator_Y_kfl_msg_h) = est_estimator_rtZkfl_msg; - (void) memset(&est_estimator_Y_P_out[0], 0, - 13689U*sizeof(real32_T)); - } - - return est_estimator_M; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.h b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.h deleted file mode 100644 index 72e5704cb0..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.h +++ /dev/null @@ -1,1604 +0,0 @@ -// -// File: est_estimator.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_est_estimator_h_ -#define RTW_HEADER_est_estimator_h_ -#include -#include -#include -#ifndef est_estimator_COMMON_INCLUDES_ -# define est_estimator_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#include "compute_delta_state_and_cov.h" -#include "apply_delta_state.h" -#include "of_residual_and_h.h" -#include "matrix_multiply.h" -#endif // est_estimator_COMMON_INCLUDES_ - -#include "est_estimator_types.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "aaiepphlecjelnoh_permute.h" -#include "aimgbiekknohhdjm_norm.h" -#include "baaanophjmgddbie_sum.h" -#include "baieimopcbaiaaai_eulers_to_quat.h" -#include "biecdbieglngohlf_pinv.h" -#include "biekcjmgdbaadbim_eye.h" -#include "div_nzp_s32_floor.h" -#include "djmgjecbcbiengln_power.h" -#include "ecjedbaiaiekohln_quaternion_to_rotation.h" -#include "fkfcbaiengdjgdje_quaternion_to_rotation.h" -#include "fkngdjekgdjepphl_sum.h" -#include "gdjmmglnmgdjlfkf_quat_rotation_vec.h" -#include "glfcngdjgdjmmglf_pinv.h" -#include "hdbaohdbkngdbimo_PadeApproximantOfDegree.h" -#include "hdbihlngknopkfcj_repmat.h" -#include "iecjbieccbailfcb_abs.h" -#include "iecjopppiecjmgln_quatmult.h" -#include "iekfiecjknopophd_pinv.h" -#include "imohcjmoimopimoh_nullAssignment.h" -#include "imohknglphlfpphd_repmat.h" -#include "jekfopppngdbhlng_diag.h" -#include "jmglopphppphkfkf_qr.h" -#include "jmohiecblfcjnohl_qr.h" -#include "kngldbimhdbaimgd_quat_propagate_step.h" -#include "mgdbbiekfknonglf_nullAssignment.h" -#include "mglfbimobiechdbi_bitget.h" -#include "mglnkfkfmglfjekn_PadeApproximantOfDegree.h" -#include "moppbaaafkfkimgd_diag.h" -#include "ngdjjecbgdbaglfc_eye.h" -#include "nohdcbaibiecnohl_power.h" -#include "nohlcjekmohddjmg_abs.h" -#include "ophlcjmgkfcbmohl_nullAssignment.h" -#include "rt_powf_snf.h" -#include "rt_roundd_snf.h" -#include "rtGetInf.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -# define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -# define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Exported data define - -// Definition for custom storage class: Define -#define ASE_ML_NUM_FEATURES 50 -#define ASE_OF_NUM_AUG 16 -#define ASE_OF_NUM_FEATURES 50 - -// Block signals for system '/MATLAB Function1' -typedef struct { - real32_T num_out[3]; // '/MATLAB Function1' - real32_T den_out[3]; // '/MATLAB Function1' -} B_MATLABFunction1_est_estimat_T; - -// Block states (auto storage) for system '/MATLAB Function1' -typedef struct { - real32_T y; // '/MATLAB Function1' - real32_T num[3]; // '/MATLAB Function1' - real32_T den[3]; // '/MATLAB Function1' - real32_T prev_impeller_speed; // '/MATLAB Function1' -} DW_MATLABFunction1_est_estima_T; - -// Block signals for system '/CoreSubsys' -typedef struct { - B_MATLABFunction1_est_estimat_T sf_MATLABFunction1;// '/MATLAB Function1' -} B_CoreSubsys_est_estimator_T; - -// Block states (auto storage) for system '/CoreSubsys' -typedef struct { - real32_T uHzLowPass_states; // '/3 Hz Low Pass' - real32_T DiscreteTransferFcn_states[2];// '/Discrete Transfer Fcn' - DW_MATLABFunction1_est_estima_T sf_MATLABFunction1;// '/MATLAB Function1' -} DW_CoreSubsys_est_estimator_T; - -// Block signals for system '/CoreSubsys' -typedef struct { - B_MATLABFunction1_est_estimat_T sf_MATLABFunction1;// '/MATLAB Function1' -} B_CoreSubsys_est_estimator_k_T; - -// Block states (auto storage) for system '/CoreSubsys' -typedef struct { - real32_T uHzLowPass_states; // '/3 Hz Low Pass' - real32_T DiscreteTransferFcn_states[2];// '/Discrete Transfer Fcn' - real32_T HighPassFilter_states; // '/High Pass Filter' - DW_MATLABFunction1_est_estima_T sf_MATLABFunction1;// '/MATLAB Function1' -} DW_CoreSubsys_est_estimator_a_T; - -// Block signals for system '/MATLAB Function' -typedef struct { - real32_T y[16]; // '/MATLAB Function' -} B_MATLABFunction_est_estimato_T; - -// Block signals (auto storage) -typedef struct { - real_T tmp_data[22500]; - real32_T q1_data[22500]; - real_T tmp_data_m[10000]; - real32_T ex_compute_delta_state_an_e[13689];// '/ex_compute_delta_state_and_cov' - real32_T Assignment[13689]; // '/Assignment' - real32_T Switch1[13689]; - real32_T MatrixConcatenate2[11934]; // '/Matrix Concatenate2' - real32_T ex_of_residual_and_h_o3[11232];// '/ex_of_residual_and_h' - real32_T q1_data_c[10000]; - real32_T ex_of_residual_and_h_o7[9216];// '/ex_of_residual_and_h' - ase_cov_datatype Switch1_k[8100]; - real32_T x_data[2500]; - ase_cov_datatype Switch1_c[1890]; - real32_T MatrixConcatenate[1755]; // '/Matrix Concatenate' - real32_T of_measured_in[1600]; - real32_T of_measured[1600]; // '/compute_of_global_points' - real32_T b_x_data[1600]; - real32_T of_measured_in_data[1600]; - ase_cov_datatype Selector1[1530]; // '/Selector1' - ase_cov_datatype P_IC[1530]; // '/MATLAB Function2' - real32_T r_out[6]; // '/Merge' - real32_T H_out[702]; // '/Merge' - real32_T R_mat[36]; // '/Merge' - int32_T error_out; // '/Merge' - uint8_T SumofElements3[50]; // '/Sum of Elements3' - B_MATLABFunction_est_estimato_T sf_MATLABFunction_l0;// '/MATLAB Function' - B_MATLABFunction_est_estimato_T sf_MATLABFunction_o;// '/MATLAB Function' - B_MATLABFunction_est_estimato_T sf_MATLABFunction;// '/MATLAB Function' - B_CoreSubsys_est_estimator_k_T CoreSubsys_l[3];// '/CoreSubsys' - B_CoreSubsys_est_estimator_T CoreSubsys[3];// '/CoreSubsys' -} B_est_estimator_T; - -// Block states (auto storage) for system '' -typedef struct { - real_T UnitDelay20_DSTATE[3]; // '/Unit Delay20' - real_T UnitDelay21_DSTATE[3]; // '/Unit Delay21' - real_T UnitDelay22_DSTATE[48]; // '/Unit Delay22' - real_T UnitDelay23_DSTATE[48]; // '/Unit Delay23' - real_T UnitDelay18_DSTATE[50]; // '/Unit Delay18' - real_T UnitDelay19_DSTATE[50]; // '/Unit Delay19' - real_T UnitDelay_DSTATE_j; // '/Unit Delay' - real_T num_consecutive_rejected_meas;// '/Compute Residual and H' - ase_cov_datatype Delay_DSTATE[13689];// '/Delay' - real32_T UnitDelay1_DSTATE[4]; // '/Unit Delay1' - real32_T UnitDelay3_DSTATE[3]; // '/Unit Delay3' - real32_T UnitDelay4_DSTATE[3]; // '/Unit Delay4' - real32_T UnitDelay6_DSTATE[3]; // '/Unit Delay6' - real32_T UnitDelay7_DSTATE[3]; // '/Unit Delay7' - real32_T UnitDelay10_DSTATE[4]; // '/Unit Delay10' - real32_T UnitDelay11_DSTATE[3]; // '/Unit Delay11' - real32_T UnitDelay12_DSTATE[64]; // '/Unit Delay12' - real32_T UnitDelay13_DSTATE[48]; // '/Unit Delay13' - ase_cov_datatype UnitDelay14_DSTATE[117];// '/Unit Delay14' - real32_T UnitDelay24_DSTATE[3]; // '/Unit Delay24' - real32_T UnitDelay25_DSTATE[4]; // '/Unit Delay25' - real32_T UnitDelay26_DSTATE[3]; // '/Unit Delay26' - uint32_T UnitDelay9_DSTATE; // '/Unit Delay9' - uint32_T DelayInput1_DSTATE; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_h; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_o; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_n; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_b; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_d; // '/Delay Input1' - real32_T aug_velocity[48]; // '/MATLAB Function' - real32_T aug_velocity_mag[16]; // '/MATLAB Function' - real32_T aug_omega[48]; // '/MATLAB Function' - real32_T aug_omega_mag[16]; // '/MATLAB Function' - real32_T hr_P_hr_ISS_ISS_pers[3]; // '/Compute Global positions of Handrail Features' - real32_T hr_quat_ISS2hr_pers[4]; // '/Compute Global positions of Handrail Features' - uint16_T UnitDelay15_DSTATE; // '/Unit Delay15' - uint8_T UnitDelay8_DSTATE; // '/Unit Delay8' - uint8_T UnitDelay16_DSTATE; // '/Unit Delay16' - uint8_T UnitDelay17_DSTATE; // '/Unit Delay17' - uint8_T DelayInput1_DSTATE_k; // '/Delay Input1' - uint8_T icLoad; // '/Delay' - boolean_T aug_velocity_not_empty; // '/MATLAB Function' - boolean_T aug_velocity_mag_not_empty;// '/MATLAB Function' - boolean_T aug_omega_not_empty; // '/MATLAB Function' - boolean_T aug_omega_mag_not_empty; // '/MATLAB Function' - boolean_T hr_P_hr_ISS_ISS_pers_not_empty;// '/Compute Global positions of Handrail Features' - boolean_T hr_quat_ISS2hr_pers_not_empty;// '/Compute Global positions of Handrail Features' - DW_CoreSubsys_est_estimator_a_T CoreSubsys_l[3];// '/CoreSubsys' - DW_CoreSubsys_est_estimator_T CoreSubsys[3];// '/CoreSubsys' -} DW_est_estimator_T; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_est_estimator_T_ { - real32_T uHzLowPass_NumCoef; // Expression: single([0.2604]) - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_DenCoef[2]; // Expression: single([1 -0.7396]) - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_InitialStates; // Computed Parameter: uHzLowPass_InitialStates - // Referenced by: '/3 Hz Low Pass' - - real32_T DiscreteTransferFcn_InitialStat;// Computed Parameter: DiscreteTransferFcn_InitialStat - // Referenced by: '/Discrete Transfer Fcn' - -}; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_est_estimator_g_T_ { - real32_T uHzLowPass_NumCoef; // Expression: single([0.2604]) - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_DenCoef[2]; // Expression: single([1 -0.7396]) - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_InitialStates; // Computed Parameter: uHzLowPass_InitialStates - // Referenced by: '/3 Hz Low Pass' - - real32_T DiscreteTransferFcn_InitialStat;// Computed Parameter: DiscreteTransferFcn_InitialStat - // Referenced by: '/Discrete Transfer Fcn' - - real32_T HighPassFilter_InitialStates;// Computed Parameter: HighPassFilter_InitialStates - // Referenced by: '/High Pass Filter' - -}; - -// Parameters for system: '/Normalize' -struct P_Normalize_est_estimator_T_ { - real_T Constant1_Value; // Expression: -1 - // Referenced by: '/Constant1' - -}; - -// Parameters (auto storage) -struct P_est_estimator_T_ { - real_T ase_minumum_resid_thresh; // Variable: ase_minumum_resid_thresh - // Referenced by: - // '/Constant1' - // '/Constant1' - - real_T ase_ts; // Variable: ase_ts - // Referenced by: - // '/Constant' - // '/Gain' - // '/Gain1' - // '/MATLAB Function' - // '/Gain3' - - real_T astrobee_fsw_step_size; // Variable: astrobee_fsw_step_size - // Referenced by: - // '/Constant2' - // '/Constant2' - - real32_T ase_accel_fixed_bias[3]; // Variable: ase_accel_fixed_bias - // Referenced by: '/Constant5' - - real32_T ase_gyro_fixed_bias[3]; // Variable: ase_gyro_fixed_bias - // Referenced by: '/Constant1' - - real32_T ase_state_ic_A_B_ISS_ISS[3];// Variable: ase_state_ic_A_B_ISS_ISS - // Referenced by: '/Unit Delay5' - - real32_T ase_state_ic_accel_bias[3]; // Variable: ase_state_ic_accel_bias - // Referenced by: '/Unit Delay6' - - real32_T ase_state_ic_cov_diag[117]; // Variable: ase_state_ic_cov_diag - // Referenced by: '/Unit Delay14' - - real32_T ase_state_ic_gyro_bias[3]; // Variable: ase_state_ic_gyro_bias - // Referenced by: '/Unit Delay3' - - real32_T ase_state_ic_ml_P_cam_ISS_ISS[3];// Variable: ase_state_ic_ml_P_cam_ISS_ISS - // Referenced by: '/Unit Delay11' - - real32_T ase_state_ic_ml_quat_ISS2cam[4];// Variable: ase_state_ic_ml_quat_ISS2cam - // Referenced by: '/Unit Delay10' - - real32_T ase_state_ic_of_P_cam_ISS_ISS[48];// Variable: ase_state_ic_of_P_cam_ISS_ISS - // Referenced by: '/Unit Delay13' - - real32_T ase_state_ic_of_quat_ISS2cam[64];// Variable: ase_state_ic_of_quat_ISS2cam - // Referenced by: '/Unit Delay12' - - real32_T fam_impeller_speeds[3]; // Variable: fam_impeller_speeds - // Referenced by: - // '/Constant' - // '/Constant' - - real32_T tun_abp_p_dockcam_imu_est[3];// Variable: tun_abp_p_dockcam_imu_est - // Referenced by: '/Constant2' - - real32_T tun_abp_p_imu_body_body[3]; // Variable: tun_abp_p_imu_body_body - // Referenced by: '/Constant5' - - real32_T tun_abp_p_navcam_imu_est[3];// Variable: tun_abp_p_navcam_imu_est - // Referenced by: - // '/MATLAB Function' - // '/Constant1' - // '/Constant1' - - real32_T tun_abp_p_perchcam_imu_est[3];// Variable: tun_abp_p_perchcam_imu_est - // Referenced by: '/Constant4' - - real32_T tun_abp_q_body2dockcam[4]; // Variable: tun_abp_q_body2dockcam - // Referenced by: - // '/Constant7' - // '/Constant1' - - real32_T tun_abp_q_body2navcam[4]; // Variable: tun_abp_q_body2navcam - // Referenced by: - // '/MATLAB Function' - // '/Constant2' - // '/Constant6' - // '/Constant' - - real32_T tun_abp_q_body2perchcam[4]; // Variable: tun_abp_q_body2perchcam - // Referenced by: - // '/Constant8' - // '/Compute Residual and H' - - real32_T tun_abp_quat_body2imu[4]; // Variable: tun_abp_quat_body2imu - // Referenced by: - // '/Constant' - // '/Constant2' - - real32_T tun_ase_Q_imu[12]; // Variable: tun_ase_Q_imu - // Referenced by: '/Constant9' - - real32_T tun_ase_acquired_ticks; // Variable: tun_ase_acquired_ticks - // Referenced by: - // '/Saturation' - // '/Constant' - - real32_T tun_ase_converged_thresh; // Variable: tun_ase_converged_thresh - // Referenced by: '/Constant' - - real32_T tun_ase_diverged_thresh; // Variable: tun_ase_diverged_thresh - // Referenced by: '/Constant' - - real32_T tun_ase_dock_r_mag; // Variable: tun_ase_dock_r_mag - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_dockcam_distortion; // Variable: tun_ase_dockcam_distortion - // Referenced by: '/Constant5' - - real32_T tun_ase_dockcam_inv_focal_length;// Variable: tun_ase_dockcam_inv_focal_length - // Referenced by: '/Constant3' - - real32_T tun_ase_gravity_accel[3]; // Variable: tun_ase_gravity_accel - // Referenced by: '/Constant3' - - real32_T tun_ase_hr_distance_r; // Variable: tun_ase_hr_distance_r - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_hr_r_mag; // Variable: tun_ase_hr_r_mag - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_mahal_distance_max; // Variable: tun_ase_mahal_distance_max - // Referenced by: - // '/ex_of_residual_and_h' - // '/Compute Residual and H' - - real32_T tun_ase_map_error; // Variable: tun_ase_map_error - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_max_accel; // Variable: tun_ase_max_accel - // Referenced by: '/diag' - - real32_T tun_ase_max_gyro; // Variable: tun_ase_max_gyro - // Referenced by: '/diag' - - real32_T tun_ase_min_ar_meas; // Variable: tun_ase_min_ar_meas - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_min_ml_meas; // Variable: tun_ase_min_ml_meas - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_ml_forward_projection_time;// Variable: tun_ase_ml_forward_projection_time - // Referenced by: - // '/Constant' - // '/Gain' - // '/Constant' - // '/Gain' - // '/Compute Residual and H' - // '/Compute Residual and H' - - real32_T tun_ase_navcam_distortion; // Variable: tun_ase_navcam_distortion - // Referenced by: - // '/ex_of_residual_and_h' - // '/Constant4' - - real32_T tun_ase_navcam_inv_focal_length;// Variable: tun_ase_navcam_inv_focal_length - // Referenced by: - // '/ex_of_residual_and_h' - // '/Constant2' - - real32_T tun_ase_of_r_mag; // Variable: tun_ase_of_r_mag - // Referenced by: '/ex_of_residual_and_h' - - real32_T tun_ase_q_saturated_accel; // Variable: tun_ase_q_saturated_accel - // Referenced by: '/diag' - - real32_T tun_ase_q_saturated_gyro; // Variable: tun_ase_q_saturated_gyro - // Referenced by: '/diag' - - real32_T tun_ase_state_ic_P_B_ISS_ISS[3];// Variable: tun_ase_state_ic_P_B_ISS_ISS - // Referenced by: '/Unit Delay7' - - real32_T tun_ase_state_ic_P_EST_ISS_ISS[3];// Variable: tun_ase_state_ic_P_EST_ISS_ISS - // Referenced by: '/Unit Delay26' - - real32_T tun_ase_state_ic_V_B_ISS_ISS[3];// Variable: tun_ase_state_ic_V_B_ISS_ISS - // Referenced by: '/Unit Delay4' - - real32_T tun_ase_state_ic_cov_diag[15];// Variable: tun_ase_state_ic_cov_diag - // Referenced by: '/Constant4' - - real32_T tun_ase_state_ic_omega_B_ISS_B[3];// Variable: tun_ase_state_ic_omega_B_ISS_B - // Referenced by: '/Unit Delay2' - - real32_T tun_ase_state_ic_quat_ISS2B[4];// Variable: tun_ase_state_ic_quat_ISS2B - // Referenced by: '/Unit Delay1' - - real32_T tun_ase_vis_r_mag; // Variable: tun_ase_vis_r_mag - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_vocam_inv_focal_length;// Variable: tun_ase_vocam_inv_focal_length - // Referenced by: '/compute_of_global_points' - - real32_T tun_aug_ic_cov; // Variable: tun_aug_ic_cov - // Referenced by: '/Gain' - - real32_T tun_grav_hp_den[2]; // Variable: tun_grav_hp_den - // Referenced by: '/High Pass Filter' - - real32_T tun_grav_hp_num[2]; // Variable: tun_grav_hp_num - // Referenced by: '/High Pass Filter' - - real32_T tun_ic_cov_pos; // Variable: tun_ic_cov_pos - // Referenced by: '/Constant3' - - real32_T tun_ic_cov_quat; // Variable: tun_ic_cov_quat - // Referenced by: '/Constant1' - - real32_T tun_max_mahal_reject_frames;// Variable: tun_max_mahal_reject_frames - // Referenced by: '/Compute Residual and H' - - uint32_T ase_aug_state_bitmask; // Variable: ase_aug_state_bitmask - // Referenced by: '/Constant' - - uint8_T ase_local_mode_docking; // Variable: ase_local_mode_docking - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T ase_local_mode_map; // Variable: ase_local_mode_map - // Referenced by: - // '/Constant' - // '/Constant' - // '/Constant' - - uint8_T ase_local_mode_perching; // Variable: ase_local_mode_perching - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T ase_state_ic_aug_state_enum; // Variable: ase_state_ic_aug_state_enum - // Referenced by: '/Unit Delay9' - - uint8_T ase_state_ic_confidence; // Variable: ase_state_ic_confidence - // Referenced by: '/Unit Delay8' - - uint8_T ase_state_ic_status; // Variable: ase_state_ic_status - // Referenced by: '/Unit Delay15' - - uint8_T ase_status_acquiring; // Variable: ase_status_acquiring - // Referenced by: - // '/Constant4' - // '/Constant' - // '/Constant' - - uint8_T ase_status_converged; // Variable: ase_status_converged - // Referenced by: - // '/Constant1' - // '/Constant' - // '/Constant' - // '/Constant' - - uint8_T ase_status_diverged; // Variable: ase_status_diverged - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T fam_impeller_speeds_cnt; // Variable: fam_impeller_speeds_cnt - // Referenced by: '/Saturation' - - uint8_T tun_ase_enable_of; // Variable: tun_ase_enable_of - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T tun_ase_gravity_removal; // Variable: tun_ase_gravity_removal - // Referenced by: '/Constant4' - - uint8_T tun_grav_hp_enable_f; // Variable: tun_grav_hp_enable_f - // Referenced by: '/Constant1' - - uint32_T FixPtBitwiseOperator3_BitMask;// Mask Parameter: FixPtBitwiseOperator3_BitMask - // Referenced by: '/FixPt Bitwise Operator3' - - uint32_T BitwiseOperator2_BitMask; // Mask Parameter: BitwiseOperator2_BitMask - // Referenced by: '/Bitwise Operator2' - - uint32_T BitwiseOperator_BitMask; // Mask Parameter: BitwiseOperator_BitMask - // Referenced by: '/Bitwise Operator' - - uint32_T DetectChange6_vinit; // Mask Parameter: DetectChange6_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange7_vinit; // Mask Parameter: DetectChange7_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange_vinit; // Mask Parameter: DetectChange_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange1_vinit; // Mask Parameter: DetectChange1_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange4_vinit; // Mask Parameter: DetectChange4_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange5_vinit; // Mask Parameter: DetectChange5_vinit - // Referenced by: '/Delay Input1' - - uint16_T BitwiseOperator_BitMask_c; // Mask Parameter: BitwiseOperator_BitMask_c - // Referenced by: '/Bitwise Operator' - - uint16_T BitwiseOperator1_BitMask; // Mask Parameter: BitwiseOperator1_BitMask - // Referenced by: '/Bitwise Operator1' - - uint16_T BitwiseOperator_BitMask_l; // Mask Parameter: BitwiseOperator_BitMask_l - // Referenced by: '/Bitwise Operator' - - uint8_T CompareToConstant7_const; // Mask Parameter: CompareToConstant7_const - // Referenced by: '/Constant' - - uint8_T CompareToConstant_const; // Mask Parameter: CompareToConstant_const - // Referenced by: '/Constant' - - uint8_T DetectChange_vinit_i; // Mask Parameter: DetectChange_vinit_i - // Referenced by: '/Delay Input1' - - kfl_msg State_out_Y0; // Computed Parameter: State_out_Y0 - // Referenced by: '/State_out' - - kfl_msg UnitDelay_InitialCondition; // Computed Parameter: UnitDelay_InitialCondition - // Referenced by: '/Unit Delay' - - kfl_msg State_out_Y0_h; // Computed Parameter: State_out_Y0_h - // Referenced by: '/State_out' - - kfl_msg UnitDelay_InitialCondition_p;// Computed Parameter: UnitDelay_InitialCondition_p - // Referenced by: '/Unit Delay' - - real_T Constant_Value[50]; // Expression: zeros(ase_ml_num_features, 1) - // Referenced by: '/Constant' - - real_T Merge_6_InitialOutput; // Computed Parameter: Merge_6_InitialOutput - // Referenced by: '/Merge' - - real_T Constant3_Value[50]; // Expression: zeros(ase_of_num_features,1) - // Referenced by: '/Constant3' - - real_T Constant2_Value[50]; // Expression: zeros(ase_ml_num_features,1) - // Referenced by: '/Constant2' - - real_T Constant2_Value_j[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant_Value_p[3]; // Expression: [0 0 0] - // Referenced by: '/Constant' - - real_T Constant1_Value; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_l; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_l[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_p; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_m[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_m; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_m; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_b[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_n; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_d; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant3_Value_pv; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_jy[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_pi; // Expression: 0 - // Referenced by: '/Constant3' - - real_T UnitDelay20_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay20' - - real_T UnitDelay21_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay21' - - real_T UnitDelay22_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay22' - - real_T UnitDelay23_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay23' - - real_T UnitDelay18_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay18' - - real_T UnitDelay19_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay19' - - real_T Constant_Value_l[3]; // Expression: [0 0 0] - // Referenced by: '/Constant' - - real_T Constant2_Value_a[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_mb; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_k; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_ja[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_c; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_c; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_bl[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_l; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_o; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_p[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_k; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_i; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_bd[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_e; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_kc; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Merge2_1_InitialOutput; // Computed Parameter: Merge2_1_InitialOutput - // Referenced by: '/Merge2' - - real_T Merge2_2_InitialOutput; // Computed Parameter: Merge2_2_InitialOutput - // Referenced by: '/Merge2' - - real_T Merge2_3_InitialOutput; // Computed Parameter: Merge2_3_InitialOutput - // Referenced by: '/Merge2' - - real_T Merge2_4_InitialOutput; // Computed Parameter: Merge2_4_InitialOutput - // Referenced by: '/Merge2' - - real_T Constant2_Value_h; // Expression: 0 - // Referenced by: '/Constant2' - - real_T UnitDelay_InitialCondition_h; // Expression: 0 - // Referenced by: '/Unit Delay' - - real_T Constant3_Value_p3; // Expression: 1 - // Referenced by: '/Constant3' - - real_T Saturation_LowerSat; // Expression: 0 - // Referenced by: '/Saturation' - - real_T Constant2_Value_d[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_ec; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_ol; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Merge2_1_InitialOutput_c; // Computed Parameter: Merge2_1_InitialOutput_c - // Referenced by: '/Merge2' - - real_T Merge2_2_InitialOutput_c; // Computed Parameter: Merge2_2_InitialOutput_c - // Referenced by: '/Merge2' - - real_T Merge2_3_InitialOutput_c; // Computed Parameter: Merge2_3_InitialOutput_c - // Referenced by: '/Merge2' - - real_T Merge2_4_InitialOutput_c; // Computed Parameter: Merge2_4_InitialOutput_c - // Referenced by: '/Merge2' - - real32_T P_out_Y0; // Computed Parameter: P_out_Y0 - // Referenced by: '/P_out' - - real32_T UnitDelay1_InitialCondition;// Computed Parameter: UnitDelay1_InitialCondition - // Referenced by: '/Unit Delay1' - - real32_T Merge_1_InitialOutput; // Computed Parameter: Merge_1_InitialOutput - // Referenced by: '/Merge' - - real32_T Merge_3_InitialOutput; // Computed Parameter: Merge_3_InitialOutput - // Referenced by: '/Merge' - - real32_T Merge_4_InitialOutput; // Computed Parameter: Merge_4_InitialOutput - // Referenced by: '/Merge' - - real32_T Merge_7_InitialOutput; // Computed Parameter: Merge_7_InitialOutput - // Referenced by: '/Merge' - - real32_T Merge_8_InitialOutput; // Computed Parameter: Merge_8_InitialOutput - // Referenced by: '/Merge' - - real32_T UnitDelay2_InitialCondition;// Computed Parameter: UnitDelay2_InitialCondition - // Referenced by: '/Unit Delay2' - - real32_T P_out_Y0_e; // Computed Parameter: P_out_Y0_e - // Referenced by: '/P_out' - - real32_T UnitDelay2_InitialCondition_f;// Computed Parameter: UnitDelay2_InitialCondition_f - // Referenced by: '/Unit Delay2' - - real32_T UnitDelay1_InitialCondition_m;// Computed Parameter: UnitDelay1_InitialCondition_m - // Referenced by: '/Unit Delay1' - - real32_T Gain2_Gain; // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_e; // Computed Parameter: Gain2_Gain_e - // Referenced by: '/Gain2' - - real32_T Gain1_Gain; // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T Gain1_Gain_d; // Computed Parameter: Gain1_Gain_d - // Referenced by: '/Gain1' - - real32_T Gain_Gain_l; // Computed Parameter: Gain_Gain_l - // Referenced by: '/Gain' - - real32_T Constant1_Value_cn; // Computed Parameter: Constant1_Value_cn - // Referenced by: '/Constant1' - - real32_T Constant3_Value_f; // Computed Parameter: Constant3_Value_f - // Referenced by: '/Constant3' - - real32_T Constant3_Value_cc; // Computed Parameter: Constant3_Value_cc - // Referenced by: '/Constant3' - - real32_T Gain_Gain_h; // Computed Parameter: Gain_Gain_h - // Referenced by: '/Gain' - - real32_T Gain1_Gain_b; // Computed Parameter: Gain1_Gain_b - // Referenced by: '/Gain1' - - real32_T Constant2_Value_pd; // Computed Parameter: Constant2_Value_pd - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_h; // Computed Parameter: Gain2_Gain_h - // Referenced by: '/Gain2' - - real32_T Gain3_Gain; // Computed Parameter: Gain3_Gain - // Referenced by: '/Gain3' - - real32_T Gain4_Gain; // Computed Parameter: Gain4_Gain - // Referenced by: '/Gain4' - - real32_T Constant1_Value_j; // Computed Parameter: Constant1_Value_j - // Referenced by: '/Constant1' - - real32_T Gain5_Gain; // Computed Parameter: Gain5_Gain - // Referenced by: '/Gain5' - - real32_T Constant_Value_f; // Computed Parameter: Constant_Value_f - // Referenced by: '/Constant' - - real32_T Constant3_Value_iq; // Computed Parameter: Constant3_Value_iq - // Referenced by: '/Constant3' - - real32_T Gain1_Gain_j; // Computed Parameter: Gain1_Gain_j - // Referenced by: '/Gain1' - - real32_T Gain_Gain_n; // Computed Parameter: Gain_Gain_n - // Referenced by: '/Gain' - - real32_T Gain1_Gain_f; // Computed Parameter: Gain1_Gain_f - // Referenced by: '/Gain1' - - real32_T Constant2_Value_n; // Computed Parameter: Constant2_Value_n - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_a; // Computed Parameter: Gain2_Gain_a - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_l; // Computed Parameter: Gain3_Gain_l - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_o; // Computed Parameter: Gain4_Gain_o - // Referenced by: '/Gain4' - - real32_T Constant1_Value_nk; // Computed Parameter: Constant1_Value_nk - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_j; // Computed Parameter: Gain5_Gain_j - // Referenced by: '/Gain5' - - real32_T Constant_Value_j; // Computed Parameter: Constant_Value_j - // Referenced by: '/Constant' - - real32_T Constant2_Value_p3; // Computed Parameter: Constant2_Value_p3 - // Referenced by: '/Constant2' - - real32_T Gain_Gain_a; // Computed Parameter: Gain_Gain_a - // Referenced by: '/Gain' - - real32_T Gain1_Gain_e; // Computed Parameter: Gain1_Gain_e - // Referenced by: '/Gain1' - - real32_T Gain_Gain_f; // Computed Parameter: Gain_Gain_f - // Referenced by: '/Gain' - - real32_T Gain1_Gain_k; // Computed Parameter: Gain1_Gain_k - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_i; // Computed Parameter: Gain2_Gain_i - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_p; // Computed Parameter: Gain2_Gain_p - // Referenced by: '/Gain2' - - real32_T Gain_Gain_g; // Computed Parameter: Gain_Gain_g - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jc; // Computed Parameter: Gain1_Gain_jc - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_o; // Computed Parameter: Gain2_Gain_o - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_i; // Computed Parameter: Gain1_Gain_i - // Referenced by: '/Gain1' - - real32_T Constant1_Value_a; // Computed Parameter: Constant1_Value_a - // Referenced by: '/Constant1' - - real32_T Constant3_Value_da; // Computed Parameter: Constant3_Value_da - // Referenced by: '/Constant3' - - real32_T Constant3_Value_oe; // Computed Parameter: Constant3_Value_oe - // Referenced by: '/Constant3' - - real32_T Constant5_Value[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant5' - - real32_T Gain_Gain_d; // Computed Parameter: Gain_Gain_d - // Referenced by: '/Gain' - - real32_T Gain1_Gain_n; // Computed Parameter: Gain1_Gain_n - // Referenced by: '/Gain1' - - real32_T Constant2_Value_m2; // Computed Parameter: Constant2_Value_m2 - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_k; // Computed Parameter: Gain2_Gain_k - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_f; // Computed Parameter: Gain3_Gain_f - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_b; // Computed Parameter: Gain4_Gain_b - // Referenced by: '/Gain4' - - real32_T Constant1_Value_i; // Computed Parameter: Constant1_Value_i - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_f; // Computed Parameter: Gain5_Gain_f - // Referenced by: '/Gain5' - - real32_T Constant_Value_m; // Computed Parameter: Constant_Value_m - // Referenced by: '/Constant' - - real32_T Constant3_Value_op; // Computed Parameter: Constant3_Value_op - // Referenced by: '/Constant3' - - real32_T Gain1_Gain_o; // Computed Parameter: Gain1_Gain_o - // Referenced by: '/Gain1' - - real32_T Gain_Gain_ng; // Computed Parameter: Gain_Gain_ng - // Referenced by: '/Gain' - - real32_T Gain1_Gain_a; // Computed Parameter: Gain1_Gain_a - // Referenced by: '/Gain1' - - real32_T Constant2_Value_o; // Computed Parameter: Constant2_Value_o - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_b; // Computed Parameter: Gain2_Gain_b - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_o; // Computed Parameter: Gain3_Gain_o - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_i; // Computed Parameter: Gain4_Gain_i - // Referenced by: '/Gain4' - - real32_T Constant1_Value_b; // Computed Parameter: Constant1_Value_b - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_c; // Computed Parameter: Gain5_Gain_c - // Referenced by: '/Gain5' - - real32_T Constant_Value_px; // Computed Parameter: Constant_Value_px - // Referenced by: '/Constant' - - real32_T Constant2_Value_mp; // Computed Parameter: Constant2_Value_mp - // Referenced by: '/Constant2' - - real32_T Gain_Gain_c; // Computed Parameter: Gain_Gain_c - // Referenced by: '/Gain' - - real32_T Gain1_Gain_nq; // Computed Parameter: Gain1_Gain_nq - // Referenced by: '/Gain1' - - real32_T Gain_Gain_fr; // Computed Parameter: Gain_Gain_fr - // Referenced by: '/Gain' - - real32_T Gain1_Gain_c; // Computed Parameter: Gain1_Gain_c - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_hc; // Computed Parameter: Gain2_Gain_hc - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_g; // Computed Parameter: Gain2_Gain_g - // Referenced by: '/Gain2' - - ase_cov_datatype Constant3_Value_l2[90];// Computed Parameter: Constant3_Value_l2 - // Referenced by: '/Constant3' - - real32_T Gain_Gain_cu; // Computed Parameter: Gain_Gain_cu - // Referenced by: '/Gain' - - real32_T Gain1_Gain_l; // Computed Parameter: Gain1_Gain_l - // Referenced by: '/Gain1' - - real32_T Gain_Gain_ne; // Computed Parameter: Gain_Gain_ne - // Referenced by: '/Gain' - - real32_T Gain1_Gain_j4; // Computed Parameter: Gain1_Gain_j4 - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_n; // Computed Parameter: Gain2_Gain_n - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_p0; // Computed Parameter: Gain2_Gain_p0 - // Referenced by: '/Gain2' - - real32_T Gain_Gain_fx; // Computed Parameter: Gain_Gain_fx - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ik; // Computed Parameter: Gain1_Gain_ik - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_m; // Computed Parameter: Gain2_Gain_m - // Referenced by: '/Gain2' - - real32_T Gain_Gain_j; // Computed Parameter: Gain_Gain_j - // Referenced by: '/Gain' - - real32_T Gain1_Gain_g; // Computed Parameter: Gain1_Gain_g - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_eo; // Computed Parameter: Gain2_Gain_eo - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_ei; // Computed Parameter: Gain1_Gain_ei - // Referenced by: '/Gain1' - - real32_T Constant2_Value_ad[13689]; // Expression: zeros(ase_total_num_states^2,1, 'single') - // Referenced by: '/Constant2' - - real32_T Constant5_Value_a[102]; // Expression: ones(1, ase_total_num_states - size(tun_ase_state_ic_cov_diag, 2), 'single') - // Referenced by: '/Constant5' - - real32_T UnitDelay24_InitialCondition;// Computed Parameter: UnitDelay24_InitialCondition - // Referenced by: '/Unit Delay24' - - real32_T UnitDelay25_InitialCondition;// Computed Parameter: UnitDelay25_InitialCondition - // Referenced by: '/Unit Delay25' - - real32_T Constant1_Value_i3; // Computed Parameter: Constant1_Value_i3 - // Referenced by: '/Constant1' - - real32_T Constant3_Value_dc; // Computed Parameter: Constant3_Value_dc - // Referenced by: '/Constant3' - - real32_T Constant3_Value_c4; // Computed Parameter: Constant3_Value_c4 - // Referenced by: '/Constant3' - - real32_T Gain_Gain_n1; // Computed Parameter: Gain_Gain_n1 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_au; // Computed Parameter: Gain1_Gain_au - // Referenced by: '/Gain1' - - real32_T Constant2_Value_am; // Computed Parameter: Constant2_Value_am - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_a5; // Computed Parameter: Gain2_Gain_a5 - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_m; // Computed Parameter: Gain3_Gain_m - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_h; // Computed Parameter: Gain4_Gain_h - // Referenced by: '/Gain4' - - real32_T Constant1_Value_p; // Computed Parameter: Constant1_Value_p - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_a; // Computed Parameter: Gain5_Gain_a - // Referenced by: '/Gain5' - - real32_T Constant_Value_d; // Computed Parameter: Constant_Value_d - // Referenced by: '/Constant' - - real32_T Constant3_Value_dg; // Computed Parameter: Constant3_Value_dg - // Referenced by: '/Constant3' - - real32_T Gain_Gain_g0; // Computed Parameter: Gain_Gain_g0 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jx; // Computed Parameter: Gain1_Gain_jx - // Referenced by: '/Gain1' - - real32_T Gain_Gain_e; // Computed Parameter: Gain_Gain_e - // Referenced by: '/Gain' - - real32_T Gain1_Gain_nf; // Computed Parameter: Gain1_Gain_nf - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_h5; // Computed Parameter: Gain2_Gain_h5 - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_gg; // Computed Parameter: Gain2_Gain_gg - // Referenced by: '/Gain2' - - real32_T Gain_Gain_co; // Computed Parameter: Gain_Gain_co - // Referenced by: '/Gain' - - real32_T Gain1_Gain_cz; // Computed Parameter: Gain1_Gain_cz - // Referenced by: '/Gain1' - - real32_T Constant2_Value_c; // Computed Parameter: Constant2_Value_c - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_g4; // Computed Parameter: Gain2_Gain_g4 - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_fu; // Computed Parameter: Gain3_Gain_fu - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_g; // Computed Parameter: Gain4_Gain_g - // Referenced by: '/Gain4' - - real32_T Constant1_Value_o; // Computed Parameter: Constant1_Value_o - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_jr; // Computed Parameter: Gain5_Gain_jr - // Referenced by: '/Gain5' - - real32_T Constant_Value_lw; // Computed Parameter: Constant_Value_lw - // Referenced by: '/Constant' - - real32_T Constant2_Value_j0; // Computed Parameter: Constant2_Value_j0 - // Referenced by: '/Constant2' - - real32_T Gain_Gain_c0; // Computed Parameter: Gain_Gain_c0 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_il; // Computed Parameter: Gain1_Gain_il - // Referenced by: '/Gain1' - - real32_T Gain_Gain_lj; // Computed Parameter: Gain_Gain_lj - // Referenced by: '/Gain' - - real32_T Gain1_Gain_lb; // Computed Parameter: Gain1_Gain_lb - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_m4; // Computed Parameter: Gain2_Gain_m4 - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_j; // Computed Parameter: Gain2_Gain_j - // Referenced by: '/Gain2' - - real32_T Gain_Gain_p; // Computed Parameter: Gain_Gain_p - // Referenced by: '/Gain' - - real32_T Gain1_Gain_fe; // Computed Parameter: Gain1_Gain_fe - // Referenced by: '/Gain1' - - real32_T Gain_Gain_az; // Computed Parameter: Gain_Gain_az - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ln; // Computed Parameter: Gain1_Gain_ln - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_c; // Computed Parameter: Gain2_Gain_c - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_i5; // Computed Parameter: Gain2_Gain_i5 - // Referenced by: '/Gain2' - - real32_T Constant6_Value[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant6' - - ase_cov_datatype Constant5_Value_p[180];// Computed Parameter: Constant5_Value_p - // Referenced by: '/Constant5' - - real32_T Gain_Gain_fm; // Computed Parameter: Gain_Gain_fm - // Referenced by: '/Gain' - - real32_T Gain1_Gain_l4; // Computed Parameter: Gain1_Gain_l4 - // Referenced by: '/Gain1' - - real32_T Gain_Gain_k; // Computed Parameter: Gain_Gain_k - // Referenced by: '/Gain' - - real32_T Gain1_Gain_j5; // Computed Parameter: Gain1_Gain_j5 - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_i5w; // Computed Parameter: Gain2_Gain_i5w - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_kj; // Computed Parameter: Gain2_Gain_kj - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_cx; // Computed Parameter: Gain2_Gain_cx - // Referenced by: '/Gain2' - - real32_T Gain_Gain_h3; // Computed Parameter: Gain_Gain_h3 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jb; // Computed Parameter: Gain1_Gain_jb - // Referenced by: '/Gain1' - - real32_T Gain_Gain_j2; // Computed Parameter: Gain_Gain_j2 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jp; // Computed Parameter: Gain1_Gain_jp - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_ko; // Computed Parameter: Gain2_Gain_ko - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_a2; // Computed Parameter: Gain2_Gain_a2 - // Referenced by: '/Gain2' - - int32_T Switch3_Threshold; // Computed Parameter: Switch3_Threshold - // Referenced by: '/Switch3' - - int32_T Switch2_Threshold; // Computed Parameter: Switch2_Threshold - // Referenced by: '/Switch2' - - int32_T Merge_2_InitialOutput; // Computed Parameter: Merge_2_InitialOutput - // Referenced by: '/Merge' - - int32_T Switch2_Threshold_b; // Computed Parameter: Switch2_Threshold_b - // Referenced by: '/Switch2' - - int32_T Switch3_Threshold_d; // Computed Parameter: Switch3_Threshold_d - // Referenced by: '/Switch3' - - uint32_T Constant_Value_h; // Expression: uint32(1) - // Referenced by: '/Constant' - - uint32_T Delay_DelayLength; // Computed Parameter: Delay_DelayLength - // Referenced by: '/Delay' - - uint32_T BitwiseOperator1_BitMask_l; // Expression: BitMask - // Referenced by: '/Bitwise Operator1' - - uint16_T Constant_Value_ln; // Expression: uint16(1) - // Referenced by: '/Constant' - - uint16_T Constant_Value_c; // Expression: uint16(2) - // Referenced by: '/Constant' - - uint16_T Constant_Value_o; // Computed Parameter: Constant_Value_o - // Referenced by: '/Constant' - - uint8_T Merge_5_InitialOutput; // Computed Parameter: Merge_5_InitialOutput - // Referenced by: '/Merge' - - uint8_T Constant2_Value_n4; // Expression: uint8(0) - // Referenced by: '/Constant2' - - uint8_T Constant_Value_dd; // Expression: uint8(0) - // Referenced by: '/Constant' - - uint8_T Constant_Value_oa; // Expression: uint8(0) - // Referenced by: '/Constant' - - uint8_T Constant1_Value_f; // Expression: uint8(0) - // Referenced by: '/Constant1' - - uint8_T Out1_Y0; // Computed Parameter: Out1_Y0 - // Referenced by: '/Out1' - - uint8_T UnitDelay16_InitialCondition;// Computed Parameter: UnitDelay16_InitialCondition - // Referenced by: '/Unit Delay16' - - uint8_T UnitDelay17_InitialCondition;// Computed Parameter: UnitDelay17_InitialCondition - // Referenced by: '/Unit Delay17' - - uint8_T Constant_Value_oq; // Expression: const - // Referenced by: '/Constant' - - uint8_T Saturation_LowerSat_n; // Computed Parameter: Saturation_LowerSat_n - // Referenced by: '/Saturation' - - uint8_T Switch_Threshold; // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - uint8_T Constant_Value_lf; // Expression: const - // Referenced by: '/Constant' - - uint8_T Constant_Value_js; // Expression: const - // Referenced by: '/Constant' - - uint8_T Constant_Value_e; // Expression: const - // Referenced by: '/Constant' - - P_Normalize_est_estimator_T Normalize_i;// '/Normalize' - P_Normalize_est_estimator_T Normalize_h;// '/Normalize' - P_Normalize_est_estimator_T Normalize;// '/Normalize' - P_CoreSubsys_est_estimator_g_T CoreSubsys_l;// '/CoreSubsys' - P_CoreSubsys_est_estimator_T CoreSubsys;// '/CoreSubsys' -}; - -// Real-time Model Data Structure -struct tag_RTM_est_estimator_T { - const char_T * volatile errorStatus; - B_est_estimator_T *blockIO; - P_est_estimator_T *defaultParam; - boolean_T paramIsMalloced; - DW_est_estimator_T *dwork; -}; - -#ifdef __cplusplus - -extern "C" { - -#endif - -#ifdef __cplusplus - -} -#endif - -// External data declarations for dependent source files -extern const cvs_landmark_msg est_estimator_rtZcvs_landmark_msg;// cvs_landmark_msg ground -extern const cvs_registration_pulse est_estimator_rtZcvs_registration_pulse;// cvs_registration_pulse ground -extern const cvs_optical_flow_msg est_estimator_rtZcvs_optical_flow_msg;// cvs_optical_flow_msg ground -extern const cvs_handrail_msg est_estimator_rtZcvs_handrail_msg;// cvs_handrail_msg ground -extern const imu_msg est_estimator_rtZimu_msg;// imu_msg ground -extern const cmc_msg est_estimator_rtZcmc_msg;// cmc_msg ground -extern const kfl_msg est_estimator_rtZkfl_msg;// kfl_msg ground - -#ifdef __cplusplus - -extern "C" { - -#endif - - extern const char *RT_MEMORY_ALLOCATION_ERROR; - -#ifdef __cplusplus - -} -#endif - -extern P_est_estimator_T est_estimator_P;// parameters - -#ifdef __cplusplus - -extern "C" { - -#endif - - // Model entry point functions - extern RT_MODEL_est_estimator_T *est_estimator(cvs_landmark_msg - *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]); - extern void est_estimator_initialize(RT_MODEL_est_estimator_T *const - est_estimator_M, cvs_landmark_msg *est_estimator_U_landmark_msg, - cvs_registration_pulse *est_estimator_U_VisionRegistration, - cvs_optical_flow_msg *est_estimator_U_cvs_optical_flow_msg_n, - cvs_handrail_msg *est_estimator_U_handrail_msg, imu_msg - *est_estimator_U_imu_msg_c, cmc_msg *est_estimator_U_cmc_msg_o, real32_T - est_estimator_U_Q_ISS2B[4], kfl_msg *est_estimator_Y_kfl_msg_h, - ase_cov_datatype est_estimator_Y_P_out[13689]); - extern void est_estimator_step(RT_MODEL_est_estimator_T *const est_estimator_M, - cvs_landmark_msg *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]); - extern void est_estimator_terminate(RT_MODEL_est_estimator_T * est_estimator_M); - -#ifdef __cplusplus - -} -#endif - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Note that this particular code originates from a subsystem build, -// and has its own system numbers different from the parent model. -// Refer to the system hierarchy for this subsystem below, and use the -// MATLAB hilite_system command to trace the generated code back -// to the parent model. For example, -// -// hilite_system('astrobee/fsw_lib/est_estimator') - opens subsystem astrobee/fsw_lib/est_estimator -// hilite_system('astrobee/fsw_lib/est_estimator/Kp') - opens and selects block Kp -// -// Here is the system hierarchy for this model -// -// '' : 'astrobee/fsw_lib' -// '' : 'astrobee/fsw_lib/est_estimator' -// '' : 'astrobee/fsw_lib/est_estimator/Delay - Initial Conditions' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep' -// '' : 'astrobee/fsw_lib/est_estimator/predictor' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/If Action Subsystem1' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/ML_correector' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/update_validity_check' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1/Compute Global positions of Handrail Features' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1/Compute Residual and H' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H/Compare To Constant' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H/Compare To Constant2' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H/Compute Residual and H' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/ML_correector/apply_delta_state' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/Compute_DeltaState_and_Covariance' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/Simulink Compute H and R' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/apply_delta_state' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/compute_of_global_points' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/Simulink Compute H and R/compress_of_residual_and_h' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/Simulink Compute H and R/compute_of_residual_and_h' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant1' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant2' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant3' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant4' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant5' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant6' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant7' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant8' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Enabled Row-Wise SUM' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/Detect Change2' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/Detect Change3' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/filter' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/filter_with_HP_filter' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/filter/MATLAB Function1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/filter_with_HP_filter/MATLAB Function1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep/Bit Clear' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep/Detect Change' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep/dummy_subsystem' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/handrail_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/landmark_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/optical_flow_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/handrail_prep/Detect Change6' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/handrail_prep/Detect Change7' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/landmark_prep/Detect Change' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/landmark_prep/Detect Change1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/optical_flow_prep/Detect Change4' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/optical_flow_prep/Detect Change5' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant1' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant2' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant3' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant4' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant5' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant6' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant7' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Filter_10hz_1order' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/MATLAB Function2' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/diag' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/create_omega_matrix' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/create_omega_matrix1' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/Compare To Constant' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/create_omega_matrix' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/create_omega_matrix1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/Compare To Constant1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/Compare To Constant3' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/Compare To Constant5' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/Compare To Zero' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem4' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/create_omega_matrix' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/create_omega_matrix1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/skew_symetric_matrix_operator/Data Type Conversion Inherited' - -#endif // RTW_HEADER_est_estimator_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_data.cpp b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_data.cpp deleted file mode 100644 index 2a786a1bd7..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_data.cpp +++ /dev/null @@ -1,1943 +0,0 @@ -// -// File: est_estimator_data.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "est_estimator.h" -#include "est_estimator_private.h" - -// Block parameters (auto storage) -P_est_estimator_T est_estimator_P = { - 0.01, - 0.016, - 0.016, - - { -0.0F, -0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F - }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - - { 209.439514F, 261.799408F, 293.215332F }, - - { -0.1308F, -0.0723F, -0.0158F }, - - { 0.0247F, 0.0183F, 0.0094F }, - - { 0.093F, -0.0605F, -0.092F }, - - { -0.1578F, 0.0326F, -0.026F }, - - { 0.5F, -0.5F, -0.5F, 0.5F }, - - { 0.5F, 0.5F, 0.5F, 0.5F }, - - { 0.0F, -0.707107F, 0.0F, 0.707107F }, - - { 0.0F, 0.0F, 0.707107F, 0.707107F }, - - { 5.9705E-8F, 5.9705E-8F, 5.9705E-8F, 3.04617E-18F, 3.04617E-18F, 3.04617E-18F, - 1.0E-8F, 1.0E-8F, 1.0E-8F, 1.0E-10F, 1.0E-10F, 1.0E-10F }, - 6250.0F, - 0.01F, - 0.0225F, - 2.0F, - 0.998468F, - 0.00120481F, - - { 0.0F, 0.0F, 9.80665F }, - 0.01F, - 0.5F, - 20.0F, - 0.02F, - 29.43F, - 2.61799F, - 4.0F, - 15.0F, - 0.0333333F, - 0.998468F, - 0.00163652F, - 1.0F, - 1.0F, - 0.00121847F, - - { 0.0F, 0.0F, -0.7F }, - - { 0.0247F, 0.0183F, -0.6906F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0001F, 0.0001F, 0.0001F, 2.0E-11F, 2.0E-11F, 2.0E-11F, 0.0001F, 0.0001F, - 0.0001F, 1.0E-9F, 1.0E-9F, 1.0E-9F, 0.001F, 0.001F, 0.001F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 4.0F, - 0.00163652F, - 1.0E-5F, - - { 2.16F, -1.84F }, - - { 2.0F, -2.0F }, - 0.001F, - 0.0001F, - 200000.0F, - 131070U, - 1U, - 0U, - 2U, - 0U, - 0U, - 1U, - 1U, - 0U, - 2U, - 3U, - 1U, - 0U, - 0U, - 1U, - 1U, - 1U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 65532U, - 1U, - 1U, - 3U, - 0U, - 0U, - - { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS - }, - - { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS - }, - - { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS - }, - - { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS - }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 0.5F, - 0.5F, - 0.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 48.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - 0.5F, - 0.5F, - 0.0F, - - { 0.0F, 0.0F, 0.0F }, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 48.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F - }, - - { 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - 0.0F, - 0.0F, - 0.5F, - 0.5F, - 0.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 0.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 48.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - - { 0.0F, 0.0F, 0.0F }, - - { -1.0F, -0.0F, -0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, -0.0F, -1.0F, -0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, -0.0F, -0.0F, -1.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, - 0.0F }, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 1, - 1, - 0, - 1, - 1, - 1U, - 1U, - 131070U, - 1U, - 2U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 10U, - 1U, - 0U, - 15U, - 4U, - 10U, - - // Start of '/Normalize' - { - -1.0 - } - // End of '/Normalize' - , - - // Start of '/Normalize' - { - -1.0 - } - // End of '/Normalize' - , - - // Start of '/Normalize' - { - -1.0 - } - // End of '/Normalize' - , - - // Start of '/CoreSubsys' - { - 0.2604F, - - { 1.0F, -0.7396F }, - 0.0F, - 0.0F, - 0.0F - } - // End of '/CoreSubsys' - , - - // Start of '/CoreSubsys' - { - 0.2604F, - - { 1.0F, -0.7396F }, - 0.0F, - 0.0F - } - // End of '/CoreSubsys' -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_private.h b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_private.h deleted file mode 100644 index 3c20d88349..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_private.h +++ /dev/null @@ -1,66 +0,0 @@ -// -// File: est_estimator_private.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_est_estimator_private_h_ -#define RTW_HEADER_est_estimator_private_h_ -#include "rtwtypes.h" -#include "est_estimator.h" -#if !defined(rt_VALIDATE_MEMORY) -#define rt_VALIDATE_MEMORY(S, ptr) if(!(ptr)) {\ - rtmSetErrorStatus(est_estimator_M, RT_MEMORY_ALLOCATION_ERROR);\ - } -#endif - -#if !defined(rt_FREE) -#if !defined(_WIN32) -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((ptr));\ - (ptr) = (NULL);\ - } -#else - -// Visual and other windows compilers declare free without const -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((void *)(ptr));\ - (ptr) = (NULL);\ - } -#endif -#endif - -void est_estima_MATLABFunction1_Init(DW_MATLABFunction1_est_estima_T *localDW); -void est_estim_MATLABFunction1_Start(B_MATLABFunction1_est_estimat_T *localB); -void est_estimator_MATLABFunction1(real_T rtu_Ts, real32_T rtu_impeller_speed, - B_MATLABFunction1_est_estimat_T *localB, DW_MATLABFunction1_est_estima_T - *localDW); -void est_estimator_MATLABFunction(const real32_T rtu_u[16], - B_MATLABFunction_est_estimato_T *localB); -void est_estimator_Normalize(const real32_T rtu_q_in[4], real32_T - rty_positive_scalar_q[4], P_Normalize_est_estimator_T *localP); -void est_estimator_Normalize_p(const real32_T rtu_Vec[4], real32_T rtu_Magnitude, - real32_T rty_Normalized_Vec[4]); -void est_estimato_IfActionSubsystem1(const kfl_msg *rtu_state_in, const - ase_cov_datatype rtu_P_in[13689], const real_T rtu_aug_velocity[3], const - real_T rtu_aug_velocity_p[3], const real_T rtu_aug_velocity_l[48], const - real_T rtu_aug_velocity_o[48], kfl_msg *rty_state_out, ase_cov_datatype - rty_P_out[13689], real_T rty_aug_velocity_out[3], real_T - rty_aug_velocity_out_p[3], real_T rty_aug_velocity_out_l[48], real_T - rty_aug_velocity_out_o[48]); - -#endif // RTW_HEADER_est_estimator_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_types.h b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_types.h deleted file mode 100644 index cbaa3256b9..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_types.h +++ /dev/null @@ -1,186 +0,0 @@ -// -// File: est_estimator_types.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_est_estimator_types_h_ -#define RTW_HEADER_est_estimator_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_cvs_landmark_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_landmark_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_landmarks[150]; - real32_T cvs_observations[100]; - uint8_T cvs_valid_flag[50]; -} cvs_landmark_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_registration_pulse_ -#define DEFINED_TYPEDEF_FOR_cvs_registration_pulse_ - -typedef struct { - uint8_T cvs_ar_tag_pulse; - uint8_T cvs_landmark_pulse; - uint8_T cvs_optical_flow_pulse; - uint8_T cvs_handrail_pulse; -} cvs_registration_pulse; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_optical_flow_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_optical_flow_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_observations[1600]; - uint8_T cvs_valid_flag[800]; - real32_T cvs_id_tag[50]; -} cvs_optical_flow_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_handrail_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_handrail_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_landmarks[150]; - real32_T cvs_observations[150]; - uint8_T cvs_valid_flag[50]; - uint8_T cvs_3d_knowledge_flag; - real32_T cvs_handrail_local_pos[3]; - real32_T cvs_handrail_local_quat[4]; - uint8_T cvs_handrail_update_global_pose_flag; -} cvs_handrail_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_imu_msg_ -#define DEFINED_TYPEDEF_FOR_imu_msg_ - -typedef struct { - uint32_T imu_timestamp_sec; - uint32_T imu_timestamp_nsec; - real32_T imu_A_B_ECI_sensor[3]; - real32_T imu_accel_bias[3]; - real32_T imu_omega_B_ECI_sensor[3]; - real32_T imu_gyro_bias[3]; - uint8_T imu_validity_flag; - uint8_T imu_sat_flag; -} imu_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmc_state_cmd_ -#define DEFINED_TYPEDEF_FOR_cmc_state_cmd_ - -typedef struct { - uint32_T timestamp_sec; - uint32_T timestamp_nsec; - real32_T P_B_ISS_ISS[3]; - real32_T V_B_ISS_ISS[3]; - real32_T A_B_ISS_ISS[3]; - real32_T quat_ISS2B[4]; - real32_T omega_B_ISS_B[3]; - real32_T alpha_B_ISS_B[3]; -} cmc_state_cmd; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmc_msg_ -#define DEFINED_TYPEDEF_FOR_cmc_msg_ - -typedef struct { - cmc_state_cmd cmc_state_cmd_a; - cmc_state_cmd cmc_state_cmd_b; - uint8_T cmc_mode_cmd; - uint8_T speed_gain_cmd; - uint8_T localization_mode_cmd; - real32_T att_kp[3]; - real32_T att_ki[3]; - real32_T omega_kd[3]; - real32_T pos_kp[3]; - real32_T pos_ki[3]; - real32_T vel_kd[3]; - real32_T center_of_mass[3]; - real32_T inertia_matrix[9]; - real32_T mass; -} cmc_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ase_cov_datatype_ -#define DEFINED_TYPEDEF_FOR_ase_cov_datatype_ - -typedef real32_T ase_cov_datatype; -typedef creal32_T case_cov_datatype; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_kfl_msg_ -#define DEFINED_TYPEDEF_FOR_kfl_msg_ - -typedef struct { - real32_T quat_ISS2B[4]; - real32_T omega_B_ISS_B[3]; - real32_T gyro_bias[3]; - real32_T V_B_ISS_ISS[3]; - real32_T A_B_ISS_ISS[3]; - real32_T accel_bias[3]; - real32_T P_B_ISS_ISS[3]; - uint8_T confidence; - uint32_T aug_state_enum; - real32_T ml_quat_ISS2cam[4]; - real32_T ml_P_cam_ISS_ISS[3]; - real32_T of_quat_ISS2cam[64]; - real32_T of_P_cam_ISS_ISS[48]; - ase_cov_datatype cov_diag[117]; - uint16_T kfl_status; - uint8_T update_OF_tracks_cnt; - uint8_T update_ML_features_cnt; - real_T of_mahal_distance[50]; - real_T ml_mahal_distance[50]; - real32_T hr_P_hr_ISS_ISS[3]; - real32_T hr_quat_ISS2hr[4]; - real32_T P_EST_ISS_ISS[3]; -} kfl_msg; - -#endif - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_est_estimator_T_ P_CoreSubsys_est_estimator_T; - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_est_estimator_g_T_ P_CoreSubsys_est_estimator_g_T; - -// Parameters for system: '/Normalize' -typedef struct P_Normalize_est_estimator_T_ P_Normalize_est_estimator_T; - -// Parameters (auto storage) -typedef struct P_est_estimator_T_ P_est_estimator_T; - -// Forward declaration for rtModel -typedef struct tag_RTM_est_estimator_T RT_MODEL_est_estimator_T; - -#endif // RTW_HEADER_est_estimator_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.cpp b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.cpp deleted file mode 100644 index 4f8553077e..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// Autogenerated file, see write_tunable_param_functions for details - -#include "est_tunable_funcs.h" -#include -#include -#include - -// Do config setup and add includes -void est_ReadParams(config_reader::ConfigReader* config, RT_MODEL_est_estimator_T* est) { - - auto & p = est->defaultParam; - - if (!msg_conversions::config_read_array(config, "tun_abp_p_dockcam_imu_est", 3 , p->tun_abp_p_dockcam_imu_est)) - ROS_FATAL("Unspecified tun_abp_p_dockcam_imu_est."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_imu_body_body", 3 , p->tun_abp_p_imu_body_body)) - ROS_FATAL("Unspecified tun_abp_p_imu_body_body."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_navcam_imu_est", 3 , p->tun_abp_p_navcam_imu_est)) - ROS_FATAL("Unspecified tun_abp_p_navcam_imu_est."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_perchcam_imu_est", 3 , p->tun_abp_p_perchcam_imu_est)) - ROS_FATAL("Unspecified tun_abp_p_perchcam_imu_est."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2dockcam", 4 , p->tun_abp_q_body2dockcam)) - ROS_FATAL("Unspecified tun_abp_q_body2dockcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2navcam", 4 , p->tun_abp_q_body2navcam)) - ROS_FATAL("Unspecified tun_abp_q_body2navcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2perchcam", 4 , p->tun_abp_q_body2perchcam)) - ROS_FATAL("Unspecified tun_abp_q_body2perchcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_quat_body2imu", 4 , p->tun_abp_quat_body2imu)) - ROS_FATAL("Unspecified tun_abp_quat_body2imu."); - if (!msg_conversions::config_read_array(config, "tun_ase_Q_imu", 12 , p->tun_ase_Q_imu)) - ROS_FATAL("Unspecified tun_ase_Q_imu."); - if (!config->GetReal("tun_ase_acquired_ticks", &p->tun_ase_acquired_ticks)) - ROS_FATAL("Unspecified tun_ase_acquired_ticks."); - if (!config->GetReal("tun_ase_converged_thresh", &p->tun_ase_converged_thresh)) - ROS_FATAL("Unspecified tun_ase_converged_thresh."); - if (!config->GetReal("tun_ase_diverged_thresh", &p->tun_ase_diverged_thresh)) - ROS_FATAL("Unspecified tun_ase_diverged_thresh."); - if (!config->GetReal("tun_ase_dock_r_mag", &p->tun_ase_dock_r_mag)) - ROS_FATAL("Unspecified tun_ase_dock_r_mag."); - if (!config->GetReal("tun_ase_dockcam_distortion", &p->tun_ase_dockcam_distortion)) - ROS_FATAL("Unspecified tun_ase_dockcam_distortion."); - if (!config->GetReal("tun_ase_dockcam_inv_focal_length", &p->tun_ase_dockcam_inv_focal_length)) - ROS_FATAL("Unspecified tun_ase_dockcam_inv_focal_length."); - if (!msg_conversions::config_read_array(config, "tun_ase_gravity_accel", 3 , p->tun_ase_gravity_accel)) - ROS_FATAL("Unspecified tun_ase_gravity_accel."); - if (!config->GetReal("tun_ase_hr_distance_r", &p->tun_ase_hr_distance_r)) - ROS_FATAL("Unspecified tun_ase_hr_distance_r."); - if (!config->GetReal("tun_ase_hr_r_mag", &p->tun_ase_hr_r_mag)) - ROS_FATAL("Unspecified tun_ase_hr_r_mag."); - if (!config->GetReal("tun_ase_mahal_distance_max", &p->tun_ase_mahal_distance_max)) - ROS_FATAL("Unspecified tun_ase_mahal_distance_max."); - if (!config->GetReal("tun_ase_map_error", &p->tun_ase_map_error)) - ROS_FATAL("Unspecified tun_ase_map_error."); - if (!config->GetReal("tun_ase_max_accel", &p->tun_ase_max_accel)) - ROS_FATAL("Unspecified tun_ase_max_accel."); - if (!config->GetReal("tun_ase_max_gyro", &p->tun_ase_max_gyro)) - ROS_FATAL("Unspecified tun_ase_max_gyro."); - if (!config->GetReal("tun_ase_min_ar_meas", &p->tun_ase_min_ar_meas)) - ROS_FATAL("Unspecified tun_ase_min_ar_meas."); - if (!config->GetReal("tun_ase_min_ml_meas", &p->tun_ase_min_ml_meas)) - ROS_FATAL("Unspecified tun_ase_min_ml_meas."); - if (!config->GetReal("tun_ase_ml_forward_projection_time", &p->tun_ase_ml_forward_projection_time)) - ROS_FATAL("Unspecified tun_ase_ml_forward_projection_time."); - if (!config->GetReal("tun_ase_navcam_distortion", &p->tun_ase_navcam_distortion)) - ROS_FATAL("Unspecified tun_ase_navcam_distortion."); - if (!config->GetReal("tun_ase_navcam_inv_focal_length", &p->tun_ase_navcam_inv_focal_length)) - ROS_FATAL("Unspecified tun_ase_navcam_inv_focal_length."); - if (!config->GetReal("tun_ase_of_r_mag", &p->tun_ase_of_r_mag)) - ROS_FATAL("Unspecified tun_ase_of_r_mag."); - if (!config->GetReal("tun_ase_q_saturated_accel", &p->tun_ase_q_saturated_accel)) - ROS_FATAL("Unspecified tun_ase_q_saturated_accel."); - if (!config->GetReal("tun_ase_q_saturated_gyro", &p->tun_ase_q_saturated_gyro)) - ROS_FATAL("Unspecified tun_ase_q_saturated_gyro."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_P_B_ISS_ISS", 3 , p->tun_ase_state_ic_P_B_ISS_ISS)) - ROS_FATAL("Unspecified tun_ase_state_ic_P_B_ISS_ISS."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_P_EST_ISS_ISS", 3 , p->tun_ase_state_ic_P_EST_ISS_ISS)) - ROS_FATAL("Unspecified tun_ase_state_ic_P_EST_ISS_ISS."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_V_B_ISS_ISS", 3 , p->tun_ase_state_ic_V_B_ISS_ISS)) - ROS_FATAL("Unspecified tun_ase_state_ic_V_B_ISS_ISS."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_cov_diag", 15 , p->tun_ase_state_ic_cov_diag)) - ROS_FATAL("Unspecified tun_ase_state_ic_cov_diag."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_omega_B_ISS_B", 3 , p->tun_ase_state_ic_omega_B_ISS_B)) - ROS_FATAL("Unspecified tun_ase_state_ic_omega_B_ISS_B."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_quat_ISS2B", 4 , p->tun_ase_state_ic_quat_ISS2B)) - ROS_FATAL("Unspecified tun_ase_state_ic_quat_ISS2B."); - if (!config->GetReal("tun_ase_vis_r_mag", &p->tun_ase_vis_r_mag)) - ROS_FATAL("Unspecified tun_ase_vis_r_mag."); - if (!config->GetReal("tun_ase_vocam_inv_focal_length", &p->tun_ase_vocam_inv_focal_length)) - ROS_FATAL("Unspecified tun_ase_vocam_inv_focal_length."); - if (!config->GetReal("tun_aug_ic_cov", &p->tun_aug_ic_cov)) - ROS_FATAL("Unspecified tun_aug_ic_cov."); - if (!msg_conversions::config_read_array(config, "tun_grav_hp_den", 2 , p->tun_grav_hp_den)) - ROS_FATAL("Unspecified tun_grav_hp_den."); - if (!msg_conversions::config_read_array(config, "tun_grav_hp_num", 2 , p->tun_grav_hp_num)) - ROS_FATAL("Unspecified tun_grav_hp_num."); - if (!config->GetReal("tun_ic_cov_pos", &p->tun_ic_cov_pos)) - ROS_FATAL("Unspecified tun_ic_cov_pos."); - if (!config->GetReal("tun_ic_cov_quat", &p->tun_ic_cov_quat)) - ROS_FATAL("Unspecified tun_ic_cov_quat."); - if (!config->GetReal("tun_max_mahal_reject_frames", &p->tun_max_mahal_reject_frames)) - ROS_FATAL("Unspecified tun_max_mahal_reject_frames."); - bool ase_enable_of; - if (!config->GetBool("tun_ase_enable_of", &ase_enable_of)) - ROS_FATAL("Unspecified tun_ase_enable_of."); - p->tun_ase_enable_of = ase_enable_of; - bool ase_gravity_removal; - if (!config->GetBool("tun_ase_gravity_removal", &ase_gravity_removal)) - ROS_FATAL("Unspecified tun_ase_gravity_removal."); - p->tun_ase_gravity_removal = ase_gravity_removal; - bool grav_hp_enable_f; - if (!config->GetBool("tun_grav_hp_enable_f", &grav_hp_enable_f)) - ROS_FATAL("Unspecified tun_grav_hp_enable_f."); - p->tun_grav_hp_enable_f = grav_hp_enable_f; -} diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.h b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.h deleted file mode 100644 index 936b2baafb..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef READ_PARAMS_CPP -#define READ_PARAMS_CPP - -namespace config_reader { - class ConfigReader; -} - -#include "est_estimator.h" -void est_ReadParams(config_reader::ConfigReader* config, RT_MODEL_est_estimator_T* est); - -#endif \ No newline at end of file diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/modelsources.txt b/gnc/matlab/code_generation/est_estimator_ert_rtw/modelsources.txt deleted file mode 100644 index bd11587882..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/modelsources.txt +++ /dev/null @@ -1 +0,0 @@ - est_estimator.cpp compute_delta_state_and_cov.cpp apply_delta_state.cpp of_residual_and_h.cpp matrix_multiply.cpp diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/rtw_proj.tmw b/gnc/matlab/code_generation/est_estimator_ert_rtw/rtw_proj.tmw deleted file mode 100644 index e83d0266b9..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/rtw_proj.tmw +++ /dev/null @@ -1,4 +0,0 @@ -Simulink Coder project for est_estimator using . MATLAB root = /usr/local/MATLAB/R2016b. SimStruct date: 22-Jul-2016 13:46:14 -This file is generated by Simulink Coder for use by the make utility -to determine when to rebuild objects when the name of the current Simulink Coder project changes. -The rtwinfomat located at: ../slprj/ert/est_estimator/tmwinternal/binfo.mat diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/buildInfo.mat b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/buildInfo.mat deleted file mode 100644 index 45f503cf95..0000000000 Binary files a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/codeInfo.mat b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/codeInfo.mat deleted file mode 100644 index f9c5fee9ed..0000000000 Binary files a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/codeInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/defines.txt b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/defines.txt deleted file mode 100644 index aad93a8949..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/defines.txt +++ /dev/null @@ -1,15 +0,0 @@ -RT_MALLOC -MODEL=fam_force_allocation_module -NUMST=1 -NCSTATES=0 -HAVESTDIO -UNIX -ONESTEPFCN=1 -TERMFCN=1 -MAT_FILE=0 -MULTI_INSTANCE_CODE=1 -INTEGER_CODE=0 -MT=0 -CLASSIC_INTERFACE=0 -ALLOCATIONFCN=1 -TID01EQ=0 diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/ert_main.cpp b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/ert_main.cpp deleted file mode 100644 index 0822671775..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/ert_main.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// -// File: ert_main.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:44:37 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include -#include // This ert_main.c example uses printf/fflush -#include "fam_force_allocation_module.h" // Model's header file -#include "rtwtypes.h" - -// '/current_time' -static ex_time_msg fam_force_allocation_module_U_current_time; - -// '/cmd_msg' -static cmd_msg fam_force_allocation_module_U_cmd_msg_f; - -// '/ctl_msg' -static ctl_msg fam_force_allocation_module_U_ctl_msg_n; - -// '/cmc_msg' -static cmc_msg fam_force_allocation_module_U_cmc_msg_h; - -// '/act_msg' -static act_msg fam_force_allocation_module_Y_act_msg_c; -const char *RT_MEMORY_ALLOCATION_ERROR = "memory allocation error"; - -// -// Associating rt_OneStep with a real-time clock or interrupt service routine -// is what makes the generated code "real-time". The function rt_OneStep is -// always associated with the base rate of the model. Subrates are managed -// by the base rate from inside the generated code. Enabling/disabling -// interrupts and floating point context switches are target specific. This -// example code indicates where these should take place relative to executing -// the generated code step function. Overrun behavior should be tailored to -// your application needs. This example simply sets an error status in the -// real-time model and returns from rt_OneStep. -// -void rt_OneStep(RT_MODEL_fam_force_allocation_T *const - fam_force_allocation_module_M); -void rt_OneStep(RT_MODEL_fam_force_allocation_T *const - fam_force_allocation_module_M) -{ - static boolean_T OverrunFlag = false; - - // Disable interrupts here - - // Check for overrun - if (OverrunFlag) { - rtmSetErrorStatus(fam_force_allocation_module_M, "Overrun"); - return; - } - - OverrunFlag = true; - - // Save FPU context here (if necessary) - // Re-enable timer or interrupt here - // Set model inputs here - - // Step the model - fam_force_allocation_module_step(fam_force_allocation_module_M, - &fam_force_allocation_module_U_current_time, - &fam_force_allocation_module_U_cmd_msg_f, - &fam_force_allocation_module_U_ctl_msg_n, - &fam_force_allocation_module_U_cmc_msg_h, - &fam_force_allocation_module_Y_act_msg_c); - - // Get model outputs here - - // Indicate task complete - OverrunFlag = false; - - // Disable interrupts here - // Restore FPU context here (if necessary) - // Enable interrupts here -} - -// -// The example "main" function illustrates what is required by your -// application code to initialize, execute, and terminate the generated code. -// Attaching rt_OneStep to a real-time clock is target specific. This example -// illustrates how you do this relative to initializing the model. -// -int_T main(int_T argc, const char *argv[]) -{ - RT_MODEL_fam_force_allocation_T *fam_force_allocation_module_M; - - // Unused arguments - (void)(argc); - (void)(argv); - - // Allocate model data - fam_force_allocation_module_M = fam_force_allocation_module - (&fam_force_allocation_module_U_current_time, - &fam_force_allocation_module_U_cmd_msg_f, - &fam_force_allocation_module_U_ctl_msg_n, - &fam_force_allocation_module_U_cmc_msg_h, - &fam_force_allocation_module_Y_act_msg_c); - if (fam_force_allocation_module_M == NULL) { - (void)fprintf(stderr,"Memory allocation error during model " - "registration"); - return(1); - } - - if (rtmGetErrorStatus(fam_force_allocation_module_M) != NULL) { - (void)fprintf(stderr,"Error during model registration: %s\n", - rtmGetErrorStatus(fam_force_allocation_module_M)); - - // Disable rt_OneStep() here - - // Terminate model - fam_force_allocation_module_terminate(fam_force_allocation_module_M); - return(1); - } - - // Initialize model - fam_force_allocation_module_initialize(fam_force_allocation_module_M, - &fam_force_allocation_module_U_current_time, - &fam_force_allocation_module_U_cmd_msg_f, - &fam_force_allocation_module_U_ctl_msg_n, - &fam_force_allocation_module_U_cmc_msg_h, - &fam_force_allocation_module_Y_act_msg_c); - - // Attach rt_OneStep to a timer or interrupt service routine with - // period 0.016 seconds (the model's base sample time) here. The - // call syntax for rt_OneStep is - // - // rt_OneStep(fam_force_allocation_module_M); - - printf("Warning: The simulation will run forever. " - "Generated ERT main won't simulate model step behavior. " - "To change this behavior select the 'MAT-file logging' option.\n"); - fflush((NULL)); - while (rtmGetErrorStatus(fam_force_allocation_module_M) == (NULL)) { - // Perform other application tasks here - } - - // Disable rt_OneStep() here - - // Terminate model - fam_force_allocation_module_terminate(fam_force_allocation_module_M); - return 0; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module.cpp b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module.cpp deleted file mode 100644 index 5bd63f3671..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module.cpp +++ /dev/null @@ -1,1043 +0,0 @@ -// -// File: fam_force_allocation_module.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:44:37 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "fam_force_allocation_module.h" -#include "fam_force_allocation_module_private.h" - -const act_msg fam_force_allocation_module_rtZact_msg = { - 0U, // act_timestamp_sec - 0U, // act_timestamp_nsec - - { - 0U, 0U } - , // act_impeller_speed_cmd - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // act_servo_pwm_cmd - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // act_nozzle_theta - - { - 0.0F, 0.0F, 0.0F } - , // act_predicted_force_B - - { - 0.0F, 0.0F, 0.0F } - // act_predicted_torque_B -} ; // act_msg ground - -const cmd_msg fam_force_allocation_module_r_0 = { 0U,// cmd_timestamp_sec - 0U, // cmd_timestamp_nsec - 0U, // cmd_mode - 0U, // speed_gain_cmd - 0U, // cmd_B_inuse - { 0.0F, 0.0F, 0.0F }, // traj_pos - { 0.0F, 0.0F, 0.0F }, // traj_vel - { 0.0F, 0.0F, 0.0F }, // traj_accel - { 0.0F, 0.0F, 0.0F, 0.0F }, // traj_quat - { 0.0F, 0.0F, 0.0F }, // traj_omega - { 0.0F, 0.0F, 0.0F } // traj_alpha -}; - -const ctl_msg fam_force_allocation_module_r_1 = { { 0.0F, 0.0F, 0.0F },// body_force_cmd - { 0.0F, 0.0F, 0.0F }, // body_accel_cmd - { 0.0F, 0.0F, 0.0F }, // pos_err - { 0.0F, 0.0F, 0.0F }, // pos_err_int - { 0.0F, 0.0F, 0.0F }, // body_torque_cmd - { 0.0F, 0.0F, 0.0F }, // body_alpha_cmd - { 0.0F, 0.0F, 0.0F }, // att_err - 0.0F, // att_err_mag - { 0.0F, 0.0F, 0.0F }, // att_err_int - 0U, // ctl_status - 0.0F, // traj_error_pos - 0.0F, // traj_error_att - 0.0F, // traj_error_vel - 0.0F // traj_error_omega -}; - -const cmc_msg fam_force_allocation_module_r_2 = { { 0U,// timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmc_state_cmd_a - { 0U, // timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmc_state_cmd_b - 0U, // cmc_mode_cmd - 0U, // speed_gain_cmd - 0U, // localization_mode_cmd - { 0.0F, 0.0F, 0.0F }, // att_kp - { 0.0F, 0.0F, 0.0F }, // att_ki - { 0.0F, 0.0F, 0.0F }, // omega_kd - { 0.0F, 0.0F, 0.0F }, // pos_kp - { 0.0F, 0.0F, 0.0F }, // pos_ki - { 0.0F, 0.0F, 0.0F }, // vel_kd - { 0.0F, 0.0F, 0.0F }, // center_of_mass - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// inertia_matrix - 0.0F // mass -}; - -const ex_time_msg fam_force_allocation_module_rtZ = { 0U,// timestamp_sec - 0U // timestamp_nsec -}; - -// Model step function -void fam_force_allocation_module_step(RT_MODEL_fam_force_allocation_T *const - fam_force_allocation_module_M, ex_time_msg - *fam_force_allocation_module_U_current_time, cmd_msg - *fam_force_allocation_module_U_cmd_msg_f, ctl_msg - *fam_force_allocation_module_U_ctl_msg_n, cmc_msg - *fam_force_allocation_module_U_cmc_msg_h, act_msg - *fam_force_allocation_module_Y_act_msg_c) -{ - P_fam_force_allocation_module_T *fam_force_allocation_module_P = - ((P_fam_force_allocation_module_T *) - fam_force_allocation_module_M->defaultParam); - B_fam_force_allocation_module_T *fam_force_allocation_module_B = - ((B_fam_force_allocation_module_T *) fam_force_allocation_module_M->blockIO); - DW_fam_force_allocation_modul_T *fam_force_allocation_module_DW = - ((DW_fam_force_allocation_modul_T *) fam_force_allocation_module_M->dwork); - real32_T nozzle_moment_arm[36]; - real32_T c[36]; - real32_T X[72]; - real32_T A[72]; - real32_T V[36]; - int32_T r; - int32_T vcol; - real32_T tol; - real32_T U[72]; - boolean_T b_p; - int32_T ar; - int32_T ia; - int32_T ic; - int32_T b; - int32_T ib; - int32_T b_ic; - uint32_T bpIdx; - real32_T rtb_Switch; - real32_T rtb_Switch_n; - real32_T rtb_thrust_per_nozzle[12]; - real32_T rtb_positive_thrust_per_nozzle[12]; - real32_T rtb_thrust_norm_by_Cd[6]; - real32_T rtb_cmd_area_per_nozzle[6]; - real32_T rtb_calibrated_nozzle_theta[6]; - real32_T rtb_thrust_norm_by_Cd_h[6]; - real32_T rtb_cmd_area_per_nozzle_f[6]; - real32_T rtb_calibrated_nozzle_theta_h[6]; - real32_T rtb_Product9[12]; - real32_T rtb_VectorConcatenate3[12]; - real32_T rtb_thrust2torque_B[36]; - real32_T rtb_Switch8_idx_0; - real32_T rtb_Switch8_idx_2; - real32_T rtb_Switch8_idx_1; - real32_T tmp; - real32_T tmp_0; - real32_T unusedExpr[36]; - real32_T unusedExpr_0[36]; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/cmd_msg' - // Lookup_n-D: '/impeller_speed_lookup' - // RelationalOperator: '/Compare' - - if (fam_force_allocation_module_U_cmd_msg_f->speed_gain_cmd == - fam_force_allocation_module_P->CompareToConstant_const) { - rtb_Switch = fam_force_allocation_module_P->Constant1_Value; - } else { - // Lookup_n-D: '/impeller_speed_lookup' - bpIdx = plook_u32f_binckpan((real32_T) - fam_force_allocation_module_U_cmd_msg_f->speed_gain_cmd, *(real32_T (*)[3]) - &fam_force_allocation_module_P->impeller_speed_lookup_bp01Data[0], 2U, - &fam_force_allocation_module_DW->m_bpIndex_b); - rtb_Switch = fam_force_allocation_module_P->fam_impeller_speeds[bpIdx]; - } - - // End of Switch: '/Switch' - - // DataTypeConversion: '/Data Type Conversion' incorporates: - // Constant: '/Constant' - // Product: '/Divide2' - // Rounding: '/Rounding Function' - - tmp_0 = (real32_T)floor((real_T)rt_roundf_snf - (fam_force_allocation_module_P->abp_impeller_speed2pwm * rtb_Switch)); - if (rtIsNaNF(tmp_0) || rtIsInfF(tmp_0)) { - tmp_0 = 0.0F; - } else { - tmp_0 = (real32_T)fmod((real_T)tmp_0, (real_T)256.0F); - } - - // Switch: '/Switch' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/cmd_msg' - // Lookup_n-D: '/impeller_speed_lookup' - // RelationalOperator: '/Compare' - - if (fam_force_allocation_module_U_cmd_msg_f->speed_gain_cmd == - fam_force_allocation_module_P->CompareToConstant_const_n) { - rtb_Switch_n = fam_force_allocation_module_P->Constant1_Value_h; - } else { - // Lookup_n-D: '/impeller_speed_lookup' - bpIdx = plook_u32f_binckpan((real32_T) - fam_force_allocation_module_U_cmd_msg_f->speed_gain_cmd, *(real32_T (*)[3]) - &fam_force_allocation_module_P->impeller_speed_lookup_bp01Dat_l[0], 2U, - &fam_force_allocation_module_DW->m_bpIndex_e); - rtb_Switch_n = fam_force_allocation_module_P->fam_impeller_speeds[bpIdx]; - } - - // End of Switch: '/Switch' - - // DataTypeConversion: '/Data Type Conversion' incorporates: - // Constant: '/Constant' - // Product: '/Divide2' - // Rounding: '/Rounding Function' - - tmp = (real32_T)floor((real_T)rt_roundf_snf - (fam_force_allocation_module_P->abp_impeller_speed2pwm * - rtb_Switch_n)); - if (rtIsNaNF(tmp) || rtIsInfF(tmp)) { - tmp = 0.0F; - } else { - tmp = (real32_T)fmod((real_T)tmp, (real_T)256.0F); - } - - // Outputs for Enabled SubSystem: '/latch_nozzle_thrust_matricies' incorporates: - // EnablePort: '/Enable' - - // Logic: '/Logical Operator' incorporates: - // Inport: '/cmc_msg' - // RelationalOperator: '/FixPt Relational Operator' - // SignalConversion: '/Signal Conversion' - // UnitDelay: '/Delay Input1' - - if ((fam_force_allocation_module_U_cmc_msg_h->center_of_mass[0] != - fam_force_allocation_module_DW->DelayInput1_DSTATE[0]) || - (fam_force_allocation_module_U_cmc_msg_h->center_of_mass[1] != - fam_force_allocation_module_DW->DelayInput1_DSTATE[1]) || - (fam_force_allocation_module_U_cmc_msg_h->center_of_mass[2] != - fam_force_allocation_module_DW->DelayInput1_DSTATE[2])) { - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - - // MATLAB Function 'fam_force_allocation_module/calc_nozzle_thrust/latch_nozzle_thrust_matricies/MATLAB Function': ':1' - // NOTE: if number of nozzles change, must change this hard coded '12' below - // ':1:5' - for (r = 0; r < 3; r++) { - vcol = (int32_T)(r * 12); - for (ic = 0; ic < 12; ic++) { - nozzle_moment_arm[(int32_T)(vcol + ic)] = - fam_force_allocation_module_U_cmc_msg_h->center_of_mass[r]; - } - } - - for (r = 0; r < 36; r++) { - nozzle_moment_arm[r] = fam_force_allocation_module_P->fam_P_nozzle_B_B[r] - - nozzle_moment_arm[r]; - } - - // [m] Distance from nozzles to CG - // ':1:7' - for (r = 0; r < 12; r++) { - c[r] = nozzle_moment_arm[(int32_T)(r + 12)] * - fam_force_allocation_module_P->fam_nozzle_orientations[(int32_T)(r + 24)] - - nozzle_moment_arm[(int32_T)(r + 24)] * - fam_force_allocation_module_P->fam_nozzle_orientations[(int32_T)(r + 12)]; - c[(int32_T)(r + 12)] = nozzle_moment_arm[(int32_T)(r + 24)] * - fam_force_allocation_module_P->fam_nozzle_orientations[r] - - fam_force_allocation_module_P->fam_nozzle_orientations[(int32_T)(r + 24)] - * nozzle_moment_arm[r]; - c[(int32_T)(r + 24)] = - fam_force_allocation_module_P->fam_nozzle_orientations[(int32_T)(r + 12)] - * nozzle_moment_arm[r] - nozzle_moment_arm[(int32_T)(r + 12)] * - fam_force_allocation_module_P->fam_nozzle_orientations[r]; - rtb_thrust2torque_B[(int32_T)(3 * r)] = -c[r]; - rtb_thrust2torque_B[(int32_T)(1 + (int32_T)(3 * r))] = -c[(int32_T)(r + 12)]; - rtb_thrust2torque_B[(int32_T)(2 + (int32_T)(3 * r))] = -c[(int32_T)(r + 24)]; - } - - // [-] Converts a nozzle thrust into resulting body torques - // ':1:8' - pphlopppohlnphdb_pinv(rtb_thrust2torque_B, unusedExpr); - - // [-] Converts body torques into nozzle thrust - // ':1:9' - for (r = 0; r < 12; r++) { - nozzle_moment_arm[(int32_T)(3 * r)] = - -fam_force_allocation_module_P->fam_nozzle_orientations[r]; - nozzle_moment_arm[(int32_T)(1 + (int32_T)(3 * r))] = - -fam_force_allocation_module_P->fam_nozzle_orientations[(int32_T)(r + 12)]; - nozzle_moment_arm[(int32_T)(2 + (int32_T)(3 * r))] = - -fam_force_allocation_module_P->fam_nozzle_orientations[(int32_T)(r + 24)]; - } - - // [-] Converts a nozzle thrust into resulting body forces - // ':1:10' - pphlopppohlnphdb_pinv(nozzle_moment_arm, unusedExpr_0); - - // [-] Converts body forces into nozzle thrust - // ':1:12' - for (r = 0; r < 3; r++) { - for (vcol = 0; vcol < 12; vcol++) { - A[(int32_T)(vcol + (int32_T)(12 * r))] = nozzle_moment_arm[(int32_T) - ((int32_T)(3 * vcol) + r)]; - A[(int32_T)(vcol + (int32_T)(12 * (int32_T)(r + 3)))] = - rtb_thrust2torque_B[(int32_T)((int32_T)(3 * vcol) + r)]; - } - } - - b_p = true; - for (r = 0; r < 72; r++) { - X[r] = 0.0F; - if (b_p && ((!rtIsInfF(A[r])) && (!rtIsNaNF(A[r])))) { - } else { - b_p = false; - } - } - - if (b_p) { - ekfccjmgknopmoph_svd(A, U, c, V); - } else { - for (r = 0; r < 72; r++) { - U[r] = (rtNaNF); - } - - memset(&c[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (r = 0; r < 6; r++) { - c[(int32_T)(r + (int32_T)(6 * r))] = (rtNaNF); - } - - for (r = 0; r < 36; r++) { - V[r] = (rtNaNF); - } - } - - tol = 12.0F * c[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while (((int32_T)(vcol + 1) < 7) && (c[(int32_T)((int32_T)(6 * vcol) + vcol)] - > tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (ic = 0; (int32_T)(ic + 1) <= r; ic++) { - tol = 1.0F / c[(int32_T)((int32_T)(6 * ic) + ic)]; - for (ar = vcol; (int32_T)(ar + 1) <= (int32_T)(vcol + 6); ar++) { - V[ar] *= tol; - } - - vcol += 6; - } - - for (vcol = 0; vcol <= 67; vcol += 6) { - for (ic = vcol; (int32_T)(ic + 1) <= (int32_T)(vcol + 6); ic++) { - X[ic] = 0.0F; - } - } - - vcol = -1; - for (ic = 0; ic <= 67; ic += 6) { - ar = -1; - vcol++; - b = (int32_T)((int32_T)((int32_T)((int32_T)(r - 1) * 12) + vcol) + 1); - for (ib = vcol; (int32_T)(ib + 1) <= b; ib += 12) { - if (U[ib] != 0.0F) { - ia = ar; - for (b_ic = ic; (int32_T)(b_ic + 1) <= (int32_T)(ic + 6); b_ic++) { - ia++; - X[b_ic] += U[ib] * V[ia]; - } - } - - ar += 6; - } - } - } - - // SignalConversion: '/OutportBufferForforcetorque2thrust_B' incorporates: - // MATLAB Function: '/MATLAB Function' - - for (r = 0; r < 6; r++) { - for (vcol = 0; vcol < 12; vcol++) { - fam_force_allocation_module_B->OutportBufferForforcetorque2thr[(int32_T) - (vcol + (int32_T)(12 * r))] = X[(int32_T)((int32_T)(6 * vcol) + r)]; - } - } - - // End of SignalConversion: '/OutportBufferForforcetorque2thrust_B' - - // SignalConversion: '/OutportBufferForthrust2force_B' - memcpy(&fam_force_allocation_module_B->OutportBufferForthrust2force_B[0], - &nozzle_moment_arm[0], (uint32_T)(36U * sizeof(real32_T))); - - // SignalConversion: '/OutportBufferForthrust2torque_B' - memcpy(&fam_force_allocation_module_B->OutportBufferForthrust2torque_B[0], - &rtb_thrust2torque_B[0], (uint32_T)(36U * sizeof(real32_T))); - } - - // End of Logic: '/Logical Operator' - // End of Outputs for SubSystem: '/latch_nozzle_thrust_matricies' - - // SignalConversion: '/TmpSignal ConversionAtProduct2Inport2' incorporates: - // Inport: '/ctl_msg' - - rtb_thrust_norm_by_Cd[0] = - fam_force_allocation_module_U_ctl_msg_n->body_force_cmd[0]; - rtb_thrust_norm_by_Cd[3] = - fam_force_allocation_module_U_ctl_msg_n->body_torque_cmd[0]; - rtb_thrust_norm_by_Cd[1] = - fam_force_allocation_module_U_ctl_msg_n->body_force_cmd[1]; - rtb_thrust_norm_by_Cd[4] = - fam_force_allocation_module_U_ctl_msg_n->body_torque_cmd[1]; - rtb_thrust_norm_by_Cd[2] = - fam_force_allocation_module_U_ctl_msg_n->body_force_cmd[2]; - rtb_thrust_norm_by_Cd[5] = - fam_force_allocation_module_U_ctl_msg_n->body_torque_cmd[2]; - - // Product: '/Product2' incorporates: - // SignalConversion: '/TmpSignal ConversionAtProduct2Inport2' - - for (r = 0; r < 12; r++) { - rtb_thrust_per_nozzle[r] = 0.0F; - for (vcol = 0; vcol < 6; vcol++) { - rtb_thrust_per_nozzle[r] += - fam_force_allocation_module_B->OutportBufferForforcetorque2thr[(int32_T) - ((int32_T)(12 * vcol) + r)] * rtb_thrust_norm_by_Cd[vcol]; - } - } - - // End of Product: '/Product2' - - // MinMax: '/MinMax6' - if ((rtb_thrust_per_nozzle[0] <= rtb_thrust_per_nozzle[1]) || rtIsNaNF - (rtb_thrust_per_nozzle[1])) { - tol = rtb_thrust_per_nozzle[0]; - } else { - tol = rtb_thrust_per_nozzle[1]; - } - - if (!((tol <= rtb_thrust_per_nozzle[6]) || rtIsNaNF(rtb_thrust_per_nozzle[6]))) - { - tol = rtb_thrust_per_nozzle[6]; - } - - if (!((tol <= rtb_thrust_per_nozzle[7]) || rtIsNaNF(rtb_thrust_per_nozzle[7]))) - { - tol = rtb_thrust_per_nozzle[7]; - } - - // Switch: '/Switch6' incorporates: - // Constant: '/Constant' - // MinMax: '/MinMax6' - // RelationalOperator: '/Compare' - // Sum: '/Add7' - - if (tol < fam_force_allocation_module_P->Constant_Value) { - // Abs: '/Abs' - tol = (real32_T)fabs((real_T)tol); - rtb_Switch8_idx_0 = rtb_thrust_per_nozzle[0] + tol; - rtb_Switch8_idx_2 = rtb_thrust_per_nozzle[6] + tol; - rtb_Switch8_idx_1 = rtb_thrust_per_nozzle[1] + tol; - tol += rtb_thrust_per_nozzle[7]; - } else { - rtb_Switch8_idx_0 = rtb_thrust_per_nozzle[0]; - rtb_Switch8_idx_2 = rtb_thrust_per_nozzle[6]; - rtb_Switch8_idx_1 = rtb_thrust_per_nozzle[1]; - tol = rtb_thrust_per_nozzle[7]; - } - - // End of Switch: '/Switch6' - - // Assignment: '/assign_x' - rtb_positive_thrust_per_nozzle[0] = rtb_Switch8_idx_0; - rtb_positive_thrust_per_nozzle[1] = rtb_Switch8_idx_1; - rtb_positive_thrust_per_nozzle[6] = rtb_Switch8_idx_2; - rtb_positive_thrust_per_nozzle[7] = tol; - - // MinMax: '/MinMax7' - if ((rtb_thrust_per_nozzle[2] <= rtb_thrust_per_nozzle[3]) || rtIsNaNF - (rtb_thrust_per_nozzle[3])) { - tol = rtb_thrust_per_nozzle[2]; - } else { - tol = rtb_thrust_per_nozzle[3]; - } - - if (!((tol <= rtb_thrust_per_nozzle[8]) || rtIsNaNF(rtb_thrust_per_nozzle[8]))) - { - tol = rtb_thrust_per_nozzle[8]; - } - - if (!((tol <= rtb_thrust_per_nozzle[9]) || rtIsNaNF(rtb_thrust_per_nozzle[9]))) - { - tol = rtb_thrust_per_nozzle[9]; - } - - // Switch: '/Switch7' incorporates: - // Constant: '/Constant' - // MinMax: '/MinMax7' - // RelationalOperator: '/Compare' - // Sum: '/Add8' - - if (tol < fam_force_allocation_module_P->Constant_Value_k) { - // Abs: '/Abs1' - tol = (real32_T)fabs((real_T)tol); - rtb_Switch8_idx_0 = rtb_thrust_per_nozzle[2] + tol; - rtb_Switch8_idx_2 = rtb_thrust_per_nozzle[8] + tol; - rtb_Switch8_idx_1 = rtb_thrust_per_nozzle[3] + tol; - tol += rtb_thrust_per_nozzle[9]; - } else { - rtb_Switch8_idx_0 = rtb_thrust_per_nozzle[2]; - rtb_Switch8_idx_2 = rtb_thrust_per_nozzle[8]; - rtb_Switch8_idx_1 = rtb_thrust_per_nozzle[3]; - tol = rtb_thrust_per_nozzle[9]; - } - - // End of Switch: '/Switch7' - - // Assignment: '/assign_y' - rtb_positive_thrust_per_nozzle[2] = rtb_Switch8_idx_0; - rtb_positive_thrust_per_nozzle[3] = rtb_Switch8_idx_1; - rtb_positive_thrust_per_nozzle[8] = rtb_Switch8_idx_2; - rtb_positive_thrust_per_nozzle[9] = tol; - - // MinMax: '/MinMax8' - if ((rtb_thrust_per_nozzle[4] <= rtb_thrust_per_nozzle[5]) || rtIsNaNF - (rtb_thrust_per_nozzle[5])) { - tol = rtb_thrust_per_nozzle[4]; - } else { - tol = rtb_thrust_per_nozzle[5]; - } - - if (!((tol <= rtb_thrust_per_nozzle[10]) || rtIsNaNF(rtb_thrust_per_nozzle[10]))) - { - tol = rtb_thrust_per_nozzle[10]; - } - - if (!((tol <= rtb_thrust_per_nozzle[11]) || rtIsNaNF(rtb_thrust_per_nozzle[11]))) - { - tol = rtb_thrust_per_nozzle[11]; - } - - // Switch: '/Switch8' incorporates: - // Constant: '/Constant' - // MinMax: '/MinMax8' - // RelationalOperator: '/Compare' - // Sum: '/Add9' - - if (tol < fam_force_allocation_module_P->Constant_Value_b) { - // Abs: '/Abs2' - tol = (real32_T)fabs((real_T)tol); - rtb_Switch8_idx_0 = rtb_thrust_per_nozzle[4] + tol; - rtb_Switch8_idx_2 = rtb_thrust_per_nozzle[10] + tol; - rtb_Switch8_idx_1 = rtb_thrust_per_nozzle[5] + tol; - tol += rtb_thrust_per_nozzle[11]; - } else { - rtb_Switch8_idx_0 = rtb_thrust_per_nozzle[4]; - rtb_Switch8_idx_2 = rtb_thrust_per_nozzle[10]; - rtb_Switch8_idx_1 = rtb_thrust_per_nozzle[5]; - tol = rtb_thrust_per_nozzle[11]; - } - - // End of Switch: '/Switch8' - - // Assignment: '/assign_z' - rtb_positive_thrust_per_nozzle[4] = rtb_Switch8_idx_0; - rtb_positive_thrust_per_nozzle[5] = rtb_Switch8_idx_1; - rtb_positive_thrust_per_nozzle[10] = rtb_Switch8_idx_2; - rtb_positive_thrust_per_nozzle[11] = tol; - - // Product: '/Product2' incorporates: - // Constant: '/Constant5' - - for (vcol = 0; vcol < 6; vcol++) { - rtb_thrust_norm_by_Cd[vcol] = rtb_positive_thrust_per_nozzle[vcol] / - fam_force_allocation_module_P->abp_PM1_discharge_coeff[vcol]; - } - - // End of Product: '/Product2' - - // Sum: '/Add' - tol = rtb_thrust_norm_by_Cd[0]; - for (b = 0; b < 5; b++) { - tol += rtb_thrust_norm_by_Cd[(int32_T)(b + 1)]; - } - - // End of Sum: '/Add' - - // Product: '/Divide' - tol *= 1.0F / rtb_Switch / rtb_Switch; - - // Lookup_n-D: '/fam_Cdp_lookup_pm1' - tol = look1_iflf_pbinlcapw(tol, - fam_force_allocation_module_P->fam_PM1_lookup_thrust_breakpoints, - fam_force_allocation_module_P->fam_PM1_lookup_Cdp_data, - &fam_force_allocation_module_DW->m_bpIndex, 315U); - - // Product: '/Divide1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - - rtb_Switch = rtb_Switch * rtb_Switch * - fam_force_allocation_module_P->abp_impeller_diameter * - fam_force_allocation_module_P->abp_impeller_diameter * - fam_force_allocation_module_P->const_air_density * tol; - for (vcol = 0; vcol < 6; vcol++) { - // Product: '/Product7' incorporates: - // Constant: '/Constant4' - // Constant: '/Constant5' - // Product: '/Product2' - - tol = fam_force_allocation_module_P->Constant4_Value_o * - fam_force_allocation_module_P->abp_PM1_discharge_coeff[vcol] * - fam_force_allocation_module_P->abp_PM1_discharge_coeff[vcol] * rtb_Switch; - - // Product: '/Product6' - rtb_cmd_area_per_nozzle[vcol] = rtb_positive_thrust_per_nozzle[vcol] / tol; - - // Product: '/Product7' - rtb_thrust_norm_by_Cd[vcol] = tol; - } - - // Saturate: '/Saturation' - rtb_Switch = (real32_T)cos((real_T) - fam_force_allocation_module_P->abp_nozzle_min_open_angle); - tol = (real32_T)cos((real_T) - fam_force_allocation_module_P->abp_nozzle_max_open_angle); - for (vcol = 0; vcol < 6; vcol++) { - // Product: '/Product5' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant6' - // Constant: '/Constant7' - // Constant: '/Constant9' - // Product: '/Product3' - // Product: '/Product4' - // Sum: '/Subtract2' - - rtb_Switch8_idx_1 = (fam_force_allocation_module_P->abp_nozzle_intake_height - - rtb_cmd_area_per_nozzle[vcol] / - fam_force_allocation_module_P->abp_nozzle_flap_count / - fam_force_allocation_module_P-> - abp_PM1_nozzle_widths[vcol]) / - fam_force_allocation_module_P->abp_nozzle_flap_length; - - // Saturate: '/Saturation' - if (rtb_Switch8_idx_1 > rtb_Switch) { - rtb_Switch8_idx_1 = rtb_Switch; - } else { - if (rtb_Switch8_idx_1 < tol) { - rtb_Switch8_idx_1 = tol; - } - } - - // Trigonometry: '/Trigonometric Function' - if (rtb_Switch8_idx_1 > 1.0F) { - rtb_Switch8_idx_1 = 1.0F; - } else { - if (rtb_Switch8_idx_1 < -1.0F) { - rtb_Switch8_idx_1 = -1.0F; - } - } - - // Sum: '/Subtract3' incorporates: - // Constant: '/Constant8' - // Trigonometry: '/Trigonometric Function' - - rtb_Switch8_idx_0 = (real32_T)acos((real_T)rtb_Switch8_idx_1) - - fam_force_allocation_module_P->abp_nozzle_min_open_angle; - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Product: '/Product1' - - rtb_thrust_per_nozzle[vcol] = rtb_Switch8_idx_0 * - fam_force_allocation_module_P->fam_nozzle_angle2pwm + - fam_force_allocation_module_P->abp_servo_min_PWM; - - // Product: '/Product2' incorporates: - // Constant: '/Constant5' - - rtb_thrust_norm_by_Cd_h[vcol] = rtb_positive_thrust_per_nozzle[(int32_T) - (vcol + 6)] / fam_force_allocation_module_P->abp_PM2_discharge_coeff[vcol]; - - // Sum: '/Subtract3' - rtb_calibrated_nozzle_theta[vcol] = rtb_Switch8_idx_0; - } - - // Sum: '/Add' - tol = rtb_thrust_norm_by_Cd_h[0]; - for (b = 0; b < 5; b++) { - tol += rtb_thrust_norm_by_Cd_h[(int32_T)(b + 1)]; - } - - // End of Sum: '/Add' - - // Product: '/Divide' - tol *= 1.0F / rtb_Switch_n / rtb_Switch_n; - - // Lookup_n-D: '/fam_Cdp_lookup_pm1' - tol = look1_iflf_pbinlcapw(tol, - fam_force_allocation_module_P->fam_PM2_lookup_thrust_breakpoints, - fam_force_allocation_module_P->fam_PM2_lookup_Cdp_data, - &fam_force_allocation_module_DW->m_bpIndex_a, 315U); - - // Product: '/Divide1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - - rtb_Switch_n = rtb_Switch_n * rtb_Switch_n * - fam_force_allocation_module_P->abp_impeller_diameter * - fam_force_allocation_module_P->abp_impeller_diameter * - fam_force_allocation_module_P->const_air_density * tol; - - // Saturate: '/Saturation' - rtb_Switch = (real32_T)cos((real_T) - fam_force_allocation_module_P->abp_nozzle_min_open_angle); - tol = (real32_T)cos((real_T) - fam_force_allocation_module_P->abp_nozzle_max_open_angle); - for (vcol = 0; vcol < 6; vcol++) { - // Product: '/Product7' incorporates: - // Constant: '/Constant4' - // Constant: '/Constant5' - // Product: '/Product2' - - rtb_Switch8_idx_0 = fam_force_allocation_module_P->Constant4_Value_m * - fam_force_allocation_module_P->abp_PM2_discharge_coeff[vcol] * - fam_force_allocation_module_P->abp_PM2_discharge_coeff[vcol] * - rtb_Switch_n; - - // Product: '/Product6' - rtb_Switch8_idx_2 = rtb_positive_thrust_per_nozzle[(int32_T)(vcol + 6)] / - rtb_Switch8_idx_0; - - // Product: '/Product5' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant6' - // Constant: '/Constant7' - // Constant: '/Constant9' - // Product: '/Product3' - // Product: '/Product4' - // Sum: '/Subtract2' - - rtb_Switch8_idx_1 = (fam_force_allocation_module_P->abp_nozzle_intake_height - - rtb_Switch8_idx_2 / - fam_force_allocation_module_P->abp_nozzle_flap_count / - fam_force_allocation_module_P-> - abp_PM2_nozzle_widths[vcol]) / - fam_force_allocation_module_P->abp_nozzle_flap_length; - - // Saturate: '/Saturation' - if (rtb_Switch8_idx_1 > rtb_Switch) { - rtb_Switch8_idx_1 = rtb_Switch; - } else { - if (rtb_Switch8_idx_1 < tol) { - rtb_Switch8_idx_1 = tol; - } - } - - // Trigonometry: '/Trigonometric Function' - if (rtb_Switch8_idx_1 > 1.0F) { - rtb_Switch8_idx_1 = 1.0F; - } else { - if (rtb_Switch8_idx_1 < -1.0F) { - rtb_Switch8_idx_1 = -1.0F; - } - } - - // Sum: '/Subtract3' incorporates: - // Constant: '/Constant8' - // Trigonometry: '/Trigonometric Function' - - rtb_Switch8_idx_1 = (real32_T)acos((real_T)rtb_Switch8_idx_1) - - fam_force_allocation_module_P->abp_nozzle_min_open_angle; - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Product: '/Product1' - - rtb_thrust_per_nozzle[(int32_T)(vcol + 6)] = rtb_Switch8_idx_1 * - fam_force_allocation_module_P->fam_nozzle_angle2pwm + - fam_force_allocation_module_P->abp_servo_min_PWM; - - // Product: '/Product7' - rtb_thrust_norm_by_Cd_h[vcol] = rtb_Switch8_idx_0; - - // Product: '/Product6' - rtb_cmd_area_per_nozzle_f[vcol] = rtb_Switch8_idx_2; - - // Sum: '/Subtract3' - rtb_calibrated_nozzle_theta_h[vcol] = rtb_Switch8_idx_1; - } - - for (vcol = 0; vcol < 6; vcol++) { - // SignalConversion: '/ConcatBufferAtVector Concatenate1In1' - rtb_positive_thrust_per_nozzle[vcol] = rtb_calibrated_nozzle_theta[vcol]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate1In2' - rtb_positive_thrust_per_nozzle[(int32_T)(vcol + 6)] = - rtb_calibrated_nozzle_theta_h[vcol]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate2In1' - rtb_Product9[vcol] = rtb_cmd_area_per_nozzle[vcol]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate2In2' - rtb_Product9[(int32_T)(vcol + 6)] = rtb_cmd_area_per_nozzle_f[vcol]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate3In1' - rtb_VectorConcatenate3[vcol] = rtb_thrust_norm_by_Cd[vcol]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate3In2' - rtb_VectorConcatenate3[(int32_T)(vcol + 6)] = rtb_thrust_norm_by_Cd_h[vcol]; - } - - // Outport: '/act_msg' incorporates: - // BusCreator: '/Bus Creator2' - // DataTypeConversion: '/Data Type Conversion' - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/current_time' - - fam_force_allocation_module_Y_act_msg_c->act_timestamp_sec = - fam_force_allocation_module_U_current_time->timestamp_sec; - fam_force_allocation_module_Y_act_msg_c->act_timestamp_nsec = - fam_force_allocation_module_U_current_time->timestamp_nsec; - fam_force_allocation_module_Y_act_msg_c->act_impeller_speed_cmd[0] = (uint8_T) - (tmp_0 < 0.0F ? (int32_T)(uint8_T)(int32_T)-(int32_T)(int8_T)(uint8_T)-tmp_0 - : (int32_T)(uint8_T)tmp_0); - fam_force_allocation_module_Y_act_msg_c->act_impeller_speed_cmd[1] = (uint8_T) - (tmp < 0.0F ? (int32_T)(uint8_T)(int32_T)-(int32_T)(int8_T)(uint8_T)-tmp : - (int32_T)(uint8_T)tmp); - for (vcol = 0; vcol < 12; vcol++) { - // Outport: '/act_msg' incorporates: - // BusCreator: '/Bus Creator2' - - fam_force_allocation_module_Y_act_msg_c->act_servo_pwm_cmd[vcol] = - rtb_thrust_per_nozzle[vcol]; - fam_force_allocation_module_Y_act_msg_c->act_nozzle_theta[vcol] = - rtb_positive_thrust_per_nozzle[vcol]; - - // Product: '/Product9' - rtb_Product9[vcol] *= rtb_VectorConcatenate3[vcol]; - } - - for (vcol = 0; vcol < 3; vcol++) { - // Outport: '/act_msg' incorporates: - // Product: '/Product3' - // Product: '/Product4' - - fam_force_allocation_module_Y_act_msg_c->act_predicted_force_B[vcol] = 0.0F; - fam_force_allocation_module_Y_act_msg_c->act_predicted_torque_B[vcol] = 0.0F; - for (r = 0; r < 12; r++) { - fam_force_allocation_module_Y_act_msg_c->act_predicted_force_B[vcol] += - fam_force_allocation_module_B->OutportBufferForthrust2force_B[(int32_T) - ((int32_T)(3 * r) + vcol)] * rtb_Product9[r]; - fam_force_allocation_module_Y_act_msg_c->act_predicted_torque_B[vcol] += - fam_force_allocation_module_B->OutportBufferForthrust2torque_B[(int32_T) - ((int32_T)(3 * r) + vcol)] * rtb_Product9[r]; - } - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Inport: '/cmc_msg' - // SignalConversion: '/Signal Conversion' - - fam_force_allocation_module_DW->DelayInput1_DSTATE[vcol] = - fam_force_allocation_module_U_cmc_msg_h->center_of_mass[vcol]; - } -} - -// Model initialize function -void fam_force_allocation_module_initialize(RT_MODEL_fam_force_allocation_T * - const fam_force_allocation_module_M, ex_time_msg - *fam_force_allocation_module_U_current_time, cmd_msg - *fam_force_allocation_module_U_cmd_msg_f, ctl_msg - *fam_force_allocation_module_U_ctl_msg_n, cmc_msg - *fam_force_allocation_module_U_cmc_msg_h, act_msg - *fam_force_allocation_module_Y_act_msg_c) -{ - P_fam_force_allocation_module_T *fam_force_allocation_module_P = - ((P_fam_force_allocation_module_T *) - fam_force_allocation_module_M->defaultParam); - B_fam_force_allocation_module_T *fam_force_allocation_module_B = - ((B_fam_force_allocation_module_T *) fam_force_allocation_module_M->blockIO); - DW_fam_force_allocation_modul_T *fam_force_allocation_module_DW = - ((DW_fam_force_allocation_modul_T *) fam_force_allocation_module_M->dwork); - - { - int32_T i; - - // InitializeConditions for UnitDelay: '/Delay Input1' - fam_force_allocation_module_DW->DelayInput1_DSTATE[0] = - fam_force_allocation_module_P->DetectChange_vinit; - fam_force_allocation_module_DW->DelayInput1_DSTATE[1] = - fam_force_allocation_module_P->DetectChange_vinit; - fam_force_allocation_module_DW->DelayInput1_DSTATE[2] = - fam_force_allocation_module_P->DetectChange_vinit; - - // SystemInitialize for Enabled SubSystem: '/latch_nozzle_thrust_matricies' - for (i = 0; i < 36; i++) { - // SystemInitialize for Outport: '/thrust2force_B' - fam_force_allocation_module_B->OutportBufferForthrust2force_B[i] = - fam_force_allocation_module_P->thrust2force_B_Y0; - - // SystemInitialize for Outport: '/thrust2torque_B' - fam_force_allocation_module_B->OutportBufferForthrust2torque_B[i] = - fam_force_allocation_module_P->thrust2torque_B_Y0; - } - - // SystemInitialize for Outport: '/forcetorque2thrust_B' - for (i = 0; i < 72; i++) { - fam_force_allocation_module_B->OutportBufferForforcetorque2thr[i] = - fam_force_allocation_module_P->forcetorque2thrust_B_Y0; - } - - // End of SystemInitialize for Outport: '/forcetorque2thrust_B' - // End of SystemInitialize for SubSystem: '/latch_nozzle_thrust_matricies' - } -} - -// Model terminate function -void fam_force_allocation_module_terminate(RT_MODEL_fam_force_allocation_T - * fam_force_allocation_module_M) -{ - // model code - rt_FREE(fam_force_allocation_module_M->blockIO); - if (fam_force_allocation_module_M->paramIsMalloced) { - rt_FREE(fam_force_allocation_module_M->defaultParam); - } - - rt_FREE(fam_force_allocation_module_M->dwork); - rt_FREE(fam_force_allocation_module_M); -} - -// Model data allocation function -RT_MODEL_fam_force_allocation_T *fam_force_allocation_module(ex_time_msg - *fam_force_allocation_module_U_current_time, cmd_msg - *fam_force_allocation_module_U_cmd_msg_f, ctl_msg - *fam_force_allocation_module_U_ctl_msg_n, cmc_msg - *fam_force_allocation_module_U_cmc_msg_h, act_msg - *fam_force_allocation_module_Y_act_msg_c) -{ - RT_MODEL_fam_force_allocation_T *fam_force_allocation_module_M; - fam_force_allocation_module_M = (RT_MODEL_fam_force_allocation_T *) malloc - (sizeof(RT_MODEL_fam_force_allocation_T)); - if (fam_force_allocation_module_M == NULL) { - return NULL; - } - - (void) memset((char *)fam_force_allocation_module_M, 0, - sizeof(RT_MODEL_fam_force_allocation_T)); - - // block I/O - { - B_fam_force_allocation_module_T *b = (B_fam_force_allocation_module_T *) - malloc(sizeof(B_fam_force_allocation_module_T)); - rt_VALIDATE_MEMORY(fam_force_allocation_module_M,b); - fam_force_allocation_module_M->blockIO = (b); - } - - // parameters - { - P_fam_force_allocation_module_T *p; - static int_T pSeen = 0; - - // only malloc on multiple model instantiation - if (pSeen == 1 ) { - p = (P_fam_force_allocation_module_T *) malloc(sizeof - (P_fam_force_allocation_module_T)); - rt_VALIDATE_MEMORY(fam_force_allocation_module_M,p); - (void) memcpy(p, &fam_force_allocation_module_P, - sizeof(P_fam_force_allocation_module_T)); - fam_force_allocation_module_M->paramIsMalloced = (true); - } else { - p = &fam_force_allocation_module_P; - fam_force_allocation_module_M->paramIsMalloced = (false); - pSeen = 1; - } - - fam_force_allocation_module_M->defaultParam = (p); - } - - // states (dwork) - { - DW_fam_force_allocation_modul_T *dwork = (DW_fam_force_allocation_modul_T *) - malloc(sizeof(DW_fam_force_allocation_modul_T)); - rt_VALIDATE_MEMORY(fam_force_allocation_module_M,dwork); - fam_force_allocation_module_M->dwork = (dwork); - } - - { - P_fam_force_allocation_module_T *fam_force_allocation_module_P = - ((P_fam_force_allocation_module_T *) - fam_force_allocation_module_M->defaultParam); - B_fam_force_allocation_module_T *fam_force_allocation_module_B = - ((B_fam_force_allocation_module_T *) - fam_force_allocation_module_M->blockIO); - DW_fam_force_allocation_modul_T *fam_force_allocation_module_DW = - ((DW_fam_force_allocation_modul_T *) fam_force_allocation_module_M->dwork); - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // non-finite (run-time) assignments - fam_force_allocation_module_P->DetectChange_vinit = rtInfF; - - // block I/O - (void) memset(((void *) fam_force_allocation_module_B), 0, - sizeof(B_fam_force_allocation_module_T)); - - // states (dwork) - (void) memset((void *)fam_force_allocation_module_DW, 0, - sizeof(DW_fam_force_allocation_modul_T)); - - // external inputs - *fam_force_allocation_module_U_current_time = - fam_force_allocation_module_rtZ; - *fam_force_allocation_module_U_cmd_msg_f = fam_force_allocation_module_r_0; - *fam_force_allocation_module_U_ctl_msg_n = fam_force_allocation_module_r_1; - *fam_force_allocation_module_U_cmc_msg_h = fam_force_allocation_module_r_2; - - // external outputs - (*fam_force_allocation_module_Y_act_msg_c) = - fam_force_allocation_module_rtZact_msg; - } - - return fam_force_allocation_module_M; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module.h b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module.h deleted file mode 100644 index dcf8b66c2e..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module.h +++ /dev/null @@ -1,338 +0,0 @@ -// -// File: fam_force_allocation_module.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:44:37 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_fam_force_allocation_module_h_ -#define RTW_HEADER_fam_force_allocation_module_h_ -#include -#include -#include -#ifndef fam_force_allocation_module_COMMON_INCLUDES_ -# define fam_force_allocation_module_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#endif // fam_force_allocation_module_COMMON_INCLUDES_ - -#include "fam_force_allocation_module_types.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "ekfccjmgknopmoph_svd.h" -#include "look1_iflf_pbinlcapw.h" -#include "plook_u32f_binckpan.h" -#include "pphlopppohlnphdb_pinv.h" -#include "rt_roundf_snf.h" -#include "rtGetInf.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -# define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -# define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Block signals (auto storage) -typedef struct { - real32_T OutportBufferForforcetorque2thr[72];// '/MATLAB Function' - real32_T OutportBufferForthrust2force_B[36];// '/MATLAB Function' - real32_T OutportBufferForthrust2torque_B[36];// '/MATLAB Function' -} B_fam_force_allocation_module_T; - -// Block states (auto storage) for system '' -typedef struct { - real32_T DelayInput1_DSTATE[3]; // '/Delay Input1' - uint32_T m_bpIndex; // '/fam_Cdp_lookup_pm1' - uint32_T m_bpIndex_a; // '/fam_Cdp_lookup_pm1' - uint32_T m_bpIndex_e; // '/impeller_speed_lookup' - uint32_T m_bpIndex_b; // '/impeller_speed_lookup' -} DW_fam_force_allocation_modul_T; - -// Parameters (auto storage) -struct P_fam_force_allocation_module_T_ { - real32_T abp_PM1_discharge_coeff[6]; // Variable: abp_PM1_discharge_coeff - // Referenced by: - // '/Constant5' - // '/Constant5' - - real32_T abp_PM1_nozzle_widths[6]; // Variable: abp_PM1_nozzle_widths - // Referenced by: '/Constant7' - - real32_T abp_PM2_discharge_coeff[6]; // Variable: abp_PM2_discharge_coeff - // Referenced by: - // '/Constant5' - // '/Constant5' - - real32_T abp_PM2_nozzle_widths[6]; // Variable: abp_PM2_nozzle_widths - // Referenced by: '/Constant7' - - real32_T abp_impeller_diameter; // Variable: abp_impeller_diameter - // Referenced by: - // '/Constant1' - // '/Constant1' - - real32_T abp_impeller_speed2pwm; // Variable: abp_impeller_speed2pwm - // Referenced by: - // '/Constant' - // '/Constant' - - real32_T abp_nozzle_flap_count; // Variable: abp_nozzle_flap_count - // Referenced by: - // '/Constant3' - // '/Constant3' - - real32_T abp_nozzle_flap_length; // Variable: abp_nozzle_flap_length - // Referenced by: - // '/Constant6' - // '/Constant6' - - real32_T abp_nozzle_intake_height; // Variable: abp_nozzle_intake_height - // Referenced by: - // '/Constant9' - // '/Constant9' - - real32_T abp_nozzle_max_open_angle; // Variable: abp_nozzle_max_open_angle - // Referenced by: - // '/Saturation' - // '/Saturation' - - real32_T abp_nozzle_min_open_angle; // Variable: abp_nozzle_min_open_angle - // Referenced by: - // '/Constant8' - // '/Saturation' - // '/Constant8' - // '/Saturation' - - real32_T abp_servo_min_PWM; // Variable: abp_servo_min_PWM - // Referenced by: - // '/Constant2' - // '/Constant2' - - real32_T const_air_density; // Variable: const_air_density - // Referenced by: - // '/Constant2' - // '/Constant2' - - real32_T fam_PM1_lookup_Cdp_data[316];// Variable: fam_PM1_lookup_Cdp_data - // Referenced by: '/fam_Cdp_lookup_pm1' - - real32_T fam_PM1_lookup_thrust_breakpoints[316];// Variable: fam_PM1_lookup_thrust_breakpoints - // Referenced by: '/fam_Cdp_lookup_pm1' - - real32_T fam_PM2_lookup_Cdp_data[316];// Variable: fam_PM2_lookup_Cdp_data - // Referenced by: '/fam_Cdp_lookup_pm1' - - real32_T fam_PM2_lookup_thrust_breakpoints[316];// Variable: fam_PM2_lookup_thrust_breakpoints - // Referenced by: '/fam_Cdp_lookup_pm1' - - real32_T fam_P_nozzle_B_B[36]; // Variable: fam_P_nozzle_B_B - // Referenced by: '/Constant1' - - real32_T fam_impeller_speeds[3]; // Variable: fam_impeller_speeds - // Referenced by: - // '/impeller_speed_lookup' - // '/impeller_speed_lookup' - - real32_T fam_nozzle_angle2pwm; // Variable: fam_nozzle_angle2pwm - // Referenced by: - // '/Constant1' - // '/Constant1' - - real32_T fam_nozzle_orientations[36];// Variable: fam_nozzle_orientations - // Referenced by: '/Constant3' - - real32_T DetectChange_vinit; // Mask Parameter: DetectChange_vinit - // Referenced by: '/Delay Input1' - - uint8_T CompareToConstant_const; // Mask Parameter: CompareToConstant_const - // Referenced by: '/Constant' - - uint8_T CompareToConstant_const_n; // Mask Parameter: CompareToConstant_const_n - // Referenced by: '/Constant' - - real32_T force2thrust_B_Y0; // Computed Parameter: force2thrust_B_Y0 - // Referenced by: '/force2thrust_B' - - real32_T torque2thrust_B_Y0; // Computed Parameter: torque2thrust_B_Y0 - // Referenced by: '/torque2thrust_B' - - real32_T thrust2force_B_Y0; // Computed Parameter: thrust2force_B_Y0 - // Referenced by: '/thrust2force_B' - - real32_T thrust2torque_B_Y0; // Computed Parameter: thrust2torque_B_Y0 - // Referenced by: '/thrust2torque_B' - - real32_T forcetorque2thrust_B_Y0; // Computed Parameter: forcetorque2thrust_B_Y0 - // Referenced by: '/forcetorque2thrust_B' - - real32_T impeller_speed_lookup_bp01Data[3];// Computed Parameter: impeller_speed_lookup_bp01Data - // Referenced by: '/impeller_speed_lookup' - - real32_T impeller_speed_lookup_bp01Dat_l[3];// Computed Parameter: impeller_speed_lookup_bp01Dat_l - // Referenced by: '/impeller_speed_lookup' - - real32_T Constant1_Value; // Expression: single(0) - // Referenced by: '/Constant1' - - real32_T Constant1_Value_h; // Expression: single(0) - // Referenced by: '/Constant1' - - real32_T Constant4_Value[12]; // Expression: zeros(12,1,'single') - // Referenced by: '/Constant4' - - real32_T Constant_Value; // Computed Parameter: Constant_Value - // Referenced by: '/Constant' - - real32_T Constant_Value_k; // Computed Parameter: Constant_Value_k - // Referenced by: '/Constant' - - real32_T Constant_Value_b; // Computed Parameter: Constant_Value_b - // Referenced by: '/Constant' - - real32_T Constant4_Value_o; // Expression: single(2) - // Referenced by: '/Constant4' - - real32_T Constant4_Value_m; // Expression: single(2) - // Referenced by: '/Constant4' - -}; - -// Real-time Model Data Structure -struct tag_RTM_fam_force_allocation__T { - const char_T * volatile errorStatus; - B_fam_force_allocation_module_T *blockIO; - P_fam_force_allocation_module_T *defaultParam; - boolean_T paramIsMalloced; - DW_fam_force_allocation_modul_T *dwork; -}; - -#ifdef __cplusplus - -extern "C" { - -#endif - -#ifdef __cplusplus - -} -#endif - -// External data declarations for dependent source files -extern const ex_time_msg fam_force_allocation_module_rtZex_time_msg;// ex_time_msg ground -extern const cmd_msg fam_force_allocation_module_rtZcmd_msg;// cmd_msg ground -extern const ctl_msg fam_force_allocation_module_rtZctl_msg;// ctl_msg ground -extern const cmc_msg fam_force_allocation_module_rtZcmc_msg;// cmc_msg ground -extern const act_msg fam_force_allocation_module_rtZact_msg;// act_msg ground - -#ifdef __cplusplus - -extern "C" { - -#endif - - extern const char *RT_MEMORY_ALLOCATION_ERROR; - -#ifdef __cplusplus - -} -#endif - -extern P_fam_force_allocation_module_T fam_force_allocation_module_P;// parameters - -#ifdef __cplusplus - -extern "C" { - -#endif - - // Model entry point functions - extern RT_MODEL_fam_force_allocation_T *fam_force_allocation_module - (ex_time_msg *fam_force_allocation_module_U_current_time, cmd_msg - *fam_force_allocation_module_U_cmd_msg_f, ctl_msg - *fam_force_allocation_module_U_ctl_msg_n, cmc_msg - *fam_force_allocation_module_U_cmc_msg_h, act_msg - *fam_force_allocation_module_Y_act_msg_c); - extern void fam_force_allocation_module_initialize - (RT_MODEL_fam_force_allocation_T *const fam_force_allocation_module_M, - ex_time_msg *fam_force_allocation_module_U_current_time, cmd_msg - *fam_force_allocation_module_U_cmd_msg_f, ctl_msg - *fam_force_allocation_module_U_ctl_msg_n, cmc_msg - *fam_force_allocation_module_U_cmc_msg_h, act_msg - *fam_force_allocation_module_Y_act_msg_c); - extern void fam_force_allocation_module_step(RT_MODEL_fam_force_allocation_T * - const fam_force_allocation_module_M, ex_time_msg - *fam_force_allocation_module_U_current_time, cmd_msg - *fam_force_allocation_module_U_cmd_msg_f, ctl_msg - *fam_force_allocation_module_U_ctl_msg_n, cmc_msg - *fam_force_allocation_module_U_cmc_msg_h, act_msg - *fam_force_allocation_module_Y_act_msg_c); - extern void fam_force_allocation_module_terminate - (RT_MODEL_fam_force_allocation_T * fam_force_allocation_module_M); - -#ifdef __cplusplus - -} -#endif - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Note that this particular code originates from a subsystem build, -// and has its own system numbers different from the parent model. -// Refer to the system hierarchy for this subsystem below, and use the -// MATLAB hilite_system command to trace the generated code back -// to the parent model. For example, -// -// hilite_system('astrobee/fsw_lib/fam_force_allocation_module') - opens subsystem astrobee/fsw_lib/fam_force_allocation_module -// hilite_system('astrobee/fsw_lib/fam_force_allocation_module/Kp') - opens and selects block Kp -// -// Here is the system hierarchy for this model -// -// '' : 'astrobee/fsw_lib' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/back_predict_force_and_torque' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust_nnlsq' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust_old' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM1_servo_command' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM2_servo_command' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust/Compare To Zero6' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust/Compare To Zero7' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust/Compare To Zero8' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust/Detect Change' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust/latch_nozzle_thrust_matricies' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust/latch_nozzle_thrust_matricies/MATLAB Function' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust_nnlsq/MATLAB Function' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust_nnlsq/latch_nozzle_thrust_matricies' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust_nnlsq/latch_nozzle_thrust_matricies/MATLAB Function' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust_old/latch_nozzle_thrust_matricies' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calc_nozzle_thrust_old/latch_nozzle_thrust_matricies/MATLAB Function' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM1_servo_command/calc_impeller_speed' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM1_servo_command/calc_servo_cmd' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM1_servo_command/lookup_data_tables' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM1_servo_command/calc_impeller_speed/Compare To Constant' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM2_servo_command/calc_impeller_speed' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM2_servo_command/calc_servo_cmd' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM2_servo_command/lookup_data_tables' -// '' : 'astrobee/fsw_lib/fam_force_allocation_module/calculate_PM2_servo_command/calc_impeller_speed/Compare To Constant' - -#endif // RTW_HEADER_fam_force_allocation_module_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_data.cpp b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_data.cpp deleted file mode 100644 index 532ce80339..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_data.cpp +++ /dev/null @@ -1,367 +0,0 @@ -// -// File: fam_force_allocation_module_data.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:44:37 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "fam_force_allocation_module.h" -#include "fam_force_allocation_module_private.h" - -// Block parameters (auto storage) -P_fam_force_allocation_module_T fam_force_allocation_module_P = { - { 0.914971054F, 0.755778253F, 0.940762937F, 0.792109787F, 0.9240188F, - 0.930319786F }, - - { 0.127F, 0.127F, 0.07112F, 0.07112F, 0.07112F, 0.07112F }, - - { 0.947114F, 0.764468908F, 1.0F, 0.904809415F, 0.936555624F, 0.893794775F }, - - { 0.127F, 0.127F, 0.07112F, 0.07112F, 0.07112F, 0.07112F }, - 0.1397F, - 0.792095F, - 2.0F, - 0.0135966204F, - 0.01309116F, - 1.39469266F, - 0.273667634F, - 0.0F, - 1.2F, - - { 0.0825F, 0.082625635F, 0.08274699F, 0.0828641355F, 0.082977131F, - 0.083086051F, 0.0831909552F, 0.083291918F, 0.083389F, 0.083482258F, - 0.0835717544F, 0.0836575627F, 0.0837397277F, 0.083818309F, 0.0838933736F, - 0.0839649737F, 0.0840331689F, 0.0840980038F, 0.0841595381F, 0.0842178315F, - 0.0842729211F, 0.084324874F, 0.0843737274F, 0.0844195336F, 0.0844623446F, - 0.0845022127F, 0.0845391676F, 0.0845732689F, 0.0846045539F, 0.0846330673F, - 0.0846588612F, 0.0846819654F, 0.0847024247F, 0.0847202763F, 0.0847355723F, - 0.0847483352F, 0.0847586095F, 0.0847664326F, 0.0847718418F, 0.0847748667F, - 0.0847755447F, 0.084773913F, 0.08477F, 0.0847638398F, 0.0847554579F, - 0.084744893F, 0.0847321674F, 0.0847173184F, 0.0847003683F, 0.0846813396F, - 0.0846602693F, 0.0846371725F, 0.0846120864F, 0.0845850259F, 0.0845560208F, - 0.0845250934F, 0.0844922587F, 0.0844575465F, 0.0844209716F, 0.0843825564F, - 0.0843423232F, 0.0843002871F, 0.0842564702F, 0.084210895F, 0.0841635615F, - 0.0841145F, 0.0840637162F, 0.0840112418F, 0.0839570761F, 0.083901234F, - 0.0838437378F, 0.0837845877F, 0.0837238058F, 0.0836614F, 0.0835973844F, - 0.0835317597F, 0.083464548F, 0.0833957493F, 0.0833253786F, 0.0832534432F, - 0.0831799433F, 0.0831048936F, 0.0830283F, 0.0829501674F, 0.0828705F, - 0.0827893F, 0.0827065781F, 0.0826223418F, 0.0825365856F, 0.0824493095F, - 0.0823605284F, 0.0822702423F, 0.0821784437F, 0.08208514F, 0.0819903314F, - 0.0818940252F, 0.0817962065F, 0.0816968903F, 0.081596069F, 0.0814937353F, - 0.081389904F, 0.0812845528F, 0.0811777F, 0.0810693204F, 0.0809594318F, - 0.0808480158F, 0.0807350799F, 0.0806206167F, 0.0805046186F, 0.0803870782F, - 0.080268F, 0.0801473707F, 0.0800251886F, 0.0799014494F, 0.0797761381F, - 0.0796492621F, 0.0795208F, 0.0793907568F, 0.0792591125F, 0.0791258737F, - 0.0789910182F, 0.0788545534F, 0.0787164569F, 0.0785767287F, 0.0784353614F, - 0.0782923326F, 0.0781476498F, 0.0780012906F, 0.077853255F, 0.0777035281F, - 0.0775521F, 0.0773989633F, 0.0772441104F, 0.0770875216F, 0.0769291893F, - 0.0767691135F, 0.0766072646F, 0.0764436498F, 0.0762782469F, 0.076111041F, - 0.0759420395F, 0.0757712126F, 0.0755985528F, 0.0754240528F, 0.0752477F, - 0.0750694722F, 0.0748893768F, 0.0747073889F, 0.0745235F, 0.0743377F, - 0.0741499662F, 0.0739603F, 0.0737686828F, 0.0735751F, 0.0733795539F, - 0.0731820166F, 0.0729824826F, 0.072780937F, 0.0725773647F, 0.072371766F, - 0.0721641108F, 0.0719544F, 0.071742624F, 0.0715287626F, 0.0713128075F, - 0.0710947439F, 0.0708745569F, 0.0706522539F, 0.0704278F, 0.0702012F, - 0.0699724406F, 0.0697415F, 0.0695083737F, 0.0692730546F, 0.0690355226F, - 0.0687957704F, 0.0685537905F, 0.0683095828F, 0.0680631176F, 0.0678143948F, - 0.0675634F, 0.0673101321F, 0.0670545697F, 0.0667967126F, 0.0665365383F, - 0.0662740618F, 0.0660092533F, 0.0657421201F, 0.0654726326F, 0.0652008057F, - 0.0649266243F, 0.0646500662F, 0.0643711537F, 0.0640898496F, 0.0638061538F, - 0.0635200813F, 0.0632316F, 0.0629407242F, 0.0626474321F, 0.0623517223F, - 0.0620536F, 0.0617530458F, 0.0614500828F, 0.0611446686F, 0.0608368181F, - 0.0605265312F, 0.0602138F, 0.0598986298F, 0.0595810153F, 0.0592609458F, - 0.0589384325F, 0.0586134642F, 0.0582860447F, 0.0579561703F, 0.0576238409F, - 0.0572890751F, 0.0569518507F, 0.0566121712F, 0.0562700555F, 0.0559254885F, - 0.0555784889F, 0.0552290566F, 0.054877162F, 0.0545228682F, 0.0541661382F, - 0.0538069829F, 0.0534454137F, 0.0530814491F, 0.0527150668F, 0.0523463078F, - 0.0519751534F, 0.0516016223F, 0.0512257218F, 0.0508474559F, 0.0504668579F, - 0.0500839F, 0.0496986173F, 0.0493110269F, 0.0489211343F, 0.0485289469F, - 0.0481344871F, 0.0477377549F, 0.04733878F, 0.0469375588F, 0.0465341397F, - 0.0461285152F, 0.0457207039F, 0.0453107134F, 0.0448985882F, 0.0444843248F, - 0.0440679714F, 0.0436495095F, 0.0432290025F, 0.0428064205F, 0.0423818417F, - 0.0419552475F, 0.0415266976F, 0.0410961919F, 0.0406637266F, 0.0402293913F, - 0.0397931822F, 0.0393551365F, 0.038915243F, 0.0384735875F, 0.0380301885F, - 0.0375850387F, 0.0371381938F, 0.0366896801F, 0.0362395607F, 0.0357878208F, - 0.0353345275F, 0.0348797F, 0.0344234072F, 0.0339656323F, 0.033506453F, - 0.033045914F, 0.032584019F, 0.0321208276F, 0.0316563807F, 0.0311907455F, - 0.0307239257F, 0.0302559771F, 0.0297869369F, 0.0293168686F, 0.0288458094F, - 0.0283738412F, 0.0279009566F, 0.0274272151F, 0.0269527063F, 0.0264774524F, - 0.0260014758F, 0.0255248956F, 0.0250477232F, 0.0245700441F, 0.0240918733F, - 0.0236133039F, 0.0231343843F, 0.0226551369F, 0.0221756734F, 0.0216960311F, - 0.0212163F, 0.0207364894F, 0.0202567242F, 0.0197770186F, 0.0192974396F, - 0.0188181363F, 0.0183390342F, 0.0178603418F, 0.0173820443F, 0.0169042759F, - 0.0164270438F, 0.0159504749F, 0.0154745989F, 0.01499952F, 0.0145253353F, - 0.0140520222F }, - - { -1.72601576E-5F, -1.7229886E-5F, -1.71986339E-5F, -1.71664233E-5F, - -1.71332704E-5F, -1.70991971E-5F, -1.70642161E-5F, -1.70283474E-5F, - -1.69916093E-5F, -1.69540162E-5F, -1.69155865E-5F, -1.68763381E-5F, - -1.68362858E-5F, -1.67954422E-5F, -1.67538255E-5F, -1.67114522E-5F, - -1.66683349E-5F, -1.66244881E-5F, -1.65799283E-5F, -1.65346701E-5F, - -1.64887242E-5F, -1.64421035E-5F, -1.63948262E-5F, -1.63469012E-5F, - -1.62983433E-5F, -1.62491633E-5F, -1.61993758E-5F, -1.61489916E-5F, - -1.60980198E-5F, -1.6046477E-5F, -1.59943702E-5F, -1.59417141E-5F, - -1.58885177E-5F, -1.58347902E-5F, -1.57805443E-5F, -1.57257891E-5F, - -1.56705355E-5F, -1.56147944E-5F, -1.55585712E-5F, -1.5501877E-5F, - -1.54447225E-5F, -1.53871151E-5F, -1.53290657E-5F, -1.52705816E-5F, - -1.52116681E-5F, -1.51523363E-5F, -1.50925953E-5F, -1.50324504E-5F, - -1.49719108E-5F, -1.49109837E-5F, -1.48496747E-5F, -1.47879928E-5F, - -1.47259452E-5F, -1.46635357E-5F, -1.46007742E-5F, -1.45376662E-5F, - -1.44742153E-5F, -1.44104315E-5F, -1.43463167E-5F, -1.42818799E-5F, - -1.42171248E-5F, -1.41520568E-5F, -1.4086685E-5F, -1.40210113E-5F, - -1.39550393E-5F, -1.38887754E-5F, -1.3822224E-5F, -1.37553907E-5F, - -1.36882791E-5F, -1.36208964E-5F, -1.35532428E-5F, -1.34853208E-5F, - -1.34171405E-5F, -1.33487029E-5F, -1.32800124E-5F, -1.3211069E-5F, - -1.3141881E-5F, -1.30724484E-5F, -1.30027765E-5F, -1.29328673E-5F, - -1.28627235E-5F, -1.27923504E-5F, -1.272175E-5F, -1.2650924E-5F, - -1.25798742E-5F, -1.25086062E-5F, -1.24371181E-5F, -1.2365419E-5F, - -1.22935044E-5F, -1.22213805E-5F, -1.21490484E-5F, -1.20765108E-5F, - -1.20037676E-5F, -1.19308233E-5F, -1.18576763E-5F, -1.17843329E-5F, - -1.17107929E-5F, -1.16370575E-5F, -1.15631265E-5F, -1.14890026E-5F, - -1.14146933E-5F, -1.13401893E-5F, -1.12655016E-5F, -1.11906247E-5F, - -1.11155623E-5F, -1.10403153E-5F, -1.09648845E-5F, -1.08892737E-5F, - -1.08134809E-5F, -1.07375072E-5F, -1.0661357E-5F, -1.05850268E-5F, - -1.05085192E-5F, -1.04318369E-5F, -1.03549764E-5F, -1.02779441E-5F, - -1.02007352E-5F, -1.01233545E-5F, -1.0045801E-5F, -9.96807648E-6F, - -9.89017735E-6F, -9.81211178E-6F, -9.7338725E-6F, -9.65546678E-6F, - -9.57689099E-6F, -9.49814785E-6F, -9.41923736E-6F, -9.3401577E-6F, - -9.26091434E-6F, -9.1815009E-6F, -9.10192648E-6F, -9.0221838E-6F, - -8.94228197E-6F, -8.86221096E-6F, -8.7819808E-6F, -8.7015851E-6F, - -8.62102934E-6F, -8.54031259E-6F, -8.45943305E-6F, -8.37839434E-6F, - -8.29719738E-6F, -8.21584E-6F, -8.13432325E-6F, -8.05265245E-6F, - -7.97082248E-6F, -7.88883699E-6F, -7.80669507E-6F, -7.72440126E-6F, - -7.64195374E-6F, -7.55935071E-6F, -7.47659669E-6F, -7.3936917E-6F, - -7.31063665E-6F, -7.22743061E-6F, -7.14408088E-6F, -7.06058108E-6F, - -6.97693667E-6F, -6.89314766E-6F, -6.80921494E-6F, -6.72514034E-6F, - -6.64092295E-6F, -6.5565664E-6F, -6.47207435E-6F, -6.38744405E-6F, - -6.30267914E-6F, -6.21777872E-6F, -6.13274733E-6F, -6.04758588E-6F, - -5.9622962E-6F, -5.876881E-6F, -5.79134121E-6F, -5.70567863E-6F, - -5.61989418E-6F, -5.53399241E-6F, -5.44797422E-6F, -5.36184143E-6F, - -5.27559496E-6F, -5.18924389E-6F, -5.10278369E-6F, -5.01622E-6F, - -4.92955405E-6F, -4.84279E-6F, -4.75592969E-6F, -4.66897836E-6F, - -4.58193335E-6F, -4.4948074E-6F, -4.40759413E-6F, -4.3203E-6F, -4.23293E-6F, - -4.14548867E-6F, -4.057978E-6F, -3.97039821E-6F, -3.88276203E-6F, - -3.79506309E-6F, -3.70731141E-6F, -3.61951152E-6F, -3.53166342E-6F, - -3.44377713E-6F, -3.35585082E-6F, -3.2678945E-6F, -3.1799118E-6F, - -3.09190636E-6F, -3.00388547E-6F, -2.91585E-6F, -2.82780729E-6F, - -2.73976457E-6F, -2.65172457E-6F, -2.56369549E-6F, -2.47568278E-6F, - -2.38769098E-6F, -2.29972738E-6F, -2.21179471E-6F, -2.12390478E-6F, - -2.03606123E-6F, -1.94826953E-6F, -1.86054149E-6F, -1.77287893E-6F, - -1.68529E-6F, -1.59778301E-6F, -1.5103642E-6F, -1.42304452E-6F, - -1.33582944E-6F, -1.24872258E-6F, -1.16174215E-6F, -1.07488813E-6F, - -9.88170541E-7F, -9.01599378E-7F, -8.15184649E-7F, -7.28932719E-7F, - -6.42854502E-7F, -5.56959094E-7F, -4.71256499E-7F, -3.85754E-7F, - -3.00464308E-7F, -2.15397449E-7F, -1.30563421E-7F, -4.59713192E-8F, - 3.83661245E-8F, 1.22436177E-7F, 2.06231562E-7F, 2.89737727E-7F, - 3.72944669E-7F, 4.55840564E-7F, 5.38414497E-7F, 6.20648279E-7F, - 7.02531906E-7F, 7.84057193E-7F, 8.65206857E-7F, 9.45966349E-7F, - 1.02632475E-6F, 1.10626479E-6F, 1.18577827E-6F, 1.26484156E-6F, - 1.34344918E-6F, 1.42157842E-6F, 1.49922016E-6F, 1.57635168E-6F, - 1.65296387E-6F, 1.72903947E-6F, 1.80455572E-6F, 1.87950081E-6F, - 1.95385655E-6F, 2.02760566E-6F, 2.10073E-6F, 2.17320576E-6F, 2.24502401E-6F, - 2.31615923E-6F, 2.38659231E-6F, 2.45630326E-6F, 2.52527616E-6F, - 2.59348326E-6F, 2.66090819E-6F, 2.72752641E-6F, 2.79331607E-6F, - 2.85825718E-6F, 2.92232107E-6F, 2.98549139E-6F, 3.04773721E-6F, - 3.10903852E-6F, 3.16936394E-6F, 3.22869391E-6F, 3.28699889E-6F, - 3.34425204E-6F, 3.40042652E-6F, 3.45549051E-6F, 3.50941446E-6F, - 3.56217106E-6F, 3.6137294E-6F, 3.66405538E-6F, 3.71311808E-6F, - 3.76088246E-6F, 3.80731444E-6F, 3.85237854E-6F, 3.89603611E-6F, - 3.93825212E-6F, 3.97898657E-6F, 4.01819625E-6F, 4.05584478E-6F, - 4.09188397E-6F, 4.12627151E-6F, 4.15895875E-6F, 4.1899E-6F, 4.21904315E-6F, - 4.24633618E-6F, 4.2717279E-6F, 4.29515876E-6F, 4.31656963E-6F, 4.3358973E-6F, - 4.35308084E-6F, 4.36804976E-6F, 4.38073221E-6F, 4.39105452E-6F, - 4.39893483E-6F, 4.40428903E-6F, 4.4070307E-6F, 4.40705935E-6F }, - - { 0.0825F, 0.082625635F, 0.08274699F, 0.0828641355F, 0.082977131F, - 0.083086051F, 0.0831909552F, 0.083291918F, 0.083389F, 0.083482258F, - 0.0835717544F, 0.0836575627F, 0.0837397277F, 0.083818309F, 0.0838933736F, - 0.0839649737F, 0.0840331689F, 0.0840980038F, 0.0841595381F, 0.0842178315F, - 0.0842729211F, 0.084324874F, 0.0843737274F, 0.0844195336F, 0.0844623446F, - 0.0845022127F, 0.0845391676F, 0.0845732689F, 0.0846045539F, 0.0846330673F, - 0.0846588612F, 0.0846819654F, 0.0847024247F, 0.0847202763F, 0.0847355723F, - 0.0847483352F, 0.0847586095F, 0.0847664326F, 0.0847718418F, 0.0847748667F, - 0.0847755447F, 0.084773913F, 0.08477F, 0.0847638398F, 0.0847554579F, - 0.084744893F, 0.0847321674F, 0.0847173184F, 0.0847003683F, 0.0846813396F, - 0.0846602693F, 0.0846371725F, 0.0846120864F, 0.0845850259F, 0.0845560208F, - 0.0845250934F, 0.0844922587F, 0.0844575465F, 0.0844209716F, 0.0843825564F, - 0.0843423232F, 0.0843002871F, 0.0842564702F, 0.084210895F, 0.0841635615F, - 0.0841145F, 0.0840637162F, 0.0840112418F, 0.0839570761F, 0.083901234F, - 0.0838437378F, 0.0837845877F, 0.0837238058F, 0.0836614F, 0.0835973844F, - 0.0835317597F, 0.083464548F, 0.0833957493F, 0.0833253786F, 0.0832534432F, - 0.0831799433F, 0.0831048936F, 0.0830283F, 0.0829501674F, 0.0828705F, - 0.0827893F, 0.0827065781F, 0.0826223418F, 0.0825365856F, 0.0824493095F, - 0.0823605284F, 0.0822702423F, 0.0821784437F, 0.08208514F, 0.0819903314F, - 0.0818940252F, 0.0817962065F, 0.0816968903F, 0.081596069F, 0.0814937353F, - 0.081389904F, 0.0812845528F, 0.0811777F, 0.0810693204F, 0.0809594318F, - 0.0808480158F, 0.0807350799F, 0.0806206167F, 0.0805046186F, 0.0803870782F, - 0.080268F, 0.0801473707F, 0.0800251886F, 0.0799014494F, 0.0797761381F, - 0.0796492621F, 0.0795208F, 0.0793907568F, 0.0792591125F, 0.0791258737F, - 0.0789910182F, 0.0788545534F, 0.0787164569F, 0.0785767287F, 0.0784353614F, - 0.0782923326F, 0.0781476498F, 0.0780012906F, 0.077853255F, 0.0777035281F, - 0.0775521F, 0.0773989633F, 0.0772441104F, 0.0770875216F, 0.0769291893F, - 0.0767691135F, 0.0766072646F, 0.0764436498F, 0.0762782469F, 0.076111041F, - 0.0759420395F, 0.0757712126F, 0.0755985528F, 0.0754240528F, 0.0752477F, - 0.0750694722F, 0.0748893768F, 0.0747073889F, 0.0745235F, 0.0743377F, - 0.0741499662F, 0.0739603F, 0.0737686828F, 0.0735751F, 0.0733795539F, - 0.0731820166F, 0.0729824826F, 0.072780937F, 0.0725773647F, 0.072371766F, - 0.0721641108F, 0.0719544F, 0.071742624F, 0.0715287626F, 0.0713128075F, - 0.0710947439F, 0.0708745569F, 0.0706522539F, 0.0704278F, 0.0702012F, - 0.0699724406F, 0.0697415F, 0.0695083737F, 0.0692730546F, 0.0690355226F, - 0.0687957704F, 0.0685537905F, 0.0683095828F, 0.0680631176F, 0.0678143948F, - 0.0675634F, 0.0673101321F, 0.0670545697F, 0.0667967126F, 0.0665365383F, - 0.0662740618F, 0.0660092533F, 0.0657421201F, 0.0654726326F, 0.0652008057F, - 0.0649266243F, 0.0646500662F, 0.0643711537F, 0.0640898496F, 0.0638061538F, - 0.0635200813F, 0.0632316F, 0.0629407242F, 0.0626474321F, 0.0623517223F, - 0.0620536F, 0.0617530458F, 0.0614500828F, 0.0611446686F, 0.0608368181F, - 0.0605265312F, 0.0602138F, 0.0598986298F, 0.0595810153F, 0.0592609458F, - 0.0589384325F, 0.0586134642F, 0.0582860447F, 0.0579561703F, 0.0576238409F, - 0.0572890751F, 0.0569518507F, 0.0566121712F, 0.0562700555F, 0.0559254885F, - 0.0555784889F, 0.0552290566F, 0.054877162F, 0.0545228682F, 0.0541661382F, - 0.0538069829F, 0.0534454137F, 0.0530814491F, 0.0527150668F, 0.0523463078F, - 0.0519751534F, 0.0516016223F, 0.0512257218F, 0.0508474559F, 0.0504668579F, - 0.0500839F, 0.0496986173F, 0.0493110269F, 0.0489211343F, 0.0485289469F, - 0.0481344871F, 0.0477377549F, 0.04733878F, 0.0469375588F, 0.0465341397F, - 0.0461285152F, 0.0457207039F, 0.0453107134F, 0.0448985882F, 0.0444843248F, - 0.0440679714F, 0.0436495095F, 0.0432290025F, 0.0428064205F, 0.0423818417F, - 0.0419552475F, 0.0415266976F, 0.0410961919F, 0.0406637266F, 0.0402293913F, - 0.0397931822F, 0.0393551365F, 0.038915243F, 0.0384735875F, 0.0380301885F, - 0.0375850387F, 0.0371381938F, 0.0366896801F, 0.0362395607F, 0.0357878208F, - 0.0353345275F, 0.0348797F, 0.0344234072F, 0.0339656323F, 0.033506453F, - 0.033045914F, 0.032584019F, 0.0321208276F, 0.0316563807F, 0.0311907455F, - 0.0307239257F, 0.0302559771F, 0.0297869369F, 0.0293168686F, 0.0288458094F, - 0.0283738412F, 0.0279009566F, 0.0274272151F, 0.0269527063F, 0.0264774524F, - 0.0260014758F, 0.0255248956F, 0.0250477232F, 0.0245700441F, 0.0240918733F, - 0.0236133039F, 0.0231343843F, 0.0226551369F, 0.0221756734F, 0.0216960311F, - 0.0212163F, 0.0207364894F, 0.0202567242F, 0.0197770186F, 0.0192974396F, - 0.0188181363F, 0.0183390342F, 0.0178603418F, 0.0173820443F, 0.0169042759F, - 0.0164270438F, 0.0159504749F, 0.0154745989F, 0.01499952F, 0.0145253353F, - 0.0140520222F }, - - { -1.72601576E-5F, -1.7229886E-5F, -1.71986339E-5F, -1.71664233E-5F, - -1.71332704E-5F, -1.70991971E-5F, -1.70642161E-5F, -1.70283474E-5F, - -1.69916093E-5F, -1.69540162E-5F, -1.69155865E-5F, -1.68763381E-5F, - -1.68362858E-5F, -1.67954422E-5F, -1.67538255E-5F, -1.67114522E-5F, - -1.66683349E-5F, -1.66244881E-5F, -1.65799283E-5F, -1.65346701E-5F, - -1.64887242E-5F, -1.64421035E-5F, -1.63948262E-5F, -1.63469012E-5F, - -1.62983433E-5F, -1.62491633E-5F, -1.61993758E-5F, -1.61489916E-5F, - -1.60980198E-5F, -1.6046477E-5F, -1.59943702E-5F, -1.59417141E-5F, - -1.58885177E-5F, -1.58347902E-5F, -1.57805443E-5F, -1.57257891E-5F, - -1.56705355E-5F, -1.56147944E-5F, -1.55585712E-5F, -1.5501877E-5F, - -1.54447225E-5F, -1.53871151E-5F, -1.53290657E-5F, -1.52705816E-5F, - -1.52116681E-5F, -1.51523363E-5F, -1.50925953E-5F, -1.50324504E-5F, - -1.49719108E-5F, -1.49109837E-5F, -1.48496747E-5F, -1.47879928E-5F, - -1.47259452E-5F, -1.46635357E-5F, -1.46007742E-5F, -1.45376662E-5F, - -1.44742153E-5F, -1.44104315E-5F, -1.43463167E-5F, -1.42818799E-5F, - -1.42171248E-5F, -1.41520568E-5F, -1.4086685E-5F, -1.40210113E-5F, - -1.39550393E-5F, -1.38887754E-5F, -1.3822224E-5F, -1.37553907E-5F, - -1.36882791E-5F, -1.36208964E-5F, -1.35532428E-5F, -1.34853208E-5F, - -1.34171405E-5F, -1.33487029E-5F, -1.32800124E-5F, -1.3211069E-5F, - -1.3141881E-5F, -1.30724484E-5F, -1.30027765E-5F, -1.29328673E-5F, - -1.28627235E-5F, -1.27923504E-5F, -1.272175E-5F, -1.2650924E-5F, - -1.25798742E-5F, -1.25086062E-5F, -1.24371181E-5F, -1.2365419E-5F, - -1.22935044E-5F, -1.22213805E-5F, -1.21490484E-5F, -1.20765108E-5F, - -1.20037676E-5F, -1.19308233E-5F, -1.18576763E-5F, -1.17843329E-5F, - -1.17107929E-5F, -1.16370575E-5F, -1.15631265E-5F, -1.14890026E-5F, - -1.14146933E-5F, -1.13401893E-5F, -1.12655016E-5F, -1.11906247E-5F, - -1.11155623E-5F, -1.10403153E-5F, -1.09648845E-5F, -1.08892737E-5F, - -1.08134809E-5F, -1.07375072E-5F, -1.0661357E-5F, -1.05850268E-5F, - -1.05085192E-5F, -1.04318369E-5F, -1.03549764E-5F, -1.02779441E-5F, - -1.02007352E-5F, -1.01233545E-5F, -1.0045801E-5F, -9.96807648E-6F, - -9.89017735E-6F, -9.81211178E-6F, -9.7338725E-6F, -9.65546678E-6F, - -9.57689099E-6F, -9.49814785E-6F, -9.41923736E-6F, -9.3401577E-6F, - -9.26091434E-6F, -9.1815009E-6F, -9.10192648E-6F, -9.0221838E-6F, - -8.94228197E-6F, -8.86221096E-6F, -8.7819808E-6F, -8.7015851E-6F, - -8.62102934E-6F, -8.54031259E-6F, -8.45943305E-6F, -8.37839434E-6F, - -8.29719738E-6F, -8.21584E-6F, -8.13432325E-6F, -8.05265245E-6F, - -7.97082248E-6F, -7.88883699E-6F, -7.80669507E-6F, -7.72440126E-6F, - -7.64195374E-6F, -7.55935071E-6F, -7.47659669E-6F, -7.3936917E-6F, - -7.31063665E-6F, -7.22743061E-6F, -7.14408088E-6F, -7.06058108E-6F, - -6.97693667E-6F, -6.89314766E-6F, -6.80921494E-6F, -6.72514034E-6F, - -6.64092295E-6F, -6.5565664E-6F, -6.47207435E-6F, -6.38744405E-6F, - -6.30267914E-6F, -6.21777872E-6F, -6.13274733E-6F, -6.04758588E-6F, - -5.9622962E-6F, -5.876881E-6F, -5.79134121E-6F, -5.70567863E-6F, - -5.61989418E-6F, -5.53399241E-6F, -5.44797422E-6F, -5.36184143E-6F, - -5.27559496E-6F, -5.18924389E-6F, -5.10278369E-6F, -5.01622E-6F, - -4.92955405E-6F, -4.84279E-6F, -4.75592969E-6F, -4.66897836E-6F, - -4.58193335E-6F, -4.4948074E-6F, -4.40759413E-6F, -4.3203E-6F, -4.23293E-6F, - -4.14548867E-6F, -4.057978E-6F, -3.97039821E-6F, -3.88276203E-6F, - -3.79506309E-6F, -3.70731141E-6F, -3.61951152E-6F, -3.53166342E-6F, - -3.44377713E-6F, -3.35585082E-6F, -3.2678945E-6F, -3.1799118E-6F, - -3.09190636E-6F, -3.00388547E-6F, -2.91585E-6F, -2.82780729E-6F, - -2.73976457E-6F, -2.65172457E-6F, -2.56369549E-6F, -2.47568278E-6F, - -2.38769098E-6F, -2.29972738E-6F, -2.21179471E-6F, -2.12390478E-6F, - -2.03606123E-6F, -1.94826953E-6F, -1.86054149E-6F, -1.77287893E-6F, - -1.68529E-6F, -1.59778301E-6F, -1.5103642E-6F, -1.42304452E-6F, - -1.33582944E-6F, -1.24872258E-6F, -1.16174215E-6F, -1.07488813E-6F, - -9.88170541E-7F, -9.01599378E-7F, -8.15184649E-7F, -7.28932719E-7F, - -6.42854502E-7F, -5.56959094E-7F, -4.71256499E-7F, -3.85754E-7F, - -3.00464308E-7F, -2.15397449E-7F, -1.30563421E-7F, -4.59713192E-8F, - 3.83661245E-8F, 1.22436177E-7F, 2.06231562E-7F, 2.89737727E-7F, - 3.72944669E-7F, 4.55840564E-7F, 5.38414497E-7F, 6.20648279E-7F, - 7.02531906E-7F, 7.84057193E-7F, 8.65206857E-7F, 9.45966349E-7F, - 1.02632475E-6F, 1.10626479E-6F, 1.18577827E-6F, 1.26484156E-6F, - 1.34344918E-6F, 1.42157842E-6F, 1.49922016E-6F, 1.57635168E-6F, - 1.65296387E-6F, 1.72903947E-6F, 1.80455572E-6F, 1.87950081E-6F, - 1.95385655E-6F, 2.02760566E-6F, 2.10073E-6F, 2.17320576E-6F, 2.24502401E-6F, - 2.31615923E-6F, 2.38659231E-6F, 2.45630326E-6F, 2.52527616E-6F, - 2.59348326E-6F, 2.66090819E-6F, 2.72752641E-6F, 2.79331607E-6F, - 2.85825718E-6F, 2.92232107E-6F, 2.98549139E-6F, 3.04773721E-6F, - 3.10903852E-6F, 3.16936394E-6F, 3.22869391E-6F, 3.28699889E-6F, - 3.34425204E-6F, 3.40042652E-6F, 3.45549051E-6F, 3.50941446E-6F, - 3.56217106E-6F, 3.6137294E-6F, 3.66405538E-6F, 3.71311808E-6F, - 3.76088246E-6F, 3.80731444E-6F, 3.85237854E-6F, 3.89603611E-6F, - 3.93825212E-6F, 3.97898657E-6F, 4.01819625E-6F, 4.05584478E-6F, - 4.09188397E-6F, 4.12627151E-6F, 4.15895875E-6F, 4.1899E-6F, 4.21904315E-6F, - 4.24633618E-6F, 4.2717279E-6F, 4.29515876E-6F, 4.31656963E-6F, 4.3358973E-6F, - 4.35308084E-6F, 4.36804976E-6F, 4.38073221E-6F, 4.39105452E-6F, - 4.39893483E-6F, 4.40428903E-6F, 4.4070307E-6F, 4.40705935E-6F }, - - { 0.1524F, -0.1524F, 0.0718819946F, -0.0718819946F, -0.067564F, 0.067564F, - -0.1524F, 0.1524F, -0.0718819946F, 0.0718819946F, 0.067564F, -0.067564F, - 0.101854004F, 0.101854004F, 0.1524F, 0.1524F, 0.101854004F, 0.101854004F, - -0.101854004F, -0.101854004F, -0.1524F, -0.1524F, -0.101854004F, - -0.101854004F, -0.0396239981F, 0.0396239981F, 0.0718819946F, -0.0718819946F, - 0.1524F, -0.1524F, -0.0396239981F, 0.0396239981F, 0.0718819946F, - -0.0718819946F, 0.1524F, -0.1524F }, - - { 209.439514F, 261.799408F, 293.215332F }, - 227.470383F, - - { 1.0F, -1.0F, 0.0F, 0.0F, 0.0F, 0.0F, -1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, -1.0F, -1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, -1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, -1.0F }, - 0.0F, - 0U, - 0U, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - - { 1.0F, 2.0F, 3.0F }, - - { 1.0F, 2.0F, 3.0F }, - 0.0F, - 0.0F, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - 0.0F, - 0.0F, - 0.0F, - 2.0F, - 2.0F -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_private.h b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_private.h deleted file mode 100644 index dc52b45bc2..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_private.h +++ /dev/null @@ -1,45 +0,0 @@ -// -// File: fam_force_allocation_module_private.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:44:37 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_fam_force_allocation_module_private_h_ -#define RTW_HEADER_fam_force_allocation_module_private_h_ -#include "rtwtypes.h" -#if !defined(rt_VALIDATE_MEMORY) -#define rt_VALIDATE_MEMORY(S, ptr) if(!(ptr)) {\ - rtmSetErrorStatus(fam_force_allocation_module_M, RT_MEMORY_ALLOCATION_ERROR);\ - } -#endif - -#if !defined(rt_FREE) -#if !defined(_WIN32) -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((ptr));\ - (ptr) = (NULL);\ - } -#else - -// Visual and other windows compilers declare free without const -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((void *)(ptr));\ - (ptr) = (NULL);\ - } -#endif -#endif -#endif // RTW_HEADER_fam_force_allocation_module_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_types.h b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_types.h deleted file mode 100644 index 2736de7484..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_force_allocation_module_types.h +++ /dev/null @@ -1,134 +0,0 @@ -// -// File: fam_force_allocation_module_types.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:44:37 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_fam_force_allocation_module_types_h_ -#define RTW_HEADER_fam_force_allocation_module_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_ex_time_msg_ -#define DEFINED_TYPEDEF_FOR_ex_time_msg_ - -typedef struct { - uint32_T timestamp_sec; - uint32_T timestamp_nsec; -} ex_time_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmd_msg_ -#define DEFINED_TYPEDEF_FOR_cmd_msg_ - -typedef struct { - uint32_T cmd_timestamp_sec; - uint32_T cmd_timestamp_nsec; - uint8_T cmd_mode; - uint8_T speed_gain_cmd; - uint8_T cmd_B_inuse; - real32_T traj_pos[3]; - real32_T traj_vel[3]; - real32_T traj_accel[3]; - real32_T traj_quat[4]; - real32_T traj_omega[3]; - real32_T traj_alpha[3]; -} cmd_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ctl_msg_ -#define DEFINED_TYPEDEF_FOR_ctl_msg_ - -typedef struct { - real32_T body_force_cmd[3]; - real32_T body_accel_cmd[3]; - real32_T pos_err[3]; - real32_T pos_err_int[3]; - real32_T body_torque_cmd[3]; - real32_T body_alpha_cmd[3]; - real32_T att_err[3]; - real32_T att_err_mag; - real32_T att_err_int[3]; - uint8_T ctl_status; - real32_T traj_error_pos; - real32_T traj_error_att; - real32_T traj_error_vel; - real32_T traj_error_omega; -} ctl_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmc_state_cmd_ -#define DEFINED_TYPEDEF_FOR_cmc_state_cmd_ - -typedef struct { - uint32_T timestamp_sec; - uint32_T timestamp_nsec; - real32_T P_B_ISS_ISS[3]; - real32_T V_B_ISS_ISS[3]; - real32_T A_B_ISS_ISS[3]; - real32_T quat_ISS2B[4]; - real32_T omega_B_ISS_B[3]; - real32_T alpha_B_ISS_B[3]; -} cmc_state_cmd; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmc_msg_ -#define DEFINED_TYPEDEF_FOR_cmc_msg_ - -typedef struct { - cmc_state_cmd cmc_state_cmd_a; - cmc_state_cmd cmc_state_cmd_b; - uint8_T cmc_mode_cmd; - uint8_T speed_gain_cmd; - uint8_T localization_mode_cmd; - real32_T att_kp[3]; - real32_T att_ki[3]; - real32_T omega_kd[3]; - real32_T pos_kp[3]; - real32_T pos_ki[3]; - real32_T vel_kd[3]; - real32_T center_of_mass[3]; - real32_T inertia_matrix[9]; - real32_T mass; -} cmc_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_act_msg_ -#define DEFINED_TYPEDEF_FOR_act_msg_ - -typedef struct { - uint32_T act_timestamp_sec; - uint32_T act_timestamp_nsec; - uint8_T act_impeller_speed_cmd[2]; - real32_T act_servo_pwm_cmd[12]; - real32_T act_nozzle_theta[12]; - real32_T act_predicted_force_B[3]; - real32_T act_predicted_torque_B[3]; -} act_msg; - -#endif - -// Parameters (auto storage) -typedef struct P_fam_force_allocation_module_T_ P_fam_force_allocation_module_T; - -// Forward declaration for rtModel -typedef struct tag_RTM_fam_force_allocation__T RT_MODEL_fam_force_allocation_T; - -#endif // RTW_HEADER_fam_force_allocation_module_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_tunable_funcs.cpp b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_tunable_funcs.cpp deleted file mode 100644 index a80abf878f..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_tunable_funcs.cpp +++ /dev/null @@ -1,13 +0,0 @@ -// Autogenerated file, see write_tunable_param_functions for details - -#include "fam_tunable_funcs.h" -#include -#include -#include - -// Do config setup and add includes -void fam_ReadParams(config_reader::ConfigReader* config, RT_MODEL_fam_force_allocation_T* fam) { - - auto & p = fam->defaultParam; - -} diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_tunable_funcs.h b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_tunable_funcs.h deleted file mode 100644 index 51b76fdf67..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/fam_tunable_funcs.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef READ_PARAMS_CPP -#define READ_PARAMS_CPP - -namespace config_reader { - class ConfigReader; -} - -#include "fam_force_allocation_module.h" -void fam_ReadParams(config_reader::ConfigReader* config, RT_MODEL_fam_force_allocation_T* fam); - -#endif \ No newline at end of file diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/modelsources.txt b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/modelsources.txt deleted file mode 100644 index e551853564..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/modelsources.txt +++ /dev/null @@ -1 +0,0 @@ - fam_force_allocation_module.cpp diff --git a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/rtw_proj.tmw b/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/rtw_proj.tmw deleted file mode 100644 index 8551dd0592..0000000000 --- a/gnc/matlab/code_generation/fam_force_allocation_module_ert_rtw/rtw_proj.tmw +++ /dev/null @@ -1,4 +0,0 @@ -Simulink Coder project for fam_force_allocation_module using . MATLAB root = /usr/local/MATLAB/R2016b. SimStruct date: 22-Jul-2016 13:46:14 -This file is generated by Simulink Coder for use by the make utility -to determine when to rebuild objects when the name of the current Simulink Coder project changes. -The rtwinfomat located at: ../slprj/ert/fam_force_allocation_module/tmwinternal/binfo.mat diff --git a/gnc/matlab/code_generation/sharedutils/aaaacjecophlngdj_svd.cpp b/gnc/matlab/code_generation/sharedutils/aaaacjecophlngdj_svd.cpp deleted file mode 100644 index e531d5fa14..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaaacjecophlngdj_svd.cpp +++ /dev/null @@ -1,546 +0,0 @@ -// -// File: aaaacjecophlngdj_svd.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:39 2017 -// -#include "rtwtypes.h" -#include -#include -#include "cbielfcbnophecbi_xaxpy.h" -#include "cjekaaieekfkbimo_xnrm2.h" -#include "cjmgkfkfphdjaaai_xscal.h" -#include "djmgbimooppphlno_xswap.h" -#include "ekfklfkfohdjaimg_xaxpy.h" -#include "eknofcjenophiecj_xswap.h" -#include "fcbiohlnbimolnop_xnrm2.h" -#include "gdjmmgdjekfccjmg_xrot.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "hlfkiecjohlfopph_xaxpy.h" -#include "jekncbaiknglnoph_xdotc.h" -#include "mglnohlnekfcekfk_xdotc.h" -#include "ophdimgdcjekekfc_xrot.h" -#include "ophlaaaiophdbaai_xscal.h" -#include "opppmglfglfkdbaa_xaxpy.h" -#include "rt_nonfinite.h" -#include "aaaacjecophlngdj_svd.h" - -// Function for MATLAB Function: '/MATLAB Function' -void aaaacjecophlngdj_svd(const real32_T A[36], real32_T U[36], real32_T S[9], - real32_T V[9]) -{ - real32_T b_A[36]; - real32_T s[3]; - real32_T e[3]; - real32_T work[12]; - real32_T Vf[9]; - int32_T q; - boolean_T apply_transform; - int32_T m; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - int32_T j_ii; - real32_T varargin_1[5]; - int32_T i; - memcpy(&b_A[0], &A[0], 36U * sizeof(real32_T)); - e[0] = 0.0F; - e[1] = 0.0F; - e[2] = 0.0F; - for (i = 0; i < 12; i++) { - work[i] = 0.0F; - } - - memset(&U[0], 0, 36U * sizeof(real32_T)); - for (i = 0; i < 9; i++) { - Vf[i] = 0.0F; - } - - i = 0; - apply_transform = false; - snorm = cjekaaieekfkbimo_xnrm2(12, b_A, 1); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[0] < 0.0F) { - s[0] = -snorm; - } else { - s[0] = snorm; - } - - if ((real32_T)fabs((real_T)s[0]) >= 9.86076132E-32F) { - snorm = 1.0F / s[0]; - for (kase = 0; kase + 1 < 13; kase++) { - b_A[kase] *= snorm; - } - } else { - for (kase = 0; kase + 1 < 13; kase++) { - b_A[kase] /= s[0]; - } - } - - b_A[0]++; - s[0] = -s[0]; - } else { - s[0] = 0.0F; - } - - for (q = 1; q + 1 < 4; q++) { - kase = 12 * q; - if (apply_transform) { - hlfkiecjohlfopph_xaxpy(12, -(jekncbaiknglnoph_xdotc(12, b_A, 1, b_A, kase - + 1) / b_A[0]), 1, b_A, kase + 1); - } - - e[q] = b_A[kase]; - } - - while (i + 1 < 13) { - U[i] = b_A[i]; - i++; - } - - snorm = fcbiohlnbimolnop_xnrm2(2, e, 2); - if (snorm == 0.0F) { - e[0] = 0.0F; - } else { - if (e[1] < 0.0F) { - snorm = -snorm; - } - - e[0] = snorm; - if ((real32_T)fabs((real_T)snorm) >= 9.86076132E-32F) { - snorm = 1.0F / snorm; - for (i = 1; i + 1 < 4; i++) { - e[i] *= snorm; - } - } else { - for (i = 1; i + 1 < 4; i++) { - e[i] /= snorm; - } - } - - e[1]++; - e[0] = -e[0]; - for (i = 2; i < 13; i++) { - work[i - 1] = 0.0F; - } - - for (i = 1; i + 1 < 4; i++) { - opppmglfglfkdbaa_xaxpy(11, e[i], b_A, 12 * i + 2, work, 2); - } - - for (i = 1; i + 1 < 4; i++) { - cbielfcbnophecbi_xaxpy(11, -e[i] / e[1], work, 2, b_A, 12 * i + 2); - } - } - - for (i = 1; i + 1 < 4; i++) { - Vf[i] = e[i]; - } - - apply_transform = false; - snorm = cjekaaieekfkbimo_xnrm2(11, b_A, 14); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[13] < 0.0F) { - s[1] = -snorm; - } else { - s[1] = snorm; - } - - if ((real32_T)fabs((real_T)s[1]) >= 9.86076132E-32F) { - snorm = 1.0F / s[1]; - for (kase = 13; kase + 1 < 25; kase++) { - b_A[kase] *= snorm; - } - } else { - for (kase = 13; kase + 1 < 25; kase++) { - b_A[kase] /= s[1]; - } - } - - b_A[13]++; - s[1] = -s[1]; - } else { - s[1] = 0.0F; - } - - for (q = 2; q + 1 < 4; q++) { - kase = 12 * q + 1; - if (apply_transform) { - hlfkiecjohlfopph_xaxpy(11, -(jekncbaiknglnoph_xdotc(11, b_A, 14, b_A, kase - + 1) / b_A[13]), 14, b_A, kase + 1); - } - - e[q] = b_A[kase]; - } - - for (i = 1; i + 1 < 13; i++) { - U[i + 12] = b_A[i + 12]; - } - - snorm = cjekaaieekfkbimo_xnrm2(10, b_A, 27); - if (snorm > 0.0F) { - if (b_A[26] < 0.0F) { - s[2] = -snorm; - } else { - s[2] = snorm; - } - - if ((real32_T)fabs((real_T)s[2]) >= 9.86076132E-32F) { - snorm = 1.0F / s[2]; - for (kase = 26; kase + 1 < 37; kase++) { - b_A[kase] *= snorm; - } - } else { - for (kase = 26; kase + 1 < 37; kase++) { - b_A[kase] /= s[2]; - } - } - - b_A[26]++; - s[2] = -s[2]; - } else { - s[2] = 0.0F; - } - - for (i = 2; i + 1 < 13; i++) { - U[i + 24] = b_A[i + 24]; - } - - m = 1; - for (q = 2; q >= 0; q += -1) { - i = 12 * q + q; - if (s[q] != 0.0F) { - for (qs = q + 1; qs + 1 < 4; qs++) { - kase = (12 * qs + q) + 1; - hlfkiecjohlfopph_xaxpy(12 - q, -(jekncbaiknglnoph_xdotc(12 - q, U, i + 1, - U, kase) / U[i]), i + 1, U, kase); - } - - for (kase = q; kase + 1 < 13; kase++) { - U[kase + 12 * q] = -U[12 * q + kase]; - } - - U[i]++; - for (i = 1; i <= q; i++) { - U[(i + 12 * q) - 1] = 0.0F; - } - } else { - for (kase = 0; kase < 12; kase++) { - U[kase + 12 * q] = 0.0F; - } - - U[i] = 1.0F; - } - } - - for (i = 2; i >= 0; i += -1) { - if ((i + 1 <= 1) && (e[0] != 0.0F)) { - for (q = 2; q < 4; q++) { - kase = (q - 1) * 3 + 2; - ekfklfkfohdjaimg_xaxpy(2, -(mglnohlnekfcekfk_xdotc(2, Vf, 2, Vf, kase) / - Vf[1]), 2, Vf, kase); - } - } - - Vf[3 * i] = 0.0F; - Vf[1 + 3 * i] = 0.0F; - Vf[2 + 3 * i] = 0.0F; - Vf[i + 3 * i] = 1.0F; - } - - ztest = e[0]; - if (s[0] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[0]); - snorm = s[0] / ztest0; - s[0] = ztest0; - ztest = e[0] / snorm; - cjmgkfkfphdjaaai_xscal(snorm, U, 1); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[1] *= snorm; - ophlaaaiophdbaai_xscal(snorm, Vf, 4); - } - - e[0] = ztest; - ztest = b_A[25]; - if (s[1] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[1]); - snorm = s[1] / ztest0; - s[1] = ztest0; - ztest = b_A[25] / snorm; - cjmgkfkfphdjaaai_xscal(snorm, U, 13); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[2] *= snorm; - ophlaaaiophdbaai_xscal(snorm, Vf, 7); - } - - e[1] = ztest; - if (s[2] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[2]); - snorm = s[2] / ztest0; - s[2] = ztest0; - cjmgkfkfphdjaaai_xscal(snorm, U, 25); - } - - e[2] = 0.0F; - i = 0; - snorm = 0.0F; - if ((s[0] >= e[0]) || rtIsNaNF(e[0])) { - ztest0 = s[0]; - } else { - ztest0 = e[0]; - } - - if (!((0.0F >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - if ((s[1] >= ztest) || rtIsNaNF(ztest)) { - ztest = s[1]; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - - if (s[2] >= 0.0F) { - ztest0 = s[2]; - } else { - ztest0 = 0.0F; - } - - if (!((snorm >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - while ((m + 2 > 0) && (!(i >= 75))) { - kase = m + 1; - do { - qs = 0; - q = kase; - if (kase == 0) { - qs = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e[kase - 1]); - if ((ztest0 <= ((real32_T)fabs((real_T)s[kase - 1]) + (real32_T)fabs - ((real_T)s[kase])) * 1.1920929E-7F) || (ztest0 <= - 9.86076132E-32F) || ((i > 20) && (ztest0 <= 1.1920929E-7F * snorm))) - { - e[kase - 1] = 0.0F; - qs = 1; - } else { - kase--; - } - } - } while (qs == 0); - - if (m + 1 == kase) { - kase = 4; - } else { - qs = m + 2; - j_ii = m + 2; - apply_transform = false; - while ((!apply_transform) && (j_ii >= kase)) { - qs = j_ii; - if (j_ii == kase) { - apply_transform = true; - } else { - ztest0 = 0.0F; - if (j_ii < m + 2) { - ztest0 = (real32_T)fabs((real_T)e[j_ii - 1]); - } - - if (j_ii > kase + 1) { - ztest0 += (real32_T)fabs((real_T)e[j_ii - 2]); - } - - ztest = (real32_T)fabs((real_T)s[j_ii - 1]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s[j_ii - 1] = 0.0F; - apply_transform = true; - } else { - j_ii--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if (m + 2 == qs) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[m]; - e[m] = 0.0F; - for (kase = m; kase + 1 >= q + 1; kase--) { - ztest = s[kase]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[kase] = ztest; - if (kase + 1 > q + 1) { - ztest0 = -smm1 * e[0]; - e[0] *= sqds; - } - - gdjmmgdjekfccjmg_xrot(Vf, 1 + 3 * kase, 1 + 3 * (m + 1), sqds, smm1); - } - break; - - case 2: - ztest0 = e[q - 1]; - e[q - 1] = 0.0F; - for (kase = q; kase + 1 <= m + 2; kase++) { - ztest = s[kase]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[kase] = ztest; - ztest0 = -smm1 * e[kase]; - e[kase] *= sqds; - ophdimgdcjekekfc_xrot(U, 1 + 12 * kase, 1 + 12 * (q - 1), sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s[m + 1]); - varargin_1[1] = (real32_T)fabs((real_T)s[m]); - varargin_1[2] = (real32_T)fabs((real_T)e[m]); - varargin_1[3] = (real32_T)fabs((real_T)s[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - kase = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - qs = 2; - apply_transform = false; - while ((!apply_transform) && (qs < 6)) { - kase = qs; - if (!rtIsNaNF(varargin_1[qs - 1])) { - ztest = varargin_1[qs - 1]; - apply_transform = true; - } else { - qs++; - } - } - } - - if (kase < 5) { - while (kase + 1 < 6) { - if (varargin_1[kase] > ztest) { - ztest = varargin_1[kase]; - } - - kase++; - } - } - - ztest0 = s[m + 1] / ztest; - smm1 = s[m] / ztest; - emm1 = e[m] / ztest; - sqds = s[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - for (kase = q + 1; kase <= m + 1; kase++) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - if (kase > q + 1) { - e[0] = ztest0; - } - - ztest0 = s[kase - 1] * sqds + e[kase - 1] * smm1; - e[kase - 1] = e[kase - 1] * sqds - s[kase - 1] * smm1; - ztest = smm1 * s[kase]; - s[kase] *= sqds; - gdjmmgdjekfccjmg_xrot(Vf, 1 + 3 * (kase - 1), 1 + 3 * kase, sqds, smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s[kase - 1] = ztest0; - ztest0 = e[kase - 1] * sqds + smm1 * s[kase]; - s[kase] = e[kase - 1] * -smm1 + sqds * s[kase]; - ztest = smm1 * e[kase]; - e[kase] *= sqds; - ophdimgdcjekekfc_xrot(U, 1 + 12 * (kase - 1), 1 + 12 * kase, sqds, smm1); - } - - e[m] = ztest0; - i++; - break; - - default: - if (s[q] < 0.0F) { - s[q] = -s[q]; - ophlaaaiophdbaai_xscal(-1.0F, Vf, 1 + 3 * q); - } - - i = q + 1; - while ((q + 1 < 3) && (s[q] < s[i])) { - ztest0 = s[q]; - s[q] = s[i]; - s[i] = ztest0; - djmgbimooppphlno_xswap(Vf, 1 + 3 * q, 1 + 3 * (q + 1)); - eknofcjenophiecj_xswap(U, 1 + 12 * q, 1 + 12 * (q + 1)); - q = i; - i++; - } - - i = 0; - m--; - break; - } - } - - for (m = 0; m < 3; m++) { - e[m] = s[m]; - V[3 * m] = Vf[3 * m]; - V[1 + 3 * m] = Vf[3 * m + 1]; - V[2 + 3 * m] = Vf[3 * m + 2]; - } - - for (i = 0; i < 9; i++) { - S[i] = 0.0F; - } - - S[0] = e[0]; - S[4] = e[1]; - S[8] = e[2]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaaacjecophlngdj_svd.h b/gnc/matlab/code_generation/sharedutils/aaaacjecophlngdj_svd.h deleted file mode 100644 index a7b1804bf0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaaacjecophlngdj_svd.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: aaaacjecophlngdj_svd.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:39 2017 -// -#ifndef SHARE_aaaacjecophlngdj_svd -#define SHARE_aaaacjecophlngdj_svd -#include "rtwtypes.h" - -extern void aaaacjecophlngdj_svd(const real32_T A[36], real32_T U[36], real32_T - S[9], real32_T V[9]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaaahdbaecbaiecj_quatmult.cpp b/gnc/matlab/code_generation/sharedutils/aaaahdbaecbaiecj_quatmult.cpp deleted file mode 100644 index 309974fceb..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaaahdbaecbaiecj_quatmult.cpp +++ /dev/null @@ -1,37 +0,0 @@ -// -// File: aaaahdbaecbaiecj_quatmult.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#include "rtwtypes.h" -#include "aaaahdbaecbaiecj_quatmult.h" - -// Function for MATLAB Function: '/Compute Global positions of Handrail Features' -void aaaahdbaecbaiecj_quatmult(const real32_T p[4], const real32_T q[4], - real32_T qOut[4]) -{ - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - // - // Jesse C. Fusco jesse.c.fusco@nasa.gov - qOut[0] = (q[3] * p[0] + p[3] * q[0]) + (p[1] * q[2] - p[2] * q[1]); - qOut[1] = (q[3] * p[1] + p[3] * q[1]) + (p[2] * q[0] - p[0] * q[2]); - qOut[2] = (q[3] * p[2] + p[3] * q[2]) + (p[0] * q[1] - p[1] * q[0]); - qOut[3] = p[3] * q[3] - ((p[0] * q[0] + p[1] * q[1]) + p[2] * q[2]); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaaahdbaecbaiecj_quatmult.h b/gnc/matlab/code_generation/sharedutils/aaaahdbaecbaiecj_quatmult.h deleted file mode 100644 index 7de02732f3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaaahdbaecbaiecj_quatmult.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: aaaahdbaecbaiecj_quatmult.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#ifndef SHARE_aaaahdbaecbaiecj_quatmult -#define SHARE_aaaahdbaecbaiecj_quatmult -#include "rtwtypes.h" - -extern void aaaahdbaecbaiecj_quatmult(const real32_T p[4], const real32_T q[4], - real32_T qOut[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaiehlfccbimimgd_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/aaiehlfccbimimgd_xzlarf.cpp deleted file mode 100644 index 41ced356bc..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaiehlfccbimimgd_xzlarf.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// File: aaiehlfccbimimgd_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "biecdbaijecjlngd_xgemv.h" -#include "lfcjpphlnoppphdb_xgerc.h" -#include "ppppaaaacbimpphd_ilazlc.h" -#include "aaiehlfccbimimgd_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void aaiehlfccbimimgd_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work_data[]) -{ - int32_T lastv; - int32_T lastc; - if (tau != 0.0F) { - lastv = m; - lastc = (int32_T)(iv0 + m); - while ((lastv > 0) && (C_data[(int32_T)(lastc - 2)] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = ppppaaaacbimpphd_ilazlc(lastv, n, C_data, ic0, ldc); - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - biecdbaijecjlngd_xgemv(lastv, lastc, C_data, ic0, ldc, C_data, iv0, - work_data); - lfcjpphlnoppphdb_xgerc(lastv, lastc, -tau, iv0, work_data, C_data, ic0, ldc); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaiehlfccbimimgd_xzlarf.h b/gnc/matlab/code_generation/sharedutils/aaiehlfccbimimgd_xzlarf.h deleted file mode 100644 index 91fdbb66da..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaiehlfccbimimgd_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: aaiehlfccbimimgd_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_aaiehlfccbimimgd_xzlarf -#define SHARE_aaiehlfccbimimgd_xzlarf -#include "rtwtypes.h" - -extern void aaiehlfccbimimgd_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work_data[]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaiepphlecjelnoh_permute.cpp b/gnc/matlab/code_generation/sharedutils/aaiepphlecjelnoh_permute.cpp deleted file mode 100644 index aa22d62d2c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaiepphlecjelnoh_permute.cpp +++ /dev/null @@ -1,102 +0,0 @@ -// -// File: aaiepphlecjelnoh_permute.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "aaiepphlecjelnoh_permute.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void aaiepphlecjelnoh_permute(const real32_T a_data[], const int32_T a_sizes[3], - real32_T b_data[], int32_T b_sizes[3]) -{ - int8_T outsz[3]; - int32_T iwork[3]; - int32_T isrc; - int32_T c_k; - boolean_T c_b; - int32_T plast; - static const int8_T e[3] = { 3, 1, 2 }; - - int8_T b_a_idx_0; - int32_T inc_idx_0; - int32_T exitg1; - boolean_T exitg2; - boolean_T guard1 = false; - b_a_idx_0 = (int8_T)a_sizes[0]; - outsz[0] = 16; - outsz[1] = b_a_idx_0; - outsz[2] = 2; - b_sizes[0] = 16; - b_sizes[1] = (int32_T)b_a_idx_0; - b_sizes[2] = 2; - c_b = true; - if (!(a_sizes[0] == 0)) { - plast = 0; - isrc = 0; - exitg2 = false; - while ((!exitg2) && ((int32_T)(isrc + 1) < 4)) { - guard1 = false; - if (a_sizes[(int32_T)((int32_T)e[isrc] - 1)] != 1) { - if (plast > (int32_T)e[isrc]) { - c_b = false; - exitg2 = true; - } else { - plast = (int32_T)e[isrc]; - guard1 = true; - } - } else { - guard1 = true; - } - - if (guard1) { - isrc++; - } - } - } - - if (c_b) { - plast = (int32_T)(a_sizes[0] << 5); - for (isrc = 0; (int32_T)(isrc + 1) <= plast; isrc++) { - b_data[isrc] = a_data[isrc]; - } - } else { - inc_idx_0 = (int32_T)((int32_T)b_a_idx_0 * (int32_T)(int8_T)a_sizes[1]); - iwork[0] = 0; - iwork[1] = 0; - iwork[2] = 0; - plast = 0; - do { - isrc = (int32_T)((int32_T)(iwork[2] * (int32_T)b_a_idx_0) + iwork[1]); - for (c_k = 0; c_k < 16; c_k++) { - b_data[plast] = a_data[isrc]; - plast++; - isrc += inc_idx_0; - } - - isrc = 1; - do { - exitg1 = 0; - iwork[isrc]++; - if (iwork[isrc] < (int32_T)outsz[isrc]) { - exitg1 = 2; - } else if ((int32_T)(isrc + 1) == 3) { - exitg1 = 1; - } else { - iwork[1] = 0; - isrc = 2; - } - } while (exitg1 == 0); - } while (!(exitg1 == 1)); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaiepphlecjelnoh_permute.h b/gnc/matlab/code_generation/sharedutils/aaiepphlecjelnoh_permute.h deleted file mode 100644 index 063c6bfea4..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaiepphlecjelnoh_permute.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: aaiepphlecjelnoh_permute.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_aaiepphlecjelnoh_permute -#define SHARE_aaiepphlecjelnoh_permute -#include "rtwtypes.h" - -extern void aaiepphlecjelnoh_permute(const real32_T a_data[], const int32_T - a_sizes[3], real32_T b_data[], int32_T b_sizes[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaimfknongdbecjm_quat_rotation_vec.cpp b/gnc/matlab/code_generation/sharedutils/aaimfknongdbecjm_quat_rotation_vec.cpp deleted file mode 100644 index a15c7861fb..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaimfknongdbecjm_quat_rotation_vec.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// -// File: aaimfknongdbecjm_quat_rotation_vec.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Jun 19 10:14:31 2017 -// -#include "rtwtypes.h" -#include "aaimfknongdbecjm_quat_rotation_vec.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void aaimfknongdbecjm_quat_rotation_vec(const real_T vector[3], const real32_T - Q2[4], real32_T p_new[3]) -{ - real32_T qOut_idx_0; - real32_T qOut_idx_1; - real32_T qOut_idx_2; - real32_T qOut_idx_3; - - // - // p_new = quat_rotation(vector, Q2) - // - // rotates a vector of 3-vectors by a vector of quaternion Q2 - // - // if Q2 represents the rotation from frame 1 to frame 2 OR - // equivalently, the rotation of frame 2 with respect to - // frame 1, then rotating a vector p_expr_in_2 by quaternion - // q changes the vector to p_expr_in_1 - // - // p_new = vector part of Q2 x Q1 x Q2* - // where Q2* = [-1 -1 -1 1]' .* Q2 - qOut_idx_0 = ((0.0F * Q2[0] + (real32_T)vector[2] * Q2[1]) - (real32_T)vector - [1] * Q2[2]) + (real32_T)vector[0] * Q2[3]; - qOut_idx_1 = (((real32_T)-vector[2] * Q2[0] + 0.0F * Q2[1]) + (real32_T) - vector[0] * Q2[2]) + (real32_T)vector[1] * Q2[3]; - qOut_idx_2 = (((real32_T)vector[1] * Q2[0] - (real32_T)vector[0] * Q2[1]) + - 0.0F * Q2[2]) + (real32_T)vector[2] * Q2[3]; - qOut_idx_3 = (((real32_T)-vector[0] * Q2[0] - (real32_T)vector[1] * Q2[1]) - - (real32_T)vector[2] * Q2[2]) + 0.0F * Q2[3]; - - // q_mid = [ q2(4) q2(3) -q2(2) q2(1); - // -q2(3) q2(4) q2(1) q2(2); - // q2(2) -q2(1) q2(4) q2(3); - // -q2(1) -q2(2) -q2(3) q2(4)]; - // q_mid = [ q2(4) q2(3) -q2(2) q2(1); - // -q2(3) q2(4) q2(1) q2(2); - // q2(2) -q2(1) q2(4) q2(3); - // -q2(1) -q2(2) -q2(3) q2(4)]; - p_new[0] = ((Q2[3] * qOut_idx_0 + -Q2[2] * qOut_idx_1) - -Q2[1] * qOut_idx_2) - + -Q2[0] * qOut_idx_3; - p_new[1] = ((-(-Q2[2]) * qOut_idx_0 + Q2[3] * qOut_idx_1) + -Q2[0] * - qOut_idx_2) + -Q2[1] * qOut_idx_3; - p_new[2] = ((-Q2[1] * qOut_idx_0 - -Q2[0] * qOut_idx_1) + Q2[3] * qOut_idx_2) - + -Q2[2] * qOut_idx_3; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aaimfknongdbecjm_quat_rotation_vec.h b/gnc/matlab/code_generation/sharedutils/aaimfknongdbecjm_quat_rotation_vec.h deleted file mode 100644 index 0f8a80c914..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aaimfknongdbecjm_quat_rotation_vec.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: aaimfknongdbecjm_quat_rotation_vec.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Jun 19 10:14:31 2017 -// -#ifndef SHARE_aaimfknongdbecjm_quat_rotation_vec -#define SHARE_aaimfknongdbecjm_quat_rotation_vec -#include "rtwtypes.h" - -extern void aaimfknongdbecjm_quat_rotation_vec(const real_T vector[3], const - real32_T Q2[4], real32_T p_new[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aieciecjdjmgglfc_eulers_to_quat.cpp b/gnc/matlab/code_generation/sharedutils/aieciecjdjmgglfc_eulers_to_quat.cpp deleted file mode 100644 index d86009cd53..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aieciecjdjmgglfc_eulers_to_quat.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// -// File: aieciecjdjmgglfc_eulers_to_quat.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#include "rtwtypes.h" -#include -#include "aieciecjdjmgglfc_eulers_to_quat.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void aieciecjdjmgglfc_eulers_to_quat(real32_T phi, real32_T theta, real32_T psi, - real32_T quat[4]) -{ - real32_T c_psi; - real32_T s_psi; - real32_T c_theta; - real32_T s_theta; - real32_T c_phi; - real32_T s_phi; - - // Convert Euler angles to quaternions. Assumes 3-2-1 rotation sequence for - // euler angles. Corrected typo where first and 3rd rows were flipped. - // From: Fundamentals of Spacecraft Attitude Determination and Control, F. - // Landis Markley and John L. Crassisdis. Table: B.5 - // - // Jesse. C. Fusco jesse.c.fusco@nasa.gov - // - c_psi = (real32_T)cos((real_T)(psi / 2.0F)); - s_psi = (real32_T)sin((real_T)(psi / 2.0F)); - c_theta = (real32_T)cos((real_T)(theta / 2.0F)); - s_theta = (real32_T)sin((real_T)(theta / 2.0F)); - c_phi = (real32_T)cos((real_T)(phi / 2.0F)); - s_phi = (real32_T)sin((real_T)(phi / 2.0F)); - quat[0] = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi; - quat[2] = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi; - quat[1] = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi; - quat[3] = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aieciecjdjmgglfc_eulers_to_quat.h b/gnc/matlab/code_generation/sharedutils/aieciecjdjmgglfc_eulers_to_quat.h deleted file mode 100644 index b62589ac6e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aieciecjdjmgglfc_eulers_to_quat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: aieciecjdjmgglfc_eulers_to_quat.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#ifndef SHARE_aieciecjdjmgglfc_eulers_to_quat -#define SHARE_aieciecjdjmgglfc_eulers_to_quat -#include "rtwtypes.h" - -extern void aieciecjdjmgglfc_eulers_to_quat(real32_T phi, real32_T theta, - real32_T psi, real32_T quat[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aiekjmohekfccbaa_eulers_to_quat.cpp b/gnc/matlab/code_generation/sharedutils/aiekjmohekfccbaa_eulers_to_quat.cpp deleted file mode 100644 index 08de5bfba8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aiekjmohekfccbaa_eulers_to_quat.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// -// File: aiekjmohekfccbaa_eulers_to_quat.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include -#include "aiekjmohekfccbaa_eulers_to_quat.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void aiekjmohekfccbaa_eulers_to_quat(real32_T rz, real32_T ry, real32_T rx, - real32_T quat[4]) -{ - real32_T result; - int32_T i; - int32_T loop_ub; - real32_T quat_data[4]; - int32_T ii_sizes_idx_0; - int32_T ii_sizes_idx_1; - int32_T quat_sizes_idx_0; - quat[0] = (real32_T)cos((real_T)(rz / 2.0F)) * (real32_T)cos((real_T)(ry / - 2.0F)) * (real32_T)sin((real_T)(rx / 2.0F)) - (real32_T)sin((real_T)(rz / - 2.0F)) * (real32_T)sin((real_T)(ry / 2.0F)) * (real32_T)cos((real_T)(rx / - 2.0F)); - quat[1] = (real32_T)cos((real_T)(rz / 2.0F)) * (real32_T)sin((real_T)(ry / - 2.0F)) * (real32_T)cos((real_T)(rx / 2.0F)) + (real32_T)sin((real_T)(rz / - 2.0F)) * (real32_T)cos((real_T)(ry / 2.0F)) * (real32_T)sin((real_T)(rx / - 2.0F)); - quat[2] = (real32_T)sin((real_T)(rz / 2.0F)) * (real32_T)cos((real_T)(ry / - 2.0F)) * (real32_T)cos((real_T)(rx / 2.0F)) - (real32_T)cos((real_T)(rz / - 2.0F)) * (real32_T)sin((real_T)(ry / 2.0F)) * (real32_T)sin((real_T)(rx / - 2.0F)); - quat[3] = (real32_T)cos((real_T)(rz / 2.0F)) * (real32_T)cos((real_T)(ry / - 2.0F)) * (real32_T)cos((real_T)(rx / 2.0F)) + (real32_T)sin((real_T)(rz / - 2.0F)) * (real32_T)sin((real_T)(ry / 2.0F)) * (real32_T)sin((real_T)(rx / - 2.0F)); - - // Normalize the quaternion and make the scalar term positive - if (quat[3] < 0.0F) { - ii_sizes_idx_0 = 1; - ii_sizes_idx_1 = 1; - } else { - ii_sizes_idx_0 = 0; - ii_sizes_idx_1 = 0; - } - - quat_sizes_idx_0 = ii_sizes_idx_0 * ii_sizes_idx_1; - loop_ub = ii_sizes_idx_0 * ii_sizes_idx_1; - for (i = 0; i < loop_ub; i++) { - quat_data[i] = -quat[0]; - } - - loop_ub = ii_sizes_idx_0 * ii_sizes_idx_1; - for (i = 0; i < loop_ub; i++) { - quat_data[i + quat_sizes_idx_0] = -quat[1]; - } - - loop_ub = ii_sizes_idx_0 * ii_sizes_idx_1; - for (i = 0; i < loop_ub; i++) { - quat_data[i + (quat_sizes_idx_0 << 1)] = -quat[2]; - } - - loop_ub = ii_sizes_idx_0 * ii_sizes_idx_1; - for (i = 0; i < loop_ub; i++) { - quat_data[i + quat_sizes_idx_0 * 3] = -quat[3]; - } - - for (i = 0; i < quat_sizes_idx_0; i++) { - quat[0] = quat_data[i]; - } - - for (i = 0; i < quat_sizes_idx_0; i++) { - quat[1] = quat_data[i + quat_sizes_idx_0]; - } - - for (i = 0; i < quat_sizes_idx_0; i++) { - quat[2] = quat_data[(quat_sizes_idx_0 << 1) + i]; - } - - for (i = 0; i < quat_sizes_idx_0; i++) { - quat[3] = quat_data[quat_sizes_idx_0 * 3 + i]; - } - - // - // Column_Vector = RSSROW(Matrix) - // - // where Matrix is an n-by-m matrix. This function will return the RSS of - // each row into an n-by-1 column vector for any number of m columns. - // - // Douglas Adams, Feb, 2005 - // - result = 1.0F / (real32_T)sqrt((real_T)(((quat[0] * quat[0] + quat[1] * quat[1]) - + quat[2] * quat[2]) + quat[3] * quat[3])); - quat[0] *= result; - quat[1] *= result; - quat[2] *= result; - quat[3] *= result; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aiekjmohekfccbaa_eulers_to_quat.h b/gnc/matlab/code_generation/sharedutils/aiekjmohekfccbaa_eulers_to_quat.h deleted file mode 100644 index b554fa646d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aiekjmohekfccbaa_eulers_to_quat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: aiekjmohekfccbaa_eulers_to_quat.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_aiekjmohekfccbaa_eulers_to_quat -#define SHARE_aiekjmohekfccbaa_eulers_to_quat -#include "rtwtypes.h" - -extern void aiekjmohekfccbaa_eulers_to_quat(real32_T rz, real32_T ry, real32_T - rx, real32_T quat[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aimgbiekknohhdjm_norm.cpp b/gnc/matlab/code_generation/sharedutils/aimgbiekknohhdjm_norm.cpp deleted file mode 100644 index a94d9de056..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aimgbiekknohhdjm_norm.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// File: aimgbiekknohhdjm_norm.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "aimgbiekknohhdjm_norm.h" - -// Function for MATLAB Function: '/MATLAB Function' -real32_T aimgbiekknohhdjm_norm(const real32_T x[3]) -{ - real32_T y; - real32_T scale; - real32_T absxk; - real32_T t; - scale = 1.17549435E-38F; - absxk = (real32_T)fabs((real_T)x[0]); - if (absxk > 1.17549435E-38F) { - y = 1.0F; - scale = absxk; - } else { - t = absxk / 1.17549435E-38F; - y = t * t; - } - - absxk = (real32_T)fabs((real_T)x[1]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - - absxk = (real32_T)fabs((real_T)x[2]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - - return scale * (real32_T)sqrt((real_T)y); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aimgbiekknohhdjm_norm.h b/gnc/matlab/code_generation/sharedutils/aimgbiekknohhdjm_norm.h deleted file mode 100644 index dc579825d6..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aimgbiekknohhdjm_norm.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: aimgbiekknohhdjm_norm.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_aimgbiekknohhdjm_norm -#define SHARE_aimgbiekknohhdjm_norm -#include "rtwtypes.h" - -extern real32_T aimgbiekknohhdjm_norm(const real32_T x[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aimgimglphlffkfk_xswap.cpp b/gnc/matlab/code_generation/sharedutils/aimgimglphlffkfk_xswap.cpp deleted file mode 100644 index bb5368e990..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aimgimglphlffkfk_xswap.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// -// File: aimgimglphlffkfk_xswap.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "aimgimglphlffkfk_xswap.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void aimgimglphlffkfk_xswap(real32_T x[16], int32_T ix0, int32_T iy0) -{ - int32_T ix; - int32_T iy; - real32_T temp; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; - ix++; - iy++; - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; - ix++; - iy++; - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; - ix++; - iy++; - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aimgimglphlffkfk_xswap.h b/gnc/matlab/code_generation/sharedutils/aimgimglphlffkfk_xswap.h deleted file mode 100644 index 84574dd271..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aimgimglphlffkfk_xswap.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: aimgimglphlffkfk_xswap.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_aimgimglphlffkfk_xswap -#define SHARE_aimgimglphlffkfk_xswap -#include "rtwtypes.h" - -extern void aimgimglphlffkfk_xswap(real32_T x[16], int32_T ix0, int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aimgnophaiekjecb_eml_rand_mt19937ar.cpp b/gnc/matlab/code_generation/sharedutils/aimgnophaiekjecb_eml_rand_mt19937ar.cpp deleted file mode 100644 index 676b3ddbad..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aimgnophaiekjecb_eml_rand_mt19937ar.cpp +++ /dev/null @@ -1,160 +0,0 @@ -// -// File: aimgnophaiekjecb_eml_rand_mt19937ar.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "aimgnophaiekjecb_eml_rand_mt19937ar.h" - -// Function for MATLAB Function: '/generate_output' -real_T aimgnophaiekjecb_eml_rand_mt19937ar(uint32_T state[625]) -{ - real_T r; - uint32_T u[2]; - uint32_T y; - int32_T kk; - int32_T k; - boolean_T b_isvalid; - uint32_T c_r; - int32_T exitg1; - boolean_T exitg2; - - // ========================= COPYRIGHT NOTICE ============================ - // This is a uniform (0,1) pseudorandom number generator based on: - // - // A C-program for MT19937, with initialization improved 2002/1/26. - // Coded by Takuji Nishimura and Makoto Matsumoto. - // - // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, - // All rights reserved. - // - // Redistribution and use in source and binary forms, with or without - // modification, are permitted provided that the following conditions - // are met: - // - // 1. Redistributions of source code must retain the above copyright - // notice, this list of conditions and the following disclaimer. - // - // 2. Redistributions in binary form must reproduce the above copyright - // notice, this list of conditions and the following disclaimer - // in the documentation and/or other materials provided with the - // distribution. - // - // 3. The names of its contributors may not be used to endorse or - // promote products derived from this software without specific - // prior written permission. - // - // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR - // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, - // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY - // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - // - // ============================= END ================================= - do { - exitg1 = 0; - for (k = 0; k < 2; k++) { - c_r = (uint32_T)(state[624] + 1U); - if (c_r >= 625U) { - for (kk = 0; kk < 227; kk++) { - y = (uint32_T)((uint32_T)(state[(int32_T)(1 + kk)] & 2147483647U) | - (uint32_T)(state[kk] & 2147483648U)); - if ((int32_T)(uint32_T)(y & 1U) == 0) { - y >>= 1U; - } else { - y = (uint32_T)((uint32_T)(y >> 1U) ^ 2567483615U); - } - - state[kk] = (uint32_T)(state[(int32_T)(397 + kk)] ^ y); - } - - for (kk = 0; kk < 396; kk++) { - y = (uint32_T)((uint32_T)(state[(int32_T)(kk + 227)] & 2147483648U) | - (uint32_T)(state[(int32_T)(228 + kk)] & 2147483647U)); - if ((int32_T)(uint32_T)(y & 1U) == 0) { - y >>= 1U; - } else { - y = (uint32_T)((uint32_T)(y >> 1U) ^ 2567483615U); - } - - state[(int32_T)(kk + 227)] = (uint32_T)(state[kk] ^ y); - } - - y = (uint32_T)((uint32_T)(state[623] & 2147483648U) | (uint32_T)(state[0] - & 2147483647U)); - if ((int32_T)(uint32_T)(y & 1U) == 0) { - y >>= 1U; - } else { - y = (uint32_T)((uint32_T)(y >> 1U) ^ 2567483615U); - } - - state[623] = (uint32_T)(state[396] ^ y); - c_r = 1U; - } - - y = state[(int32_T)((int32_T)c_r - 1)]; - state[624] = c_r; - y ^= (uint32_T)(y >> 11U); - y ^= (uint32_T)((uint32_T)(y << 7U) & 2636928640U); - y ^= (uint32_T)((uint32_T)(y << 15U) & 4022730752U); - y ^= (uint32_T)(y >> 18U); - u[k] = y; - } - - r = ((real_T)(uint32_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)(uint32_T)(u[1] - >> 6U)) * 1.1102230246251565E-16; - if (r == 0.0) { - if ((state[624] >= 1U) && (state[624] < 625U)) { - b_isvalid = true; - } else { - b_isvalid = false; - } - - if (b_isvalid) { - b_isvalid = false; - k = 1; - exitg2 = false; - while ((!exitg2) && (k < 625)) { - if (state[(int32_T)(k - 1)] == 0U) { - k++; - } else { - b_isvalid = true; - exitg2 = true; - } - } - } - - if (!b_isvalid) { - c_r = 5489U; - state[0] = 5489U; - for (k = 0; k < 623; k++) { - c_r = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(c_r >> - 30U) ^ c_r) * 1812433253U) + (uint32_T)k) + 1U); - state[(int32_T)(k + 1)] = c_r; - } - - state[624] = 624U; - } - } else { - exitg1 = 1; - } - } while (exitg1 == 0); - - return r; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/aimgnophaiekjecb_eml_rand_mt19937ar.h b/gnc/matlab/code_generation/sharedutils/aimgnophaiekjecb_eml_rand_mt19937ar.h deleted file mode 100644 index c736668010..0000000000 --- a/gnc/matlab/code_generation/sharedutils/aimgnophaiekjecb_eml_rand_mt19937ar.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: aimgnophaiekjecb_eml_rand_mt19937ar.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_aimgnophaiekjecb_eml_rand_mt19937ar -#define SHARE_aimgnophaiekjecb_eml_rand_mt19937ar -#include "rtwtypes.h" - -extern real_T aimgnophaiekjecb_eml_rand_mt19937ar(uint32_T state[625]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baaanophjmgddbie_sum.cpp b/gnc/matlab/code_generation/sharedutils/baaanophjmgddbie_sum.cpp deleted file mode 100644 index 3fcf75fb69..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baaanophjmgddbie_sum.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// -// File: baaanophjmgddbie_sum.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "baaanophjmgddbie_sum.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real_T baaanophjmgddbie_sum(const int32_T x[16]) -{ - real_T y; - int32_T k; - y = (real_T)x[0]; - for (k = 0; k < 15; k++) { - y += (real_T)x[(int32_T)(k + 1)]; - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baaanophjmgddbie_sum.h b/gnc/matlab/code_generation/sharedutils/baaanophjmgddbie_sum.h deleted file mode 100644 index 137ec5640f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baaanophjmgddbie_sum.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: baaanophjmgddbie_sum.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_baaanophjmgddbie_sum -#define SHARE_baaanophjmgddbie_sum -#include "rtwtypes.h" - -extern real_T baaanophjmgddbie_sum(const int32_T x[16]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baaicbiepphdbiek_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/baaicbiepphdbiek_xaxpy.cpp deleted file mode 100644 index 5fbb7df074..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baaicbiepphdbiek_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: baaicbiepphdbiek_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "baaicbiepphdbiek_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void baaicbiepphdbiek_xaxpy(int32_T n, real32_T a, const real32_T x_data[], - int32_T ix0, real32_T y_data[], int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y_data[iy] += a * x_data[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baaicbiepphdbiek_xaxpy.h b/gnc/matlab/code_generation/sharedutils/baaicbiepphdbiek_xaxpy.h deleted file mode 100644 index 36eb2259be..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baaicbiepphdbiek_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: baaicbiepphdbiek_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_baaicbiepphdbiek_xaxpy -#define SHARE_baaicbiepphdbiek_xaxpy -#include "rtwtypes.h" - -extern void baaicbiepphdbiek_xaxpy(int32_T n, real32_T a, const real32_T x_data[], - int32_T ix0, real32_T y_data[], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baaiimglmglniekf_sortIdx.cpp b/gnc/matlab/code_generation/sharedutils/baaiimglmglniekf_sortIdx.cpp deleted file mode 100644 index 13a5a01bb8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baaiimglmglniekf_sortIdx.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// -// File: baaiimglmglniekf_sortIdx.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "baaiimglmglniekf_sortIdx.h" - -// Function for MATLAB Function: '/generate_output' -void baaiimglmglniekf_sortIdx(const real_T x_data[], const int32_T x_sizes[2], - int32_T idx_data[], int32_T idx_sizes[2]) -{ - int32_T n; - int32_T k; - int32_T i; - int32_T i2; - int32_T j; - int32_T pEnd; - int32_T p; - int32_T q; - int32_T qEnd; - int32_T kEnd; - int32_T iwork_data[504]; - n = (int32_T)(x_sizes[1] + 1); - idx_sizes[0] = 1; - idx_sizes[1] = x_sizes[1]; - i = x_sizes[1]; - for (i2 = 0; i2 <= (int32_T)(i - 1); i2++) { - idx_data[i2] = 0; - } - - if (x_sizes[1] != 0) { - for (i = 1; i <= (int32_T)(n - 2); i += 2) { - if (x_data[(int32_T)(i - 1)] <= x_data[i]) { - idx_data[(int32_T)(i - 1)] = i; - idx_data[i] = (int32_T)(i + 1); - } else { - idx_data[(int32_T)(i - 1)] = (int32_T)(i + 1); - idx_data[i] = i; - } - } - - if ((int32_T)(x_sizes[1] & 1) != 0) { - idx_data[(int32_T)(x_sizes[1] - 1)] = x_sizes[1]; - } - - i = 2; - while (i < (int32_T)(n - 1)) { - i2 = (int32_T)(i << 1); - j = 1; - pEnd = (int32_T)(1 + i); - while (pEnd < n) { - p = j; - q = pEnd; - qEnd = (int32_T)(j + i2); - if (qEnd > n) { - qEnd = n; - } - - k = 0; - kEnd = (int32_T)(qEnd - j); - while ((int32_T)(k + 1) <= kEnd) { - if (x_data[(int32_T)(idx_data[(int32_T)(p - 1)] - 1)] <= x_data - [(int32_T)(idx_data[(int32_T)(q - 1)] - 1)]) { - iwork_data[k] = idx_data[(int32_T)(p - 1)]; - p++; - if (p == pEnd) { - while (q < qEnd) { - k++; - iwork_data[k] = idx_data[(int32_T)(q - 1)]; - q++; - } - } - } else { - iwork_data[k] = idx_data[(int32_T)(q - 1)]; - q++; - if (q == qEnd) { - while (p < pEnd) { - k++; - iwork_data[k] = idx_data[(int32_T)(p - 1)]; - p++; - } - } - } - - k++; - } - - for (pEnd = 0; (int32_T)(pEnd + 1) <= kEnd; pEnd++) { - idx_data[(int32_T)((int32_T)(j + pEnd) - 1)] = iwork_data[pEnd]; - } - - j = qEnd; - pEnd = (int32_T)(qEnd + i); - } - - i = i2; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baaiimglmglniekf_sortIdx.h b/gnc/matlab/code_generation/sharedutils/baaiimglmglniekf_sortIdx.h deleted file mode 100644 index ce4eb1d8d0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baaiimglmglniekf_sortIdx.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: baaiimglmglniekf_sortIdx.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_baaiimglmglniekf_sortIdx -#define SHARE_baaiimglmglniekf_sortIdx -#include "rtwtypes.h" - -extern void baaiimglmglniekf_sortIdx(const real_T x_data[], const int32_T - x_sizes[2], int32_T idx_data[], int32_T idx_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baieimopcbaiaaai_eulers_to_quat.cpp b/gnc/matlab/code_generation/sharedutils/baieimopcbaiaaai_eulers_to_quat.cpp deleted file mode 100644 index e790b63e99..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baieimopcbaiaaai_eulers_to_quat.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// -// File: baieimopcbaiaaai_eulers_to_quat.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "baieimopcbaiaaai_eulers_to_quat.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void baieimopcbaiaaai_eulers_to_quat(real32_T phi, real32_T theta, real32_T psi, - real32_T quat[4]) -{ - real32_T c_psi; - real32_T s_psi; - real32_T c_theta; - real32_T s_theta; - real32_T c_phi; - real32_T s_phi; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Convert Euler angles to quaternions. Assumes 3-2-1 rotation sequence for - // euler angles. Corrected typo where first and 3rd rows were flipped. - // From: Fundamentals of Spacecraft Attitude Determination and Control, F. - // Landis Markley and John L. Crassisdis. Table: B.5 - c_psi = (real32_T)cos((real_T)(psi / 2.0F)); - s_psi = (real32_T)sin((real_T)(psi / 2.0F)); - c_theta = (real32_T)cos((real_T)(theta / 2.0F)); - s_theta = (real32_T)sin((real_T)(theta / 2.0F)); - c_phi = (real32_T)cos((real_T)(phi / 2.0F)); - s_phi = (real32_T)sin((real_T)(phi / 2.0F)); - quat[0] = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi; - quat[2] = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi; - quat[1] = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi; - quat[3] = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baieimopcbaiaaai_eulers_to_quat.h b/gnc/matlab/code_generation/sharedutils/baieimopcbaiaaai_eulers_to_quat.h deleted file mode 100644 index 786b74256e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baieimopcbaiaaai_eulers_to_quat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: baieimopcbaiaaai_eulers_to_quat.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_baieimopcbaiaaai_eulers_to_quat -#define SHARE_baieimopcbaiaaai_eulers_to_quat -#include "rtwtypes.h" - -extern void baieimopcbaiaaai_eulers_to_quat(real32_T phi, real32_T theta, - real32_T psi, real32_T quat[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baimfcjeeknonglf_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/baimfcjeeknonglf_xzlarf.cpp deleted file mode 100644 index 9906f676f9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baimfcjeeknonglf_xzlarf.cpp +++ /dev/null @@ -1,112 +0,0 @@ -// -// File: baimfcjeeknonglf_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "baimfcjeeknonglf_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void baimfcjeeknonglf_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]) -{ - int32_T lastv; - int32_T lastc; - int32_T ix; - real32_T c; - int32_T iy; - int32_T iac; - int32_T d; - int32_T b_ia; - int32_T jy; - boolean_T exitg2; - if (tau != 0.0F) { - lastv = m; - lastc = iv0 + m; - while ((lastv > 0) && (C_data[lastc - 2] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = n; - exitg2 = false; - while ((!exitg2) && (lastc > 0)) { - iy = (lastc - 1) * ldc + ic0; - jy = iy; - do { - iac = 0; - if (jy <= (iy + lastv) - 1) { - if (C_data[jy - 1] != 0.0F) { - iac = 1; - } else { - jy++; - } - } else { - lastc--; - iac = 2; - } - } while (iac == 0); - - if (iac == 1) { - exitg2 = true; - } - } - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - if (lastc != 0) { - for (iy = 1; iy <= lastc; iy++) { - work[iy - 1] = 0.0F; - } - - iy = 0; - jy = (lastc - 1) * ldc + ic0; - iac = ic0; - while ((ldc > 0) && (iac <= jy)) { - ix = iv0; - c = 0.0F; - d = (iac + lastv) - 1; - for (b_ia = iac; b_ia <= d; b_ia++) { - c += C_data[b_ia - 1] * C_data[ix - 1]; - ix++; - } - - work[iy] += c; - iy++; - iac += ldc; - } - } - - if (!(-tau == 0.0F)) { - iy = ic0 - 1; - jy = 0; - for (iac = 1; iac <= lastc; iac++) { - if (work[jy] != 0.0F) { - c = work[jy] * -tau; - ix = iv0; - d = lastv + iy; - for (b_ia = iy; b_ia + 1 <= d; b_ia++) { - C_data[b_ia] += C_data[ix - 1] * c; - ix++; - } - } - - jy++; - iy += ldc; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baimfcjeeknonglf_xzlarf.h b/gnc/matlab/code_generation/sharedutils/baimfcjeeknonglf_xzlarf.h deleted file mode 100644 index af93fd9161..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baimfcjeeknonglf_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: baimfcjeeknonglf_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_baimfcjeeknonglf_xzlarf -#define SHARE_baimfcjeeknonglf_xzlarf -#include "rtwtypes.h" - -extern void baimfcjeeknonglf_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baimopphohlfhlfk_quat_rotation_vec.cpp b/gnc/matlab/code_generation/sharedutils/baimopphohlfhlfk_quat_rotation_vec.cpp deleted file mode 100644 index 925c5a23c2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baimopphohlfhlfk_quat_rotation_vec.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// -// File: baimopphohlfhlfk_quat_rotation_vec.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#include "rtwtypes.h" -#include "baimopphohlfhlfk_quat_rotation_vec.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void baimopphohlfhlfk_quat_rotation_vec(const real_T vector[3], const real32_T - Q2[4], real32_T p_new[3]) -{ - real32_T qOut_idx_0; - real32_T qOut_idx_1; - real32_T qOut_idx_2; - real32_T qOut_idx_3; - - // - // Q2 = q_a2b, rotation represents a vector rotating from frame b2a - // p_new = quat_rotation(vector, Q2) - // - // rotates a vector of 3-vectors by a vector of quaternion Q2 - // - // if Q2 represents the rotation from frame 1 to frame 2 OR - // equivalently, the rotation of frame 2 with respect to - // frame 1, then rotating a vector p_expr_in_2 by quaternion - // q changes the vector to p_expr_in_1 - // - // p_new = vector part of Q2 x Q1 x Q2* - // where Q2* = [-1 -1 -1 1]' .* Q2 - qOut_idx_0 = ((0.0F * Q2[0] + (real32_T)vector[2] * Q2[1]) - (real32_T)vector - [1] * Q2[2]) + (real32_T)vector[0] * Q2[3]; - qOut_idx_1 = (((real32_T)-vector[2] * Q2[0] + 0.0F * Q2[1]) + (real32_T) - vector[0] * Q2[2]) + (real32_T)vector[1] * Q2[3]; - qOut_idx_2 = (((real32_T)vector[1] * Q2[0] - (real32_T)vector[0] * Q2[1]) + - 0.0F * Q2[2]) + (real32_T)vector[2] * Q2[3]; - qOut_idx_3 = (((real32_T)-vector[0] * Q2[0] - (real32_T)vector[1] * Q2[1]) - - (real32_T)vector[2] * Q2[2]) + 0.0F * Q2[3]; - - // q_mid = [ q2(4) q2(3) -q2(2) q2(1); - // -q2(3) q2(4) q2(1) q2(2); - // q2(2) -q2(1) q2(4) q2(3); - // -q2(1) -q2(2) -q2(3) q2(4)]; - // q_mid = [ q2(4) q2(3) -q2(2) q2(1); - // -q2(3) q2(4) q2(1) q2(2); - // q2(2) -q2(1) q2(4) q2(3); - // -q2(1) -q2(2) -q2(3) q2(4)]; - p_new[0] = ((Q2[3] * qOut_idx_0 + -Q2[2] * qOut_idx_1) - -Q2[1] * qOut_idx_2) - + -Q2[0] * qOut_idx_3; - p_new[1] = ((-(-Q2[2]) * qOut_idx_0 + Q2[3] * qOut_idx_1) + -Q2[0] * - qOut_idx_2) + -Q2[1] * qOut_idx_3; - p_new[2] = ((-Q2[1] * qOut_idx_0 - -Q2[0] * qOut_idx_1) + Q2[3] * qOut_idx_2) - + -Q2[2] * qOut_idx_3; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/baimopphohlfhlfk_quat_rotation_vec.h b/gnc/matlab/code_generation/sharedutils/baimopphohlfhlfk_quat_rotation_vec.h deleted file mode 100644 index 161adae1b2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/baimopphohlfhlfk_quat_rotation_vec.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: baimopphohlfhlfk_quat_rotation_vec.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#ifndef SHARE_baimopphohlfhlfk_quat_rotation_vec -#define SHARE_baimopphohlfhlfk_quat_rotation_vec -#include "rtwtypes.h" - -extern void baimopphohlfhlfk_quat_rotation_vec(const real_T vector[3], const - real32_T Q2[4], real32_T p_new[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biecdbaijecjlngd_xgemv.cpp b/gnc/matlab/code_generation/sharedutils/biecdbaijecjlngd_xgemv.cpp deleted file mode 100644 index 15022671f6..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biecdbaijecjlngd_xgemv.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: biecdbaijecjlngd_xgemv.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "biecdbaijecjlngd_xgemv.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void biecdbaijecjlngd_xgemv(int32_T m, int32_T n, const real32_T A_data[], - int32_T ia0, int32_T lda, const real32_T x_data[], int32_T ix0, real32_T - y_data[]) -{ - int32_T ix; - real32_T c; - int32_T iy; - int32_T b; - int32_T iac; - int32_T d; - int32_T ia; - if (n != 0) { - for (iy = 1; iy <= n; iy++) { - y_data[(int32_T)(iy - 1)] = 0.0F; - } - - iy = 0; - b = (int32_T)((int32_T)((int32_T)(n - 1) * lda) + ia0); - iac = ia0; - while ((lda > 0) && (iac <= b)) { - ix = ix0; - c = 0.0F; - d = (int32_T)((int32_T)(iac + m) - 1); - for (ia = iac; ia <= d; ia++) { - c += A_data[(int32_T)(ia - 1)] * x_data[(int32_T)(ix - 1)]; - ix++; - } - - y_data[iy] += c; - iy++; - iac += lda; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biecdbaijecjlngd_xgemv.h b/gnc/matlab/code_generation/sharedutils/biecdbaijecjlngd_xgemv.h deleted file mode 100644 index 4e8c02d777..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biecdbaijecjlngd_xgemv.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: biecdbaijecjlngd_xgemv.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_biecdbaijecjlngd_xgemv -#define SHARE_biecdbaijecjlngd_xgemv -#include "rtwtypes.h" - -extern void biecdbaijecjlngd_xgemv(int32_T m, int32_T n, const real32_T A_data[], - int32_T ia0, int32_T lda, const real32_T x_data[], int32_T ix0, real32_T - y_data[]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biecdbieglngohlf_pinv.cpp b/gnc/matlab/code_generation/sharedutils/biecdbieglngohlf_pinv.cpp deleted file mode 100644 index 5ed3d29673..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biecdbieglngohlf_pinv.cpp +++ /dev/null @@ -1,152 +0,0 @@ -// -// File: biecdbieglngohlf_pinv.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "gdjmjekfdjecpppp_svd.h" -#include "biecdbieglngohlf_pinv.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void biecdbieglngohlf_pinv(const real32_T A[16], real32_T X[16]) -{ - real32_T V[16]; - int32_T vcol; - real32_T tol; - int32_T j; - real32_T U[16]; - real32_T S[16]; - boolean_T b_p; - int32_T ar; - int32_T ia; - int32_T b_ic; - int32_T r; - b_p = true; - for (r = 0; r < 16; r++) { - X[r] = 0.0F; - if (b_p && ((!rtIsInfF(A[r])) && (!rtIsNaNF(A[r])))) { - } else { - b_p = false; - } - } - - if (b_p) { - gdjmjekfdjecpppp_svd(A, U, S, V); - } else { - for (r = 0; r < 16; r++) { - U[r] = (rtNaNF); - S[r] = 0.0F; - V[r] = (rtNaNF); - } - - S[0] = (rtNaNF); - S[5] = (rtNaNF); - S[10] = (rtNaNF); - S[15] = (rtNaNF); - } - - tol = 4.0F * S[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while (((int32_T)(vcol + 1) < 5) && (S[(int32_T)((int32_T)(vcol << 2) + vcol)] - > tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (j = 0; (int32_T)(j + 1) <= r; j++) { - tol = 1.0F / S[(int32_T)((int32_T)(j << 2) + j)]; - for (ar = vcol; (int32_T)(ar + 1) <= (int32_T)(vcol + 4); ar++) { - V[ar] *= tol; - } - - vcol += 4; - } - - for (j = 0; (int32_T)(j + 1) < 5; j++) { - X[j] = 0.0F; - } - - for (j = 4; (int32_T)(j + 1) < 9; j++) { - X[j] = 0.0F; - } - - for (j = 8; (int32_T)(j + 1) < 13; j++) { - X[j] = 0.0F; - } - - for (j = 12; (int32_T)(j + 1) < 17; j++) { - X[j] = 0.0F; - } - - ar = -1; - vcol = (int32_T)((int32_T)((int32_T)(r - 1) << 2) + 1); - for (j = 0; (int32_T)(j + 1) <= vcol; j += 4) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 0; (int32_T)(b_ic + 1) < 5; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 4; - } - - ar = -1; - vcol = (int32_T)((int32_T)((int32_T)(r - 1) << 2) + 2); - for (j = 1; (int32_T)(j + 1) <= vcol; j += 4) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 4; (int32_T)(b_ic + 1) < 9; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 4; - } - - ar = -1; - vcol = (int32_T)((int32_T)((int32_T)(r - 1) << 2) + 3); - for (j = 2; (int32_T)(j + 1) <= vcol; j += 4) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 8; (int32_T)(b_ic + 1) < 13; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 4; - } - - ar = -1; - vcol = (int32_T)((int32_T)((int32_T)(r - 1) << 2) + 4); - for (j = 3; (int32_T)(j + 1) <= vcol; j += 4) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 12; (int32_T)(b_ic + 1) < 17; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 4; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biecdbieglngohlf_pinv.h b/gnc/matlab/code_generation/sharedutils/biecdbieglngohlf_pinv.h deleted file mode 100644 index dbb4e26ad9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biecdbieglngohlf_pinv.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: biecdbieglngohlf_pinv.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_biecdbieglngohlf_pinv -#define SHARE_biecdbieglngohlf_pinv -#include "rtwtypes.h" - -extern void biecdbieglngohlf_pinv(const real32_T A[16], real32_T X[16]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bieciekfbaimdjek_norm.cpp b/gnc/matlab/code_generation/sharedutils/bieciekfbaimdjek_norm.cpp deleted file mode 100644 index a690667004..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bieciekfbaimdjek_norm.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: bieciekfbaimdjek_norm.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Thu Mar 2 13:46:15 2017 -// -#include "rtwtypes.h" -#include -#include "bieciekfbaimdjek_norm.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T bieciekfbaimdjek_norm(const real32_T x_data[], const int32_T x_sizes) -{ - real32_T y; - real32_T scale; - real32_T absxk; - real32_T t; - int32_T k; - if (x_sizes == 0) { - y = 0.0F; - } else { - y = 0.0F; - if (x_sizes == 1) { - y = (real32_T)fabs((real_T)x_data[0]); - } else { - scale = 1.17549435E-38F; - for (k = 1; k <= x_sizes; k++) { - absxk = (real32_T)fabs((real_T)x_data[k - 1]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bieciekfbaimdjek_norm.h b/gnc/matlab/code_generation/sharedutils/bieciekfbaimdjek_norm.h deleted file mode 100644 index d97662ccb7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bieciekfbaimdjek_norm.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: bieciekfbaimdjek_norm.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Thu Mar 2 13:46:15 2017 -// -#ifndef SHARE_bieciekfbaimdjek_norm -#define SHARE_bieciekfbaimdjek_norm -#include "rtwtypes.h" - -extern real32_T bieciekfbaimdjek_norm(const real32_T x_data[], const int32_T - x_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bieckfkfhlnodbie_norm.cpp b/gnc/matlab/code_generation/sharedutils/bieckfkfhlnodbie_norm.cpp deleted file mode 100644 index 04c9218674..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bieckfkfhlnodbie_norm.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// -// File: bieckfkfhlnodbie_norm.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Mon Mar 6 17:33:02 2017 -// -#include "rtwtypes.h" -#include -#include "bieckfkfhlnodbie_norm.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T bieckfkfhlnodbie_norm(const real32_T x[2]) -{ - real32_T y; - real32_T scale; - real32_T absxk; - real32_T t; - scale = 1.17549435E-38F; - absxk = (real32_T)fabs((real_T)x[0]); - if (absxk > 1.17549435E-38F) { - y = 1.0F; - scale = absxk; - } else { - t = absxk / 1.17549435E-38F; - y = t * t; - } - - absxk = (real32_T)fabs((real_T)x[1]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - - return scale * (real32_T)sqrt((real_T)y); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bieckfkfhlnodbie_norm.h b/gnc/matlab/code_generation/sharedutils/bieckfkfhlnodbie_norm.h deleted file mode 100644 index 2c56ce4c5b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bieckfkfhlnodbie_norm.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: bieckfkfhlnodbie_norm.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Mon Mar 6 17:33:02 2017 -// -#ifndef SHARE_bieckfkfhlnodbie_norm -#define SHARE_bieckfkfhlnodbie_norm -#include "rtwtypes.h" - -extern real32_T bieckfkfhlnodbie_norm(const real32_T x[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekaimoppppjmoh_cat.cpp b/gnc/matlab/code_generation/sharedutils/biekaimoppppjmoh_cat.cpp deleted file mode 100644 index bdb3b0adc0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekaimoppppjmoh_cat.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: biekaimoppppjmoh_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "biekaimoppppjmoh_cat.h" - -// Function for MATLAB Function: '/generate_output' -void biekaimoppppjmoh_cat(const int32_T varargin_1_sizes, const int32_T - varargin_2_sizes[2], uint8_T y_data[], int32_T y_sizes[2]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = (int8_T)varargin_1_sizes; - y_sizes[1] = 16; - iy = -1; - for (j = 1; j <= varargin_1_sizes; j++) { - iy++; - y_data[iy] = 1U; - } - - j = varargin_2_sizes[0] * 15; - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = 0U; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekaimoppppjmoh_cat.h b/gnc/matlab/code_generation/sharedutils/biekaimoppppjmoh_cat.h deleted file mode 100644 index 79de162e1b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekaimoppppjmoh_cat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: biekaimoppppjmoh_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_biekaimoppppjmoh_cat -#define SHARE_biekaimoppppjmoh_cat -#include "rtwtypes.h" - -extern void biekaimoppppjmoh_cat(const int32_T varargin_1_sizes, const int32_T - varargin_2_sizes[2], uint8_T y_data[], int32_T y_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekbimofknojmop_pinv.cpp b/gnc/matlab/code_generation/sharedutils/biekbimofknojmop_pinv.cpp deleted file mode 100644 index abd4775fde..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekbimofknojmop_pinv.cpp +++ /dev/null @@ -1,93 +0,0 @@ -// -// File: biekbimofknojmop_pinv.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "jmgljmgdphdjlfcj_svd.h" -#include "biekbimofknojmop_pinv.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void biekbimofknojmop_pinv(const real32_T A[4], real32_T X[4]) -{ - real32_T V[4]; - int32_T r; - int32_T vcol; - real32_T U[4]; - real32_T S[4]; - real32_T tol; - int32_T j; - int32_T ar; - int32_T ia; - int32_T b_ic; - X[0] = 0.0F; - X[1] = 0.0F; - X[2] = 0.0F; - X[3] = 0.0F; - jmgljmgdphdjlfcj_svd(A, U, S, V); - tol = 2.0F * S[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while ((vcol + 1 < 3) && (S[(vcol << 1) + vcol] > tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (j = 0; j + 1 <= r; j++) { - tol = 1.0F / S[(j << 1) + j]; - for (ar = vcol; ar + 1 <= vcol + 2; ar++) { - V[ar] *= tol; - } - - vcol += 2; - } - - for (j = 0; j + 1 < 3; j++) { - X[j] = 0.0F; - } - - for (j = 2; j + 1 < 5; j++) { - X[j] = 0.0F; - } - - ar = -1; - vcol = ((r - 1) << 1) + 1; - for (j = 0; j + 1 <= vcol; j += 2) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 0; b_ic + 1 < 3; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 2; - } - - ar = -1; - vcol = ((r - 1) << 1) + 2; - for (j = 1; j + 1 <= vcol; j += 2) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 2; b_ic + 1 < 5; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 2; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekbimofknojmop_pinv.h b/gnc/matlab/code_generation/sharedutils/biekbimofknojmop_pinv.h deleted file mode 100644 index abd2b8dc9f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekbimofknojmop_pinv.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: biekbimofknojmop_pinv.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_biekbimofknojmop_pinv -#define SHARE_biekbimofknojmop_pinv -#include "rtwtypes.h" - -extern void biekbimofknojmop_pinv(const real32_T A[4], real32_T X[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekcjmgdbaadbim_eye.cpp b/gnc/matlab/code_generation/sharedutils/biekcjmgdbaadbim_eye.cpp deleted file mode 100644 index b93e459984..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekcjmgdbaadbim_eye.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// -// File: biekcjmgdbaadbim_eye.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "biekcjmgdbaadbim_eye.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void biekcjmgdbaadbim_eye(real32_T I[36]) -{ - int32_T k; - memset(&I[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (k = 0; k < 6; k++) { - I[(int32_T)(k + (int32_T)(6 * k))] = 1.0F; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekcjmgdbaadbim_eye.h b/gnc/matlab/code_generation/sharedutils/biekcjmgdbaadbim_eye.h deleted file mode 100644 index 534c39018e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekcjmgdbaadbim_eye.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: biekcjmgdbaadbim_eye.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_biekcjmgdbaadbim_eye -#define SHARE_biekcjmgdbaadbim_eye -#include "rtwtypes.h" - -extern void biekcjmgdbaadbim_eye(real32_T I[36]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekecbihdbaecjm_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/biekecbihdbaecjm_xnrm2.cpp deleted file mode 100644 index 6e18e41edd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekecbihdbaecjm_xnrm2.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: biekecbihdbaecjm_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "biekecbihdbaecjm_xnrm2.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T biekecbihdbaecjm_xnrm2(int32_T n, const real32_T x_data[], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (!(n < 1)) { - if (n == 1) { - y = (real32_T)fabs((real_T)x_data[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x_data[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekecbihdbaecjm_xnrm2.h b/gnc/matlab/code_generation/sharedutils/biekecbihdbaecjm_xnrm2.h deleted file mode 100644 index a72af0fde3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekecbihdbaecjm_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: biekecbihdbaecjm_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_biekecbihdbaecjm_xnrm2 -#define SHARE_biekecbihdbaecjm_xnrm2 -#include "rtwtypes.h" - -extern real32_T biekecbihdbaecjm_xnrm2(int32_T n, const real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekmglfbimoknoh_eml_null_assignment.cpp b/gnc/matlab/code_generation/sharedutils/biekmglfbimoknoh_eml_null_assignment.cpp deleted file mode 100644 index edfd171a21..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekmglfbimoknoh_eml_null_assignment.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// -// File: biekmglfbimoknoh_eml_null_assignment.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "biekmglfbimoknoh_eml_null_assignment.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void biekmglfbimoknoh_eml_null_assignment(real32_T x_data[], int32_T *x_sizes, - const boolean_T idx_data[], const int32_T idx_sizes) -{ - int32_T nxin; - int32_T nxout; - int32_T k0; - int32_T k; - real32_T x_data_0[100]; - nxin = *x_sizes; - nxout = 0; - for (k0 = 1; k0 <= idx_sizes; k0++) { - nxout += idx_data[k0 - 1]; - } - - nxout = *x_sizes - nxout; - k0 = -1; - for (k = 1; k <= nxin; k++) { - if ((k > idx_sizes) || (!idx_data[k - 1])) { - k0++; - x_data[k0] = x_data[k - 1]; - } - } - - if (*x_sizes != 1) { - if (1 > nxout) { - nxout = 0; - } - - for (k0 = 0; k0 < nxout; k0++) { - x_data_0[k0] = x_data[k0]; - } - - *x_sizes = nxout; - for (k0 = 0; k0 < nxout; k0++) { - x_data[k0] = x_data_0[k0]; - } - } else if (1 > nxout) { - *x_sizes = 0; - } else { - *x_sizes = nxout; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/biekmglfbimoknoh_eml_null_assignment.h b/gnc/matlab/code_generation/sharedutils/biekmglfbimoknoh_eml_null_assignment.h deleted file mode 100644 index e0e1410993..0000000000 --- a/gnc/matlab/code_generation/sharedutils/biekmglfbimoknoh_eml_null_assignment.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: biekmglfbimoknoh_eml_null_assignment.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_biekmglfbimoknoh_eml_null_assignment -#define SHARE_biekmglfbimoknoh_eml_null_assignment -#include "rtwtypes.h" - -extern void biekmglfbimoknoh_eml_null_assignment(real32_T x_data[], int32_T - *x_sizes, const boolean_T idx_data[], const int32_T idx_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bimgkfkndbimekfk_sum.cpp b/gnc/matlab/code_generation/sharedutils/bimgkfkndbimekfk_sum.cpp deleted file mode 100644 index a254e64dce..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bimgkfkndbimekfk_sum.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// -// File: bimgkfkndbimekfk_sum.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "bimgkfkndbimekfk_sum.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void bimgkfkndbimekfk_sum(const uint8_T x[800], real_T y[50]) -{ - real_T s; - int32_T ix; - int32_T iy; - int32_T ixstart; - int32_T j; - int32_T k; - iy = -1; - ixstart = -1; - for (j = 0; j < 50; j++) { - ixstart++; - ix = ixstart; - s = x[ixstart]; - for (k = 0; k < 15; k++) { - ix += 50; - s += (real_T)x[ix]; - } - - iy++; - y[iy] = s; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bimgkfkndbimekfk_sum.h b/gnc/matlab/code_generation/sharedutils/bimgkfkndbimekfk_sum.h deleted file mode 100644 index 5a7f8eb2c7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bimgkfkndbimekfk_sum.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: bimgkfkndbimekfk_sum.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_bimgkfkndbimekfk_sum -#define SHARE_bimgkfkndbimekfk_sum -#include "rtwtypes.h" - -extern void bimgkfkndbimekfk_sum(const uint8_T x[800], real_T y[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bimgngdbbimgjecb_quat_propagate_step.cpp b/gnc/matlab/code_generation/sharedutils/bimgngdbbimgjecb_quat_propagate_step.cpp deleted file mode 100644 index 6789e97208..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bimgngdbbimgjecb_quat_propagate_step.cpp +++ /dev/null @@ -1,100 +0,0 @@ -// -// File: bimgngdbbimgjecb_quat_propagate_step.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 29 10:56:09 2017 -// -#include "rtwtypes.h" -#include -#include "bimgngdbbimgjecb_quat_propagate_step.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void bimgngdbbimgjecb_quat_propagate_step(const real32_T quat_in[4], const - real32_T omega[3], real32_T time_in, real_T quat_out[4]) -{ - real32_T omega_mag; - real32_T c; - real32_T sine_mag; - real_T x; - real_T c_y; - real32_T c_0[16]; - int32_T i; - real32_T s_idx_0; - real32_T s_idx_1; - - // omega row vector - // - // From: Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra - // Equation below is from Eq. 122, with Omega matrix and - // identity matrix multiplied together - // - // Takes the root-sum square of each row of the matrix a - // - // Jesse C. Fusco - omega_mag = (real32_T)sqrt((real_T)((omega[0] * omega[0] + omega[1] * omega[1]) - + omega[2] * omega[2])); - if ((omega_mag == 0.0F) || (time_in <= 0.0F)) { - quat_out[0] = (real_T)quat_in[0]; - quat_out[1] = (real_T)quat_in[1]; - quat_out[2] = (real_T)quat_in[2]; - quat_out[3] = (real_T)quat_in[3]; - } else { - c = (real32_T)cos((real_T)(0.5F * omega_mag * time_in)); - sine_mag = (real32_T)sin((real_T)(0.5F * omega_mag * time_in)); - s_idx_0 = sine_mag * omega[0] / omega_mag; - s_idx_1 = sine_mag * omega[1] / omega_mag; - omega_mag = sine_mag * omega[2] / omega_mag; - - // Rollup of trig equations and Omega matrix - c_0[0] = c; - c_0[1] = -omega_mag; - c_0[2] = s_idx_1; - c_0[3] = -s_idx_0; - c_0[4] = omega_mag; - c_0[5] = c; - c_0[6] = -s_idx_0; - c_0[7] = -s_idx_1; - c_0[8] = -s_idx_1; - c_0[9] = s_idx_0; - c_0[10] = c; - c_0[11] = -omega_mag; - c_0[12] = s_idx_0; - c_0[13] = s_idx_1; - c_0[14] = omega_mag; - c_0[15] = c; - for (i = 0; i < 4; i++) { - c = c_0[(int32_T)(i + 12)] * quat_in[3] + (c_0[(int32_T)(i + 8)] * - quat_in[2] + (c_0[(int32_T)(i + 4)] * quat_in[1] + c_0[i] * quat_in[0])); - quat_out[i] = (real_T)c; - } - - if (quat_out[3] < 0.0) { - x = -1.0; - } else if (quat_out[3] > 0.0) { - x = 1.0; - } else if (quat_out[3] == 0.0) { - x = 0.0; - } else { - x = quat_out[3]; - } - - // Takes the root-sum square of each row of the matrix a - // - // Jesse C. Fusco - c_y = sqrt(((quat_out[0] * quat_out[0] + quat_out[1] * quat_out[1]) + - quat_out[2] * quat_out[2]) + quat_out[3] * quat_out[3]); - quat_out[0] = x * quat_out[0] / c_y; - quat_out[1] = x * quat_out[1] / c_y; - quat_out[2] = x * quat_out[2] / c_y; - quat_out[3] = x * quat_out[3] / c_y; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bimgngdbbimgjecb_quat_propagate_step.h b/gnc/matlab/code_generation/sharedutils/bimgngdbbimgjecb_quat_propagate_step.h deleted file mode 100644 index f594f5657e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bimgngdbbimgjecb_quat_propagate_step.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: bimgngdbbimgjecb_quat_propagate_step.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 29 10:56:09 2017 -// -#ifndef SHARE_bimgngdbbimgjecb_quat_propagate_step -#define SHARE_bimgngdbbimgjecb_quat_propagate_step -#include "rtwtypes.h" - -extern void bimgngdbbimgjecb_quat_propagate_step(const real32_T quat_in[4], - const real32_T omega[3], real32_T time_in, real_T quat_out[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bimohlfkbaaidbai_xscal.cpp b/gnc/matlab/code_generation/sharedutils/bimohlfkbaaidbai_xscal.cpp deleted file mode 100644 index 92a37330e9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bimohlfkbaaidbai_xscal.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: bimohlfkbaaidbai_xscal.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "bimohlfkbaaidbai_xscal.h" - -// Function for MATLAB Function: '/MATLAB Function' -void bimohlfkbaaidbai_xscal(real32_T a, real32_T x[72], int32_T ix0) -{ - int32_T k; - for (k = ix0; k <= (int32_T)(ix0 + 11); k++) { - x[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bimohlfkbaaidbai_xscal.h b/gnc/matlab/code_generation/sharedutils/bimohlfkbaaidbai_xscal.h deleted file mode 100644 index 2a389050ca..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bimohlfkbaaidbai_xscal.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: bimohlfkbaaidbai_xscal.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_bimohlfkbaaidbai_xscal -#define SHARE_bimohlfkbaaidbai_xscal -#include "rtwtypes.h" - -extern void bimohlfkbaaidbai_xscal(real32_T a, real32_T x[72], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bimoiekfmgdbjmop_rdivide.cpp b/gnc/matlab/code_generation/sharedutils/bimoiekfmgdbjmop_rdivide.cpp deleted file mode 100644 index 3a24007a75..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bimoiekfmgdbjmop_rdivide.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: bimoiekfmgdbjmop_rdivide.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "bimoiekfmgdbjmop_rdivide.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void bimoiekfmgdbjmop_rdivide(const real32_T x[2], real32_T y, real32_T z[2]) -{ - z[0] = x[0] / y; - z[1] = x[1] / y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/bimoiekfmgdbjmop_rdivide.h b/gnc/matlab/code_generation/sharedutils/bimoiekfmgdbjmop_rdivide.h deleted file mode 100644 index 80c9e8ac5d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/bimoiekfmgdbjmop_rdivide.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: bimoiekfmgdbjmop_rdivide.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_bimoiekfmgdbjmop_rdivide -#define SHARE_bimoiekfmgdbjmop_rdivide -#include "rtwtypes.h" - -extern void bimoiekfmgdbjmop_rdivide(const real32_T x[2], real32_T y, real32_T - z[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/binsearch_u32f_prevIdx.cpp b/gnc/matlab/code_generation/sharedutils/binsearch_u32f_prevIdx.cpp deleted file mode 100644 index fc6f651f49..0000000000 --- a/gnc/matlab/code_generation/sharedutils/binsearch_u32f_prevIdx.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// -// File: binsearch_u32f_prevIdx.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "binsearch_u32f_prevIdx.h" - -uint32_T binsearch_u32f_prevIdx(real32_T u, const real32_T bp[], uint32_T - startIndex, uint32_T maxIndex) -{ - uint32_T bpIndex; - uint32_T iRght; - uint32_T iLeft; - uint32_T found; - - // Binary Search using Previous Index - bpIndex = startIndex; - iLeft = 0U; - iRght = maxIndex; - found = 0U; - while (found == 0U) { - if (u < bp[bpIndex]) { - iRght = (uint32_T)(bpIndex - 1U); - bpIndex = (uint32_T)((uint32_T)(iRght + iLeft) >> 1U); - } else if (u < bp[(uint32_T)(bpIndex + 1U)]) { - found = 1U; - } else { - iLeft = (uint32_T)(bpIndex + 1U); - bpIndex = (uint32_T)((uint32_T)(iRght + iLeft) >> 1U); - } - } - - return bpIndex; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/binsearch_u32f_prevIdx.h b/gnc/matlab/code_generation/sharedutils/binsearch_u32f_prevIdx.h deleted file mode 100644 index f946e9c93a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/binsearch_u32f_prevIdx.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: binsearch_u32f_prevIdx.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_binsearch_u32f_prevIdx -#define SHARE_binsearch_u32f_prevIdx -#include "rtwtypes.h" - -extern uint32_T binsearch_u32f_prevIdx(real32_T u, const real32_T bp[], uint32_T - startIndex, uint32_T maxIndex); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbiecbiehlngfkno_power.cpp b/gnc/matlab/code_generation/sharedutils/cbiecbiehlngfkno_power.cpp deleted file mode 100644 index 84fbfd7d01..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbiecbiehlngfkno_power.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// -// File: cbiecbiehlngfkno_power.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "cbiecbiehlngfkno_power.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void cbiecbiehlngfkno_power(const real32_T a_data[], const int32_T a_sizes, - real32_T y_data[], int32_T *y_sizes) -{ - int32_T k; - *y_sizes = (uint8_T)a_sizes; - for (k = 0; k + 1 <= a_sizes; k++) { - y_data[k] = a_data[k] * a_data[k]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbiecbiehlngfkno_power.h b/gnc/matlab/code_generation/sharedutils/cbiecbiehlngfkno_power.h deleted file mode 100644 index 0ece99e626..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbiecbiehlngfkno_power.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cbiecbiehlngfkno_power.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_cbiecbiehlngfkno_power -#define SHARE_cbiecbiehlngfkno_power -#include "rtwtypes.h" - -extern void cbiecbiehlngfkno_power(const real32_T a_data[], const int32_T - a_sizes, real32_T y_data[], int32_T *y_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbielfcbnophecbi_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/cbielfcbnophecbi_xaxpy.cpp deleted file mode 100644 index 8bfa6465e5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbielfcbnophecbi_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: cbielfcbnophecbi_xaxpy.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "cbielfcbnophecbi_xaxpy.h" - -// Function for MATLAB Function: '/MATLAB Function' -void cbielfcbnophecbi_xaxpy(int32_T n, real32_T a, const real32_T x[12], int32_T - ix0, real32_T y[36], int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * x[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbielfcbnophecbi_xaxpy.h b/gnc/matlab/code_generation/sharedutils/cbielfcbnophecbi_xaxpy.h deleted file mode 100644 index cc8ef14d2f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbielfcbnophecbi_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cbielfcbnophecbi_xaxpy.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_cbielfcbnophecbi_xaxpy -#define SHARE_cbielfcbnophecbi_xaxpy -#include "rtwtypes.h" - -extern void cbielfcbnophecbi_xaxpy(int32_T n, real32_T a, const real32_T x[12], - int32_T ix0, real32_T y[36], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbiemoppekfknopp_quatmult.cpp b/gnc/matlab/code_generation/sharedutils/cbiemoppekfknopp_quatmult.cpp deleted file mode 100644 index 7d16871fef..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbiemoppekfknopp_quatmult.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// -// File: cbiemoppekfknopp_quatmult.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Dec 4 08:33:06 2017 -// -#include "rtwtypes.h" -#include "cbiemoppekfknopp_quatmult.h" - -// Function for MATLAB Function: '/Compute Global positions of Handrail Features' -void cbiemoppekfknopp_quatmult(const real32_T p[4], const real32_T q[4], - real32_T qOut[4]) -{ - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - qOut[0] = (q[3] * p[0] + p[3] * q[0]) + (p[1] * q[2] - p[2] * q[1]); - qOut[1] = (q[3] * p[1] + p[3] * q[1]) + (p[2] * q[0] - p[0] * q[2]); - qOut[2] = (q[3] * p[2] + p[3] * q[2]) + (p[0] * q[1] - p[1] * q[0]); - qOut[3] = p[3] * q[3] - ((p[0] * q[0] + p[1] * q[1]) + p[2] * q[2]); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbiemoppekfknopp_quatmult.h b/gnc/matlab/code_generation/sharedutils/cbiemoppekfknopp_quatmult.h deleted file mode 100644 index df900cc80b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbiemoppekfknopp_quatmult.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cbiemoppekfknopp_quatmult.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Dec 4 08:33:06 2017 -// -#ifndef SHARE_cbiemoppekfknopp_quatmult -#define SHARE_cbiemoppekfknopp_quatmult -#include "rtwtypes.h" - -extern void cbiemoppekfknopp_quatmult(const real32_T p[4], const real32_T q[4], - real32_T qOut[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbiephdblnopophl_cat.cpp b/gnc/matlab/code_generation/sharedutils/cbiephdblnopophl_cat.cpp deleted file mode 100644 index 91f1b31d27..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbiephdblnopophl_cat.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// -// File: cbiephdblnopophl_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "cbiephdblnopophl_cat.h" - -// Function for MATLAB Function: '/generate_output' -void cbiephdblnopophl_cat(const real32_T varargin_2[1500], real32_T y[1600]) -{ - int32_T iy; - int32_T j; - iy = -1; - for (j = 0; j < 100; j++) { - iy++; - y[iy] = 0.0F; - } - - for (j = 0; j < 1500; j++) { - iy++; - y[iy] = varargin_2[j]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbiephdblnopophl_cat.h b/gnc/matlab/code_generation/sharedutils/cbiephdblnopophl_cat.h deleted file mode 100644 index 4cf06f1ead..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbiephdblnopophl_cat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cbiephdblnopophl_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_cbiephdblnopophl_cat -#define SHARE_cbiephdblnopophl_cat -#include "rtwtypes.h" - -extern void cbiephdblnopophl_cat(const real32_T varargin_2[1500], real32_T y - [1600]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbimbiekfcjmaaie_svd.cpp b/gnc/matlab/code_generation/sharedutils/cbimbiekfcjmaaie_svd.cpp deleted file mode 100644 index 8fcee48975..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbimbiekfcjmaaie_svd.cpp +++ /dev/null @@ -1,633 +0,0 @@ -// -// File: cbimbiekfcjmaaie_svd.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include -#include -#include "aimgimglphlffkfk_xswap.h" -#include "cjekkfkfmoppfkfc_xdotc.h" -#include "dbainohddbimecba_xnrm2.h" -#include "dbiebiecimopdbaa_xnrm2.h" -#include "glfkdbaibaaakfcb_xscal.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "ieknhdjmlngdkfcb_xaxpy.h" -#include "jecbfkfkhdbangdj_xaxpy.h" -#include "moppjecbaaaaimop_xrot.h" -#include "phlniekfphlndjmo_xaxpy.h" -#include "rt_nonfinite.h" -#include "cbimbiekfcjmaaie_svd.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void cbimbiekfcjmaaie_svd(const real32_T A[16], real32_T U[16], real32_T S[16], - real32_T V[16]) -{ - real32_T b_A[16]; - real32_T s[4]; - real32_T e[4]; - real32_T work[4]; - real32_T Vf[16]; - int32_T q; - boolean_T apply_transform; - int32_T m; - int32_T iter; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - int32_T k_ii; - real32_T varargin_1[5]; - e[0] = 0.0F; - work[0] = 0.0F; - e[1] = 0.0F; - work[1] = 0.0F; - e[2] = 0.0F; - work[2] = 0.0F; - e[3] = 0.0F; - work[3] = 0.0F; - for (iter = 0; iter < 16; iter++) { - b_A[iter] = A[iter]; - U[iter] = 0.0F; - Vf[iter] = 0.0F; - } - - iter = 0; - apply_transform = false; - snorm = dbiebiecimopdbaa_xnrm2(4, b_A, 1); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[0] < 0.0F) { - s[0] = -snorm; - } else { - s[0] = snorm; - } - - if ((real32_T)fabs((real_T)s[0]) >= 9.86076132E-32F) { - snorm = 1.0F / s[0]; - for (kase = 0; kase + 1 < 5; kase++) { - b_A[kase] *= snorm; - } - } else { - for (kase = 0; kase + 1 < 5; kase++) { - b_A[kase] /= s[0]; - } - } - - b_A[0]++; - s[0] = -s[0]; - } else { - s[0] = 0.0F; - } - - for (q = 1; q + 1 < 5; q++) { - kase = q << 2; - if (apply_transform) { - phlniekfphlndjmo_xaxpy(4, -(cjekkfkfmoppfkfc_xdotc(4, b_A, 1, b_A, kase + - 1) / b_A[0]), 1, b_A, kase + 1); - } - - e[q] = b_A[kase]; - } - - while (iter + 1 < 5) { - U[iter] = b_A[iter]; - iter++; - } - - snorm = dbainohddbimecba_xnrm2(3, e, 2); - if (snorm == 0.0F) { - e[0] = 0.0F; - } else { - if (e[1] < 0.0F) { - e[0] = -snorm; - } else { - e[0] = snorm; - } - - snorm = e[0]; - if ((real32_T)fabs((real_T)e[0]) >= 9.86076132E-32F) { - snorm = 1.0F / e[0]; - for (iter = 1; iter + 1 < 5; iter++) { - e[iter] *= snorm; - } - } else { - for (iter = 1; iter + 1 < 5; iter++) { - e[iter] /= snorm; - } - } - - e[1]++; - e[0] = -e[0]; - for (iter = 1; iter + 1 < 5; iter++) { - work[iter] = 0.0F; - } - - for (iter = 1; iter + 1 < 5; iter++) { - jecbfkfkhdbangdj_xaxpy(3, e[iter], b_A, (iter << 2) + 2, work, 2); - } - - for (iter = 1; iter + 1 < 5; iter++) { - ieknhdjmlngdkfcb_xaxpy(3, -e[iter] / e[1], work, 2, b_A, (iter << 2) + 2); - } - } - - for (iter = 1; iter + 1 < 5; iter++) { - Vf[iter] = e[iter]; - } - - apply_transform = false; - snorm = dbiebiecimopdbaa_xnrm2(3, b_A, 6); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[5] < 0.0F) { - s[1] = -snorm; - } else { - s[1] = snorm; - } - - if ((real32_T)fabs((real_T)s[1]) >= 9.86076132E-32F) { - snorm = 1.0F / s[1]; - for (kase = 5; kase + 1 < 9; kase++) { - b_A[kase] *= snorm; - } - } else { - for (kase = 5; kase + 1 < 9; kase++) { - b_A[kase] /= s[1]; - } - } - - b_A[5]++; - s[1] = -s[1]; - } else { - s[1] = 0.0F; - } - - for (q = 2; q + 1 < 5; q++) { - kase = (q << 2) + 1; - if (apply_transform) { - phlniekfphlndjmo_xaxpy(3, -(cjekkfkfmoppfkfc_xdotc(3, b_A, 6, b_A, kase + - 1) / b_A[5]), 6, b_A, kase + 1); - } - - e[q] = b_A[kase]; - } - - for (iter = 1; iter + 1 < 5; iter++) { - U[iter + 4] = b_A[iter + 4]; - } - - snorm = dbainohddbimecba_xnrm2(2, e, 3); - if (snorm == 0.0F) { - e[1] = 0.0F; - } else { - if (e[2] < 0.0F) { - e[1] = -snorm; - } else { - e[1] = snorm; - } - - snorm = e[1]; - if ((real32_T)fabs((real_T)e[1]) >= 9.86076132E-32F) { - snorm = 1.0F / e[1]; - for (iter = 2; iter + 1 < 5; iter++) { - e[iter] *= snorm; - } - } else { - for (iter = 2; iter + 1 < 5; iter++) { - e[iter] /= snorm; - } - } - - e[2]++; - e[1] = -e[1]; - for (iter = 2; iter + 1 < 5; iter++) { - work[iter] = 0.0F; - } - - for (iter = 2; iter + 1 < 5; iter++) { - jecbfkfkhdbangdj_xaxpy(2, e[iter], b_A, (iter << 2) + 3, work, 3); - } - - for (iter = 2; iter + 1 < 5; iter++) { - ieknhdjmlngdkfcb_xaxpy(2, -e[iter] / e[2], work, 3, b_A, (iter << 2) + 3); - } - } - - for (iter = 2; iter + 1 < 5; iter++) { - Vf[iter + 4] = e[iter]; - } - - apply_transform = false; - snorm = dbiebiecimopdbaa_xnrm2(2, b_A, 11); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[10] < 0.0F) { - s[2] = -snorm; - } else { - s[2] = snorm; - } - - if ((real32_T)fabs((real_T)s[2]) >= 9.86076132E-32F) { - snorm = 1.0F / s[2]; - for (kase = 10; kase + 1 < 13; kase++) { - b_A[kase] *= snorm; - } - } else { - for (kase = 10; kase + 1 < 13; kase++) { - b_A[kase] /= s[2]; - } - } - - b_A[10]++; - s[2] = -s[2]; - } else { - s[2] = 0.0F; - } - - for (q = 3; q + 1 < 5; q++) { - kase = (q << 2) + 2; - if (apply_transform) { - phlniekfphlndjmo_xaxpy(2, -(cjekkfkfmoppfkfc_xdotc(2, b_A, 11, b_A, kase + - 1) / b_A[10]), 11, b_A, kase + 1); - } - - e[q] = b_A[kase]; - } - - for (iter = 2; iter + 1 < 5; iter++) { - U[iter + 8] = b_A[iter + 8]; - } - - m = 2; - s[3] = b_A[15]; - e[2] = b_A[14]; - e[3] = 0.0F; - U[12] = 0.0F; - U[13] = 0.0F; - U[14] = 0.0F; - U[15] = 1.0F; - for (q = 2; q >= 0; q += -1) { - iter = (q << 2) + q; - if (s[q] != 0.0F) { - for (qs = q + 1; qs + 1 < 5; qs++) { - kase = ((qs << 2) + q) + 1; - phlniekfphlndjmo_xaxpy(4 - q, -(cjekkfkfmoppfkfc_xdotc(4 - q, U, iter + - 1, U, kase) / U[iter]), iter + 1, U, kase); - } - - for (kase = q; kase + 1 < 5; kase++) { - U[kase + (q << 2)] = -U[(q << 2) + kase]; - } - - U[iter]++; - for (iter = 1; iter <= q; iter++) { - U[(iter + (q << 2)) - 1] = 0.0F; - } - } else { - U[q << 2] = 0.0F; - U[1 + (q << 2)] = 0.0F; - U[2 + (q << 2)] = 0.0F; - U[3 + (q << 2)] = 0.0F; - U[iter] = 1.0F; - } - } - - for (iter = 3; iter >= 0; iter += -1) { - if ((iter + 1 <= 2) && (e[iter] != 0.0F)) { - q = ((iter << 2) + iter) + 2; - for (kase = iter + 1; kase + 1 < 5; kase++) { - qs = ((kase << 2) + iter) + 2; - phlniekfphlndjmo_xaxpy(3 - iter, -(cjekkfkfmoppfkfc_xdotc(3 - iter, Vf, - q, Vf, qs) / Vf[q - 1]), q, Vf, qs); - } - } - - Vf[iter << 2] = 0.0F; - Vf[1 + (iter << 2)] = 0.0F; - Vf[2 + (iter << 2)] = 0.0F; - Vf[3 + (iter << 2)] = 0.0F; - Vf[iter + (iter << 2)] = 1.0F; - } - - ztest = e[0]; - if (s[0] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[0]); - snorm = s[0] / ztest0; - s[0] = ztest0; - ztest = e[0] / snorm; - glfkdbaibaaakfcb_xscal(snorm, U, 1); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[1] *= snorm; - glfkdbaibaaakfcb_xscal(snorm, Vf, 5); - } - - e[0] = ztest; - ztest = e[1]; - if (s[1] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[1]); - snorm = s[1] / ztest0; - s[1] = ztest0; - ztest = e[1] / snorm; - glfkdbaibaaakfcb_xscal(snorm, U, 5); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[2] *= snorm; - glfkdbaibaaakfcb_xscal(snorm, Vf, 9); - } - - e[1] = ztest; - ztest = b_A[14]; - if (s[2] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[2]); - snorm = s[2] / ztest0; - s[2] = ztest0; - ztest = b_A[14] / snorm; - glfkdbaibaaakfcb_xscal(snorm, U, 9); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[3] = b_A[15] * snorm; - glfkdbaibaaakfcb_xscal(snorm, Vf, 13); - } - - e[2] = ztest; - if (s[3] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[3]); - snorm = s[3] / ztest0; - s[3] = ztest0; - glfkdbaibaaakfcb_xscal(snorm, U, 13); - } - - e[3] = 0.0F; - iter = 0; - snorm = 0.0F; - if ((s[0] >= e[0]) || rtIsNaNF(e[0])) { - ztest0 = s[0]; - } else { - ztest0 = e[0]; - } - - if (!((0.0F >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - if ((s[1] >= e[1]) || rtIsNaNF(e[1])) { - ztest0 = s[1]; - } else { - ztest0 = e[1]; - } - - if (!((snorm >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - if ((s[2] >= ztest) || rtIsNaNF(ztest)) { - ztest = s[2]; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - - if (s[3] >= 0.0F) { - ztest0 = s[3]; - } else { - ztest0 = 0.0F; - } - - if (!((snorm >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - while ((m + 2 > 0) && (!(iter >= 75))) { - kase = m + 1; - do { - qs = 0; - q = kase; - if (kase == 0) { - qs = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e[kase - 1]); - if ((ztest0 <= ((real32_T)fabs((real_T)s[kase - 1]) + (real32_T)fabs - ((real_T)s[kase])) * 1.1920929E-7F) || (ztest0 <= - 9.86076132E-32F) || ((iter > 20) && (ztest0 <= 1.1920929E-7F * - snorm))) { - e[kase - 1] = 0.0F; - qs = 1; - } else { - kase--; - } - } - } while (qs == 0); - - if (m + 1 == kase) { - kase = 4; - } else { - qs = m + 2; - k_ii = m + 2; - apply_transform = false; - while ((!apply_transform) && (k_ii >= kase)) { - qs = k_ii; - if (k_ii == kase) { - apply_transform = true; - } else { - ztest0 = 0.0F; - if (k_ii < m + 2) { - ztest0 = (real32_T)fabs((real_T)e[k_ii - 1]); - } - - if (k_ii > kase + 1) { - ztest0 += (real32_T)fabs((real_T)e[k_ii - 2]); - } - - ztest = (real32_T)fabs((real_T)s[k_ii - 1]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s[k_ii - 1] = 0.0F; - apply_transform = true; - } else { - k_ii--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if (m + 2 == qs) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[m]; - e[m] = 0.0F; - for (kase = m; kase + 1 >= q + 1; kase--) { - ztest = s[kase]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[kase] = ztest; - if (kase + 1 > q + 1) { - ztest0 = e[kase - 1] * -smm1; - e[kase - 1] *= sqds; - } - - moppjecbaaaaimop_xrot(Vf, 1 + (kase << 2), 1 + ((m + 1) << 2), sqds, - smm1); - } - break; - - case 2: - ztest0 = e[q - 1]; - e[q - 1] = 0.0F; - for (kase = q; kase + 1 <= m + 2; kase++) { - ztest = s[kase]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[kase] = ztest; - ztest0 = -smm1 * e[kase]; - e[kase] *= sqds; - moppjecbaaaaimop_xrot(U, 1 + (kase << 2), 1 + ((q - 1) << 2), sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s[m + 1]); - varargin_1[1] = (real32_T)fabs((real_T)s[m]); - varargin_1[2] = (real32_T)fabs((real_T)e[m]); - varargin_1[3] = (real32_T)fabs((real_T)s[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - kase = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - qs = 2; - apply_transform = false; - while ((!apply_transform) && (qs < 6)) { - kase = qs; - if (!rtIsNaNF(varargin_1[qs - 1])) { - ztest = varargin_1[qs - 1]; - apply_transform = true; - } else { - qs++; - } - } - } - - if (kase < 5) { - while (kase + 1 < 6) { - if (varargin_1[kase] > ztest) { - ztest = varargin_1[kase]; - } - - kase++; - } - } - - ztest0 = s[m + 1] / ztest; - smm1 = s[m] / ztest; - emm1 = e[m] / ztest; - sqds = s[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - for (kase = q + 1; kase <= m + 1; kase++) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - if (kase > q + 1) { - e[kase - 2] = ztest0; - } - - ztest0 = s[kase - 1] * sqds + e[kase - 1] * smm1; - e[kase - 1] = e[kase - 1] * sqds - s[kase - 1] * smm1; - ztest = smm1 * s[kase]; - s[kase] *= sqds; - moppjecbaaaaimop_xrot(Vf, 1 + ((kase - 1) << 2), 1 + (kase << 2), sqds, - smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s[kase - 1] = ztest0; - ztest0 = e[kase - 1] * sqds + smm1 * s[kase]; - s[kase] = e[kase - 1] * -smm1 + sqds * s[kase]; - ztest = smm1 * e[kase]; - e[kase] *= sqds; - moppjecbaaaaimop_xrot(U, 1 + ((kase - 1) << 2), 1 + (kase << 2), sqds, - smm1); - } - - e[m] = ztest0; - iter++; - break; - - default: - if (s[q] < 0.0F) { - s[q] = -s[q]; - glfkdbaibaaakfcb_xscal(-1.0F, Vf, 1 + (q << 2)); - } - - iter = q + 1; - while ((q + 1 < 4) && (s[q] < s[iter])) { - ztest0 = s[q]; - s[q] = s[iter]; - s[iter] = ztest0; - aimgimglphlffkfk_xswap(Vf, 1 + (q << 2), 1 + ((q + 1) << 2)); - aimgimglphlffkfk_xswap(U, 1 + (q << 2), 1 + ((q + 1) << 2)); - q = iter; - iter++; - } - - iter = 0; - m--; - break; - } - } - - memcpy(&V[0], &Vf[0], sizeof(real32_T) << 4U); - for (m = 0; m < 4; m++) { - e[m] = s[m]; - } - - memset(&S[0], 0, sizeof(real32_T) << 4U); - S[0] = e[0]; - S[5] = e[1]; - S[10] = e[2]; - S[15] = e[3]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbimbiekfcjmaaie_svd.h b/gnc/matlab/code_generation/sharedutils/cbimbiekfcjmaaie_svd.h deleted file mode 100644 index dac90c0be0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbimbiekfcjmaaie_svd.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cbimbiekfcjmaaie_svd.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_cbimbiekfcjmaaie_svd -#define SHARE_cbimbiekfcjmaaie_svd -#include "rtwtypes.h" - -extern void cbimbiekfcjmaaie_svd(const real32_T A[16], real32_T U[16], real32_T - S[16], real32_T V[16]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbimeknolnglpphd_xswap.cpp b/gnc/matlab/code_generation/sharedutils/cbimeknolnglpphd_xswap.cpp deleted file mode 100644 index e2c7af5da9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbimeknolnglpphd_xswap.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// -// File: cbimeknolnglpphd_xswap.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "cbimeknolnglpphd_xswap.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void cbimeknolnglpphd_xswap(real32_T x[4]) -{ - real32_T temp; - temp = x[0]; - x[0] = x[2]; - x[2] = temp; - temp = x[1]; - x[1] = x[3]; - x[3] = temp; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbimeknolnglpphd_xswap.h b/gnc/matlab/code_generation/sharedutils/cbimeknolnglpphd_xswap.h deleted file mode 100644 index 569d5fe70a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbimeknolnglpphd_xswap.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: cbimeknolnglpphd_xswap.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_cbimeknolnglpphd_xswap -#define SHARE_cbimeknolnglpphd_xswap -#include "rtwtypes.h" - -extern void cbimeknolnglpphd_xswap(real32_T x[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbimpphlphlnekfc_xswap.cpp b/gnc/matlab/code_generation/sharedutils/cbimpphlphlnekfc_xswap.cpp deleted file mode 100644 index 3386ad2474..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbimpphlphlnekfc_xswap.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: cbimpphlphlnekfc_xswap.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "cbimpphlphlnekfc_xswap.h" - -// Function for MATLAB Function: '/MATLAB Function' -void cbimpphlphlnekfc_xswap(real32_T x[36], int32_T ix0, int32_T iy0) -{ - int32_T ix; - int32_T iy; - real32_T temp; - int32_T k; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k < 6; k++) { - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; - ix++; - iy++; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cbimpphlphlnekfc_xswap.h b/gnc/matlab/code_generation/sharedutils/cbimpphlphlnekfc_xswap.h deleted file mode 100644 index e16b686eca..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cbimpphlphlnekfc_xswap.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: cbimpphlphlnekfc_xswap.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_cbimpphlphlnekfc_xswap -#define SHARE_cbimpphlphlnekfc_xswap -#include "rtwtypes.h" - -extern void cbimpphlphlnekfc_xswap(real32_T x[36], int32_T ix0, int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/checksummap.mat b/gnc/matlab/code_generation/sharedutils/checksummap.mat deleted file mode 100644 index bb9a1528c1..0000000000 Binary files a/gnc/matlab/code_generation/sharedutils/checksummap.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/sharedutils/cjecbaaifkfkkngd_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/cjecbaaifkfkkngd_xaxpy.cpp deleted file mode 100644 index 87cd63bd32..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjecbaaifkfkkngd_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: cjecbaaifkfkkngd_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "cjecbaaifkfkkngd_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void cjecbaaifkfkkngd_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T y_data[], - int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!(a == 0.0F)) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y_data[iy] += a * y_data[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjecbaaifkfkkngd_xaxpy.h b/gnc/matlab/code_generation/sharedutils/cjecbaaifkfkkngd_xaxpy.h deleted file mode 100644 index a080a8ab62..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjecbaaifkfkkngd_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cjecbaaifkfkkngd_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_cjecbaaifkfkkngd_xaxpy -#define SHARE_cjecbaaifkfkkngd_xaxpy -#include "rtwtypes.h" - -extern void cjecbaaifkfkkngd_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T - y_data[], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjecjecjbiekdjmo_rdivide.cpp b/gnc/matlab/code_generation/sharedutils/cjecjecjbiekdjmo_rdivide.cpp deleted file mode 100644 index f30feb2427..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjecjecjbiekdjmo_rdivide.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// -// File: cjecjecjbiekdjmo_rdivide.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "cjecjecjbiekdjmo_rdivide.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void cjecjecjbiekdjmo_rdivide(const real32_T x_data[], const int32_T x_sizes[2], - const real32_T y_data[], real32_T z_data[], int32_T z_sizes[2]) -{ - int32_T i; - int32_T loop_ub; - z_sizes[0] = 2; - z_sizes[1] = x_sizes[1]; - loop_ub = x_sizes[0] * x_sizes[1]; - for (i = 0; i < loop_ub; i++) { - z_data[i] = x_data[i] / y_data[i]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjecjecjbiekdjmo_rdivide.h b/gnc/matlab/code_generation/sharedutils/cjecjecjbiekdjmo_rdivide.h deleted file mode 100644 index 5a1906f0e3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjecjecjbiekdjmo_rdivide.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cjecjecjbiekdjmo_rdivide.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_cjecjecjbiekdjmo_rdivide -#define SHARE_cjecjecjbiekdjmo_rdivide -#include "rtwtypes.h" - -extern void cjecjecjbiekdjmo_rdivide(const real32_T x_data[], const int32_T - x_sizes[2], const real32_T y_data[], real32_T z_data[], int32_T z_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjekaaieekfkbimo_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/cjekaaieekfkbimo_xnrm2.cpp deleted file mode 100644 index 617104e9a2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjekaaieekfkbimo_xnrm2.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: cjekaaieekfkbimo_xnrm2.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include -#include "cjekaaieekfkbimo_xnrm2.h" - -// Function for MATLAB Function: '/MATLAB Function' -real32_T cjekaaieekfkbimo_xnrm2(int32_T n, const real32_T x[36], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (!(n < 1)) { - if (n == 1) { - y = (real32_T)fabs((real_T)x[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjekaaieekfkbimo_xnrm2.h b/gnc/matlab/code_generation/sharedutils/cjekaaieekfkbimo_xnrm2.h deleted file mode 100644 index 9ff6bd3dfe..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjekaaieekfkbimo_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cjekaaieekfkbimo_xnrm2.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_cjekaaieekfkbimo_xnrm2 -#define SHARE_cjekaaieekfkbimo_xnrm2 -#include "rtwtypes.h" - -extern real32_T cjekaaieekfkbimo_xnrm2(int32_T n, const real32_T x[36], int32_T - ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjekkfkfmoppfkfc_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/cjekkfkfmoppfkfc_xdotc.cpp deleted file mode 100644 index 18f3143e72..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjekkfkfmoppfkfc_xdotc.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: cjekkfkfmoppfkfc_xdotc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "cjekkfkfmoppfkfc_xdotc.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T cjekkfkfmoppfkfc_xdotc(int32_T n, const real32_T x[16], int32_T ix0, - const real32_T y[16], int32_T iy0) -{ - real32_T d; - int32_T ix; - int32_T iy; - int32_T k; - d = 0.0F; - if (!(n < 1)) { - ix = ix0; - iy = iy0; - for (k = 1; k <= n; k++) { - d += x[(int32_T)(ix - 1)] * y[(int32_T)(iy - 1)]; - ix++; - iy++; - } - } - - return d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjekkfkfmoppfkfc_xdotc.h b/gnc/matlab/code_generation/sharedutils/cjekkfkfmoppfkfc_xdotc.h deleted file mode 100644 index f04ab6f0e9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjekkfkfmoppfkfc_xdotc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cjekkfkfmoppfkfc_xdotc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_cjekkfkfmoppfkfc_xdotc -#define SHARE_cjekkfkfmoppfkfc_xdotc -#include "rtwtypes.h" - -extern real32_T cjekkfkfmoppfkfc_xdotc(int32_T n, const real32_T x[16], int32_T - ix0, const real32_T y[16], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjekmgdbimohophl_pinv.cpp b/gnc/matlab/code_generation/sharedutils/cjekmgdbimohophl_pinv.cpp deleted file mode 100644 index fd2a42d820..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjekmgdbimohophl_pinv.cpp +++ /dev/null @@ -1,213 +0,0 @@ -// -// File: cjekmgdbimohophl_pinv.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "djecdbimhdjmdjmg_svd.h" -#include "jecjjmglfkfcaimo_svd.h" -#include "cjekmgdbimohophl_pinv.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void cjekmgdbimohophl_pinv(const real32_T A_data[], const int32_T A_sizes[2], - real32_T X_data[], int32_T X_sizes[2]) -{ - int32_T m; - int32_T r; - int32_T vcol; - real32_T tol; - int32_T br; - int32_T ar; - int32_T ia; - int32_T c; - int32_T ib; - int32_T b_ic; - int32_T d_ic; - int32_T loop_ub; - real32_T V_data[9]; - int32_T V_sizes[2]; - real32_T U_data[96]; - int32_T U_sizes[2]; - real32_T S_data[9]; - int32_T S_sizes[2]; - real32_T x_data[9]; - real32_T b_X_data[6]; - real32_T b_V_data[4]; - int32_T b_V_sizes[2]; - real32_T b_U_data[6]; - real32_T b_S_data[4]; - real32_T b_x_data[96]; - int32_T b_X_sizes_idx_0; - int8_T b_idx_1; - if (A_sizes[0] < 3) { - loop_ub = A_sizes[0]; - for (ar = 0; ar < loop_ub; ar++) { - U_data[3 * ar] = A_data[ar]; - U_data[1 + 3 * ar] = A_data[ar + A_sizes[0]]; - U_data[2 + 3 * ar] = A_data[(A_sizes[0] << 1) + ar]; - } - - m = A_sizes[0]; - b_idx_1 = (int8_T)A_sizes[0]; - b_X_sizes_idx_0 = b_idx_1; - loop_ub = b_idx_1 * 3; - for (ar = 0; ar < loop_ub; ar++) { - b_X_data[ar] = 0.0F; - } - - if (A_sizes[0] != 0) { - U_sizes[0] = 3; - U_sizes[1] = A_sizes[0]; - jecjjmglfkfcaimo_svd(U_data, U_sizes, b_U_data, S_sizes, b_S_data, V_sizes, - b_V_data, b_V_sizes); - tol = 3.0F * b_S_data[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while ((vcol + 1 <= m) && (b_S_data[V_sizes[0] * vcol + vcol] > tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (br = 0; br + 1 <= r; br++) { - tol = 1.0F / b_S_data[V_sizes[0] * br + br]; - loop_ub = b_V_sizes[0] * b_V_sizes[1]; - for (ar = 0; ar < loop_ub; ar++) { - b_x_data[ar] = b_V_data[ar]; - } - - loop_ub = vcol + m; - for (ar = vcol; ar + 1 <= loop_ub; ar++) { - b_x_data[ar] *= tol; - } - - loop_ub = b_V_sizes[0] * b_V_sizes[1]; - for (ar = 0; ar < loop_ub; ar++) { - b_V_data[ar] = b_x_data[ar]; - } - - vcol += m; - } - - vcol = A_sizes[0] << 1; - br = 0; - while ((m > 0) && (br <= vcol)) { - loop_ub = br + m; - for (ar = br; ar + 1 <= loop_ub; ar++) { - b_X_data[ar] = 0.0F; - } - - br += m; - } - - br = -1; - loop_ub = 0; - while ((m > 0) && (loop_ub <= vcol)) { - ar = 0; - br++; - c = ((r - 1) * 3 + br) + 1; - for (ib = br; ib + 1 <= c; ib += 3) { - if (b_U_data[ib] != 0.0F) { - ia = ar; - b_ic = loop_ub + m; - for (d_ic = loop_ub; d_ic + 1 <= b_ic; d_ic++) { - ia++; - b_X_data[d_ic] += b_V_data[ia - 1] * b_U_data[ib]; - } - } - - ar += m; - } - - loop_ub += m; - } - } - } - - X_sizes[0] = 3; - X_sizes[1] = b_idx_1; - for (ar = 0; ar < b_X_sizes_idx_0; ar++) { - X_data[3 * ar] = b_X_data[ar]; - X_data[1 + 3 * ar] = b_X_data[ar + b_idx_1]; - X_data[2 + 3 * ar] = b_X_data[(b_idx_1 << 1) + ar]; - } - } else { - m = A_sizes[0]; - b_idx_1 = (int8_T)A_sizes[0]; - X_sizes[0] = 3; - X_sizes[1] = b_idx_1; - loop_ub = 3 * b_idx_1; - for (ar = 0; ar < loop_ub; ar++) { - X_data[ar] = 0.0F; - } - - djecdbimhdjmdjmg_svd(A_data, A_sizes, U_data, U_sizes, S_data, S_sizes, - V_data, V_sizes); - tol = (real32_T)A_sizes[0] * S_data[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while ((vcol + 1 < 4) && (S_data[S_sizes[0] * vcol + vcol] > tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (br = 0; br + 1 <= r; br++) { - tol = 1.0F / S_data[S_sizes[0] * br + br]; - loop_ub = V_sizes[0] * V_sizes[1]; - for (ar = 0; ar < loop_ub; ar++) { - x_data[ar] = V_data[ar]; - } - - for (loop_ub = vcol; loop_ub + 1 <= vcol + 3; loop_ub++) { - x_data[loop_ub] *= tol; - } - - V_sizes[0] = 3; - V_sizes[1] = 3; - for (ar = 0; ar < 9; ar++) { - V_data[ar] = x_data[ar]; - } - - vcol += 3; - } - - vcol = (A_sizes[0] - 1) * 3; - for (br = 0; br <= vcol; br += 3) { - for (loop_ub = br + 1; loop_ub <= br + 3; loop_ub++) { - X_data[loop_ub - 1] = 0.0F; - } - } - - br = -1; - for (loop_ub = 0; loop_ub <= vcol; loop_ub += 3) { - ar = 0; - br++; - c = ((r - 1) * m + br) + 1; - for (ib = br; ib + 1 <= c; ib += m) { - if (U_data[ib] != 0.0F) { - ia = ar; - for (b_ic = loop_ub; b_ic + 1 <= loop_ub + 3; b_ic++) { - ia++; - X_data[b_ic] += V_data[ia - 1] * U_data[ib]; - } - } - - ar += 3; - } - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjekmgdbimohophl_pinv.h b/gnc/matlab/code_generation/sharedutils/cjekmgdbimohophl_pinv.h deleted file mode 100644 index 9c06e77852..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjekmgdbimohophl_pinv.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: cjekmgdbimohophl_pinv.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_cjekmgdbimohophl_pinv -#define SHARE_cjekmgdbimohophl_pinv -#include "rtwtypes.h" - -extern void cjekmgdbimohophl_pinv(const real32_T A_data[], const int32_T - A_sizes[2], real32_T X_data[], int32_T X_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjmggdbimohlknop_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/cjmggdbimohlknop_xnrm2.cpp deleted file mode 100644 index db3d02e5e8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjmggdbimohlknop_xnrm2.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// -// File: cjmggdbimohlknop_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "cjmggdbimohlknop_xnrm2.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T cjmggdbimohlknop_xnrm2(const real32_T x[3], int32_T ix0) -{ - real32_T y; - real32_T scale; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - scale = 1.17549435E-38F; - for (k = ix0; k <= (int32_T)(ix0 + 1); k++) { - absxk = (real32_T)fabs((real_T)x[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt((real_T)y); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjmggdbimohlknop_xnrm2.h b/gnc/matlab/code_generation/sharedutils/cjmggdbimohlknop_xnrm2.h deleted file mode 100644 index 545e81a3f9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjmggdbimohlknop_xnrm2.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: cjmggdbimohlknop_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_cjmggdbimohlknop_xnrm2 -#define SHARE_cjmggdbimohlknop_xnrm2 -#include "rtwtypes.h" - -extern real32_T cjmggdbimohlknop_xnrm2(const real32_T x[3], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjmgkfkfphdjaaai_xscal.cpp b/gnc/matlab/code_generation/sharedutils/cjmgkfkfphdjaaai_xscal.cpp deleted file mode 100644 index 3c3b51fa92..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjmgkfkfphdjaaai_xscal.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: cjmgkfkfphdjaaai_xscal.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "cjmgkfkfphdjaaai_xscal.h" - -// Function for MATLAB Function: '/MATLAB Function' -void cjmgkfkfphdjaaai_xscal(real32_T a, real32_T x[36], int32_T ix0) -{ - int32_T k; - for (k = ix0; k <= (int32_T)(ix0 + 11); k++) { - x[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjmgkfkfphdjaaai_xscal.h b/gnc/matlab/code_generation/sharedutils/cjmgkfkfphdjaaai_xscal.h deleted file mode 100644 index 80c736806e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjmgkfkfphdjaaai_xscal.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: cjmgkfkfphdjaaai_xscal.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_cjmgkfkfphdjaaai_xscal -#define SHARE_cjmgkfkfphdjaaai_xscal -#include "rtwtypes.h" - -extern void cjmgkfkfphdjaaai_xscal(real32_T a, real32_T x[36], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjmolnohmglfbiec_svd.cpp b/gnc/matlab/code_generation/sharedutils/cjmolnohmglfbiec_svd.cpp deleted file mode 100644 index 79cac4b81d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjmolnohmglfbiec_svd.cpp +++ /dev/null @@ -1,566 +0,0 @@ -// -// File: cjmolnohmglfbiec_svd.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "baaicbiepphdbiek_xaxpy.h" -#include "cjmggdbimohlknop_xnrm2.h" -#include "djmgbimooppphlno_xswap.h" -#include "ekfklfkfohdjaimg_xaxpy.h" -#include "fcjebiekeknggdba_xdotc.h" -#include "gdjmmgdjekfccjmg_xrot.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "kfkfopppmophfcjm_xaxpy.h" -#include "mglnohlnekfcekfk_xdotc.h" -#include "nohdgdbadbiejmop_xnrm2.h" -#include "ohdbaiekngdjbaaa_xdotc.h" -#include "ohdjhlfcnohdhdba_xscal.h" -#include "ophdjekfieknglfk_xswap.h" -#include "ophlaaaiophdbaai_xscal.h" -#include "opppnohdmglfjekf_xaxpy.h" -#include "pphdglfkecjmjekf_xrot.h" -#include "ppppjmohbaaigdje_xaxpy.h" -#include "rt_nonfinite.h" -#include "cjmolnohmglfbiec_svd.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void cjmolnohmglfbiec_svd(const real32_T A_data[], const int32_T A_sizes[2], - real32_T U_data[], int32_T U_sizes[2], real32_T S_data[], int32_T S_sizes[2], - real32_T V_data[], int32_T V_sizes[2]) -{ - int32_T n; - real32_T e[3]; - real32_T Vf[9]; - int32_T nct; - int32_T q; - boolean_T apply_transform; - int32_T qjj; - int32_T m; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - real32_T varargin_1[5]; - int32_T g_k; - real32_T b_A_data[96]; - int32_T b_A_sizes[2]; - real32_T s_data[3]; - real32_T work_data[32]; - int32_T work_sizes; - int8_T d_idx_0; - boolean_T exitg1; - boolean_T exitg2; - int32_T exitg3; - b_A_sizes[0] = A_sizes[0]; - b_A_sizes[1] = 3; - nct = (int32_T)(A_sizes[0] * A_sizes[1]); - for (m = 0; m <= (int32_T)(nct - 1); m++) { - b_A_data[m] = A_data[m]; - } - - n = A_sizes[0]; - s_data[0] = 0.0F; - e[0] = 0.0F; - s_data[1] = 0.0F; - e[1] = 0.0F; - s_data[2] = 0.0F; - e[2] = 0.0F; - d_idx_0 = (int8_T)A_sizes[0]; - work_sizes = (int32_T)d_idx_0; - nct = (int32_T)d_idx_0; - for (m = 0; m <= (int32_T)(nct - 1); m++) { - work_data[m] = 0.0F; - } - - d_idx_0 = (int8_T)A_sizes[0]; - U_sizes[0] = (int32_T)d_idx_0; - U_sizes[1] = 3; - nct = (int32_T)((int32_T)d_idx_0 * 3); - for (m = 0; m <= (int32_T)(nct - 1); m++) { - U_data[m] = 0.0F; - } - - for (m = 0; m < 9; m++) { - Vf[m] = 0.0F; - } - - if (A_sizes[0] > 1) { - nct = (int32_T)(A_sizes[0] - 1); - } else { - nct = 0; - } - - if (!(nct <= 3)) { - nct = 3; - } - - if (nct >= 1) { - m = nct; - } else { - m = 1; - } - - for (qs = 0; (int32_T)(qs + 1) <= m; qs++) { - kase = (int32_T)((int32_T)(n * qs) + qs); - q = (int32_T)(n - qs); - apply_transform = false; - if ((int32_T)(qs + 1) <= nct) { - snorm = nohdgdbadbiejmop_xnrm2(q, b_A_data, (int32_T)(kase + 1)); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A_data[kase] < 0.0F) { - s_data[qs] = -snorm; - } else { - s_data[qs] = snorm; - } - - if ((real32_T)fabs((real_T)s_data[qs]) >= 9.86076132E-32F) { - snorm = 1.0F / s_data[qs]; - qjj = (int32_T)(kase + q); - for (g_k = kase; (int32_T)(g_k + 1) <= qjj; g_k++) { - b_A_data[g_k] *= snorm; - } - } else { - qjj = (int32_T)(kase + q); - for (g_k = kase; (int32_T)(g_k + 1) <= qjj; g_k++) { - b_A_data[g_k] /= s_data[qs]; - } - } - - b_A_data[kase]++; - s_data[qs] = -s_data[qs]; - } else { - s_data[qs] = 0.0F; - } - } - - for (g_k = (int32_T)(qs + 1); (int32_T)(g_k + 1) < 4; g_k++) { - qjj = (int32_T)((int32_T)(n * g_k) + qs); - if (apply_transform) { - ppppjmohbaaigdje_xaxpy(q, -(ohdbaiekngdjbaaa_xdotc(q, b_A_data, (int32_T) - (kase + 1), b_A_data, (int32_T)(qjj + 1)) / b_A_data[(int32_T)(qs + - (int32_T)(b_A_sizes[0] * qs))]), (int32_T)(kase + 1), b_A_data, - (int32_T)(qjj + 1)); - } - - e[g_k] = b_A_data[qjj]; - } - - if ((int32_T)(qs + 1) <= nct) { - for (kase = qs; (int32_T)(kase + 1) <= n; kase++) { - U_data[(int32_T)(kase + (int32_T)((int32_T)d_idx_0 * qs))] = b_A_data - [(int32_T)((int32_T)(b_A_sizes[0] * qs) + kase)]; - } - } - - if ((int32_T)(qs + 1) <= 1) { - snorm = cjmggdbimohlknop_xnrm2(e, 2); - if (snorm == 0.0F) { - e[0] = 0.0F; - } else { - if (e[1] < 0.0F) { - snorm = -snorm; - } - - e[0] = snorm; - if ((real32_T)fabs((real_T)snorm) >= 9.86076132E-32F) { - snorm = 1.0F / snorm; - for (kase = 1; (int32_T)(kase + 1) < 4; kase++) { - e[kase] *= snorm; - } - } else { - for (kase = 1; (int32_T)(kase + 1) < 4; kase++) { - e[kase] /= snorm; - } - } - - e[1]++; - e[0] = -e[0]; - for (kase = 2; kase <= n; kase++) { - work_data[(int32_T)(kase - 1)] = 0.0F; - } - - for (kase = 1; (int32_T)(kase + 1) < 4; kase++) { - baaicbiepphdbiek_xaxpy((int32_T)(q - 1), e[kase], b_A_data, (int32_T) - ((int32_T)(n * kase) + 2), work_data, 2); - } - - for (kase = 1; (int32_T)(kase + 1) < 4; kase++) { - kfkfopppmophfcjm_xaxpy((int32_T)(q - 1), -e[kase] / e[1], work_data, 2, - b_A_data, (int32_T)((int32_T)(n * kase) + 2)); - } - } - - for (q = 1; (int32_T)(q + 1) < 4; q++) { - Vf[q] = e[q]; - } - } - } - - m = 1; - if (nct < 3) { - s_data[nct] = b_A_data[(int32_T)((int32_T)(b_A_sizes[0] * nct) + nct)]; - } - - if ((int32_T)(nct + 1) <= 3) { - for (q = nct; (int32_T)(q + 1) < 4; q++) { - for (qs = 1; qs <= n; qs++) { - U_data[(int32_T)((int32_T)(qs + (int32_T)((int32_T)d_idx_0 * q)) - 1)] = - 0.0F; - } - - U_data[(int32_T)(q + (int32_T)((int32_T)d_idx_0 * q))] = 1.0F; - } - } - - for (nct--; (int32_T)(nct + 1) > 0; nct--) { - q = (int32_T)(n - nct); - kase = (int32_T)((int32_T)(n * nct) + nct); - if (s_data[nct] != 0.0F) { - for (qs = (int32_T)(nct + 1); (int32_T)(qs + 1) < 4; qs++) { - qjj = (int32_T)((int32_T)((int32_T)(n * qs) + nct) + 1); - opppnohdmglfjekf_xaxpy(q, -(fcjebiekeknggdba_xdotc(q, U_data, (int32_T) - (kase + 1), U_data, qjj) / U_data[kase]), (int32_T)(kase + 1), U_data, - qjj); - } - - for (q = nct; (int32_T)(q + 1) <= n; q++) { - U_data[(int32_T)(q + (int32_T)(U_sizes[0] * nct))] = -U_data[(int32_T) - ((int32_T)(U_sizes[0] * nct) + q)]; - } - - U_data[kase]++; - for (q = 1; q <= nct; q++) { - U_data[(int32_T)((int32_T)(q + (int32_T)(U_sizes[0] * nct)) - 1)] = 0.0F; - } - } else { - for (q = 1; q <= n; q++) { - U_data[(int32_T)((int32_T)(q + (int32_T)(U_sizes[0] * nct)) - 1)] = 0.0F; - } - - U_data[kase] = 1.0F; - } - } - - for (nct = 2; nct >= 0; nct += -1) { - if (((int32_T)(nct + 1) <= 1) && (e[0] != 0.0F)) { - ekfklfkfohdjaimg_xaxpy(2, -(mglnohlnekfcekfk_xdotc(2, Vf, 2, Vf, 5) / Vf[1]), - 2, Vf, 5); - ekfklfkfohdjaimg_xaxpy(2, -(mglnohlnekfcekfk_xdotc(2, Vf, 2, Vf, 8) / Vf[1]), - 2, Vf, 8); - } - - Vf[(int32_T)(3 * nct)] = 0.0F; - Vf[(int32_T)(1 + (int32_T)(3 * nct))] = 0.0F; - Vf[(int32_T)(2 + (int32_T)(3 * nct))] = 0.0F; - Vf[(int32_T)(nct + (int32_T)(3 * nct))] = 1.0F; - } - - ztest = e[0]; - if (s_data[0] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s_data[0]); - snorm = s_data[0] / ztest0; - s_data[0] = ztest0; - ztest = e[0] / snorm; - ohdjhlfcnohdhdba_xscal(A_sizes[0], snorm, U_data, 1); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s_data[1] *= snorm; - ophlaaaiophdbaai_xscal(snorm, Vf, 4); - } - - e[0] = ztest; - ztest = b_A_data[(int32_T)((int32_T)(b_A_sizes[0] << 1) + 1)]; - if (s_data[1] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s_data[1]); - snorm = s_data[1] / ztest0; - s_data[1] = ztest0; - ztest = b_A_data[(int32_T)((int32_T)(b_A_sizes[0] << 1) + 1)] / snorm; - ohdjhlfcnohdhdba_xscal(A_sizes[0], snorm, U_data, (int32_T)(1 + A_sizes[0])); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s_data[2] *= snorm; - ophlaaaiophdbaai_xscal(snorm, Vf, 7); - } - - e[1] = ztest; - if (s_data[2] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s_data[2]); - snorm = s_data[2] / ztest0; - s_data[2] = ztest0; - ohdjhlfcnohdhdba_xscal(A_sizes[0], snorm, U_data, (int32_T)(1 + (int32_T) - (A_sizes[0] << 1))); - } - - e[2] = 0.0F; - nct = 0; - snorm = 0.0F; - if ((s_data[0] >= e[0]) || rtIsNaNF(e[0])) { - ztest0 = s_data[0]; - } else { - ztest0 = e[0]; - } - - if (!((0.0F >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - if ((s_data[1] >= ztest) || rtIsNaNF(ztest)) { - ztest = s_data[1]; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - - if (s_data[2] >= 0.0F) { - ztest0 = s_data[2]; - } else { - ztest0 = 0.0F; - } - - if (!((snorm >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - while (((int32_T)(m + 2) > 0) && (!(nct >= 75))) { - kase = (int32_T)(m + 1); - do { - exitg3 = 0; - q = kase; - if (kase == 0) { - exitg3 = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e[(int32_T)(kase - 1)]); - if ((ztest0 <= ((real32_T)fabs((real_T)s_data[(int32_T)(kase - 1)]) + - (real32_T)fabs((real_T)s_data[kase])) * 1.1920929E-7F) || - ((ztest0 <= 9.86076132E-32F) || ((nct > 20) && (ztest0 <= - 1.1920929E-7F * snorm)))) { - e[(int32_T)(kase - 1)] = 0.0F; - exitg3 = 1; - } else { - kase--; - } - } - } while (exitg3 == 0); - - if ((int32_T)(m + 1) == kase) { - kase = 4; - } else { - qs = (int32_T)(m + 2); - qjj = (int32_T)(m + 2); - exitg2 = false; - while ((!exitg2) && (qjj >= kase)) { - qs = qjj; - if (qjj == kase) { - exitg2 = true; - } else { - ztest0 = 0.0F; - if (qjj < (int32_T)(m + 2)) { - ztest0 = (real32_T)fabs((real_T)e[(int32_T)(qjj - 1)]); - } - - if (qjj > (int32_T)(kase + 1)) { - ztest0 += (real32_T)fabs((real_T)e[(int32_T)(qjj - 2)]); - } - - ztest = (real32_T)fabs((real_T)s_data[(int32_T)(qjj - 1)]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s_data[(int32_T)(qjj - 1)] = 0.0F; - exitg2 = true; - } else { - qjj--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if ((int32_T)(m + 2) == qs) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[m]; - e[m] = 0.0F; - for (qs = m; (int32_T)(qs + 1) >= (int32_T)(q + 1); qs--) { - ztest = s_data[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s_data[qs] = ztest; - if ((int32_T)(qs + 1) > (int32_T)(q + 1)) { - ztest0 = -smm1 * e[0]; - e[0] *= sqds; - } - - gdjmmgdjekfccjmg_xrot(Vf, (int32_T)(1 + (int32_T)(3 * qs)), (int32_T)(1 - + (int32_T)(3 * (int32_T)(m + 1))), sqds, smm1); - } - break; - - case 2: - ztest0 = e[(int32_T)(q - 1)]; - e[(int32_T)(q - 1)] = 0.0F; - for (qs = q; (int32_T)(qs + 1) <= (int32_T)(m + 2); qs++) { - ztest = s_data[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s_data[qs] = ztest; - ztest0 = -smm1 * e[qs]; - e[qs] *= sqds; - pphdglfkecjmjekf_xrot(n, U_data, (int32_T)(1 + (int32_T)(n * qs)), - (int32_T)(1 + (int32_T)(n * (int32_T)(q - 1))), - sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s_data[(int32_T)(m + 1)]); - varargin_1[1] = (real32_T)fabs((real_T)s_data[m]); - varargin_1[2] = (real32_T)fabs((real_T)e[m]); - varargin_1[3] = (real32_T)fabs((real_T)s_data[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - qs = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - kase = 2; - exitg1 = false; - while ((!exitg1) && (kase < 6)) { - qs = kase; - if (!rtIsNaNF(varargin_1[(int32_T)(kase - 1)])) { - ztest = varargin_1[(int32_T)(kase - 1)]; - exitg1 = true; - } else { - kase++; - } - } - } - - if (qs < 5) { - while ((int32_T)(qs + 1) < 6) { - if (varargin_1[qs] > ztest) { - ztest = varargin_1[qs]; - } - - qs++; - } - } - - ztest0 = s_data[(int32_T)(m + 1)] / ztest; - smm1 = s_data[m] / ztest; - emm1 = e[m] / ztest; - sqds = s_data[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - for (qs = (int32_T)(q + 1); qs <= (int32_T)(m + 1); qs++) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - if (qs > (int32_T)(q + 1)) { - e[0] = ztest0; - } - - ztest0 = s_data[(int32_T)(qs - 1)] * sqds + e[(int32_T)(qs - 1)] * smm1; - e[(int32_T)(qs - 1)] = e[(int32_T)(qs - 1)] * sqds - s_data[(int32_T)(qs - - 1)] * smm1; - ztest = smm1 * s_data[qs]; - s_data[qs] *= sqds; - gdjmmgdjekfccjmg_xrot(Vf, (int32_T)(1 + (int32_T)(3 * (int32_T)(qs - 1))), - (int32_T)(1 + (int32_T)(3 * qs)), sqds, smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s_data[(int32_T)(qs - 1)] = ztest0; - ztest0 = e[(int32_T)(qs - 1)] * sqds + smm1 * s_data[qs]; - s_data[qs] = e[(int32_T)(qs - 1)] * -smm1 + sqds * s_data[qs]; - ztest = smm1 * e[qs]; - e[qs] *= sqds; - pphdglfkecjmjekf_xrot(n, U_data, (int32_T)(1 + (int32_T)(n * (int32_T) - (qs - 1))), (int32_T)(1 + (int32_T)(n * qs)), sqds, smm1); - } - - e[m] = ztest0; - nct++; - break; - - default: - if (s_data[q] < 0.0F) { - s_data[q] = -s_data[q]; - ophlaaaiophdbaai_xscal(-1.0F, Vf, (int32_T)(1 + (int32_T)(3 * q))); - } - - nct = (int32_T)(q + 1); - while (((int32_T)(q + 1) < 3) && (s_data[q] < s_data[nct])) { - ztest0 = s_data[q]; - s_data[q] = s_data[nct]; - s_data[nct] = ztest0; - djmgbimooppphlno_xswap(Vf, (int32_T)(1 + (int32_T)(3 * q)), (int32_T)(1 - + (int32_T)(3 * (int32_T)(q + 1)))); - ophdjekfieknglfk_xswap(n, U_data, (int32_T)(1 + (int32_T)(n * q)), - (int32_T)(1 + (int32_T)(n * (int32_T)(q + 1)))); - q = nct; - nct++; - } - - nct = 0; - m--; - break; - } - } - - V_sizes[0] = 3; - V_sizes[1] = 3; - for (n = 0; n < 3; n++) { - V_data[(int32_T)(3 * n)] = Vf[(int32_T)(3 * n)]; - V_data[(int32_T)(1 + (int32_T)(3 * n))] = Vf[(int32_T)((int32_T)(3 * n) + 1)]; - V_data[(int32_T)(2 + (int32_T)(3 * n))] = Vf[(int32_T)((int32_T)(3 * n) + 2)]; - } - - S_sizes[0] = 3; - S_sizes[1] = 3; - for (m = 0; m < 9; m++) { - S_data[m] = 0.0F; - } - - S_data[0] = s_data[0]; - S_data[4] = s_data[1]; - S_data[8] = s_data[2]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/cjmolnohmglfbiec_svd.h b/gnc/matlab/code_generation/sharedutils/cjmolnohmglfbiec_svd.h deleted file mode 100644 index 011f189c06..0000000000 --- a/gnc/matlab/code_generation/sharedutils/cjmolnohmglfbiec_svd.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: cjmolnohmglfbiec_svd.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_cjmolnohmglfbiec_svd -#define SHARE_cjmolnohmglfbiec_svd -#include "rtwtypes.h" - -extern void cjmolnohmglfbiec_svd(const real32_T A_data[], const int32_T A_sizes - [2], real32_T U_data[], int32_T U_sizes[2], real32_T S_data[], int32_T - S_sizes[2], real32_T V_data[], int32_T V_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/const_params.cpp b/gnc/matlab/code_generation/sharedutils/const_params.cpp deleted file mode 100644 index 059d791c99..0000000000 --- a/gnc/matlab/code_generation/sharedutils/const_params.cpp +++ /dev/null @@ -1,13 +0,0 @@ -// -// const_params.cpp -// -// Code generation for model "sim_model_lib0". -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C++ source code generated on : Tue Oct 16 10:08:00 2018 - -#include "rtwtypes.h" - -extern const real_T rtCP_pooled_15LYgUQHWYtd[6]; -const real_T rtCP_pooled_15LYgUQHWYtd[6] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 } ; diff --git a/gnc/matlab/code_generation/sharedutils/dbaidjecmohdphln_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/dbaidjecmohdphln_xzlarf.cpp deleted file mode 100644 index 176e063dcc..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbaidjecmohdphln_xzlarf.cpp +++ /dev/null @@ -1,112 +0,0 @@ -// -// File: dbaidjecmohdphln_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "dbaidjecmohdphln_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void dbaidjecmohdphln_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]) -{ - int32_T lastv; - int32_T lastc; - int32_T ix; - real32_T c; - int32_T iy; - int32_T iac; - int32_T d; - int32_T b_ia; - int32_T jy; - boolean_T exitg2; - if (tau != 0.0F) { - lastv = m; - lastc = iv0 + m; - while ((lastv > 0) && (C_data[lastc - 2] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = n; - exitg2 = false; - while ((!exitg2) && (lastc > 0)) { - iy = (lastc - 1) * ldc + ic0; - jy = iy; - do { - iac = 0; - if (jy <= (iy + lastv) - 1) { - if (C_data[jy - 1] != 0.0F) { - iac = 1; - } else { - jy++; - } - } else { - lastc--; - iac = 2; - } - } while (iac == 0); - - if (iac == 1) { - exitg2 = true; - } - } - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - if (lastc != 0) { - for (iy = 1; iy <= lastc; iy++) { - work[iy - 1] = 0.0F; - } - - iy = 0; - jy = (lastc - 1) * ldc + ic0; - iac = ic0; - while ((ldc > 0) && (iac <= jy)) { - ix = iv0; - c = 0.0F; - d = (iac + lastv) - 1; - for (b_ia = iac; b_ia <= d; b_ia++) { - c += C_data[b_ia - 1] * C_data[ix - 1]; - ix++; - } - - work[iy] += c; - iy++; - iac += ldc; - } - } - - if (!(-tau == 0.0F)) { - iy = ic0 - 1; - jy = 0; - for (iac = 1; iac <= lastc; iac++) { - if (work[jy] != 0.0F) { - c = work[jy] * -tau; - ix = iv0; - d = lastv + iy; - for (b_ia = iy; b_ia + 1 <= d; b_ia++) { - C_data[b_ia] += C_data[ix - 1] * c; - ix++; - } - } - - jy++; - iy += ldc; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbaidjecmohdphln_xzlarf.h b/gnc/matlab/code_generation/sharedutils/dbaidjecmohdphln_xzlarf.h deleted file mode 100644 index ee595ad95d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbaidjecmohdphln_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: dbaidjecmohdphln_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_dbaidjecmohdphln_xzlarf -#define SHARE_dbaidjecmohdphln_xzlarf -#include "rtwtypes.h" - -extern void dbaidjecmohdphln_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbainohddbimecba_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/dbainohddbimecba_xnrm2.cpp deleted file mode 100644 index 63684896c9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbainohddbimecba_xnrm2.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// -// File: dbainohddbimecba_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "dbainohddbimecba_xnrm2.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T dbainohddbimecba_xnrm2(int32_T n, const real32_T x[4], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt((real_T)y); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbainohddbimecba_xnrm2.h b/gnc/matlab/code_generation/sharedutils/dbainohddbimecba_xnrm2.h deleted file mode 100644 index 9b2a0cce1d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbainohddbimecba_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: dbainohddbimecba_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_dbainohddbimecba_xnrm2 -#define SHARE_dbainohddbimecba_xnrm2 -#include "rtwtypes.h" - -extern real32_T dbainohddbimecba_xnrm2(int32_T n, const real32_T x[4], int32_T - ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbiebiecimopdbaa_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/dbiebiecimopdbaa_xnrm2.cpp deleted file mode 100644 index 7301b92292..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbiebiecimopdbaa_xnrm2.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// -// File: dbiebiecimopdbaa_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "dbiebiecimopdbaa_xnrm2.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T dbiebiecimopdbaa_xnrm2(int32_T n, const real32_T x[16], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt((real_T)y); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbiebiecimopdbaa_xnrm2.h b/gnc/matlab/code_generation/sharedutils/dbiebiecimopdbaa_xnrm2.h deleted file mode 100644 index 07325cc964..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbiebiecimopdbaa_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: dbiebiecimopdbaa_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_dbiebiecimopdbaa_xnrm2 -#define SHARE_dbiebiecimopdbaa_xnrm2 -#include "rtwtypes.h" - -extern real32_T dbiebiecimopdbaa_xnrm2(int32_T n, const real32_T x[16], int32_T - ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbiehlnglnohaaai_cat.cpp b/gnc/matlab/code_generation/sharedutils/dbiehlnglnohaaai_cat.cpp deleted file mode 100644 index 5f524dff97..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbiehlnglnohaaai_cat.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// File: dbiehlnglnohaaai_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "dbiehlnglnohaaai_cat.h" - -// Function for MATLAB Function: '/generate_output' -void dbiehlnglnohaaai_cat(const uint8_T varargin_2_data[], const int32_T - varargin_2_sizes[2], const uint8_T varargin_3_data[], const int32_T - varargin_3_sizes[2], uint8_T y_data[], int32_T y_sizes[2]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = 50; - y_sizes[1] = 16; - iy = -1; - for (j = 0; j < 50; j++) { - iy++; - y_data[iy] = 0U; - } - - j = (int32_T)(50 * varargin_2_sizes[1]); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_2_data[(int32_T)(b_j - 1)]; - } - - j = (int32_T)(50 * varargin_3_sizes[1]); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_3_data[(int32_T)(b_j - 1)]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbiehlnglnohaaai_cat.h b/gnc/matlab/code_generation/sharedutils/dbiehlnglnohaaai_cat.h deleted file mode 100644 index f636b02564..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbiehlnglnohaaai_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: dbiehlnglnohaaai_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_dbiehlnglnohaaai_cat -#define SHARE_dbiehlnglnohaaai_cat -#include "rtwtypes.h" - -extern void dbiehlnglnohaaai_cat(const uint8_T varargin_2_data[], const int32_T - varargin_2_sizes[2], const uint8_T varargin_3_data[], const int32_T - varargin_3_sizes[2], uint8_T y_data[], int32_T y_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbiekfcbglfkkfcb_sortLE.cpp b/gnc/matlab/code_generation/sharedutils/dbiekfcbglfkkfcb_sortLE.cpp deleted file mode 100644 index dd290321ca..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbiekfcbglfkkfcb_sortLE.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// -// File: dbiekfcbglfkkfcb_sortLE.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "rt_nonfinite.h" -#include "dbiekfcbglfkkfcb_sortLE.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T dbiekfcbglfkkfcb_sortLE(const real32_T v_data[], const int32_T - v_sizes[2], const int32_T col[2], int32_T irow1, int32_T irow2) -{ - boolean_T p; - int32_T coloffset; - boolean_T b; - int32_T k; - boolean_T exitg1; - p = true; - k = 0; - exitg1 = false; - while ((!exitg1) && (k < 2)) { - coloffset = (int32_T)((int32_T)((int32_T)(col[k] - 1) * v_sizes[0]) - 1); - if ((v_data[(int32_T)(coloffset + irow1)] == v_data[(int32_T)(coloffset + - irow2)]) || (rtIsNaNF(v_data[(int32_T)(coloffset + irow1)]) && - rtIsNaNF(v_data[(int32_T)(coloffset + irow2)]))) { - b = true; - } else { - b = false; - } - - if (!b) { - if ((v_data[(int32_T)(coloffset + irow1)] <= v_data[(int32_T)(coloffset + - irow2)]) || rtIsNaNF(v_data[(int32_T)(coloffset + irow2)])) { - } else { - p = false; - } - - exitg1 = true; - } else { - k++; - } - } - - return p; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbiekfcbglfkkfcb_sortLE.h b/gnc/matlab/code_generation/sharedutils/dbiekfcbglfkkfcb_sortLE.h deleted file mode 100644 index a6b4f4f623..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbiekfcbglfkkfcb_sortLE.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: dbiekfcbglfkkfcb_sortLE.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_dbiekfcbglfkkfcb_sortLE -#define SHARE_dbiekfcbglfkkfcb_sortLE -#include "rtwtypes.h" - -extern boolean_T dbiekfcbglfkkfcb_sortLE(const real32_T v_data[], const int32_T - v_sizes[2], const int32_T col[2], int32_T irow1, int32_T irow2); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbielngdhlfkohdb_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/dbielngdhlfkohdb_xnrm2.cpp deleted file mode 100644 index 51b746c941..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbielngdhlfkohdb_xnrm2.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: dbielngdhlfkohdb_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "dbielngdhlfkohdb_xnrm2.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T dbielngdhlfkohdb_xnrm2(int32_T n, const real32_T x_data[], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (!(n < 1)) { - if (n == 1) { - y = (real32_T)fabs((real_T)x_data[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x_data[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/dbielngdhlfkohdb_xnrm2.h b/gnc/matlab/code_generation/sharedutils/dbielngdhlfkohdb_xnrm2.h deleted file mode 100644 index 6302e5d2e0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/dbielngdhlfkohdb_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: dbielngdhlfkohdb_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_dbielngdhlfkohdb_xnrm2 -#define SHARE_dbielngdhlfkohdb_xnrm2 -#include "rtwtypes.h" - -extern real32_T dbielngdhlfkohdb_xnrm2(int32_T n, const real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/div_nzp_s32_floor.cpp b/gnc/matlab/code_generation/sharedutils/div_nzp_s32_floor.cpp deleted file mode 100644 index 52f1ab8c5f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/div_nzp_s32_floor.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// File: div_nzp_s32_floor.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "div_nzp_s32_floor.h" - -int32_T div_nzp_s32_floor(int32_T numerator, int32_T denominator) -{ - uint32_T absNumerator; - uint32_T absDenominator; - uint32_T tempAbsQuotient; - boolean_T quotientNeedsNegation; - absNumerator = numerator < 0 ? (uint32_T)((uint32_T)~(uint32_T)numerator + 1U) - : (uint32_T)numerator; - absDenominator = denominator < 0 ? (uint32_T)((uint32_T)~(uint32_T)denominator - + 1U) : (uint32_T)denominator; - quotientNeedsNegation = ((numerator < 0) != (denominator < 0)); - tempAbsQuotient = (uint32_T)(absNumerator / absDenominator); - if (quotientNeedsNegation) { - absNumerator %= absDenominator; - if (absNumerator > 0U) { - tempAbsQuotient++; - } - } - - return quotientNeedsNegation ? (int32_T)-(int32_T)tempAbsQuotient : (int32_T) - tempAbsQuotient; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/div_nzp_s32_floor.h b/gnc/matlab/code_generation/sharedutils/div_nzp_s32_floor.h deleted file mode 100644 index 62a0313765..0000000000 --- a/gnc/matlab/code_generation/sharedutils/div_nzp_s32_floor.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: div_nzp_s32_floor.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_div_nzp_s32_floor -#define SHARE_div_nzp_s32_floor -#include "rtwtypes.h" - -extern int32_T div_nzp_s32_floor(int32_T numerator, int32_T denominator); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djecdbimhdjmdjmg_svd.cpp b/gnc/matlab/code_generation/sharedutils/djecdbimhdjmdjmg_svd.cpp deleted file mode 100644 index 8199827b78..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djecdbimhdjmdjmg_svd.cpp +++ /dev/null @@ -1,601 +0,0 @@ -// -// File: djecdbimhdjmdjmg_svd.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include -#include "baaicbiepphdbiek_xaxpy.h" -#include "cjmggdbimohlknop_xnrm2.h" -#include "djmgbimooppphlno_xswap.h" -#include "ekfklfkfohdjaimg_xaxpy.h" -#include "fcjebiekeknggdba_xdotc.h" -#include "gdjmmgdjekfccjmg_xrot.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "kfkfopppmophfcjm_xaxpy.h" -#include "mglnohlnekfcekfk_xdotc.h" -#include "nohdgdbadbiejmop_xnrm2.h" -#include "ohdbaiekngdjbaaa_xdotc.h" -#include "ohdjhlfcnohdhdba_xscal.h" -#include "ophdjekfieknglfk_xswap.h" -#include "ophlaaaiophdbaai_xscal.h" -#include "opppnohdmglfjekf_xaxpy.h" -#include "pphdglfkecjmjekf_xrot.h" -#include "ppppjmohbaaigdje_xaxpy.h" -#include "rt_nonfinite.h" -#include "djecdbimhdjmdjmg_svd.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void djecdbimhdjmdjmg_svd(const real32_T A_data[], const int32_T A_sizes[2], - real32_T U_data[], int32_T U_sizes[2], real32_T S_data[], int32_T S_sizes[2], - real32_T V_data[], int32_T V_sizes[2]) -{ - int32_T n; - real32_T e[3]; - real32_T Vf[9]; - int32_T nct; - int32_T q; - boolean_T apply_transform; - int32_T qjj; - int32_T m; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - real32_T varargin_1[5]; - int32_T i; - int32_T loop_ub; - real32_T b_A_data[96]; - int32_T b_A_sizes[2]; - real32_T s_data[3]; - real32_T work_data[32]; - int32_T work_sizes; - real32_T u_data[96]; - int32_T u_sizes[2]; - int8_T d_idx_0; - b_A_sizes[0] = A_sizes[0]; - b_A_sizes[1] = 3; - loop_ub = A_sizes[0] * A_sizes[1]; - for (i = 0; i < loop_ub; i++) { - b_A_data[i] = A_data[i]; - } - - n = A_sizes[0]; - s_data[0] = 0.0F; - e[0] = 0.0F; - s_data[1] = 0.0F; - e[1] = 0.0F; - s_data[2] = 0.0F; - e[2] = 0.0F; - d_idx_0 = (int8_T)A_sizes[0]; - work_sizes = d_idx_0; - loop_ub = d_idx_0; - for (i = 0; i < loop_ub; i++) { - work_data[i] = 0.0F; - } - - d_idx_0 = (int8_T)A_sizes[0]; - U_sizes[0] = d_idx_0; - U_sizes[1] = 3; - loop_ub = d_idx_0 * 3; - for (i = 0; i < loop_ub; i++) { - U_data[i] = 0.0F; - } - - for (i = 0; i < 9; i++) { - Vf[i] = 0.0F; - } - - if (A_sizes[0] < 1) { - nct = 0; - } else { - nct = A_sizes[0] - 1; - } - - if (!(nct <= 3)) { - nct = 3; - } - - if (nct >= 1) { - m = nct; - } else { - m = 1; - } - - for (kase = 0; kase + 1 <= m; kase++) { - qs = n * kase + kase; - q = n - kase; - apply_transform = false; - if (kase + 1 <= nct) { - snorm = nohdgdbadbiejmop_xnrm2(q, b_A_data, qs + 1); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A_data[qs] < 0.0F) { - s_data[kase] = -snorm; - } else { - s_data[kase] = snorm; - } - - if ((real32_T)fabs((real_T)s_data[kase]) >= 9.86076132E-32F) { - snorm = 1.0F / s_data[kase]; - qjj = qs + q; - for (loop_ub = qs; loop_ub + 1 <= qjj; loop_ub++) { - b_A_data[loop_ub] *= snorm; - } - } else { - qjj = qs + q; - for (loop_ub = qs; loop_ub + 1 <= qjj; loop_ub++) { - b_A_data[loop_ub] /= s_data[kase]; - } - } - - b_A_data[qs]++; - s_data[kase] = -s_data[kase]; - } else { - s_data[kase] = 0.0F; - } - } - - for (loop_ub = kase + 1; loop_ub + 1 < 4; loop_ub++) { - qjj = n * loop_ub + kase; - if (apply_transform) { - ppppjmohbaaigdje_xaxpy(q, -(ohdbaiekngdjbaaa_xdotc(q, b_A_data, qs + 1, - b_A_data, qjj + 1) / b_A_data[kase + b_A_sizes[0] * kase]), qs + 1, - b_A_data, qjj + 1); - } - - e[loop_ub] = b_A_data[qjj]; - } - - if (kase + 1 <= nct) { - for (qs = kase; qs + 1 <= n; qs++) { - U_data[qs + d_idx_0 * kase] = b_A_data[b_A_sizes[0] * kase + qs]; - } - } - - if (kase + 1 <= 1) { - snorm = cjmggdbimohlknop_xnrm2(e, 2); - if (snorm == 0.0F) { - e[0] = 0.0F; - } else { - if (e[1] < 0.0F) { - snorm = -snorm; - } - - e[0] = snorm; - if ((real32_T)fabs((real_T)snorm) >= 9.86076132E-32F) { - snorm = 1.0F / snorm; - for (qs = 1; qs + 1 < 4; qs++) { - e[qs] *= snorm; - } - } else { - for (qs = 1; qs + 1 < 4; qs++) { - e[qs] /= snorm; - } - } - - e[1]++; - e[0] = -e[0]; - for (qs = 2; qs <= n; qs++) { - work_data[qs - 1] = 0.0F; - } - - for (qs = 1; qs + 1 < 4; qs++) { - baaicbiepphdbiek_xaxpy(q - 1, e[qs], b_A_data, n * qs + 2, work_data, - 2); - } - - for (qs = 1; qs + 1 < 4; qs++) { - kfkfopppmophfcjm_xaxpy(q - 1, -e[qs] / e[1], work_data, 2, b_A_data, n - * qs + 2); - } - } - - for (q = 1; q + 1 < 4; q++) { - Vf[q] = e[q]; - } - } - } - - m = 1; - if (nct < 3) { - s_data[nct] = b_A_data[b_A_sizes[0] * nct + nct]; - } - - e[1] = b_A_data[(b_A_sizes[0] << 1) + 1]; - e[2] = 0.0F; - if (nct + 1 <= 3) { - for (q = nct; q + 1 < 4; q++) { - for (kase = 1; kase <= n; kase++) { - U_data[(kase + d_idx_0 * q) - 1] = 0.0F; - } - - U_data[q + d_idx_0 * q] = 1.0F; - } - } - - for (nct--; nct + 1 > 0; nct--) { - q = n - nct; - qs = n * nct + nct; - if (s_data[nct] != 0.0F) { - for (kase = nct + 1; kase + 1 < 4; kase++) { - qjj = (n * kase + nct) + 1; - u_sizes[0] = U_sizes[0]; - u_sizes[1] = 3; - loop_ub = U_sizes[0] * U_sizes[1]; - for (i = 0; i < loop_ub; i++) { - u_data[i] = U_data[i]; - } - - opppnohdmglfjekf_xaxpy(q, -(fcjebiekeknggdba_xdotc(q, U_data, qs + 1, - U_data, qjj) / U_data[qs]), qs + 1, u_data, qjj); - U_sizes[0] = u_sizes[0]; - U_sizes[1] = u_sizes[1]; - loop_ub = u_sizes[0] * u_sizes[1]; - for (i = 0; i < loop_ub; i++) { - U_data[i] = u_data[i]; - } - } - - for (q = nct; q + 1 <= n; q++) { - U_data[q + U_sizes[0] * nct] = -U_data[U_sizes[0] * nct + q]; - } - - U_data[qs]++; - for (q = 1; q <= nct; q++) { - U_data[(q + U_sizes[0] * nct) - 1] = 0.0F; - } - } else { - for (q = 1; q <= n; q++) { - U_data[(q + U_sizes[0] * nct) - 1] = 0.0F; - } - - U_data[qs] = 1.0F; - } - } - - for (nct = 2; nct >= 0; nct += -1) { - if ((nct + 1 <= 1) && (e[0] != 0.0F)) { - ekfklfkfohdjaimg_xaxpy(2, -(mglnohlnekfcekfk_xdotc(2, Vf, 2, Vf, 5) / Vf[1]), - 2, Vf, 5); - ekfklfkfohdjaimg_xaxpy(2, -(mglnohlnekfcekfk_xdotc(2, Vf, 2, Vf, 8) / Vf[1]), - 2, Vf, 8); - } - - Vf[3 * nct] = 0.0F; - Vf[1 + 3 * nct] = 0.0F; - Vf[2 + 3 * nct] = 0.0F; - Vf[nct + 3 * nct] = 1.0F; - } - - for (nct = 0; nct < 3; nct++) { - if (s_data[nct] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s_data[nct]); - snorm = s_data[nct] / ztest0; - s_data[nct] = ztest0; - if (nct + 1 < 3) { - e[nct] /= snorm; - } - - u_sizes[0] = U_sizes[0]; - u_sizes[1] = 3; - loop_ub = U_sizes[0] * U_sizes[1]; - for (i = 0; i < loop_ub; i++) { - u_data[i] = U_data[i]; - } - - ohdjhlfcnohdhdba_xscal(n, snorm, u_data, 1 + n * nct); - U_sizes[0] = u_sizes[0]; - U_sizes[1] = u_sizes[1]; - loop_ub = u_sizes[0] * u_sizes[1]; - for (i = 0; i < loop_ub; i++) { - U_data[i] = u_data[i]; - } - } - - if ((nct + 1 < 3) && (e[nct] != 0.0F)) { - ztest0 = (real32_T)fabs((real_T)e[nct]); - snorm = ztest0 / e[nct]; - e[nct] = ztest0; - s_data[nct + 1] *= snorm; - ophlaaaiophdbaai_xscal(snorm, Vf, 1 + 3 * (nct + 1)); - } - } - - nct = 0; - snorm = 0.0F; - ztest0 = (real32_T)fabs((real_T)s_data[0]); - ztest = (real32_T)fabs((real_T)e[0]); - if ((ztest0 >= ztest) || rtIsNaNF(ztest)) { - ztest = ztest0; - } - - if (!((0.0F >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - - ztest0 = (real32_T)fabs((real_T)s_data[1]); - ztest = (real32_T)fabs((real_T)e[1]); - if ((ztest0 >= ztest) || rtIsNaNF(ztest)) { - ztest = ztest0; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - - ztest0 = (real32_T)fabs((real_T)s_data[2]); - ztest = (real32_T)fabs((real_T)e[2]); - if ((ztest0 >= ztest) || rtIsNaNF(ztest)) { - ztest = ztest0; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - - while ((m + 2 > 0) && (!(nct >= 75))) { - kase = m + 1; - do { - qs = 0; - q = kase; - if (kase == 0) { - qs = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e[kase - 1]); - if ((ztest0 <= ((real32_T)fabs((real_T)s_data[kase - 1]) + (real32_T) - fabs((real_T)s_data[kase])) * 1.1920929E-7F) || (ztest0 <= - 9.86076132E-32F) || ((nct > 20) && (ztest0 <= 1.1920929E-7F * snorm))) - { - e[kase - 1] = 0.0F; - qs = 1; - } else { - kase--; - } - } - } while (qs == 0); - - if (m + 1 == kase) { - kase = 4; - } else { - qs = m + 2; - qjj = m + 2; - apply_transform = false; - while ((!apply_transform) && (qjj >= kase)) { - qs = qjj; - if (qjj == kase) { - apply_transform = true; - } else { - ztest0 = 0.0F; - if (qjj < m + 2) { - ztest0 = (real32_T)fabs((real_T)e[qjj - 1]); - } - - if (qjj > kase + 1) { - ztest0 += (real32_T)fabs((real_T)e[qjj - 2]); - } - - ztest = (real32_T)fabs((real_T)s_data[qjj - 1]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s_data[qjj - 1] = 0.0F; - apply_transform = true; - } else { - qjj--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if (m + 2 == qs) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[m]; - e[m] = 0.0F; - for (kase = m; kase + 1 >= q + 1; kase--) { - ztest = s_data[kase]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s_data[kase] = ztest; - if (kase + 1 > q + 1) { - ztest0 = -smm1 * e[0]; - e[0] *= sqds; - } - - gdjmmgdjekfccjmg_xrot(Vf, 1 + 3 * kase, 1 + 3 * (m + 1), sqds, smm1); - } - break; - - case 2: - ztest0 = e[q - 1]; - e[q - 1] = 0.0F; - for (kase = q; kase + 1 <= m + 2; kase++) { - ztest = s_data[kase]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s_data[kase] = ztest; - ztest0 = -smm1 * e[kase]; - e[kase] *= sqds; - u_sizes[0] = U_sizes[0]; - u_sizes[1] = 3; - loop_ub = U_sizes[0] * U_sizes[1]; - for (i = 0; i < loop_ub; i++) { - u_data[i] = U_data[i]; - } - - pphdglfkecjmjekf_xrot(n, u_data, 1 + n * kase, 1 + n * (q - 1), sqds, - smm1); - U_sizes[0] = u_sizes[0]; - U_sizes[1] = u_sizes[1]; - loop_ub = u_sizes[0] * u_sizes[1]; - for (i = 0; i < loop_ub; i++) { - U_data[i] = u_data[i]; - } - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s_data[m + 1]); - varargin_1[1] = (real32_T)fabs((real_T)s_data[m]); - varargin_1[2] = (real32_T)fabs((real_T)e[m]); - varargin_1[3] = (real32_T)fabs((real_T)s_data[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - kase = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - qs = 2; - apply_transform = false; - while ((!apply_transform) && (qs < 6)) { - kase = qs; - if (!rtIsNaNF(varargin_1[qs - 1])) { - ztest = varargin_1[qs - 1]; - apply_transform = true; - } else { - qs++; - } - } - } - - if (kase < 5) { - while (kase + 1 < 6) { - if (varargin_1[kase] > ztest) { - ztest = varargin_1[kase]; - } - - kase++; - } - } - - ztest0 = s_data[m + 1] / ztest; - smm1 = s_data[m] / ztest; - emm1 = e[m] / ztest; - sqds = s_data[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - for (kase = q + 1; kase <= m + 1; kase++) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - if (kase > q + 1) { - e[0] = ztest0; - } - - ztest0 = s_data[kase - 1] * sqds + e[kase - 1] * smm1; - e[kase - 1] = e[kase - 1] * sqds - s_data[kase - 1] * smm1; - ztest = smm1 * s_data[kase]; - s_data[kase] *= sqds; - gdjmmgdjekfccjmg_xrot(Vf, 1 + 3 * (kase - 1), 1 + 3 * kase, sqds, smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s_data[kase - 1] = ztest0; - ztest0 = e[kase - 1] * sqds + smm1 * s_data[kase]; - s_data[kase] = e[kase - 1] * -smm1 + sqds * s_data[kase]; - ztest = smm1 * e[kase]; - e[kase] *= sqds; - u_sizes[0] = U_sizes[0]; - u_sizes[1] = 3; - loop_ub = U_sizes[0] * U_sizes[1]; - for (i = 0; i < loop_ub; i++) { - u_data[i] = U_data[i]; - } - - pphdglfkecjmjekf_xrot(n, u_data, 1 + n * (kase - 1), 1 + n * kase, sqds, - smm1); - U_sizes[0] = u_sizes[0]; - U_sizes[1] = u_sizes[1]; - loop_ub = u_sizes[0] * u_sizes[1]; - for (i = 0; i < loop_ub; i++) { - U_data[i] = u_data[i]; - } - } - - e[m] = ztest0; - nct++; - break; - - default: - if (s_data[q] < 0.0F) { - s_data[q] = -s_data[q]; - ophlaaaiophdbaai_xscal(-1.0F, Vf, 1 + 3 * q); - } - - nct = q + 1; - while ((q + 1 < 3) && (s_data[q] < s_data[nct])) { - ztest0 = s_data[q]; - s_data[q] = s_data[nct]; - s_data[nct] = ztest0; - djmgbimooppphlno_xswap(Vf, 1 + 3 * q, 1 + 3 * (q + 1)); - u_sizes[0] = U_sizes[0]; - u_sizes[1] = 3; - loop_ub = U_sizes[0] * U_sizes[1]; - for (i = 0; i < loop_ub; i++) { - u_data[i] = U_data[i]; - } - - ophdjekfieknglfk_xswap(n, u_data, 1 + n * q, 1 + n * (q + 1)); - U_sizes[0] = u_sizes[0]; - U_sizes[1] = u_sizes[1]; - loop_ub = u_sizes[0] * u_sizes[1]; - for (i = 0; i < loop_ub; i++) { - U_data[i] = u_data[i]; - } - - q = nct; - nct++; - } - - nct = 0; - m--; - break; - } - } - - V_sizes[0] = 3; - V_sizes[1] = 3; - for (n = 0; n < 3; n++) { - V_data[3 * n] = Vf[3 * n]; - V_data[1 + 3 * n] = Vf[3 * n + 1]; - V_data[2 + 3 * n] = Vf[3 * n + 2]; - } - - S_sizes[0] = 3; - S_sizes[1] = 3; - for (i = 0; i < 9; i++) { - S_data[i] = 0.0F; - } - - S_data[0] = s_data[0]; - S_data[4] = s_data[1]; - S_data[8] = s_data[2]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djecdbimhdjmdjmg_svd.h b/gnc/matlab/code_generation/sharedutils/djecdbimhdjmdjmg_svd.h deleted file mode 100644 index e28a029019..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djecdbimhdjmdjmg_svd.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: djecdbimhdjmdjmg_svd.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_djecdbimhdjmdjmg_svd -#define SHARE_djecdbimhdjmdjmg_svd -#include "rtwtypes.h" - -extern void djecdbimhdjmdjmg_svd(const real32_T A_data[], const int32_T A_sizes - [2], real32_T U_data[], int32_T U_sizes[2], real32_T S_data[], int32_T - S_sizes[2], real32_T V_data[], int32_T V_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djecfknojekfgdba_mldivide.cpp b/gnc/matlab/code_generation/sharedutils/djecfknojekfgdba_mldivide.cpp deleted file mode 100644 index e540216121..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djecfknojekfgdba_mldivide.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// -// File: djecfknojekfgdba_mldivide.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include -#include "djecfknojekfgdba_mldivide.h" - -// Function for MATLAB Function: '/MATLAB Function' -void djecfknojekfgdba_mldivide(const real32_T A[225], real32_T B[225]) -{ - real32_T temp; - real32_T b_A[225]; - int8_T ipiv[15]; - int32_T j; - real32_T s; - int32_T iy; - int32_T c_ix; - int32_T d; - int32_T ijA; - int32_T b_jBcol; - int32_T b_kAcol; - int32_T c_i; - memcpy(&b_A[0], &A[0], (uint32_T)(225U * sizeof(real32_T))); - for (j = 0; j < 15; j++) { - ipiv[j] = (int8_T)(int32_T)(1 + j); - } - - for (j = 0; j < 14; j++) { - b_jBcol = (int32_T)(j << 4); - iy = 0; - b_kAcol = b_jBcol; - temp = (real32_T)fabs((real_T)b_A[b_jBcol]); - for (c_i = 2; c_i <= (int32_T)(15 - j); c_i++) { - b_kAcol++; - s = (real32_T)fabs((real_T)b_A[b_kAcol]); - if (s > temp) { - iy = (int32_T)(c_i - 1); - temp = s; - } - } - - if (b_A[(int32_T)(b_jBcol + iy)] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)(int32_T)((int32_T)(j + iy) + 1); - b_kAcol = j; - iy += j; - for (c_i = 0; c_i < 15; c_i++) { - temp = b_A[b_kAcol]; - b_A[b_kAcol] = b_A[iy]; - b_A[iy] = temp; - b_kAcol += 15; - iy += 15; - } - } - - iy = (int32_T)((int32_T)(b_jBcol - j) + 15); - for (b_kAcol = (int32_T)(b_jBcol + 1); (int32_T)(b_kAcol + 1) <= iy; - b_kAcol++) { - b_A[b_kAcol] /= b_A[b_jBcol]; - } - } - - iy = b_jBcol; - b_kAcol = (int32_T)(b_jBcol + 15); - for (c_i = 1; c_i <= (int32_T)(14 - j); c_i++) { - temp = b_A[b_kAcol]; - if (b_A[b_kAcol] != 0.0F) { - c_ix = (int32_T)(b_jBcol + 1); - d = (int32_T)((int32_T)(iy - j) + 30); - for (ijA = (int32_T)(16 + iy); (int32_T)(ijA + 1) <= d; ijA++) { - b_A[ijA] += b_A[c_ix] * -temp; - c_ix++; - } - } - - b_kAcol += 15; - iy += 15; - } - } - - for (j = 0; j < 14; j++) { - if ((int32_T)(j + 1) != (int32_T)ipiv[j]) { - b_jBcol = (int32_T)((int32_T)ipiv[j] - 1); - for (iy = 0; iy < 15; iy++) { - temp = B[(int32_T)((int32_T)(15 * iy) + j)]; - B[(int32_T)(j + (int32_T)(15 * iy))] = B[(int32_T)((int32_T)(15 * iy) + - b_jBcol)]; - B[(int32_T)(b_jBcol + (int32_T)(15 * iy))] = temp; - } - } - } - - for (j = 0; j < 15; j++) { - b_jBcol = (int32_T)(15 * j); - for (iy = 0; iy < 15; iy++) { - b_kAcol = (int32_T)(15 * iy); - if (B[(int32_T)(iy + b_jBcol)] != 0.0F) { - for (c_i = (int32_T)(iy + 1); (int32_T)(c_i + 1) < 16; c_i++) { - B[(int32_T)(c_i + b_jBcol)] -= B[(int32_T)(iy + b_jBcol)] * b_A - [(int32_T)(c_i + b_kAcol)]; - } - } - } - } - - for (j = 0; j < 15; j++) { - b_jBcol = (int32_T)(15 * j); - for (iy = 14; iy >= 0; iy += -1) { - b_kAcol = (int32_T)(15 * iy); - if (B[(int32_T)(iy + b_jBcol)] != 0.0F) { - B[(int32_T)(iy + b_jBcol)] /= b_A[(int32_T)(iy + b_kAcol)]; - for (c_i = 0; (int32_T)(c_i + 1) <= iy; c_i++) { - B[(int32_T)(c_i + b_jBcol)] -= B[(int32_T)(iy + b_jBcol)] * b_A - [(int32_T)(c_i + b_kAcol)]; - } - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djecfknojekfgdba_mldivide.h b/gnc/matlab/code_generation/sharedutils/djecfknojekfgdba_mldivide.h deleted file mode 100644 index 827d36a586..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djecfknojekfgdba_mldivide.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: djecfknojekfgdba_mldivide.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_djecfknojekfgdba_mldivide -#define SHARE_djecfknojekfgdba_mldivide -#include "rtwtypes.h" - -extern void djecfknojekfgdba_mldivide(const real32_T A[225], real32_T B[225]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djeclngddbimoppp_cat.cpp b/gnc/matlab/code_generation/sharedutils/djeclngddbimoppp_cat.cpp deleted file mode 100644 index f3afa285af..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djeclngddbimoppp_cat.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: djeclngddbimoppp_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1051 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Tue Nov 8 14:36:07 2016 -// -#include "rtwtypes.h" -#include "djeclngddbimoppp_cat.h" - -// Function for MATLAB Function: '/generate_output' -void djeclngddbimoppp_cat(const int32_T varargin_1_sizes, const int32_T - varargin_2_sizes[2], uint8_T y_data[], int32_T y_sizes[2]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = (int16_T)varargin_1_sizes; - y_sizes[1] = 16; - iy = -1; - for (j = 1; j <= varargin_1_sizes; j++) { - iy++; - y_data[iy] = 1U; - } - - j = varargin_2_sizes[0] * 15; - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = 0U; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djeclngddbimoppp_cat.h b/gnc/matlab/code_generation/sharedutils/djeclngddbimoppp_cat.h deleted file mode 100644 index 91ad7725a7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djeclngddbimoppp_cat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: djeclngddbimoppp_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1051 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Tue Nov 8 14:36:07 2016 -// -#ifndef SHARE_djeclngddbimoppp_cat -#define SHARE_djeclngddbimoppp_cat -#include "rtwtypes.h" - -extern void djeclngddbimoppp_cat(const int32_T varargin_1_sizes, const int32_T - varargin_2_sizes[2], uint8_T y_data[], int32_T y_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djmgbimooppphlno_xswap.cpp b/gnc/matlab/code_generation/sharedutils/djmgbimooppphlno_xswap.cpp deleted file mode 100644 index 9823b6077e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djmgbimooppphlno_xswap.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// File: djmgbimooppphlno_xswap.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "djmgbimooppphlno_xswap.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void djmgbimooppphlno_xswap(real32_T x[9], int32_T ix0, int32_T iy0) -{ - int32_T ix; - int32_T iy; - real32_T temp; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; - ix++; - iy++; - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; - ix++; - iy++; - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djmgbimooppphlno_xswap.h b/gnc/matlab/code_generation/sharedutils/djmgbimooppphlno_xswap.h deleted file mode 100644 index c7772daeb1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djmgbimooppphlno_xswap.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: djmgbimooppphlno_xswap.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_djmgbimooppphlno_xswap -#define SHARE_djmgbimooppphlno_xswap -#include "rtwtypes.h" - -extern void djmgbimooppphlno_xswap(real32_T x[9], int32_T ix0, int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djmgjecbcbiengln_power.cpp b/gnc/matlab/code_generation/sharedutils/djmgjecbcbiengln_power.cpp deleted file mode 100644 index 7ed2385af4..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djmgjecbcbiengln_power.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// -// File: djmgjecbcbiengln_power.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "djmgjecbcbiengln_power.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void djmgjecbcbiengln_power(const real32_T a_data[], const int32_T a_sizes, - real32_T y_data[], int32_T *y_sizes) -{ - int32_T k; - *y_sizes = (int32_T)(uint8_T)a_sizes; - for (k = 0; (int32_T)(k + 1) <= a_sizes; k++) { - y_data[k] = a_data[k] * a_data[k]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djmgjecbcbiengln_power.h b/gnc/matlab/code_generation/sharedutils/djmgjecbcbiengln_power.h deleted file mode 100644 index fab463a76e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djmgjecbcbiengln_power.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: djmgjecbcbiengln_power.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_djmgjecbcbiengln_power -#define SHARE_djmgjecbcbiengln_power -#include "rtwtypes.h" - -extern void djmgjecbcbiengln_power(const real32_T a_data[], const int32_T - a_sizes, real32_T y_data[], int32_T *y_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djmolnoppphdglno_quat_rotation_vec.cpp b/gnc/matlab/code_generation/sharedutils/djmolnoppphdglno_quat_rotation_vec.cpp deleted file mode 100644 index 4719b71720..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djmolnoppphdglno_quat_rotation_vec.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// -// File: djmolnoppphdglno_quat_rotation_vec.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#include "rtwtypes.h" -#include "djmolnoppphdglno_quat_rotation_vec.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void djmolnoppphdglno_quat_rotation_vec(real_T vector[3], const real32_T Q[4], - real32_T vec_out[3]) -{ - real32_T qOut_idx_3; - real32_T qOut_idx_1; - real32_T qOut_idx_2; - real32_T qOut_idx_0; - - // - // Q = q_a2b, rotation represents a vector rotating from frame b2a - // q_out = quat_rotation(vector, Q2) - // - // rotates a row-vector of 3-vectors by a row-vector of quaternion Q2 - // - // From: Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra - // Equation below is from Eq. 77, with the the quaternions inverted because - // our quat multiplication convention has the reverse order from the - // mulitplication used in the paper - // - // Check to see if the vector is a single row - // Vec_out = Q * [V 0]' * Q^-1 - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - // - // Jesse C. Fusco jesse.c.fusco@nasa.gov - qOut_idx_0 = (0.0F * Q[0] + Q[3] * (real32_T)vector[0]) + (Q[1] * (real32_T) - vector[2] - Q[2] * (real32_T)vector[1]); - qOut_idx_1 = (0.0F * Q[1] + Q[3] * (real32_T)vector[1]) + (Q[2] * (real32_T) - vector[0] - Q[0] * (real32_T)vector[2]); - qOut_idx_2 = (0.0F * Q[2] + Q[3] * (real32_T)vector[2]) + (Q[0] * (real32_T) - vector[1] - Q[1] * (real32_T)vector[0]); - qOut_idx_3 = Q[3] * 0.0F - ((Q[0] * (real32_T)vector[0] + Q[1] * (real32_T) - vector[1]) + Q[2] * (real32_T)vector[2]); - - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - // - // Jesse C. Fusco jesse.c.fusco@nasa.gov - vec_out[0] = (Q[3] * qOut_idx_0 + qOut_idx_3 * -Q[0]) + (qOut_idx_1 * -Q[2] - - qOut_idx_2 * -Q[1]); - vec_out[1] = (Q[3] * qOut_idx_1 + qOut_idx_3 * -Q[1]) + (qOut_idx_2 * -Q[0] - - qOut_idx_0 * -Q[2]); - vec_out[2] = (Q[3] * qOut_idx_2 + qOut_idx_3 * -Q[2]) + (qOut_idx_0 * -Q[1] - - qOut_idx_1 * -Q[0]); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/djmolnoppphdglno_quat_rotation_vec.h b/gnc/matlab/code_generation/sharedutils/djmolnoppphdglno_quat_rotation_vec.h deleted file mode 100644 index ab4cc60eb1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/djmolnoppphdglno_quat_rotation_vec.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: djmolnoppphdglno_quat_rotation_vec.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#ifndef SHARE_djmolnoppphdglno_quat_rotation_vec -#define SHARE_djmolnoppphdglno_quat_rotation_vec -#include "rtwtypes.h" - -extern void djmolnoppphdglno_quat_rotation_vec(real_T vector[3], const real32_T - Q[4], real32_T vec_out[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ecbaphdbnglnimoh_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/ecbaphdbnglnimoh_xdotc.cpp deleted file mode 100644 index 197f2a1226..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ecbaphdbnglnimoh_xdotc.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ecbaphdbnglnimoh_xdotc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ecbaphdbnglnimoh_xdotc.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T ecbaphdbnglnimoh_xdotc(const real32_T x[4], const real32_T y[4]) -{ - return x[0] * y[2] + x[1] * y[3]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ecbaphdbnglnimoh_xdotc.h b/gnc/matlab/code_generation/sharedutils/ecbaphdbnglnimoh_xdotc.h deleted file mode 100644 index 6be6bee95c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ecbaphdbnglnimoh_xdotc.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: ecbaphdbnglnimoh_xdotc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ecbaphdbnglnimoh_xdotc -#define SHARE_ecbaphdbnglnimoh_xdotc -#include "rtwtypes.h" - -extern real32_T ecbaphdbnglnimoh_xdotc(const real32_T x[4], const real32_T y[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ecbilfcbnglnimoh_xzlarfg.cpp b/gnc/matlab/code_generation/sharedutils/ecbilfcbnglnimoh_xzlarfg.cpp deleted file mode 100644 index 486d5e69bb..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ecbilfcbnglnimoh_xzlarfg.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -// File: ecbilfcbnglnimoh_xzlarfg.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "biekecbihdbaecjm_xnrm2.h" -#include "rt_hypotf_snf.h" -#include "ecbilfcbnglnimoh_xzlarfg.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T ecbilfcbnglnimoh_xzlarfg(int32_T n, real32_T *alpha1, real32_T x_data[], - int32_T ix0) -{ - real32_T tau; - real32_T xnorm; - int32_T knt; - int32_T b_k; - int32_T c_k; - tau = 0.0F; - if (!(n <= 0)) { - xnorm = biekecbihdbaecjm_xnrm2((int32_T)(n - 1), x_data, ix0); - if (xnorm != 0.0F) { - xnorm = rt_hypotf_snf(*alpha1, xnorm); - if (*alpha1 >= 0.0F) { - xnorm = -xnorm; - } - - if ((real32_T)fabs((real_T)xnorm) < 9.86076132E-32F) { - knt = 0; - do { - knt++; - b_k = (int32_T)((int32_T)(ix0 + n) - 2); - for (c_k = ix0; c_k <= b_k; c_k++) { - x_data[(int32_T)(c_k - 1)] *= 1.01412048E+31F; - } - - xnorm *= 1.01412048E+31F; - *alpha1 *= 1.01412048E+31F; - } while (!((real32_T)fabs((real_T)xnorm) >= 9.86076132E-32F)); - - xnorm = rt_hypotf_snf(*alpha1, biekecbihdbaecjm_xnrm2((int32_T)(n - 1), - x_data, ix0)); - if (*alpha1 >= 0.0F) { - xnorm = -xnorm; - } - - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - b_k = (int32_T)((int32_T)(ix0 + n) - 2); - for (c_k = ix0; c_k <= b_k; c_k++) { - x_data[(int32_T)(c_k - 1)] *= *alpha1; - } - - for (b_k = 1; b_k <= knt; b_k++) { - xnorm *= 9.86076132E-32F; - } - - *alpha1 = xnorm; - } else { - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - knt = (int32_T)((int32_T)(ix0 + n) - 2); - for (b_k = ix0; b_k <= knt; b_k++) { - x_data[(int32_T)(b_k - 1)] *= *alpha1; - } - - *alpha1 = xnorm; - } - } - } - - return tau; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ecbilfcbnglnimoh_xzlarfg.h b/gnc/matlab/code_generation/sharedutils/ecbilfcbnglnimoh_xzlarfg.h deleted file mode 100644 index 7a8e7a4f70..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ecbilfcbnglnimoh_xzlarfg.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ecbilfcbnglnimoh_xzlarfg.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ecbilfcbnglnimoh_xzlarfg -#define SHARE_ecbilfcbnglnimoh_xzlarfg -#include "rtwtypes.h" - -extern real32_T ecbilfcbnglnimoh_xzlarfg(int32_T n, real32_T *alpha1, real32_T - x_data[], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ecjedbaiaiekohln_quaternion_to_rotation.cpp b/gnc/matlab/code_generation/sharedutils/ecjedbaiaiekohln_quaternion_to_rotation.cpp deleted file mode 100644 index 09a0f49c39..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ecjedbaiaiekohln_quaternion_to_rotation.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// -// File: ecjedbaiaiekohln_quaternion_to_rotation.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ecjedbaiaiekohln_quaternion_to_rotation.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void ecjedbaiaiekohln_quaternion_to_rotation(const real_T q[4], real_T R[9]) -{ - real_T S[9]; - static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - real_T q_0; - real_T q_1; - real_T q_2[12]; - real_T q_3[12]; - int32_T i; - int32_T i_0; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - S[0] = 0.0; - S[3] = -q[2]; - S[6] = q[1]; - S[1] = q[2]; - S[4] = 0.0; - S[7] = -q[0]; - S[2] = -q[1]; - S[5] = q[0]; - S[8] = 0.0; - q_0 = q[3]; - q_1 = q[3]; - for (i = 0; i < 3; i++) { - q_2[(int32_T)(3 * i)] = q_0 * (real_T)b[i] + S[i]; - q_3[(int32_T)(i << 2)] = (real_T)b[(int32_T)(3 * i)] * q_1 - S[(int32_T)(3 * - i)]; - q_2[(int32_T)(1 + (int32_T)(3 * i))] = (real_T)b[(int32_T)(i + 3)] * q_0 + - S[(int32_T)(i + 3)]; - q_3[(int32_T)(1 + (int32_T)(i << 2))] = (real_T)b[(int32_T)((int32_T)(3 * i) - + 1)] * q_1 - S[(int32_T)((int32_T)(3 * i) + 1)]; - q_2[(int32_T)(2 + (int32_T)(3 * i))] = (real_T)b[(int32_T)(i + 6)] * q_0 + - S[(int32_T)(i + 6)]; - q_3[(int32_T)(2 + (int32_T)(i << 2))] = (real_T)b[(int32_T)((int32_T)(3 * i) - + 2)] * q_1 - S[(int32_T)((int32_T)(3 * i) + 2)]; - q_2[(int32_T)(9 + i)] = -q[i]; - q_3[(int32_T)(3 + (int32_T)(i << 2))] = -q[i]; - } - - for (i = 0; i < 3; i++) { - for (i_0 = 0; i_0 < 3; i_0++) { - R[(int32_T)(i_0 + (int32_T)(3 * i))] = 0.0; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)(i << 2)] * q_2[i_0]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 1)] * q_2[(int32_T)(i_0 + 3)]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 2)] * q_2[(int32_T)(i_0 + 6)]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 3)] * q_2[(int32_T)(i_0 + 9)]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ecjedbaiaiekohln_quaternion_to_rotation.h b/gnc/matlab/code_generation/sharedutils/ecjedbaiaiekohln_quaternion_to_rotation.h deleted file mode 100644 index e33f210e87..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ecjedbaiaiekohln_quaternion_to_rotation.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ecjedbaiaiekohln_quaternion_to_rotation.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ecjedbaiaiekohln_quaternion_to_rotation -#define SHARE_ecjedbaiaiekohln_quaternion_to_rotation -#include "rtwtypes.h" - -extern void ecjedbaiaiekohln_quaternion_to_rotation(const real_T q[4], real_T R - [9]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ecjegdjmmgdbimgl_norm.cpp b/gnc/matlab/code_generation/sharedutils/ecjegdjmmgdbimgl_norm.cpp deleted file mode 100644 index 9bab86d9f5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ecjegdjmmgdbimgl_norm.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// -// File: ecjegdjmmgdbimgl_norm.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Mon Mar 6 17:13:18 2017 -// -#include "rtwtypes.h" -#include -#include "ecjegdjmmgdbimgl_norm.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T ecjegdjmmgdbimgl_norm(const real32_T x[2]) -{ - real32_T y; - real32_T scale; - real32_T absxk; - real32_T t; - scale = 1.17549435E-38F; - absxk = (real32_T)fabs((real_T)x[0]); - if (absxk > 1.17549435E-38F) { - y = 1.0F; - scale = absxk; - } else { - t = absxk / 1.17549435E-38F; - y = t * t; - } - - absxk = (real32_T)fabs((real_T)x[1]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - - return scale * (real32_T)sqrt((real_T)y); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ecjegdjmmgdbimgl_norm.h b/gnc/matlab/code_generation/sharedutils/ecjegdjmmgdbimgl_norm.h deleted file mode 100644 index 6c4946f6f3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ecjegdjmmgdbimgl_norm.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: ecjegdjmmgdbimgl_norm.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Mon Mar 6 17:13:18 2017 -// -#ifndef SHARE_ecjegdjmmgdbimgl_norm -#define SHARE_ecjegdjmmgdbimgl_norm -#include "rtwtypes.h" - -extern real32_T ecjegdjmmgdbimgl_norm(const real32_T x[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ekfccjmgknopmoph_svd.cpp b/gnc/matlab/code_generation/sharedutils/ekfccjmgknopmoph_svd.cpp deleted file mode 100644 index 2d14c6d15f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ekfccjmgknopmoph_svd.cpp +++ /dev/null @@ -1,468 +0,0 @@ -// -// File: ekfccjmgknopmoph_svd.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include -#include -#include "bimohlfkbaaidbai_xscal.h" -#include "cbimpphlphlnekfc_xswap.h" -#include "ekfkdjmomglnngln_xdotc.h" -#include "fkngjekfaimgmohd_xaxpy.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "imopmohdaaaimgln_xrot.h" -#include "knohlnohphdbkfcj_xnrm2.h" -#include "knoplnopecjmpphd_xaxpy.h" -#include "lfcjdbieaimomoph_xdotc.h" -#include "mgdjglnoimohglno_xaxpy.h" -#include "nglndjmgaimophdj_xswap.h" -#include "nohlkfkngdjmkfcb_xrot.h" -#include "ohlfeknojmopaimg_xscal.h" -#include "opphjmohhlnodjmo_xaxpy.h" -#include "ppppfkfkiekfimop_xnrm2.h" -#include "rt_nonfinite.h" -#include "ekfccjmgknopmoph_svd.h" - -// Function for MATLAB Function: '/MATLAB Function' -void ekfccjmgknopmoph_svd(const real32_T A[72], real32_T U[72], real32_T S[36], - real32_T V[36]) -{ - real32_T b_A[72]; - real32_T s[6]; - real32_T e[6]; - real32_T work[12]; - real32_T Vf[36]; - int32_T q; - boolean_T apply_transform; - int32_T iter; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - int32_T j_ii; - real32_T varargin_1[5]; - int32_T i; - boolean_T exitg1; - boolean_T exitg2; - int32_T exitg3; - memcpy(&b_A[0], &A[0], (uint32_T)(72U * sizeof(real32_T))); - for (i = 0; i < 6; i++) { - s[i] = 0.0F; - e[i] = 0.0F; - } - - for (i = 0; i < 12; i++) { - work[i] = 0.0F; - } - - memset(&U[0], 0, (uint32_T)(72U * sizeof(real32_T))); - memset(&Vf[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (i = 0; i < 6; i++) { - iter = (int32_T)((int32_T)(12 * i) + i); - apply_transform = false; - snorm = ppppfkfkiekfimop_xnrm2((int32_T)(12 - i), b_A, (int32_T)(iter + 1)); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[iter] < 0.0F) { - s[i] = -snorm; - } else { - s[i] = snorm; - } - - if ((real32_T)fabs((real_T)s[i]) >= 9.86076132E-32F) { - snorm = 1.0F / s[i]; - q = (int32_T)((int32_T)(iter - i) + 12); - for (qs = iter; (int32_T)(qs + 1) <= q; qs++) { - b_A[qs] *= snorm; - } - } else { - q = (int32_T)((int32_T)(iter - i) + 12); - for (qs = iter; (int32_T)(qs + 1) <= q; qs++) { - b_A[qs] /= s[i]; - } - } - - b_A[iter]++; - s[i] = -s[i]; - } else { - s[i] = 0.0F; - } - - for (q = (int32_T)(i + 1); (int32_T)(q + 1) < 7; q++) { - qs = (int32_T)((int32_T)(12 * q) + i); - if (apply_transform) { - fkngjekfaimgmohd_xaxpy((int32_T)(12 - i), -(ekfkdjmomglnngln_xdotc - ((int32_T)(12 - i), b_A, (int32_T)(iter + 1), b_A, (int32_T)(qs + 1)) / - b_A[(int32_T)(i + (int32_T)(12 * i))]), (int32_T)(iter + 1), b_A, - (int32_T)(qs + 1)); - } - - e[q] = b_A[qs]; - } - - for (iter = i; (int32_T)(iter + 1) < 13; iter++) { - U[(int32_T)(iter + (int32_T)(12 * i))] = b_A[(int32_T)((int32_T)(12 * i) + - iter)]; - } - - if ((int32_T)(i + 1) <= 4) { - snorm = knohlnohphdbkfcj_xnrm2((int32_T)(5 - i), e, (int32_T)(i + 2)); - if (snorm == 0.0F) { - e[i] = 0.0F; - } else { - if (e[(int32_T)(i + 1)] < 0.0F) { - e[i] = -snorm; - } else { - e[i] = snorm; - } - - snorm = e[i]; - if ((real32_T)fabs((real_T)e[i]) >= 9.86076132E-32F) { - snorm = 1.0F / e[i]; - for (iter = (int32_T)(i + 1); (int32_T)(iter + 1) < 7; iter++) { - e[iter] *= snorm; - } - } else { - for (iter = (int32_T)(i + 1); (int32_T)(iter + 1) < 7; iter++) { - e[iter] /= snorm; - } - } - - e[(int32_T)(i + 1)]++; - e[i] = -e[i]; - for (iter = (int32_T)(i + 1); (int32_T)(iter + 1) < 13; iter++) { - work[iter] = 0.0F; - } - - for (iter = (int32_T)(i + 1); (int32_T)(iter + 1) < 7; iter++) { - mgdjglnoimohglno_xaxpy((int32_T)(11 - i), e[iter], b_A, (int32_T) - ((int32_T)(i + (int32_T)(12 * iter)) + 2), work, (int32_T)(i + 2)); - } - - for (iter = (int32_T)(i + 1); (int32_T)(iter + 1) < 7; iter++) { - opphjmohhlnodjmo_xaxpy((int32_T)(11 - i), -e[iter] / e[(int32_T)(i + 1)], - work, (int32_T)(i + 2), b_A, (int32_T)((int32_T)(i + (int32_T)(12 * - iter)) + 2)); - } - } - - for (iter = (int32_T)(i + 1); (int32_T)(iter + 1) < 7; iter++) { - Vf[(int32_T)(iter + (int32_T)(6 * i))] = e[iter]; - } - } - } - - i = 4; - e[4] = b_A[64]; - e[5] = 0.0F; - for (q = 5; q >= 0; q += -1) { - iter = (int32_T)((int32_T)(12 * q) + q); - if (s[q] != 0.0F) { - for (kase = (int32_T)(q + 1); (int32_T)(kase + 1) < 7; kase++) { - qs = (int32_T)((int32_T)((int32_T)(12 * kase) + q) + 1); - fkngjekfaimgmohd_xaxpy((int32_T)(12 - q), -(ekfkdjmomglnngln_xdotc - ((int32_T)(12 - q), U, (int32_T)(iter + 1), U, qs) / U[iter]), - (int32_T)(iter + 1), U, qs); - } - - for (qs = q; (int32_T)(qs + 1) < 13; qs++) { - U[(int32_T)(qs + (int32_T)(12 * q))] = -U[(int32_T)((int32_T)(12 * q) + - qs)]; - } - - U[iter]++; - for (iter = 1; iter <= q; iter++) { - U[(int32_T)((int32_T)(iter + (int32_T)(12 * q)) - 1)] = 0.0F; - } - } else { - for (qs = 0; qs < 12; qs++) { - U[(int32_T)(qs + (int32_T)(12 * q))] = 0.0F; - } - - U[iter] = 1.0F; - } - } - - for (iter = 5; iter >= 0; iter += -1) { - if (((int32_T)(iter + 1) <= 4) && (e[iter] != 0.0F)) { - q = (int32_T)((int32_T)((int32_T)(6 * iter) + iter) + 2); - for (qs = (int32_T)(iter + 1); (int32_T)(qs + 1) < 7; qs++) { - kase = (int32_T)((int32_T)((int32_T)(6 * qs) + iter) + 2); - knoplnopecjmpphd_xaxpy((int32_T)(5 - iter), -(lfcjdbieaimomoph_xdotc - ((int32_T)(5 - iter), Vf, q, Vf, kase) / Vf[(int32_T)(q - 1)]), q, Vf, - kase); - } - } - - for (q = 0; q < 6; q++) { - Vf[(int32_T)(q + (int32_T)(6 * iter))] = 0.0F; - } - - Vf[(int32_T)(iter + (int32_T)(6 * iter))] = 1.0F; - } - - for (iter = 0; iter < 6; iter++) { - ztest = e[iter]; - if (s[iter] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[iter]); - snorm = s[iter] / ztest0; - s[iter] = ztest0; - if ((int32_T)(iter + 1) < 6) { - ztest = e[iter] / snorm; - } - - bimohlfkbaaidbai_xscal(snorm, U, (int32_T)(1 + (int32_T)(12 * iter))); - } - - if (((int32_T)(iter + 1) < 6) && (ztest != 0.0F)) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[(int32_T)(iter + 1)] *= snorm; - ohlfeknojmopaimg_xscal(snorm, Vf, (int32_T)(1 + (int32_T)(6 * (int32_T) - (iter + 1)))); - } - - e[iter] = ztest; - } - - iter = 0; - snorm = 0.0F; - for (q = 0; q < 6; q++) { - ztest0 = (real32_T)fabs((real_T)s[q]); - ztest = (real32_T)fabs((real_T)e[q]); - if ((ztest0 >= ztest) || rtIsNaNF(ztest)) { - ztest = ztest0; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - } - - while (((int32_T)(i + 2) > 0) && (!(iter >= 75))) { - kase = (int32_T)(i + 1); - do { - exitg3 = 0; - q = kase; - if (kase == 0) { - exitg3 = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e[(int32_T)(kase - 1)]); - if ((ztest0 <= ((real32_T)fabs((real_T)s[(int32_T)(kase - 1)]) + - (real32_T)fabs((real_T)s[kase])) * 1.1920929E-7F) || - ((ztest0 <= 9.86076132E-32F) || ((iter > 20) && (ztest0 <= - 1.1920929E-7F * snorm)))) { - e[(int32_T)(kase - 1)] = 0.0F; - exitg3 = 1; - } else { - kase--; - } - } - } while (exitg3 == 0); - - if ((int32_T)(i + 1) == kase) { - kase = 4; - } else { - qs = (int32_T)(i + 2); - j_ii = (int32_T)(i + 2); - exitg2 = false; - while ((!exitg2) && (j_ii >= kase)) { - qs = j_ii; - if (j_ii == kase) { - exitg2 = true; - } else { - ztest0 = 0.0F; - if (j_ii < (int32_T)(i + 2)) { - ztest0 = (real32_T)fabs((real_T)e[(int32_T)(j_ii - 1)]); - } - - if (j_ii > (int32_T)(kase + 1)) { - ztest0 += (real32_T)fabs((real_T)e[(int32_T)(j_ii - 2)]); - } - - ztest = (real32_T)fabs((real_T)s[(int32_T)(j_ii - 1)]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s[(int32_T)(j_ii - 1)] = 0.0F; - exitg2 = true; - } else { - j_ii--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if ((int32_T)(i + 2) == qs) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[i]; - e[i] = 0.0F; - for (qs = i; (int32_T)(qs + 1) >= (int32_T)(q + 1); qs--) { - ztest = s[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[qs] = ztest; - if ((int32_T)(qs + 1) > (int32_T)(q + 1)) { - ztest0 = e[(int32_T)(qs - 1)] * -smm1; - e[(int32_T)(qs - 1)] *= sqds; - } - - imopmohdaaaimgln_xrot(Vf, (int32_T)(1 + (int32_T)(6 * qs)), (int32_T)(1 - + (int32_T)(6 * (int32_T)(i + 1))), sqds, smm1); - } - break; - - case 2: - ztest0 = e[(int32_T)(q - 1)]; - e[(int32_T)(q - 1)] = 0.0F; - for (qs = q; (int32_T)(qs + 1) <= (int32_T)(i + 2); qs++) { - ztest = s[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[qs] = ztest; - ztest0 = -smm1 * e[qs]; - e[qs] *= sqds; - nohlkfkngdjmkfcb_xrot(U, (int32_T)(1 + (int32_T)(12 * qs)), (int32_T)(1 - + (int32_T)(12 * (int32_T)(q - 1))), sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s[(int32_T)(i + 1)]); - varargin_1[1] = (real32_T)fabs((real_T)s[i]); - varargin_1[2] = (real32_T)fabs((real_T)e[i]); - varargin_1[3] = (real32_T)fabs((real_T)s[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - qs = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - kase = 2; - exitg1 = false; - while ((!exitg1) && (kase < 6)) { - qs = kase; - if (!rtIsNaNF(varargin_1[(int32_T)(kase - 1)])) { - ztest = varargin_1[(int32_T)(kase - 1)]; - exitg1 = true; - } else { - kase++; - } - } - } - - if (qs < 5) { - while ((int32_T)(qs + 1) < 6) { - if (varargin_1[qs] > ztest) { - ztest = varargin_1[qs]; - } - - qs++; - } - } - - ztest0 = s[(int32_T)(i + 1)] / ztest; - smm1 = s[i] / ztest; - emm1 = e[i] / ztest; - sqds = s[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - for (qs = (int32_T)(q + 1); qs <= (int32_T)(i + 1); qs++) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - if (qs > (int32_T)(q + 1)) { - e[(int32_T)(qs - 2)] = ztest0; - } - - ztest0 = s[(int32_T)(qs - 1)] * sqds + e[(int32_T)(qs - 1)] * smm1; - e[(int32_T)(qs - 1)] = e[(int32_T)(qs - 1)] * sqds - s[(int32_T)(qs - 1)] - * smm1; - ztest = smm1 * s[qs]; - s[qs] *= sqds; - imopmohdaaaimgln_xrot(Vf, (int32_T)(1 + (int32_T)(6 * (int32_T)(qs - 1))), - (int32_T)(1 + (int32_T)(6 * qs)), sqds, smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s[(int32_T)(qs - 1)] = ztest0; - ztest0 = e[(int32_T)(qs - 1)] * sqds + smm1 * s[qs]; - s[qs] = e[(int32_T)(qs - 1)] * -smm1 + sqds * s[qs]; - ztest = smm1 * e[qs]; - e[qs] *= sqds; - nohlkfkngdjmkfcb_xrot(U, (int32_T)(1 + (int32_T)(12 * (int32_T)(qs - 1))), - (int32_T)(1 + (int32_T)(12 * qs)), sqds, smm1); - } - - e[i] = ztest0; - iter++; - break; - - default: - if (s[q] < 0.0F) { - s[q] = -s[q]; - ohlfeknojmopaimg_xscal(-1.0F, Vf, (int32_T)(1 + (int32_T)(6 * q))); - } - - iter = (int32_T)(q + 1); - while (((int32_T)(q + 1) < 6) && (s[q] < s[iter])) { - ztest0 = s[q]; - s[q] = s[iter]; - s[iter] = ztest0; - cbimpphlphlnekfc_xswap(Vf, (int32_T)(1 + (int32_T)(6 * q)), (int32_T)(1 - + (int32_T)(6 * (int32_T)(q + 1)))); - nglndjmgaimophdj_xswap(U, (int32_T)(1 + (int32_T)(12 * q)), (int32_T)(1 - + (int32_T)(12 * (int32_T)(q + 1)))); - q = iter; - iter++; - } - - iter = 0; - i--; - break; - } - } - - for (i = 0; i < 6; i++) { - e[i] = s[i]; - for (iter = 0; iter < 6; iter++) { - V[(int32_T)(iter + (int32_T)(6 * i))] = Vf[(int32_T)((int32_T)(6 * i) + - iter)]; - } - } - - memset(&S[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (i = 0; i < 6; i++) { - S[(int32_T)(i + (int32_T)(6 * i))] = e[i]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ekfccjmgknopmoph_svd.h b/gnc/matlab/code_generation/sharedutils/ekfccjmgknopmoph_svd.h deleted file mode 100644 index 5e7d925fab..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ekfccjmgknopmoph_svd.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ekfccjmgknopmoph_svd.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_ekfccjmgknopmoph_svd -#define SHARE_ekfccjmgknopmoph_svd -#include "rtwtypes.h" - -extern void ekfccjmgknopmoph_svd(const real32_T A[72], real32_T U[72], real32_T - S[36], real32_T V[36]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ekfcmgdbdjekfkfk_permute.cpp b/gnc/matlab/code_generation/sharedutils/ekfcmgdbdjekfkfk_permute.cpp deleted file mode 100644 index 0a4b8910aa..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ekfcmgdbdjekfkfk_permute.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// -// File: ekfcmgdbdjekfkfk_permute.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "ekfcmgdbdjekfkfk_permute.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void ekfcmgdbdjekfkfk_permute(const real32_T a_data[], const int32_T a_sizes[3], - real32_T b_data[], int32_T b_sizes[3]) -{ - int8_T outsz[3]; - int32_T iwork[3]; - int32_T isrc; - int32_T c_k; - boolean_T c_b; - int32_T plast; - static const int8_T e[3] = { 3, 1, 2 }; - - boolean_T exitg2; - boolean_T guard1; - int8_T b_a_idx_0; - int32_T inc_idx_0; - b_a_idx_0 = (int8_T)a_sizes[0]; - outsz[0] = 16; - outsz[1] = b_a_idx_0; - outsz[2] = 2; - b_sizes[0] = 16; - b_sizes[1] = b_a_idx_0; - b_sizes[2] = 2; - c_b = true; - if (!(a_sizes[0] == 0)) { - plast = 0; - isrc = 0; - exitg2 = false; - while ((!exitg2) && (isrc + 1 < 4)) { - guard1 = false; - if (a_sizes[e[isrc] - 1] != 1) { - if (plast > e[isrc]) { - c_b = false; - exitg2 = true; - } else { - plast = e[isrc]; - guard1 = true; - } - } else { - guard1 = true; - } - - if (guard1) { - isrc++; - } - } - } - - if (c_b) { - plast = a_sizes[0] << 5; - for (isrc = 0; isrc + 1 <= plast; isrc++) { - b_data[isrc] = a_data[isrc]; - } - } else { - inc_idx_0 = b_a_idx_0 * (int8_T)a_sizes[1]; - iwork[0] = 0; - iwork[1] = 0; - iwork[2] = 0; - plast = 0; - do { - isrc = iwork[2] * b_a_idx_0 + iwork[1]; - for (c_k = 0; c_k < 16; c_k++) { - b_data[plast] = a_data[isrc]; - plast++; - isrc += inc_idx_0; - } - - isrc = 1; - do { - c_k = 0; - iwork[isrc]++; - if (iwork[isrc] < outsz[isrc]) { - c_k = 2; - } else if (isrc + 1 == 3) { - c_k = 1; - } else { - iwork[1] = 0; - isrc = 2; - } - } while (c_k == 0); - } while (!(c_k == 1)); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ekfcmgdbdjekfkfk_permute.h b/gnc/matlab/code_generation/sharedutils/ekfcmgdbdjekfkfk_permute.h deleted file mode 100644 index 72e6deb4c2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ekfcmgdbdjekfkfk_permute.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ekfcmgdbdjekfkfk_permute.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_ekfcmgdbdjekfkfk_permute -#define SHARE_ekfcmgdbdjekfkfk_permute -#include "rtwtypes.h" - -extern void ekfcmgdbdjekfkfk_permute(const real32_T a_data[], const int32_T - a_sizes[3], real32_T b_data[], int32_T b_sizes[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ekfkdjmomglnngln_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/ekfkdjmomglnngln_xdotc.cpp deleted file mode 100644 index aa809297f0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ekfkdjmomglnngln_xdotc.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: ekfkdjmomglnngln_xdotc.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "ekfkdjmomglnngln_xdotc.h" - -// Function for MATLAB Function: '/MATLAB Function' -real32_T ekfkdjmomglnngln_xdotc(int32_T n, const real32_T x[72], int32_T ix0, - const real32_T y[72], int32_T iy0) -{ - real32_T d; - int32_T ix; - int32_T iy; - int32_T k; - d = 0.0F; - if (!(n < 1)) { - ix = ix0; - iy = iy0; - for (k = 1; k <= n; k++) { - d += x[(int32_T)(ix - 1)] * y[(int32_T)(iy - 1)]; - ix++; - iy++; - } - } - - return d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ekfkdjmomglnngln_xdotc.h b/gnc/matlab/code_generation/sharedutils/ekfkdjmomglnngln_xdotc.h deleted file mode 100644 index 5f817c7d6e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ekfkdjmomglnngln_xdotc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ekfkdjmomglnngln_xdotc.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_ekfkdjmomglnngln_xdotc -#define SHARE_ekfkdjmomglnngln_xdotc -#include "rtwtypes.h" - -extern real32_T ekfkdjmomglnngln_xdotc(int32_T n, const real32_T x[72], int32_T - ix0, const real32_T y[72], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ekfklfkfohdjaimg_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/ekfklfkfohdjaimg_xaxpy.cpp deleted file mode 100644 index f2fb95ed4b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ekfklfkfohdjaimg_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: ekfklfkfohdjaimg_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ekfklfkfohdjaimg_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void ekfklfkfohdjaimg_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T y[9], - int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * y[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ekfklfkfohdjaimg_xaxpy.h b/gnc/matlab/code_generation/sharedutils/ekfklfkfohdjaimg_xaxpy.h deleted file mode 100644 index bb3df25ade..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ekfklfkfohdjaimg_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ekfklfkfohdjaimg_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ekfklfkfohdjaimg_xaxpy -#define SHARE_ekfklfkfohdjaimg_xaxpy -#include "rtwtypes.h" - -extern void ekfklfkfohdjaimg_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T - y[9], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/eknofcjenophiecj_xswap.cpp b/gnc/matlab/code_generation/sharedutils/eknofcjenophiecj_xswap.cpp deleted file mode 100644 index 18d69a2147..0000000000 --- a/gnc/matlab/code_generation/sharedutils/eknofcjenophiecj_xswap.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: eknofcjenophiecj_xswap.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "eknofcjenophiecj_xswap.h" - -// Function for MATLAB Function: '/MATLAB Function' -void eknofcjenophiecj_xswap(real32_T x[36], int32_T ix0, int32_T iy0) -{ - int32_T ix; - int32_T iy; - real32_T temp; - int32_T k; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k < 12; k++) { - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; - ix++; - iy++; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/eknofcjenophiecj_xswap.h b/gnc/matlab/code_generation/sharedutils/eknofcjenophiecj_xswap.h deleted file mode 100644 index 6be0fc6524..0000000000 --- a/gnc/matlab/code_generation/sharedutils/eknofcjenophiecj_xswap.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: eknofcjenophiecj_xswap.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_eknofcjenophiecj_xswap -#define SHARE_eknofcjenophiecj_xswap -#include "rtwtypes.h" - -extern void eknofcjenophiecj_xswap(real32_T x[36], int32_T ix0, int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/eknomophmophdjmg_xgeqrf.cpp b/gnc/matlab/code_generation/sharedutils/eknomophmophdjmg_xgeqrf.cpp deleted file mode 100644 index 95037caf38..0000000000 --- a/gnc/matlab/code_generation/sharedutils/eknomophmophdjmg_xgeqrf.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// -// File: eknomophmophdjmg_xgeqrf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "fcbagdbapphliecj_xzlarfg.h" -#include "jmgdecjelfkfhdba_xzlarf.h" -#include "eknomophmophdjmg_xgeqrf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void eknomophmophdjmg_xgeqrf(real32_T A_data[], int32_T A_sizes[2], real32_T - tau_data[], int32_T *tau_sizes) -{ - int32_T m; - int32_T n; - int32_T mn; - int32_T i_i; - int32_T mmi; - real32_T b; - real32_T b_atmp; - int32_T loop_ub; - real32_T work_data[150]; - int32_T work_sizes; - uint8_T c_idx_0; - m = A_sizes[0]; - n = A_sizes[1]; - work_sizes = A_sizes[0]; - mn = A_sizes[1]; - if (work_sizes <= mn) { - mn = work_sizes; - } - - *tau_sizes = (uint8_T)mn; - if (!((A_sizes[0] == 0) || (A_sizes[1] == 0))) { - c_idx_0 = (uint8_T)A_sizes[1]; - work_sizes = c_idx_0; - loop_ub = c_idx_0; - for (i_i = 0; i_i < loop_ub; i_i++) { - work_data[i_i] = 0.0F; - } - - for (loop_ub = 0; loop_ub + 1 <= mn; loop_ub++) { - i_i = loop_ub * m + loop_ub; - mmi = m - loop_ub; - if (loop_ub + 1 < m) { - b_atmp = A_data[i_i]; - b = fcbagdbapphliecj_xzlarfg(mmi, &b_atmp, A_data, i_i + 2); - tau_data[loop_ub] = b; - A_data[i_i] = b_atmp; - } else { - tau_data[loop_ub] = 0.0F; - } - - if (loop_ub + 1 < n) { - b_atmp = A_data[i_i]; - A_data[i_i] = 1.0F; - jmgdecjelfkfhdba_xzlarf(mmi, (n - loop_ub) - 1, i_i + 1, - tau_data[loop_ub], A_data, (loop_ub + (loop_ub + 1) * m) + 1, m, - work_data); - A_data[i_i] = b_atmp; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/eknomophmophdjmg_xgeqrf.h b/gnc/matlab/code_generation/sharedutils/eknomophmophdjmg_xgeqrf.h deleted file mode 100644 index acd02eba56..0000000000 --- a/gnc/matlab/code_generation/sharedutils/eknomophmophdjmg_xgeqrf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: eknomophmophdjmg_xgeqrf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_eknomophmophdjmg_xgeqrf -#define SHARE_eknomophmophdjmg_xgeqrf -#include "rtwtypes.h" - -extern void eknomophmophdjmg_xgeqrf(real32_T A_data[], int32_T A_sizes[2], - real32_T tau_data[], int32_T *tau_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/eknoopphfcjegdbi_xgeqrf.cpp b/gnc/matlab/code_generation/sharedutils/eknoopphfcjegdbi_xgeqrf.cpp deleted file mode 100644 index a992243ade..0000000000 --- a/gnc/matlab/code_generation/sharedutils/eknoopphfcjegdbi_xgeqrf.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// -// File: eknoopphfcjegdbi_xgeqrf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "dbaidjecmohdphln_xzlarf.h" -#include "ecbilfcbnglnimoh_xzlarfg.h" -#include "eknoopphfcjegdbi_xgeqrf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void eknoopphfcjegdbi_xgeqrf(real32_T A_data[], int32_T A_sizes[2], real32_T - tau_data[], int32_T *tau_sizes) -{ - int32_T m; - int32_T mn; - real32_T work[6]; - int32_T i_i; - int32_T mmi; - real32_T b; - real32_T b_atmp; - int32_T i; - m = A_sizes[0]; - mn = A_sizes[0]; - if (!(mn <= 6)) { - mn = 6; - } - - *tau_sizes = (int8_T)mn; - if (A_sizes[0] != 0) { - for (i = 0; i < 6; i++) { - work[i] = 0.0F; - } - - for (i = 0; i + 1 <= mn; i++) { - i_i = i * m + i; - mmi = m - i; - if (i + 1 < m) { - b_atmp = A_data[i_i]; - b = ecbilfcbnglnimoh_xzlarfg(mmi, &b_atmp, A_data, i_i + 2); - tau_data[i] = b; - A_data[i_i] = b_atmp; - } else { - tau_data[i] = 0.0F; - } - - if (i + 1 < 6) { - b_atmp = A_data[i_i]; - A_data[i_i] = 1.0F; - dbaidjecmohdphln_xzlarf(mmi, 5 - i, i_i + 1, tau_data[i], A_data, (i + - (i + 1) * m) + 1, m, work); - A_data[i_i] = b_atmp; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/eknoopphfcjegdbi_xgeqrf.h b/gnc/matlab/code_generation/sharedutils/eknoopphfcjegdbi_xgeqrf.h deleted file mode 100644 index c16d8f7a31..0000000000 --- a/gnc/matlab/code_generation/sharedutils/eknoopphfcjegdbi_xgeqrf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: eknoopphfcjegdbi_xgeqrf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_eknoopphfcjegdbi_xgeqrf -#define SHARE_eknoopphfcjegdbi_xgeqrf -#include "rtwtypes.h" - -extern void eknoopphfcjegdbi_xgeqrf(real32_T A_data[], int32_T A_sizes[2], - real32_T tau_data[], int32_T *tau_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcbadjmgohlnpphl_rows_differ.cpp b/gnc/matlab/code_generation/sharedutils/fcbadjmgohlnpphl_rows_differ.cpp deleted file mode 100644 index f9e3873334..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcbadjmgohlnpphl_rows_differ.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// -// File: fcbadjmgohlnpphl_rows_differ.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "fcbadjmgohlnpphl_rows_differ.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T fcbadjmgohlnpphl_rows_differ(const real32_T b_data[], const int32_T - b_sizes[2], int32_T k0, int32_T k) -{ - boolean_T p; - int32_T j; - boolean_T b_p; - real32_T absxk; - int32_T exponent; - boolean_T exitg1; - p = false; - j = 0; - exitg1 = false; - while ((!exitg1) && (j < 2)) { - absxk = (real32_T)fabs((real_T)(b_data[(int32_T)((int32_T)((int32_T) - (b_sizes[0] * j) + k) - 1)] / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, (int32_T)(exponent - 24)); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(b_data[(int32_T)((int32_T)((int32_T)(b_sizes[0] - * j) + k) - 1)] - b_data[(int32_T)((int32_T)((int32_T)(b_sizes[0] * - j) + k0) - 1)])) < absxk) || (rtIsInfF(b_data[(int32_T)((int32_T) - ((int32_T)(b_sizes[0] * j) + k0) - 1)]) && rtIsInfF(b_data[(int32_T) - ((int32_T)((int32_T)(b_sizes[0] * j) + k) - 1)]) && ((b_data[(int32_T) - ((int32_T)((int32_T)(b_sizes[0] * j) + k0) - 1)] > 0.0F) == (b_data - [(int32_T)((int32_T)((int32_T)(b_sizes[0] * j) + k) - 1)] > 0.0F)))) - { - b_p = true; - } else { - b_p = false; - } - - if (!b_p) { - p = true; - exitg1 = true; - } else { - j++; - } - } - - return p; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcbadjmgohlnpphl_rows_differ.h b/gnc/matlab/code_generation/sharedutils/fcbadjmgohlnpphl_rows_differ.h deleted file mode 100644 index 11c6eb831f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcbadjmgohlnpphl_rows_differ.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fcbadjmgohlnpphl_rows_differ.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_fcbadjmgohlnpphl_rows_differ -#define SHARE_fcbadjmgohlnpphl_rows_differ -#include "rtwtypes.h" - -extern boolean_T fcbadjmgohlnpphl_rows_differ(const real32_T b_data[], const - int32_T b_sizes[2], int32_T k0, int32_T k); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcbagdbapphliecj_xzlarfg.cpp b/gnc/matlab/code_generation/sharedutils/fcbagdbapphliecj_xzlarfg.cpp deleted file mode 100644 index 04e0c0429a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcbagdbapphliecj_xzlarfg.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -// File: fcbagdbapphliecj_xzlarfg.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "dbielngdhlfkohdb_xnrm2.h" -#include "rt_hypotf_snf.h" -#include "fcbagdbapphliecj_xzlarfg.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T fcbagdbapphliecj_xzlarfg(int32_T n, real32_T *alpha1, real32_T x_data[], - int32_T ix0) -{ - real32_T tau; - real32_T xnorm; - int32_T knt; - int32_T b_k; - int32_T c_k; - tau = 0.0F; - if (!(n <= 0)) { - xnorm = dbielngdhlfkohdb_xnrm2((int32_T)(n - 1), x_data, ix0); - if (xnorm != 0.0F) { - xnorm = rt_hypotf_snf(*alpha1, xnorm); - if (*alpha1 >= 0.0F) { - xnorm = -xnorm; - } - - if ((real32_T)fabs((real_T)xnorm) < 9.86076132E-32F) { - knt = 0; - do { - knt++; - b_k = (int32_T)((int32_T)(ix0 + n) - 2); - for (c_k = ix0; c_k <= b_k; c_k++) { - x_data[(int32_T)(c_k - 1)] *= 1.01412048E+31F; - } - - xnorm *= 1.01412048E+31F; - *alpha1 *= 1.01412048E+31F; - } while (!((real32_T)fabs((real_T)xnorm) >= 9.86076132E-32F)); - - xnorm = rt_hypotf_snf(*alpha1, dbielngdhlfkohdb_xnrm2((int32_T)(n - 1), - x_data, ix0)); - if (*alpha1 >= 0.0F) { - xnorm = -xnorm; - } - - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - b_k = (int32_T)((int32_T)(ix0 + n) - 2); - for (c_k = ix0; c_k <= b_k; c_k++) { - x_data[(int32_T)(c_k - 1)] *= *alpha1; - } - - for (b_k = 1; b_k <= knt; b_k++) { - xnorm *= 9.86076132E-32F; - } - - *alpha1 = xnorm; - } else { - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - knt = (int32_T)((int32_T)(ix0 + n) - 2); - for (b_k = ix0; b_k <= knt; b_k++) { - x_data[(int32_T)(b_k - 1)] *= *alpha1; - } - - *alpha1 = xnorm; - } - } - } - - return tau; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcbagdbapphliecj_xzlarfg.h b/gnc/matlab/code_generation/sharedutils/fcbagdbapphliecj_xzlarfg.h deleted file mode 100644 index 5bf9aea53d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcbagdbapphliecj_xzlarfg.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fcbagdbapphliecj_xzlarfg.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_fcbagdbapphliecj_xzlarfg -#define SHARE_fcbagdbapphliecj_xzlarfg -#include "rtwtypes.h" - -extern real32_T fcbagdbapphliecj_xzlarfg(int32_T n, real32_T *alpha1, real32_T - x_data[], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcbijmgdglngglfc_xgeqrf.cpp b/gnc/matlab/code_generation/sharedutils/fcbijmgdglngglfc_xgeqrf.cpp deleted file mode 100644 index 74178750e9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcbijmgdglngglfc_xgeqrf.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// -// File: fcbijmgdglngglfc_xgeqrf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "aaiehlfccbimimgd_xzlarf.h" -#include "jecjfcjmbaaaaiek_xzlarfg.h" -#include "fcbijmgdglngglfc_xgeqrf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void fcbijmgdglngglfc_xgeqrf(real32_T A_data[], int32_T A_sizes[2], real32_T - tau_data[], int32_T *tau_sizes) -{ - int32_T m; - int32_T n; - int32_T mn; - int32_T i_i; - int32_T mmi; - real32_T b; - real32_T b_atmp; - int32_T loop_ub; - real32_T work_data[100]; - int32_T work_sizes; - int8_T c_idx_0; - m = A_sizes[0]; - n = A_sizes[1]; - work_sizes = A_sizes[0]; - mn = A_sizes[1]; - if (work_sizes <= mn) { - mn = work_sizes; - } - - *tau_sizes = (int32_T)(int8_T)mn; - if (!((A_sizes[0] == 0) || (A_sizes[1] == 0))) { - c_idx_0 = (int8_T)A_sizes[1]; - work_sizes = (int32_T)c_idx_0; - loop_ub = (int32_T)c_idx_0; - for (i_i = 0; i_i <= (int32_T)(loop_ub - 1); i_i++) { - work_data[i_i] = 0.0F; - } - - for (loop_ub = 0; (int32_T)(loop_ub + 1) <= mn; loop_ub++) { - i_i = (int32_T)((int32_T)(loop_ub * m) + loop_ub); - mmi = (int32_T)(m - loop_ub); - if ((int32_T)(loop_ub + 1) < m) { - b_atmp = A_data[i_i]; - b = jecjfcjmbaaaaiek_xzlarfg(mmi, &b_atmp, A_data, (int32_T)(i_i + 2)); - tau_data[loop_ub] = b; - A_data[i_i] = b_atmp; - } else { - tau_data[loop_ub] = 0.0F; - } - - if ((int32_T)(loop_ub + 1) < n) { - b_atmp = A_data[i_i]; - A_data[i_i] = 1.0F; - aaiehlfccbimimgd_xzlarf(mmi, (int32_T)((int32_T)(n - loop_ub) - 1), - (int32_T)(i_i + 1), tau_data[loop_ub], A_data, (int32_T)((int32_T) - (loop_ub + (int32_T)((int32_T)(loop_ub + 1) * m)) + 1), m, work_data); - A_data[i_i] = b_atmp; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcbijmgdglngglfc_xgeqrf.h b/gnc/matlab/code_generation/sharedutils/fcbijmgdglngglfc_xgeqrf.h deleted file mode 100644 index bc773b15e0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcbijmgdglngglfc_xgeqrf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fcbijmgdglngglfc_xgeqrf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_fcbijmgdglngglfc_xgeqrf -#define SHARE_fcbijmgdglngglfc_xgeqrf -#include "rtwtypes.h" - -extern void fcbijmgdglngglfc_xgeqrf(real32_T A_data[], int32_T A_sizes[2], - real32_T tau_data[], int32_T *tau_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcbiohlnbimolnop_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/fcbiohlnbimolnop_xnrm2.cpp deleted file mode 100644 index 6319c61a82..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcbiohlnbimolnop_xnrm2.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: fcbiohlnbimolnop_xnrm2.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include -#include "fcbiohlnbimolnop_xnrm2.h" - -// Function for MATLAB Function: '/MATLAB Function' -real32_T fcbiohlnbimolnop_xnrm2(int32_T n, const real32_T x[3], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (!(n < 1)) { - if (n == 1) { - y = (real32_T)fabs((real_T)x[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcbiohlnbimolnop_xnrm2.h b/gnc/matlab/code_generation/sharedutils/fcbiohlnbimolnop_xnrm2.h deleted file mode 100644 index 72e0233f97..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcbiohlnbimolnop_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fcbiohlnbimolnop_xnrm2.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_fcbiohlnbimolnop_xnrm2 -#define SHARE_fcbiohlnbimolnop_xnrm2 -#include "rtwtypes.h" - -extern real32_T fcbiohlnbimolnop_xnrm2(int32_T n, const real32_T x[3], int32_T - ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcjebiekeknggdba_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/fcjebiekeknggdba_xdotc.cpp deleted file mode 100644 index a78dbb047d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcjebiekeknggdba_xdotc.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: fcjebiekeknggdba_xdotc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "fcjebiekeknggdba_xdotc.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T fcjebiekeknggdba_xdotc(int32_T n, const real32_T x_data[], int32_T ix0, - const real32_T y_data[], int32_T iy0) -{ - real32_T d; - int32_T ix; - int32_T iy; - int32_T k; - d = 0.0F; - if (!(n < 1)) { - ix = ix0; - iy = iy0; - for (k = 1; k <= n; k++) { - d += x_data[(int32_T)(ix - 1)] * y_data[(int32_T)(iy - 1)]; - ix++; - iy++; - } - } - - return d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcjebiekeknggdba_xdotc.h b/gnc/matlab/code_generation/sharedutils/fcjebiekeknggdba_xdotc.h deleted file mode 100644 index 5a9cda8688..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcjebiekeknggdba_xdotc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fcjebiekeknggdba_xdotc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_fcjebiekeknggdba_xdotc -#define SHARE_fcjebiekeknggdba_xdotc -#include "rtwtypes.h" - -extern real32_T fcjebiekeknggdba_xdotc(int32_T n, const real32_T x_data[], - int32_T ix0, const real32_T y_data[], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcjejmgdgdbicjek_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/fcjejmgdgdbicjek_xaxpy.cpp deleted file mode 100644 index 4240cc5e2d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcjejmgdgdbicjek_xaxpy.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: fcjejmgdgdbicjek_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "fcjejmgdgdbicjek_xaxpy.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void fcjejmgdgdbicjek_xaxpy(real32_T a, real32_T y[4]) -{ - if (!(a == 0.0F)) { - y[2] += a * y[0]; - y[3] += a * y[1]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcjejmgdgdbicjek_xaxpy.h b/gnc/matlab/code_generation/sharedutils/fcjejmgdgdbicjek_xaxpy.h deleted file mode 100644 index f788ae17ff..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcjejmgdgdbicjek_xaxpy.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: fcjejmgdgdbicjek_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_fcjejmgdgdbicjek_xaxpy -#define SHARE_fcjejmgdgdbicjek_xaxpy -#include "rtwtypes.h" - -extern void fcjejmgdgdbicjek_xaxpy(real32_T a, real32_T y[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcjmdjmgppphecje_qr.cpp b/gnc/matlab/code_generation/sharedutils/fcjmdjmgppphecje_qr.cpp deleted file mode 100644 index 8d310e2ad8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcjmdjmgppphecje_qr.cpp +++ /dev/null @@ -1,168 +0,0 @@ -// -// File: fcjmdjmgppphecje_qr.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "eknomophmophdjmg_xgeqrf.h" -#include "eknoopphfcjegdbi_xgeqrf.h" -#include "iecbaaaikfkfaaie_xzlarf.h" -#include "jmgdecjelfkfhdba_xzlarf.h" -#include "knglnoppglnoimoh_xscal.h" -#include "fcjmdjmgppphecje_qr.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void fcjmdjmgppphecje_qr(const real32_T A_data[], const int32_T A_sizes[2], - real32_T Q_data[], int32_T Q_sizes[2], real32_T R_data[], int32_T R_sizes[2]) -{ - int32_T m; - int32_T i_i; - int32_T itau; - real32_T work[6]; - int32_T iaii; - int32_T c; - int32_T i; - real32_T b_A_data[900]; - int32_T b_A_sizes[2]; - real32_T tau_data[6]; - real32_T b_tau_data[150]; - real32_T b_work_data[150]; - uint8_T b_idx_0; - m = A_sizes[0]; - b_idx_0 = (uint8_T)A_sizes[0]; - Q_sizes[0] = (uint8_T)A_sizes[0]; - Q_sizes[1] = (uint8_T)A_sizes[0]; - R_sizes[0] = A_sizes[0]; - R_sizes[1] = A_sizes[1]; - if (A_sizes[0] > 6) { - for (itau = 0; itau < 6; itau++) { - for (i_i = 0; i_i + 1 <= m; i_i++) { - Q_data[i_i + b_idx_0 * itau] = A_data[A_sizes[0] * itau + i_i]; - } - } - - for (itau = 7; itau <= m; itau++) { - for (i_i = 1; i_i <= m; i_i++) { - Q_data[(i_i + b_idx_0 * (itau - 1)) - 1] = 0.0F; - } - } - - eknomophmophdjmg_xgeqrf(Q_data, Q_sizes, b_tau_data, &itau); - for (itau = 0; itau < 6; itau++) { - for (i_i = 0; i_i + 1 <= itau + 1; i_i++) { - R_data[i_i + R_sizes[0] * itau] = Q_data[Q_sizes[0] * itau + i_i]; - } - - for (i_i = itau + 1; i_i + 1 <= m; i_i++) { - R_data[i_i + R_sizes[0] * itau] = 0.0F; - } - } - - for (itau = 6; itau < m; itau++) { - i_i = itau * m; - for (iaii = 0; iaii < m; iaii++) { - Q_data[i_i + iaii] = 0.0F; - } - - Q_data[i_i + itau] = 1.0F; - } - - itau = 5; - b_idx_0 = (uint8_T)Q_sizes[1]; - i = b_idx_0; - i_i = b_idx_0; - for (iaii = 0; iaii < i_i; iaii++) { - b_work_data[iaii] = 0.0F; - } - - for (i_i = 5; i_i >= 0; i_i += -1) { - iaii = i_i * m + i_i; - Q_data[iaii] = 1.0F; - jmgdecjelfkfhdba_xzlarf(m - i_i, (m - i_i) - 1, iaii + 1, b_tau_data[itau], - Q_data, (iaii + m) + 1, m, b_work_data); - knglnoppglnoimoh_xscal((m - i_i) - 1, -b_tau_data[itau], Q_data, iaii + 2); - Q_data[iaii] = 1.0F - b_tau_data[itau]; - for (c = 1; c <= i_i; c++) { - Q_data[iaii - c] = 0.0F; - } - - itau--; - } - } else { - b_A_sizes[0] = A_sizes[0]; - b_A_sizes[1] = 6; - i_i = A_sizes[0] * A_sizes[1]; - for (iaii = 0; iaii < i_i; iaii++) { - b_A_data[iaii] = A_data[iaii]; - } - - eknoopphfcjegdbi_xgeqrf(b_A_data, b_A_sizes, tau_data, &itau); - for (itau = 0; itau + 1 <= m; itau++) { - for (i_i = 0; i_i + 1 <= itau + 1; i_i++) { - R_data[i_i + R_sizes[0] * itau] = b_A_data[b_A_sizes[0] * itau + i_i]; - } - - for (i_i = itau + 1; i_i + 1 <= m; i_i++) { - R_data[i_i + R_sizes[0] * itau] = 0.0F; - } - } - - for (itau = A_sizes[0]; itau + 1 < 7; itau++) { - for (i_i = 0; i_i + 1 <= m; i_i++) { - R_data[i_i + R_sizes[0] * itau] = b_A_data[b_A_sizes[0] * itau + i_i]; - } - } - - if (!(A_sizes[0] < 1)) { - for (itau = A_sizes[0]; itau < m; itau++) { - i_i = itau * m; - for (iaii = 0; iaii < m; iaii++) { - b_A_data[i_i + iaii] = 0.0F; - } - - b_A_data[i_i + itau] = 1.0F; - } - - itau = A_sizes[0] - 1; - for (i = 0; i < 6; i++) { - work[i] = 0.0F; - } - - for (i_i = A_sizes[0]; i_i >= 1; i_i--) { - iaii = (i_i - 1) * m + i_i; - if (i_i < m) { - b_A_data[iaii - 1] = 1.0F; - iecbaaaikfkfaaie_xzlarf((m - i_i) + 1, m - i_i, iaii, tau_data[itau], - b_A_data, iaii + m, m, work); - c = (iaii + m) - i_i; - for (i = iaii; i + 1 <= c; i++) { - b_A_data[i] *= -tau_data[itau]; - } - } - - b_A_data[iaii - 1] = 1.0F - tau_data[itau]; - for (c = 1; c < i_i; c++) { - b_A_data[(iaii - c) - 1] = 0.0F; - } - - itau--; - } - } - - for (itau = 0; itau + 1 <= m; itau++) { - for (i_i = 0; i_i + 1 <= m; i_i++) { - Q_data[i_i + b_idx_0 * itau] = b_A_data[b_A_sizes[0] * itau + i_i]; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcjmdjmgppphecje_qr.h b/gnc/matlab/code_generation/sharedutils/fcjmdjmgppphecje_qr.h deleted file mode 100644 index 0a1575db08..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcjmdjmgppphecje_qr.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fcjmdjmgppphecje_qr.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_fcjmdjmgppphecje_qr -#define SHARE_fcjmdjmgppphecje_qr -#include "rtwtypes.h" - -extern void fcjmdjmgppphecje_qr(const real32_T A_data[], const int32_T A_sizes[2], - real32_T Q_data[], int32_T Q_sizes[2], real32_T R_data[], int32_T R_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcjmkfcbglngohdj_quat_propagate_step.cpp b/gnc/matlab/code_generation/sharedutils/fcjmkfcbglngohdj_quat_propagate_step.cpp deleted file mode 100644 index 4edba9324f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcjmkfcbglngohdj_quat_propagate_step.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// -// File: fcjmkfcbglngohdj_quat_propagate_step.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#include "rtwtypes.h" -#include -#include "fcjmkfcbglngohdj_quat_propagate_step.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void fcjmkfcbglngohdj_quat_propagate_step(const real32_T quat_in[4], const - real32_T omega[3], real32_T time_in, real_T quat_out[4]) -{ - real32_T omega_mag; - real32_T c; - real32_T sine_mag; - real_T x; - real_T c_y; - real32_T c_0[16]; - int32_T i; - real32_T s_idx_0; - real32_T s_idx_1; - - // omega row vector - // - // From Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra - // Equation below is from Eq. 122, with Omega matrix simplified with - // identity matrix. - // - // Column_Vector = RSSROW(Matrix) - // - omega_mag = (real32_T)sqrt((real_T)((omega[0] * omega[0] + omega[1] * omega[1]) - + omega[2] * omega[2])); - if ((omega_mag == 0.0F) || (time_in <= 0.0F)) { - quat_out[0] = (real_T)quat_in[0]; - quat_out[1] = (real_T)quat_in[1]; - quat_out[2] = (real_T)quat_in[2]; - quat_out[3] = (real_T)quat_in[3]; - } else { - c = (real32_T)cos((real_T)(0.5F * omega_mag * time_in)); - sine_mag = (real32_T)sin((real_T)(0.5F * omega_mag * time_in)); - s_idx_0 = sine_mag * omega[0] / omega_mag; - s_idx_1 = sine_mag * omega[1] / omega_mag; - omega_mag = sine_mag * omega[2] / omega_mag; - - // Rollup of trig equations and Omega matrix - c_0[0] = c; - c_0[1] = -omega_mag; - c_0[2] = s_idx_1; - c_0[3] = -s_idx_0; - c_0[4] = omega_mag; - c_0[5] = c; - c_0[6] = -s_idx_0; - c_0[7] = -s_idx_1; - c_0[8] = -s_idx_1; - c_0[9] = s_idx_0; - c_0[10] = c; - c_0[11] = -omega_mag; - c_0[12] = s_idx_0; - c_0[13] = s_idx_1; - c_0[14] = omega_mag; - c_0[15] = c; - for (i = 0; i < 4; i++) { - c = c_0[(int32_T)(i + 12)] * quat_in[3] + (c_0[(int32_T)(i + 8)] * - quat_in[2] + (c_0[(int32_T)(i + 4)] * quat_in[1] + c_0[i] * quat_in[0])); - quat_out[i] = (real_T)c; - } - - if (quat_out[3] < 0.0) { - x = -1.0; - } else if (quat_out[3] > 0.0) { - x = 1.0; - } else if (quat_out[3] == 0.0) { - x = 0.0; - } else { - x = quat_out[3]; - } - - // - // Column_Vector = RSSROW(Matrix) - // - c_y = sqrt(((quat_out[0] * quat_out[0] + quat_out[1] * quat_out[1]) + - quat_out[2] * quat_out[2]) + quat_out[3] * quat_out[3]); - quat_out[0] = x * quat_out[0] / c_y; - quat_out[1] = x * quat_out[1] / c_y; - quat_out[2] = x * quat_out[2] / c_y; - quat_out[3] = x * quat_out[3] / c_y; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fcjmkfcbglngohdj_quat_propagate_step.h b/gnc/matlab/code_generation/sharedutils/fcjmkfcbglngohdj_quat_propagate_step.h deleted file mode 100644 index 497fb32656..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fcjmkfcbglngohdj_quat_propagate_step.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fcjmkfcbglngohdj_quat_propagate_step.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#ifndef SHARE_fcjmkfcbglngohdj_quat_propagate_step -#define SHARE_fcjmkfcbglngohdj_quat_propagate_step -#include "rtwtypes.h" - -extern void fcjmkfcbglngohdj_quat_propagate_step(const real32_T quat_in[4], - const real32_T omega[3], real32_T time_in, real_T quat_out[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/filemap.mat b/gnc/matlab/code_generation/sharedutils/filemap.mat deleted file mode 100644 index 93636157a7..0000000000 Binary files a/gnc/matlab/code_generation/sharedutils/filemap.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/sharedutils/fkfcbaiengdjgdje_quaternion_to_rotation.cpp b/gnc/matlab/code_generation/sharedutils/fkfcbaiengdjgdje_quaternion_to_rotation.cpp deleted file mode 100644 index 1042e8ca79..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkfcbaiengdjgdje_quaternion_to_rotation.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// -// File: fkfcbaiengdjgdje_quaternion_to_rotation.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "fkfcbaiengdjgdje_quaternion_to_rotation.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void fkfcbaiengdjgdje_quaternion_to_rotation(const real32_T q[4], real32_T R[9]) -{ - real32_T S[9]; - static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - real32_T q_0; - real32_T q_1; - real32_T q_2[12]; - real32_T q_3[12]; - int32_T i; - int32_T i_0; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - S[0] = 0.0F; - S[3] = -q[2]; - S[6] = q[1]; - S[1] = q[2]; - S[4] = 0.0F; - S[7] = -q[0]; - S[2] = -q[1]; - S[5] = q[0]; - S[8] = 0.0F; - q_0 = q[3]; - q_1 = q[3]; - for (i = 0; i < 3; i++) { - q_2[(int32_T)(3 * i)] = q_0 * (real32_T)b[i] + S[i]; - q_3[(int32_T)(i << 2)] = (real32_T)b[(int32_T)(3 * i)] * q_1 - S[(int32_T)(3 - * i)]; - q_2[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b[(int32_T)(i + 3)] * q_0 + - S[(int32_T)(i + 3)]; - q_3[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b[(int32_T)((int32_T)(3 * - i) + 1)] * q_1 - S[(int32_T)((int32_T)(3 * i) + 1)]; - q_2[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b[(int32_T)(i + 6)] * q_0 + - S[(int32_T)(i + 6)]; - q_3[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b[(int32_T)((int32_T)(3 * - i) + 2)] * q_1 - S[(int32_T)((int32_T)(3 * i) + 2)]; - q_2[(int32_T)(9 + i)] = -q[i]; - q_3[(int32_T)(3 + (int32_T)(i << 2))] = -q[i]; - } - - for (i = 0; i < 3; i++) { - for (i_0 = 0; i_0 < 3; i_0++) { - R[(int32_T)(i_0 + (int32_T)(3 * i))] = 0.0F; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)(i << 2)] * q_2[i_0]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 1)] * q_2[(int32_T)(i_0 + 3)]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 2)] * q_2[(int32_T)(i_0 + 6)]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 3)] * q_2[(int32_T)(i_0 + 9)]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkfcbaiengdjgdje_quaternion_to_rotation.h b/gnc/matlab/code_generation/sharedutils/fkfcbaiengdjgdje_quaternion_to_rotation.h deleted file mode 100644 index 7a31ec8659..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkfcbaiengdjgdje_quaternion_to_rotation.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fkfcbaiengdjgdje_quaternion_to_rotation.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_fkfcbaiengdjgdje_quaternion_to_rotation -#define SHARE_fkfcbaiengdjgdje_quaternion_to_rotation -#include "rtwtypes.h" - -extern void fkfcbaiengdjgdje_quaternion_to_rotation(const real32_T q[4], - real32_T R[9]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkfcnglnimopfcjm_rows_differ.cpp b/gnc/matlab/code_generation/sharedutils/fkfcnglnimopfcjm_rows_differ.cpp deleted file mode 100644 index 04f4e12539..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkfcnglnimopfcjm_rows_differ.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// -// File: fkfcnglnimopfcjm_rows_differ.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "fkfcnglnimopfcjm_rows_differ.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T fkfcnglnimopfcjm_rows_differ(const real32_T b_data[], const int32_T - b_sizes[2], int32_T k0, int32_T k) -{ - boolean_T p; - int32_T j; - real32_T absxk; - int32_T exponent; - boolean_T exitg1; - p = false; - j = 0; - exitg1 = false; - while ((!exitg1) && (j < 2)) { - absxk = (real32_T)fabs((real_T)(b_data[(b_sizes[0] * j + k) - 1] / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, exponent - 24); - } - } else { - absxk = (rtNaNF); - } - - if (!(((real32_T)fabs((real_T)(b_data[(b_sizes[0] * j + k) - 1] - b_data - [(b_sizes[0] * j + k0) - 1])) < absxk) || (rtIsInfF(b_data[(b_sizes - [0] * j + k0) - 1]) && rtIsInfF(b_data[(b_sizes[0] * j + k) - 1]) && - ((b_data[(b_sizes[0] * j + k0) - 1] > 0.0F) == (b_data[(b_sizes[0] * j - + k) - 1] > 0.0F))))) { - p = true; - exitg1 = true; - } else { - j++; - } - } - - return p; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkfcnglnimopfcjm_rows_differ.h b/gnc/matlab/code_generation/sharedutils/fkfcnglnimopfcjm_rows_differ.h deleted file mode 100644 index 41e2df5052..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkfcnglnimopfcjm_rows_differ.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fkfcnglnimopfcjm_rows_differ.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_fkfcnglnimopfcjm_rows_differ -#define SHARE_fkfcnglnimopfcjm_rows_differ -#include "rtwtypes.h" - -extern boolean_T fkfcnglnimopfcjm_rows_differ(const real32_T b_data[], const - int32_T b_sizes[2], int32_T k0, int32_T k); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkfkbiecjecjbaie_cat.cpp b/gnc/matlab/code_generation/sharedutils/fkfkbiecjecjbaie_cat.cpp deleted file mode 100644 index 45708b717f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkfkbiecjecjbaie_cat.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: fkfkbiecjecjbaie_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "fkfkbiecjecjbaie_cat.h" - -// Function for MATLAB Function: '/generate_output' -void fkfkbiecjecjbaie_cat(const int32_T varargin_1_sizes, const int32_T - varargin_2_sizes[2], uint8_T y_data[], int32_T y_sizes[2]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = (int32_T)(int8_T)varargin_1_sizes; - y_sizes[1] = 16; - iy = -1; - for (j = 1; j <= varargin_1_sizes; j++) { - iy++; - y_data[iy] = 1U; - } - - j = (int32_T)(varargin_2_sizes[0] * 15); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = 0U; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkfkbiecjecjbaie_cat.h b/gnc/matlab/code_generation/sharedutils/fkfkbiecjecjbaie_cat.h deleted file mode 100644 index f054d8ff6b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkfkbiecjecjbaie_cat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fkfkbiecjecjbaie_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_fkfkbiecjecjbaie_cat -#define SHARE_fkfkbiecjecjbaie_cat -#include "rtwtypes.h" - -extern void fkfkbiecjecjbaie_cat(const int32_T varargin_1_sizes, const int32_T - varargin_2_sizes[2], uint8_T y_data[], int32_T y_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkngdjekgdjepphl_sum.cpp b/gnc/matlab/code_generation/sharedutils/fkngdjekgdjepphl_sum.cpp deleted file mode 100644 index a97c0cd1f9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkngdjekgdjepphl_sum.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// -// File: fkngdjekgdjepphl_sum.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "fkngdjekgdjepphl_sum.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void fkngdjekgdjepphl_sum(const uint8_T x[800], real_T y[50]) -{ - real_T s; - int32_T j; - int32_T k; - for (j = 0; j < 50; j++) { - s = (real_T)x[j]; - for (k = 0; k < 15; k++) { - s += (real_T)x[(int32_T)((int32_T)((int32_T)(k + 1) * 50) + j)]; - } - - y[j] = s; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkngdjekgdjepphl_sum.h b/gnc/matlab/code_generation/sharedutils/fkngdjekgdjepphl_sum.h deleted file mode 100644 index b26564713e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkngdjekgdjepphl_sum.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: fkngdjekgdjepphl_sum.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_fkngdjekgdjepphl_sum -#define SHARE_fkngdjekgdjepphl_sum -#include "rtwtypes.h" - -extern void fkngdjekgdjepphl_sum(const uint8_T x[800], real_T y[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkngjekfaimgmohd_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/fkngjekfaimgmohd_xaxpy.cpp deleted file mode 100644 index 5bdb219215..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkngjekfaimgmohd_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: fkngjekfaimgmohd_xaxpy.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "fkngjekfaimgmohd_xaxpy.h" - -// Function for MATLAB Function: '/MATLAB Function' -void fkngjekfaimgmohd_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T y[72], - int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * y[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fkngjekfaimgmohd_xaxpy.h b/gnc/matlab/code_generation/sharedutils/fkngjekfaimgmohd_xaxpy.h deleted file mode 100644 index 91db0a1bf5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fkngjekfaimgmohd_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: fkngjekfaimgmohd_xaxpy.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_fkngjekfaimgmohd_xaxpy -#define SHARE_fkngjekfaimgmohd_xaxpy -#include "rtwtypes.h" - -extern void fkngjekfaimgmohd_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T - y[72], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fknoaimojecjmohd_sort.cpp b/gnc/matlab/code_generation/sharedutils/fknoaimojecjmohd_sort.cpp deleted file mode 100644 index 5304771e7e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fknoaimojecjmohd_sort.cpp +++ /dev/null @@ -1,217 +0,0 @@ -// -// File: fknoaimojecjmohd_sort.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include -#include "gdbafcbifcbapphd_merge.h" -#include "rt_nonfinite.h" -#include "fknoaimojecjmohd_sort.h" - -// Function for MATLAB Function: '/generate_output' -void fknoaimojecjmohd_sort(real32_T x[50], int32_T idx[50]) -{ - real32_T xwork[50]; - int32_T iwork[50]; - real32_T x4[4]; - int8_T idx4[4]; - int32_T ib; - int32_T m; - int8_T perm[4]; - int32_T i3; - int32_T i4; - int32_T tailOffset; - int32_T nTail; - int32_T i; - x4[0] = 0.0F; - idx4[0] = 0; - x4[1] = 0.0F; - idx4[1] = 0; - x4[2] = 0.0F; - idx4[2] = 0; - x4[3] = 0.0F; - idx4[3] = 0; - for (i = 0; i < 50; i++) { - idx[i] = 0; - xwork[i] = 0.0F; - } - - i = 0; - ib = 0; - for (m = 0; m < 50; m++) { - if (rtIsNaNF(x[m])) { - idx[49 - i] = m + 1; - xwork[49 - i] = x[m]; - i++; - } else { - ib++; - idx4[ib - 1] = (int8_T)(m + 1); - x4[ib - 1] = x[m]; - if (ib == 4) { - ib = m - i; - if (x4[0] <= x4[1]) { - tailOffset = 1; - nTail = 2; - } else { - tailOffset = 2; - nTail = 1; - } - - if (x4[2] <= x4[3]) { - i3 = 3; - i4 = 4; - } else { - i3 = 4; - i4 = 3; - } - - if (x4[tailOffset - 1] <= x4[i3 - 1]) { - if (x4[nTail - 1] <= x4[i3 - 1]) { - perm[0] = (int8_T)tailOffset; - perm[1] = (int8_T)nTail; - perm[2] = (int8_T)i3; - perm[3] = (int8_T)i4; - } else if (x4[nTail - 1] <= x4[i4 - 1]) { - perm[0] = (int8_T)tailOffset; - perm[1] = (int8_T)i3; - perm[2] = (int8_T)nTail; - perm[3] = (int8_T)i4; - } else { - perm[0] = (int8_T)tailOffset; - perm[1] = (int8_T)i3; - perm[2] = (int8_T)i4; - perm[3] = (int8_T)nTail; - } - } else if (x4[tailOffset - 1] <= x4[i4 - 1]) { - if (x4[nTail - 1] <= x4[i4 - 1]) { - perm[0] = (int8_T)i3; - perm[1] = (int8_T)tailOffset; - perm[2] = (int8_T)nTail; - perm[3] = (int8_T)i4; - } else { - perm[0] = (int8_T)i3; - perm[1] = (int8_T)tailOffset; - perm[2] = (int8_T)i4; - perm[3] = (int8_T)nTail; - } - } else { - perm[0] = (int8_T)i3; - perm[1] = (int8_T)i4; - perm[2] = (int8_T)tailOffset; - perm[3] = (int8_T)nTail; - } - - idx[ib - 3] = idx4[perm[0] - 1]; - idx[ib - 2] = idx4[perm[1] - 1]; - idx[ib - 1] = idx4[perm[2] - 1]; - idx[ib] = idx4[perm[3] - 1]; - x[ib - 3] = x4[perm[0] - 1]; - x[ib - 2] = x4[perm[1] - 1]; - x[ib - 1] = x4[perm[2] - 1]; - x[ib] = x4[perm[3] - 1]; - ib = 0; - } - } - } - - if (ib > 0) { - perm[1] = 0; - perm[2] = 0; - perm[3] = 0; - if (ib == 1) { - perm[0] = 1; - } else if (ib == 2) { - if (x4[0] <= x4[1]) { - perm[0] = 1; - perm[1] = 2; - } else { - perm[0] = 2; - perm[1] = 1; - } - } else if (x4[0] <= x4[1]) { - if (x4[1] <= x4[2]) { - perm[0] = 1; - perm[1] = 2; - perm[2] = 3; - } else if (x4[0] <= x4[2]) { - perm[0] = 1; - perm[1] = 3; - perm[2] = 2; - } else { - perm[0] = 3; - perm[1] = 1; - perm[2] = 2; - } - } else if (x4[0] <= x4[2]) { - perm[0] = 2; - perm[1] = 1; - perm[2] = 3; - } else if (x4[1] <= x4[2]) { - perm[0] = 2; - perm[1] = 3; - perm[2] = 1; - } else { - perm[0] = 3; - perm[1] = 2; - perm[2] = 1; - } - - for (m = 50; m - 49 <= ib; m++) { - idx[(m - i) - ib] = idx4[perm[m - 50] - 1]; - x[(m - i) - ib] = x4[perm[m - 50] - 1]; - } - } - - m = i >> 1; - for (ib = 1; ib <= m; ib++) { - tailOffset = idx[(ib - i) + 49]; - idx[(ib - i) + 49] = idx[50 - ib]; - idx[50 - ib] = tailOffset; - x[(ib - i) + 49] = xwork[50 - ib]; - x[50 - ib] = xwork[(ib - i) + 49]; - } - - if ((i & 1) != 0) { - x[(m - i) + 50] = xwork[(m - i) + 50]; - } - - if (50 - i > 1) { - memset(&iwork[0], 0, 50U * sizeof(int32_T)); - ib = (50 - i) >> 2; - m = 4; - while (ib > 1) { - if ((ib & 1) != 0) { - ib--; - tailOffset = m * ib; - nTail = 50 - (i + tailOffset); - if (nTail > m) { - gdbafcbifcbapphd_merge(idx, x, tailOffset, m, nTail - m, iwork, xwork); - } - } - - tailOffset = m << 1; - ib >>= 1; - for (nTail = 1; nTail <= ib; nTail++) { - gdbafcbifcbapphd_merge(idx, x, (nTail - 1) * tailOffset, m, m, iwork, - xwork); - } - - m = tailOffset; - } - - if (50 - i > m) { - gdbafcbifcbapphd_merge(idx, x, 0, m, 50 - (i + m), iwork, xwork); - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/fknoaimojecjmohd_sort.h b/gnc/matlab/code_generation/sharedutils/fknoaimojecjmohd_sort.h deleted file mode 100644 index 55ac7878a9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/fknoaimojecjmohd_sort.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: fknoaimojecjmohd_sort.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_fknoaimojecjmohd_sort -#define SHARE_fknoaimojecjmohd_sort -#include "rtwtypes.h" - -extern void fknoaimojecjmohd_sort(real32_T x[50], int32_T idx[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdbafcbifcbapphd_merge.cpp b/gnc/matlab/code_generation/sharedutils/gdbafcbifcbapphd_merge.cpp deleted file mode 100644 index eb5d951bdd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdbafcbifcbapphd_merge.cpp +++ /dev/null @@ -1,68 +0,0 @@ -// -// File: gdbafcbifcbapphd_merge.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "gdbafcbifcbapphd_merge.h" - -// Function for MATLAB Function: '/generate_output' -void gdbafcbifcbapphd_merge(int32_T idx[50], real32_T x[50], int32_T offset, - int32_T np, int32_T nq, int32_T iwork[50], real32_T xwork[50]) -{ - int32_T n; - int32_T q; - int32_T qend; - int32_T iout; - int32_T offset1; - int32_T b_j; - if (nq != 0) { - n = np + nq; - for (q = 0; q + 1 <= n; q++) { - iwork[q] = idx[offset + q]; - xwork[q] = x[offset + q]; - } - - n = 0; - q = np; - qend = np + nq; - iout = offset - 1; - do { - offset1 = 0; - iout++; - if (xwork[n] <= xwork[q]) { - idx[iout] = iwork[n]; - x[iout] = xwork[n]; - if (n + 1 < np) { - n++; - } else { - offset1 = 1; - } - } else { - idx[iout] = iwork[q]; - x[iout] = xwork[q]; - if (q + 1 < qend) { - q++; - } else { - offset1 = (iout - n) + 1; - for (b_j = n; b_j + 1 <= np; b_j++) { - idx[offset1 + b_j] = iwork[b_j]; - x[offset1 + b_j] = xwork[b_j]; - } - - offset1 = 1; - } - } - } while (offset1 == 0); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdbafcbifcbapphd_merge.h b/gnc/matlab/code_generation/sharedutils/gdbafcbifcbapphd_merge.h deleted file mode 100644 index 18e3539d60..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdbafcbifcbapphd_merge.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: gdbafcbifcbapphd_merge.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_gdbafcbifcbapphd_merge -#define SHARE_gdbafcbifcbapphd_merge -#include "rtwtypes.h" - -extern void gdbafcbifcbapphd_merge(int32_T idx[50], real32_T x[50], int32_T - offset, int32_T np, int32_T nq, int32_T iwork[50], real32_T xwork[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdbijekfimglfcje_xrot.cpp b/gnc/matlab/code_generation/sharedutils/gdbijekfimglfcje_xrot.cpp deleted file mode 100644 index 37fc997092..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdbijekfimglfcje_xrot.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: gdbijekfimglfcje_xrot.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "gdbijekfimglfcje_xrot.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void gdbijekfimglfcje_xrot(real32_T x[4], int32_T ix0, int32_T iy0, real32_T c, - real32_T s) -{ - int32_T ix; - int32_T iy; - real32_T temp; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdbijekfimglfcje_xrot.h b/gnc/matlab/code_generation/sharedutils/gdbijekfimglfcje_xrot.h deleted file mode 100644 index 31682b070c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdbijekfimglfcje_xrot.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: gdbijekfimglfcje_xrot.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_gdbijekfimglfcje_xrot -#define SHARE_gdbijekfimglfcje_xrot -#include "rtwtypes.h" - -extern void gdbijekfimglfcje_xrot(real32_T x[4], int32_T ix0, int32_T iy0, - real32_T c, real32_T s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjecbiecbaapppp_quat_rotation_vec.cpp b/gnc/matlab/code_generation/sharedutils/gdjecbiecbaapppp_quat_rotation_vec.cpp deleted file mode 100644 index 80fd8cf0d2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjecbiecbaapppp_quat_rotation_vec.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// -// File: gdjecbiecbaapppp_quat_rotation_vec.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Dec 4 08:33:06 2017 -// -#include "rtwtypes.h" -#include "cbiemoppekfknopp_quatmult.h" -#include "gdjecbiecbaapppp_quat_rotation_vec.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void gdjecbiecbaapppp_quat_rotation_vec(real_T vector[3], const real32_T Q[4], - real32_T vec_out[3]) -{ - real32_T V[4]; - real32_T Q_0[4]; - real32_T tmp[4]; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // - // Q = q_a2b, rotation represents a vector rotating from frame b2a - // q_out = quat_rotation(vector, Q2) - // - // rotates a row-vector of 3-vectors by a row-vector of quaternion Q2 - // - // From: Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra - // Equation below is from Eq. 77, with the the quaternions inverted because - // our quat multiplication convention has the reverse order from the - // mulitplication used in the paper - // - // Check to see if the vector is a single row - // Vec_out = Q * [V 0]' * Q^-1 - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - V[0] = (0.0F * Q[0] + Q[3] * (real32_T)vector[0]) + (Q[1] * (real32_T)vector[2] - - Q[2] * (real32_T)vector[1]); - V[1] = (0.0F * Q[1] + Q[3] * (real32_T)vector[1]) + (Q[2] * (real32_T)vector[0] - - Q[0] * (real32_T)vector[2]); - V[2] = (0.0F * Q[2] + Q[3] * (real32_T)vector[2]) + (Q[0] * (real32_T)vector[1] - - Q[1] * (real32_T)vector[0]); - V[3] = Q[3] * 0.0F - ((Q[0] * (real32_T)vector[0] + Q[1] * (real32_T)vector[1]) - + Q[2] * (real32_T)vector[2]); - Q_0[0] = -Q[0]; - Q_0[1] = -Q[1]; - Q_0[2] = -Q[2]; - Q_0[3] = Q[3]; - cbiemoppekfknopp_quatmult(V, Q_0, tmp); - vec_out[0] = tmp[0]; - vec_out[1] = tmp[1]; - vec_out[2] = tmp[2]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjecbiecbaapppp_quat_rotation_vec.h b/gnc/matlab/code_generation/sharedutils/gdjecbiecbaapppp_quat_rotation_vec.h deleted file mode 100644 index 01636459d7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjecbiecbaapppp_quat_rotation_vec.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: gdjecbiecbaapppp_quat_rotation_vec.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Dec 4 08:33:06 2017 -// -#ifndef SHARE_gdjecbiecbaapppp_quat_rotation_vec -#define SHARE_gdjecbiecbaapppp_quat_rotation_vec -#include "rtwtypes.h" - -extern void gdjecbiecbaapppp_quat_rotation_vec(real_T vector[3], const real32_T - Q[4], real32_T vec_out[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjmaiekcbaiecjm_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/gdjmaiekcbaiecjm_xzlarf.cpp deleted file mode 100644 index 0b584e032d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjmaiekcbaiecjm_xzlarf.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// -// File: gdjmaiekcbaiecjm_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "gdjmaiekcbaiecjm_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void gdjmaiekcbaiecjm_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]) -{ - int32_T lastv; - int32_T lastc; - int32_T coltop; - int32_T ix; - real32_T c; - int32_T iac; - int32_T d; - int32_T b_ia; - int32_T jy; - int32_T exitg1; - boolean_T exitg2; - if (tau != 0.0F) { - lastv = m; - lastc = (int32_T)(iv0 + m); - while ((lastv > 0) && (C_data[(int32_T)(lastc - 2)] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = n; - exitg2 = false; - while ((!exitg2) && (lastc > 0)) { - coltop = (int32_T)((int32_T)((int32_T)(lastc - 1) * ldc) + ic0); - jy = coltop; - do { - exitg1 = 0; - if (jy <= (int32_T)((int32_T)(coltop + lastv) - 1)) { - if (C_data[(int32_T)(jy - 1)] != 0.0F) { - exitg1 = 1; - } else { - jy++; - } - } else { - lastc--; - exitg1 = 2; - } - } while (exitg1 == 0); - - if (exitg1 == 1) { - exitg2 = true; - } - } - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - if (lastc != 0) { - for (coltop = 1; coltop <= lastc; coltop++) { - work[(int32_T)(coltop - 1)] = 0.0F; - } - - coltop = 0; - jy = (int32_T)((int32_T)((int32_T)(lastc - 1) * ldc) + ic0); - iac = ic0; - while ((ldc > 0) && (iac <= jy)) { - ix = iv0; - c = 0.0F; - d = (int32_T)((int32_T)(iac + lastv) - 1); - for (b_ia = iac; b_ia <= d; b_ia++) { - c += C_data[(int32_T)(b_ia - 1)] * C_data[(int32_T)(ix - 1)]; - ix++; - } - - work[coltop] += c; - coltop++; - iac += ldc; - } - } - - if (!(-tau == 0.0F)) { - coltop = (int32_T)(ic0 - 1); - jy = 0; - for (iac = 1; iac <= lastc; iac++) { - if (work[jy] != 0.0F) { - c = work[jy] * -tau; - ix = iv0; - d = (int32_T)(lastv + coltop); - for (b_ia = coltop; (int32_T)(b_ia + 1) <= d; b_ia++) { - C_data[b_ia] += C_data[(int32_T)(ix - 1)] * c; - ix++; - } - } - - jy++; - coltop += ldc; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjmaiekcbaiecjm_xzlarf.h b/gnc/matlab/code_generation/sharedutils/gdjmaiekcbaiecjm_xzlarf.h deleted file mode 100644 index c681718c51..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjmaiekcbaiecjm_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: gdjmaiekcbaiecjm_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_gdjmaiekcbaiecjm_xzlarf -#define SHARE_gdjmaiekcbaiecjm_xzlarf -#include "rtwtypes.h" - -extern void gdjmaiekcbaiecjm_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjmjekfdjecpppp_svd.cpp b/gnc/matlab/code_generation/sharedutils/gdjmjekfdjecpppp_svd.cpp deleted file mode 100644 index 1e3ba4646a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjmjekfdjecpppp_svd.cpp +++ /dev/null @@ -1,650 +0,0 @@ -// -// File: gdjmjekfdjecpppp_svd.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include -#include "aimgimglphlffkfk_xswap.h" -#include "cjekkfkfmoppfkfc_xdotc.h" -#include "dbainohddbimecba_xnrm2.h" -#include "dbiebiecimopdbaa_xnrm2.h" -#include "glfkdbaibaaakfcb_xscal.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "ieknhdjmlngdkfcb_xaxpy.h" -#include "jecbfkfkhdbangdj_xaxpy.h" -#include "moppjecbaaaaimop_xrot.h" -#include "phlniekfphlndjmo_xaxpy.h" -#include "rt_nonfinite.h" -#include "gdjmjekfdjecpppp_svd.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void gdjmjekfdjecpppp_svd(const real32_T A[16], real32_T U[16], real32_T S[16], - real32_T V[16]) -{ - real32_T b_A[16]; - real32_T s[4]; - real32_T e[4]; - real32_T work[4]; - real32_T Vf[16]; - int32_T q; - boolean_T apply_transform; - int32_T m; - int32_T iter; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - int32_T k_ii; - real32_T varargin_1[5]; - boolean_T exitg1; - boolean_T exitg2; - int32_T exitg3; - e[0] = 0.0F; - work[0] = 0.0F; - e[1] = 0.0F; - work[1] = 0.0F; - e[2] = 0.0F; - work[2] = 0.0F; - e[3] = 0.0F; - work[3] = 0.0F; - for (iter = 0; iter < 16; iter++) { - b_A[iter] = A[iter]; - U[iter] = 0.0F; - Vf[iter] = 0.0F; - } - - iter = 0; - apply_transform = false; - snorm = dbiebiecimopdbaa_xnrm2(4, b_A, 1); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[0] < 0.0F) { - s[0] = -snorm; - } else { - s[0] = snorm; - } - - if ((real32_T)fabs((real_T)s[0]) >= 9.86076132E-32F) { - snorm = 1.0F / s[0]; - for (qs = 0; (int32_T)(qs + 1) < 5; qs++) { - b_A[qs] *= snorm; - } - } else { - for (qs = 0; (int32_T)(qs + 1) < 5; qs++) { - b_A[qs] /= s[0]; - } - } - - b_A[0]++; - s[0] = -s[0]; - } else { - s[0] = 0.0F; - } - - for (q = 1; (int32_T)(q + 1) < 5; q++) { - qs = (int32_T)(q << 2); - if (apply_transform) { - phlniekfphlndjmo_xaxpy(4, -(cjekkfkfmoppfkfc_xdotc(4, b_A, 1, b_A, - (int32_T)(qs + 1)) / b_A[0]), 1, b_A, (int32_T)(qs + 1)); - } - - e[q] = b_A[qs]; - } - - while ((int32_T)(iter + 1) < 5) { - U[iter] = b_A[iter]; - iter++; - } - - snorm = dbainohddbimecba_xnrm2(3, e, 2); - if (snorm == 0.0F) { - e[0] = 0.0F; - } else { - if (e[1] < 0.0F) { - e[0] = -snorm; - } else { - e[0] = snorm; - } - - snorm = e[0]; - if ((real32_T)fabs((real_T)e[0]) >= 9.86076132E-32F) { - snorm = 1.0F / e[0]; - for (iter = 1; (int32_T)(iter + 1) < 5; iter++) { - e[iter] *= snorm; - } - } else { - for (iter = 1; (int32_T)(iter + 1) < 5; iter++) { - e[iter] /= snorm; - } - } - - e[1]++; - e[0] = -e[0]; - for (iter = 1; (int32_T)(iter + 1) < 5; iter++) { - work[iter] = 0.0F; - } - - for (iter = 1; (int32_T)(iter + 1) < 5; iter++) { - jecbfkfkhdbangdj_xaxpy(3, e[iter], b_A, (int32_T)((int32_T)(iter << 2) + 2), - work, 2); - } - - for (iter = 1; (int32_T)(iter + 1) < 5; iter++) { - ieknhdjmlngdkfcb_xaxpy(3, -e[iter] / e[1], work, 2, b_A, (int32_T) - ((int32_T)(iter << 2) + 2)); - } - } - - for (iter = 1; (int32_T)(iter + 1) < 5; iter++) { - Vf[iter] = e[iter]; - } - - apply_transform = false; - snorm = dbiebiecimopdbaa_xnrm2(3, b_A, 6); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[5] < 0.0F) { - s[1] = -snorm; - } else { - s[1] = snorm; - } - - if ((real32_T)fabs((real_T)s[1]) >= 9.86076132E-32F) { - snorm = 1.0F / s[1]; - for (qs = 5; (int32_T)(qs + 1) < 9; qs++) { - b_A[qs] *= snorm; - } - } else { - for (qs = 5; (int32_T)(qs + 1) < 9; qs++) { - b_A[qs] /= s[1]; - } - } - - b_A[5]++; - s[1] = -s[1]; - } else { - s[1] = 0.0F; - } - - for (q = 2; (int32_T)(q + 1) < 5; q++) { - qs = (int32_T)((int32_T)(q << 2) + 1); - if (apply_transform) { - phlniekfphlndjmo_xaxpy(3, -(cjekkfkfmoppfkfc_xdotc(3, b_A, 6, b_A, - (int32_T)(qs + 1)) / b_A[5]), 6, b_A, (int32_T)(qs + 1)); - } - - e[q] = b_A[qs]; - } - - for (iter = 1; (int32_T)(iter + 1) < 5; iter++) { - U[(int32_T)(iter + 4)] = b_A[(int32_T)(iter + 4)]; - } - - snorm = dbainohddbimecba_xnrm2(2, e, 3); - if (snorm == 0.0F) { - e[1] = 0.0F; - } else { - if (e[2] < 0.0F) { - e[1] = -snorm; - } else { - e[1] = snorm; - } - - snorm = e[1]; - if ((real32_T)fabs((real_T)e[1]) >= 9.86076132E-32F) { - snorm = 1.0F / e[1]; - for (iter = 2; (int32_T)(iter + 1) < 5; iter++) { - e[iter] *= snorm; - } - } else { - for (iter = 2; (int32_T)(iter + 1) < 5; iter++) { - e[iter] /= snorm; - } - } - - e[2]++; - e[1] = -e[1]; - for (iter = 2; (int32_T)(iter + 1) < 5; iter++) { - work[iter] = 0.0F; - } - - for (iter = 2; (int32_T)(iter + 1) < 5; iter++) { - jecbfkfkhdbangdj_xaxpy(2, e[iter], b_A, (int32_T)((int32_T)(iter << 2) + 3), - work, 3); - } - - for (iter = 2; (int32_T)(iter + 1) < 5; iter++) { - ieknhdjmlngdkfcb_xaxpy(2, -e[iter] / e[2], work, 3, b_A, (int32_T) - ((int32_T)(iter << 2) + 3)); - } - } - - for (iter = 2; (int32_T)(iter + 1) < 5; iter++) { - Vf[(int32_T)(iter + 4)] = e[iter]; - } - - apply_transform = false; - snorm = dbiebiecimopdbaa_xnrm2(2, b_A, 11); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[10] < 0.0F) { - s[2] = -snorm; - } else { - s[2] = snorm; - } - - if ((real32_T)fabs((real_T)s[2]) >= 9.86076132E-32F) { - snorm = 1.0F / s[2]; - for (qs = 10; (int32_T)(qs + 1) < 13; qs++) { - b_A[qs] *= snorm; - } - } else { - for (qs = 10; (int32_T)(qs + 1) < 13; qs++) { - b_A[qs] /= s[2]; - } - } - - b_A[10]++; - s[2] = -s[2]; - } else { - s[2] = 0.0F; - } - - for (q = 3; (int32_T)(q + 1) < 5; q++) { - qs = (int32_T)((int32_T)(q << 2) + 2); - if (apply_transform) { - phlniekfphlndjmo_xaxpy(2, -(cjekkfkfmoppfkfc_xdotc(2, b_A, 11, b_A, - (int32_T)(qs + 1)) / b_A[10]), 11, b_A, (int32_T)(qs + 1)); - } - - e[q] = b_A[qs]; - } - - for (iter = 2; (int32_T)(iter + 1) < 5; iter++) { - U[(int32_T)(iter + 8)] = b_A[(int32_T)(iter + 8)]; - } - - m = 2; - s[3] = b_A[15]; - e[2] = b_A[14]; - e[3] = 0.0F; - U[12] = 0.0F; - U[13] = 0.0F; - U[14] = 0.0F; - U[15] = 1.0F; - for (q = 2; q >= 0; q += -1) { - iter = (int32_T)((int32_T)(q << 2) + q); - if (s[q] != 0.0F) { - for (kase = (int32_T)(q + 1); (int32_T)(kase + 1) < 5; kase++) { - qs = (int32_T)((int32_T)((int32_T)(kase << 2) + q) + 1); - phlniekfphlndjmo_xaxpy((int32_T)(4 - q), -(cjekkfkfmoppfkfc_xdotc - ((int32_T)(4 - q), U, (int32_T)(iter + 1), U, qs) / U[iter]), (int32_T) - (iter + 1), U, qs); - } - - for (qs = q; (int32_T)(qs + 1) < 5; qs++) { - U[(int32_T)(qs + (int32_T)(q << 2))] = -U[(int32_T)((int32_T)(q << 2) + - qs)]; - } - - U[iter]++; - for (iter = 1; iter <= q; iter++) { - U[(int32_T)((int32_T)(iter + (int32_T)(q << 2)) - 1)] = 0.0F; - } - } else { - U[(int32_T)(q << 2)] = 0.0F; - U[(int32_T)(1 + (int32_T)(q << 2))] = 0.0F; - U[(int32_T)(2 + (int32_T)(q << 2))] = 0.0F; - U[(int32_T)(3 + (int32_T)(q << 2))] = 0.0F; - U[iter] = 1.0F; - } - } - - for (iter = 3; iter >= 0; iter += -1) { - if (((int32_T)(iter + 1) <= 2) && (e[iter] != 0.0F)) { - q = (int32_T)((int32_T)((int32_T)(iter << 2) + iter) + 2); - for (qs = (int32_T)(iter + 1); (int32_T)(qs + 1) < 5; qs++) { - kase = (int32_T)((int32_T)((int32_T)(qs << 2) + iter) + 2); - phlniekfphlndjmo_xaxpy((int32_T)(3 - iter), -(cjekkfkfmoppfkfc_xdotc - ((int32_T)(3 - iter), Vf, q, Vf, kase) / Vf[(int32_T)(q - 1)]), q, Vf, - kase); - } - } - - Vf[(int32_T)(iter << 2)] = 0.0F; - Vf[(int32_T)(1 + (int32_T)(iter << 2))] = 0.0F; - Vf[(int32_T)(2 + (int32_T)(iter << 2))] = 0.0F; - Vf[(int32_T)(3 + (int32_T)(iter << 2))] = 0.0F; - Vf[(int32_T)(iter + (int32_T)(iter << 2))] = 1.0F; - } - - ztest = e[0]; - if (s[0] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[0]); - snorm = s[0] / ztest0; - s[0] = ztest0; - ztest = e[0] / snorm; - glfkdbaibaaakfcb_xscal(snorm, U, 1); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[1] *= snorm; - glfkdbaibaaakfcb_xscal(snorm, Vf, 5); - } - - e[0] = ztest; - ztest = e[1]; - if (s[1] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[1]); - snorm = s[1] / ztest0; - s[1] = ztest0; - ztest = e[1] / snorm; - glfkdbaibaaakfcb_xscal(snorm, U, 5); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[2] *= snorm; - glfkdbaibaaakfcb_xscal(snorm, Vf, 9); - } - - e[1] = ztest; - ztest = b_A[14]; - if (s[2] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[2]); - snorm = s[2] / ztest0; - s[2] = ztest0; - ztest = b_A[14] / snorm; - glfkdbaibaaakfcb_xscal(snorm, U, 9); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[3] = b_A[15] * snorm; - glfkdbaibaaakfcb_xscal(snorm, Vf, 13); - } - - e[2] = ztest; - if (s[3] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[3]); - snorm = s[3] / ztest0; - s[3] = ztest0; - glfkdbaibaaakfcb_xscal(snorm, U, 13); - } - - e[3] = 0.0F; - iter = 0; - snorm = 0.0F; - if ((s[0] >= e[0]) || rtIsNaNF(e[0])) { - ztest0 = s[0]; - } else { - ztest0 = e[0]; - } - - if (!((0.0F >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - if ((s[1] >= e[1]) || rtIsNaNF(e[1])) { - ztest0 = s[1]; - } else { - ztest0 = e[1]; - } - - if (!((snorm >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - if ((s[2] >= ztest) || rtIsNaNF(ztest)) { - ztest = s[2]; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - - if (s[3] >= 0.0F) { - ztest0 = s[3]; - } else { - ztest0 = 0.0F; - } - - if (!((snorm >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - while (((int32_T)(m + 2) > 0) && (!(iter >= 75))) { - kase = (int32_T)(m + 1); - do { - exitg3 = 0; - q = kase; - if (kase == 0) { - exitg3 = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e[(int32_T)(kase - 1)]); - if ((ztest0 <= ((real32_T)fabs((real_T)s[(int32_T)(kase - 1)]) + - (real32_T)fabs((real_T)s[kase])) * 1.1920929E-7F) || - ((ztest0 <= 9.86076132E-32F) || ((iter > 20) && (ztest0 <= - 1.1920929E-7F * snorm)))) { - e[(int32_T)(kase - 1)] = 0.0F; - exitg3 = 1; - } else { - kase--; - } - } - } while (exitg3 == 0); - - if ((int32_T)(m + 1) == kase) { - kase = 4; - } else { - qs = (int32_T)(m + 2); - k_ii = (int32_T)(m + 2); - exitg2 = false; - while ((!exitg2) && (k_ii >= kase)) { - qs = k_ii; - if (k_ii == kase) { - exitg2 = true; - } else { - ztest0 = 0.0F; - if (k_ii < (int32_T)(m + 2)) { - ztest0 = (real32_T)fabs((real_T)e[(int32_T)(k_ii - 1)]); - } - - if (k_ii > (int32_T)(kase + 1)) { - ztest0 += (real32_T)fabs((real_T)e[(int32_T)(k_ii - 2)]); - } - - ztest = (real32_T)fabs((real_T)s[(int32_T)(k_ii - 1)]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s[(int32_T)(k_ii - 1)] = 0.0F; - exitg2 = true; - } else { - k_ii--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if ((int32_T)(m + 2) == qs) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[m]; - e[m] = 0.0F; - for (qs = m; (int32_T)(qs + 1) >= (int32_T)(q + 1); qs--) { - ztest = s[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[qs] = ztest; - if ((int32_T)(qs + 1) > (int32_T)(q + 1)) { - ztest0 = e[(int32_T)(qs - 1)] * -smm1; - e[(int32_T)(qs - 1)] *= sqds; - } - - moppjecbaaaaimop_xrot(Vf, (int32_T)(1 + (int32_T)(qs << 2)), (int32_T)(1 - + (int32_T)((int32_T)(m + 1) << 2)), sqds, smm1); - } - break; - - case 2: - ztest0 = e[(int32_T)(q - 1)]; - e[(int32_T)(q - 1)] = 0.0F; - for (qs = q; (int32_T)(qs + 1) <= (int32_T)(m + 2); qs++) { - ztest = s[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[qs] = ztest; - ztest0 = -smm1 * e[qs]; - e[qs] *= sqds; - moppjecbaaaaimop_xrot(U, (int32_T)(1 + (int32_T)(qs << 2)), (int32_T)(1 - + (int32_T)((int32_T)(q - 1) << 2)), sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s[(int32_T)(m + 1)]); - varargin_1[1] = (real32_T)fabs((real_T)s[m]); - varargin_1[2] = (real32_T)fabs((real_T)e[m]); - varargin_1[3] = (real32_T)fabs((real_T)s[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - qs = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - kase = 2; - exitg1 = false; - while ((!exitg1) && (kase < 6)) { - qs = kase; - if (!rtIsNaNF(varargin_1[(int32_T)(kase - 1)])) { - ztest = varargin_1[(int32_T)(kase - 1)]; - exitg1 = true; - } else { - kase++; - } - } - } - - if (qs < 5) { - while ((int32_T)(qs + 1) < 6) { - if (varargin_1[qs] > ztest) { - ztest = varargin_1[qs]; - } - - qs++; - } - } - - ztest0 = s[(int32_T)(m + 1)] / ztest; - smm1 = s[m] / ztest; - emm1 = e[m] / ztest; - sqds = s[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - for (qs = (int32_T)(q + 1); qs <= (int32_T)(m + 1); qs++) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - if (qs > (int32_T)(q + 1)) { - e[(int32_T)(qs - 2)] = ztest0; - } - - ztest0 = s[(int32_T)(qs - 1)] * sqds + e[(int32_T)(qs - 1)] * smm1; - e[(int32_T)(qs - 1)] = e[(int32_T)(qs - 1)] * sqds - s[(int32_T)(qs - 1)] - * smm1; - ztest = smm1 * s[qs]; - s[qs] *= sqds; - moppjecbaaaaimop_xrot(Vf, (int32_T)(1 + (int32_T)((int32_T)(qs - 1) << 2)), - (int32_T)(1 + (int32_T)(qs << 2)), sqds, smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s[(int32_T)(qs - 1)] = ztest0; - ztest0 = e[(int32_T)(qs - 1)] * sqds + smm1 * s[qs]; - s[qs] = e[(int32_T)(qs - 1)] * -smm1 + sqds * s[qs]; - ztest = smm1 * e[qs]; - e[qs] *= sqds; - moppjecbaaaaimop_xrot(U, (int32_T)(1 + (int32_T)((int32_T)(qs - 1) << 2)), - (int32_T)(1 + (int32_T)(qs << 2)), sqds, smm1); - } - - e[m] = ztest0; - iter++; - break; - - default: - if (s[q] < 0.0F) { - s[q] = -s[q]; - glfkdbaibaaakfcb_xscal(-1.0F, Vf, (int32_T)(1 + (int32_T)(q << 2))); - } - - iter = (int32_T)(q + 1); - while (((int32_T)(q + 1) < 4) && (s[q] < s[iter])) { - ztest0 = s[q]; - s[q] = s[iter]; - s[iter] = ztest0; - aimgimglphlffkfk_xswap(Vf, (int32_T)(1 + (int32_T)(q << 2)), (int32_T)(1 - + (int32_T)((int32_T)(q + 1) << 2))); - aimgimglphlffkfk_xswap(U, (int32_T)(1 + (int32_T)(q << 2)), (int32_T)(1 - + (int32_T)((int32_T)(q + 1) << 2))); - q = iter; - iter++; - } - - iter = 0; - m--; - break; - } - } - - for (m = 0; m < 4; m++) { - e[m] = s[m]; - V[(int32_T)(m << 2)] = Vf[(int32_T)(m << 2)]; - V[(int32_T)(1 + (int32_T)(m << 2))] = Vf[(int32_T)((int32_T)(m << 2) + 1)]; - V[(int32_T)(2 + (int32_T)(m << 2))] = Vf[(int32_T)((int32_T)(m << 2) + 2)]; - V[(int32_T)(3 + (int32_T)(m << 2))] = Vf[(int32_T)((int32_T)(m << 2) + 3)]; - } - - memset(&S[0], 0, (uint32_T)(sizeof(real32_T) << 4U)); - S[0] = e[0]; - S[5] = e[1]; - S[10] = e[2]; - S[15] = e[3]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjmjekfdjecpppp_svd.h b/gnc/matlab/code_generation/sharedutils/gdjmjekfdjecpppp_svd.h deleted file mode 100644 index 6f8b07eeed..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjmjekfdjecpppp_svd.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: gdjmjekfdjecpppp_svd.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_gdjmjekfdjecpppp_svd -#define SHARE_gdjmjekfdjecpppp_svd -#include "rtwtypes.h" - -extern void gdjmjekfdjecpppp_svd(const real32_T A[16], real32_T U[16], real32_T - S[16], real32_T V[16]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjmmgdjekfccjmg_xrot.cpp b/gnc/matlab/code_generation/sharedutils/gdjmmgdjekfccjmg_xrot.cpp deleted file mode 100644 index f2c7083e50..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjmmgdjekfccjmg_xrot.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// -// File: gdjmmgdjekfccjmg_xrot.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "gdjmmgdjekfccjmg_xrot.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void gdjmmgdjekfccjmg_xrot(real32_T x[9], int32_T ix0, int32_T iy0, real32_T c, - real32_T s) -{ - int32_T ix; - int32_T iy; - real32_T temp; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjmmgdjekfccjmg_xrot.h b/gnc/matlab/code_generation/sharedutils/gdjmmgdjekfccjmg_xrot.h deleted file mode 100644 index 0865f2a6f0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjmmgdjekfccjmg_xrot.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: gdjmmgdjekfccjmg_xrot.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_gdjmmgdjekfccjmg_xrot -#define SHARE_gdjmmgdjekfccjmg_xrot -#include "rtwtypes.h" - -extern void gdjmmgdjekfccjmg_xrot(real32_T x[9], int32_T ix0, int32_T iy0, - real32_T c, real32_T s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjmmglnmgdjlfkf_quat_rotation_vec.cpp b/gnc/matlab/code_generation/sharedutils/gdjmmglnmgdjlfkf_quat_rotation_vec.cpp deleted file mode 100644 index 10fe021893..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjmmglnmgdjlfkf_quat_rotation_vec.cpp +++ /dev/null @@ -1,136 +0,0 @@ -// -// File: gdjmmglnmgdjlfkf_quat_rotation_vec.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "gdjmmglnmgdjlfkf_quat_rotation_vec.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void gdjmmglnmgdjlfkf_quat_rotation_vec(real_T vector[3], const real32_T Q[4], - real_T vec_out[3]) -{ - int32_T jcol; - int8_T I[9]; - real32_T b_a; - real32_T y; - real32_T tmp[9]; - real32_T Q_0[9]; - real32_T b_a_0[9]; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // - // Q = q_a2b, rotation represents a vector rotating from frame b2a - // q_out = quat_rotation(vector, Q2) - // - // rotates a row-vector of 3-vectors by a row-vector of quaternion Q2 - // - // We accomplish this rotation by using DCMs as an intermediate step - // - // Check to see if the vector is a single row - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Convert quaterion to a DCM. DCM will rotate a vector V by Q_A2B from - // reference frame A to reference frame B. - // - // Indirect Kalman Filter for 3D Attitude Estimation: A Tutorial for Aquaternion Algebra. - // Nikolas Trawny and Stergios I. Roumeliotis - // Equation 78 - for (jcol = 0; jcol < 9; jcol++) { - I[jcol] = 0; - } - - b_a = Q[3] * Q[3] * 2.0F - 1.0F; - y = 2.0F * Q[3]; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - tmp[0] = 0.0F; - tmp[3] = -Q[2]; - tmp[6] = Q[1]; - tmp[1] = Q[2]; - tmp[4] = 0.0F; - tmp[7] = -Q[0]; - tmp[2] = -Q[1]; - tmp[5] = Q[0]; - tmp[8] = 0.0F; - for (jcol = 0; jcol < 3; jcol++) { - I[(int32_T)(jcol + (int32_T)(3 * jcol))] = 1; - Q_0[jcol] = Q[jcol] * Q[0]; - Q_0[(int32_T)(jcol + 3)] = Q[jcol] * Q[1]; - Q_0[(int32_T)(jcol + 6)] = Q[jcol] * Q[2]; - } - - for (jcol = 0; jcol < 3; jcol++) { - b_a_0[(int32_T)(3 * jcol)] = ((real32_T)I[(int32_T)(3 * jcol)] * b_a - tmp - [(int32_T)(3 * jcol)] * y) + Q_0[(int32_T)(3 * jcol)] * 2.0F; - b_a_0[(int32_T)(1 + (int32_T)(3 * jcol))] = ((real32_T)I[(int32_T)((int32_T) - (3 * jcol) + 1)] * b_a - tmp[(int32_T)((int32_T)(3 * jcol) + 1)] * y) + - Q_0[(int32_T)((int32_T)(3 * jcol) + 1)] * 2.0F; - b_a_0[(int32_T)(2 + (int32_T)(3 * jcol))] = ((real32_T)I[(int32_T)((int32_T) - (3 * jcol) + 2)] * b_a - tmp[(int32_T)((int32_T)(3 * jcol) + 2)] * y) + - Q_0[(int32_T)((int32_T)(3 * jcol) + 2)] * 2.0F; - } - - for (jcol = 0; jcol < 3; jcol++) { - vec_out[jcol] = (real_T)((b_a_0[(int32_T)(jcol + 3)] * (real32_T)vector[1] + - b_a_0[jcol] * (real32_T)vector[0]) + b_a_0[(int32_T)(jcol + 6)] * - (real32_T)vector[2]); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/gdjmmglnmgdjlfkf_quat_rotation_vec.h b/gnc/matlab/code_generation/sharedutils/gdjmmglnmgdjlfkf_quat_rotation_vec.h deleted file mode 100644 index 4f0470476f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/gdjmmglnmgdjlfkf_quat_rotation_vec.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: gdjmmglnmgdjlfkf_quat_rotation_vec.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_gdjmmglnmgdjlfkf_quat_rotation_vec -#define SHARE_gdjmmglnmgdjlfkf_quat_rotation_vec -#include "rtwtypes.h" - -extern void gdjmmglnmgdjlfkf_quat_rotation_vec(real_T vector[3], const real32_T - Q[4], real_T vec_out[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/glfcngdjgdjmmglf_pinv.cpp b/gnc/matlab/code_generation/sharedutils/glfcngdjgdjmmglf_pinv.cpp deleted file mode 100644 index 6db8e4fb77..0000000000 --- a/gnc/matlab/code_generation/sharedutils/glfcngdjgdjmmglf_pinv.cpp +++ /dev/null @@ -1,341 +0,0 @@ -// -// File: glfcngdjgdjmmglf_pinv.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "cjmolnohmglfbiec_svd.h" -#include "lfcjmoppknohbimo_svd.h" -#include "glfcngdjgdjmmglf_pinv.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void glfcngdjgdjmmglf_pinv(const real32_T A_data[], const int32_T A_sizes[2], - real32_T X_data[], int32_T X_sizes[2]) -{ - int32_T m; - int32_T vcol; - real32_T tol; - int32_T n; - boolean_T b_p; - int32_T br; - int32_T ar; - int32_T ia; - int32_T d; - int32_T ib; - int32_T b_ic; - int32_T d_ic; - int32_T loop_ub; - real32_T V_data[9]; - int32_T V_sizes[2]; - real32_T U1_data[96]; - int32_T U1_sizes[2]; - real32_T V1_data[9]; - int32_T V1_sizes[2]; - real32_T S_data[9]; - int32_T S_sizes[2]; - real32_T x_data[9]; - real32_T b_X_data[6]; - real32_T b_V_data[4]; - real32_T b_U1_data[6]; - real32_T b_V1_data[4]; - real32_T c_S_data[4]; - real32_T tmp_data[96]; - int32_T tmp_sizes[2]; - real32_T tmp_data_0[6]; - int32_T b_X_sizes_idx_0; - int8_T S1_idx_0; - int8_T S1_idx_1; - int8_T b_idx_0; - int8_T b_idx_1; - if (A_sizes[0] < 3) { - loop_ub = A_sizes[0]; - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - tmp_data[(int32_T)(3 * ar)] = A_data[ar]; - tmp_data[(int32_T)(1 + (int32_T)(3 * ar))] = A_data[(int32_T)(ar + - A_sizes[0])]; - tmp_data[(int32_T)(2 + (int32_T)(3 * ar))] = A_data[(int32_T)((int32_T) - (A_sizes[0] << 1) + ar)]; - } - - m = A_sizes[0]; - b_idx_0 = (int8_T)A_sizes[0]; - b_X_sizes_idx_0 = (int32_T)b_idx_0; - loop_ub = (int32_T)((int32_T)b_idx_0 * 3); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - b_X_data[ar] = 0.0F; - } - - if (A_sizes[0] != 0) { - b_p = true; - n = (int32_T)(3 * A_sizes[0]); - for (vcol = 0; vcol <= (int32_T)(n - 1); vcol++) { - if (b_p && ((!rtIsInfF(tmp_data[vcol])) && (!rtIsNaNF(tmp_data[vcol])))) - { - } else { - b_p = false; - } - } - - if (b_p) { - tmp_sizes[0] = 3; - tmp_sizes[1] = A_sizes[0]; - lfcjmoppknohbimo_svd(tmp_data, tmp_sizes, b_U1_data, U1_sizes, c_S_data, - V1_sizes, b_V_data, V_sizes); - } else { - b_idx_1 = (int8_T)A_sizes[0]; - tmp_sizes[0] = 3; - tmp_sizes[1] = (int32_T)b_idx_1; - loop_ub = (int32_T)(3 * (int32_T)b_idx_1); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - tmp_data_0[ar] = 0.0F; - } - - lfcjmoppknohbimo_svd(tmp_data_0, tmp_sizes, b_U1_data, U1_sizes, - b_V_data, V_sizes, b_V1_data, S_sizes); - loop_ub = U1_sizes[1]; - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - b_U1_data[(int32_T)(3 * ar)] = (rtNaNF); - b_U1_data[(int32_T)(1 + (int32_T)(3 * ar))] = (rtNaNF); - b_U1_data[(int32_T)(2 + (int32_T)(3 * ar))] = (rtNaNF); - } - - S1_idx_0 = (int8_T)V_sizes[0]; - b_idx_0 = (int8_T)V_sizes[0]; - S1_idx_1 = (int8_T)V_sizes[1]; - b_idx_1 = (int8_T)V_sizes[1]; - loop_ub = (int32_T)((int32_T)S1_idx_0 * (int32_T)S1_idx_1); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - V1_data[ar] = 0.0F; - } - - if ((int32_T)b_idx_0 <= (int32_T)b_idx_1) { - n = (int32_T)b_idx_0; - } else { - n = (int32_T)b_idx_1; - } - - for (vcol = 0; (int32_T)(vcol + 1) <= n; vcol++) { - V1_data[(int32_T)(vcol + (int32_T)((int32_T)S1_idx_0 * vcol))] = - (rtNaNF); - } - - V1_sizes[0] = (int32_T)S1_idx_0; - loop_ub = (int32_T)((int32_T)S1_idx_0 * (int32_T)S1_idx_1); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - c_S_data[ar] = V1_data[ar]; - } - - V_sizes[0] = S_sizes[0]; - V_sizes[1] = S_sizes[1]; - loop_ub = (int32_T)(S_sizes[0] * S_sizes[1]); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - b_V_data[ar] = (rtNaNF); - } - } - - tol = 3.0F * c_S_data[0] * 1.1920929E-7F; - n = 0; - vcol = 0; - while (((int32_T)(vcol + 1) <= m) && (c_S_data[(int32_T)((int32_T) - (V1_sizes[0] * vcol) + vcol)] > tol)) { - n++; - vcol++; - } - - if (n > 0) { - vcol = 0; - for (br = 0; (int32_T)(br + 1) <= n; br++) { - tol = 1.0F / c_S_data[(int32_T)((int32_T)(V1_sizes[0] * br) + br)]; - loop_ub = (int32_T)(V_sizes[0] * V_sizes[1]); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - U1_data[ar] = b_V_data[ar]; - } - - loop_ub = (int32_T)(vcol + m); - for (ar = vcol; (int32_T)(ar + 1) <= loop_ub; ar++) { - U1_data[ar] *= tol; - } - - loop_ub = (int32_T)(V_sizes[0] * V_sizes[1]); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - b_V_data[ar] = U1_data[ar]; - } - - vcol += m; - } - - vcol = (int32_T)(A_sizes[0] << 1); - br = 0; - while ((m > 0) && (br <= vcol)) { - loop_ub = (int32_T)(br + m); - for (ar = br; (int32_T)(ar + 1) <= loop_ub; ar++) { - b_X_data[ar] = 0.0F; - } - - br += m; - } - - br = -1; - loop_ub = 0; - while ((m > 0) && (loop_ub <= vcol)) { - ar = 0; - br++; - d = (int32_T)((int32_T)((int32_T)((int32_T)(n - 1) * 3) + br) + 1); - for (ib = br; (int32_T)(ib + 1) <= d; ib += 3) { - if (b_U1_data[ib] != 0.0F) { - ia = ar; - b_ic = (int32_T)(loop_ub + m); - for (d_ic = loop_ub; (int32_T)(d_ic + 1) <= b_ic; d_ic++) { - ia++; - b_X_data[d_ic] += b_V_data[(int32_T)(ia - 1)] * b_U1_data[ib]; - } - } - - ar += m; - } - - loop_ub += m; - } - } - } - - X_sizes[0] = 3; - X_sizes[1] = b_X_sizes_idx_0; - for (ar = 0; ar <= (int32_T)(b_X_sizes_idx_0 - 1); ar++) { - X_data[(int32_T)(3 * ar)] = b_X_data[ar]; - X_data[(int32_T)(1 + (int32_T)(3 * ar))] = b_X_data[(int32_T)(ar + - b_X_sizes_idx_0)]; - X_data[(int32_T)(2 + (int32_T)(3 * ar))] = b_X_data[(int32_T)((int32_T) - (b_X_sizes_idx_0 << 1) + ar)]; - } - } else { - m = A_sizes[0]; - b_idx_1 = (int8_T)A_sizes[0]; - X_sizes[0] = 3; - X_sizes[1] = (int32_T)b_idx_1; - loop_ub = (int32_T)(3 * (int32_T)b_idx_1); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - X_data[ar] = 0.0F; - } - - b_p = true; - n = (int32_T)(A_sizes[0] * 3); - for (vcol = 0; vcol <= (int32_T)(n - 1); vcol++) { - if (b_p && ((!rtIsInfF(A_data[vcol])) && (!rtIsNaNF(A_data[vcol])))) { - } else { - b_p = false; - } - } - - if (b_p) { - cjmolnohmglfbiec_svd(A_data, A_sizes, U1_data, U1_sizes, S_data, S_sizes, - V_data, V_sizes); - } else { - b_idx_0 = (int8_T)A_sizes[0]; - tmp_sizes[0] = (int32_T)b_idx_0; - tmp_sizes[1] = 3; - loop_ub = (int32_T)((int32_T)b_idx_0 * 3); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - tmp_data[ar] = 0.0F; - } - - cjmolnohmglfbiec_svd(tmp_data, tmp_sizes, U1_data, U1_sizes, V_data, - V_sizes, V1_data, V1_sizes); - loop_ub = U1_sizes[1]; - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - n = U1_sizes[0]; - for (vcol = 0; vcol <= (int32_T)(n - 1); vcol++) { - U1_data[(int32_T)(vcol + (int32_T)(U1_sizes[0] * ar))] = (rtNaNF); - } - } - - for (ar = 0; ar < 9; ar++) { - V1_data[ar] = 0.0F; - } - - for (n = 0; (int32_T)(n + 1) < 4; n++) { - V1_data[(int32_T)(n + (int32_T)(3 * n))] = (rtNaNF); - } - - S_sizes[0] = 3; - V_sizes[0] = 3; - V_sizes[1] = 3; - for (ar = 0; ar < 9; ar++) { - S_data[ar] = V1_data[ar]; - V_data[ar] = (rtNaNF); - } - } - - tol = (real32_T)A_sizes[0] * S_data[0] * 1.1920929E-7F; - n = 0; - vcol = 0; - while (((int32_T)(vcol + 1) < 4) && (S_data[(int32_T)((int32_T)(S_sizes[0] * - vcol) + vcol)] > tol)) { - n++; - vcol++; - } - - if (n > 0) { - vcol = 0; - for (br = 0; (int32_T)(br + 1) <= n; br++) { - tol = 1.0F / S_data[(int32_T)((int32_T)(S_sizes[0] * br) + br)]; - loop_ub = (int32_T)(V_sizes[0] * V_sizes[1]); - for (ar = 0; ar <= (int32_T)(loop_ub - 1); ar++) { - x_data[ar] = V_data[ar]; - } - - for (loop_ub = vcol; (int32_T)(loop_ub + 1) <= (int32_T)(vcol + 3); - loop_ub++) { - x_data[loop_ub] *= tol; - } - - V_sizes[0] = 3; - V_sizes[1] = 3; - for (ar = 0; ar < 9; ar++) { - V_data[ar] = x_data[ar]; - } - - vcol += 3; - } - - vcol = (int32_T)((int32_T)(A_sizes[0] - 1) * 3); - for (br = 0; br <= vcol; br += 3) { - for (loop_ub = (int32_T)(br + 1); loop_ub <= (int32_T)(br + 3); loop_ub - ++) { - X_data[(int32_T)(loop_ub - 1)] = 0.0F; - } - } - - br = -1; - for (loop_ub = 0; loop_ub <= vcol; loop_ub += 3) { - ar = 0; - br++; - d = (int32_T)((int32_T)((int32_T)((int32_T)(n - 1) * m) + br) + 1); - for (ib = br; (int32_T)(ib + 1) <= d; ib += m) { - if (U1_data[ib] != 0.0F) { - ia = ar; - for (b_ic = loop_ub; (int32_T)(b_ic + 1) <= (int32_T)(loop_ub + 3); - b_ic++) { - ia++; - X_data[b_ic] += V_data[(int32_T)(ia - 1)] * U1_data[ib]; - } - } - - ar += 3; - } - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/glfcngdjgdjmmglf_pinv.h b/gnc/matlab/code_generation/sharedutils/glfcngdjgdjmmglf_pinv.h deleted file mode 100644 index eeb9cc5e07..0000000000 --- a/gnc/matlab/code_generation/sharedutils/glfcngdjgdjmmglf_pinv.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: glfcngdjgdjmmglf_pinv.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_glfcngdjgdjmmglf_pinv -#define SHARE_glfcngdjgdjmmglf_pinv -#include "rtwtypes.h" - -extern void glfcngdjgdjmmglf_pinv(const real32_T A_data[], const int32_T - A_sizes[2], real32_T X_data[], int32_T X_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/glfkdbaibaaakfcb_xscal.cpp b/gnc/matlab/code_generation/sharedutils/glfkdbaibaaakfcb_xscal.cpp deleted file mode 100644 index 10326282f2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/glfkdbaibaaakfcb_xscal.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: glfkdbaibaaakfcb_xscal.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "glfkdbaibaaakfcb_xscal.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void glfkdbaibaaakfcb_xscal(real32_T a, real32_T x[16], int32_T ix0) -{ - int32_T k; - for (k = ix0; k <= (int32_T)(ix0 + 3); k++) { - x[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/glfkdbaibaaakfcb_xscal.h b/gnc/matlab/code_generation/sharedutils/glfkdbaibaaakfcb_xscal.h deleted file mode 100644 index fded2ade06..0000000000 --- a/gnc/matlab/code_generation/sharedutils/glfkdbaibaaakfcb_xscal.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: glfkdbaibaaakfcb_xscal.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_glfkdbaibaaakfcb_xscal -#define SHARE_glfkdbaibaaakfcb_xscal -#include "rtwtypes.h" - -extern void glfkdbaibaaakfcb_xscal(real32_T a, real32_T x[16], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/glfkgdbifknoecba_all.cpp b/gnc/matlab/code_generation/sharedutils/glfkgdbifknoecba_all.cpp deleted file mode 100644 index 6d5685b0aa..0000000000 --- a/gnc/matlab/code_generation/sharedutils/glfkgdbifknoecba_all.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// -// File: glfkgdbifknoecba_all.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "glfkgdbifknoecba_all.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T glfkgdbifknoecba_all(const boolean_T x[4]) -{ - boolean_T y; - int32_T k; - boolean_T exitg1; - y = true; - k = 0; - exitg1 = false; - while ((!exitg1) && (k < 4)) { - if (!x[k]) { - y = false; - exitg1 = true; - } else { - k++; - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/glfkgdbifknoecba_all.h b/gnc/matlab/code_generation/sharedutils/glfkgdbifknoecba_all.h deleted file mode 100644 index f9aeabe51f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/glfkgdbifknoecba_all.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: glfkgdbifknoecba_all.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_glfkgdbifknoecba_all -#define SHARE_glfkgdbifknoecba_all -#include "rtwtypes.h" - -extern boolean_T glfkgdbifknoecba_all(const boolean_T x[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbaohdbkngdbimo_PadeApproximantOfDegree.cpp b/gnc/matlab/code_generation/sharedutils/hdbaohdbkngdbimo_PadeApproximantOfDegree.cpp deleted file mode 100644 index 843845d46e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbaohdbkngdbimo_PadeApproximantOfDegree.cpp +++ /dev/null @@ -1,169 +0,0 @@ -// -// File: hdbaohdbkngdbimo_PadeApproximantOfDegree.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "djecfknojekfgdba_mldivide.h" -#include "hdbaohdbkngdbimo_PadeApproximantOfDegree.h" - -// Function for MATLAB Function: '/MATLAB Function' -void hdbaohdbkngdbimo_PadeApproximantOfDegree(const real32_T A[225], uint8_T m, - real32_T F[225]) -{ - real32_T A2[225]; - int32_T d; - real32_T A3[225]; - real32_T A4[225]; - int32_T d_k; - real32_T A_0[225]; - int32_T i; - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - A2[(int32_T)(d_k + (int32_T)(15 * d))] = 0.0F; - for (i = 0; i < 15; i++) { - A2[(int32_T)(d_k + (int32_T)(15 * d))] += A[(int32_T)((int32_T)(15 * i) - + d_k)] * A[(int32_T)((int32_T)(15 * d) + i)]; - } - } - } - - if ((int32_T)m == 3) { - memcpy(&F[0], &A2[0], (uint32_T)(225U * sizeof(real32_T))); - for (d = 0; d < 15; d++) { - F[(int32_T)(d + (int32_T)(15 * d))] += 60.0F; - } - - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - A_0[(int32_T)(d_k + (int32_T)(15 * d))] = 0.0F; - for (i = 0; i < 15; i++) { - A_0[(int32_T)(d_k + (int32_T)(15 * d))] += A[(int32_T)((int32_T)(15 * - i) + d_k)] * F[(int32_T)((int32_T)(15 * d) + i)]; - } - } - } - - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - F[(int32_T)(d_k + (int32_T)(15 * d))] = A_0[(int32_T)((int32_T)(15 * d) - + d_k)]; - } - } - - for (d = 0; d < 225; d++) { - A4[d] = 12.0F * A2[d]; - } - - d = 120; - } else { - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - A3[(int32_T)(d_k + (int32_T)(15 * d))] = 0.0F; - for (i = 0; i < 15; i++) { - A3[(int32_T)(d_k + (int32_T)(15 * d))] += A2[(int32_T)((int32_T)(15 * - i) + d_k)] * A2[(int32_T)((int32_T)(15 * d) + i)]; - } - } - } - - if ((int32_T)m == 5) { - for (d = 0; d < 225; d++) { - F[d] = 420.0F * A2[d] + A3[d]; - } - - for (d = 0; d < 15; d++) { - F[(int32_T)(d + (int32_T)(15 * d))] += 15120.0F; - } - - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - A_0[(int32_T)(d_k + (int32_T)(15 * d))] = 0.0F; - for (i = 0; i < 15; i++) { - A_0[(int32_T)(d_k + (int32_T)(15 * d))] += A[(int32_T)((int32_T)(15 * - i) + d_k)] * F[(int32_T)((int32_T)(15 * d) + i)]; - } - } - } - - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - F[(int32_T)(d_k + (int32_T)(15 * d))] = A_0[(int32_T)((int32_T)(15 * d) - + d_k)]; - } - } - - for (d = 0; d < 225; d++) { - A4[d] = 30.0F * A3[d] + 3360.0F * A2[d]; - } - - d = 30240; - } else { - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - A4[(int32_T)(d_k + (int32_T)(15 * d))] = 0.0F; - for (i = 0; i < 15; i++) { - A4[(int32_T)(d_k + (int32_T)(15 * d))] += A3[(int32_T)((int32_T)(15 * - i) + d_k)] * A2[(int32_T)((int32_T)(15 * d) + i)]; - } - } - } - - for (d = 0; d < 225; d++) { - F[d] = (1512.0F * A3[d] + A4[d]) + 277200.0F * A2[d]; - } - - for (d = 0; d < 15; d++) { - F[(int32_T)(d + (int32_T)(15 * d))] += 8.64864E+6F; - } - - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - A_0[(int32_T)(d_k + (int32_T)(15 * d))] = 0.0F; - for (i = 0; i < 15; i++) { - A_0[(int32_T)(d_k + (int32_T)(15 * d))] += A[(int32_T)((int32_T)(15 * - i) + d_k)] * F[(int32_T)((int32_T)(15 * d) + i)]; - } - } - } - - for (d = 0; d < 15; d++) { - for (d_k = 0; d_k < 15; d_k++) { - F[(int32_T)(d_k + (int32_T)(15 * d))] = A_0[(int32_T)((int32_T)(15 * d) - + d_k)]; - } - } - - for (d = 0; d < 225; d++) { - A4[d] = (56.0F * A4[d] + 25200.0F * A3[d]) + 1.99584E+6F * A2[d]; - } - - d = 17297280; - } - } - - for (d_k = 0; d_k < 15; d_k++) { - A4[(int32_T)(d_k + (int32_T)(15 * d_k))] += (real32_T)d; - } - - for (d = 0; d < 225; d++) { - A4[d] -= F[d]; - F[d] *= 2.0F; - } - - djecfknojekfgdba_mldivide(A4, F); - for (d = 0; d < 15; d++) { - F[(int32_T)(d + (int32_T)(15 * d))]++; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbaohdbkngdbimo_PadeApproximantOfDegree.h b/gnc/matlab/code_generation/sharedutils/hdbaohdbkngdbimo_PadeApproximantOfDegree.h deleted file mode 100644 index 9e25f80d88..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbaohdbkngdbimo_PadeApproximantOfDegree.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: hdbaohdbkngdbimo_PadeApproximantOfDegree.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_hdbaohdbkngdbimo_PadeApproximantOfDegree -#define SHARE_hdbaohdbkngdbimo_PadeApproximantOfDegree -#include "rtwtypes.h" - -extern void hdbaohdbkngdbimo_PadeApproximantOfDegree(const real32_T A[225], - uint8_T m, real32_T F[225]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbiaaaabiecmgdb_do_vectors.cpp b/gnc/matlab/code_generation/sharedutils/hdbiaaaabiecmgdb_do_vectors.cpp deleted file mode 100644 index adef88c17f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbiaaaabiecmgdb_do_vectors.cpp +++ /dev/null @@ -1,148 +0,0 @@ -// -// File: hdbiaaaabiecmgdb_do_vectors.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "kngdbaaaiecjbiec_skip_to_last_equal_value.h" -#include "mgdjimgdopphcjek_skip_to_last_equal_value.h" -#include "hdbiaaaabiecmgdb_do_vectors.h" - -// Function for MATLAB Function: '/generate_output' -void hdbiaaaabiecmgdb_do_vectors(const real32_T a[50], const real32_T b_data[], - const int32_T b_sizes, real32_T c_data[], int32_T *c_sizes, int32_T ia_data[], - int32_T *ia_sizes, int32_T ib_data[], int32_T *ib_sizes) -{ - int32_T nc; - int32_T iafirst; - int32_T ialast; - int32_T ibfirst; - int32_T iblast; - int32_T b_ialast; - real32_T ak; - int32_T b_iblast; - real32_T bk; - boolean_T p; - real32_T absxk; - int32_T exponent; - int32_T ia_data_0[50]; - real32_T c_data_0[50]; - if (50 <= b_sizes) { - *ia_sizes = 50; - } else { - *ia_sizes = b_sizes; - } - - *c_sizes = (int32_T)(int8_T)*ia_sizes; - *ib_sizes = *ia_sizes; - nc = 0; - iafirst = 0; - ialast = 1; - ibfirst = 0; - iblast = 1; - while ((ialast <= 50) && (iblast <= b_sizes)) { - b_ialast = ialast; - ak = kngdbaaaiecjbiec_skip_to_last_equal_value(&b_ialast, a); - ialast = b_ialast; - b_iblast = iblast; - bk = mgdjimgdopphcjek_skip_to_last_equal_value(&b_iblast, b_data, b_sizes); - iblast = b_iblast; - absxk = (real32_T)fabs((real_T)(bk / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, (int32_T)(exponent - 24)); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(bk - ak)) < absxk) || (rtIsInfF(ak) && rtIsInfF - (bk) && ((ak > 0.0F) == (bk > 0.0F)))) { - p = true; - } else { - p = false; - } - - if (p) { - nc++; - c_data[(int32_T)(nc - 1)] = ak; - ia_data[(int32_T)(nc - 1)] = (int32_T)(iafirst + 1); - ib_data[(int32_T)(nc - 1)] = (int32_T)(ibfirst + 1); - ialast = (int32_T)(b_ialast + 1); - iafirst = b_ialast; - iblast = (int32_T)(b_iblast + 1); - ibfirst = b_iblast; - } else { - if ((ak < bk) || rtIsNaNF(bk)) { - p = true; - } - - if (p) { - ialast = (int32_T)(b_ialast + 1); - iafirst = b_ialast; - } else { - iblast = (int32_T)(b_iblast + 1); - ibfirst = b_iblast; - } - } - } - - if (*ia_sizes > 0) { - if (1 > nc) { - *ia_sizes = 0; - } else { - *ia_sizes = nc; - } - - for (iafirst = 0; iafirst <= (int32_T)(*ia_sizes - 1); iafirst++) { - ia_data_0[iafirst] = ia_data[iafirst]; - } - - for (iafirst = 0; iafirst <= (int32_T)(*ia_sizes - 1); iafirst++) { - ia_data[iafirst] = ia_data_0[iafirst]; - } - - if (1 > nc) { - *ib_sizes = 0; - } else { - *ib_sizes = nc; - } - - for (iafirst = 0; iafirst <= (int32_T)(*ib_sizes - 1); iafirst++) { - ia_data_0[iafirst] = ib_data[iafirst]; - } - - for (iafirst = 0; iafirst <= (int32_T)(*ib_sizes - 1); iafirst++) { - ib_data[iafirst] = ia_data_0[iafirst]; - } - - if (1 > nc) { - nc = 0; - } - - for (iafirst = 0; iafirst <= (int32_T)(nc - 1); iafirst++) { - c_data_0[iafirst] = c_data[iafirst]; - } - - *c_sizes = nc; - for (iafirst = 0; iafirst <= (int32_T)(nc - 1); iafirst++) { - c_data[iafirst] = c_data_0[iafirst]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbiaaaabiecmgdb_do_vectors.h b/gnc/matlab/code_generation/sharedutils/hdbiaaaabiecmgdb_do_vectors.h deleted file mode 100644 index 53e4bdcfeb..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbiaaaabiecmgdb_do_vectors.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: hdbiaaaabiecmgdb_do_vectors.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_hdbiaaaabiecmgdb_do_vectors -#define SHARE_hdbiaaaabiecmgdb_do_vectors -#include "rtwtypes.h" - -extern void hdbiaaaabiecmgdb_do_vectors(const real32_T a[50], const real32_T - b_data[], const int32_T b_sizes, real32_T c_data[], int32_T *c_sizes, int32_T - ia_data[], int32_T *ia_sizes, int32_T ib_data[], int32_T *ib_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbihlngknopkfcj_repmat.cpp b/gnc/matlab/code_generation/sharedutils/hdbihlngknopkfcj_repmat.cpp deleted file mode 100644 index 00e8bf5bc3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbihlngknopkfcj_repmat.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// File: hdbihlngknopkfcj_repmat.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "hdbihlngknopkfcj_repmat.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void hdbihlngknopkfcj_repmat(real_T b[256]) -{ - int32_T iacol; - int32_T ibmat; - int32_T ibcol; - int32_T jcol; - int32_T itilerow; - static const int8_T c[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; - - for (jcol = 0; jcol < 4; jcol++) { - iacol = (int32_T)(jcol << 2); - ibmat = (int32_T)(jcol << 6); - for (itilerow = 0; itilerow < 16; itilerow++) { - ibcol = (int32_T)((int32_T)(itilerow << 2) + ibmat); - b[ibcol] = (real_T)c[iacol]; - b[(int32_T)(ibcol + 1)] = (real_T)c[(int32_T)(iacol + 1)]; - b[(int32_T)(ibcol + 2)] = (real_T)c[(int32_T)(iacol + 2)]; - b[(int32_T)(ibcol + 3)] = (real_T)c[(int32_T)(iacol + 3)]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbihlngknopkfcj_repmat.h b/gnc/matlab/code_generation/sharedutils/hdbihlngknopkfcj_repmat.h deleted file mode 100644 index 6528226238..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbihlngknopkfcj_repmat.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: hdbihlngknopkfcj_repmat.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_hdbihlngknopkfcj_repmat -#define SHARE_hdbihlngknopkfcj_repmat -#include "rtwtypes.h" - -extern void hdbihlngknopkfcj_repmat(real_T b[256]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbiimglekngjmoh_cat.cpp b/gnc/matlab/code_generation/sharedutils/hdbiimglekngjmoh_cat.cpp deleted file mode 100644 index 2c47d3a58a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbiimglekngjmoh_cat.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// File: hdbiimglekngjmoh_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:43:55 2017 -// -#include "rtwtypes.h" -#include "hdbiimglekngjmoh_cat.h" - -// Function for MATLAB Function: '/generate_output' -void hdbiimglekngjmoh_cat(const uint8_T varargin_2_data[], const int32_T - varargin_2_sizes[2], const uint8_T varargin_3_data[], const int32_T - varargin_3_sizes[2], uint8_T y_data[], int32_T y_sizes[2]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = 50; - y_sizes[1] = 16; - iy = -1; - for (j = 0; j < 50; j++) { - iy++; - y_data[iy] = 0U; - } - - j = (int32_T)(50 * varargin_2_sizes[1]); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_2_data[(int32_T)(b_j - 1)]; - } - - j = (int32_T)(50 * varargin_3_sizes[1]); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_3_data[(int32_T)(b_j - 1)]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbiimglekngjmoh_cat.h b/gnc/matlab/code_generation/sharedutils/hdbiimglekngjmoh_cat.h deleted file mode 100644 index 7bd5ca5d7e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbiimglekngjmoh_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: hdbiimglekngjmoh_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:43:55 2017 -// -#ifndef SHARE_hdbiimglekngjmoh_cat -#define SHARE_hdbiimglekngjmoh_cat -#include "rtwtypes.h" - -extern void hdbiimglekngjmoh_cat(const uint8_T varargin_2_data[], const int32_T - varargin_2_sizes[2], const uint8_T varargin_3_data[], const int32_T - varargin_3_sizes[2], uint8_T y_data[], int32_T y_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbinophmgdjjmoh_xrotg.cpp b/gnc/matlab/code_generation/sharedutils/hdbinophmgdjjmoh_xrotg.cpp deleted file mode 100644 index 1ec5d534a2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbinophmgdjjmoh_xrotg.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// -// File: hdbinophmgdjjmoh_xrotg.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "hdbinophmgdjjmoh_xrotg.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void hdbinophmgdjjmoh_xrotg(real32_T *a, real32_T *b, real32_T *c, real32_T *s) -{ - real32_T roe; - real32_T absa; - real32_T absb; - real32_T scale; - real32_T ads; - real32_T bds; - roe = *b; - absa = (real32_T)fabs((real_T)*a); - absb = (real32_T)fabs((real_T)*b); - if (absa > absb) { - roe = *a; - } - - scale = absa + absb; - if (scale == 0.0F) { - *s = 0.0F; - *c = 1.0F; - scale = 0.0F; - absa = 0.0F; - } else { - ads = absa / scale; - bds = absb / scale; - scale *= (real32_T)sqrt((real_T)(ads * ads + bds * bds)); - if (roe < 0.0F) { - scale = -scale; - } - - *c = *a / scale; - *s = *b / scale; - if (absa > absb) { - absa = *s; - } else if (*c != 0.0F) { - absa = 1.0F / *c; - } else { - absa = 1.0F; - } - } - - *a = scale; - *b = absa; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdbinophmgdjjmoh_xrotg.h b/gnc/matlab/code_generation/sharedutils/hdbinophmgdjjmoh_xrotg.h deleted file mode 100644 index 53414da113..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdbinophmgdjjmoh_xrotg.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: hdbinophmgdjjmoh_xrotg.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_hdbinophmgdjjmoh_xrotg -#define SHARE_hdbinophmgdjjmoh_xrotg -#include "rtwtypes.h" - -extern void hdbinophmgdjjmoh_xrotg(real32_T *a, real32_T *b, real32_T *c, - real32_T *s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdjmknohknopimgl_sort.cpp b/gnc/matlab/code_generation/sharedutils/hdjmknohknopimgl_sort.cpp deleted file mode 100644 index e11bedf2f3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdjmknohknopimgl_sort.cpp +++ /dev/null @@ -1,219 +0,0 @@ -// -// File: hdjmknohknopimgl_sort.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include -#include "phlnlfkfphdjohln_merge.h" -#include "rt_nonfinite.h" -#include "hdjmknohknopimgl_sort.h" - -// Function for MATLAB Function: '/generate_output' -void hdjmknohknopimgl_sort(real32_T x[50], int32_T idx[50]) -{ - int32_T nNaNs; - real32_T xwork[50]; - int32_T iwork[50]; - real32_T x4[4]; - int8_T idx4[4]; - int32_T ib; - int32_T m; - int8_T perm[4]; - int32_T i3; - int32_T i4; - int32_T tailOffset; - int32_T nTail; - x4[0] = 0.0F; - idx4[0] = 0; - x4[1] = 0.0F; - idx4[1] = 0; - x4[2] = 0.0F; - idx4[2] = 0; - x4[3] = 0.0F; - idx4[3] = 0; - memset(&idx[0], 0, (uint32_T)(50U * sizeof(int32_T))); - memset(&xwork[0], 0, (uint32_T)(50U * sizeof(real32_T))); - nNaNs = 0; - ib = 0; - for (m = 0; m < 50; m++) { - if (rtIsNaNF(x[m])) { - idx[(int32_T)(49 - nNaNs)] = (int32_T)(m + 1); - xwork[(int32_T)(49 - nNaNs)] = x[m]; - nNaNs++; - } else { - ib++; - idx4[(int32_T)(ib - 1)] = (int8_T)(int32_T)(m + 1); - x4[(int32_T)(ib - 1)] = x[m]; - if (ib == 4) { - ib = (int32_T)(m - nNaNs); - if (x4[0] <= x4[1]) { - tailOffset = 1; - nTail = 2; - } else { - tailOffset = 2; - nTail = 1; - } - - if (x4[2] <= x4[3]) { - i3 = 3; - i4 = 4; - } else { - i3 = 4; - i4 = 3; - } - - if (x4[(int32_T)(tailOffset - 1)] <= x4[(int32_T)(i3 - 1)]) { - if (x4[(int32_T)(nTail - 1)] <= x4[(int32_T)(i3 - 1)]) { - perm[0] = (int8_T)tailOffset; - perm[1] = (int8_T)nTail; - perm[2] = (int8_T)i3; - perm[3] = (int8_T)i4; - } else if (x4[(int32_T)(nTail - 1)] <= x4[(int32_T)(i4 - 1)]) { - perm[0] = (int8_T)tailOffset; - perm[1] = (int8_T)i3; - perm[2] = (int8_T)nTail; - perm[3] = (int8_T)i4; - } else { - perm[0] = (int8_T)tailOffset; - perm[1] = (int8_T)i3; - perm[2] = (int8_T)i4; - perm[3] = (int8_T)nTail; - } - } else if (x4[(int32_T)(tailOffset - 1)] <= x4[(int32_T)(i4 - 1)]) { - if (x4[(int32_T)(nTail - 1)] <= x4[(int32_T)(i4 - 1)]) { - perm[0] = (int8_T)i3; - perm[1] = (int8_T)tailOffset; - perm[2] = (int8_T)nTail; - perm[3] = (int8_T)i4; - } else { - perm[0] = (int8_T)i3; - perm[1] = (int8_T)tailOffset; - perm[2] = (int8_T)i4; - perm[3] = (int8_T)nTail; - } - } else { - perm[0] = (int8_T)i3; - perm[1] = (int8_T)i4; - perm[2] = (int8_T)tailOffset; - perm[3] = (int8_T)nTail; - } - - idx[(int32_T)(ib - 3)] = (int32_T)idx4[(int32_T)((int32_T)perm[0] - 1)]; - idx[(int32_T)(ib - 2)] = (int32_T)idx4[(int32_T)((int32_T)perm[1] - 1)]; - idx[(int32_T)(ib - 1)] = (int32_T)idx4[(int32_T)((int32_T)perm[2] - 1)]; - idx[ib] = (int32_T)idx4[(int32_T)((int32_T)perm[3] - 1)]; - x[(int32_T)(ib - 3)] = x4[(int32_T)((int32_T)perm[0] - 1)]; - x[(int32_T)(ib - 2)] = x4[(int32_T)((int32_T)perm[1] - 1)]; - x[(int32_T)(ib - 1)] = x4[(int32_T)((int32_T)perm[2] - 1)]; - x[ib] = x4[(int32_T)((int32_T)perm[3] - 1)]; - ib = 0; - } - } - } - - if (ib > 0) { - perm[1] = 0; - perm[2] = 0; - perm[3] = 0; - if (ib == 1) { - perm[0] = 1; - } else if (ib == 2) { - if (x4[0] <= x4[1]) { - perm[0] = 1; - perm[1] = 2; - } else { - perm[0] = 2; - perm[1] = 1; - } - } else if (x4[0] <= x4[1]) { - if (x4[1] <= x4[2]) { - perm[0] = 1; - perm[1] = 2; - perm[2] = 3; - } else if (x4[0] <= x4[2]) { - perm[0] = 1; - perm[1] = 3; - perm[2] = 2; - } else { - perm[0] = 3; - perm[1] = 1; - perm[2] = 2; - } - } else if (x4[0] <= x4[2]) { - perm[0] = 2; - perm[1] = 1; - perm[2] = 3; - } else if (x4[1] <= x4[2]) { - perm[0] = 2; - perm[1] = 3; - perm[2] = 1; - } else { - perm[0] = 3; - perm[1] = 2; - perm[2] = 1; - } - - for (m = 50; (int32_T)(m - 49) <= ib; m++) { - idx[(int32_T)((int32_T)(m - nNaNs) - ib)] = (int32_T)idx4[(int32_T) - ((int32_T)perm[(int32_T)(m - 50)] - 1)]; - x[(int32_T)((int32_T)(m - nNaNs) - ib)] = x4[(int32_T)((int32_T)perm - [(int32_T)(m - 50)] - 1)]; - } - } - - m = (int32_T)(nNaNs >> 1); - for (ib = 1; ib <= m; ib++) { - tailOffset = idx[(int32_T)((int32_T)(ib - nNaNs) + 49)]; - idx[(int32_T)((int32_T)(ib - nNaNs) + 49)] = idx[(int32_T)(50 - ib)]; - idx[(int32_T)(50 - ib)] = tailOffset; - x[(int32_T)((int32_T)(ib - nNaNs) + 49)] = xwork[(int32_T)(50 - ib)]; - x[(int32_T)(50 - ib)] = xwork[(int32_T)((int32_T)(ib - nNaNs) + 49)]; - } - - if ((int32_T)(nNaNs & 1) != 0) { - x[(int32_T)((int32_T)(m - nNaNs) + 50)] = xwork[(int32_T)((int32_T)(m - - nNaNs) + 50)]; - } - - if ((int32_T)(50 - nNaNs) > 1) { - memset(&iwork[0], 0, (uint32_T)(50U * sizeof(int32_T))); - ib = (int32_T)((int32_T)(50 - nNaNs) >> 2); - m = 4; - while (ib > 1) { - if ((int32_T)(ib & 1) != 0) { - ib--; - tailOffset = (int32_T)(m * ib); - nTail = (int32_T)(50 - (int32_T)(nNaNs + tailOffset)); - if (nTail > m) { - phlnlfkfphdjohln_merge(idx, x, tailOffset, m, (int32_T)(nTail - m), - iwork, xwork); - } - } - - tailOffset = (int32_T)(m << 1); - ib >>= 1; - for (nTail = 1; nTail <= ib; nTail++) { - phlnlfkfphdjohln_merge(idx, x, (int32_T)((int32_T)(nTail - 1) * - tailOffset), m, m, iwork, xwork); - } - - m = tailOffset; - } - - if ((int32_T)(50 - nNaNs) > m) { - phlnlfkfphdjohln_merge(idx, x, 0, m, (int32_T)(50 - (int32_T)(nNaNs + m)), - iwork, xwork); - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hdjmknohknopimgl_sort.h b/gnc/matlab/code_generation/sharedutils/hdjmknohknopimgl_sort.h deleted file mode 100644 index 4b70f6ec85..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hdjmknohknopimgl_sort.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: hdjmknohknopimgl_sort.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_hdjmknohknopimgl_sort -#define SHARE_hdjmknohknopimgl_sort -#include "rtwtypes.h" - -extern void hdjmknohknopimgl_sort(real32_T x[50], int32_T idx[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hlfcbiecmglfkfkf_quaternion_to_rotation.cpp b/gnc/matlab/code_generation/sharedutils/hlfcbiecmglfkfkf_quaternion_to_rotation.cpp deleted file mode 100644 index 650c3ac986..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hlfcbiecmglfkfkf_quaternion_to_rotation.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// -// File: hlfcbiecmglfkfkf_quaternion_to_rotation.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#include "rtwtypes.h" -#include "hlfcbiecmglfkfkf_quaternion_to_rotation.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void hlfcbiecmglfkfkf_quaternion_to_rotation(const real_T q[4], real_T R[9]) -{ - real_T S[9]; - static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - real_T q_0; - real_T q_1; - real_T q_2[12]; - real_T q_3[12]; - int32_T i; - int32_T i_0; - - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // - // 8/11/2015 - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // - // 8/11/2015 - S[0] = 0.0; - S[3] = -q[2]; - S[6] = q[1]; - S[1] = q[2]; - S[4] = 0.0; - S[7] = -q[0]; - S[2] = -q[1]; - S[5] = q[0]; - S[8] = 0.0; - q_0 = q[3]; - q_1 = q[3]; - for (i = 0; i < 3; i++) { - q_2[(int32_T)(3 * i)] = q_0 * (real_T)b[i] + S[i]; - q_3[(int32_T)(i << 2)] = (real_T)b[(int32_T)(3 * i)] * q_1 - S[(int32_T)(3 * - i)]; - q_2[(int32_T)(1 + (int32_T)(3 * i))] = (real_T)b[(int32_T)(i + 3)] * q_0 + - S[(int32_T)(i + 3)]; - q_3[(int32_T)(1 + (int32_T)(i << 2))] = (real_T)b[(int32_T)((int32_T)(3 * i) - + 1)] * q_1 - S[(int32_T)((int32_T)(3 * i) + 1)]; - q_2[(int32_T)(2 + (int32_T)(3 * i))] = (real_T)b[(int32_T)(i + 6)] * q_0 + - S[(int32_T)(i + 6)]; - q_3[(int32_T)(2 + (int32_T)(i << 2))] = (real_T)b[(int32_T)((int32_T)(3 * i) - + 2)] * q_1 - S[(int32_T)((int32_T)(3 * i) + 2)]; - q_2[(int32_T)(9 + i)] = -q[i]; - q_3[(int32_T)(3 + (int32_T)(i << 2))] = -q[i]; - } - - for (i = 0; i < 3; i++) { - for (i_0 = 0; i_0 < 3; i_0++) { - R[(int32_T)(i_0 + (int32_T)(3 * i))] = 0.0; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)(i << 2)] * q_2[i_0]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 1)] * q_2[(int32_T)(i_0 + 3)]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 2)] * q_2[(int32_T)(i_0 + 6)]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 3)] * q_2[(int32_T)(i_0 + 9)]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hlfcbiecmglfkfkf_quaternion_to_rotation.h b/gnc/matlab/code_generation/sharedutils/hlfcbiecmglfkfkf_quaternion_to_rotation.h deleted file mode 100644 index 9d7e5ff220..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hlfcbiecmglfkfkf_quaternion_to_rotation.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: hlfcbiecmglfkfkf_quaternion_to_rotation.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#ifndef SHARE_hlfcbiecmglfkfkf_quaternion_to_rotation -#define SHARE_hlfcbiecmglfkfkf_quaternion_to_rotation -#include "rtwtypes.h" - -extern void hlfcbiecmglfkfkf_quaternion_to_rotation(const real_T q[4], real_T R - [9]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hlfkiecjohlfopph_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/hlfkiecjohlfopph_xaxpy.cpp deleted file mode 100644 index 972f651ac7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hlfkiecjohlfopph_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: hlfkiecjohlfopph_xaxpy.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "hlfkiecjohlfopph_xaxpy.h" - -// Function for MATLAB Function: '/MATLAB Function' -void hlfkiecjohlfopph_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T y[36], - int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * y[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hlfkiecjohlfopph_xaxpy.h b/gnc/matlab/code_generation/sharedutils/hlfkiecjohlfopph_xaxpy.h deleted file mode 100644 index 2066fc5b94..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hlfkiecjohlfopph_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: hlfkiecjohlfopph_xaxpy.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_hlfkiecjohlfopph_xaxpy -#define SHARE_hlfkiecjohlfopph_xaxpy -#include "rtwtypes.h" - -extern void hlfkiecjohlfopph_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T - y[36], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hlnglfcjaiecmohl_eulers_to_quat.cpp b/gnc/matlab/code_generation/sharedutils/hlnglfcjaiecmohl_eulers_to_quat.cpp deleted file mode 100644 index d9dd814892..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hlnglfcjaiecmohl_eulers_to_quat.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// -// File: hlnglfcjaiecmohl_eulers_to_quat.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Thu Aug 17 13:31:41 2017 -// -#include "rtwtypes.h" -#include -#include "hlnglfcjaiecmohl_eulers_to_quat.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void hlnglfcjaiecmohl_eulers_to_quat(real32_T r1, real32_T r2, real32_T r3, - real32_T quat[4]) -{ - real32_T cx; - real32_T sx; - real32_T cy; - real32_T sy; - real32_T cz; - real32_T sz; - - // Convert Euler angles to quaternions. Assumes 3-2-1 rotation sequence for - // euler angles. - // From: Fundamentals of Spacecraft Attitude Determination and Control, F. - // Landix Markley and John L. Crassisdis. Table: B.5 - // - // Jesse. C. Fusco jesse.c.fusco@nasa.gov - cx = (real32_T)cos((real_T)(r3 / 2.0F)); - sx = (real32_T)sin((real_T)(r3 / 2.0F)); - cy = (real32_T)cos((real_T)(r2 / 2.0F)); - sy = (real32_T)sin((real_T)(r2 / 2.0F)); - cz = (real32_T)cos((real_T)(r1 / 2.0F)); - sz = (real32_T)sin((real_T)(r1 / 2.0F)); - quat[0] = cx * cy * sz - sx * sy * cz; - quat[1] = cx * sy * cz + sx * cy * sz; - quat[2] = sx * cy * cz - cx * sy * sz; - quat[3] = cx * cy * cz + sx * sy * sz; - - // function quat = eulers_to_quat(rz, ry, rx) - // - // quat = zeros(size(rz, 1), 4, 'like', rx); - // - // quat(:,1) = cos(rz/2).*cos(ry/2).*sin(rx/2) - sin(rz/2).*sin(ry/2).*cos(rx/2); - // - // quat(:,2) = cos(rz/2).*sin(ry/2).*cos(rx/2) + sin(rz/2).*cos(ry/2).*sin(rx/2); - // - // quat(:,3) = sin(rz/2).*cos(ry/2).*cos(rx/2) - cos(rz/2).*sin(ry/2).*sin(rx/2); - // - // quat(:,4) = cos(rz/2).*cos(ry/2).*cos(rx/2) + sin(rz/2).*sin(ry/2).*sin(rx/2); - // - // %Normalize the quaternion and make the scalar term positive - // idx = find(quat(:,4) < 0); - // quat(idx,:) = quat(idx,:).*-1; - // quat = diag(1./rssrow(quat))*quat; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/hlnglfcjaiecmohl_eulers_to_quat.h b/gnc/matlab/code_generation/sharedutils/hlnglfcjaiecmohl_eulers_to_quat.h deleted file mode 100644 index cd76a5cb2a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/hlnglfcjaiecmohl_eulers_to_quat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: hlnglfcjaiecmohl_eulers_to_quat.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Thu Aug 17 13:31:41 2017 -// -#ifndef SHARE_hlnglfcjaiecmohl_eulers_to_quat -#define SHARE_hlnglfcjaiecmohl_eulers_to_quat -#include "rtwtypes.h" - -extern void hlnglfcjaiecmohl_eulers_to_quat(real32_T r1, real32_T r2, real32_T - r3, real32_T quat[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iecbaaaikfkfaaie_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/iecbaaaikfkfaaie_xzlarf.cpp deleted file mode 100644 index 5245089dc9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iecbaaaikfkfaaie_xzlarf.cpp +++ /dev/null @@ -1,112 +0,0 @@ -// -// File: iecbaaaikfkfaaie_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "iecbaaaikfkfaaie_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void iecbaaaikfkfaaie_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]) -{ - int32_T lastv; - int32_T lastc; - int32_T ix; - real32_T c; - int32_T iy; - int32_T iac; - int32_T d; - int32_T b_ia; - int32_T jy; - boolean_T exitg2; - if (tau != 0.0F) { - lastv = m; - lastc = iv0 + m; - while ((lastv > 0) && (C_data[lastc - 2] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = n; - exitg2 = false; - while ((!exitg2) && (lastc > 0)) { - iy = (lastc - 1) * ldc + ic0; - jy = iy; - do { - iac = 0; - if (jy <= (iy + lastv) - 1) { - if (C_data[jy - 1] != 0.0F) { - iac = 1; - } else { - jy++; - } - } else { - lastc--; - iac = 2; - } - } while (iac == 0); - - if (iac == 1) { - exitg2 = true; - } - } - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - if (lastc != 0) { - for (iy = 1; iy <= lastc; iy++) { - work[iy - 1] = 0.0F; - } - - iy = 0; - jy = (lastc - 1) * ldc + ic0; - iac = ic0; - while ((ldc > 0) && (iac <= jy)) { - ix = iv0; - c = 0.0F; - d = (iac + lastv) - 1; - for (b_ia = iac; b_ia <= d; b_ia++) { - c += C_data[b_ia - 1] * C_data[ix - 1]; - ix++; - } - - work[iy] += c; - iy++; - iac += ldc; - } - } - - if (!(-tau == 0.0F)) { - iy = ic0 - 1; - jy = 0; - for (iac = 1; iac <= lastc; iac++) { - if (work[jy] != 0.0F) { - c = work[jy] * -tau; - ix = iv0; - d = lastv + iy; - for (b_ia = iy; b_ia + 1 <= d; b_ia++) { - C_data[b_ia] += C_data[ix - 1] * c; - ix++; - } - } - - jy++; - iy += ldc; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iecbaaaikfkfaaie_xzlarf.h b/gnc/matlab/code_generation/sharedutils/iecbaaaikfkfaaie_xzlarf.h deleted file mode 100644 index fbcf319219..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iecbaaaikfkfaaie_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: iecbaaaikfkfaaie_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_iecbaaaikfkfaaie_xzlarf -#define SHARE_iecbaaaikfkfaaie_xzlarf -#include "rtwtypes.h" - -extern void iecbaaaikfkfaaie_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iecjbieccbailfcb_abs.cpp b/gnc/matlab/code_generation/sharedutils/iecjbieccbailfcb_abs.cpp deleted file mode 100644 index 1d7c510ee8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iecjbieccbailfcb_abs.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// -// File: iecjbieccbailfcb_abs.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "iecjbieccbailfcb_abs.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void iecjbieccbailfcb_abs(const real32_T x_data[], const int32_T x_sizes, - real32_T y_data[], int32_T *y_sizes) -{ - int32_T k; - *y_sizes = (int32_T)(int8_T)x_sizes; - for (k = 0; (int32_T)(k + 1) <= x_sizes; k++) { - y_data[k] = (real32_T)fabs((real_T)x_data[k]); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iecjbieccbailfcb_abs.h b/gnc/matlab/code_generation/sharedutils/iecjbieccbailfcb_abs.h deleted file mode 100644 index 8b25453aaa..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iecjbieccbailfcb_abs.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: iecjbieccbailfcb_abs.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_iecjbieccbailfcb_abs -#define SHARE_iecjbieccbailfcb_abs -#include "rtwtypes.h" - -extern void iecjbieccbailfcb_abs(const real32_T x_data[], const int32_T x_sizes, - real32_T y_data[], int32_T *y_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iecjnohldjmohlno_qr.cpp b/gnc/matlab/code_generation/sharedutils/iecjnohldjmohlno_qr.cpp deleted file mode 100644 index 9598f50904..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iecjnohldjmohlno_qr.cpp +++ /dev/null @@ -1,168 +0,0 @@ -// -// File: iecjnohldjmohlno_qr.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "baimfcjeeknonglf_xzlarf.h" -#include "knohbaiengdbnoph_xscal.h" -#include "nglnkfcbnophmohd_xgeqrf.h" -#include "nohlfkfkhlfcdbai_xzlarf.h" -#include "ppphhdjekfkfdjmg_xgeqrf.h" -#include "iecjnohldjmohlno_qr.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void iecjnohldjmohlno_qr(const real32_T A_data[], const int32_T A_sizes[2], - real32_T Q_data[], int32_T Q_sizes[2], real32_T R_data[], int32_T R_sizes[2]) -{ - int32_T m; - int32_T i_i; - int32_T itau; - real32_T work[6]; - int32_T iaii; - int32_T c; - int32_T i; - real32_T b_A_data[600]; - int32_T b_A_sizes[2]; - real32_T tau_data[6]; - real32_T b_tau_data[100]; - real32_T b_work_data[100]; - int8_T b_idx_0; - m = A_sizes[0]; - b_idx_0 = (int8_T)A_sizes[0]; - Q_sizes[0] = (int8_T)A_sizes[0]; - Q_sizes[1] = (int8_T)A_sizes[0]; - R_sizes[0] = A_sizes[0]; - R_sizes[1] = A_sizes[1]; - if (A_sizes[0] > 6) { - for (itau = 0; itau < 6; itau++) { - for (i_i = 0; i_i + 1 <= m; i_i++) { - Q_data[i_i + b_idx_0 * itau] = A_data[A_sizes[0] * itau + i_i]; - } - } - - for (itau = 7; itau <= m; itau++) { - for (i_i = 1; i_i <= m; i_i++) { - Q_data[(i_i + b_idx_0 * (itau - 1)) - 1] = 0.0F; - } - } - - ppphhdjekfkfdjmg_xgeqrf(Q_data, Q_sizes, b_tau_data, &itau); - for (itau = 0; itau < 6; itau++) { - for (i_i = 0; i_i + 1 <= itau + 1; i_i++) { - R_data[i_i + R_sizes[0] * itau] = Q_data[Q_sizes[0] * itau + i_i]; - } - - for (i_i = itau + 1; i_i + 1 <= m; i_i++) { - R_data[i_i + R_sizes[0] * itau] = 0.0F; - } - } - - for (itau = 6; itau < m; itau++) { - i_i = itau * m; - for (iaii = 0; iaii < m; iaii++) { - Q_data[i_i + iaii] = 0.0F; - } - - Q_data[i_i + itau] = 1.0F; - } - - itau = 5; - b_idx_0 = (int8_T)Q_sizes[1]; - i = b_idx_0; - i_i = b_idx_0; - for (iaii = 0; iaii < i_i; iaii++) { - b_work_data[iaii] = 0.0F; - } - - for (i_i = 5; i_i >= 0; i_i += -1) { - iaii = i_i * m + i_i; - Q_data[iaii] = 1.0F; - nohlfkfkhlfcdbai_xzlarf(m - i_i, (m - i_i) - 1, iaii + 1, b_tau_data[itau], - Q_data, (iaii + m) + 1, m, b_work_data); - knohbaiengdbnoph_xscal((m - i_i) - 1, -b_tau_data[itau], Q_data, iaii + 2); - Q_data[iaii] = 1.0F - b_tau_data[itau]; - for (c = 1; c <= i_i; c++) { - Q_data[iaii - c] = 0.0F; - } - - itau--; - } - } else { - b_A_sizes[0] = A_sizes[0]; - b_A_sizes[1] = 6; - i_i = A_sizes[0] * A_sizes[1]; - for (iaii = 0; iaii < i_i; iaii++) { - b_A_data[iaii] = A_data[iaii]; - } - - nglnkfcbnophmohd_xgeqrf(b_A_data, b_A_sizes, tau_data, &itau); - for (itau = 0; itau + 1 <= m; itau++) { - for (i_i = 0; i_i + 1 <= itau + 1; i_i++) { - R_data[i_i + R_sizes[0] * itau] = b_A_data[b_A_sizes[0] * itau + i_i]; - } - - for (i_i = itau + 1; i_i + 1 <= m; i_i++) { - R_data[i_i + R_sizes[0] * itau] = 0.0F; - } - } - - for (itau = A_sizes[0]; itau + 1 < 7; itau++) { - for (i_i = 0; i_i + 1 <= m; i_i++) { - R_data[i_i + R_sizes[0] * itau] = b_A_data[b_A_sizes[0] * itau + i_i]; - } - } - - if (!(A_sizes[0] < 1)) { - for (itau = A_sizes[0]; itau < m; itau++) { - i_i = itau * m; - for (iaii = 0; iaii < m; iaii++) { - b_A_data[i_i + iaii] = 0.0F; - } - - b_A_data[i_i + itau] = 1.0F; - } - - itau = A_sizes[0] - 1; - for (i = 0; i < 6; i++) { - work[i] = 0.0F; - } - - for (i_i = A_sizes[0]; i_i >= 1; i_i--) { - iaii = (i_i - 1) * m + i_i; - if (i_i < m) { - b_A_data[iaii - 1] = 1.0F; - baimfcjeeknonglf_xzlarf((m - i_i) + 1, m - i_i, iaii, tau_data[itau], - b_A_data, iaii + m, m, work); - c = (iaii + m) - i_i; - for (i = iaii; i + 1 <= c; i++) { - b_A_data[i] *= -tau_data[itau]; - } - } - - b_A_data[iaii - 1] = 1.0F - tau_data[itau]; - for (c = 1; c < i_i; c++) { - b_A_data[(iaii - c) - 1] = 0.0F; - } - - itau--; - } - } - - for (itau = 0; itau + 1 <= m; itau++) { - for (i_i = 0; i_i + 1 <= m; i_i++) { - Q_data[i_i + b_idx_0 * itau] = b_A_data[b_A_sizes[0] * itau + i_i]; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iecjnohldjmohlno_qr.h b/gnc/matlab/code_generation/sharedutils/iecjnohldjmohlno_qr.h deleted file mode 100644 index e1af184db5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iecjnohldjmohlno_qr.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: iecjnohldjmohlno_qr.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_iecjnohldjmohlno_qr -#define SHARE_iecjnohldjmohlno_qr -#include "rtwtypes.h" - -extern void iecjnohldjmohlno_qr(const real32_T A_data[], const int32_T A_sizes[2], - real32_T Q_data[], int32_T Q_sizes[2], real32_T R_data[], int32_T R_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iecjopppiecjmgln_quatmult.cpp b/gnc/matlab/code_generation/sharedutils/iecjopppiecjmgln_quatmult.cpp deleted file mode 100644 index 8329b82649..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iecjopppiecjmgln_quatmult.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// -// File: iecjopppiecjmgln_quatmult.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "iecjopppiecjmgln_quatmult.h" - -// Function for MATLAB Function: '/Compute Global positions of Handrail Features' -void iecjopppiecjmgln_quatmult(const real32_T p[4], const real32_T q[4], - real32_T qOut[4]) -{ - real32_T c; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - qOut[0] = (q[3] * p[0] + p[3] * q[0]) + (p[1] * q[2] - p[2] * q[1]); - qOut[1] = (q[3] * p[1] + p[3] * q[1]) + (p[2] * q[0] - p[0] * q[2]); - qOut[2] = (q[3] * p[2] + p[3] * q[2]) + (p[0] * q[1] - p[1] * q[0]); - qOut[3] = p[3] * q[3] - ((p[0] * q[0] + p[1] * q[1]) + p[2] * q[2]); - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Takes the root-sum square of each row of the matrix a - c = (real32_T)sqrt((real_T)(((qOut[0] * qOut[0] + qOut[1] * qOut[1]) + qOut[2] - * qOut[2]) + qOut[3] * qOut[3])); - qOut[0] /= c; - qOut[1] /= c; - qOut[2] /= c; - qOut[3] /= c; - - // Normalize -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iecjopppiecjmgln_quatmult.h b/gnc/matlab/code_generation/sharedutils/iecjopppiecjmgln_quatmult.h deleted file mode 100644 index 58ed8bfbf9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iecjopppiecjmgln_quatmult.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: iecjopppiecjmgln_quatmult.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_iecjopppiecjmgln_quatmult -#define SHARE_iecjopppiecjmgln_quatmult -#include "rtwtypes.h" - -extern void iecjopppiecjmgln_quatmult(const real32_T p[4], const real32_T q[4], - real32_T qOut[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iekfiecjknopophd_pinv.cpp b/gnc/matlab/code_generation/sharedutils/iekfiecjknopophd_pinv.cpp deleted file mode 100644 index f97632730c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iekfiecjknopophd_pinv.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// -// File: iekfiecjknopophd_pinv.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "moppaaimimgdecjm_svd.h" -#include "iekfiecjknopophd_pinv.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void iekfiecjknopophd_pinv(const real32_T A[4], real32_T X[4]) -{ - real32_T V[4]; - int32_T r; - int32_T vcol; - real32_T tol; - int32_T j; - real32_T U[4]; - real32_T S[4]; - boolean_T b_p; - int32_T ar; - int32_T ia; - int32_T b_ic; - b_p = true; - X[0] = 0.0F; - if (!((!rtIsInfF(A[0])) && (!rtIsNaNF(A[0])))) { - b_p = false; - } - - X[1] = 0.0F; - if (b_p && ((!rtIsInfF(A[1])) && (!rtIsNaNF(A[1])))) { - } else { - b_p = false; - } - - X[2] = 0.0F; - if (b_p && ((!rtIsInfF(A[2])) && (!rtIsNaNF(A[2])))) { - } else { - b_p = false; - } - - X[3] = 0.0F; - if (b_p && ((!rtIsInfF(A[3])) && (!rtIsNaNF(A[3])))) { - } else { - b_p = false; - } - - if (b_p) { - moppaaimimgdecjm_svd(A, U, S, V); - } else { - U[0] = (rtNaNF); - U[1] = (rtNaNF); - S[1] = 0.0F; - U[2] = (rtNaNF); - S[2] = 0.0F; - U[3] = (rtNaNF); - S[0] = (rtNaNF); - S[3] = (rtNaNF); - V[0] = (rtNaNF); - V[1] = (rtNaNF); - V[2] = (rtNaNF); - V[3] = (rtNaNF); - } - - tol = 2.0F * S[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while (((int32_T)(vcol + 1) < 3) && (S[(int32_T)((int32_T)(vcol << 1) + vcol)] - > tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (j = 0; (int32_T)(j + 1) <= r; j++) { - tol = 1.0F / S[(int32_T)((int32_T)(j << 1) + j)]; - for (ar = vcol; (int32_T)(ar + 1) <= (int32_T)(vcol + 2); ar++) { - V[ar] *= tol; - } - - vcol += 2; - } - - for (j = 0; (int32_T)(j + 1) < 3; j++) { - X[j] = 0.0F; - } - - for (j = 2; (int32_T)(j + 1) < 5; j++) { - X[j] = 0.0F; - } - - ar = -1; - vcol = (int32_T)((int32_T)((int32_T)(r - 1) << 1) + 1); - for (j = 0; (int32_T)(j + 1) <= vcol; j += 2) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 0; (int32_T)(b_ic + 1) < 3; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 2; - } - - ar = -1; - vcol = (int32_T)((int32_T)((int32_T)(r - 1) << 1) + 2); - for (j = 1; (int32_T)(j + 1) <= vcol; j += 2) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 2; (int32_T)(b_ic + 1) < 5; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 2; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iekfiecjknopophd_pinv.h b/gnc/matlab/code_generation/sharedutils/iekfiecjknopophd_pinv.h deleted file mode 100644 index d6ab24b674..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iekfiecjknopophd_pinv.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: iekfiecjknopophd_pinv.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_iekfiecjknopophd_pinv -#define SHARE_iekfiecjknopophd_pinv -#include "rtwtypes.h" - -extern void iekfiecjknopophd_pinv(const real32_T A[4], real32_T X[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iekfnglngdbajekn_xgeqrf.cpp b/gnc/matlab/code_generation/sharedutils/iekfnglngdbajekn_xgeqrf.cpp deleted file mode 100644 index 626e84e436..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iekfnglngdbajekn_xgeqrf.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// -// File: iekfnglngdbajekn_xgeqrf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "mgdjjekffknghlfk_xzlarf.h" -#include "phdjbaaiglngbaaa_xzlarfg.h" -#include "iekfnglngdbajekn_xgeqrf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void iekfnglngdbajekn_xgeqrf(real32_T A_data[], int32_T A_sizes[2], real32_T - tau_data[], int32_T *tau_sizes) -{ - int32_T m; - int32_T mn; - real32_T work[6]; - int32_T i_i; - int32_T mmi; - real32_T b; - real32_T b_atmp; - int32_T i; - m = A_sizes[0]; - mn = A_sizes[0]; - if (!(mn <= 6)) { - mn = 6; - } - - *tau_sizes = (int32_T)(int8_T)mn; - if (A_sizes[0] != 0) { - for (i = 0; i < 6; i++) { - work[i] = 0.0F; - } - - for (i = 0; (int32_T)(i + 1) <= mn; i++) { - i_i = (int32_T)((int32_T)(i * m) + i); - mmi = (int32_T)(m - i); - if ((int32_T)(i + 1) < m) { - b_atmp = A_data[i_i]; - b = phdjbaaiglngbaaa_xzlarfg(mmi, &b_atmp, A_data, (int32_T)(i_i + 2)); - tau_data[i] = b; - A_data[i_i] = b_atmp; - } else { - tau_data[i] = 0.0F; - } - - if ((int32_T)(i + 1) < 6) { - b_atmp = A_data[i_i]; - A_data[i_i] = 1.0F; - mgdjjekffknghlfk_xzlarf(mmi, (int32_T)(5 - i), (int32_T)(i_i + 1), - tau_data[i], A_data, (int32_T)((int32_T)(i + (int32_T)((int32_T)(i + 1) - * m)) + 1), m, work); - A_data[i_i] = b_atmp; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/iekfnglngdbajekn_xgeqrf.h b/gnc/matlab/code_generation/sharedutils/iekfnglngdbajekn_xgeqrf.h deleted file mode 100644 index 722f0728e0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/iekfnglngdbajekn_xgeqrf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: iekfnglngdbajekn_xgeqrf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_iekfnglngdbajekn_xgeqrf -#define SHARE_iekfnglngdbajekn_xgeqrf -#include "rtwtypes.h" - -extern void iekfnglngdbajekn_xgeqrf(real32_T A_data[], int32_T A_sizes[2], - real32_T tau_data[], int32_T *tau_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ieknhdjmlngdkfcb_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/ieknhdjmlngdkfcb_xaxpy.cpp deleted file mode 100644 index 9b4e82e1fc..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ieknhdjmlngdkfcb_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: ieknhdjmlngdkfcb_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ieknhdjmlngdkfcb_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void ieknhdjmlngdkfcb_xaxpy(int32_T n, real32_T a, const real32_T x[4], int32_T - ix0, real32_T y[16], int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!(a == 0.0F)) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * x[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ieknhdjmlngdkfcb_xaxpy.h b/gnc/matlab/code_generation/sharedutils/ieknhdjmlngdkfcb_xaxpy.h deleted file mode 100644 index a9a9165c27..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ieknhdjmlngdkfcb_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ieknhdjmlngdkfcb_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ieknhdjmlngdkfcb_xaxpy -#define SHARE_ieknhdjmlngdkfcb_xaxpy -#include "rtwtypes.h" - -extern void ieknhdjmlngdkfcb_xaxpy(int32_T n, real32_T a, const real32_T x[4], - int32_T ix0, real32_T y[16], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imgdgdbadbaihlng_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/imgdgdbadbaihlng_xnrm2.cpp deleted file mode 100644 index 8b484167ee..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imgdgdbadbaihlng_xnrm2.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// -// File: imgdgdbadbaihlng_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "imgdgdbadbaihlng_xnrm2.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T imgdgdbadbaihlng_xnrm2(const real32_T x[4]) -{ - real32_T y; - real32_T scale; - real32_T absxk; - real32_T t; - scale = 1.17549435E-38F; - absxk = (real32_T)fabs((real_T)x[0]); - if (absxk > 1.17549435E-38F) { - y = 1.0F; - scale = absxk; - } else { - t = absxk / 1.17549435E-38F; - y = t * t; - } - - absxk = (real32_T)fabs((real_T)x[1]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - - return scale * (real32_T)sqrt((real_T)y); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imgdgdbadbaihlng_xnrm2.h b/gnc/matlab/code_generation/sharedutils/imgdgdbadbaihlng_xnrm2.h deleted file mode 100644 index 064cf5373a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imgdgdbadbaihlng_xnrm2.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: imgdgdbadbaihlng_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_imgdgdbadbaihlng_xnrm2 -#define SHARE_imgdgdbadbaihlng_xnrm2 -#include "rtwtypes.h" - -extern real32_T imgdgdbadbaihlng_xnrm2(const real32_T x[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imglknohophddjmg_eml_null_assignment.cpp b/gnc/matlab/code_generation/sharedutils/imglknohophddjmg_eml_null_assignment.cpp deleted file mode 100644 index 061df8105b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imglknohophddjmg_eml_null_assignment.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// -// File: imglknohophddjmg_eml_null_assignment.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "imglknohophddjmg_eml_null_assignment.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void imglknohophddjmg_eml_null_assignment(real32_T x_data[], int32_T x_sizes[2], - const boolean_T idx_data[], const int32_T idx_sizes) -{ - int32_T nrowx; - int32_T nrows; - int32_T i; - int32_T k; - int32_T j; - real32_T x_data_0[600]; - nrowx = x_sizes[0]; - nrows = 0; - for (i = 1; i <= idx_sizes; i++) { - nrows += idx_data[i - 1]; - } - - nrows = x_sizes[0] - nrows; - i = 0; - for (k = 1; k <= nrowx; k++) { - if ((k > idx_sizes) || (!idx_data[k - 1])) { - for (j = 0; j < 6; j++) { - x_data[i + x_sizes[0] * j] = x_data[(x_sizes[0] * j + k) - 1]; - } - - i++; - } - } - - if (1 > nrows) { - nrows = 0; - } - - for (nrowx = 0; nrowx < 6; nrowx++) { - for (i = 0; i < nrows; i++) { - x_data_0[i + nrows * nrowx] = x_data[x_sizes[0] * nrowx + i]; - } - } - - x_sizes[0] = nrows; - x_sizes[1] = 6; - for (nrowx = 0; nrowx < 6; nrowx++) { - for (i = 0; i < nrows; i++) { - x_data[i + x_sizes[0] * nrowx] = x_data_0[nrows * nrowx + i]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imglknohophddjmg_eml_null_assignment.h b/gnc/matlab/code_generation/sharedutils/imglknohophddjmg_eml_null_assignment.h deleted file mode 100644 index 610f2e3dd0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imglknohophddjmg_eml_null_assignment.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: imglknohophddjmg_eml_null_assignment.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_imglknohophddjmg_eml_null_assignment -#define SHARE_imglknohophddjmg_eml_null_assignment -#include "rtwtypes.h" - -extern void imglknohophddjmg_eml_null_assignment(real32_T x_data[], int32_T - x_sizes[2], const boolean_T idx_data[], const int32_T idx_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohaimgbaimjmgl_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/imohaimgbaimjmgl_xdotc.cpp deleted file mode 100644 index 4093639ac8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohaimgbaimjmgl_xdotc.cpp +++ /dev/null @@ -1,37 +0,0 @@ -// -// File: imohaimgbaimjmgl_xdotc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "imohaimgbaimjmgl_xdotc.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T imohaimgbaimjmgl_xdotc(int32_T n, const real32_T x_data[], int32_T ix0, - const real32_T y_data[], int32_T iy0) -{ - real32_T d; - int32_T ix; - int32_T iy; - int32_T k; - d = 0.0F; - ix = ix0; - iy = iy0; - for (k = 1; k <= n; k++) { - d += x_data[(int32_T)(ix - 1)] * y_data[(int32_T)(iy - 1)]; - ix++; - iy++; - } - - return d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohaimgbaimjmgl_xdotc.h b/gnc/matlab/code_generation/sharedutils/imohaimgbaimjmgl_xdotc.h deleted file mode 100644 index 0c30852639..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohaimgbaimjmgl_xdotc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: imohaimgbaimjmgl_xdotc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_imohaimgbaimjmgl_xdotc -#define SHARE_imohaimgbaimjmgl_xdotc -#include "rtwtypes.h" - -extern real32_T imohaimgbaimjmgl_xdotc(int32_T n, const real32_T x_data[], - int32_T ix0, const real32_T y_data[], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohbaimoppphlno_cat.cpp b/gnc/matlab/code_generation/sharedutils/imohbaimoppphlno_cat.cpp deleted file mode 100644 index 095fb8946b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohbaimoppphlno_cat.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// -// File: imohbaimoppphlno_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1051 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Tue Nov 8 14:36:07 2016 -// -#include "rtwtypes.h" -#include "imohbaimoppphlno_cat.h" - -// Function for MATLAB Function: '/generate_output' -void imohbaimoppphlno_cat(const real32_T varargin_1_data[], const int32_T - varargin_1_sizes[2], const int32_T varargin_2_sizes[3], real32_T y_data[], - int32_T y_sizes[3]) -{ - int32_T iy; - int32_T b; - int32_T j; - y_sizes[0] = (int8_T)varargin_1_sizes[0]; - y_sizes[1] = (int8_T)varargin_1_sizes[1]; - y_sizes[2] = 16; - iy = -1; - b = varargin_1_sizes[0] << 1; - for (j = 1; j <= b; j++) { - iy++; - y_data[iy] = varargin_1_data[j - 1]; - } - - b = (varargin_2_sizes[0] << 1) * 15; - for (j = 1; j <= b; j++) { - iy++; - y_data[iy] = 0.0F; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohbaimoppphlno_cat.h b/gnc/matlab/code_generation/sharedutils/imohbaimoppphlno_cat.h deleted file mode 100644 index c53ceed7d5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohbaimoppphlno_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: imohbaimoppphlno_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1051 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Tue Nov 8 14:36:07 2016 -// -#ifndef SHARE_imohbaimoppphlno_cat -#define SHARE_imohbaimoppphlno_cat -#include "rtwtypes.h" - -extern void imohbaimoppphlno_cat(const real32_T varargin_1_data[], const int32_T - varargin_1_sizes[2], const int32_T varargin_2_sizes[3], real32_T y_data[], - int32_T y_sizes[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohcjmoimopimoh_nullAssignment.cpp b/gnc/matlab/code_generation/sharedutils/imohcjmoimopimoh_nullAssignment.cpp deleted file mode 100644 index ffb2398054..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohcjmoimopimoh_nullAssignment.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// -// File: imohcjmoimopimoh_nullAssignment.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "imohcjmoimopimoh_nullAssignment.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void imohcjmoimopimoh_nullAssignment(real32_T x_data[], int32_T x_sizes[2], - const boolean_T idx_data[], const int32_T idx_sizes) -{ - int32_T nrowx; - int32_T nrows; - int32_T i; - int32_T k; - int32_T j; - real32_T x_data_0[600]; - nrowx = x_sizes[0]; - nrows = 0; - for (i = 1; i <= idx_sizes; i++) { - nrows += (int32_T)idx_data[(int32_T)(i - 1)]; - } - - nrows = (int32_T)(x_sizes[0] - nrows); - i = 0; - for (k = 1; k <= nrowx; k++) { - if ((k > idx_sizes) || (!idx_data[(int32_T)(k - 1)])) { - for (j = 0; j < 6; j++) { - x_data[(int32_T)(i + (int32_T)(x_sizes[0] * j))] = x_data[(int32_T) - ((int32_T)((int32_T)(x_sizes[0] * j) + k) - 1)]; - } - - i++; - } - } - - if (1 > nrows) { - nrows = 0; - } - - for (nrowx = 0; nrowx < 6; nrowx++) { - for (i = 0; i <= (int32_T)(nrows - 1); i++) { - x_data_0[(int32_T)(i + (int32_T)(nrows * nrowx))] = x_data[(int32_T) - ((int32_T)(x_sizes[0] * nrowx) + i)]; - } - } - - x_sizes[0] = nrows; - x_sizes[1] = 6; - for (nrowx = 0; nrowx < 6; nrowx++) { - for (i = 0; i <= (int32_T)(nrows - 1); i++) { - x_data[(int32_T)(i + (int32_T)(x_sizes[0] * nrowx))] = x_data_0[(int32_T) - ((int32_T)(nrows * nrowx) + i)]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohcjmoimopimoh_nullAssignment.h b/gnc/matlab/code_generation/sharedutils/imohcjmoimopimoh_nullAssignment.h deleted file mode 100644 index 4450d1888a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohcjmoimopimoh_nullAssignment.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: imohcjmoimopimoh_nullAssignment.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_imohcjmoimopimoh_nullAssignment -#define SHARE_imohcjmoimopimoh_nullAssignment -#include "rtwtypes.h" - -extern void imohcjmoimopimoh_nullAssignment(real32_T x_data[], int32_T x_sizes[2], - const boolean_T idx_data[], const int32_T idx_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohimopmglfaaim_sortLE.cpp b/gnc/matlab/code_generation/sharedutils/imohimopmglfaaim_sortLE.cpp deleted file mode 100644 index 236d7b4551..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohimopmglfaaim_sortLE.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// -// File: imohimopmglfaaim_sortLE.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "rt_nonfinite.h" -#include "imohimopmglfaaim_sortLE.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T imohimopmglfaaim_sortLE(const real32_T v[17264], const int32_T col[2], - int32_T irow1, int32_T irow2) -{ - boolean_T p; - int32_T coloffset; - int32_T k; - boolean_T exitg1; - p = true; - k = 0; - exitg1 = false; - while ((!exitg1) && (k < 2)) { - coloffset = (col[k] - 1) * 8632 - 1; - if (!((v[coloffset + irow1] == v[coloffset + irow2]) || (rtIsNaNF - (v[coloffset + irow1]) && rtIsNaNF(v[coloffset + irow2])))) { - p = ((v[coloffset + irow1] <= v[coloffset + irow2]) || rtIsNaNF - (v[coloffset + irow2])); - exitg1 = true; - } else { - k++; - } - } - - return p; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohimopmglfaaim_sortLE.h b/gnc/matlab/code_generation/sharedutils/imohimopmglfaaim_sortLE.h deleted file mode 100644 index 2ffb19653a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohimopmglfaaim_sortLE.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: imohimopmglfaaim_sortLE.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_imohimopmglfaaim_sortLE -#define SHARE_imohimopmglfaaim_sortLE -#include "rtwtypes.h" - -extern boolean_T imohimopmglfaaim_sortLE(const real32_T v[17264], const int32_T - col[2], int32_T irow1, int32_T irow2); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohknglphlfpphd_repmat.cpp b/gnc/matlab/code_generation/sharedutils/imohknglphlfpphd_repmat.cpp deleted file mode 100644 index 1a87888c16..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohknglphlfpphd_repmat.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// File: imohknglphlfpphd_repmat.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "imohknglphlfpphd_repmat.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void imohknglphlfpphd_repmat(real32_T b[256]) -{ - int32_T iacol; - int32_T ibmat; - int32_T ibcol; - int32_T jcol; - int32_T itilerow; - static const int8_T c[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; - - for (jcol = 0; jcol < 4; jcol++) { - iacol = (int32_T)(jcol << 2); - ibmat = (int32_T)(jcol << 6); - for (itilerow = 0; itilerow < 16; itilerow++) { - ibcol = (int32_T)((int32_T)(itilerow << 2) + ibmat); - b[ibcol] = (real32_T)c[iacol]; - b[(int32_T)(ibcol + 1)] = (real32_T)c[(int32_T)(iacol + 1)]; - b[(int32_T)(ibcol + 2)] = (real32_T)c[(int32_T)(iacol + 2)]; - b[(int32_T)(ibcol + 3)] = (real32_T)c[(int32_T)(iacol + 3)]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohknglphlfpphd_repmat.h b/gnc/matlab/code_generation/sharedutils/imohknglphlfpphd_repmat.h deleted file mode 100644 index 66db593929..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohknglphlfpphd_repmat.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: imohknglphlfpphd_repmat.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_imohknglphlfpphd_repmat -#define SHARE_imohknglphlfpphd_repmat -#include "rtwtypes.h" - -extern void imohknglphlfpphd_repmat(real32_T b[256]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohpphdkfcbimgl_eml_rand_mt19937ar.cpp b/gnc/matlab/code_generation/sharedutils/imohpphdkfcbimgl_eml_rand_mt19937ar.cpp deleted file mode 100644 index d82d4df312..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohpphdkfcbimgl_eml_rand_mt19937ar.cpp +++ /dev/null @@ -1,152 +0,0 @@ -// -// File: imohpphdkfcbimgl_eml_rand_mt19937ar.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "imohpphdkfcbimgl_eml_rand_mt19937ar.h" - -// Function for MATLAB Function: '/generate_output' -real_T imohpphdkfcbimgl_eml_rand_mt19937ar(uint32_T state[625]) -{ - real_T r; - uint32_T u[2]; - uint32_T y; - int32_T kk; - boolean_T b_isvalid; - uint32_T c_r; - int32_T exitg1; - boolean_T exitg2; - int32_T k; - - // ========================= COPYRIGHT NOTICE ============================ - // This is a uniform (0,1) pseudorandom number generator based on: - // - // A C-program for MT19937, with initialization improved 2002/1/26. - // Coded by Takuji Nishimura and Makoto Matsumoto. - // - // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, - // All rights reserved. - // - // Redistribution and use in source and binary forms, with or without - // modification, are permitted provided that the following conditions - // are met: - // - // 1. Redistributions of source code must retain the above copyright - // notice, this list of conditions and the following disclaimer. - // - // 2. Redistributions in binary form must reproduce the above copyright - // notice, this list of conditions and the following disclaimer - // in the documentation and/or other materials provided with the - // distribution. - // - // 3. The names of its contributors may not be used to endorse or - // promote products derived from this software without specific - // prior written permission. - // - // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR - // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, - // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY - // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - // - // ============================= END ================================= - do { - exitg1 = 0; - for (k = 0; k < 2; k++) { - u[k] = 0U; - c_r = state[624] + 1U; - if (c_r >= 625U) { - for (kk = 0; kk < 227; kk++) { - y = (state[1 + kk] & 2147483647U) | (state[kk] & 2147483648U); - if ((int32_T)(y & 1U) == 0) { - y >>= 1U; - } else { - y = y >> 1U ^ 2567483615U; - } - - state[kk] = state[397 + kk] ^ y; - } - - for (kk = 0; kk < 396; kk++) { - y = (state[kk + 227] & 2147483648U) | (state[228 + kk] & 2147483647U); - if ((int32_T)(y & 1U) == 0) { - y >>= 1U; - } else { - y = y >> 1U ^ 2567483615U; - } - - state[kk + 227] = state[kk] ^ y; - } - - y = (state[623] & 2147483648U) | (state[0] & 2147483647U); - if ((int32_T)(y & 1U) == 0) { - y >>= 1U; - } else { - y = y >> 1U ^ 2567483615U; - } - - state[623] = state[396] ^ y; - c_r = 1U; - } - - y = state[(int32_T)c_r - 1]; - state[624] = c_r; - y ^= y >> 11U; - y ^= y << 7U & 2636928640U; - y ^= y << 15U & 4022730752U; - y ^= y >> 18U; - u[k] = y; - } - - r = ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)(u[1] >> 6U)) * - 1.1102230246251565E-16; - if (r == 0.0) { - b_isvalid = ((state[624] >= 1U) && (state[624] < 625U)); - if (b_isvalid) { - b_isvalid = false; - k = 1; - exitg2 = false; - while ((!exitg2) && (k < 625)) { - if (state[k - 1] == 0U) { - k++; - } else { - b_isvalid = true; - exitg2 = true; - } - } - } - - if (!b_isvalid) { - c_r = 5489U; - state[0] = 5489U; - for (k = 0; k < 623; k++) { - c_r = ((c_r >> 30U ^ c_r) * 1812433253U + k) + 1U; - state[k + 1] = c_r; - } - - state[624] = 624U; - } - } else { - exitg1 = 1; - } - } while (exitg1 == 0); - - return r; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imohpphdkfcbimgl_eml_rand_mt19937ar.h b/gnc/matlab/code_generation/sharedutils/imohpphdkfcbimgl_eml_rand_mt19937ar.h deleted file mode 100644 index 8997eab56f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imohpphdkfcbimgl_eml_rand_mt19937ar.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: imohpphdkfcbimgl_eml_rand_mt19937ar.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_imohpphdkfcbimgl_eml_rand_mt19937ar -#define SHARE_imohpphdkfcbimgl_eml_rand_mt19937ar -#include "rtwtypes.h" - -extern real_T imohpphdkfcbimgl_eml_rand_mt19937ar(uint32_T state[625]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imopjmglppphcbai_xgemv.cpp b/gnc/matlab/code_generation/sharedutils/imopjmglppphcbai_xgemv.cpp deleted file mode 100644 index 313a31eb66..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imopjmglppphcbai_xgemv.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: imopjmglppphcbai_xgemv.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "imopjmglppphcbai_xgemv.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void imopjmglppphcbai_xgemv(int32_T m, int32_T n, const real32_T A_data[], - int32_T ia0, int32_T lda, const real32_T x_data[], int32_T ix0, real32_T - y_data[]) -{ - int32_T ix; - real32_T c; - int32_T iy; - int32_T b; - int32_T iac; - int32_T d; - int32_T ia; - if (n != 0) { - for (iy = 1; iy <= n; iy++) { - y_data[(int32_T)(iy - 1)] = 0.0F; - } - - iy = 0; - b = (int32_T)((int32_T)((int32_T)(n - 1) * lda) + ia0); - iac = ia0; - while ((lda > 0) && (iac <= b)) { - ix = ix0; - c = 0.0F; - d = (int32_T)((int32_T)(iac + m) - 1); - for (ia = iac; ia <= d; ia++) { - c += A_data[(int32_T)(ia - 1)] * x_data[(int32_T)(ix - 1)]; - ix++; - } - - y_data[iy] += c; - iy++; - iac += lda; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imopjmglppphcbai_xgemv.h b/gnc/matlab/code_generation/sharedutils/imopjmglppphcbai_xgemv.h deleted file mode 100644 index e5542261f6..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imopjmglppphcbai_xgemv.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: imopjmglppphcbai_xgemv.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_imopjmglppphcbai_xgemv -#define SHARE_imopjmglppphcbai_xgemv -#include "rtwtypes.h" - -extern void imopjmglppphcbai_xgemv(int32_T m, int32_T n, const real32_T A_data[], - int32_T ia0, int32_T lda, const real32_T x_data[], int32_T ix0, real32_T - y_data[]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imopmohdaaaimgln_xrot.cpp b/gnc/matlab/code_generation/sharedutils/imopmohdaaaimgln_xrot.cpp deleted file mode 100644 index bbddb3b1c2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imopmohdaaaimgln_xrot.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: imopmohdaaaimgln_xrot.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "imopmohdaaaimgln_xrot.h" - -// Function for MATLAB Function: '/MATLAB Function' -void imopmohdaaaimgln_xrot(real32_T x[36], int32_T ix0, int32_T iy0, real32_T c, - real32_T s) -{ - int32_T ix; - int32_T iy; - real32_T temp; - int32_T k; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k < 6; k++) { - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imopmohdaaaimgln_xrot.h b/gnc/matlab/code_generation/sharedutils/imopmohdaaaimgln_xrot.h deleted file mode 100644 index b7198fff68..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imopmohdaaaimgln_xrot.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: imopmohdaaaimgln_xrot.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_imopmohdaaaimgln_xrot -#define SHARE_imopmohdaaaimgln_xrot -#include "rtwtypes.h" - -extern void imopmohdaaaimgln_xrot(real32_T x[36], int32_T ix0, int32_T iy0, - real32_T c, real32_T s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imopmoppohdbjekn_cat.cpp b/gnc/matlab/code_generation/sharedutils/imopmoppohdbjekn_cat.cpp deleted file mode 100644 index 0f96c7da0a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imopmoppohdbjekn_cat.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// File: imopmoppohdbjekn_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "imopmoppohdbjekn_cat.h" - -// Function for MATLAB Function: '/generate_output' -void imopmoppohdbjekn_cat(const uint8_T varargin_2_data[], const int32_T - varargin_2_sizes[2], const uint8_T varargin_3_data[], const int32_T - varargin_3_sizes[2], uint8_T y_data[], int32_T y_sizes[2]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = 50; - y_sizes[1] = (int8_T)((int8_T)(1 + varargin_2_sizes[1]) + varargin_3_sizes[1]); - iy = -1; - for (j = 0; j < 50; j++) { - iy++; - y_data[iy] = 0U; - } - - j = 50 * varargin_2_sizes[1]; - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_2_data[b_j - 1]; - } - - j = 50 * varargin_3_sizes[1]; - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_3_data[b_j - 1]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/imopmoppohdbjekn_cat.h b/gnc/matlab/code_generation/sharedutils/imopmoppohdbjekn_cat.h deleted file mode 100644 index dadc6d2077..0000000000 --- a/gnc/matlab/code_generation/sharedutils/imopmoppohdbjekn_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: imopmoppohdbjekn_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_imopmoppohdbjekn_cat -#define SHARE_imopmoppohdbjekn_cat -#include "rtwtypes.h" - -extern void imopmoppohdbjekn_cat(const uint8_T varargin_2_data[], const int32_T - varargin_2_sizes[2], const uint8_T varargin_3_data[], const int32_T - varargin_3_sizes[2], uint8_T y_data[], int32_T y_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecbfkfkhdbangdj_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/jecbfkfkhdbangdj_xaxpy.cpp deleted file mode 100644 index 7fa93f341b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecbfkfkhdbangdj_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: jecbfkfkhdbangdj_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "jecbfkfkhdbangdj_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void jecbfkfkhdbangdj_xaxpy(int32_T n, real32_T a, const real32_T x[16], int32_T - ix0, real32_T y[4], int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!(a == 0.0F)) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * x[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecbfkfkhdbangdj_xaxpy.h b/gnc/matlab/code_generation/sharedutils/jecbfkfkhdbangdj_xaxpy.h deleted file mode 100644 index e008ac96cd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecbfkfkhdbangdj_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jecbfkfkhdbangdj_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_jecbfkfkhdbangdj_xaxpy -#define SHARE_jecbfkfkhdbangdj_xaxpy -#include "rtwtypes.h" - -extern void jecbfkfkhdbangdj_xaxpy(int32_T n, real32_T a, const real32_T x[16], - int32_T ix0, real32_T y[4], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecbmglnphlfjmop_skip_to_last_equal_value.cpp b/gnc/matlab/code_generation/sharedutils/jecbmglnphlfjmop_skip_to_last_equal_value.cpp deleted file mode 100644 index 50240890a5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecbmglnphlfjmop_skip_to_last_equal_value.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// -// File: jecbmglnphlfjmop_skip_to_last_equal_value.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "jecbmglnphlfjmop_skip_to_last_equal_value.h" - -// Function for MATLAB Function: '/generate_output' -real32_T jecbmglnphlfjmop_skip_to_last_equal_value(int32_T *k, const real32_T - x_data[], const int32_T x_sizes) -{ - real32_T xk; - real32_T absxk; - int32_T exponent; - boolean_T exitg1; - xk = x_data[*k - 1]; - exitg1 = false; - while ((!exitg1) && (*k < x_sizes)) { - absxk = (real32_T)fabs((real_T)(xk / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, exponent - 24); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(xk - x_data[*k])) < absxk) || (rtIsInfF(x_data[* - k]) && rtIsInfF(xk) && ((x_data[*k] > 0.0F) == (xk > 0.0F)))) { - (*k)++; - } else { - exitg1 = true; - } - } - - return xk; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecbmglnphlfjmop_skip_to_last_equal_value.h b/gnc/matlab/code_generation/sharedutils/jecbmglnphlfjmop_skip_to_last_equal_value.h deleted file mode 100644 index 67ba3f0618..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecbmglnphlfjmop_skip_to_last_equal_value.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jecbmglnphlfjmop_skip_to_last_equal_value.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_jecbmglnphlfjmop_skip_to_last_equal_value -#define SHARE_jecbmglnphlfjmop_skip_to_last_equal_value -#include "rtwtypes.h" - -extern real32_T jecbmglnphlfjmop_skip_to_last_equal_value(int32_T *k, const - real32_T x_data[], const int32_T x_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjcjmgfkfkophl_sum.cpp b/gnc/matlab/code_generation/sharedutils/jecjcjmgfkfkophl_sum.cpp deleted file mode 100644 index e30ad35136..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjcjmgfkfkophl_sum.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// -// File: jecjcjmgfkfkophl_sum.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "jecjcjmgfkfkophl_sum.h" - -// Function for MATLAB Function: '/generate_output' -real_T jecjcjmgfkfkophl_sum(const boolean_T x[50]) -{ - real_T y; - int32_T k; - y = (real_T)x[0]; - for (k = 0; k < 49; k++) { - y += (real_T)x[(int32_T)(k + 1)]; - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjcjmgfkfkophl_sum.h b/gnc/matlab/code_generation/sharedutils/jecjcjmgfkfkophl_sum.h deleted file mode 100644 index 8c023c1d22..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjcjmgfkfkophl_sum.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: jecjcjmgfkfkophl_sum.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_jecjcjmgfkfkophl_sum -#define SHARE_jecjcjmgfkfkophl_sum -#include "rtwtypes.h" - -extern real_T jecjcjmgfkfkophl_sum(const boolean_T x[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjfcjmbaaaaiek_xzlarfg.cpp b/gnc/matlab/code_generation/sharedutils/jecjfcjmbaaaaiek_xzlarfg.cpp deleted file mode 100644 index 3db866638e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjfcjmbaaaaiek_xzlarfg.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -// File: jecjfcjmbaaaaiek_xzlarfg.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "kfknaaaamglfppph_xnrm2.h" -#include "rt_hypotf_snf.h" -#include "jecjfcjmbaaaaiek_xzlarfg.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T jecjfcjmbaaaaiek_xzlarfg(int32_T n, real32_T *alpha1, real32_T x_data[], - int32_T ix0) -{ - real32_T tau; - real32_T xnorm; - int32_T knt; - int32_T b_k; - int32_T c_k; - tau = 0.0F; - if (!(n <= 0)) { - xnorm = kfknaaaamglfppph_xnrm2((int32_T)(n - 1), x_data, ix0); - if (xnorm != 0.0F) { - xnorm = rt_hypotf_snf(*alpha1, xnorm); - if (*alpha1 >= 0.0F) { - xnorm = -xnorm; - } - - if ((real32_T)fabs((real_T)xnorm) < 9.86076132E-32F) { - knt = 0; - do { - knt++; - b_k = (int32_T)((int32_T)(ix0 + n) - 2); - for (c_k = ix0; c_k <= b_k; c_k++) { - x_data[(int32_T)(c_k - 1)] *= 1.01412048E+31F; - } - - xnorm *= 1.01412048E+31F; - *alpha1 *= 1.01412048E+31F; - } while (!((real32_T)fabs((real_T)xnorm) >= 9.86076132E-32F)); - - xnorm = rt_hypotf_snf(*alpha1, kfknaaaamglfppph_xnrm2((int32_T)(n - 1), - x_data, ix0)); - if (*alpha1 >= 0.0F) { - xnorm = -xnorm; - } - - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - b_k = (int32_T)((int32_T)(ix0 + n) - 2); - for (c_k = ix0; c_k <= b_k; c_k++) { - x_data[(int32_T)(c_k - 1)] *= *alpha1; - } - - for (b_k = 1; b_k <= knt; b_k++) { - xnorm *= 9.86076132E-32F; - } - - *alpha1 = xnorm; - } else { - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - knt = (int32_T)((int32_T)(ix0 + n) - 2); - for (b_k = ix0; b_k <= knt; b_k++) { - x_data[(int32_T)(b_k - 1)] *= *alpha1; - } - - *alpha1 = xnorm; - } - } - } - - return tau; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjfcjmbaaaaiek_xzlarfg.h b/gnc/matlab/code_generation/sharedutils/jecjfcjmbaaaaiek_xzlarfg.h deleted file mode 100644 index 4a488316bd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjfcjmbaaaaiek_xzlarfg.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jecjfcjmbaaaaiek_xzlarfg.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_jecjfcjmbaaaaiek_xzlarfg -#define SHARE_jecjfcjmbaaaaiek_xzlarfg -#include "rtwtypes.h" - -extern real32_T jecjfcjmbaaaaiek_xzlarfg(int32_T n, real32_T *alpha1, real32_T - x_data[], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjhdbaohdbdbim_cat.cpp b/gnc/matlab/code_generation/sharedutils/jecjhdbaohdbdbim_cat.cpp deleted file mode 100644 index cb49bea088..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjhdbaohdbdbim_cat.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// -// File: jecjhdbaohdbdbim_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "jecjhdbaohdbdbim_cat.h" - -// Function for MATLAB Function: '/generate_output' -void jecjhdbaohdbdbim_cat(const real32_T varargin_1_data[], const int32_T - varargin_1_sizes[2], const int32_T varargin_2_sizes[3], real32_T y_data[], - int32_T y_sizes[3]) -{ - int32_T iy; - int32_T b; - int32_T j; - y_sizes[0] = (int8_T)varargin_1_sizes[0]; - y_sizes[1] = (int8_T)varargin_1_sizes[1]; - y_sizes[2] = 16; - iy = -1; - b = varargin_1_sizes[0] << 1; - for (j = 1; j <= b; j++) { - iy++; - y_data[iy] = varargin_1_data[j - 1]; - } - - b = (varargin_2_sizes[0] << 1) * 15; - for (j = 1; j <= b; j++) { - iy++; - y_data[iy] = 0.0F; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjhdbaohdbdbim_cat.h b/gnc/matlab/code_generation/sharedutils/jecjhdbaohdbdbim_cat.h deleted file mode 100644 index 9fdec79f39..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjhdbaohdbdbim_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: jecjhdbaohdbdbim_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_jecjhdbaohdbdbim_cat -#define SHARE_jecjhdbaohdbdbim_cat -#include "rtwtypes.h" - -extern void jecjhdbaohdbdbim_cat(const real32_T varargin_1_data[], const int32_T - varargin_1_sizes[2], const int32_T varargin_2_sizes[3], real32_T y_data[], - int32_T y_sizes[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjjmglfkfcaimo_svd.cpp b/gnc/matlab/code_generation/sharedutils/jecjjmglfkfcaimo_svd.cpp deleted file mode 100644 index b4b9424434..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjjmglfkfcaimo_svd.cpp +++ /dev/null @@ -1,539 +0,0 @@ -// -// File: jecjjmglfkfcaimo_svd.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include -#include "cjecbaaifkfkkngd_xaxpy.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "imohaimgbaimjmgl_xdotc.h" -#include "jmgljecbphlnngln_xnrm2.h" -#include "jmohnoppbieciecj_xrot.h" -#include "lfcjglnghlfknglf_xscal.h" -#include "ohdjhlfcnohdhdba_xscal.h" -#include "ophdjekfieknglfk_xswap.h" -#include "phlfglfcdbaajmgd_xswap.h" -#include "pphdglfkecjmjekf_xrot.h" -#include "rt_nonfinite.h" -#include "jecjjmglfkfcaimo_svd.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void jecjjmglfkfcaimo_svd(const real32_T A_data[], const int32_T A_sizes[2], - real32_T U_data[], int32_T U_sizes[2], real32_T S_data[], int32_T S_sizes[2], - real32_T V_data[], int32_T V_sizes[2]) -{ - int32_T p; - int32_T minnp; - int32_T nct; - int32_T q; - boolean_T apply_transform; - int32_T iter; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - int32_T j_ii; - real32_T varargin_1[5]; - real32_T b_A_data[6]; - int32_T b_A_sizes[2]; - real32_T s_data[2]; - real32_T e_data[2]; - real32_T Vf_data[4]; - real32_T n_data[9]; - int32_T n_sizes[2]; - real32_T o_data[96]; - int32_T o_sizes[2]; - int8_T d_idx_1; - int8_T d_idx_0; - int32_T Vf_sizes_idx_0; - int32_T Vf_sizes_idx_1; - b_A_sizes[0] = 3; - b_A_sizes[1] = A_sizes[1]; - kase = A_sizes[0] * A_sizes[1]; - for (qs = 0; qs < kase; qs++) { - b_A_data[qs] = A_data[qs]; - } - - p = A_sizes[1]; - minnp = A_sizes[1]; - kase = (int8_T)A_sizes[1]; - for (qs = 0; qs < kase; qs++) { - s_data[qs] = 0.0F; - } - - kase = (int8_T)A_sizes[1]; - for (qs = 0; qs < kase; qs++) { - e_data[qs] = 0.0F; - } - - d_idx_1 = (int8_T)A_sizes[1]; - U_sizes[0] = 3; - U_sizes[1] = d_idx_1; - kase = 3 * d_idx_1; - for (qs = 0; qs < kase; qs++) { - U_data[qs] = 0.0F; - } - - d_idx_0 = (int8_T)A_sizes[1]; - d_idx_1 = (int8_T)A_sizes[1]; - Vf_sizes_idx_0 = d_idx_0; - Vf_sizes_idx_1 = d_idx_1; - kase = d_idx_0 * d_idx_1; - for (qs = 0; qs < kase; qs++) { - Vf_data[qs] = 0.0F; - } - - if (A_sizes[1] == 0) { - for (nct = 0; nct + 1 <= minnp; nct++) { - U_data[nct + 3 * nct] = 1.0F; - } - } else { - nct = A_sizes[1]; - for (q = 0; q + 1 <= nct; q++) { - iter = 3 * q + q; - apply_transform = false; - if (q + 1 <= nct) { - snorm = jmgljecbphlnngln_xnrm2(3 - q, b_A_data, iter + 1); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A_data[iter] < 0.0F) { - s_data[q] = -snorm; - } else { - s_data[q] = snorm; - } - - if ((real32_T)fabs((real_T)s_data[q]) >= 9.86076132E-32F) { - snorm = 1.0F / s_data[q]; - kase = (iter - q) + 3; - for (qs = iter; qs + 1 <= kase; qs++) { - b_A_data[qs] *= snorm; - } - } else { - kase = (iter - q) + 3; - for (qs = iter; qs + 1 <= kase; qs++) { - b_A_data[qs] /= s_data[q]; - } - } - - b_A_data[iter]++; - s_data[q] = -s_data[q]; - } else { - s_data[q] = 0.0F; - } - } - - kase = q + 2; - while (kase <= p) { - if (apply_transform) { - cjecbaaifkfkkngd_xaxpy(3 - q, -(imohaimgbaimjmgl_xdotc(3 - q, b_A_data, - iter + 1, b_A_data, q + 4) / b_A_data[q + b_A_sizes[0] * q]), iter + - 1, b_A_data, q + 4); - } - - e_data[1] = b_A_data[q + 3]; - kase = 3; - } - - if (q + 1 <= nct) { - for (iter = q; iter + 1 < 4; iter++) { - U_data[iter + 3 * q] = b_A_data[b_A_sizes[0] * q + iter]; - } - } - } - - nct = A_sizes[1]; - if (1 < A_sizes[1]) { - e_data[0] = b_A_data[b_A_sizes[0]]; - } - - e_data[A_sizes[1] - 1] = 0.0F; - if (A_sizes[1] + 1 <= A_sizes[1]) { - iter = A_sizes[1] + 1; - while (iter <= 2) { - U_data[3] = 0.0F; - U_data[5] = 0.0F; - iter = 3; - U_data[4] = 1.0F; - } - } - - for (q = A_sizes[1] - 1; q + 1 > 0; q--) { - iter = 3 * q + q; - if (s_data[q] != 0.0F) { - kase = q + 2; - while (kase <= minnp) { - cjecbaaifkfkkngd_xaxpy(3 - q, -(imohaimgbaimjmgl_xdotc(3 - q, U_data, - iter + 1, U_data, q + 4) / U_data[iter]), iter + 1, U_data, q + 4); - kase = 3; - } - - for (kase = q; kase + 1 < 4; kase++) { - U_data[kase + U_sizes[0] * q] = -U_data[U_sizes[0] * q + kase]; - } - - U_data[iter]++; - iter = 1; - while (iter <= q) { - U_data[U_sizes[0] * q] = 0.0F; - iter = 2; - } - } else { - U_data[U_sizes[0] * q] = 0.0F; - U_data[1 + U_sizes[0] * q] = 0.0F; - U_data[2 + U_sizes[0] * q] = 0.0F; - U_data[iter] = 1.0F; - } - } - - for (iter = A_sizes[1] - 1; iter + 1 > 0; iter--) { - for (q = 1; q <= p; q++) { - Vf_data[(q + d_idx_0 * iter) - 1] = 0.0F; - } - - Vf_data[iter + d_idx_0 * iter] = 1.0F; - } - - for (iter = 0; iter + 1 <= nct; iter++) { - if (s_data[iter] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s_data[iter]); - snorm = s_data[iter] / ztest0; - s_data[iter] = ztest0; - if (iter + 1 < nct) { - e_data[0] /= snorm; - } - - n_sizes[0] = 3; - n_sizes[1] = U_sizes[1]; - kase = U_sizes[0] * U_sizes[1]; - for (qs = 0; qs < kase; qs++) { - n_data[qs] = U_data[qs]; - } - - lfcjglnghlfknglf_xscal(snorm, n_data, 1 + 3 * iter); - U_sizes[0] = 3; - U_sizes[1] = n_sizes[1]; - kase = n_sizes[0] * n_sizes[1]; - for (qs = 0; qs < kase; qs++) { - U_data[qs] = n_data[qs]; - } - } - - if ((iter + 1 < nct) && (e_data[0] != 0.0F)) { - ztest0 = (real32_T)fabs((real_T)e_data[0]); - snorm = ztest0 / e_data[0]; - e_data[0] = ztest0; - s_data[1] *= snorm; - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - kase = Vf_sizes_idx_0 * Vf_sizes_idx_1; - for (qs = 0; qs < kase; qs++) { - o_data[qs] = Vf_data[qs]; - } - - ohdjhlfcnohdhdba_xscal(p, snorm, o_data, 1 + p); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - kase = o_sizes[0] * o_sizes[1]; - for (qs = 0; qs < kase; qs++) { - Vf_data[qs] = o_data[qs]; - } - } - } - - iter = 0; - snorm = 0.0F; - for (q = 0; q + 1 <= nct; q++) { - ztest0 = (real32_T)fabs((real_T)s_data[q]); - ztest = (real32_T)fabs((real_T)e_data[q]); - if ((ztest0 >= ztest) || rtIsNaNF(ztest)) { - ztest = ztest0; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - } - - while ((nct > 0) && (!(iter >= 75))) { - kase = nct - 1; - do { - qs = 0; - q = kase; - if (kase == 0) { - qs = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e_data[0]); - if ((ztest0 <= ((real32_T)fabs((real_T)s_data[0]) + (real32_T)fabs - ((real_T)s_data[1])) * 1.1920929E-7F) || (ztest0 <= - 9.86076132E-32F) || ((iter > 20) && (ztest0 <= 1.1920929E-7F * - snorm))) { - e_data[0] = 0.0F; - qs = 1; - } else { - kase = 0; - } - } - } while (qs == 0); - - if (nct - 1 == kase) { - kase = 4; - } else { - qs = nct; - j_ii = nct; - apply_transform = false; - while ((!apply_transform) && (j_ii >= kase)) { - qs = j_ii; - if (j_ii == kase) { - apply_transform = true; - } else { - ztest0 = 0.0F; - if (j_ii < nct) { - ztest0 = (real32_T)fabs((real_T)e_data[j_ii - 1]); - } - - if (j_ii > kase + 1) { - ztest0 += (real32_T)fabs((real_T)e_data[0]); - } - - ztest = (real32_T)fabs((real_T)s_data[j_ii - 1]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) - { - s_data[j_ii - 1] = 0.0F; - apply_transform = true; - } else { - j_ii--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if (qs == nct) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e_data[nct - 2]; - e_data[nct - 2] = 0.0F; - kase = nct - 1; - while (kase >= q + 1) { - ztest = s_data[0]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s_data[0] = ztest; - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - kase = Vf_sizes_idx_0 * Vf_sizes_idx_1; - for (qs = 0; qs < kase; qs++) { - o_data[qs] = Vf_data[qs]; - } - - pphdglfkecjmjekf_xrot(p, o_data, 1, 1 + p * (nct - 1), sqds, smm1); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - kase = o_sizes[0] * o_sizes[1]; - for (qs = 0; qs < kase; qs++) { - Vf_data[qs] = o_data[qs]; - } - - kase = 0; - } - break; - - case 2: - ztest0 = e_data[q - 1]; - e_data[q - 1] = 0.0F; - for (kase = q; kase + 1 <= nct; kase++) { - ztest = s_data[kase]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s_data[kase] = ztest; - ztest0 = -smm1 * e_data[kase]; - e_data[kase] *= sqds; - jmohnoppbieciecj_xrot(U_data, 1 + 3 * kase, 1 + 3 * (q - 1), sqds, - smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s_data[nct - 1]); - varargin_1[1] = (real32_T)fabs((real_T)s_data[nct - 2]); - varargin_1[2] = (real32_T)fabs((real_T)e_data[nct - 2]); - varargin_1[3] = (real32_T)fabs((real_T)s_data[q]); - varargin_1[4] = (real32_T)fabs((real_T)e_data[q]); - kase = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - qs = 2; - apply_transform = false; - while ((!apply_transform) && (qs < 6)) { - kase = qs; - if (!rtIsNaNF(varargin_1[qs - 1])) { - ztest = varargin_1[qs - 1]; - apply_transform = true; - } else { - qs++; - } - } - } - - if (kase < 5) { - while (kase + 1 < 6) { - if (varargin_1[kase] > ztest) { - ztest = varargin_1[kase]; - } - - kase++; - } - } - - ztest0 = s_data[nct - 1] / ztest; - smm1 = s_data[nct - 2] / ztest; - emm1 = e_data[nct - 2] / ztest; - sqds = s_data[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e_data[q] / ztest * sqds; - while (q + 1 <= nct - 1) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - ztest0 = sqds * s_data[0] + smm1 * e_data[0]; - e_data[0] = sqds * e_data[0] - smm1 * s_data[0]; - ztest = smm1 * s_data[1]; - s_data[1] *= sqds; - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - kase = Vf_sizes_idx_0 * Vf_sizes_idx_1; - for (qs = 0; qs < kase; qs++) { - o_data[qs] = Vf_data[qs]; - } - - pphdglfkecjmjekf_xrot(p, o_data, 1, 1 + p, sqds, smm1); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - kase = o_sizes[0] * o_sizes[1]; - for (qs = 0; qs < kase; qs++) { - Vf_data[qs] = o_data[qs]; - } - - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s_data[0] = ztest0; - ztest0 = sqds * e_data[0] + smm1 * s_data[1]; - s_data[1] = -smm1 * e_data[0] + sqds * s_data[1]; - ztest = smm1 * e_data[1]; - e_data[1] *= sqds; - jmohnoppbieciecj_xrot(U_data, 1, 4, sqds, smm1); - q = 1; - } - - e_data[nct - 2] = ztest0; - iter++; - break; - - default: - if (s_data[q] < 0.0F) { - s_data[q] = -s_data[q]; - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - kase = Vf_sizes_idx_0 * Vf_sizes_idx_1; - for (qs = 0; qs < kase; qs++) { - o_data[qs] = Vf_data[qs]; - } - - ohdjhlfcnohdhdba_xscal(p, -1.0F, o_data, 1 + p * q); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - kase = o_sizes[0] * o_sizes[1]; - for (qs = 0; qs < kase; qs++) { - Vf_data[qs] = o_data[qs]; - } - } - - while ((q + 1 < A_sizes[1]) && (s_data[0] < s_data[1])) { - ztest0 = s_data[0]; - s_data[0] = s_data[1]; - s_data[1] = ztest0; - if (1 < p) { - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - kase = Vf_sizes_idx_0 * Vf_sizes_idx_1; - for (qs = 0; qs < kase; qs++) { - o_data[qs] = Vf_data[qs]; - } - - ophdjekfieknglfk_xswap(2, o_data, 1, 3); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - kase = o_sizes[0] * o_sizes[1]; - for (qs = 0; qs < kase; qs++) { - Vf_data[qs] = o_data[qs]; - } - } - - phlfglfcdbaajmgd_xswap(U_data); - q = 1; - } - - iter = 0; - nct--; - break; - } - } - } - - Vf_sizes_idx_1 = A_sizes[1]; - for (nct = 0; nct + 1 <= minnp; nct++) { - e_data[nct] = s_data[nct]; - } - - d_idx_0 = (int8_T)A_sizes[1]; - V_sizes[0] = (int8_T)A_sizes[1]; - V_sizes[1] = (int8_T)A_sizes[1]; - for (nct = 0; nct + 1 <= minnp; nct++) { - for (iter = 0; iter + 1 <= p; iter++) { - V_data[iter + d_idx_0 * nct] = Vf_data[Vf_sizes_idx_0 * nct + iter]; - } - } - - S_sizes[0] = A_sizes[1]; - S_sizes[1] = A_sizes[1]; - kase = A_sizes[1] * A_sizes[1]; - for (qs = 0; qs < kase; qs++) { - S_data[qs] = 0.0F; - } - - for (p = 0; p < Vf_sizes_idx_1; p++) { - S_data[p + Vf_sizes_idx_1 * p] = e_data[p]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjjmglfkfcaimo_svd.h b/gnc/matlab/code_generation/sharedutils/jecjjmglfkfcaimo_svd.h deleted file mode 100644 index 41bc4f5945..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjjmglfkfcaimo_svd.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: jecjjmglfkfcaimo_svd.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_jecjjmglfkfcaimo_svd -#define SHARE_jecjjmglfkfcaimo_svd -#include "rtwtypes.h" - -extern void jecjjmglfkfcaimo_svd(const real32_T A_data[], const int32_T A_sizes - [2], real32_T U_data[], int32_T U_sizes[2], real32_T S_data[], int32_T - S_sizes[2], real32_T V_data[], int32_T V_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjppphpphdnopp_rdivide.cpp b/gnc/matlab/code_generation/sharedutils/jecjppphpphdnopp_rdivide.cpp deleted file mode 100644 index 977b270a67..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjppphpphdnopp_rdivide.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jecjppphpphdnopp_rdivide.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "jecjppphpphdnopp_rdivide.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T jecjppphpphdnopp_rdivide(real32_T x, real32_T y) -{ - return x / y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jecjppphpphdnopp_rdivide.h b/gnc/matlab/code_generation/sharedutils/jecjppphpphdnopp_rdivide.h deleted file mode 100644 index 24eefb378b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jecjppphpphdnopp_rdivide.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: jecjppphpphdnopp_rdivide.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_jecjppphpphdnopp_rdivide -#define SHARE_jecjppphpphdnopp_rdivide -#include "rtwtypes.h" - -extern real32_T jecjppphpphdnopp_rdivide(real32_T x, real32_T y); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jekfopppngdbhlng_diag.cpp b/gnc/matlab/code_generation/sharedutils/jekfopppngdbhlng_diag.cpp deleted file mode 100644 index ba57c52004..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jekfopppngdbhlng_diag.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// File: jekfopppngdbhlng_diag.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "jekfopppngdbhlng_diag.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void jekfopppngdbhlng_diag(const real_T v_data[], const int32_T v_sizes, real_T - d_data[], int32_T d_sizes[2]) -{ - int32_T i; - int32_T loop_ub; - uint8_T b_idx_0; - uint8_T b_idx_1; - b_idx_0 = (uint8_T)v_sizes; - b_idx_1 = (uint8_T)v_sizes; - d_sizes[0] = (int32_T)b_idx_0; - d_sizes[1] = (int32_T)b_idx_1; - loop_ub = (int32_T)((int32_T)b_idx_0 * (int32_T)b_idx_1); - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - d_data[i] = 0.0; - } - - for (loop_ub = 0; (int32_T)(loop_ub + 1) <= v_sizes; loop_ub++) { - d_data[(int32_T)(loop_ub + (int32_T)((int32_T)b_idx_0 * loop_ub))] = - v_data[loop_ub]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jekfopppngdbhlng_diag.h b/gnc/matlab/code_generation/sharedutils/jekfopppngdbhlng_diag.h deleted file mode 100644 index 5ef7585a62..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jekfopppngdbhlng_diag.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jekfopppngdbhlng_diag.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_jekfopppngdbhlng_diag -#define SHARE_jekfopppngdbhlng_diag -#include "rtwtypes.h" - -extern void jekfopppngdbhlng_diag(const real_T v_data[], const int32_T v_sizes, - real_T d_data[], int32_T d_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jekncbaiknglnoph_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/jekncbaiknglnoph_xdotc.cpp deleted file mode 100644 index 2ec6d83652..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jekncbaiknglnoph_xdotc.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: jekncbaiknglnoph_xdotc.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "jekncbaiknglnoph_xdotc.h" - -// Function for MATLAB Function: '/MATLAB Function' -real32_T jekncbaiknglnoph_xdotc(int32_T n, const real32_T x[36], int32_T ix0, - const real32_T y[36], int32_T iy0) -{ - real32_T d; - int32_T ix; - int32_T iy; - int32_T k; - d = 0.0F; - if (!(n < 1)) { - ix = ix0; - iy = iy0; - for (k = 1; k <= n; k++) { - d += x[(int32_T)(ix - 1)] * y[(int32_T)(iy - 1)]; - ix++; - iy++; - } - } - - return d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jekncbaiknglnoph_xdotc.h b/gnc/matlab/code_generation/sharedutils/jekncbaiknglnoph_xdotc.h deleted file mode 100644 index da836c7b7a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jekncbaiknglnoph_xdotc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jekncbaiknglnoph_xdotc.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_jekncbaiknglnoph_xdotc -#define SHARE_jekncbaiknglnoph_xdotc -#include "rtwtypes.h" - -extern real32_T jekncbaiknglnoph_xdotc(int32_T n, const real32_T x[36], int32_T - ix0, const real32_T y[36], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmgdecjelfkfhdba_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/jmgdecjelfkfhdba_xzlarf.cpp deleted file mode 100644 index e9cb8efe0c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmgdecjelfkfhdba_xzlarf.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// File: jmgdecjelfkfhdba_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "imopjmglppphcbai_xgemv.h" -#include "mgdjohdjcjmglfcj_ilazlc.h" -#include "ohlfohlnbiekcjmg_xgerc.h" -#include "jmgdecjelfkfhdba_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void jmgdecjelfkfhdba_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work_data[]) -{ - int32_T lastv; - int32_T lastc; - if (tau != 0.0F) { - lastv = m; - lastc = iv0 + m; - while ((lastv > 0) && (C_data[lastc - 2] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = mgdjohdjcjmglfcj_ilazlc(lastv, n, C_data, ic0, ldc); - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - imopjmglppphcbai_xgemv(lastv, lastc, C_data, ic0, ldc, C_data, iv0, - work_data); - ohlfohlnbiekcjmg_xgerc(lastv, lastc, -tau, iv0, work_data, C_data, ic0, ldc); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmgdecjelfkfhdba_xzlarf.h b/gnc/matlab/code_generation/sharedutils/jmgdecjelfkfhdba_xzlarf.h deleted file mode 100644 index dd4c5c31e5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmgdecjelfkfhdba_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jmgdecjelfkfhdba_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_jmgdecjelfkfhdba_xzlarf -#define SHARE_jmgdecjelfkfhdba_xzlarf -#include "rtwtypes.h" - -extern void jmgdecjelfkfhdba_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work_data[]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmgdmophieknbaim_eml_null_assignment.cpp b/gnc/matlab/code_generation/sharedutils/jmgdmophieknbaim_eml_null_assignment.cpp deleted file mode 100644 index 0a8b218e9e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmgdmophieknbaim_eml_null_assignment.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// -// File: jmgdmophieknbaim_eml_null_assignment.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "jmgdmophieknbaim_eml_null_assignment.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void jmgdmophieknbaim_eml_null_assignment(real_T x_data[], int32_T *x_sizes, - const boolean_T idx_data[], const int32_T idx_sizes) -{ - int32_T nxin; - int32_T nxout; - int32_T k0; - int32_T k; - real_T x_data_0[100]; - nxin = *x_sizes; - nxout = 0; - for (k0 = 1; k0 <= idx_sizes; k0++) { - nxout += idx_data[k0 - 1]; - } - - nxout = *x_sizes - nxout; - k0 = -1; - for (k = 1; k <= nxin; k++) { - if ((k > idx_sizes) || (!idx_data[k - 1])) { - k0++; - x_data[k0] = x_data[k - 1]; - } - } - - if (*x_sizes != 1) { - if (1 > nxout) { - nxout = 0; - } - - for (k0 = 0; k0 < nxout; k0++) { - x_data_0[k0] = x_data[k0]; - } - - *x_sizes = nxout; - for (k0 = 0; k0 < nxout; k0++) { - x_data[k0] = x_data_0[k0]; - } - } else if (1 > nxout) { - *x_sizes = 0; - } else { - *x_sizes = nxout; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmgdmophieknbaim_eml_null_assignment.h b/gnc/matlab/code_generation/sharedutils/jmgdmophieknbaim_eml_null_assignment.h deleted file mode 100644 index 78d3d12035..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmgdmophieknbaim_eml_null_assignment.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jmgdmophieknbaim_eml_null_assignment.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_jmgdmophieknbaim_eml_null_assignment -#define SHARE_jmgdmophieknbaim_eml_null_assignment -#include "rtwtypes.h" - -extern void jmgdmophieknbaim_eml_null_assignment(real_T x_data[], int32_T - *x_sizes, const boolean_T idx_data[], const int32_T idx_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmgljecbphlnngln_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/jmgljecbphlnngln_xnrm2.cpp deleted file mode 100644 index f88ec35dd3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmgljecbphlnngln_xnrm2.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// -// File: jmgljecbphlnngln_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "jmgljecbphlnngln_xnrm2.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T jmgljecbphlnngln_xnrm2(int32_T n, const real32_T x_data[], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x_data[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt((real_T)y); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmgljecbphlnngln_xnrm2.h b/gnc/matlab/code_generation/sharedutils/jmgljecbphlnngln_xnrm2.h deleted file mode 100644 index e984b553e1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmgljecbphlnngln_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jmgljecbphlnngln_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_jmgljecbphlnngln_xnrm2 -#define SHARE_jmgljecbphlnngln_xnrm2 -#include "rtwtypes.h" - -extern real32_T jmgljecbphlnngln_xnrm2(int32_T n, const real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmgljmgdphdjlfcj_svd.cpp b/gnc/matlab/code_generation/sharedutils/jmgljmgdphdjlfcj_svd.cpp deleted file mode 100644 index 82a2939bfc..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmgljmgdphdjlfcj_svd.cpp +++ /dev/null @@ -1,337 +0,0 @@ -// -// File: jmgljmgdphdjlfcj_svd.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include -#include "cbimeknolnglpphd_xswap.h" -#include "ecbaphdbnglnimoh_xdotc.h" -#include "fcjejmgdgdbicjek_xaxpy.h" -#include "gdbijekfimglfcje_xrot.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "imgdgdbadbaihlng_xnrm2.h" -#include "knopcjmocbaacbaa_xscal.h" -#include "rt_nonfinite.h" -#include "jmgljmgdphdjlfcj_svd.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void jmgljmgdphdjlfcj_svd(const real32_T A[4], real32_T U[4], real32_T S[4], - real32_T V[4]) -{ - real32_T b_A[4]; - real32_T s[2]; - real32_T e[2]; - real32_T Vf[4]; - int32_T q; - boolean_T apply_transform; - real32_T nrm; - int32_T m; - int32_T iter; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - int32_T h_ii; - real32_T varargin_1[5]; - b_A[0] = A[0]; - b_A[1] = A[1]; - b_A[2] = A[2]; - b_A[3] = A[3]; - apply_transform = false; - nrm = imgdgdbadbaihlng_xnrm2(A); - if (nrm > 0.0F) { - apply_transform = true; - if (A[0] < 0.0F) { - nrm = -nrm; - } - - if ((real32_T)fabs((real_T)nrm) >= 9.86076132E-32F) { - ztest0 = 1.0F / nrm; - b_A[0] = ztest0 * A[0]; - b_A[1] = ztest0 * A[1]; - } else { - b_A[0] = A[0] / nrm; - b_A[1] = A[1] / nrm; - } - - b_A[0]++; - s[0] = -nrm; - } else { - s[0] = 0.0F; - } - - if (apply_transform) { - fcjejmgdgdbicjek_xaxpy(-(ecbaphdbnglnimoh_xdotc(b_A, b_A) / b_A[0]), b_A); - } - - U[0] = b_A[0]; - U[2] = 0.0F; - U[1] = b_A[1]; - m = 2; - s[1] = b_A[3]; - e[1] = 0.0F; - U[3] = 1.0F; - if (s[0] != 0.0F) { - fcjejmgdgdbicjek_xaxpy(-(ecbaphdbnglnimoh_xdotc(U, U) / b_A[0]), U); - U[0] = -U[0]; - U[1] = -U[1]; - U[0]++; - } else { - U[1] = 0.0F; - U[0] = 1.0F; - } - - Vf[2] = 0.0F; - Vf[3] = 1.0F; - Vf[1] = 0.0F; - Vf[0] = 1.0F; - ztest = b_A[2]; - if (s[0] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[0]); - nrm = s[0] / ztest0; - s[0] = ztest0; - ztest = b_A[2] / nrm; - knopcjmocbaacbaa_xscal(nrm, U, 1); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - nrm = ztest0 / ztest; - ztest = ztest0; - s[1] = b_A[3] * nrm; - knopcjmocbaacbaa_xscal(nrm, Vf, 3); - } - - if (s[1] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[1]); - nrm = s[1] / ztest0; - s[1] = ztest0; - knopcjmocbaacbaa_xscal(nrm, U, 3); - } - - e[0] = ztest; - iter = 0; - nrm = 0.0F; - if ((s[0] >= ztest) || rtIsNaNF(ztest)) { - ztest = s[0]; - } - - if (!((0.0F >= ztest) || rtIsNaNF(ztest))) { - nrm = ztest; - } - - if (s[1] >= 0.0F) { - ztest0 = s[1]; - } else { - ztest0 = 0.0F; - } - - if (!((nrm >= ztest0) || rtIsNaNF(ztest0))) { - nrm = ztest0; - } - - while ((m > 0) && (!(iter >= 75))) { - kase = m - 1; - do { - qs = 0; - q = kase; - if (kase == 0) { - qs = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e[0]); - if ((ztest0 <= ((real32_T)fabs((real_T)s[0]) + (real32_T)fabs((real_T)s - [1])) * 1.1920929E-7F) || (ztest0 <= 9.86076132E-32F) || ((iter > - 20) && (ztest0 <= 1.1920929E-7F * nrm))) { - e[0] = 0.0F; - qs = 1; - } else { - kase = 0; - } - } - } while (qs == 0); - - if (m - 1 == kase) { - kase = 4; - } else { - qs = m; - h_ii = m; - apply_transform = false; - while ((!apply_transform) && (h_ii >= kase)) { - qs = h_ii; - if (h_ii == kase) { - apply_transform = true; - } else { - ztest0 = 0.0F; - if (h_ii < m) { - ztest0 = (real32_T)fabs((real_T)e[0]); - } - - if (h_ii > kase + 1) { - ztest0 += (real32_T)fabs((real_T)e[0]); - } - - ztest = (real32_T)fabs((real_T)s[h_ii - 1]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s[h_ii - 1] = 0.0F; - apply_transform = true; - } else { - h_ii--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if (qs == m) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[0]; - e[0] = 0.0F; - kase = m - 1; - while (kase >= q + 1) { - ztest = s[0]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[0] = ztest; - gdbijekfimglfcje_xrot(Vf, 1, 1 + ((m - 1) << 1), sqds, smm1); - kase = 0; - } - break; - - case 2: - ztest0 = e[q - 1]; - e[q - 1] = 0.0F; - for (kase = q; kase + 1 <= m; kase++) { - ztest = s[kase]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[kase] = ztest; - ztest0 = -smm1 * e[kase]; - e[kase] *= sqds; - gdbijekfimglfcje_xrot(U, 1 + (kase << 1), 1 + ((q - 1) << 1), sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s[m - 1]); - varargin_1[1] = (real32_T)fabs((real_T)s[0]); - varargin_1[2] = (real32_T)fabs((real_T)e[0]); - varargin_1[3] = (real32_T)fabs((real_T)s[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - kase = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - qs = 2; - apply_transform = false; - while ((!apply_transform) && (qs < 6)) { - kase = qs; - if (!rtIsNaNF(varargin_1[qs - 1])) { - ztest = varargin_1[qs - 1]; - apply_transform = true; - } else { - qs++; - } - } - } - - if (kase < 5) { - while (kase + 1 < 6) { - if (varargin_1[kase] > ztest) { - ztest = varargin_1[kase]; - } - - kase++; - } - } - - ztest0 = s[m - 1] / ztest; - smm1 = s[0] / ztest; - emm1 = e[0] / ztest; - sqds = s[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - while (q + 1 <= 1) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - ztest0 = sqds * s[0] + smm1 * e[0]; - e[0] = sqds * e[0] - smm1 * s[0]; - ztest = smm1 * s[1]; - s[1] *= sqds; - gdbijekfimglfcje_xrot(Vf, 1, 3, sqds, smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s[0] = ztest0; - ztest0 = sqds * e[0] + smm1 * s[1]; - s[1] = -smm1 * e[0] + sqds * s[1]; - ztest = smm1 * e[1]; - e[1] *= sqds; - gdbijekfimglfcje_xrot(U, 1, 3, sqds, smm1); - q = 1; - } - - e[0] = ztest0; - iter++; - break; - - default: - if (s[q] < 0.0F) { - s[q] = -s[q]; - knopcjmocbaacbaa_xscal(-1.0F, Vf, 1 + (q << 1)); - } - - while ((q + 1 < 2) && (s[0] < s[1])) { - ztest0 = s[0]; - s[0] = s[1]; - s[1] = ztest0; - cbimeknolnglpphd_xswap(Vf); - cbimeknolnglpphd_xswap(U); - q = 1; - } - - iter = 0; - m--; - break; - } - } - - V[0] = Vf[0]; - V[1] = Vf[1]; - V[2] = Vf[2]; - V[3] = Vf[3]; - S[1] = 0.0F; - S[2] = 0.0F; - S[0] = s[0]; - S[3] = s[1]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmgljmgdphdjlfcj_svd.h b/gnc/matlab/code_generation/sharedutils/jmgljmgdphdjlfcj_svd.h deleted file mode 100644 index 0d4c52a45f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmgljmgdphdjlfcj_svd.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jmgljmgdphdjlfcj_svd.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_jmgljmgdphdjlfcj_svd -#define SHARE_jmgljmgdphdjlfcj_svd -#include "rtwtypes.h" - -extern void jmgljmgdphdjlfcj_svd(const real32_T A[4], real32_T U[4], real32_T S - [4], real32_T V[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmglopphppphkfkf_qr.cpp b/gnc/matlab/code_generation/sharedutils/jmglopphppphkfkf_qr.cpp deleted file mode 100644 index 07a3a51742..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmglopphppphkfkf_qr.cpp +++ /dev/null @@ -1,176 +0,0 @@ -// -// File: jmglopphppphkfkf_qr.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "aaiehlfccbimimgd_xzlarf.h" -#include "fcbijmgdglngglfc_xgeqrf.h" -#include "iekfnglngdbajekn_xgeqrf.h" -#include "knohbaiengdbnoph_xscal.h" -#include "mgdjjekffknghlfk_xzlarf.h" -#include "jmglopphppphkfkf_qr.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void jmglopphppphkfkf_qr(const real32_T A_data[], const int32_T A_sizes[2], - real32_T Q_data[], int32_T Q_sizes[2], real32_T R_data[], int32_T R_sizes[2]) -{ - int32_T m; - int32_T i_i; - int32_T itau; - real32_T work[6]; - int32_T iaii; - int32_T c; - int32_T i; - real32_T b_A_data[600]; - int32_T b_A_sizes[2]; - real32_T tau_data[6]; - real32_T b_tau_data[100]; - real32_T b_work_data[100]; - int8_T b_idx_0; - m = A_sizes[0]; - b_idx_0 = (int8_T)A_sizes[0]; - Q_sizes[0] = (int32_T)(int8_T)A_sizes[0]; - Q_sizes[1] = (int32_T)(int8_T)A_sizes[0]; - R_sizes[0] = A_sizes[0]; - R_sizes[1] = A_sizes[1]; - if (A_sizes[0] > 6) { - for (itau = 0; itau < 6; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= m; i_i++) { - Q_data[(int32_T)(i_i + (int32_T)((int32_T)b_idx_0 * itau))] = A_data - [(int32_T)((int32_T)(A_sizes[0] * itau) + i_i)]; - } - } - - for (itau = 7; itau <= m; itau++) { - for (i_i = 1; i_i <= m; i_i++) { - Q_data[(int32_T)((int32_T)(i_i + (int32_T)((int32_T)b_idx_0 * (int32_T) - (itau - 1))) - 1)] = 0.0F; - } - } - - fcbijmgdglngglfc_xgeqrf(Q_data, Q_sizes, b_tau_data, &itau); - for (itau = 0; itau < 6; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= (int32_T)(itau + 1); i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = Q_data[(int32_T) - ((int32_T)(Q_sizes[0] * itau) + i_i)]; - } - - for (i_i = (int32_T)(itau + 1); (int32_T)(i_i + 1) <= m; i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = 0.0F; - } - } - - for (itau = 6; itau <= (int32_T)(m - 1); itau++) { - i_i = (int32_T)(itau * m); - for (iaii = 0; iaii <= (int32_T)(m - 1); iaii++) { - Q_data[(int32_T)(i_i + iaii)] = 0.0F; - } - - Q_data[(int32_T)(i_i + itau)] = 1.0F; - } - - itau = 5; - b_idx_0 = (int8_T)Q_sizes[1]; - i = (int32_T)b_idx_0; - i_i = (int32_T)b_idx_0; - for (iaii = 0; iaii <= (int32_T)(i_i - 1); iaii++) { - b_work_data[iaii] = 0.0F; - } - - for (i_i = 5; i_i >= 0; i_i += -1) { - iaii = (int32_T)((int32_T)(i_i * m) + i_i); - Q_data[iaii] = 1.0F; - aaiehlfccbimimgd_xzlarf((int32_T)(m - i_i), (int32_T)((int32_T)(m - i_i) - - 1), (int32_T)(iaii + 1), b_tau_data[itau], Q_data, (int32_T)((int32_T) - (iaii + m) + 1), m, b_work_data); - knohbaiengdbnoph_xscal((int32_T)((int32_T)(m - i_i) - 1), -b_tau_data[itau], - Q_data, (int32_T)(iaii + 2)); - Q_data[iaii] = 1.0F - b_tau_data[itau]; - for (c = 1; c <= i_i; c++) { - Q_data[(int32_T)(iaii - c)] = 0.0F; - } - - itau--; - } - } else { - b_A_sizes[0] = A_sizes[0]; - b_A_sizes[1] = 6; - i_i = (int32_T)(A_sizes[0] * A_sizes[1]); - for (iaii = 0; iaii <= (int32_T)(i_i - 1); iaii++) { - b_A_data[iaii] = A_data[iaii]; - } - - iekfnglngdbajekn_xgeqrf(b_A_data, b_A_sizes, tau_data, &itau); - for (itau = 0; (int32_T)(itau + 1) <= m; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= (int32_T)(itau + 1); i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = b_A_data - [(int32_T)((int32_T)(b_A_sizes[0] * itau) + i_i)]; - } - - for (i_i = (int32_T)(itau + 1); (int32_T)(i_i + 1) <= m; i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = 0.0F; - } - } - - for (itau = A_sizes[0]; (int32_T)(itau + 1) < 7; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= m; i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = b_A_data - [(int32_T)((int32_T)(b_A_sizes[0] * itau) + i_i)]; - } - } - - if (!(A_sizes[0] < 1)) { - for (itau = A_sizes[0]; itau <= (int32_T)(m - 1); itau++) { - i_i = (int32_T)(itau * m); - for (iaii = 0; iaii <= (int32_T)(m - 1); iaii++) { - b_A_data[(int32_T)(i_i + iaii)] = 0.0F; - } - - b_A_data[(int32_T)(i_i + itau)] = 1.0F; - } - - itau = (int32_T)(A_sizes[0] - 1); - for (i = 0; i < 6; i++) { - work[i] = 0.0F; - } - - for (i_i = A_sizes[0]; i_i >= 1; i_i--) { - iaii = (int32_T)((int32_T)((int32_T)(i_i - 1) * m) + i_i); - if (i_i < m) { - b_A_data[(int32_T)(iaii - 1)] = 1.0F; - mgdjjekffknghlfk_xzlarf((int32_T)((int32_T)(m - i_i) + 1), (int32_T)(m - - i_i), iaii, tau_data[itau], b_A_data, (int32_T)(iaii + m), m, work); - c = (int32_T)((int32_T)(iaii + m) - i_i); - for (i = iaii; (int32_T)(i + 1) <= c; i++) { - b_A_data[i] *= -tau_data[itau]; - } - } - - b_A_data[(int32_T)(iaii - 1)] = 1.0F - tau_data[itau]; - for (c = 1; c <= (int32_T)(i_i - 1); c++) { - b_A_data[(int32_T)((int32_T)(iaii - c) - 1)] = 0.0F; - } - - itau--; - } - } - - for (itau = 0; (int32_T)(itau + 1) <= m; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= m; i_i++) { - Q_data[(int32_T)(i_i + (int32_T)((int32_T)b_idx_0 * itau))] = b_A_data - [(int32_T)((int32_T)(b_A_sizes[0] * itau) + i_i)]; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmglopphppphkfkf_qr.h b/gnc/matlab/code_generation/sharedutils/jmglopphppphkfkf_qr.h deleted file mode 100644 index ceca65683f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmglopphppphkfkf_qr.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jmglopphppphkfkf_qr.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_jmglopphppphkfkf_qr -#define SHARE_jmglopphppphkfkf_qr -#include "rtwtypes.h" - -extern void jmglopphppphkfkf_qr(const real32_T A_data[], const int32_T A_sizes[2], - real32_T Q_data[], int32_T Q_sizes[2], real32_T R_data[], int32_T R_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmohfcbikfkfphlf_cat.cpp b/gnc/matlab/code_generation/sharedutils/jmohfcbikfkfphlf_cat.cpp deleted file mode 100644 index be3efb932a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmohfcbikfkfphlf_cat.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// File: jmohfcbikfkfphlf_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "jmohfcbikfkfphlf_cat.h" - -// Function for MATLAB Function: '/generate_output' -void jmohfcbikfkfphlf_cat(const real32_T varargin_2_data[], const int32_T - varargin_2_sizes[3], const real32_T varargin_3_data[], const int32_T - varargin_3_sizes[3], real32_T y_data[], int32_T y_sizes[3]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = 50; - y_sizes[1] = 2; - y_sizes[2] = 16; - iy = -1; - for (j = 0; j < 100; j++) { - iy++; - y_data[iy] = 0.0F; - } - - j = 100 * varargin_2_sizes[2]; - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_2_data[b_j - 1]; - } - - j = 100 * varargin_3_sizes[2]; - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_3_data[b_j - 1]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmohfcbikfkfphlf_cat.h b/gnc/matlab/code_generation/sharedutils/jmohfcbikfkfphlf_cat.h deleted file mode 100644 index f2043be5c3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmohfcbikfkfphlf_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: jmohfcbikfkfphlf_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_jmohfcbikfkfphlf_cat -#define SHARE_jmohfcbikfkfphlf_cat -#include "rtwtypes.h" - -extern void jmohfcbikfkfphlf_cat(const real32_T varargin_2_data[], const int32_T - varargin_2_sizes[3], const real32_T varargin_3_data[], const int32_T - varargin_3_sizes[3], real32_T y_data[], int32_T y_sizes[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmohiecblfcjnohl_qr.cpp b/gnc/matlab/code_generation/sharedutils/jmohiecblfcjnohl_qr.cpp deleted file mode 100644 index 001bc6758c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmohiecblfcjnohl_qr.cpp +++ /dev/null @@ -1,176 +0,0 @@ -// -// File: jmohiecblfcjnohl_qr.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "gdjmaiekcbaiecjm_xzlarf.h" -#include "knglnoppglnoimoh_xscal.h" -#include "mgdjbieccjechlng_xgeqrf.h" -#include "ohdbhlnomohlcjek_xgeqrf.h" -#include "ophdecbiohdblnoh_xzlarf.h" -#include "jmohiecblfcjnohl_qr.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void jmohiecblfcjnohl_qr(const real32_T A_data[], const int32_T A_sizes[2], - real32_T Q_data[], int32_T Q_sizes[2], real32_T R_data[], int32_T R_sizes[2]) -{ - int32_T m; - int32_T i_i; - int32_T itau; - real32_T work[6]; - int32_T iaii; - int32_T c; - int32_T i; - real32_T b_A_data[900]; - int32_T b_A_sizes[2]; - real32_T tau_data[6]; - real32_T b_tau_data[150]; - real32_T b_work_data[150]; - uint8_T b_idx_0; - m = A_sizes[0]; - b_idx_0 = (uint8_T)A_sizes[0]; - Q_sizes[0] = (int32_T)(uint8_T)A_sizes[0]; - Q_sizes[1] = (int32_T)(uint8_T)A_sizes[0]; - R_sizes[0] = A_sizes[0]; - R_sizes[1] = A_sizes[1]; - if (A_sizes[0] > 6) { - for (itau = 0; itau < 6; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= m; i_i++) { - Q_data[(int32_T)(i_i + (int32_T)((int32_T)b_idx_0 * itau))] = A_data - [(int32_T)((int32_T)(A_sizes[0] * itau) + i_i)]; - } - } - - for (itau = 7; itau <= m; itau++) { - for (i_i = 1; i_i <= m; i_i++) { - Q_data[(int32_T)((int32_T)(i_i + (int32_T)((int32_T)b_idx_0 * (int32_T) - (itau - 1))) - 1)] = 0.0F; - } - } - - ohdbhlnomohlcjek_xgeqrf(Q_data, Q_sizes, b_tau_data, &itau); - for (itau = 0; itau < 6; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= (int32_T)(itau + 1); i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = Q_data[(int32_T) - ((int32_T)(Q_sizes[0] * itau) + i_i)]; - } - - for (i_i = (int32_T)(itau + 1); (int32_T)(i_i + 1) <= m; i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = 0.0F; - } - } - - for (itau = 6; itau <= (int32_T)(m - 1); itau++) { - i_i = (int32_T)(itau * m); - for (iaii = 0; iaii <= (int32_T)(m - 1); iaii++) { - Q_data[(int32_T)(i_i + iaii)] = 0.0F; - } - - Q_data[(int32_T)(i_i + itau)] = 1.0F; - } - - itau = 5; - b_idx_0 = (uint8_T)Q_sizes[1]; - i = (int32_T)b_idx_0; - i_i = (int32_T)b_idx_0; - for (iaii = 0; iaii <= (int32_T)(i_i - 1); iaii++) { - b_work_data[iaii] = 0.0F; - } - - for (i_i = 5; i_i >= 0; i_i += -1) { - iaii = (int32_T)((int32_T)(i_i * m) + i_i); - Q_data[iaii] = 1.0F; - ophdecbiohdblnoh_xzlarf((int32_T)(m - i_i), (int32_T)((int32_T)(m - i_i) - - 1), (int32_T)(iaii + 1), b_tau_data[itau], Q_data, (int32_T)((int32_T) - (iaii + m) + 1), m, b_work_data); - knglnoppglnoimoh_xscal((int32_T)((int32_T)(m - i_i) - 1), -b_tau_data[itau], - Q_data, (int32_T)(iaii + 2)); - Q_data[iaii] = 1.0F - b_tau_data[itau]; - for (c = 1; c <= i_i; c++) { - Q_data[(int32_T)(iaii - c)] = 0.0F; - } - - itau--; - } - } else { - b_A_sizes[0] = A_sizes[0]; - b_A_sizes[1] = 6; - i_i = (int32_T)(A_sizes[0] * A_sizes[1]); - for (iaii = 0; iaii <= (int32_T)(i_i - 1); iaii++) { - b_A_data[iaii] = A_data[iaii]; - } - - mgdjbieccjechlng_xgeqrf(b_A_data, b_A_sizes, tau_data, &itau); - for (itau = 0; (int32_T)(itau + 1) <= m; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= (int32_T)(itau + 1); i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = b_A_data - [(int32_T)((int32_T)(b_A_sizes[0] * itau) + i_i)]; - } - - for (i_i = (int32_T)(itau + 1); (int32_T)(i_i + 1) <= m; i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = 0.0F; - } - } - - for (itau = A_sizes[0]; (int32_T)(itau + 1) < 7; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= m; i_i++) { - R_data[(int32_T)(i_i + (int32_T)(R_sizes[0] * itau))] = b_A_data - [(int32_T)((int32_T)(b_A_sizes[0] * itau) + i_i)]; - } - } - - if (!(A_sizes[0] < 1)) { - for (itau = A_sizes[0]; itau <= (int32_T)(m - 1); itau++) { - i_i = (int32_T)(itau * m); - for (iaii = 0; iaii <= (int32_T)(m - 1); iaii++) { - b_A_data[(int32_T)(i_i + iaii)] = 0.0F; - } - - b_A_data[(int32_T)(i_i + itau)] = 1.0F; - } - - itau = (int32_T)(A_sizes[0] - 1); - for (i = 0; i < 6; i++) { - work[i] = 0.0F; - } - - for (i_i = A_sizes[0]; i_i >= 1; i_i--) { - iaii = (int32_T)((int32_T)((int32_T)(i_i - 1) * m) + i_i); - if (i_i < m) { - b_A_data[(int32_T)(iaii - 1)] = 1.0F; - gdjmaiekcbaiecjm_xzlarf((int32_T)((int32_T)(m - i_i) + 1), (int32_T)(m - - i_i), iaii, tau_data[itau], b_A_data, (int32_T)(iaii + m), m, work); - c = (int32_T)((int32_T)(iaii + m) - i_i); - for (i = iaii; (int32_T)(i + 1) <= c; i++) { - b_A_data[i] *= -tau_data[itau]; - } - } - - b_A_data[(int32_T)(iaii - 1)] = 1.0F - tau_data[itau]; - for (c = 1; c <= (int32_T)(i_i - 1); c++) { - b_A_data[(int32_T)((int32_T)(iaii - c) - 1)] = 0.0F; - } - - itau--; - } - } - - for (itau = 0; (int32_T)(itau + 1) <= m; itau++) { - for (i_i = 0; (int32_T)(i_i + 1) <= m; i_i++) { - Q_data[(int32_T)(i_i + (int32_T)((int32_T)b_idx_0 * itau))] = b_A_data - [(int32_T)((int32_T)(b_A_sizes[0] * itau) + i_i)]; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmohiecblfcjnohl_qr.h b/gnc/matlab/code_generation/sharedutils/jmohiecblfcjnohl_qr.h deleted file mode 100644 index 70b26a14e8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmohiecblfcjnohl_qr.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jmohiecblfcjnohl_qr.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_jmohiecblfcjnohl_qr -#define SHARE_jmohiecblfcjnohl_qr -#include "rtwtypes.h" - -extern void jmohiecblfcjnohl_qr(const real32_T A_data[], const int32_T A_sizes[2], - real32_T Q_data[], int32_T Q_sizes[2], real32_T R_data[], int32_T R_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmohnoppbieciecj_xrot.cpp b/gnc/matlab/code_generation/sharedutils/jmohnoppbieciecj_xrot.cpp deleted file mode 100644 index 957e8592e0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmohnoppbieciecj_xrot.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// -// File: jmohnoppbieciecj_xrot.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "jmohnoppbieciecj_xrot.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void jmohnoppbieciecj_xrot(real32_T x_data[], int32_T ix0, int32_T iy0, real32_T - c, real32_T s) -{ - int32_T ix; - int32_T iy; - real32_T temp; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - temp = c * x_data[ix] + s * x_data[iy]; - x_data[iy] = c * x_data[iy] - s * x_data[ix]; - x_data[ix] = temp; - iy++; - ix++; - temp = c * x_data[ix] + s * x_data[iy]; - x_data[iy] = c * x_data[iy] - s * x_data[ix]; - x_data[ix] = temp; - iy++; - ix++; - temp = c * x_data[ix] + s * x_data[iy]; - x_data[iy] = c * x_data[iy] - s * x_data[ix]; - x_data[ix] = temp; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/jmohnoppbieciecj_xrot.h b/gnc/matlab/code_generation/sharedutils/jmohnoppbieciecj_xrot.h deleted file mode 100644 index 0b2e26e1ff..0000000000 --- a/gnc/matlab/code_generation/sharedutils/jmohnoppbieciecj_xrot.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: jmohnoppbieciecj_xrot.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_jmohnoppbieciecj_xrot -#define SHARE_jmohnoppbieciecj_xrot -#include "rtwtypes.h" - -extern void jmohnoppbieciecj_xrot(real32_T x_data[], int32_T ix0, int32_T iy0, - real32_T c, real32_T s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfcbgdbiaaieekno_quat_propagate_step.cpp b/gnc/matlab/code_generation/sharedutils/kfcbgdbiaaieekno_quat_propagate_step.cpp deleted file mode 100644 index 882179c63f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfcbgdbiaaieekno_quat_propagate_step.cpp +++ /dev/null @@ -1,103 +0,0 @@ -// -// File: kfcbgdbiaaieekno_quat_propagate_step.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#include "rtwtypes.h" -#include -#include "kfcbgdbiaaieekno_quat_propagate_step.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void kfcbgdbiaaieekno_quat_propagate_step(const real32_T quat_in[4], const - real32_T omega[3], real32_T time_in, real_T quat_out[4]) -{ - real32_T omega_mag; - real32_T c; - real32_T sine_mag; - real_T x; - real_T result; - real32_T c_0[16]; - int32_T i; - real32_T s_idx_1; - real32_T s_idx_0; - - // omega row vector - // - // Column_Vector = RSSROW(Matrix) - // - // where Matrix is an n-by-m matrix. This function will return the RSS of - // each row into an n-by-1 column vector for any number of m columns. - // - // Douglas Adams, Feb, 2005 - // - omega_mag = (real32_T)sqrt((real_T)((omega[0] * omega[0] + omega[1] * omega[1]) - + omega[2] * omega[2])); - if ((omega_mag == 0.0F) || (time_in <= 0.0F)) { - quat_out[0] = (real_T)quat_in[0]; - quat_out[1] = (real_T)quat_in[1]; - quat_out[2] = (real_T)quat_in[2]; - quat_out[3] = (real_T)quat_in[3]; - } else { - c = (real32_T)cos((real_T)(0.5F * omega_mag * time_in)); - sine_mag = (real32_T)sin((real_T)(0.5F * omega_mag * time_in)); - s_idx_0 = sine_mag * omega[0] / omega_mag; - s_idx_1 = sine_mag * omega[1] / omega_mag; - omega_mag = sine_mag * omega[2] / omega_mag; - c_0[0] = c; - c_0[1] = -omega_mag; - c_0[2] = s_idx_1; - c_0[3] = -s_idx_0; - c_0[4] = omega_mag; - c_0[5] = c; - c_0[6] = -s_idx_0; - c_0[7] = -s_idx_1; - c_0[8] = -s_idx_1; - c_0[9] = s_idx_0; - c_0[10] = c; - c_0[11] = -omega_mag; - c_0[12] = s_idx_0; - c_0[13] = s_idx_1; - c_0[14] = omega_mag; - c_0[15] = c; - for (i = 0; i < 4; i++) { - c = c_0[(int32_T)(i + 12)] * quat_in[3] + (c_0[(int32_T)(i + 8)] * - quat_in[2] + (c_0[(int32_T)(i + 4)] * quat_in[1] + c_0[i] * quat_in[0])); - quat_out[i] = (real_T)c; - } - - if (quat_out[3] < 0.0) { - x = -1.0; - } else if (quat_out[3] > 0.0) { - x = 1.0; - } else if (quat_out[3] == 0.0) { - x = 0.0; - } else { - x = quat_out[3]; - } - - // - // Column_Vector = RSSROW(Matrix) - // - // where Matrix is an n-by-m matrix. This function will return the RSS of - // each row into an n-by-1 column vector for any number of m columns. - // - // Douglas Adams, Feb, 2005 - // - result = sqrt(((quat_out[0] * quat_out[0] + quat_out[1] * quat_out[1]) + - quat_out[2] * quat_out[2]) + quat_out[3] * quat_out[3]); - quat_out[0] = x * quat_out[0] / result; - quat_out[1] = x * quat_out[1] / result; - quat_out[2] = x * quat_out[2] / result; - quat_out[3] = x * quat_out[3] / result; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfcbgdbiaaieekno_quat_propagate_step.h b/gnc/matlab/code_generation/sharedutils/kfcbgdbiaaieekno_quat_propagate_step.h deleted file mode 100644 index c27afbbd41..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfcbgdbiaaieekno_quat_propagate_step.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: kfcbgdbiaaieekno_quat_propagate_step.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#ifndef SHARE_kfcbgdbiaaieekno_quat_propagate_step -#define SHARE_kfcbgdbiaaieekno_quat_propagate_step -#include "rtwtypes.h" - -extern void kfcbgdbiaaieekno_quat_propagate_step(const real32_T quat_in[4], - const real32_T omega[3], real32_T time_in, real_T quat_out[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfcbjeknaaaiiekn_pinv.cpp b/gnc/matlab/code_generation/sharedutils/kfcbjeknaaaiiekn_pinv.cpp deleted file mode 100644 index cded48a39d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfcbjeknaaaiiekn_pinv.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// -// File: kfcbjeknaaaiiekn_pinv.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include -#include "cbimbiekfcjmaaie_svd.h" -#include "kfcbjeknaaaiiekn_pinv.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void kfcbjeknaaaiiekn_pinv(const real32_T A[16], real32_T X[16]) -{ - real32_T V[16]; - int32_T r; - int32_T vcol; - real32_T U[16]; - real32_T S[16]; - real32_T tol; - int32_T j; - int32_T ar; - int32_T ia; - int32_T b_ic; - memset(&X[0], 0, sizeof(real32_T) << 4U); - cbimbiekfcjmaaie_svd(A, U, S, V); - tol = 4.0F * S[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while ((vcol + 1 < 5) && (S[(vcol << 2) + vcol] > tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (j = 0; j + 1 <= r; j++) { - tol = 1.0F / S[(j << 2) + j]; - for (ar = vcol; ar + 1 <= vcol + 4; ar++) { - V[ar] *= tol; - } - - vcol += 4; - } - - for (j = 0; j + 1 < 5; j++) { - X[j] = 0.0F; - } - - for (j = 4; j + 1 < 9; j++) { - X[j] = 0.0F; - } - - for (j = 8; j + 1 < 13; j++) { - X[j] = 0.0F; - } - - for (j = 12; j + 1 < 17; j++) { - X[j] = 0.0F; - } - - ar = -1; - vcol = ((r - 1) << 2) + 1; - for (j = 0; j + 1 <= vcol; j += 4) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 0; b_ic + 1 < 5; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 4; - } - - ar = -1; - vcol = ((r - 1) << 2) + 2; - for (j = 1; j + 1 <= vcol; j += 4) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 4; b_ic + 1 < 9; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 4; - } - - ar = -1; - vcol = ((r - 1) << 2) + 3; - for (j = 2; j + 1 <= vcol; j += 4) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 8; b_ic + 1 < 13; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 4; - } - - ar = -1; - vcol = ((r - 1) << 2) + 4; - for (j = 3; j + 1 <= vcol; j += 4) { - if (U[j] != 0.0F) { - ia = ar; - for (b_ic = 12; b_ic + 1 < 17; b_ic++) { - ia++; - X[b_ic] += U[j] * V[ia]; - } - } - - ar += 4; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfcbjeknaaaiiekn_pinv.h b/gnc/matlab/code_generation/sharedutils/kfcbjeknaaaiiekn_pinv.h deleted file mode 100644 index e63e209ecd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfcbjeknaaaiiekn_pinv.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: kfcbjeknaaaiiekn_pinv.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_kfcbjeknaaaiiekn_pinv -#define SHARE_kfcbjeknaaaiiekn_pinv -#include "rtwtypes.h" - -extern void kfcbjeknaaaiiekn_pinv(const real32_T A[16], real32_T X[16]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfcjlnglmgdbgdjm_all.cpp b/gnc/matlab/code_generation/sharedutils/kfcjlnglmgdbgdjm_all.cpp deleted file mode 100644 index 294d7e28be..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfcjlnglmgdbgdjm_all.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// -// File: kfcjlnglmgdbgdjm_all.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "kfcjlnglmgdbgdjm_all.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T kfcjlnglmgdbgdjm_all(const boolean_T x[4]) -{ - boolean_T y; - int32_T k; - boolean_T exitg1; - y = true; - k = 0; - exitg1 = false; - while ((!exitg1) && (k < 4)) { - if (!x[k]) { - y = false; - exitg1 = true; - } else { - k++; - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfcjlnglmgdbgdjm_all.h b/gnc/matlab/code_generation/sharedutils/kfcjlnglmgdbgdjm_all.h deleted file mode 100644 index 709519a138..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfcjlnglmgdbgdjm_all.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: kfcjlnglmgdbgdjm_all.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_kfcjlnglmgdbgdjm_all -#define SHARE_kfcjlnglmgdbgdjm_all -#include "rtwtypes.h" - -extern boolean_T kfcjlnglmgdbgdjm_all(const boolean_T x[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfkfcbiekfcjmohl_mldivide.cpp b/gnc/matlab/code_generation/sharedutils/kfkfcbiekfcjmohl_mldivide.cpp deleted file mode 100644 index 28b45271fc..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfkfcbiekfcjmohl_mldivide.cpp +++ /dev/null @@ -1,175 +0,0 @@ -// -// File: kfkfcbiekfcjmohl_mldivide.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include -#include "kfkfcbiekfcjmohl_mldivide.h" - -// Function for MATLAB Function: '/MATLAB Function' -void kfkfcbiekfcjmohl_mldivide(const real32_T A[16], real32_T B[16]) -{ - real32_T temp; - real32_T b_A[16]; - int8_T ipiv[4]; - int32_T j; - real32_T s; - int32_T iy; - int32_T c_ix; - int32_T d; - int32_T ijA; - int32_T b_jBcol; - int32_T b_kAcol; - int32_T c_i; - memcpy(&b_A[0], &A[0], (uint32_T)(sizeof(real32_T) << 4U)); - ipiv[0] = 1; - ipiv[1] = 2; - ipiv[2] = 3; - ipiv[3] = 4; - for (j = 0; j < 3; j++) { - b_jBcol = (int32_T)(j * 5); - iy = 0; - b_kAcol = b_jBcol; - temp = (real32_T)fabs((real_T)b_A[b_jBcol]); - for (c_i = 2; c_i <= (int32_T)(4 - j); c_i++) { - b_kAcol++; - s = (real32_T)fabs((real_T)b_A[b_kAcol]); - if (s > temp) { - iy = (int32_T)(c_i - 1); - temp = s; - } - } - - if (b_A[(int32_T)(b_jBcol + iy)] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)(int32_T)((int32_T)(j + iy) + 1); - iy += j; - temp = b_A[j]; - b_A[j] = b_A[iy]; - b_A[iy] = temp; - b_kAcol = (int32_T)(j + 4); - iy += 4; - temp = b_A[b_kAcol]; - b_A[b_kAcol] = b_A[iy]; - b_A[iy] = temp; - b_kAcol += 4; - iy += 4; - temp = b_A[b_kAcol]; - b_A[b_kAcol] = b_A[iy]; - b_A[iy] = temp; - b_kAcol += 4; - iy += 4; - temp = b_A[b_kAcol]; - b_A[b_kAcol] = b_A[iy]; - b_A[iy] = temp; - } - - iy = (int32_T)((int32_T)(b_jBcol - j) + 4); - for (b_kAcol = (int32_T)(b_jBcol + 1); (int32_T)(b_kAcol + 1) <= iy; - b_kAcol++) { - b_A[b_kAcol] /= b_A[b_jBcol]; - } - } - - iy = b_jBcol; - b_kAcol = (int32_T)(b_jBcol + 4); - for (c_i = 1; c_i <= (int32_T)(3 - j); c_i++) { - temp = b_A[b_kAcol]; - if (b_A[b_kAcol] != 0.0F) { - c_ix = (int32_T)(b_jBcol + 1); - d = (int32_T)((int32_T)(iy - j) + 8); - for (ijA = (int32_T)(5 + iy); (int32_T)(ijA + 1) <= d; ijA++) { - b_A[ijA] += b_A[c_ix] * -temp; - c_ix++; - } - } - - b_kAcol += 4; - iy += 4; - } - } - - for (j = 0; j < 3; j++) { - if ((int32_T)(j + 1) != (int32_T)ipiv[j]) { - b_jBcol = (int32_T)((int32_T)ipiv[j] - 1); - temp = B[j]; - B[j] = B[b_jBcol]; - B[b_jBcol] = temp; - temp = B[(int32_T)(j + 4)]; - B[(int32_T)(j + 4)] = B[(int32_T)(b_jBcol + 4)]; - B[(int32_T)(b_jBcol + 4)] = temp; - temp = B[(int32_T)(j + 8)]; - B[(int32_T)(j + 8)] = B[(int32_T)(b_jBcol + 8)]; - B[(int32_T)(b_jBcol + 8)] = temp; - temp = B[(int32_T)(j + 12)]; - B[(int32_T)(j + 12)] = B[(int32_T)(b_jBcol + 12)]; - B[(int32_T)(b_jBcol + 12)] = temp; - } - } - - for (j = 0; j < 4; j++) { - b_jBcol = (int32_T)(j << 2); - if (B[b_jBcol] != 0.0F) { - for (c_i = 1; (int32_T)(c_i + 1) < 5; c_i++) { - B[(int32_T)(c_i + b_jBcol)] -= B[b_jBcol] * b_A[c_i]; - } - } - - if (B[(int32_T)(1 + b_jBcol)] != 0.0F) { - for (c_i = 2; (int32_T)(c_i + 1) < 5; c_i++) { - B[(int32_T)(c_i + b_jBcol)] -= B[(int32_T)(1 + b_jBcol)] * b_A[(int32_T) - (c_i + 4)]; - } - } - - if (B[(int32_T)(2 + b_jBcol)] != 0.0F) { - for (c_i = 3; (int32_T)(c_i + 1) < 5; c_i++) { - B[(int32_T)(c_i + b_jBcol)] -= B[(int32_T)(2 + b_jBcol)] * b_A[(int32_T) - (c_i + 8)]; - } - } - } - - for (j = 0; j < 4; j++) { - b_jBcol = (int32_T)(j << 2); - if (B[(int32_T)(3 + b_jBcol)] != 0.0F) { - B[(int32_T)(3 + b_jBcol)] /= b_A[15]; - for (c_i = 0; (int32_T)(c_i + 1) < 4; c_i++) { - B[(int32_T)(c_i + b_jBcol)] -= B[(int32_T)(3 + b_jBcol)] * b_A[(int32_T) - (c_i + 12)]; - } - } - - if (B[(int32_T)(2 + b_jBcol)] != 0.0F) { - B[(int32_T)(2 + b_jBcol)] /= b_A[10]; - for (c_i = 0; (int32_T)(c_i + 1) < 3; c_i++) { - B[(int32_T)(c_i + b_jBcol)] -= B[(int32_T)(2 + b_jBcol)] * b_A[(int32_T) - (c_i + 8)]; - } - } - - if (B[(int32_T)(1 + b_jBcol)] != 0.0F) { - B[(int32_T)(1 + b_jBcol)] /= b_A[5]; - for (c_i = 0; (int32_T)(c_i + 1) < 2; c_i++) { - B[(int32_T)(c_i + b_jBcol)] -= B[(int32_T)(1 + b_jBcol)] * b_A[(int32_T) - (c_i + 4)]; - } - } - - if (B[b_jBcol] != 0.0F) { - B[b_jBcol] /= b_A[0]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfkfcbiekfcjmohl_mldivide.h b/gnc/matlab/code_generation/sharedutils/kfkfcbiekfcjmohl_mldivide.h deleted file mode 100644 index c3b6a3d829..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfkfcbiekfcjmohl_mldivide.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: kfkfcbiekfcjmohl_mldivide.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_kfkfcbiekfcjmohl_mldivide -#define SHARE_kfkfcbiekfcjmohl_mldivide -#include "rtwtypes.h" - -extern void kfkfcbiekfcjmohl_mldivide(const real32_T A[16], real32_T B[16]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfkfopppmophfcjm_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/kfkfopppmophfcjm_xaxpy.cpp deleted file mode 100644 index 877d5d5cfd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfkfopppmophfcjm_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: kfkfopppmophfcjm_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "kfkfopppmophfcjm_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void kfkfopppmophfcjm_xaxpy(int32_T n, real32_T a, const real32_T x_data[], - int32_T ix0, real32_T y_data[], int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y_data[iy] += a * x_data[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfkfopppmophfcjm_xaxpy.h b/gnc/matlab/code_generation/sharedutils/kfkfopppmophfcjm_xaxpy.h deleted file mode 100644 index 726bcb501b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfkfopppmophfcjm_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: kfkfopppmophfcjm_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_kfkfopppmophfcjm_xaxpy -#define SHARE_kfkfopppmophfcjm_xaxpy -#include "rtwtypes.h" - -extern void kfkfopppmophfcjm_xaxpy(int32_T n, real32_T a, const real32_T x_data[], - int32_T ix0, real32_T y_data[], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfknaaaamglfppph_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/kfknaaaamglfppph_xnrm2.cpp deleted file mode 100644 index 1c3dad43fe..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfknaaaamglfppph_xnrm2.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: kfknaaaamglfppph_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "kfknaaaamglfppph_xnrm2.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T kfknaaaamglfppph_xnrm2(int32_T n, const real32_T x_data[], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (!(n < 1)) { - if (n == 1) { - y = (real32_T)fabs((real_T)x_data[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x_data[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kfknaaaamglfppph_xnrm2.h b/gnc/matlab/code_generation/sharedutils/kfknaaaamglfppph_xnrm2.h deleted file mode 100644 index 4c83d35d11..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kfknaaaamglfppph_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: kfknaaaamglfppph_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_kfknaaaamglfppph_xnrm2 -#define SHARE_kfknaaaamglfppph_xnrm2 -#include "rtwtypes.h" - -extern real32_T kfknaaaamglfppph_xnrm2(int32_T n, const real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kngdbaaaiecjbiec_skip_to_last_equal_value.cpp b/gnc/matlab/code_generation/sharedutils/kngdbaaaiecjbiec_skip_to_last_equal_value.cpp deleted file mode 100644 index 4312542bb5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kngdbaaaiecjbiec_skip_to_last_equal_value.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// -// File: kngdbaaaiecjbiec_skip_to_last_equal_value.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "kngdbaaaiecjbiec_skip_to_last_equal_value.h" - -// Function for MATLAB Function: '/generate_output' -real32_T kngdbaaaiecjbiec_skip_to_last_equal_value(int32_T *k, const real32_T x - [50]) -{ - real32_T xk; - boolean_T p; - real32_T absxk; - int32_T exponent; - boolean_T exitg1; - xk = x[(int32_T)(*k - 1)]; - exitg1 = false; - while ((!exitg1) && (*k < 50)) { - absxk = (real32_T)fabs((real_T)(xk / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, (int32_T)(exponent - 24)); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(xk - x[*k])) < absxk) || (rtIsInfF(x[*k]) && - rtIsInfF(xk) && ((x[*k] > 0.0F) == (xk > 0.0F)))) { - p = true; - } else { - p = false; - } - - if (p) { - (*k)++; - } else { - exitg1 = true; - } - } - - return xk; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kngdbaaaiecjbiec_skip_to_last_equal_value.h b/gnc/matlab/code_generation/sharedutils/kngdbaaaiecjbiec_skip_to_last_equal_value.h deleted file mode 100644 index 234388f632..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kngdbaaaiecjbiec_skip_to_last_equal_value.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: kngdbaaaiecjbiec_skip_to_last_equal_value.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_kngdbaaaiecjbiec_skip_to_last_equal_value -#define SHARE_kngdbaaaiecjbiec_skip_to_last_equal_value -#include "rtwtypes.h" - -extern real32_T kngdbaaaiecjbiec_skip_to_last_equal_value(int32_T *k, const - real32_T x[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kngldbimhdbaimgd_quat_propagate_step.cpp b/gnc/matlab/code_generation/sharedutils/kngldbimhdbaimgd_quat_propagate_step.cpp deleted file mode 100644 index d28a169a9b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kngldbimhdbaimgd_quat_propagate_step.cpp +++ /dev/null @@ -1,143 +0,0 @@ -// -// File: kngldbimhdbaimgd_quat_propagate_step.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "kngldbimhdbaimgd_quat_propagate_step.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void kngldbimhdbaimgd_quat_propagate_step(const real32_T quat_in[4], const - real_T omega[3], real32_T time_in, real_T quat_out[4]) -{ - real_T omega_mag; - real32_T c; - real32_T sine_mag; - real_T d_y; - real32_T c_0[16]; - int32_T i; - real32_T s_idx_1; - real32_T s_idx_0; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // omega row vector - // - // From: Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra - // Equation below is from Eq. 122, with Omega matrix and - // identity matrix multiplied together - // - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Takes the root-sum square of each row of the matrix a - omega_mag = sqrt((omega[0] * omega[0] + omega[1] * omega[1]) + omega[2] * - omega[2]); - if ((omega_mag == 0.0) || (time_in <= 0.0F)) { - quat_out[0] = (real_T)quat_in[0]; - quat_out[1] = (real_T)quat_in[1]; - quat_out[2] = (real_T)quat_in[2]; - quat_out[3] = (real_T)quat_in[3]; - } else { - c = (real32_T)cos((real_T)((real32_T)(0.5 * omega_mag) * time_in)); - sine_mag = (real32_T)sin((real_T)((real32_T)(0.5 * omega_mag) * time_in)); - s_idx_0 = sine_mag * (real32_T)omega[0] / (real32_T)omega_mag; - s_idx_1 = sine_mag * (real32_T)omega[1] / (real32_T)omega_mag; - sine_mag = sine_mag * (real32_T)omega[2] / (real32_T)omega_mag; - - // Rollup of trig equations and Omega matrix - c_0[0] = c; - c_0[1] = -sine_mag; - c_0[2] = s_idx_1; - c_0[3] = -s_idx_0; - c_0[4] = sine_mag; - c_0[5] = c; - c_0[6] = -s_idx_0; - c_0[7] = -s_idx_1; - c_0[8] = -s_idx_1; - c_0[9] = s_idx_0; - c_0[10] = c; - c_0[11] = -sine_mag; - c_0[12] = s_idx_0; - c_0[13] = s_idx_1; - c_0[14] = sine_mag; - c_0[15] = c; - for (i = 0; i < 4; i++) { - c = c_0[(int32_T)(i + 12)] * quat_in[3] + (c_0[(int32_T)(i + 8)] * - quat_in[2] + (c_0[(int32_T)(i + 4)] * quat_in[1] + c_0[i] * quat_in[0])); - quat_out[i] = (real_T)c; - } - - if (quat_out[3] < 0.0) { - omega_mag = -1.0; - } else if (quat_out[3] > 0.0) { - omega_mag = 1.0; - } else if (quat_out[3] == 0.0) { - omega_mag = 0.0; - } else { - omega_mag = quat_out[3]; - } - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Takes the root-sum square of each row of the matrix a - d_y = sqrt(((quat_out[0] * quat_out[0] + quat_out[1] * quat_out[1]) + - quat_out[2] * quat_out[2]) + quat_out[3] * quat_out[3]); - quat_out[0] = omega_mag * quat_out[0] / d_y; - quat_out[1] = omega_mag * quat_out[1] / d_y; - quat_out[2] = omega_mag * quat_out[2] / d_y; - quat_out[3] = omega_mag * quat_out[3] / d_y; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/kngldbimhdbaimgd_quat_propagate_step.h b/gnc/matlab/code_generation/sharedutils/kngldbimhdbaimgd_quat_propagate_step.h deleted file mode 100644 index 5c1b2e0197..0000000000 --- a/gnc/matlab/code_generation/sharedutils/kngldbimhdbaimgd_quat_propagate_step.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: kngldbimhdbaimgd_quat_propagate_step.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_kngldbimhdbaimgd_quat_propagate_step -#define SHARE_kngldbimhdbaimgd_quat_propagate_step -#include "rtwtypes.h" - -extern void kngldbimhdbaimgd_quat_propagate_step(const real32_T quat_in[4], - const real_T omega[3], real32_T time_in, real_T quat_out[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knglnoppglnoimoh_xscal.cpp b/gnc/matlab/code_generation/sharedutils/knglnoppglnoimoh_xscal.cpp deleted file mode 100644 index c4c6c40b09..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knglnoppglnoimoh_xscal.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// -// File: knglnoppglnoimoh_xscal.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "knglnoppglnoimoh_xscal.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void knglnoppglnoimoh_xscal(int32_T n, real32_T a, real32_T x_data[], int32_T - ix0) -{ - int32_T b; - int32_T k; - b = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= b; k++) { - x_data[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knglnoppglnoimoh_xscal.h b/gnc/matlab/code_generation/sharedutils/knglnoppglnoimoh_xscal.h deleted file mode 100644 index ba1258390a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knglnoppglnoimoh_xscal.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: knglnoppglnoimoh_xscal.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_knglnoppglnoimoh_xscal -#define SHARE_knglnoppglnoimoh_xscal -#include "rtwtypes.h" - -extern void knglnoppglnoimoh_xscal(int32_T n, real32_T a, real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knohaiececjedbai_ilazlc.cpp b/gnc/matlab/code_generation/sharedutils/knohaiececjedbai_ilazlc.cpp deleted file mode 100644 index 21c4805775..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knohaiececjedbai_ilazlc.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: knohaiececjedbai_ilazlc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "knohaiececjedbai_ilazlc.h" - -// Function for MATLAB Function: '/Compute Residual and H' -int32_T knohaiececjedbai_ilazlc(int32_T m, int32_T n, const real32_T A_data[], - int32_T ia0, int32_T lda) -{ - int32_T j; - int32_T coltop; - int32_T ia; - int32_T exitg1; - boolean_T exitg2; - j = n; - exitg2 = false; - while ((!exitg2) && (j > 0)) { - coltop = (int32_T)((int32_T)((int32_T)(j - 1) * lda) + ia0); - ia = coltop; - do { - exitg1 = 0; - if (ia <= (int32_T)((int32_T)(coltop + m) - 1)) { - if (A_data[(int32_T)(ia - 1)] != 0.0F) { - exitg1 = 1; - } else { - ia++; - } - } else { - j--; - exitg1 = 2; - } - } while (exitg1 == 0); - - if (exitg1 == 1) { - exitg2 = true; - } - } - - return j; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knohaiececjedbai_ilazlc.h b/gnc/matlab/code_generation/sharedutils/knohaiececjedbai_ilazlc.h deleted file mode 100644 index cd6fe765f5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knohaiececjedbai_ilazlc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: knohaiececjedbai_ilazlc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_knohaiececjedbai_ilazlc -#define SHARE_knohaiececjedbai_ilazlc -#include "rtwtypes.h" - -extern int32_T knohaiececjedbai_ilazlc(int32_T m, int32_T n, const real32_T - A_data[], int32_T ia0, int32_T lda); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knohbaiengdbnoph_xscal.cpp b/gnc/matlab/code_generation/sharedutils/knohbaiengdbnoph_xscal.cpp deleted file mode 100644 index d0f408d5ed..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knohbaiengdbnoph_xscal.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// -// File: knohbaiengdbnoph_xscal.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "knohbaiengdbnoph_xscal.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void knohbaiengdbnoph_xscal(int32_T n, real32_T a, real32_T x_data[], int32_T - ix0) -{ - int32_T b; - int32_T k; - b = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= b; k++) { - x_data[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knohbaiengdbnoph_xscal.h b/gnc/matlab/code_generation/sharedutils/knohbaiengdbnoph_xscal.h deleted file mode 100644 index 8a42c4eb82..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knohbaiengdbnoph_xscal.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: knohbaiengdbnoph_xscal.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_knohbaiengdbnoph_xscal -#define SHARE_knohbaiengdbnoph_xscal -#include "rtwtypes.h" - -extern void knohbaiengdbnoph_xscal(int32_T n, real32_T a, real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knohlnohphdbkfcj_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/knohlnohphdbkfcj_xnrm2.cpp deleted file mode 100644 index 3e9e3f9328..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knohlnohphdbkfcj_xnrm2.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: knohlnohphdbkfcj_xnrm2.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include -#include "knohlnohphdbkfcj_xnrm2.h" - -// Function for MATLAB Function: '/MATLAB Function' -real32_T knohlnohphdbkfcj_xnrm2(int32_T n, const real32_T x[6], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (!(n < 1)) { - if (n == 1) { - y = (real32_T)fabs((real_T)x[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knohlnohphdbkfcj_xnrm2.h b/gnc/matlab/code_generation/sharedutils/knohlnohphdbkfcj_xnrm2.h deleted file mode 100644 index 3d3d64bf2d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knohlnohphdbkfcj_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: knohlnohphdbkfcj_xnrm2.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_knohlnohphdbkfcj_xnrm2 -#define SHARE_knohlnohphdbkfcj_xnrm2 -#include "rtwtypes.h" - -extern real32_T knohlnohphdbkfcj_xnrm2(int32_T n, const real32_T x[6], int32_T - ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knopcjmocbaacbaa_xscal.cpp b/gnc/matlab/code_generation/sharedutils/knopcjmocbaacbaa_xscal.cpp deleted file mode 100644 index 2c4cc216b2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knopcjmocbaacbaa_xscal.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: knopcjmocbaacbaa_xscal.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "knopcjmocbaacbaa_xscal.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void knopcjmocbaacbaa_xscal(real32_T a, real32_T x[4], int32_T ix0) -{ - int32_T k; - for (k = ix0; k <= (int32_T)(ix0 + 1); k++) { - x[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knopcjmocbaacbaa_xscal.h b/gnc/matlab/code_generation/sharedutils/knopcjmocbaacbaa_xscal.h deleted file mode 100644 index c75e69e6ca..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knopcjmocbaacbaa_xscal.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: knopcjmocbaacbaa_xscal.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_knopcjmocbaacbaa_xscal -#define SHARE_knopcjmocbaacbaa_xscal -#include "rtwtypes.h" - -extern void knopcjmocbaacbaa_xscal(real32_T a, real32_T x[4], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knoplnopecjmpphd_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/knoplnopecjmpphd_xaxpy.cpp deleted file mode 100644 index 57764404d2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knoplnopecjmpphd_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: knoplnopecjmpphd_xaxpy.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "knoplnopecjmpphd_xaxpy.h" - -// Function for MATLAB Function: '/MATLAB Function' -void knoplnopecjmpphd_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T y[36], - int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * y[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/knoplnopecjmpphd_xaxpy.h b/gnc/matlab/code_generation/sharedutils/knoplnopecjmpphd_xaxpy.h deleted file mode 100644 index ddb1b0d812..0000000000 --- a/gnc/matlab/code_generation/sharedutils/knoplnopecjmpphd_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: knoplnopecjmpphd_xaxpy.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_knoplnopecjmpphd_xaxpy -#define SHARE_knoplnopecjmpphd_xaxpy -#include "rtwtypes.h" - -extern void knoplnopecjmpphd_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T - y[36], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lfcjdbieaimomoph_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/lfcjdbieaimomoph_xdotc.cpp deleted file mode 100644 index b67a5f1754..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lfcjdbieaimomoph_xdotc.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: lfcjdbieaimomoph_xdotc.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "lfcjdbieaimomoph_xdotc.h" - -// Function for MATLAB Function: '/MATLAB Function' -real32_T lfcjdbieaimomoph_xdotc(int32_T n, const real32_T x[36], int32_T ix0, - const real32_T y[36], int32_T iy0) -{ - real32_T d; - int32_T ix; - int32_T iy; - int32_T k; - d = 0.0F; - if (!(n < 1)) { - ix = ix0; - iy = iy0; - for (k = 1; k <= n; k++) { - d += x[(int32_T)(ix - 1)] * y[(int32_T)(iy - 1)]; - ix++; - iy++; - } - } - - return d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lfcjdbieaimomoph_xdotc.h b/gnc/matlab/code_generation/sharedutils/lfcjdbieaimomoph_xdotc.h deleted file mode 100644 index 4eb66d0666..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lfcjdbieaimomoph_xdotc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: lfcjdbieaimomoph_xdotc.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_lfcjdbieaimomoph_xdotc -#define SHARE_lfcjdbieaimomoph_xdotc -#include "rtwtypes.h" - -extern real32_T lfcjdbieaimomoph_xdotc(int32_T n, const real32_T x[36], int32_T - ix0, const real32_T y[36], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lfcjglnghlfknglf_xscal.cpp b/gnc/matlab/code_generation/sharedutils/lfcjglnghlfknglf_xscal.cpp deleted file mode 100644 index b4063a4d23..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lfcjglnghlfknglf_xscal.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: lfcjglnghlfknglf_xscal.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "lfcjglnghlfknglf_xscal.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void lfcjglnghlfknglf_xscal(real32_T a, real32_T x_data[], int32_T ix0) -{ - int32_T k; - for (k = ix0; k <= (int32_T)(ix0 + 2); k++) { - x_data[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lfcjglnghlfknglf_xscal.h b/gnc/matlab/code_generation/sharedutils/lfcjglnghlfknglf_xscal.h deleted file mode 100644 index 88e645316e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lfcjglnghlfknglf_xscal.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: lfcjglnghlfknglf_xscal.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_lfcjglnghlfknglf_xscal -#define SHARE_lfcjglnghlfknglf_xscal -#include "rtwtypes.h" - -extern void lfcjglnghlfknglf_xscal(real32_T a, real32_T x_data[], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lfcjmoppknohbimo_svd.cpp b/gnc/matlab/code_generation/sharedutils/lfcjmoppknohbimo_svd.cpp deleted file mode 100644 index b493711c9b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lfcjmoppknohbimo_svd.cpp +++ /dev/null @@ -1,545 +0,0 @@ -// -// File: lfcjmoppknohbimo_svd.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "cjecbaaifkfkkngd_xaxpy.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "imohaimgbaimjmgl_xdotc.h" -#include "jmgljecbphlnngln_xnrm2.h" -#include "jmohnoppbieciecj_xrot.h" -#include "lfcjglnghlfknglf_xscal.h" -#include "ohdjhlfcnohdhdba_xscal.h" -#include "ophdjekfieknglfk_xswap.h" -#include "phlfglfcdbaajmgd_xswap.h" -#include "pphdglfkecjmjekf_xrot.h" -#include "rt_nonfinite.h" -#include "lfcjmoppknohbimo_svd.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void lfcjmoppknohbimo_svd(const real32_T A_data[], const int32_T A_sizes[2], - real32_T U_data[], int32_T U_sizes[2], real32_T S_data[], int32_T S_sizes[2], - real32_T V_data[], int32_T V_sizes[2]) -{ - int32_T p; - int32_T minnp; - int32_T nct; - int32_T q; - boolean_T apply_transform; - int32_T iter; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - real32_T varargin_1[5]; - real32_T b_A_data[6]; - int32_T b_A_sizes[2]; - real32_T s_data[2]; - real32_T e_data[2]; - real32_T Vf_data[4]; - real32_T n_data[9]; - int32_T n_sizes[2]; - real32_T o_data[96]; - int32_T o_sizes[2]; - int32_T Vf_sizes_idx_0; - int32_T Vf_sizes_idx_1; - int8_T d_idx_0; - int8_T d_idx_1; - boolean_T exitg1; - boolean_T exitg2; - boolean_T exitg3; - b_A_sizes[0] = 3; - b_A_sizes[1] = A_sizes[1]; - qs = (int32_T)(A_sizes[0] * A_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - b_A_data[kase] = A_data[kase]; - } - - p = A_sizes[1]; - minnp = A_sizes[1]; - qs = (int32_T)(int8_T)A_sizes[1]; - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - s_data[kase] = 0.0F; - } - - qs = (int32_T)(int8_T)A_sizes[1]; - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - e_data[kase] = 0.0F; - } - - d_idx_1 = (int8_T)A_sizes[1]; - U_sizes[0] = 3; - U_sizes[1] = (int32_T)d_idx_1; - qs = (int32_T)(3 * (int32_T)d_idx_1); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - U_data[kase] = 0.0F; - } - - d_idx_0 = (int8_T)A_sizes[1]; - d_idx_1 = (int8_T)A_sizes[1]; - Vf_sizes_idx_0 = (int32_T)d_idx_0; - Vf_sizes_idx_1 = (int32_T)d_idx_1; - qs = (int32_T)((int32_T)d_idx_0 * (int32_T)d_idx_1); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - Vf_data[kase] = 0.0F; - } - - if (A_sizes[1] == 0) { - for (nct = 0; (int32_T)(nct + 1) <= minnp; nct++) { - U_data[(int32_T)(nct + (int32_T)(3 * nct))] = 1.0F; - } - } else { - nct = A_sizes[1]; - for (q = 0; (int32_T)(q + 1) <= nct; q++) { - iter = (int32_T)((int32_T)(3 * q) + q); - apply_transform = false; - if ((int32_T)(q + 1) <= nct) { - snorm = jmgljecbphlnngln_xnrm2((int32_T)(3 - q), b_A_data, (int32_T) - (iter + 1)); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A_data[iter] < 0.0F) { - s_data[q] = -snorm; - } else { - s_data[q] = snorm; - } - - if ((real32_T)fabs((real_T)s_data[q]) >= 9.86076132E-32F) { - snorm = 1.0F / s_data[q]; - qs = (int32_T)((int32_T)(iter - q) + 3); - for (kase = iter; (int32_T)(kase + 1) <= qs; kase++) { - b_A_data[kase] *= snorm; - } - } else { - qs = (int32_T)((int32_T)(iter - q) + 3); - for (kase = iter; (int32_T)(kase + 1) <= qs; kase++) { - b_A_data[kase] /= s_data[q]; - } - } - - b_A_data[iter]++; - s_data[q] = -s_data[q]; - } else { - s_data[q] = 0.0F; - } - } - - qs = (int32_T)(q + 2); - while (qs <= p) { - if (apply_transform) { - cjecbaaifkfkkngd_xaxpy((int32_T)(3 - q), -(imohaimgbaimjmgl_xdotc - ((int32_T)(3 - q), b_A_data, (int32_T)(iter + 1), b_A_data, (int32_T) - (q + 4)) / b_A_data[(int32_T)(q + (int32_T)(b_A_sizes[0] * q))]), - (int32_T)(iter + 1), b_A_data, (int32_T)(q + 4)); - } - - e_data[1] = b_A_data[(int32_T)(q + 3)]; - qs = 3; - } - - if ((int32_T)(q + 1) <= nct) { - for (iter = q; (int32_T)(iter + 1) < 4; iter++) { - U_data[(int32_T)(iter + (int32_T)(3 * q))] = b_A_data[(int32_T) - ((int32_T)(b_A_sizes[0] * q) + iter)]; - } - } - } - - nct = A_sizes[1]; - if (1 < A_sizes[1]) { - e_data[0] = b_A_data[b_A_sizes[0]]; - } - - e_data[(int32_T)(A_sizes[1] - 1)] = 0.0F; - if ((int32_T)(A_sizes[1] + 1) <= A_sizes[1]) { - iter = (int32_T)(A_sizes[1] + 1); - while (iter <= 2) { - U_data[3] = 0.0F; - U_data[5] = 0.0F; - iter = 3; - U_data[4] = 1.0F; - } - } - - for (q = (int32_T)(A_sizes[1] - 1); (int32_T)(q + 1) > 0; q--) { - iter = (int32_T)((int32_T)(3 * q) + q); - if (s_data[q] != 0.0F) { - qs = (int32_T)(q + 2); - while (qs <= minnp) { - cjecbaaifkfkkngd_xaxpy((int32_T)(3 - q), -(imohaimgbaimjmgl_xdotc - ((int32_T)(3 - q), U_data, (int32_T)(iter + 1), U_data, (int32_T)(q - + 4)) / U_data[iter]), (int32_T)(iter + 1), U_data, (int32_T)(q + 4)); - qs = 3; - } - - for (qs = q; (int32_T)(qs + 1) < 4; qs++) { - U_data[(int32_T)(qs + (int32_T)(U_sizes[0] * q))] = -U_data[(int32_T) - ((int32_T)(U_sizes[0] * q) + qs)]; - } - - U_data[iter]++; - iter = 1; - while (iter <= q) { - U_data[(int32_T)(U_sizes[0] * q)] = 0.0F; - iter = 2; - } - } else { - U_data[(int32_T)(U_sizes[0] * q)] = 0.0F; - U_data[(int32_T)(1 + (int32_T)(U_sizes[0] * q))] = 0.0F; - U_data[(int32_T)(2 + (int32_T)(U_sizes[0] * q))] = 0.0F; - U_data[iter] = 1.0F; - } - } - - for (iter = (int32_T)(A_sizes[1] - 1); (int32_T)(iter + 1) > 0; iter--) { - for (q = 1; q <= p; q++) { - Vf_data[(int32_T)((int32_T)(q + (int32_T)((int32_T)d_idx_0 * iter)) - 1)] - = 0.0F; - } - - Vf_data[(int32_T)(iter + (int32_T)((int32_T)d_idx_0 * iter))] = 1.0F; - } - - for (iter = 0; (int32_T)(iter + 1) <= nct; iter++) { - if (s_data[iter] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s_data[iter]); - snorm = s_data[iter] / ztest0; - s_data[iter] = ztest0; - if ((int32_T)(iter + 1) < nct) { - e_data[0] /= snorm; - } - - n_sizes[0] = 3; - n_sizes[1] = U_sizes[1]; - qs = (int32_T)(U_sizes[0] * U_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - n_data[kase] = U_data[kase]; - } - - lfcjglnghlfknglf_xscal(snorm, n_data, (int32_T)(1 + (int32_T)(3 * iter))); - U_sizes[0] = 3; - U_sizes[1] = n_sizes[1]; - qs = (int32_T)(n_sizes[0] * n_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - U_data[kase] = n_data[kase]; - } - } - - if (((int32_T)(iter + 1) < nct) && (e_data[0] != 0.0F)) { - ztest0 = (real32_T)fabs((real_T)e_data[0]); - snorm = ztest0 / e_data[0]; - e_data[0] = ztest0; - s_data[1] *= snorm; - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - qs = (int32_T)(Vf_sizes_idx_0 * Vf_sizes_idx_1); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - o_data[kase] = Vf_data[kase]; - } - - ohdjhlfcnohdhdba_xscal(p, snorm, o_data, (int32_T)(1 + p)); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - qs = (int32_T)(o_sizes[0] * o_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - Vf_data[kase] = o_data[kase]; - } - } - } - - iter = 0; - snorm = 0.0F; - for (q = 0; (int32_T)(q + 1) <= nct; q++) { - ztest0 = (real32_T)fabs((real_T)s_data[q]); - ztest = (real32_T)fabs((real_T)e_data[q]); - if ((ztest0 >= ztest) || rtIsNaNF(ztest)) { - ztest = ztest0; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - } - - while ((nct > 0) && (!(iter >= 75))) { - q = (int32_T)(nct - 1); - exitg3 = false; - while (!(exitg3 || (q == 0))) { - ztest0 = (real32_T)fabs((real_T)e_data[0]); - if ((ztest0 <= ((real32_T)fabs((real_T)s_data[0]) + (real32_T)fabs - ((real_T)s_data[1])) * 1.1920929E-7F) || ((ztest0 <= - 9.86076132E-32F) || ((iter > 20) && (ztest0 <= 1.1920929E-7F * - snorm)))) { - e_data[0] = 0.0F; - exitg3 = true; - } else { - q = 0; - } - } - - if ((int32_T)(nct - 1) == q) { - kase = 4; - } else { - qs = nct; - kase = nct; - exitg2 = false; - while ((!exitg2) && (kase >= q)) { - qs = kase; - if (kase == q) { - exitg2 = true; - } else { - ztest0 = 0.0F; - if (kase < nct) { - ztest0 = (real32_T)fabs((real_T)e_data[0]); - } - - if (kase > (int32_T)(q + 1)) { - ztest0 += (real32_T)fabs((real_T)e_data[0]); - } - - ztest = (real32_T)fabs((real_T)s_data[(int32_T)(kase - 1)]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) - { - s_data[(int32_T)(kase - 1)] = 0.0F; - exitg2 = true; - } else { - kase--; - } - } - } - - if (qs == q) { - kase = 3; - } else if (qs == nct) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e_data[0]; - e_data[0] = 0.0F; - qs = (int32_T)(nct - 1); - while (qs >= (int32_T)(q + 1)) { - ztest = s_data[0]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s_data[0] = ztest; - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - qs = (int32_T)(Vf_sizes_idx_0 * Vf_sizes_idx_1); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - o_data[kase] = Vf_data[kase]; - } - - pphdglfkecjmjekf_xrot(p, o_data, 1, (int32_T)(1 + (int32_T)(p * - (int32_T)(nct - 1))), sqds, smm1); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - qs = (int32_T)(o_sizes[0] * o_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - Vf_data[kase] = o_data[kase]; - } - - qs = 0; - } - break; - - case 2: - ztest0 = e_data[(int32_T)(q - 1)]; - e_data[(int32_T)(q - 1)] = 0.0F; - for (qs = q; (int32_T)(qs + 1) <= nct; qs++) { - ztest = s_data[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s_data[qs] = ztest; - ztest0 = -smm1 * e_data[qs]; - e_data[qs] *= sqds; - jmohnoppbieciecj_xrot(U_data, (int32_T)(1 + (int32_T)(3 * qs)), - (int32_T)(1 + (int32_T)(3 * (int32_T)(q - 1))), - sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s_data[(int32_T)(nct - 1)]); - varargin_1[1] = (real32_T)fabs((real_T)s_data[0]); - varargin_1[2] = (real32_T)fabs((real_T)e_data[0]); - varargin_1[3] = (real32_T)fabs((real_T)s_data[q]); - varargin_1[4] = (real32_T)fabs((real_T)e_data[q]); - qs = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - kase = 2; - exitg1 = false; - while ((!exitg1) && (kase < 6)) { - qs = kase; - if (!rtIsNaNF(varargin_1[(int32_T)(kase - 1)])) { - ztest = varargin_1[(int32_T)(kase - 1)]; - exitg1 = true; - } else { - kase++; - } - } - } - - if (qs < 5) { - while ((int32_T)(qs + 1) < 6) { - if (varargin_1[qs] > ztest) { - ztest = varargin_1[qs]; - } - - qs++; - } - } - - ztest0 = s_data[(int32_T)(nct - 1)] / ztest; - smm1 = s_data[0] / ztest; - emm1 = e_data[0] / ztest; - sqds = s_data[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e_data[q] / ztest * sqds; - while ((int32_T)(q + 1) <= 1) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - ztest0 = sqds * s_data[0] + smm1 * e_data[0]; - e_data[0] = sqds * e_data[0] - smm1 * s_data[0]; - ztest = smm1 * s_data[1]; - s_data[1] *= sqds; - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - qs = (int32_T)(Vf_sizes_idx_0 * Vf_sizes_idx_1); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - o_data[kase] = Vf_data[kase]; - } - - pphdglfkecjmjekf_xrot(p, o_data, 1, (int32_T)(1 + p), sqds, smm1); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - qs = (int32_T)(o_sizes[0] * o_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - Vf_data[kase] = o_data[kase]; - } - - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s_data[0] = ztest0; - ztest0 = sqds * e_data[0] + smm1 * s_data[1]; - s_data[1] = -smm1 * e_data[0] + sqds * s_data[1]; - ztest = smm1 * e_data[1]; - e_data[1] *= sqds; - jmohnoppbieciecj_xrot(U_data, 1, 4, sqds, smm1); - q = 1; - } - - e_data[0] = ztest0; - iter++; - break; - - default: - if (s_data[q] < 0.0F) { - s_data[q] = -s_data[q]; - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - qs = (int32_T)(Vf_sizes_idx_0 * Vf_sizes_idx_1); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - o_data[kase] = Vf_data[kase]; - } - - ohdjhlfcnohdhdba_xscal(p, -1.0F, o_data, (int32_T)(1 + (int32_T)(p * q))); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - qs = (int32_T)(o_sizes[0] * o_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - Vf_data[kase] = o_data[kase]; - } - } - - while (((int32_T)(q + 1) < A_sizes[1]) && (s_data[0] < s_data[1])) { - ztest0 = s_data[0]; - s_data[0] = s_data[1]; - s_data[1] = ztest0; - if (1 < p) { - o_sizes[0] = Vf_sizes_idx_0; - o_sizes[1] = Vf_sizes_idx_1; - qs = (int32_T)(Vf_sizes_idx_0 * Vf_sizes_idx_1); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - o_data[kase] = Vf_data[kase]; - } - - ophdjekfieknglfk_xswap(2, o_data, 1, 3); - Vf_sizes_idx_0 = o_sizes[0]; - Vf_sizes_idx_1 = o_sizes[1]; - qs = (int32_T)(o_sizes[0] * o_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - Vf_data[kase] = o_data[kase]; - } - } - - phlfglfcdbaajmgd_xswap(U_data); - q = 1; - } - - iter = 0; - nct--; - break; - } - } - } - - Vf_sizes_idx_1 = A_sizes[1]; - for (nct = 0; (int32_T)(nct + 1) <= minnp; nct++) { - e_data[nct] = s_data[nct]; - } - - d_idx_0 = (int8_T)A_sizes[1]; - V_sizes[0] = (int32_T)(int8_T)A_sizes[1]; - V_sizes[1] = (int32_T)(int8_T)A_sizes[1]; - for (nct = 0; (int32_T)(nct + 1) <= minnp; nct++) { - for (iter = 0; (int32_T)(iter + 1) <= p; iter++) { - V_data[(int32_T)(iter + (int32_T)((int32_T)d_idx_0 * nct))] = Vf_data - [(int32_T)((int32_T)(Vf_sizes_idx_0 * nct) + iter)]; - } - } - - S_sizes[0] = A_sizes[1]; - S_sizes[1] = A_sizes[1]; - qs = (int32_T)(A_sizes[1] * A_sizes[1]); - for (kase = 0; kase <= (int32_T)(qs - 1); kase++) { - S_data[kase] = 0.0F; - } - - for (p = 0; p <= (int32_T)(Vf_sizes_idx_1 - 1); p++) { - S_data[(int32_T)(p + (int32_T)(Vf_sizes_idx_1 * p))] = e_data[p]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lfcjmoppknohbimo_svd.h b/gnc/matlab/code_generation/sharedutils/lfcjmoppknohbimo_svd.h deleted file mode 100644 index 272b606899..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lfcjmoppknohbimo_svd.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: lfcjmoppknohbimo_svd.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_lfcjmoppknohbimo_svd -#define SHARE_lfcjmoppknohbimo_svd -#include "rtwtypes.h" - -extern void lfcjmoppknohbimo_svd(const real32_T A_data[], const int32_T A_sizes - [2], real32_T U_data[], int32_T U_sizes[2], real32_T S_data[], int32_T - S_sizes[2], real32_T V_data[], int32_T V_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lfcjpphlnoppphdb_xgerc.cpp b/gnc/matlab/code_generation/sharedutils/lfcjpphlnoppphdb_xgerc.cpp deleted file mode 100644 index 1b2f47d85d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lfcjpphlnoppphdb_xgerc.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// -// File: lfcjpphlnoppphdb_xgerc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "lfcjpphlnoppphdb_xgerc.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void lfcjpphlnoppphdb_xgerc(int32_T m, int32_T n, real32_T alpha1, int32_T ix0, - const real32_T y_data[], real32_T A_data[], int32_T ia0, int32_T lda) -{ - int32_T jA; - int32_T jy; - real32_T temp; - int32_T ix; - int32_T j; - int32_T b; - int32_T ijA; - if (!(alpha1 == 0.0F)) { - jA = (int32_T)(ia0 - 1); - jy = 0; - for (j = 1; j <= n; j++) { - if (y_data[jy] != 0.0F) { - temp = y_data[jy] * alpha1; - ix = ix0; - b = (int32_T)(m + jA); - for (ijA = jA; (int32_T)(ijA + 1) <= b; ijA++) { - A_data[ijA] += A_data[(int32_T)(ix - 1)] * temp; - ix++; - } - } - - jy++; - jA += lda; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lfcjpphlnoppphdb_xgerc.h b/gnc/matlab/code_generation/sharedutils/lfcjpphlnoppphdb_xgerc.h deleted file mode 100644 index d15d2194dd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lfcjpphlnoppphdb_xgerc.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: lfcjpphlnoppphdb_xgerc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_lfcjpphlnoppphdb_xgerc -#define SHARE_lfcjpphlnoppphdb_xgerc -#include "rtwtypes.h" - -extern void lfcjpphlnoppphdb_xgerc(int32_T m, int32_T n, real32_T alpha1, - int32_T ix0, const real32_T y_data[], real32_T A_data[], int32_T ia0, int32_T - lda); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lngdpphlmgdjekno_skip_to_last_equal_value.cpp b/gnc/matlab/code_generation/sharedutils/lngdpphlmgdjekno_skip_to_last_equal_value.cpp deleted file mode 100644 index e943ab5c2c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lngdpphlmgdjekno_skip_to_last_equal_value.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// -// File: lngdpphlmgdjekno_skip_to_last_equal_value.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "lngdpphlmgdjekno_skip_to_last_equal_value.h" - -// Function for MATLAB Function: '/generate_output' -real32_T lngdpphlmgdjekno_skip_to_last_equal_value(int32_T *k, const real32_T x - [50]) -{ - real32_T xk; - real32_T absxk; - int32_T exponent; - boolean_T exitg1; - xk = x[*k - 1]; - exitg1 = false; - while ((!exitg1) && (*k < 50)) { - absxk = (real32_T)fabs((real_T)(xk / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, exponent - 24); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(xk - x[*k])) < absxk) || (rtIsInfF(x[*k]) && - rtIsInfF(xk) && ((x[*k] > 0.0F) == (xk > 0.0F)))) { - (*k)++; - } else { - exitg1 = true; - } - } - - return xk; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lngdpphlmgdjekno_skip_to_last_equal_value.h b/gnc/matlab/code_generation/sharedutils/lngdpphlmgdjekno_skip_to_last_equal_value.h deleted file mode 100644 index 73d89ebd7f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lngdpphlmgdjekno_skip_to_last_equal_value.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: lngdpphlmgdjekno_skip_to_last_equal_value.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_lngdpphlmgdjekno_skip_to_last_equal_value -#define SHARE_lngdpphlmgdjekno_skip_to_last_equal_value -#include "rtwtypes.h" - -extern real32_T lngdpphlmgdjekno_skip_to_last_equal_value(int32_T *k, const - real32_T x[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lnglhlngkngdmohd_pinv.cpp b/gnc/matlab/code_generation/sharedutils/lnglhlngkngdmohd_pinv.cpp deleted file mode 100644 index be0713b893..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lnglhlngkngdmohd_pinv.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// -// File: lnglhlngkngdmohd_pinv.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:39 2017 -// -#include "rtwtypes.h" -#include -#include "aaaacjecophlngdj_svd.h" -#include "lnglhlngkngdmohd_pinv.h" - -// Function for MATLAB Function: '/MATLAB Function' -void lnglhlngkngdmohd_pinv(const real32_T A[36], real32_T X[36]) -{ - real32_T b_X[36]; - real32_T V[9]; - int32_T r; - int32_T vcol; - real32_T tol; - int32_T j; - real32_T U[36]; - real32_T S[9]; - int32_T ar; - int32_T ia; - int32_T b; - int32_T ib; - int32_T b_ic; - real32_T A_0[36]; - memset(&b_X[0], 0, 36U * sizeof(real32_T)); - for (r = 0; r < 3; r++) { - for (vcol = 0; vcol < 12; vcol++) { - A_0[vcol + 12 * r] = A[3 * vcol + r]; - } - } - - aaaacjecophlngdj_svd(A_0, U, S, V); - tol = 12.0F * S[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while ((vcol + 1 < 4) && (S[3 * vcol + vcol] > tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (j = 0; j + 1 <= r; j++) { - tol = 1.0F / S[3 * j + j]; - for (ar = vcol; ar + 1 <= vcol + 3; ar++) { - V[ar] *= tol; - } - - vcol += 3; - } - - for (vcol = 0; vcol <= 34; vcol += 3) { - for (j = vcol; j + 1 <= vcol + 3; j++) { - b_X[j] = 0.0F; - } - } - - vcol = -1; - for (j = 0; j <= 34; j += 3) { - ar = 0; - vcol++; - b = ((r - 1) * 12 + vcol) + 1; - for (ib = vcol; ib + 1 <= b; ib += 12) { - if (U[ib] != 0.0F) { - ia = ar; - for (b_ic = j; b_ic + 1 <= j + 3; b_ic++) { - ia++; - b_X[b_ic] += V[ia - 1] * U[ib]; - } - } - - ar += 3; - } - } - } - - for (r = 0; r < 3; r++) { - for (vcol = 0; vcol < 12; vcol++) { - X[vcol + 12 * r] = b_X[3 * vcol + r]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lnglhlngkngdmohd_pinv.h b/gnc/matlab/code_generation/sharedutils/lnglhlngkngdmohd_pinv.h deleted file mode 100644 index c23c6fe407..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lnglhlngkngdmohd_pinv.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: lnglhlngkngdmohd_pinv.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:39 2017 -// -#ifndef SHARE_lnglhlngkngdmohd_pinv -#define SHARE_lnglhlngkngdmohd_pinv -#include "rtwtypes.h" - -extern void lnglhlngkngdmohd_pinv(const real32_T A[36], real32_T X[36]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lnohcjmoimglcbai_svd.cpp b/gnc/matlab/code_generation/sharedutils/lnohcjmoimglcbai_svd.cpp deleted file mode 100644 index 1de7189366..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lnohcjmoimglcbai_svd.cpp +++ /dev/null @@ -1,560 +0,0 @@ -// -// File: lnohcjmoimglcbai_svd.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include -#include -#include "cbielfcbnophecbi_xaxpy.h" -#include "cjekaaieekfkbimo_xnrm2.h" -#include "cjmgkfkfphdjaaai_xscal.h" -#include "djmgbimooppphlno_xswap.h" -#include "ekfklfkfohdjaimg_xaxpy.h" -#include "eknofcjenophiecj_xswap.h" -#include "fcbiohlnbimolnop_xnrm2.h" -#include "gdjmmgdjekfccjmg_xrot.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "hlfkiecjohlfopph_xaxpy.h" -#include "jekncbaiknglnoph_xdotc.h" -#include "mglnohlnekfcekfk_xdotc.h" -#include "ophdimgdcjekekfc_xrot.h" -#include "ophlaaaiophdbaai_xscal.h" -#include "opppmglfglfkdbaa_xaxpy.h" -#include "rt_nonfinite.h" -#include "lnohcjmoimglcbai_svd.h" - -// Function for MATLAB Function: '/MATLAB Function' -void lnohcjmoimglcbai_svd(const real32_T A[36], real32_T U[36], real32_T S[9], - real32_T V[9]) -{ - real32_T b_A[36]; - real32_T s[3]; - real32_T e[3]; - real32_T work[12]; - real32_T Vf[9]; - int32_T q; - boolean_T apply_transform; - int32_T m; - real32_T snorm; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - int32_T j_ii; - real32_T varargin_1[5]; - int32_T i; - boolean_T exitg1; - boolean_T exitg2; - int32_T exitg3; - memcpy(&b_A[0], &A[0], (uint32_T)(36U * sizeof(real32_T))); - e[0] = 0.0F; - e[1] = 0.0F; - e[2] = 0.0F; - for (i = 0; i < 12; i++) { - work[i] = 0.0F; - } - - memset(&U[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (i = 0; i < 9; i++) { - Vf[i] = 0.0F; - } - - i = 0; - apply_transform = false; - snorm = cjekaaieekfkbimo_xnrm2(12, b_A, 1); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[0] < 0.0F) { - s[0] = -snorm; - } else { - s[0] = snorm; - } - - if ((real32_T)fabs((real_T)s[0]) >= 9.86076132E-32F) { - snorm = 1.0F / s[0]; - for (qs = 0; (int32_T)(qs + 1) < 13; qs++) { - b_A[qs] *= snorm; - } - } else { - for (qs = 0; (int32_T)(qs + 1) < 13; qs++) { - b_A[qs] /= s[0]; - } - } - - b_A[0]++; - s[0] = -s[0]; - } else { - s[0] = 0.0F; - } - - for (q = 1; (int32_T)(q + 1) < 4; q++) { - qs = (int32_T)(12 * q); - if (apply_transform) { - hlfkiecjohlfopph_xaxpy(12, -(jekncbaiknglnoph_xdotc(12, b_A, 1, b_A, - (int32_T)(qs + 1)) / b_A[0]), 1, b_A, (int32_T)(qs + 1)); - } - - e[q] = b_A[qs]; - } - - while ((int32_T)(i + 1) < 13) { - U[i] = b_A[i]; - i++; - } - - snorm = fcbiohlnbimolnop_xnrm2(2, e, 2); - if (snorm == 0.0F) { - e[0] = 0.0F; - } else { - if (e[1] < 0.0F) { - snorm = -snorm; - } - - e[0] = snorm; - if ((real32_T)fabs((real_T)snorm) >= 9.86076132E-32F) { - snorm = 1.0F / snorm; - for (i = 1; (int32_T)(i + 1) < 4; i++) { - e[i] *= snorm; - } - } else { - for (i = 1; (int32_T)(i + 1) < 4; i++) { - e[i] /= snorm; - } - } - - e[1]++; - e[0] = -e[0]; - for (i = 2; i < 13; i++) { - work[(int32_T)(i - 1)] = 0.0F; - } - - for (i = 1; (int32_T)(i + 1) < 4; i++) { - opppmglfglfkdbaa_xaxpy(11, e[i], b_A, (int32_T)((int32_T)(12 * i) + 2), - work, 2); - } - - for (i = 1; (int32_T)(i + 1) < 4; i++) { - cbielfcbnophecbi_xaxpy(11, -e[i] / e[1], work, 2, b_A, (int32_T)((int32_T) - (12 * i) + 2)); - } - } - - for (i = 1; (int32_T)(i + 1) < 4; i++) { - Vf[i] = e[i]; - } - - apply_transform = false; - snorm = cjekaaieekfkbimo_xnrm2(11, b_A, 14); - if (snorm > 0.0F) { - apply_transform = true; - if (b_A[13] < 0.0F) { - s[1] = -snorm; - } else { - s[1] = snorm; - } - - if ((real32_T)fabs((real_T)s[1]) >= 9.86076132E-32F) { - snorm = 1.0F / s[1]; - for (qs = 13; (int32_T)(qs + 1) < 25; qs++) { - b_A[qs] *= snorm; - } - } else { - for (qs = 13; (int32_T)(qs + 1) < 25; qs++) { - b_A[qs] /= s[1]; - } - } - - b_A[13]++; - s[1] = -s[1]; - } else { - s[1] = 0.0F; - } - - for (q = 2; (int32_T)(q + 1) < 4; q++) { - qs = (int32_T)((int32_T)(12 * q) + 1); - if (apply_transform) { - hlfkiecjohlfopph_xaxpy(11, -(jekncbaiknglnoph_xdotc(11, b_A, 14, b_A, - (int32_T)(qs + 1)) / b_A[13]), 14, b_A, (int32_T)(qs + 1)); - } - - e[q] = b_A[qs]; - } - - for (i = 1; (int32_T)(i + 1) < 13; i++) { - U[(int32_T)(i + 12)] = b_A[(int32_T)(i + 12)]; - } - - snorm = cjekaaieekfkbimo_xnrm2(10, b_A, 27); - if (snorm > 0.0F) { - if (b_A[26] < 0.0F) { - s[2] = -snorm; - } else { - s[2] = snorm; - } - - if ((real32_T)fabs((real_T)s[2]) >= 9.86076132E-32F) { - snorm = 1.0F / s[2]; - for (qs = 26; (int32_T)(qs + 1) < 37; qs++) { - b_A[qs] *= snorm; - } - } else { - for (qs = 26; (int32_T)(qs + 1) < 37; qs++) { - b_A[qs] /= s[2]; - } - } - - b_A[26]++; - s[2] = -s[2]; - } else { - s[2] = 0.0F; - } - - for (i = 2; (int32_T)(i + 1) < 13; i++) { - U[(int32_T)(i + 24)] = b_A[(int32_T)(i + 24)]; - } - - m = 1; - for (q = 2; q >= 0; q += -1) { - i = (int32_T)((int32_T)(12 * q) + q); - if (s[q] != 0.0F) { - for (kase = (int32_T)(q + 1); (int32_T)(kase + 1) < 4; kase++) { - qs = (int32_T)((int32_T)((int32_T)(12 * kase) + q) + 1); - hlfkiecjohlfopph_xaxpy((int32_T)(12 - q), -(jekncbaiknglnoph_xdotc - ((int32_T)(12 - q), U, (int32_T)(i + 1), U, qs) / U[i]), (int32_T)(i + - 1), U, qs); - } - - for (qs = q; (int32_T)(qs + 1) < 13; qs++) { - U[(int32_T)(qs + (int32_T)(12 * q))] = -U[(int32_T)((int32_T)(12 * q) + - qs)]; - } - - U[i]++; - for (i = 1; i <= q; i++) { - U[(int32_T)((int32_T)(i + (int32_T)(12 * q)) - 1)] = 0.0F; - } - } else { - for (qs = 0; qs < 12; qs++) { - U[(int32_T)(qs + (int32_T)(12 * q))] = 0.0F; - } - - U[i] = 1.0F; - } - } - - for (i = 2; i >= 0; i += -1) { - if (((int32_T)(i + 1) <= 1) && (e[0] != 0.0F)) { - for (q = 2; q < 4; q++) { - qs = (int32_T)((int32_T)((int32_T)(q - 1) * 3) + 2); - ekfklfkfohdjaimg_xaxpy(2, -(mglnohlnekfcekfk_xdotc(2, Vf, 2, Vf, qs) / - Vf[1]), 2, Vf, qs); - } - } - - Vf[(int32_T)(3 * i)] = 0.0F; - Vf[(int32_T)(1 + (int32_T)(3 * i))] = 0.0F; - Vf[(int32_T)(2 + (int32_T)(3 * i))] = 0.0F; - Vf[(int32_T)(i + (int32_T)(3 * i))] = 1.0F; - } - - ztest = e[0]; - if (s[0] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[0]); - snorm = s[0] / ztest0; - s[0] = ztest0; - ztest = e[0] / snorm; - cjmgkfkfphdjaaai_xscal(snorm, U, 1); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[1] *= snorm; - ophlaaaiophdbaai_xscal(snorm, Vf, 4); - } - - e[0] = ztest; - ztest = b_A[25]; - if (s[1] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[1]); - snorm = s[1] / ztest0; - s[1] = ztest0; - ztest = b_A[25] / snorm; - cjmgkfkfphdjaaai_xscal(snorm, U, 13); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - snorm = ztest0 / ztest; - ztest = ztest0; - s[2] *= snorm; - ophlaaaiophdbaai_xscal(snorm, Vf, 7); - } - - e[1] = ztest; - if (s[2] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[2]); - snorm = s[2] / ztest0; - s[2] = ztest0; - cjmgkfkfphdjaaai_xscal(snorm, U, 25); - } - - e[2] = 0.0F; - i = 0; - snorm = 0.0F; - if ((s[0] >= e[0]) || rtIsNaNF(e[0])) { - ztest0 = s[0]; - } else { - ztest0 = e[0]; - } - - if (!((0.0F >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - if ((s[1] >= ztest) || rtIsNaNF(ztest)) { - ztest = s[1]; - } - - if (!((snorm >= ztest) || rtIsNaNF(ztest))) { - snorm = ztest; - } - - if (s[2] >= 0.0F) { - ztest0 = s[2]; - } else { - ztest0 = 0.0F; - } - - if (!((snorm >= ztest0) || rtIsNaNF(ztest0))) { - snorm = ztest0; - } - - while (((int32_T)(m + 2) > 0) && (!(i >= 75))) { - kase = (int32_T)(m + 1); - do { - exitg3 = 0; - q = kase; - if (kase == 0) { - exitg3 = 1; - } else { - ztest0 = (real32_T)fabs((real_T)e[(int32_T)(kase - 1)]); - if ((ztest0 <= ((real32_T)fabs((real_T)s[(int32_T)(kase - 1)]) + - (real32_T)fabs((real_T)s[kase])) * 1.1920929E-7F) || - ((ztest0 <= 9.86076132E-32F) || ((i > 20) && (ztest0 <= - 1.1920929E-7F * snorm)))) { - e[(int32_T)(kase - 1)] = 0.0F; - exitg3 = 1; - } else { - kase--; - } - } - } while (exitg3 == 0); - - if ((int32_T)(m + 1) == kase) { - kase = 4; - } else { - qs = (int32_T)(m + 2); - j_ii = (int32_T)(m + 2); - exitg2 = false; - while ((!exitg2) && (j_ii >= kase)) { - qs = j_ii; - if (j_ii == kase) { - exitg2 = true; - } else { - ztest0 = 0.0F; - if (j_ii < (int32_T)(m + 2)) { - ztest0 = (real32_T)fabs((real_T)e[(int32_T)(j_ii - 1)]); - } - - if (j_ii > (int32_T)(kase + 1)) { - ztest0 += (real32_T)fabs((real_T)e[(int32_T)(j_ii - 2)]); - } - - ztest = (real32_T)fabs((real_T)s[(int32_T)(j_ii - 1)]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s[(int32_T)(j_ii - 1)] = 0.0F; - exitg2 = true; - } else { - j_ii--; - } - } - } - - if (qs == kase) { - kase = 3; - } else if ((int32_T)(m + 2) == qs) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[m]; - e[m] = 0.0F; - for (qs = m; (int32_T)(qs + 1) >= (int32_T)(q + 1); qs--) { - ztest = s[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[qs] = ztest; - if ((int32_T)(qs + 1) > (int32_T)(q + 1)) { - ztest0 = -smm1 * e[0]; - e[0] *= sqds; - } - - gdjmmgdjekfccjmg_xrot(Vf, (int32_T)(1 + (int32_T)(3 * qs)), (int32_T)(1 - + (int32_T)(3 * (int32_T)(m + 1))), sqds, smm1); - } - break; - - case 2: - ztest0 = e[(int32_T)(q - 1)]; - e[(int32_T)(q - 1)] = 0.0F; - for (qs = q; (int32_T)(qs + 1) <= (int32_T)(m + 2); qs++) { - ztest = s[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[qs] = ztest; - ztest0 = -smm1 * e[qs]; - e[qs] *= sqds; - ophdimgdcjekekfc_xrot(U, (int32_T)(1 + (int32_T)(12 * qs)), (int32_T)(1 - + (int32_T)(12 * (int32_T)(q - 1))), sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s[(int32_T)(m + 1)]); - varargin_1[1] = (real32_T)fabs((real_T)s[m]); - varargin_1[2] = (real32_T)fabs((real_T)e[m]); - varargin_1[3] = (real32_T)fabs((real_T)s[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - qs = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - kase = 2; - exitg1 = false; - while ((!exitg1) && (kase < 6)) { - qs = kase; - if (!rtIsNaNF(varargin_1[(int32_T)(kase - 1)])) { - ztest = varargin_1[(int32_T)(kase - 1)]; - exitg1 = true; - } else { - kase++; - } - } - } - - if (qs < 5) { - while ((int32_T)(qs + 1) < 6) { - if (varargin_1[qs] > ztest) { - ztest = varargin_1[qs]; - } - - qs++; - } - } - - ztest0 = s[(int32_T)(m + 1)] / ztest; - smm1 = s[m] / ztest; - emm1 = e[m] / ztest; - sqds = s[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - for (qs = (int32_T)(q + 1); qs <= (int32_T)(m + 1); qs++) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - if (qs > (int32_T)(q + 1)) { - e[0] = ztest0; - } - - ztest0 = s[(int32_T)(qs - 1)] * sqds + e[(int32_T)(qs - 1)] * smm1; - e[(int32_T)(qs - 1)] = e[(int32_T)(qs - 1)] * sqds - s[(int32_T)(qs - 1)] - * smm1; - ztest = smm1 * s[qs]; - s[qs] *= sqds; - gdjmmgdjekfccjmg_xrot(Vf, (int32_T)(1 + (int32_T)(3 * (int32_T)(qs - 1))), - (int32_T)(1 + (int32_T)(3 * qs)), sqds, smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s[(int32_T)(qs - 1)] = ztest0; - ztest0 = e[(int32_T)(qs - 1)] * sqds + smm1 * s[qs]; - s[qs] = e[(int32_T)(qs - 1)] * -smm1 + sqds * s[qs]; - ztest = smm1 * e[qs]; - e[qs] *= sqds; - ophdimgdcjekekfc_xrot(U, (int32_T)(1 + (int32_T)(12 * (int32_T)(qs - 1))), - (int32_T)(1 + (int32_T)(12 * qs)), sqds, smm1); - } - - e[m] = ztest0; - i++; - break; - - default: - if (s[q] < 0.0F) { - s[q] = -s[q]; - ophlaaaiophdbaai_xscal(-1.0F, Vf, (int32_T)(1 + (int32_T)(3 * q))); - } - - i = (int32_T)(q + 1); - while (((int32_T)(q + 1) < 3) && (s[q] < s[i])) { - ztest0 = s[q]; - s[q] = s[i]; - s[i] = ztest0; - djmgbimooppphlno_xswap(Vf, (int32_T)(1 + (int32_T)(3 * q)), (int32_T)(1 - + (int32_T)(3 * (int32_T)(q + 1)))); - eknofcjenophiecj_xswap(U, (int32_T)(1 + (int32_T)(12 * q)), (int32_T)(1 - + (int32_T)(12 * (int32_T)(q + 1)))); - q = i; - i++; - } - - i = 0; - m--; - break; - } - } - - for (m = 0; m < 3; m++) { - e[m] = s[m]; - V[(int32_T)(3 * m)] = Vf[(int32_T)(3 * m)]; - V[(int32_T)(1 + (int32_T)(3 * m))] = Vf[(int32_T)((int32_T)(3 * m) + 1)]; - V[(int32_T)(2 + (int32_T)(3 * m))] = Vf[(int32_T)((int32_T)(3 * m) + 2)]; - } - - for (i = 0; i < 9; i++) { - S[i] = 0.0F; - } - - S[0] = e[0]; - S[4] = e[1]; - S[8] = e[2]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/lnohcjmoimglcbai_svd.h b/gnc/matlab/code_generation/sharedutils/lnohcjmoimglcbai_svd.h deleted file mode 100644 index b89b3b7178..0000000000 --- a/gnc/matlab/code_generation/sharedutils/lnohcjmoimglcbai_svd.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: lnohcjmoimglcbai_svd.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_lnohcjmoimglcbai_svd -#define SHARE_lnohcjmoimglcbai_svd -#include "rtwtypes.h" - -extern void lnohcjmoimglcbai_svd(const real32_T A[36], real32_T U[36], real32_T - S[9], real32_T V[9]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/look1_iflf_binlxpw.cpp b/gnc/matlab/code_generation/sharedutils/look1_iflf_binlxpw.cpp deleted file mode 100644 index 01a1a21747..0000000000 --- a/gnc/matlab/code_generation/sharedutils/look1_iflf_binlxpw.cpp +++ /dev/null @@ -1,73 +0,0 @@ -// -// File: look1_iflf_binlxpw.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "look1_iflf_binlxpw.h" - -real32_T look1_iflf_binlxpw(real32_T u0, const real32_T bp0[], const real32_T - table[], uint32_T maxIndex) -{ - real32_T frac; - uint32_T iRght; - uint32_T iLeft; - uint32_T bpIdx; - - // Lookup 1-D - // Search method: 'binary' - // Use previous index: 'off' - // Interpolation method: 'Linear' - // Extrapolation method: 'Linear' - // Use last breakpoint for index at or above upper limit: 'off' - // Remove protection against out-of-range input in generated code: 'off' - - // Prelookup - Index and Fraction - // Index Search method: 'binary' - // Extrapolation method: 'Linear' - // Use previous index: 'off' - // Use last breakpoint for index at or above upper limit: 'off' - // Remove protection against out-of-range input in generated code: 'off' - - if (u0 <= bp0[0U]) { - iLeft = 0U; - frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]); - } else if (u0 < bp0[maxIndex]) { - // Binary Search - bpIdx = (uint32_T)(maxIndex >> 1U); - iLeft = 0U; - iRght = maxIndex; - while ((uint32_T)(iRght - iLeft) > 1U) { - if (u0 < bp0[bpIdx]) { - iRght = bpIdx; - } else { - iLeft = bpIdx; - } - - bpIdx = (uint32_T)((uint32_T)(iRght + iLeft) >> 1U); - } - - frac = (u0 - bp0[iLeft]) / (bp0[(uint32_T)(iLeft + 1U)] - bp0[iLeft]); - } else { - iLeft = (uint32_T)(maxIndex - 1U); - frac = (u0 - bp0[(uint32_T)(maxIndex - 1U)]) / (bp0[maxIndex] - bp0 - [(uint32_T)(maxIndex - 1U)]); - } - - // Interpolation 1-D - // Interpolation method: 'Linear' - // Use last breakpoint for index at or above upper limit: 'off' - // Overflow mode: 'portable wrapping' - - return (table[(uint32_T)(iLeft + 1U)] - table[iLeft]) * frac + table[iLeft]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/look1_iflf_binlxpw.h b/gnc/matlab/code_generation/sharedutils/look1_iflf_binlxpw.h deleted file mode 100644 index 82cea07561..0000000000 --- a/gnc/matlab/code_generation/sharedutils/look1_iflf_binlxpw.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: look1_iflf_binlxpw.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_look1_iflf_binlxpw -#define SHARE_look1_iflf_binlxpw -#include "rtwtypes.h" - -extern real32_T look1_iflf_binlxpw(real32_T u0, const real32_T bp0[], const - real32_T table[], uint32_T maxIndex); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlcapw.cpp b/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlcapw.cpp deleted file mode 100644 index 35aba48d99..0000000000 --- a/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlcapw.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -// File: look1_iflf_pbinlcapw.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "look1_iflf_pbinlcapw.h" - -real32_T look1_iflf_pbinlcapw(real32_T u0, const real32_T bp0[], const real32_T - table[], uint32_T prevIndex[], uint32_T maxIndex) -{ - real32_T y; - real32_T frac; - uint32_T iRght; - uint32_T iLeft; - uint32_T bpIdx; - uint32_T found; - - // Lookup 1-D - // Search method: 'binary' - // Use previous index: 'on' - // Interpolation method: 'Linear' - // Extrapolation method: 'Clip' - // Use last breakpoint for index at or above upper limit: 'on' - // Remove protection against out-of-range input in generated code: 'off' - - // Prelookup - Index and Fraction - // Index Search method: 'binary' - // Extrapolation method: 'Clip' - // Use previous index: 'on' - // Use last breakpoint for index at or above upper limit: 'on' - // Remove protection against out-of-range input in generated code: 'off' - - if (u0 <= bp0[0U]) { - bpIdx = 0U; - frac = 0.0F; - } else if (u0 < bp0[maxIndex]) { - // Binary Search using Previous Index - bpIdx = prevIndex[0U]; - iLeft = 0U; - iRght = maxIndex; - found = 0U; - while (found == 0U) { - if (u0 < bp0[bpIdx]) { - iRght = (uint32_T)(bpIdx - 1U); - bpIdx = (uint32_T)((uint32_T)(iRght + iLeft) >> 1U); - } else if (u0 < bp0[(uint32_T)(bpIdx + 1U)]) { - found = 1U; - } else { - iLeft = (uint32_T)(bpIdx + 1U); - bpIdx = (uint32_T)((uint32_T)(iRght + iLeft) >> 1U); - } - } - - frac = (u0 - bp0[bpIdx]) / (bp0[(uint32_T)(bpIdx + 1U)] - bp0[bpIdx]); - } else { - bpIdx = maxIndex; - frac = 0.0F; - } - - prevIndex[0U] = bpIdx; - - // Interpolation 1-D - // Interpolation method: 'Linear' - // Use last breakpoint for index at or above upper limit: 'on' - // Overflow mode: 'portable wrapping' - - if (bpIdx == maxIndex) { - y = table[bpIdx]; - } else { - y = (table[(uint32_T)(bpIdx + 1U)] - table[bpIdx]) * frac + table[bpIdx]; - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlcapw.h b/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlcapw.h deleted file mode 100644 index 35bdc551e4..0000000000 --- a/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlcapw.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: look1_iflf_pbinlcapw.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_look1_iflf_pbinlcapw -#define SHARE_look1_iflf_pbinlcapw -#include "rtwtypes.h" - -extern real32_T look1_iflf_pbinlcapw(real32_T u0, const real32_T bp0[], const - real32_T table[], uint32_T prevIndex[], uint32_T maxIndex); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlxpw.cpp b/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlxpw.cpp deleted file mode 100644 index d8fc12e3ee..0000000000 --- a/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlxpw.cpp +++ /dev/null @@ -1,79 +0,0 @@ -// -// File: look1_iflf_pbinlxpw.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1051 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Fri Dec 2 10:48:20 2016 -// -#include "rtwtypes.h" -#include "look1_iflf_pbinlxpw.h" - -real32_T look1_iflf_pbinlxpw(real32_T u0, const real32_T bp0[], const real32_T - table[], uint32_T prevIndex[], uint32_T maxIndex) -{ - real32_T frac; - uint32_T startIndex; - uint32_T iRght; - uint32_T iLeft; - uint32_T found; - - // Lookup 1-D - // Search method: 'binary' - // Use previous index: 'on' - // Interpolation method: 'Linear' - // Extrapolation method: 'Linear' - // Use last breakpoint for index at or above upper limit: 'off' - // Remove protection against out-of-range input in generated code: 'off' - - // Prelookup - Index and Fraction - // Index Search method: 'binary' - // Extrapolation method: 'Linear' - // Use previous index: 'on' - // Use last breakpoint for index at or above upper limit: 'off' - // Remove protection against out-of-range input in generated code: 'off' - - if (u0 <= bp0[0U]) { - startIndex = 0U; - frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]); - } else if (u0 < bp0[maxIndex]) { - startIndex = prevIndex[0U]; - - // Binary Search using Previous Index - iLeft = 0U; - iRght = maxIndex; - found = 0U; - while (found == 0U) { - if (u0 < bp0[startIndex]) { - iRght = startIndex - 1U; - startIndex = (iRght + iLeft) >> 1U; - } else if (u0 < bp0[startIndex + 1U]) { - found = 1U; - } else { - iLeft = startIndex + 1U; - startIndex = (iRght + iLeft) >> 1U; - } - } - - frac = (u0 - bp0[startIndex]) / (bp0[startIndex + 1U] - bp0[startIndex]); - } else { - startIndex = maxIndex - 1U; - frac = (u0 - bp0[maxIndex - 1U]) / (bp0[maxIndex] - bp0[maxIndex - 1U]); - } - - prevIndex[0U] = startIndex; - - // Interpolation 1-D - // Interpolation method: 'Linear' - // Use last breakpoint for index at or above upper limit: 'off' - // Overflow mode: 'portable wrapping' - - return (table[startIndex + 1U] - table[startIndex]) * frac + table[startIndex]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlxpw.h b/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlxpw.h deleted file mode 100644 index ad5effca6f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/look1_iflf_pbinlxpw.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: look1_iflf_pbinlxpw.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1051 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Fri Dec 2 10:48:20 2016 -// -#ifndef SHARE_look1_iflf_pbinlxpw -#define SHARE_look1_iflf_pbinlxpw -#include "rtwtypes.h" - -extern real32_T look1_iflf_pbinlxpw(real32_T u0, const real32_T bp0[], const - real32_T table[], uint32_T prevIndex[], uint32_T maxIndex); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/look2_iflf_pbinlxpw.cpp b/gnc/matlab/code_generation/sharedutils/look2_iflf_pbinlxpw.cpp deleted file mode 100644 index 096cd919f6..0000000000 --- a/gnc/matlab/code_generation/sharedutils/look2_iflf_pbinlxpw.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// -// File: look2_iflf_pbinlxpw.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1051 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Fri Dec 2 10:48:20 2016 -// -#include "rtwtypes.h" -#include "look2_iflf_pbinlxpw.h" - -real32_T look2_iflf_pbinlxpw(real32_T u0, real32_T u1, const real32_T bp0[], - const real32_T bp1[], const real32_T table[], uint32_T prevIndex[], const - uint32_T maxIndex[], uint32_T stride) -{ - real32_T frac; - uint32_T bpIndices[2]; - real32_T fractions[2]; - uint32_T startIndex; - real32_T yL_1d; - uint32_T iRght; - uint32_T iLeft; - uint32_T found; - - // Lookup 2-D - // Search method: 'binary' - // Use previous index: 'on' - // Interpolation method: 'Linear' - // Extrapolation method: 'Linear' - // Use last breakpoint for index at or above upper limit: 'off' - // Remove protection against out-of-range input in generated code: 'off' - - // Prelookup - Index and Fraction - // Index Search method: 'binary' - // Extrapolation method: 'Linear' - // Use previous index: 'on' - // Use last breakpoint for index at or above upper limit: 'off' - // Remove protection against out-of-range input in generated code: 'off' - - if (u0 <= bp0[0U]) { - startIndex = 0U; - frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]); - } else if (u0 < bp0[maxIndex[0U]]) { - startIndex = prevIndex[0U]; - - // Binary Search using Previous Index - iLeft = 0U; - iRght = maxIndex[0U]; - found = 0U; - while (found == 0U) { - if (u0 < bp0[startIndex]) { - iRght = startIndex - 1U; - startIndex = (iRght + iLeft) >> 1U; - } else if (u0 < bp0[startIndex + 1U]) { - found = 1U; - } else { - iLeft = startIndex + 1U; - startIndex = (iRght + iLeft) >> 1U; - } - } - - frac = (u0 - bp0[startIndex]) / (bp0[startIndex + 1U] - bp0[startIndex]); - } else { - startIndex = maxIndex[0U] - 1U; - frac = (u0 - bp0[maxIndex[0U] - 1U]) / (bp0[maxIndex[0U]] - bp0[maxIndex[0U] - - 1U]); - } - - prevIndex[0U] = startIndex; - fractions[0U] = frac; - bpIndices[0U] = startIndex; - - // Prelookup - Index and Fraction - // Index Search method: 'binary' - // Extrapolation method: 'Linear' - // Use previous index: 'on' - // Use last breakpoint for index at or above upper limit: 'off' - // Remove protection against out-of-range input in generated code: 'off' - - if (u1 <= bp1[0U]) { - startIndex = 0U; - frac = (u1 - bp1[0U]) / (bp1[1U] - bp1[0U]); - } else if (u1 < bp1[maxIndex[1U]]) { - startIndex = prevIndex[1U]; - - // Binary Search using Previous Index - iLeft = 0U; - iRght = maxIndex[1U]; - found = 0U; - while (found == 0U) { - if (u1 < bp1[startIndex]) { - iRght = startIndex - 1U; - startIndex = (iRght + iLeft) >> 1U; - } else if (u1 < bp1[startIndex + 1U]) { - found = 1U; - } else { - iLeft = startIndex + 1U; - startIndex = (iRght + iLeft) >> 1U; - } - } - - frac = (u1 - bp1[startIndex]) / (bp1[startIndex + 1U] - bp1[startIndex]); - } else { - startIndex = maxIndex[1U] - 1U; - frac = (u1 - bp1[maxIndex[1U] - 1U]) / (bp1[maxIndex[1U]] - bp1[maxIndex[1U] - - 1U]); - } - - prevIndex[1U] = startIndex; - - // Interpolation 2-D - // Interpolation method: 'Linear' - // Use last breakpoint for index at or above upper limit: 'off' - // Overflow mode: 'portable wrapping' - - iLeft = startIndex * stride + bpIndices[0U]; - yL_1d = (table[iLeft + 1U] - table[iLeft]) * fractions[0U] + table[iLeft]; - iLeft += stride; - return (((table[iLeft + 1U] - table[iLeft]) * fractions[0U] + table[iLeft]) - - yL_1d) * frac + yL_1d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/look2_iflf_pbinlxpw.h b/gnc/matlab/code_generation/sharedutils/look2_iflf_pbinlxpw.h deleted file mode 100644 index 034a4915f3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/look2_iflf_pbinlxpw.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: look2_iflf_pbinlxpw.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1051 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Fri Dec 2 10:48:20 2016 -// -#ifndef SHARE_look2_iflf_pbinlxpw -#define SHARE_look2_iflf_pbinlxpw -#include "rtwtypes.h" - -extern real32_T look2_iflf_pbinlxpw(real32_T u0, real32_T u1, const real32_T - bp0[], const real32_T bp1[], const real32_T table[], uint32_T prevIndex[], - const uint32_T maxIndex[], uint32_T stride); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdbbiekfknonglf_nullAssignment.cpp b/gnc/matlab/code_generation/sharedutils/mgdbbiekfknonglf_nullAssignment.cpp deleted file mode 100644 index baea85e505..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdbbiekfknonglf_nullAssignment.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// -// File: mgdbbiekfknonglf_nullAssignment.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "mgdbbiekfknonglf_nullAssignment.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void mgdbbiekfknonglf_nullAssignment(real32_T x_data[], int32_T *x_sizes, const - boolean_T idx_data[], const int32_T idx_sizes) -{ - int32_T nxin; - int32_T nxout; - int32_T k0; - int32_T k; - real32_T x_data_0[100]; - nxin = *x_sizes; - nxout = 0; - for (k0 = 1; k0 <= idx_sizes; k0++) { - nxout += (int32_T)idx_data[(int32_T)(k0 - 1)]; - } - - nxout = (int32_T)(*x_sizes - nxout); - k0 = -1; - for (k = 1; k <= nxin; k++) { - if ((k > idx_sizes) || (!idx_data[(int32_T)(k - 1)])) { - k0++; - x_data[k0] = x_data[(int32_T)(k - 1)]; - } - } - - if (1 > nxout) { - nxout = 0; - } - - for (k0 = 0; k0 <= (int32_T)(nxout - 1); k0++) { - x_data_0[k0] = x_data[k0]; - } - - *x_sizes = nxout; - for (k0 = 0; k0 <= (int32_T)(nxout - 1); k0++) { - x_data[k0] = x_data_0[k0]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdbbiekfknonglf_nullAssignment.h b/gnc/matlab/code_generation/sharedutils/mgdbbiekfknonglf_nullAssignment.h deleted file mode 100644 index 826567d110..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdbbiekfknonglf_nullAssignment.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mgdbbiekfknonglf_nullAssignment.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_mgdbbiekfknonglf_nullAssignment -#define SHARE_mgdbbiekfknonglf_nullAssignment -#include "rtwtypes.h" - -extern void mgdbbiekfknonglf_nullAssignment(real32_T x_data[], int32_T *x_sizes, - const boolean_T idx_data[], const int32_T idx_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdbglfkphdbjecb_quaternion_to_rotation.cpp b/gnc/matlab/code_generation/sharedutils/mgdbglfkphdbjecb_quaternion_to_rotation.cpp deleted file mode 100644 index 8e8f643098..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdbglfkphdbjecb_quaternion_to_rotation.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// -// File: mgdbglfkphdbjecb_quaternion_to_rotation.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#include "rtwtypes.h" -#include "mgdbglfkphdbjecb_quaternion_to_rotation.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void mgdbglfkphdbjecb_quaternion_to_rotation(const real32_T q[4], real32_T R[9]) -{ - real32_T S[9]; - static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - real32_T q_0; - real32_T q_1; - real32_T q_2[12]; - real32_T q_3[12]; - int32_T i; - int32_T i_0; - - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // - // 8/11/2015 - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // - // 8/11/2015 - S[0] = 0.0F; - S[3] = -q[2]; - S[6] = q[1]; - S[1] = q[2]; - S[4] = 0.0F; - S[7] = -q[0]; - S[2] = -q[1]; - S[5] = q[0]; - S[8] = 0.0F; - q_0 = q[3]; - q_1 = q[3]; - for (i = 0; i < 3; i++) { - q_2[(int32_T)(3 * i)] = q_0 * (real32_T)b[i] + S[i]; - q_3[(int32_T)(i << 2)] = (real32_T)b[(int32_T)(3 * i)] * q_1 - S[(int32_T)(3 - * i)]; - q_2[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b[(int32_T)(i + 3)] * q_0 + - S[(int32_T)(i + 3)]; - q_3[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b[(int32_T)((int32_T)(3 * - i) + 1)] * q_1 - S[(int32_T)((int32_T)(3 * i) + 1)]; - q_2[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b[(int32_T)(i + 6)] * q_0 + - S[(int32_T)(i + 6)]; - q_3[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b[(int32_T)((int32_T)(3 * - i) + 2)] * q_1 - S[(int32_T)((int32_T)(3 * i) + 2)]; - q_2[(int32_T)(9 + i)] = -q[i]; - q_3[(int32_T)(3 + (int32_T)(i << 2))] = -q[i]; - } - - for (i = 0; i < 3; i++) { - for (i_0 = 0; i_0 < 3; i_0++) { - R[(int32_T)(i_0 + (int32_T)(3 * i))] = 0.0F; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)(i << 2)] * q_2[i_0]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 1)] * q_2[(int32_T)(i_0 + 3)]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 2)] * q_2[(int32_T)(i_0 + 6)]; - R[(int32_T)(i_0 + (int32_T)(3 * i))] += q_3[(int32_T)((int32_T)(i << 2) + - 3)] * q_2[(int32_T)(i_0 + 9)]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdbglfkphdbjecb_quaternion_to_rotation.h b/gnc/matlab/code_generation/sharedutils/mgdbglfkphdbjecb_quaternion_to_rotation.h deleted file mode 100644 index e239f788a1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdbglfkphdbjecb_quaternion_to_rotation.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mgdbglfkphdbjecb_quaternion_to_rotation.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#ifndef SHARE_mgdbglfkphdbjecb_quaternion_to_rotation -#define SHARE_mgdbglfkphdbjecb_quaternion_to_rotation -#include "rtwtypes.h" - -extern void mgdbglfkphdbjecb_quaternion_to_rotation(const real32_T q[4], - real32_T R[9]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjbieccjechlng_xgeqrf.cpp b/gnc/matlab/code_generation/sharedutils/mgdjbieccjechlng_xgeqrf.cpp deleted file mode 100644 index 0383b3a138..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjbieccjechlng_xgeqrf.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// -// File: mgdjbieccjechlng_xgeqrf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ecbilfcbnglnimoh_xzlarfg.h" -#include "gdjmaiekcbaiecjm_xzlarf.h" -#include "mgdjbieccjechlng_xgeqrf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void mgdjbieccjechlng_xgeqrf(real32_T A_data[], int32_T A_sizes[2], real32_T - tau_data[], int32_T *tau_sizes) -{ - int32_T m; - int32_T mn; - real32_T work[6]; - int32_T i_i; - int32_T mmi; - real32_T b; - real32_T b_atmp; - int32_T i; - m = A_sizes[0]; - mn = A_sizes[0]; - if (!(mn <= 6)) { - mn = 6; - } - - *tau_sizes = (int32_T)(int8_T)mn; - if (A_sizes[0] != 0) { - for (i = 0; i < 6; i++) { - work[i] = 0.0F; - } - - for (i = 0; (int32_T)(i + 1) <= mn; i++) { - i_i = (int32_T)((int32_T)(i * m) + i); - mmi = (int32_T)(m - i); - if ((int32_T)(i + 1) < m) { - b_atmp = A_data[i_i]; - b = ecbilfcbnglnimoh_xzlarfg(mmi, &b_atmp, A_data, (int32_T)(i_i + 2)); - tau_data[i] = b; - A_data[i_i] = b_atmp; - } else { - tau_data[i] = 0.0F; - } - - if ((int32_T)(i + 1) < 6) { - b_atmp = A_data[i_i]; - A_data[i_i] = 1.0F; - gdjmaiekcbaiecjm_xzlarf(mmi, (int32_T)(5 - i), (int32_T)(i_i + 1), - tau_data[i], A_data, (int32_T)((int32_T)(i + (int32_T)((int32_T)(i + 1) - * m)) + 1), m, work); - A_data[i_i] = b_atmp; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjbieccjechlng_xgeqrf.h b/gnc/matlab/code_generation/sharedutils/mgdjbieccjechlng_xgeqrf.h deleted file mode 100644 index 355d9a1057..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjbieccjechlng_xgeqrf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mgdjbieccjechlng_xgeqrf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_mgdjbieccjechlng_xgeqrf -#define SHARE_mgdjbieccjechlng_xgeqrf -#include "rtwtypes.h" - -extern void mgdjbieccjechlng_xgeqrf(real32_T A_data[], int32_T A_sizes[2], - real32_T tau_data[], int32_T *tau_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjglnoimohglno_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/mgdjglnoimohglno_xaxpy.cpp deleted file mode 100644 index 511140d1c2..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjglnoimohglno_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: mgdjglnoimohglno_xaxpy.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "mgdjglnoimohglno_xaxpy.h" - -// Function for MATLAB Function: '/MATLAB Function' -void mgdjglnoimohglno_xaxpy(int32_T n, real32_T a, const real32_T x[72], int32_T - ix0, real32_T y[12], int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * x[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjglnoimohglno_xaxpy.h b/gnc/matlab/code_generation/sharedutils/mgdjglnoimohglno_xaxpy.h deleted file mode 100644 index 041c550ab4..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjglnoimohglno_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mgdjglnoimohglno_xaxpy.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_mgdjglnoimohglno_xaxpy -#define SHARE_mgdjglnoimohglno_xaxpy -#include "rtwtypes.h" - -extern void mgdjglnoimohglno_xaxpy(int32_T n, real32_T a, const real32_T x[72], - int32_T ix0, real32_T y[12], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjimgdopphcjek_skip_to_last_equal_value.cpp b/gnc/matlab/code_generation/sharedutils/mgdjimgdopphcjek_skip_to_last_equal_value.cpp deleted file mode 100644 index 74f62078f9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjimgdopphcjek_skip_to_last_equal_value.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// -// File: mgdjimgdopphcjek_skip_to_last_equal_value.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "mgdjimgdopphcjek_skip_to_last_equal_value.h" - -// Function for MATLAB Function: '/generate_output' -real32_T mgdjimgdopphcjek_skip_to_last_equal_value(int32_T *k, const real32_T - x_data[], const int32_T x_sizes) -{ - real32_T xk; - boolean_T p; - real32_T absxk; - int32_T exponent; - boolean_T exitg1; - xk = x_data[(int32_T)(*k - 1)]; - exitg1 = false; - while ((!exitg1) && (*k < x_sizes)) { - absxk = (real32_T)fabs((real_T)(xk / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, (int32_T)(exponent - 24)); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(xk - x_data[*k])) < absxk) || (rtIsInfF(x_data[* - k]) && rtIsInfF(xk) && ((x_data[*k] > 0.0F) == (xk > 0.0F)))) { - p = true; - } else { - p = false; - } - - if (p) { - (*k)++; - } else { - exitg1 = true; - } - } - - return xk; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjimgdopphcjek_skip_to_last_equal_value.h b/gnc/matlab/code_generation/sharedutils/mgdjimgdopphcjek_skip_to_last_equal_value.h deleted file mode 100644 index 46a6e9b898..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjimgdopphcjek_skip_to_last_equal_value.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mgdjimgdopphcjek_skip_to_last_equal_value.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_mgdjimgdopphcjek_skip_to_last_equal_value -#define SHARE_mgdjimgdopphcjek_skip_to_last_equal_value -#include "rtwtypes.h" - -extern real32_T mgdjimgdopphcjek_skip_to_last_equal_value(int32_T *k, const - real32_T x_data[], const int32_T x_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjjekffknghlfk_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/mgdjjekffknghlfk_xzlarf.cpp deleted file mode 100644 index 92d0fad873..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjjekffknghlfk_xzlarf.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// -// File: mgdjjekffknghlfk_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "mgdjjekffknghlfk_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void mgdjjekffknghlfk_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]) -{ - int32_T lastv; - int32_T lastc; - int32_T coltop; - int32_T ix; - real32_T c; - int32_T iac; - int32_T d; - int32_T b_ia; - int32_T jy; - int32_T exitg1; - boolean_T exitg2; - if (tau != 0.0F) { - lastv = m; - lastc = (int32_T)(iv0 + m); - while ((lastv > 0) && (C_data[(int32_T)(lastc - 2)] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = n; - exitg2 = false; - while ((!exitg2) && (lastc > 0)) { - coltop = (int32_T)((int32_T)((int32_T)(lastc - 1) * ldc) + ic0); - jy = coltop; - do { - exitg1 = 0; - if (jy <= (int32_T)((int32_T)(coltop + lastv) - 1)) { - if (C_data[(int32_T)(jy - 1)] != 0.0F) { - exitg1 = 1; - } else { - jy++; - } - } else { - lastc--; - exitg1 = 2; - } - } while (exitg1 == 0); - - if (exitg1 == 1) { - exitg2 = true; - } - } - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - if (lastc != 0) { - for (coltop = 1; coltop <= lastc; coltop++) { - work[(int32_T)(coltop - 1)] = 0.0F; - } - - coltop = 0; - jy = (int32_T)((int32_T)((int32_T)(lastc - 1) * ldc) + ic0); - iac = ic0; - while ((ldc > 0) && (iac <= jy)) { - ix = iv0; - c = 0.0F; - d = (int32_T)((int32_T)(iac + lastv) - 1); - for (b_ia = iac; b_ia <= d; b_ia++) { - c += C_data[(int32_T)(b_ia - 1)] * C_data[(int32_T)(ix - 1)]; - ix++; - } - - work[coltop] += c; - coltop++; - iac += ldc; - } - } - - if (!(-tau == 0.0F)) { - coltop = (int32_T)(ic0 - 1); - jy = 0; - for (iac = 1; iac <= lastc; iac++) { - if (work[jy] != 0.0F) { - c = work[jy] * -tau; - ix = iv0; - d = (int32_T)(lastv + coltop); - for (b_ia = coltop; (int32_T)(b_ia + 1) <= d; b_ia++) { - C_data[b_ia] += C_data[(int32_T)(ix - 1)] * c; - ix++; - } - } - - jy++; - coltop += ldc; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjjekffknghlfk_xzlarf.h b/gnc/matlab/code_generation/sharedutils/mgdjjekffknghlfk_xzlarf.h deleted file mode 100644 index 6ff0b9fe36..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjjekffknghlfk_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mgdjjekffknghlfk_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_mgdjjekffknghlfk_xzlarf -#define SHARE_mgdjjekffknghlfk_xzlarf -#include "rtwtypes.h" - -extern void mgdjjekffknghlfk_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjohdjcjmglfcj_ilazlc.cpp b/gnc/matlab/code_generation/sharedutils/mgdjohdjcjmglfcj_ilazlc.cpp deleted file mode 100644 index 0166982497..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjohdjcjmglfcj_ilazlc.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: mgdjohdjcjmglfcj_ilazlc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "mgdjohdjcjmglfcj_ilazlc.h" - -// Function for MATLAB Function: '/Compute Residual and H' -int32_T mgdjohdjcjmglfcj_ilazlc(int32_T m, int32_T n, const real32_T A_data[], - int32_T ia0, int32_T lda) -{ - int32_T j; - int32_T coltop; - int32_T ia; - int32_T exitg1; - boolean_T exitg2; - j = n; - exitg2 = false; - while ((!exitg2) && (j > 0)) { - coltop = (j - 1) * lda + ia0; - ia = coltop; - do { - exitg1 = 0; - if (ia <= (coltop + m) - 1) { - if (A_data[ia - 1] != 0.0F) { - exitg1 = 1; - } else { - ia++; - } - } else { - j--; - exitg1 = 2; - } - } while (exitg1 == 0); - - if (exitg1 == 1) { - exitg2 = true; - } - } - - return j; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mgdjohdjcjmglfcj_ilazlc.h b/gnc/matlab/code_generation/sharedutils/mgdjohdjcjmglfcj_ilazlc.h deleted file mode 100644 index 834e1bb000..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mgdjohdjcjmglfcj_ilazlc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mgdjohdjcjmglfcj_ilazlc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_mgdjohdjcjmglfcj_ilazlc -#define SHARE_mgdjohdjcjmglfcj_ilazlc -#include "rtwtypes.h" - -extern int32_T mgdjohdjcjmglfcj_ilazlc(int32_T m, int32_T n, const real32_T - A_data[], int32_T ia0, int32_T lda); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglfbimobiechdbi_bitget.cpp b/gnc/matlab/code_generation/sharedutils/mglfbimobiechdbi_bitget.cpp deleted file mode 100644 index b6783e6523..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglfbimobiechdbi_bitget.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: mglfbimobiechdbi_bitget.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "mglfbimobiechdbi_bitget.h" - -// Function for MATLAB Function: '/MATLAB Function2' -void mglfbimobiechdbi_bitget(uint32_T a, uint32_T c[17]) -{ - int32_T k; - for (k = 0; k < 17; k++) { - c[k] = (uint32_T)((int32_T)(uint32_T)((uint32_T)(1U << k) & a) != 0); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglfbimobiechdbi_bitget.h b/gnc/matlab/code_generation/sharedutils/mglfbimobiechdbi_bitget.h deleted file mode 100644 index 55cee97126..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglfbimobiechdbi_bitget.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: mglfbimobiechdbi_bitget.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_mglfbimobiechdbi_bitget -#define SHARE_mglfbimobiechdbi_bitget -#include "rtwtypes.h" - -extern void mglfbimobiechdbi_bitget(uint32_T a, uint32_T c[17]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglflfcbcbimecje_cat.cpp b/gnc/matlab/code_generation/sharedutils/mglflfcbcbimecje_cat.cpp deleted file mode 100644 index 0ee44b887a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglflfcbcbimecje_cat.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// -// File: mglflfcbcbimecje_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "mglflfcbcbimecje_cat.h" - -// Function for MATLAB Function: '/generate_output' -void mglflfcbcbimecje_cat(const uint8_T varargin_2[750], uint8_T y[800]) -{ - int32_T iy; - int32_T j; - iy = -1; - for (j = 0; j < 50; j++) { - iy++; - y[iy] = 0U; - } - - for (j = 0; j < 750; j++) { - iy++; - y[iy] = varargin_2[j]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglflfcbcbimecje_cat.h b/gnc/matlab/code_generation/sharedutils/mglflfcbcbimecje_cat.h deleted file mode 100644 index e529009860..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglflfcbcbimecje_cat.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: mglflfcbcbimecje_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_mglflfcbcbimecje_cat -#define SHARE_mglflfcbcbimecje_cat -#include "rtwtypes.h" - -extern void mglflfcbcbimecje_cat(const uint8_T varargin_2[750], uint8_T y[800]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglncbaaiekflfcb_cat.cpp b/gnc/matlab/code_generation/sharedutils/mglncbaaiekflfcb_cat.cpp deleted file mode 100644 index 9d9cc24f3b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglncbaaiekflfcb_cat.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// -// File: mglncbaaiekflfcb_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "mglncbaaiekflfcb_cat.h" - -// Function for MATLAB Function: '/generate_output' -void mglncbaaiekflfcb_cat(const real32_T varargin_1_data[], const int32_T - varargin_1_sizes[2], const int32_T varargin_2_sizes[3], real32_T y_data[], - int32_T y_sizes[3]) -{ - int32_T iy; - int32_T b; - int32_T j; - y_sizes[0] = (int32_T)(int8_T)varargin_1_sizes[0]; - y_sizes[1] = 2; - y_sizes[2] = 16; - iy = -1; - b = (int32_T)(varargin_1_sizes[0] << 1); - for (j = 1; j <= b; j++) { - iy++; - y_data[iy] = varargin_1_data[(int32_T)(j - 1)]; - } - - b = (int32_T)((int32_T)(varargin_2_sizes[0] << 1) * 15); - for (j = 1; j <= b; j++) { - iy++; - y_data[iy] = 0.0F; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglncbaaiekflfcb_cat.h b/gnc/matlab/code_generation/sharedutils/mglncbaaiekflfcb_cat.h deleted file mode 100644 index ae321ad33c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglncbaaiekflfcb_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: mglncbaaiekflfcb_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_mglncbaaiekflfcb_cat -#define SHARE_mglncbaaiekflfcb_cat -#include "rtwtypes.h" - -extern void mglncbaaiekflfcb_cat(const real32_T varargin_1_data[], const int32_T - varargin_1_sizes[2], const int32_T varargin_2_sizes[3], real32_T y_data[], - int32_T y_sizes[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglnkfkfmglfjekn_PadeApproximantOfDegree.cpp b/gnc/matlab/code_generation/sharedutils/mglnkfkfmglfjekn_PadeApproximantOfDegree.cpp deleted file mode 100644 index dc39f5859e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglnkfkfmglfjekn_PadeApproximantOfDegree.cpp +++ /dev/null @@ -1,193 +0,0 @@ -// -// File: mglnkfkfmglfjekn_PadeApproximantOfDegree.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "kfkfcbiekfcjmohl_mldivide.h" -#include "mglnkfkfmglfjekn_PadeApproximantOfDegree.h" - -// Function for MATLAB Function: '/MATLAB Function' -void mglnkfkfmglfjekn_PadeApproximantOfDegree(const real32_T A[16], uint8_T m, - real32_T F[16]) -{ - real32_T A2[16]; - int32_T d; - real32_T A3[16]; - real32_T A4[16]; - real32_T A_0[16]; - int32_T i; - for (d = 0; d < 4; d++) { - for (i = 0; i < 4; i++) { - A2[(int32_T)(i + (int32_T)(d << 2))] = 0.0F; - A2[(int32_T)(i + (int32_T)(d << 2))] += A[(int32_T)(d << 2)] * A[i]; - A2[(int32_T)(i + (int32_T)(d << 2))] += A[(int32_T)((int32_T)(d << 2) + 1)] - * A[(int32_T)(i + 4)]; - A2[(int32_T)(i + (int32_T)(d << 2))] += A[(int32_T)((int32_T)(d << 2) + 2)] - * A[(int32_T)(i + 8)]; - A2[(int32_T)(i + (int32_T)(d << 2))] += A[(int32_T)((int32_T)(d << 2) + 3)] - * A[(int32_T)(i + 12)]; - } - } - - if ((int32_T)m == 3) { - memcpy(&F[0], &A2[0], (uint32_T)(sizeof(real32_T) << 4U)); - F[0] += 60.0F; - F[5] += 60.0F; - F[10] += 60.0F; - F[15] += 60.0F; - for (d = 0; d < 4; d++) { - for (i = 0; i < 4; i++) { - A_0[(int32_T)(i + (int32_T)(d << 2))] = 0.0F; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)(d << 2)] * A[i]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) + - 1)] * A[(int32_T)(i + 4)]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) + - 2)] * A[(int32_T)(i + 8)]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) + - 3)] * A[(int32_T)(i + 12)]; - } - } - - for (d = 0; d < 4; d++) { - F[(int32_T)(d << 2)] = A_0[(int32_T)(d << 2)]; - F[(int32_T)(1 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + 1)]; - F[(int32_T)(2 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + 2)]; - F[(int32_T)(3 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + 3)]; - } - - for (d = 0; d < 16; d++) { - A4[d] = 12.0F * A2[d]; - } - - d = 120; - } else { - for (d = 0; d < 4; d++) { - for (i = 0; i < 4; i++) { - A3[(int32_T)(i + (int32_T)(d << 2))] = 0.0F; - A3[(int32_T)(i + (int32_T)(d << 2))] += A2[(int32_T)(d << 2)] * A2[i]; - A3[(int32_T)(i + (int32_T)(d << 2))] += A2[(int32_T)((int32_T)(d << 2) + - 1)] * A2[(int32_T)(i + 4)]; - A3[(int32_T)(i + (int32_T)(d << 2))] += A2[(int32_T)((int32_T)(d << 2) + - 2)] * A2[(int32_T)(i + 8)]; - A3[(int32_T)(i + (int32_T)(d << 2))] += A2[(int32_T)((int32_T)(d << 2) + - 3)] * A2[(int32_T)(i + 12)]; - } - } - - if ((int32_T)m == 5) { - for (d = 0; d < 16; d++) { - F[d] = 420.0F * A2[d] + A3[d]; - } - - F[0] += 15120.0F; - F[5] += 15120.0F; - F[10] += 15120.0F; - F[15] += 15120.0F; - for (d = 0; d < 4; d++) { - for (i = 0; i < 4; i++) { - A_0[(int32_T)(i + (int32_T)(d << 2))] = 0.0F; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)(d << 2)] * A[i]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) - + 1)] * A[(int32_T)(i + 4)]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) - + 2)] * A[(int32_T)(i + 8)]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) - + 3)] * A[(int32_T)(i + 12)]; - } - } - - for (d = 0; d < 4; d++) { - F[(int32_T)(d << 2)] = A_0[(int32_T)(d << 2)]; - F[(int32_T)(1 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + - 1)]; - F[(int32_T)(2 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + - 2)]; - F[(int32_T)(3 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + - 3)]; - } - - for (d = 0; d < 16; d++) { - A4[d] = 30.0F * A3[d] + 3360.0F * A2[d]; - } - - d = 30240; - } else { - for (d = 0; d < 4; d++) { - for (i = 0; i < 4; i++) { - A4[(int32_T)(i + (int32_T)(d << 2))] = 0.0F; - A4[(int32_T)(i + (int32_T)(d << 2))] += A2[(int32_T)(d << 2)] * A3[i]; - A4[(int32_T)(i + (int32_T)(d << 2))] += A2[(int32_T)((int32_T)(d << 2) - + 1)] * A3[(int32_T)(i + 4)]; - A4[(int32_T)(i + (int32_T)(d << 2))] += A2[(int32_T)((int32_T)(d << 2) - + 2)] * A3[(int32_T)(i + 8)]; - A4[(int32_T)(i + (int32_T)(d << 2))] += A2[(int32_T)((int32_T)(d << 2) - + 3)] * A3[(int32_T)(i + 12)]; - } - } - - for (d = 0; d < 16; d++) { - F[d] = (1512.0F * A3[d] + A4[d]) + 277200.0F * A2[d]; - } - - F[0] += 8.64864E+6F; - F[5] += 8.64864E+6F; - F[10] += 8.64864E+6F; - F[15] += 8.64864E+6F; - for (d = 0; d < 4; d++) { - for (i = 0; i < 4; i++) { - A_0[(int32_T)(i + (int32_T)(d << 2))] = 0.0F; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)(d << 2)] * A[i]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) - + 1)] * A[(int32_T)(i + 4)]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) - + 2)] * A[(int32_T)(i + 8)]; - A_0[(int32_T)(i + (int32_T)(d << 2))] += F[(int32_T)((int32_T)(d << 2) - + 3)] * A[(int32_T)(i + 12)]; - } - } - - for (d = 0; d < 4; d++) { - F[(int32_T)(d << 2)] = A_0[(int32_T)(d << 2)]; - F[(int32_T)(1 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + - 1)]; - F[(int32_T)(2 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + - 2)]; - F[(int32_T)(3 + (int32_T)(d << 2))] = A_0[(int32_T)((int32_T)(d << 2) + - 3)]; - } - - for (d = 0; d < 16; d++) { - A4[d] = (56.0F * A4[d] + 25200.0F * A3[d]) + 1.99584E+6F * A2[d]; - } - - d = 17297280; - } - } - - A4[0] += (real32_T)d; - A4[5] += (real32_T)d; - A4[10] += (real32_T)d; - A4[15] += (real32_T)d; - for (d = 0; d < 16; d++) { - A4[d] -= F[d]; - F[d] *= 2.0F; - } - - kfkfcbiekfcjmohl_mldivide(A4, F); - F[0]++; - F[5]++; - F[10]++; - F[15]++; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglnkfkfmglfjekn_PadeApproximantOfDegree.h b/gnc/matlab/code_generation/sharedutils/mglnkfkfmglfjekn_PadeApproximantOfDegree.h deleted file mode 100644 index accab71d8f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglnkfkfmglfjekn_PadeApproximantOfDegree.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mglnkfkfmglfjekn_PadeApproximantOfDegree.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_mglnkfkfmglfjekn_PadeApproximantOfDegree -#define SHARE_mglnkfkfmglfjekn_PadeApproximantOfDegree -#include "rtwtypes.h" - -extern void mglnkfkfmglfjekn_PadeApproximantOfDegree(const real32_T A[16], - uint8_T m, real32_T F[16]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglnnophaaimmgln_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/mglnnophaaimmgln_xnrm2.cpp deleted file mode 100644 index 9342ea21bc..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglnnophaaimmgln_xnrm2.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: mglnnophaaimmgln_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "mglnnophaaimmgln_xnrm2.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T mglnnophaaimmgln_xnrm2(int32_T n, const real32_T x_data[], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (!(n < 1)) { - if (n == 1) { - y = (real32_T)fabs((real_T)x_data[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x_data[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglnnophaaimmgln_xnrm2.h b/gnc/matlab/code_generation/sharedutils/mglnnophaaimmgln_xnrm2.h deleted file mode 100644 index 31b1adfb41..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglnnophaaimmgln_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mglnnophaaimmgln_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_mglnnophaaimmgln_xnrm2 -#define SHARE_mglnnophaaimmgln_xnrm2 -#include "rtwtypes.h" - -extern real32_T mglnnophaaimmgln_xnrm2(int32_T n, const real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglnohlnekfcekfk_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/mglnohlnekfcekfk_xdotc.cpp deleted file mode 100644 index dbf3689df8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglnohlnekfcekfk_xdotc.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// File: mglnohlnekfcekfk_xdotc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "mglnohlnekfcekfk_xdotc.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T mglnohlnekfcekfk_xdotc(int32_T n, const real32_T x[9], int32_T ix0, - const real32_T y[9], int32_T iy0) -{ - real32_T d; - int32_T ix; - int32_T iy; - int32_T k; - d = 0.0F; - if (!(n < 1)) { - ix = ix0; - iy = iy0; - for (k = 1; k <= n; k++) { - d += x[(int32_T)(ix - 1)] * y[(int32_T)(iy - 1)]; - ix++; - iy++; - } - } - - return d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mglnohlnekfcekfk_xdotc.h b/gnc/matlab/code_generation/sharedutils/mglnohlnekfcekfk_xdotc.h deleted file mode 100644 index d98c582e04..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mglnohlnekfcekfk_xdotc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mglnohlnekfcekfk_xdotc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_mglnohlnekfcekfk_xdotc -#define SHARE_mglnohlnekfcekfk_xdotc -#include "rtwtypes.h" - -extern real32_T mglnohlnekfcekfk_xdotc(int32_T n, const real32_T x[9], int32_T - ix0, const real32_T y[9], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mohdaaielnohnohl_sortIdx.cpp b/gnc/matlab/code_generation/sharedutils/mohdaaielnohnohl_sortIdx.cpp deleted file mode 100644 index 2e936c4a66..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mohdaaielnohnohl_sortIdx.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// -// File: mohdaaielnohnohl_sortIdx.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "mohdaaielnohnohl_sortIdx.h" - -// Function for MATLAB Function: '/generate_output' -void mohdaaielnohnohl_sortIdx(const real_T x_data[], const int32_T x_sizes[2], - int32_T idx_data[], int32_T idx_sizes[2]) -{ - int32_T n; - int32_T k; - int32_T i; - int32_T i2; - int32_T j; - int32_T pEnd; - int32_T p; - int32_T q; - int32_T qEnd; - int32_T kEnd; - int32_T iwork_data[504]; - idx_sizes[0] = 1; - idx_sizes[1] = x_sizes[1]; - n = x_sizes[1]; - for (i = 0; i < n; i++) { - idx_data[i] = 0; - } - - if (x_sizes[1] != 0) { - n = x_sizes[1] + 1; - for (i = 1; i <= n - 2; i += 2) { - if (x_data[i - 1] <= x_data[i]) { - idx_data[i - 1] = i; - idx_data[i] = i + 1; - } else { - idx_data[i - 1] = i + 1; - idx_data[i] = i; - } - } - - if ((x_sizes[1] & 1) != 0) { - idx_data[x_sizes[1] - 1] = x_sizes[1]; - } - - i = 2; - while (i < n - 1) { - i2 = i << 1; - j = 1; - pEnd = 1 + i; - while (pEnd < n) { - p = j; - q = pEnd; - qEnd = j + i2; - if (qEnd > n) { - qEnd = n; - } - - k = 0; - kEnd = qEnd - j; - while (k + 1 <= kEnd) { - if (x_data[idx_data[p - 1] - 1] <= x_data[idx_data[q - 1] - 1]) { - iwork_data[k] = idx_data[p - 1]; - p++; - if (p == pEnd) { - while (q < qEnd) { - k++; - iwork_data[k] = idx_data[q - 1]; - q++; - } - } - } else { - iwork_data[k] = idx_data[q - 1]; - q++; - if (q == qEnd) { - while (p < pEnd) { - k++; - iwork_data[k] = idx_data[p - 1]; - p++; - } - } - } - - k++; - } - - for (pEnd = 0; pEnd + 1 <= kEnd; pEnd++) { - idx_data[(j + pEnd) - 1] = iwork_data[pEnd]; - } - - j = qEnd; - pEnd = qEnd + i; - } - - i = i2; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mohdaaielnohnohl_sortIdx.h b/gnc/matlab/code_generation/sharedutils/mohdaaielnohnohl_sortIdx.h deleted file mode 100644 index 6e9cb5cd57..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mohdaaielnohnohl_sortIdx.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mohdaaielnohnohl_sortIdx.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_mohdaaielnohnohl_sortIdx -#define SHARE_mohdaaielnohnohl_sortIdx -#include "rtwtypes.h" - -extern void mohdaaielnohnohl_sortIdx(const real_T x_data[], const int32_T - x_sizes[2], int32_T idx_data[], int32_T idx_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mohdcjmgnohlknoh_sortLE.cpp b/gnc/matlab/code_generation/sharedutils/mohdcjmgnohlknoh_sortLE.cpp deleted file mode 100644 index a08d4dbbf6..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mohdcjmgnohlknoh_sortLE.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// -// File: mohdcjmgnohlknoh_sortLE.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "rt_nonfinite.h" -#include "mohdcjmgnohlknoh_sortLE.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T mohdcjmgnohlknoh_sortLE(const real32_T v[17264], const int32_T col[2], - int32_T irow1, int32_T irow2) -{ - boolean_T p; - int32_T coloffset; - boolean_T b; - int32_T k; - boolean_T exitg1; - p = true; - k = 0; - exitg1 = false; - while ((!exitg1) && (k < 2)) { - coloffset = (int32_T)((int32_T)((int32_T)(col[k] - 1) * 8632) - 1); - if ((v[(int32_T)(coloffset + irow1)] == v[(int32_T)(coloffset + irow2)]) || - (rtIsNaNF(v[(int32_T)(coloffset + irow1)]) && rtIsNaNF(v[(int32_T) - (coloffset + irow2)]))) { - b = true; - } else { - b = false; - } - - if (!b) { - if ((v[(int32_T)(coloffset + irow1)] <= v[(int32_T)(coloffset + irow2)]) || - rtIsNaNF(v[(int32_T)(coloffset + irow2)])) { - } else { - p = false; - } - - exitg1 = true; - } else { - k++; - } - } - - return p; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/mohdcjmgnohlknoh_sortLE.h b/gnc/matlab/code_generation/sharedutils/mohdcjmgnohlknoh_sortLE.h deleted file mode 100644 index ccfdc8a52b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/mohdcjmgnohlknoh_sortLE.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: mohdcjmgnohlknoh_sortLE.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_mohdcjmgnohlknoh_sortLE -#define SHARE_mohdcjmgnohlknoh_sortLE -#include "rtwtypes.h" - -extern boolean_T mohdcjmgnohlknoh_sortLE(const real32_T v[17264], const int32_T - col[2], int32_T irow1, int32_T irow2); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/moppaaimimgdecjm_svd.cpp b/gnc/matlab/code_generation/sharedutils/moppaaimimgdecjm_svd.cpp deleted file mode 100644 index d02f8be301..0000000000 --- a/gnc/matlab/code_generation/sharedutils/moppaaimimgdecjm_svd.cpp +++ /dev/null @@ -1,336 +0,0 @@ -// -// File: moppaaimimgdecjm_svd.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "cbimeknolnglpphd_xswap.h" -#include "ecbaphdbnglnimoh_xdotc.h" -#include "fcjejmgdgdbicjek_xaxpy.h" -#include "gdbijekfimglfcje_xrot.h" -#include "hdbinophmgdjjmoh_xrotg.h" -#include "imgdgdbadbaihlng_xnrm2.h" -#include "knopcjmocbaacbaa_xscal.h" -#include "rt_nonfinite.h" -#include "moppaaimimgdecjm_svd.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void moppaaimimgdecjm_svd(const real32_T A[4], real32_T U[4], real32_T S[4], - real32_T V[4]) -{ - real32_T b_A[4]; - real32_T s[2]; - real32_T e[2]; - real32_T Vf[4]; - int32_T q; - boolean_T apply_transform; - real32_T nrm; - int32_T m; - int32_T iter; - real32_T ztest0; - int32_T kase; - int32_T qs; - real32_T ztest; - real32_T smm1; - real32_T emm1; - real32_T sqds; - real32_T shift; - real32_T varargin_1[5]; - boolean_T exitg1; - boolean_T exitg2; - boolean_T exitg3; - b_A[0] = A[0]; - b_A[1] = A[1]; - b_A[2] = A[2]; - b_A[3] = A[3]; - apply_transform = false; - nrm = imgdgdbadbaihlng_xnrm2(A); - if (nrm > 0.0F) { - apply_transform = true; - if (A[0] < 0.0F) { - nrm = -nrm; - } - - if ((real32_T)fabs((real_T)nrm) >= 9.86076132E-32F) { - ztest0 = 1.0F / nrm; - b_A[0] = ztest0 * A[0]; - b_A[1] = ztest0 * A[1]; - } else { - b_A[0] = A[0] / nrm; - b_A[1] = A[1] / nrm; - } - - b_A[0]++; - s[0] = -nrm; - } else { - s[0] = 0.0F; - } - - if (apply_transform) { - fcjejmgdgdbicjek_xaxpy(-(ecbaphdbnglnimoh_xdotc(b_A, b_A) / b_A[0]), b_A); - } - - U[0] = b_A[0]; - U[2] = 0.0F; - U[1] = b_A[1]; - m = 2; - s[1] = b_A[3]; - e[1] = 0.0F; - U[3] = 1.0F; - if (s[0] != 0.0F) { - fcjejmgdgdbicjek_xaxpy(-(ecbaphdbnglnimoh_xdotc(U, U) / b_A[0]), U); - U[0] = -U[0]; - U[1] = -U[1]; - U[0]++; - } else { - U[1] = 0.0F; - U[0] = 1.0F; - } - - Vf[2] = 0.0F; - Vf[3] = 1.0F; - Vf[1] = 0.0F; - Vf[0] = 1.0F; - ztest = b_A[2]; - if (s[0] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[0]); - nrm = s[0] / ztest0; - s[0] = ztest0; - ztest = b_A[2] / nrm; - knopcjmocbaacbaa_xscal(nrm, U, 1); - } - - if (ztest != 0.0F) { - ztest0 = (real32_T)fabs((real_T)ztest); - nrm = ztest0 / ztest; - ztest = ztest0; - s[1] = b_A[3] * nrm; - knopcjmocbaacbaa_xscal(nrm, Vf, 3); - } - - if (s[1] != 0.0F) { - ztest0 = (real32_T)fabs((real_T)s[1]); - nrm = s[1] / ztest0; - s[1] = ztest0; - knopcjmocbaacbaa_xscal(nrm, U, 3); - } - - e[0] = ztest; - iter = 0; - nrm = 0.0F; - if ((s[0] >= ztest) || rtIsNaNF(ztest)) { - ztest = s[0]; - } - - if (!((0.0F >= ztest) || rtIsNaNF(ztest))) { - nrm = ztest; - } - - if (s[1] >= 0.0F) { - ztest0 = s[1]; - } else { - ztest0 = 0.0F; - } - - if (!((nrm >= ztest0) || rtIsNaNF(ztest0))) { - nrm = ztest0; - } - - while ((m > 0) && (!(iter >= 75))) { - q = (int32_T)(m - 1); - exitg3 = false; - while (!(exitg3 || (q == 0))) { - ztest0 = (real32_T)fabs((real_T)e[0]); - if ((ztest0 <= ((real32_T)fabs((real_T)s[0]) + (real32_T)fabs((real_T)s[1])) - * 1.1920929E-7F) || ((ztest0 <= 9.86076132E-32F) || ((iter > 20) && - (ztest0 <= 1.1920929E-7F * nrm)))) { - e[0] = 0.0F; - exitg3 = true; - } else { - q = 0; - } - } - - if ((int32_T)(m - 1) == q) { - kase = 4; - } else { - qs = m; - kase = m; - exitg2 = false; - while ((!exitg2) && (kase >= q)) { - qs = kase; - if (kase == q) { - exitg2 = true; - } else { - ztest0 = 0.0F; - if (kase < m) { - ztest0 = (real32_T)fabs((real_T)e[0]); - } - - if (kase > (int32_T)(q + 1)) { - ztest0 += (real32_T)fabs((real_T)e[0]); - } - - ztest = (real32_T)fabs((real_T)s[(int32_T)(kase - 1)]); - if ((ztest <= 1.1920929E-7F * ztest0) || (ztest <= 9.86076132E-32F)) { - s[(int32_T)(kase - 1)] = 0.0F; - exitg2 = true; - } else { - kase--; - } - } - } - - if (qs == q) { - kase = 3; - } else if (qs == m) { - kase = 1; - } else { - kase = 2; - q = qs; - } - } - - switch (kase) { - case 1: - ztest0 = e[0]; - e[0] = 0.0F; - qs = (int32_T)(m - 1); - while (qs >= (int32_T)(q + 1)) { - ztest = s[0]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[0] = ztest; - gdbijekfimglfcje_xrot(Vf, 1, (int32_T)(1 + (int32_T)((int32_T)(m - 1) << - 1)), sqds, smm1); - qs = 0; - } - break; - - case 2: - ztest0 = e[(int32_T)(q - 1)]; - e[(int32_T)(q - 1)] = 0.0F; - for (qs = q; (int32_T)(qs + 1) <= m; qs++) { - ztest = s[qs]; - hdbinophmgdjjmoh_xrotg(&ztest, &ztest0, &sqds, &smm1); - s[qs] = ztest; - ztest0 = -smm1 * e[qs]; - e[qs] *= sqds; - gdbijekfimglfcje_xrot(U, (int32_T)(1 + (int32_T)(qs << 1)), (int32_T)(1 - + (int32_T)((int32_T)(q - 1) << 1)), sqds, smm1); - } - break; - - case 3: - varargin_1[0] = (real32_T)fabs((real_T)s[(int32_T)(m - 1)]); - varargin_1[1] = (real32_T)fabs((real_T)s[0]); - varargin_1[2] = (real32_T)fabs((real_T)e[0]); - varargin_1[3] = (real32_T)fabs((real_T)s[q]); - varargin_1[4] = (real32_T)fabs((real_T)e[q]); - qs = 1; - ztest = varargin_1[0]; - if (rtIsNaNF(varargin_1[0])) { - kase = 2; - exitg1 = false; - while ((!exitg1) && (kase < 6)) { - qs = kase; - if (!rtIsNaNF(varargin_1[(int32_T)(kase - 1)])) { - ztest = varargin_1[(int32_T)(kase - 1)]; - exitg1 = true; - } else { - kase++; - } - } - } - - if (qs < 5) { - while ((int32_T)(qs + 1) < 6) { - if (varargin_1[qs] > ztest) { - ztest = varargin_1[qs]; - } - - qs++; - } - } - - ztest0 = s[(int32_T)(m - 1)] / ztest; - smm1 = s[0] / ztest; - emm1 = e[0] / ztest; - sqds = s[q] / ztest; - smm1 = ((smm1 + ztest0) * (smm1 - ztest0) + emm1 * emm1) / 2.0F; - emm1 *= ztest0; - emm1 *= emm1; - if ((smm1 != 0.0F) || (emm1 != 0.0F)) { - shift = (real32_T)sqrt((real_T)(smm1 * smm1 + emm1)); - if (smm1 < 0.0F) { - shift = -shift; - } - - shift = emm1 / (smm1 + shift); - } else { - shift = 0.0F; - } - - ztest0 = (sqds + ztest0) * (sqds - ztest0) + shift; - ztest = e[q] / ztest * sqds; - while ((int32_T)(q + 1) <= 1) { - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - ztest0 = sqds * s[0] + smm1 * e[0]; - e[0] = sqds * e[0] - smm1 * s[0]; - ztest = smm1 * s[1]; - s[1] *= sqds; - gdbijekfimglfcje_xrot(Vf, 1, 3, sqds, smm1); - hdbinophmgdjjmoh_xrotg(&ztest0, &ztest, &sqds, &smm1); - s[0] = ztest0; - ztest0 = sqds * e[0] + smm1 * s[1]; - s[1] = -smm1 * e[0] + sqds * s[1]; - ztest = smm1 * e[1]; - e[1] *= sqds; - gdbijekfimglfcje_xrot(U, 1, 3, sqds, smm1); - q = 1; - } - - e[0] = ztest0; - iter++; - break; - - default: - if (s[q] < 0.0F) { - s[q] = -s[q]; - knopcjmocbaacbaa_xscal(-1.0F, Vf, (int32_T)(1 + (int32_T)(q << 1))); - } - - while (((int32_T)(q + 1) < 2) && (s[0] < s[1])) { - ztest0 = s[0]; - s[0] = s[1]; - s[1] = ztest0; - cbimeknolnglpphd_xswap(Vf); - cbimeknolnglpphd_xswap(U); - q = 1; - } - - iter = 0; - m--; - break; - } - } - - V[0] = Vf[0]; - V[1] = Vf[1]; - V[2] = Vf[2]; - V[3] = Vf[3]; - S[1] = 0.0F; - S[2] = 0.0F; - S[0] = s[0]; - S[3] = s[1]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/moppaaimimgdecjm_svd.h b/gnc/matlab/code_generation/sharedutils/moppaaimimgdecjm_svd.h deleted file mode 100644 index d6a6085b9c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/moppaaimimgdecjm_svd.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: moppaaimimgdecjm_svd.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_moppaaimimgdecjm_svd -#define SHARE_moppaaimimgdecjm_svd -#include "rtwtypes.h" - -extern void moppaaimimgdecjm_svd(const real32_T A[4], real32_T U[4], real32_T S - [4], real32_T V[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/moppbaaafkfkimgd_diag.cpp b/gnc/matlab/code_generation/sharedutils/moppbaaafkfkimgd_diag.cpp deleted file mode 100644 index 2f40b106ce..0000000000 --- a/gnc/matlab/code_generation/sharedutils/moppbaaafkfkimgd_diag.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// File: moppbaaafkfkimgd_diag.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "moppbaaafkfkimgd_diag.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void moppbaaafkfkimgd_diag(const real_T v_data[], const int32_T v_sizes, real_T - d_data[], int32_T d_sizes[2]) -{ - int32_T i; - int32_T loop_ub; - int8_T b_idx_0; - int8_T b_idx_1; - b_idx_0 = (int8_T)v_sizes; - b_idx_1 = (int8_T)v_sizes; - d_sizes[0] = (int32_T)b_idx_0; - d_sizes[1] = (int32_T)b_idx_1; - loop_ub = (int32_T)((int32_T)b_idx_0 * (int32_T)b_idx_1); - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - d_data[i] = 0.0; - } - - for (loop_ub = 0; (int32_T)(loop_ub + 1) <= v_sizes; loop_ub++) { - d_data[(int32_T)(loop_ub + (int32_T)((int32_T)b_idx_0 * loop_ub))] = - v_data[loop_ub]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/moppbaaafkfkimgd_diag.h b/gnc/matlab/code_generation/sharedutils/moppbaaafkfkimgd_diag.h deleted file mode 100644 index 41b701eb36..0000000000 --- a/gnc/matlab/code_generation/sharedutils/moppbaaafkfkimgd_diag.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: moppbaaafkfkimgd_diag.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_moppbaaafkfkimgd_diag -#define SHARE_moppbaaafkfkimgd_diag -#include "rtwtypes.h" - -extern void moppbaaafkfkimgd_diag(const real_T v_data[], const int32_T v_sizes, - real_T d_data[], int32_T d_sizes[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/moppjecbaaaaimop_xrot.cpp b/gnc/matlab/code_generation/sharedutils/moppjecbaaaaimop_xrot.cpp deleted file mode 100644 index 5d4f63ed66..0000000000 --- a/gnc/matlab/code_generation/sharedutils/moppjecbaaaaimop_xrot.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// File: moppjecbaaaaimop_xrot.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "moppjecbaaaaimop_xrot.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void moppjecbaaaaimop_xrot(real32_T x[16], int32_T ix0, int32_T iy0, real32_T c, - real32_T s) -{ - int32_T ix; - int32_T iy; - real32_T temp; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/moppjecbaaaaimop_xrot.h b/gnc/matlab/code_generation/sharedutils/moppjecbaaaaimop_xrot.h deleted file mode 100644 index 9b3fcd1bd7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/moppjecbaaaaimop_xrot.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: moppjecbaaaaimop_xrot.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_moppjecbaaaaimop_xrot -#define SHARE_moppjecbaaaaimop_xrot -#include "rtwtypes.h" - -extern void moppjecbaaaaimop_xrot(real32_T x[16], int32_T ix0, int32_T iy0, - real32_T c, real32_T s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ngdbhdbaknglaaai_do_vectors.cpp b/gnc/matlab/code_generation/sharedutils/ngdbhdbaknglaaai_do_vectors.cpp deleted file mode 100644 index 7187bb65d9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ngdbhdbaknglaaai_do_vectors.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// -// File: ngdbhdbaknglaaai_do_vectors.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "jecbmglnphlfjmop_skip_to_last_equal_value.h" -#include "lngdpphlmgdjekno_skip_to_last_equal_value.h" -#include "ngdbhdbaknglaaai_do_vectors.h" - -// Function for MATLAB Function: '/generate_output' -void ngdbhdbaknglaaai_do_vectors(const real32_T a[50], const real32_T b_data[], - const int32_T b_sizes, real32_T c_data[], int32_T *c_sizes, int32_T ia_data[], - int32_T *ia_sizes, int32_T ib_data[], int32_T *ib_sizes) -{ - int32_T nc; - int32_T iafirst; - int32_T ialast; - int32_T ibfirst; - int32_T iblast; - int32_T b_ialast; - real32_T ak; - int32_T b_iblast; - real32_T bk; - real32_T absxk; - int32_T exponent; - int32_T ia_data_0[50]; - real32_T c_data_0[50]; - if (50 <= b_sizes) { - *ia_sizes = 50; - } else { - *ia_sizes = b_sizes; - } - - *c_sizes = (int8_T)*ia_sizes; - *ib_sizes = *ia_sizes; - nc = 0; - iafirst = 0; - ialast = 1; - ibfirst = 0; - iblast = 1; - while ((ialast <= 50) && (iblast <= b_sizes)) { - b_ialast = ialast; - ak = lngdpphlmgdjekno_skip_to_last_equal_value(&b_ialast, a); - ialast = b_ialast; - b_iblast = iblast; - bk = jecbmglnphlfjmop_skip_to_last_equal_value(&b_iblast, b_data, b_sizes); - iblast = b_iblast; - absxk = (real32_T)fabs((real_T)(bk / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, exponent - 24); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(bk - ak)) < absxk) || (rtIsInfF(ak) && rtIsInfF - (bk) && ((ak > 0.0F) == (bk > 0.0F)))) { - nc++; - c_data[nc - 1] = ak; - ia_data[nc - 1] = iafirst + 1; - ib_data[nc - 1] = ibfirst + 1; - ialast = b_ialast + 1; - iafirst = b_ialast; - iblast = b_iblast + 1; - ibfirst = b_iblast; - } else if ((ak < bk) || rtIsNaNF(bk)) { - ialast = b_ialast + 1; - iafirst = b_ialast; - } else { - iblast = b_iblast + 1; - ibfirst = b_iblast; - } - } - - if (*ia_sizes > 0) { - if (1 > nc) { - *ia_sizes = 0; - } else { - *ia_sizes = nc; - } - - for (iafirst = 0; iafirst < *ia_sizes; iafirst++) { - ia_data_0[iafirst] = ia_data[iafirst]; - } - - for (iafirst = 0; iafirst < *ia_sizes; iafirst++) { - ia_data[iafirst] = ia_data_0[iafirst]; - } - - if (1 > nc) { - *ib_sizes = 0; - } else { - *ib_sizes = nc; - } - - for (iafirst = 0; iafirst < *ib_sizes; iafirst++) { - ia_data_0[iafirst] = ib_data[iafirst]; - } - - for (iafirst = 0; iafirst < *ib_sizes; iafirst++) { - ib_data[iafirst] = ia_data_0[iafirst]; - } - - if (1 > nc) { - nc = 0; - } - - for (iafirst = 0; iafirst < nc; iafirst++) { - c_data_0[iafirst] = c_data[iafirst]; - } - - *c_sizes = nc; - for (iafirst = 0; iafirst < nc; iafirst++) { - c_data[iafirst] = c_data_0[iafirst]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ngdbhdbaknglaaai_do_vectors.h b/gnc/matlab/code_generation/sharedutils/ngdbhdbaknglaaai_do_vectors.h deleted file mode 100644 index 42b147bd7e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ngdbhdbaknglaaai_do_vectors.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: ngdbhdbaknglaaai_do_vectors.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_ngdbhdbaknglaaai_do_vectors -#define SHARE_ngdbhdbaknglaaai_do_vectors -#include "rtwtypes.h" - -extern void ngdbhdbaknglaaai_do_vectors(const real32_T a[50], const real32_T - b_data[], const int32_T b_sizes, real32_T c_data[], int32_T *c_sizes, int32_T - ia_data[], int32_T *ia_sizes, int32_T ib_data[], int32_T *ib_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ngdjjecbgdbaglfc_eye.cpp b/gnc/matlab/code_generation/sharedutils/ngdjjecbgdbaglfc_eye.cpp deleted file mode 100644 index af383b17f4..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ngdjjecbgdbaglfc_eye.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// -// File: ngdjjecbgdbaglfc_eye.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "ngdjjecbgdbaglfc_eye.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void ngdjjecbgdbaglfc_eye(real32_T I[702]) -{ - int32_T k; - memset(&I[0], 0, (uint32_T)(702U * sizeof(real32_T))); - for (k = 0; k < 6; k++) { - I[(int32_T)(k + (int32_T)(6 * k))] = 1.0F; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ngdjjecbgdbaglfc_eye.h b/gnc/matlab/code_generation/sharedutils/ngdjjecbgdbaglfc_eye.h deleted file mode 100644 index e982cc7e2d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ngdjjecbgdbaglfc_eye.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: ngdjjecbgdbaglfc_eye.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ngdjjecbgdbaglfc_eye -#define SHARE_ngdjjecbgdbaglfc_eye -#include "rtwtypes.h" - -extern void ngdjjecbgdbaglfc_eye(real32_T I[702]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nglndjmgaimophdj_xswap.cpp b/gnc/matlab/code_generation/sharedutils/nglndjmgaimophdj_xswap.cpp deleted file mode 100644 index c0cd0ced2b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nglndjmgaimophdj_xswap.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: nglndjmgaimophdj_xswap.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "nglndjmgaimophdj_xswap.h" - -// Function for MATLAB Function: '/MATLAB Function' -void nglndjmgaimophdj_xswap(real32_T x[72], int32_T ix0, int32_T iy0) -{ - int32_T ix; - int32_T iy; - real32_T temp; - int32_T k; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k < 12; k++) { - temp = x[ix]; - x[ix] = x[iy]; - x[iy] = temp; - ix++; - iy++; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nglndjmgaimophdj_xswap.h b/gnc/matlab/code_generation/sharedutils/nglndjmgaimophdj_xswap.h deleted file mode 100644 index 760356633e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nglndjmgaimophdj_xswap.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: nglndjmgaimophdj_xswap.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_nglndjmgaimophdj_xswap -#define SHARE_nglndjmgaimophdj_xswap -#include "rtwtypes.h" - -extern void nglndjmgaimophdj_xswap(real32_T x[72], int32_T ix0, int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nglnglnoohdjmgdj_sortLE.cpp b/gnc/matlab/code_generation/sharedutils/nglnglnoohdjmgdj_sortLE.cpp deleted file mode 100644 index b09c7fad7e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nglnglnoohdjmgdj_sortLE.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// -// File: nglnglnoohdjmgdj_sortLE.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "rt_nonfinite.h" -#include "nglnglnoohdjmgdj_sortLE.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T nglnglnoohdjmgdj_sortLE(const real32_T v_data[], const int32_T - v_sizes[2], const int32_T col[2], int32_T irow1, int32_T irow2) -{ - boolean_T p; - int32_T coloffset; - int32_T k; - boolean_T exitg1; - p = true; - k = 0; - exitg1 = false; - while ((!exitg1) && (k < 2)) { - coloffset = (col[k] - 1) * v_sizes[0] - 1; - if (!((v_data[coloffset + irow1] == v_data[coloffset + irow2]) || (rtIsNaNF - (v_data[coloffset + irow1]) && rtIsNaNF(v_data[coloffset + irow2])))) - { - p = ((v_data[coloffset + irow1] <= v_data[coloffset + irow2]) || rtIsNaNF - (v_data[coloffset + irow2])); - exitg1 = true; - } else { - k++; - } - } - - return p; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nglnglnoohdjmgdj_sortLE.h b/gnc/matlab/code_generation/sharedutils/nglnglnoohdjmgdj_sortLE.h deleted file mode 100644 index a3247ca09e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nglnglnoohdjmgdj_sortLE.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nglnglnoohdjmgdj_sortLE.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_nglnglnoohdjmgdj_sortLE -#define SHARE_nglnglnoohdjmgdj_sortLE -#include "rtwtypes.h" - -extern boolean_T nglnglnoohdjmgdj_sortLE(const real32_T v_data[], const int32_T - v_sizes[2], const int32_T col[2], int32_T irow1, int32_T irow2); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nglnkfcbnophmohd_xgeqrf.cpp b/gnc/matlab/code_generation/sharedutils/nglnkfcbnophmohd_xgeqrf.cpp deleted file mode 100644 index f892fe20f1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nglnkfcbnophmohd_xgeqrf.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// -// File: nglnkfcbnophmohd_xgeqrf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "nopppphdfcjmcbaa_xzlarf.h" -#include "phdjbaaiglngbaaa_xzlarfg.h" -#include "nglnkfcbnophmohd_xgeqrf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void nglnkfcbnophmohd_xgeqrf(real32_T A_data[], int32_T A_sizes[2], real32_T - tau_data[], int32_T *tau_sizes) -{ - int32_T m; - int32_T mn; - real32_T work[6]; - int32_T i_i; - int32_T mmi; - real32_T b; - real32_T b_atmp; - int32_T i; - m = A_sizes[0]; - mn = A_sizes[0]; - if (!(mn <= 6)) { - mn = 6; - } - - *tau_sizes = (int8_T)mn; - if (A_sizes[0] != 0) { - for (i = 0; i < 6; i++) { - work[i] = 0.0F; - } - - for (i = 0; i + 1 <= mn; i++) { - i_i = i * m + i; - mmi = m - i; - if (i + 1 < m) { - b_atmp = A_data[i_i]; - b = phdjbaaiglngbaaa_xzlarfg(mmi, &b_atmp, A_data, i_i + 2); - tau_data[i] = b; - A_data[i_i] = b_atmp; - } else { - tau_data[i] = 0.0F; - } - - if (i + 1 < 6) { - b_atmp = A_data[i_i]; - A_data[i_i] = 1.0F; - nopppphdfcjmcbaa_xzlarf(mmi, 5 - i, i_i + 1, tau_data[i], A_data, (i + - (i + 1) * m) + 1, m, work); - A_data[i_i] = b_atmp; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nglnkfcbnophmohd_xgeqrf.h b/gnc/matlab/code_generation/sharedutils/nglnkfcbnophmohd_xgeqrf.h deleted file mode 100644 index 729d5b1adb..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nglnkfcbnophmohd_xgeqrf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nglnkfcbnophmohd_xgeqrf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_nglnkfcbnophmohd_xgeqrf -#define SHARE_nglnkfcbnophmohd_xgeqrf -#include "rtwtypes.h" - -extern void nglnkfcbnophmohd_xgeqrf(real32_T A_data[], int32_T A_sizes[2], - real32_T tau_data[], int32_T *tau_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nglnopphmohdaiek_rdivide.cpp b/gnc/matlab/code_generation/sharedutils/nglnopphmohdaiek_rdivide.cpp deleted file mode 100644 index 55db3ca151..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nglnopphmohdaiek_rdivide.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nglnopphmohdaiek_rdivide.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Thu Mar 2 13:46:15 2017 -// -#include "rtwtypes.h" -#include "nglnopphmohdaiek_rdivide.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T nglnopphmohdaiek_rdivide(real_T x, real32_T y) -{ - return (real32_T)x / y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nglnopphmohdaiek_rdivide.h b/gnc/matlab/code_generation/sharedutils/nglnopphmohdaiek_rdivide.h deleted file mode 100644 index 835f3466e9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nglnopphmohdaiek_rdivide.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: nglnopphmohdaiek_rdivide.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1090 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Thu Mar 2 13:46:15 2017 -// -#ifndef SHARE_nglnopphmohdaiek_rdivide -#define SHARE_nglnopphmohdaiek_rdivide -#include "rtwtypes.h" - -extern real32_T nglnopphmohdaiek_rdivide(real_T x, real32_T y); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohdcbaibiecnohl_power.cpp b/gnc/matlab/code_generation/sharedutils/nohdcbaibiecnohl_power.cpp deleted file mode 100644 index e01340d66a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohdcbaibiecnohl_power.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: nohdcbaibiecnohl_power.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "nohdcbaibiecnohl_power.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void nohdcbaibiecnohl_power(const real32_T a[2], real32_T y[2]) -{ - y[0] = a[0] * a[0]; - y[1] = a[1] * a[1]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohdcbaibiecnohl_power.h b/gnc/matlab/code_generation/sharedutils/nohdcbaibiecnohl_power.h deleted file mode 100644 index 62611aecfe..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohdcbaibiecnohl_power.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: nohdcbaibiecnohl_power.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_nohdcbaibiecnohl_power -#define SHARE_nohdcbaibiecnohl_power -#include "rtwtypes.h" - -extern void nohdcbaibiecnohl_power(const real32_T a[2], real32_T y[2]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohdgdbadbiejmop_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/nohdgdbadbiejmop_xnrm2.cpp deleted file mode 100644 index 6692f9e83b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohdgdbadbiejmop_xnrm2.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// -// File: nohdgdbadbiejmop_xnrm2.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "nohdgdbadbiejmop_xnrm2.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T nohdgdbadbiejmop_xnrm2(int32_T n, const real32_T x_data[], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (n == 1) { - y = (real32_T)fabs((real_T)x_data[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x_data[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohdgdbadbiejmop_xnrm2.h b/gnc/matlab/code_generation/sharedutils/nohdgdbadbiejmop_xnrm2.h deleted file mode 100644 index 99b2bd012d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohdgdbadbiejmop_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nohdgdbadbiejmop_xnrm2.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_nohdgdbadbiejmop_xnrm2 -#define SHARE_nohdgdbadbiejmop_xnrm2 -#include "rtwtypes.h" - -extern real32_T nohdgdbadbiejmop_xnrm2(int32_T n, const real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlbaimmophdbim_quat_propagate_step.cpp b/gnc/matlab/code_generation/sharedutils/nohlbaimmophdbim_quat_propagate_step.cpp deleted file mode 100644 index 6099f3aebd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlbaimmophdbim_quat_propagate_step.cpp +++ /dev/null @@ -1,144 +0,0 @@ -// -// File: nohlbaimmophdbim_quat_propagate_step.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Dec 4 08:33:06 2017 -// -#include "rtwtypes.h" -#include -#include "nohlbaimmophdbim_quat_propagate_step.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void nohlbaimmophdbim_quat_propagate_step(const real32_T quat_in[4], const - real32_T omega[3], real32_T time_in, real_T quat_out[4]) -{ - real32_T omega_mag; - real32_T c; - real32_T sine_mag; - real_T x; - real_T c_y; - real32_T c_0[16]; - int32_T i; - real32_T s_idx_0; - real32_T s_idx_1; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // omega row vector - // - // From: Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra - // Equation below is from Eq. 122, with Omega matrix and - // identity matrix multiplied together - // - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Takes the root-sum square of each row of the matrix a - omega_mag = (real32_T)sqrt((real_T)((omega[0] * omega[0] + omega[1] * omega[1]) - + omega[2] * omega[2])); - if ((omega_mag == 0.0F) || (time_in <= 0.0F)) { - quat_out[0] = (real_T)quat_in[0]; - quat_out[1] = (real_T)quat_in[1]; - quat_out[2] = (real_T)quat_in[2]; - quat_out[3] = (real_T)quat_in[3]; - } else { - c = (real32_T)cos((real_T)(0.5F * omega_mag * time_in)); - sine_mag = (real32_T)sin((real_T)(0.5F * omega_mag * time_in)); - s_idx_0 = sine_mag * omega[0] / omega_mag; - s_idx_1 = sine_mag * omega[1] / omega_mag; - omega_mag = sine_mag * omega[2] / omega_mag; - - // Rollup of trig equations and Omega matrix - c_0[0] = c; - c_0[1] = -omega_mag; - c_0[2] = s_idx_1; - c_0[3] = -s_idx_0; - c_0[4] = omega_mag; - c_0[5] = c; - c_0[6] = -s_idx_0; - c_0[7] = -s_idx_1; - c_0[8] = -s_idx_1; - c_0[9] = s_idx_0; - c_0[10] = c; - c_0[11] = -omega_mag; - c_0[12] = s_idx_0; - c_0[13] = s_idx_1; - c_0[14] = omega_mag; - c_0[15] = c; - for (i = 0; i < 4; i++) { - c = c_0[(int32_T)(i + 12)] * quat_in[3] + (c_0[(int32_T)(i + 8)] * - quat_in[2] + (c_0[(int32_T)(i + 4)] * quat_in[1] + c_0[i] * quat_in[0])); - quat_out[i] = (real_T)c; - } - - if (quat_out[3] < 0.0) { - x = -1.0; - } else if (quat_out[3] > 0.0) { - x = 1.0; - } else if (quat_out[3] == 0.0) { - x = 0.0; - } else { - x = quat_out[3]; - } - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Takes the root-sum square of each row of the matrix a - c_y = sqrt(((quat_out[0] * quat_out[0] + quat_out[1] * quat_out[1]) + - quat_out[2] * quat_out[2]) + quat_out[3] * quat_out[3]); - quat_out[0] = x * quat_out[0] / c_y; - quat_out[1] = x * quat_out[1] / c_y; - quat_out[2] = x * quat_out[2] / c_y; - quat_out[3] = x * quat_out[3] / c_y; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlbaimmophdbim_quat_propagate_step.h b/gnc/matlab/code_generation/sharedutils/nohlbaimmophdbim_quat_propagate_step.h deleted file mode 100644 index 3e8d1115a5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlbaimmophdbim_quat_propagate_step.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nohlbaimmophdbim_quat_propagate_step.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Dec 4 08:33:06 2017 -// -#ifndef SHARE_nohlbaimmophdbim_quat_propagate_step -#define SHARE_nohlbaimmophdbim_quat_propagate_step -#include "rtwtypes.h" - -extern void nohlbaimmophdbim_quat_propagate_step(const real32_T quat_in[4], - const real32_T omega[3], real32_T time_in, real_T quat_out[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlcjekmohddjmg_abs.cpp b/gnc/matlab/code_generation/sharedutils/nohlcjekmohddjmg_abs.cpp deleted file mode 100644 index c46374d6d1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlcjekmohddjmg_abs.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// -// File: nohlcjekmohddjmg_abs.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "nohlcjekmohddjmg_abs.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void nohlcjekmohddjmg_abs(const real32_T x_data[], const int32_T x_sizes, - real32_T y_data[], int32_T *y_sizes) -{ - int32_T k; - *y_sizes = (int32_T)(uint8_T)x_sizes; - for (k = 0; (int32_T)(k + 1) <= x_sizes; k++) { - y_data[k] = (real32_T)fabs((real_T)x_data[k]); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlcjekmohddjmg_abs.h b/gnc/matlab/code_generation/sharedutils/nohlcjekmohddjmg_abs.h deleted file mode 100644 index bea3418d75..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlcjekmohddjmg_abs.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nohlcjekmohddjmg_abs.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_nohlcjekmohddjmg_abs -#define SHARE_nohlcjekmohddjmg_abs -#include "rtwtypes.h" - -extern void nohlcjekmohddjmg_abs(const real32_T x_data[], const int32_T x_sizes, - real32_T y_data[], int32_T *y_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlfkfkhlfcdbai_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/nohlfkfkhlfcdbai_xzlarf.cpp deleted file mode 100644 index f620d51b0b..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlfkfkhlfcdbai_xzlarf.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// File: nohlfkfkhlfcdbai_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "biecdbaijecjlngd_xgemv.h" -#include "lfcjpphlnoppphdb_xgerc.h" -#include "ppppmophjmopekno_ilazlc.h" -#include "nohlfkfkhlfcdbai_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void nohlfkfkhlfcdbai_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work_data[]) -{ - int32_T lastv; - int32_T lastc; - if (tau != 0.0F) { - lastv = m; - lastc = iv0 + m; - while ((lastv > 0) && (C_data[lastc - 2] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = ppppmophjmopekno_ilazlc(lastv, n, C_data, ic0, ldc); - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - biecdbaijecjlngd_xgemv(lastv, lastc, C_data, ic0, ldc, C_data, iv0, - work_data); - lfcjpphlnoppphdb_xgerc(lastv, lastc, -tau, iv0, work_data, C_data, ic0, ldc); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlfkfkhlfcdbai_xzlarf.h b/gnc/matlab/code_generation/sharedutils/nohlfkfkhlfcdbai_xzlarf.h deleted file mode 100644 index 4dd3e49e02..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlfkfkhlfcdbai_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nohlfkfkhlfcdbai_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_nohlfkfkhlfcdbai_xzlarf -#define SHARE_nohlfkfkhlfcdbai_xzlarf -#include "rtwtypes.h" - -extern void nohlfkfkhlfcdbai_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work_data[]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlglfkmglnpphd_eulers_to_quat.cpp b/gnc/matlab/code_generation/sharedutils/nohlglfkmglnpphd_eulers_to_quat.cpp deleted file mode 100644 index baac4847f9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlglfkmglnpphd_eulers_to_quat.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// -// File: nohlglfkmglnpphd_eulers_to_quat.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#include "rtwtypes.h" -#include -#include "nohlglfkmglnpphd_eulers_to_quat.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void nohlglfkmglnpphd_eulers_to_quat(real32_T rz, real32_T ry, real32_T rx, - real32_T quat[4]) -{ - real32_T result; - int32_T i; - int32_T loop_ub; - real32_T quat_data[4]; - int32_T ii_sizes_idx_0; - int32_T ii_sizes_idx_1; - int32_T quat_sizes_idx_0; - quat[0] = (real32_T)cos((real_T)(rz / 2.0F)) * (real32_T)cos((real_T)(ry / - 2.0F)) * (real32_T)sin((real_T)(rx / 2.0F)) - (real32_T)sin((real_T)(rz / - 2.0F)) * (real32_T)sin((real_T)(ry / 2.0F)) * (real32_T)cos((real_T)(rx / - 2.0F)); - quat[1] = (real32_T)cos((real_T)(rz / 2.0F)) * (real32_T)sin((real_T)(ry / - 2.0F)) * (real32_T)cos((real_T)(rx / 2.0F)) + (real32_T)sin((real_T)(rz / - 2.0F)) * (real32_T)cos((real_T)(ry / 2.0F)) * (real32_T)sin((real_T)(rx / - 2.0F)); - quat[2] = (real32_T)sin((real_T)(rz / 2.0F)) * (real32_T)cos((real_T)(ry / - 2.0F)) * (real32_T)cos((real_T)(rx / 2.0F)) - (real32_T)cos((real_T)(rz / - 2.0F)) * (real32_T)sin((real_T)(ry / 2.0F)) * (real32_T)sin((real_T)(rx / - 2.0F)); - quat[3] = (real32_T)cos((real_T)(rz / 2.0F)) * (real32_T)cos((real_T)(ry / - 2.0F)) * (real32_T)cos((real_T)(rx / 2.0F)) + (real32_T)sin((real_T)(rz / - 2.0F)) * (real32_T)sin((real_T)(ry / 2.0F)) * (real32_T)sin((real_T)(rx / - 2.0F)); - - // Normalize the quaternion and make the scalar term positive - if (quat[3] < 0.0F) { - ii_sizes_idx_0 = 1; - ii_sizes_idx_1 = 1; - } else { - ii_sizes_idx_0 = 0; - ii_sizes_idx_1 = 0; - } - - quat_sizes_idx_0 = (int32_T)(ii_sizes_idx_0 * ii_sizes_idx_1); - loop_ub = (int32_T)(ii_sizes_idx_0 * ii_sizes_idx_1); - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - quat_data[i] = -quat[0]; - } - - loop_ub = (int32_T)(ii_sizes_idx_0 * ii_sizes_idx_1); - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - quat_data[(int32_T)(i + quat_sizes_idx_0)] = -quat[1]; - } - - loop_ub = (int32_T)(ii_sizes_idx_0 * ii_sizes_idx_1); - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - quat_data[(int32_T)(i + (int32_T)(quat_sizes_idx_0 << 1))] = -quat[2]; - } - - loop_ub = (int32_T)(ii_sizes_idx_0 * ii_sizes_idx_1); - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - quat_data[(int32_T)(i + (int32_T)(quat_sizes_idx_0 * 3))] = -quat[3]; - } - - for (i = 0; i <= (int32_T)(quat_sizes_idx_0 - 1); i++) { - quat[0] = quat_data[i]; - } - - for (i = 0; i <= (int32_T)(quat_sizes_idx_0 - 1); i++) { - quat[1] = quat_data[(int32_T)(i + quat_sizes_idx_0)]; - } - - for (i = 0; i <= (int32_T)(quat_sizes_idx_0 - 1); i++) { - quat[2] = quat_data[(int32_T)((int32_T)(quat_sizes_idx_0 << 1) + i)]; - } - - for (i = 0; i <= (int32_T)(quat_sizes_idx_0 - 1); i++) { - quat[3] = quat_data[(int32_T)((int32_T)(quat_sizes_idx_0 * 3) + i)]; - } - - // - // Column_Vector = RSSROW(Matrix) - // - // where Matrix is an n-by-m matrix. This function will return the RSS of - // each row into an n-by-1 column vector for any number of m columns. - // - // Douglas Adams, Feb, 2005 - // - result = 1.0F / (real32_T)sqrt((real_T)(((quat[0] * quat[0] + quat[1] * quat[1]) - + quat[2] * quat[2]) + quat[3] * quat[3])); - quat[0] *= result; - quat[1] *= result; - quat[2] *= result; - quat[3] *= result; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlglfkmglnpphd_eulers_to_quat.h b/gnc/matlab/code_generation/sharedutils/nohlglfkmglnpphd_eulers_to_quat.h deleted file mode 100644 index 1df879e947..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlglfkmglnpphd_eulers_to_quat.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nohlglfkmglnpphd_eulers_to_quat.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:42:31 2017 -// -#ifndef SHARE_nohlglfkmglnpphd_eulers_to_quat -#define SHARE_nohlglfkmglnpphd_eulers_to_quat -#include "rtwtypes.h" - -extern void nohlglfkmglnpphd_eulers_to_quat(real32_T rz, real32_T ry, real32_T - rx, real32_T quat[4]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlkfkngdjmkfcb_xrot.cpp b/gnc/matlab/code_generation/sharedutils/nohlkfkngdjmkfcb_xrot.cpp deleted file mode 100644 index 58a172bfd4..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlkfkngdjmkfcb_xrot.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: nohlkfkngdjmkfcb_xrot.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "nohlkfkngdjmkfcb_xrot.h" - -// Function for MATLAB Function: '/MATLAB Function' -void nohlkfkngdjmkfcb_xrot(real32_T x[72], int32_T ix0, int32_T iy0, real32_T c, - real32_T s) -{ - int32_T ix; - int32_T iy; - real32_T temp; - int32_T k; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k < 12; k++) { - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nohlkfkngdjmkfcb_xrot.h b/gnc/matlab/code_generation/sharedutils/nohlkfkngdjmkfcb_xrot.h deleted file mode 100644 index f767c63817..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nohlkfkngdjmkfcb_xrot.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nohlkfkngdjmkfcb_xrot.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_nohlkfkngdjmkfcb_xrot -#define SHARE_nohlkfkngdjmkfcb_xrot -#include "rtwtypes.h" - -extern void nohlkfkngdjmkfcb_xrot(real32_T x[72], int32_T ix0, int32_T iy0, - real32_T c, real32_T s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nopppphdfcjmcbaa_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/nopppphdfcjmcbaa_xzlarf.cpp deleted file mode 100644 index 9790100be8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nopppphdfcjmcbaa_xzlarf.cpp +++ /dev/null @@ -1,112 +0,0 @@ -// -// File: nopppphdfcjmcbaa_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "nopppphdfcjmcbaa_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void nopppphdfcjmcbaa_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]) -{ - int32_T lastv; - int32_T lastc; - int32_T ix; - real32_T c; - int32_T iy; - int32_T iac; - int32_T d; - int32_T b_ia; - int32_T jy; - boolean_T exitg2; - if (tau != 0.0F) { - lastv = m; - lastc = iv0 + m; - while ((lastv > 0) && (C_data[lastc - 2] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = n; - exitg2 = false; - while ((!exitg2) && (lastc > 0)) { - iy = (lastc - 1) * ldc + ic0; - jy = iy; - do { - iac = 0; - if (jy <= (iy + lastv) - 1) { - if (C_data[jy - 1] != 0.0F) { - iac = 1; - } else { - jy++; - } - } else { - lastc--; - iac = 2; - } - } while (iac == 0); - - if (iac == 1) { - exitg2 = true; - } - } - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - if (lastc != 0) { - for (iy = 1; iy <= lastc; iy++) { - work[iy - 1] = 0.0F; - } - - iy = 0; - jy = (lastc - 1) * ldc + ic0; - iac = ic0; - while ((ldc > 0) && (iac <= jy)) { - ix = iv0; - c = 0.0F; - d = (iac + lastv) - 1; - for (b_ia = iac; b_ia <= d; b_ia++) { - c += C_data[b_ia - 1] * C_data[ix - 1]; - ix++; - } - - work[iy] += c; - iy++; - iac += ldc; - } - } - - if (!(-tau == 0.0F)) { - iy = ic0 - 1; - jy = 0; - for (iac = 1; iac <= lastc; iac++) { - if (work[jy] != 0.0F) { - c = work[jy] * -tau; - ix = iv0; - d = lastv + iy; - for (b_ia = iy; b_ia + 1 <= d; b_ia++) { - C_data[b_ia] += C_data[ix - 1] * c; - ix++; - } - } - - jy++; - iy += ldc; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/nopppphdfcjmcbaa_xzlarf.h b/gnc/matlab/code_generation/sharedutils/nopppphdfcjmcbaa_xzlarf.h deleted file mode 100644 index a23438f7c5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/nopppphdfcjmcbaa_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: nopppphdfcjmcbaa_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_nopppphdfcjmcbaa_xzlarf -#define SHARE_nopppphdfcjmcbaa_xzlarf -#include "rtwtypes.h" - -extern void nopppphdfcjmcbaa_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work[6]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdbaiekngdjbaaa_xdotc.cpp b/gnc/matlab/code_generation/sharedutils/ohdbaiekngdjbaaa_xdotc.cpp deleted file mode 100644 index 9d77ca3d2f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdbaiekngdjbaaa_xdotc.cpp +++ /dev/null @@ -1,37 +0,0 @@ -// -// File: ohdbaiekngdjbaaa_xdotc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ohdbaiekngdjbaaa_xdotc.h" - -// Function for MATLAB Function: '/compute_of_global_points' -real32_T ohdbaiekngdjbaaa_xdotc(int32_T n, const real32_T x_data[], int32_T ix0, - const real32_T y_data[], int32_T iy0) -{ - real32_T d; - int32_T ix; - int32_T iy; - int32_T k; - d = 0.0F; - ix = ix0; - iy = iy0; - for (k = 1; k <= n; k++) { - d += x_data[(int32_T)(ix - 1)] * y_data[(int32_T)(iy - 1)]; - ix++; - iy++; - } - - return d; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdbaiekngdjbaaa_xdotc.h b/gnc/matlab/code_generation/sharedutils/ohdbaiekngdjbaaa_xdotc.h deleted file mode 100644 index 1b154cc7f3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdbaiekngdjbaaa_xdotc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ohdbaiekngdjbaaa_xdotc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ohdbaiekngdjbaaa_xdotc -#define SHARE_ohdbaiekngdjbaaa_xdotc -#include "rtwtypes.h" - -extern real32_T ohdbaiekngdjbaaa_xdotc(int32_T n, const real32_T x_data[], - int32_T ix0, const real32_T y_data[], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdbhlnomohlcjek_xgeqrf.cpp b/gnc/matlab/code_generation/sharedutils/ohdbhlnomohlcjek_xgeqrf.cpp deleted file mode 100644 index da7ddeef98..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdbhlnomohlcjek_xgeqrf.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// -// File: ohdbhlnomohlcjek_xgeqrf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "fcbagdbapphliecj_xzlarfg.h" -#include "ophdecbiohdblnoh_xzlarf.h" -#include "ohdbhlnomohlcjek_xgeqrf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void ohdbhlnomohlcjek_xgeqrf(real32_T A_data[], int32_T A_sizes[2], real32_T - tau_data[], int32_T *tau_sizes) -{ - int32_T m; - int32_T n; - int32_T mn; - int32_T i_i; - int32_T mmi; - real32_T b; - real32_T b_atmp; - int32_T loop_ub; - real32_T work_data[150]; - int32_T work_sizes; - uint8_T c_idx_0; - m = A_sizes[0]; - n = A_sizes[1]; - work_sizes = A_sizes[0]; - mn = A_sizes[1]; - if (work_sizes <= mn) { - mn = work_sizes; - } - - *tau_sizes = (int32_T)(uint8_T)mn; - if (!((A_sizes[0] == 0) || (A_sizes[1] == 0))) { - c_idx_0 = (uint8_T)A_sizes[1]; - work_sizes = (int32_T)c_idx_0; - loop_ub = (int32_T)c_idx_0; - for (i_i = 0; i_i <= (int32_T)(loop_ub - 1); i_i++) { - work_data[i_i] = 0.0F; - } - - for (loop_ub = 0; (int32_T)(loop_ub + 1) <= mn; loop_ub++) { - i_i = (int32_T)((int32_T)(loop_ub * m) + loop_ub); - mmi = (int32_T)(m - loop_ub); - if ((int32_T)(loop_ub + 1) < m) { - b_atmp = A_data[i_i]; - b = fcbagdbapphliecj_xzlarfg(mmi, &b_atmp, A_data, (int32_T)(i_i + 2)); - tau_data[loop_ub] = b; - A_data[i_i] = b_atmp; - } else { - tau_data[loop_ub] = 0.0F; - } - - if ((int32_T)(loop_ub + 1) < n) { - b_atmp = A_data[i_i]; - A_data[i_i] = 1.0F; - ophdecbiohdblnoh_xzlarf(mmi, (int32_T)((int32_T)(n - loop_ub) - 1), - (int32_T)(i_i + 1), tau_data[loop_ub], A_data, (int32_T)((int32_T) - (loop_ub + (int32_T)((int32_T)(loop_ub + 1) * m)) + 1), m, work_data); - A_data[i_i] = b_atmp; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdbhlnomohlcjek_xgeqrf.h b/gnc/matlab/code_generation/sharedutils/ohdbhlnomohlcjek_xgeqrf.h deleted file mode 100644 index 144009a807..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdbhlnomohlcjek_xgeqrf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ohdbhlnomohlcjek_xgeqrf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ohdbhlnomohlcjek_xgeqrf -#define SHARE_ohdbhlnomohlcjek_xgeqrf -#include "rtwtypes.h" - -extern void ohdbhlnomohlcjek_xgeqrf(real32_T A_data[], int32_T A_sizes[2], - real32_T tau_data[], int32_T *tau_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdbiekfdjechlno_rows_differ.cpp b/gnc/matlab/code_generation/sharedutils/ohdbiekfdjechlno_rows_differ.cpp deleted file mode 100644 index a082dc19e0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdbiekfdjechlno_rows_differ.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// -// File: ohdbiekfdjechlno_rows_differ.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "ohdbiekfdjechlno_rows_differ.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T ohdbiekfdjechlno_rows_differ(const real32_T b_data[], const int32_T - b_sizes[2], int32_T k0, int32_T k) -{ - boolean_T p; - int32_T j; - boolean_T b_p; - real32_T absxk; - int32_T exponent; - boolean_T exitg1; - p = false; - j = 0; - exitg1 = false; - while ((!exitg1) && (j < 2)) { - absxk = (real32_T)fabs((real_T)(b_data[(int32_T)((int32_T)((int32_T) - (b_sizes[0] * j) + k) - 1)] / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, (int32_T)(exponent - 24)); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(b_data[(int32_T)((int32_T)((int32_T)(b_sizes[0] - * j) + k) - 1)] - b_data[(int32_T)((int32_T)((int32_T)(b_sizes[0] * - j) + k0) - 1)])) < absxk) || (rtIsInfF(b_data[(int32_T)((int32_T) - ((int32_T)(b_sizes[0] * j) + k0) - 1)]) && rtIsInfF(b_data[(int32_T) - ((int32_T)((int32_T)(b_sizes[0] * j) + k) - 1)]) && ((b_data[(int32_T) - ((int32_T)((int32_T)(b_sizes[0] * j) + k0) - 1)] > 0.0F) == (b_data - [(int32_T)((int32_T)((int32_T)(b_sizes[0] * j) + k) - 1)] > 0.0F)))) - { - b_p = true; - } else { - b_p = false; - } - - if (!b_p) { - p = true; - exitg1 = true; - } else { - j++; - } - } - - return p; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdbiekfdjechlno_rows_differ.h b/gnc/matlab/code_generation/sharedutils/ohdbiekfdjechlno_rows_differ.h deleted file mode 100644 index 5b74267b13..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdbiekfdjechlno_rows_differ.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ohdbiekfdjechlno_rows_differ.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_ohdbiekfdjechlno_rows_differ -#define SHARE_ohdbiekfdjechlno_rows_differ -#include "rtwtypes.h" - -extern boolean_T ohdbiekfdjechlno_rows_differ(const real32_T b_data[], const - int32_T b_sizes[2], int32_T k0, int32_T k); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdjbaimcjmojecj_rows_differ.cpp b/gnc/matlab/code_generation/sharedutils/ohdjbaimcjmojecj_rows_differ.cpp deleted file mode 100644 index a149388abd..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdjbaimcjmojecj_rows_differ.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// -// File: ohdjbaimcjmojecj_rows_differ.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include -#include "ohdjbaimcjmojecj_rows_differ.h" - -// Function for MATLAB Function: '/generate_output' -boolean_T ohdjbaimcjmojecj_rows_differ(const real32_T b_data[], const int32_T - b_sizes[2], int32_T k0, int32_T k) -{ - boolean_T p; - int32_T j; - real32_T absxk; - int32_T exponent; - boolean_T exitg1; - p = false; - j = 0; - exitg1 = false; - while ((!exitg1) && (j < 2)) { - absxk = (real32_T)fabs((real_T)(b_data[(b_sizes[0] * j + k) - 1] / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, exponent - 24); - } - } else { - absxk = (rtNaNF); - } - - if (!(((real32_T)fabs((real_T)(b_data[(b_sizes[0] * j + k) - 1] - b_data - [(b_sizes[0] * j + k0) - 1])) < absxk) || (rtIsInfF(b_data[(b_sizes - [0] * j + k0) - 1]) && rtIsInfF(b_data[(b_sizes[0] * j + k) - 1]) && - ((b_data[(b_sizes[0] * j + k0) - 1] > 0.0F) == (b_data[(b_sizes[0] * j - + k) - 1] > 0.0F))))) { - p = true; - exitg1 = true; - } else { - j++; - } - } - - return p; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdjbaimcjmojecj_rows_differ.h b/gnc/matlab/code_generation/sharedutils/ohdjbaimcjmojecj_rows_differ.h deleted file mode 100644 index a0c1d8e6b1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdjbaimcjmojecj_rows_differ.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ohdjbaimcjmojecj_rows_differ.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:14:10 2017 -// -#ifndef SHARE_ohdjbaimcjmojecj_rows_differ -#define SHARE_ohdjbaimcjmojecj_rows_differ -#include "rtwtypes.h" - -extern boolean_T ohdjbaimcjmojecj_rows_differ(const real32_T b_data[], const - int32_T b_sizes[2], int32_T k0, int32_T k); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdjhlfcnohdhdba_xscal.cpp b/gnc/matlab/code_generation/sharedutils/ohdjhlfcnohdhdba_xscal.cpp deleted file mode 100644 index 45e3215479..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdjhlfcnohdhdba_xscal.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// -// File: ohdjhlfcnohdhdba_xscal.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ohdjhlfcnohdhdba_xscal.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void ohdjhlfcnohdhdba_xscal(int32_T n, real32_T a, real32_T x_data[], int32_T - ix0) -{ - int32_T b; - int32_T k; - b = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= b; k++) { - x_data[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohdjhlfcnohdhdba_xscal.h b/gnc/matlab/code_generation/sharedutils/ohdjhlfcnohdhdba_xscal.h deleted file mode 100644 index c52ecee7d6..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohdjhlfcnohdhdba_xscal.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ohdjhlfcnohdhdba_xscal.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ohdjhlfcnohdhdba_xscal -#define SHARE_ohdjhlfcnohdhdba_xscal -#include "rtwtypes.h" - -extern void ohdjhlfcnohdhdba_xscal(int32_T n, real32_T a, real32_T x_data[], - int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohlfeknojmopaimg_xscal.cpp b/gnc/matlab/code_generation/sharedutils/ohlfeknojmopaimg_xscal.cpp deleted file mode 100644 index 965edfd450..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohlfeknojmopaimg_xscal.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: ohlfeknojmopaimg_xscal.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "ohlfeknojmopaimg_xscal.h" - -// Function for MATLAB Function: '/MATLAB Function' -void ohlfeknojmopaimg_xscal(real32_T a, real32_T x[36], int32_T ix0) -{ - int32_T k; - for (k = ix0; k <= (int32_T)(ix0 + 5); k++) { - x[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohlfeknojmopaimg_xscal.h b/gnc/matlab/code_generation/sharedutils/ohlfeknojmopaimg_xscal.h deleted file mode 100644 index cbca31c6cc..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohlfeknojmopaimg_xscal.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: ohlfeknojmopaimg_xscal.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_ohlfeknojmopaimg_xscal -#define SHARE_ohlfeknojmopaimg_xscal -#include "rtwtypes.h" - -extern void ohlfeknojmopaimg_xscal(real32_T a, real32_T x[36], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohlfohlnbiekcjmg_xgerc.cpp b/gnc/matlab/code_generation/sharedutils/ohlfohlnbiekcjmg_xgerc.cpp deleted file mode 100644 index 691400c415..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohlfohlnbiekcjmg_xgerc.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// -// File: ohlfohlnbiekcjmg_xgerc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ohlfohlnbiekcjmg_xgerc.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void ohlfohlnbiekcjmg_xgerc(int32_T m, int32_T n, real32_T alpha1, int32_T ix0, - const real32_T y_data[], real32_T A_data[], int32_T ia0, int32_T lda) -{ - int32_T jA; - int32_T jy; - real32_T temp; - int32_T ix; - int32_T j; - int32_T b; - int32_T ijA; - if (!(alpha1 == 0.0F)) { - jA = (int32_T)(ia0 - 1); - jy = 0; - for (j = 1; j <= n; j++) { - if (y_data[jy] != 0.0F) { - temp = y_data[jy] * alpha1; - ix = ix0; - b = (int32_T)(m + jA); - for (ijA = jA; (int32_T)(ijA + 1) <= b; ijA++) { - A_data[ijA] += A_data[(int32_T)(ix - 1)] * temp; - ix++; - } - } - - jy++; - jA += lda; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohlfohlnbiekcjmg_xgerc.h b/gnc/matlab/code_generation/sharedutils/ohlfohlnbiekcjmg_xgerc.h deleted file mode 100644 index 364b414e64..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohlfohlnbiekcjmg_xgerc.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: ohlfohlnbiekcjmg_xgerc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ohlfohlnbiekcjmg_xgerc -#define SHARE_ohlfohlnbiekcjmg_xgerc -#include "rtwtypes.h" - -extern void ohlfohlnbiekcjmg_xgerc(int32_T m, int32_T n, real32_T alpha1, - int32_T ix0, const real32_T y_data[], real32_T A_data[], int32_T ia0, int32_T - lda); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohlnbaaanoppohdb_cat.cpp b/gnc/matlab/code_generation/sharedutils/ohlnbaaanoppohdb_cat.cpp deleted file mode 100644 index 49f3d55a46..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohlnbaaanoppohdb_cat.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// File: ohlnbaaanoppohdb_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:43:55 2017 -// -#include "rtwtypes.h" -#include "ohlnbaaanoppohdb_cat.h" - -// Function for MATLAB Function: '/generate_output' -void ohlnbaaanoppohdb_cat(const real32_T varargin_2_data[], const int32_T - varargin_2_sizes[3], const real32_T varargin_3_data[], const int32_T - varargin_3_sizes[3], real32_T y_data[], int32_T y_sizes[3]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = 50; - y_sizes[1] = 2; - y_sizes[2] = 16; - iy = -1; - for (j = 0; j < 100; j++) { - iy++; - y_data[iy] = 0.0F; - } - - j = (int32_T)(100 * varargin_2_sizes[2]); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_2_data[(int32_T)(b_j - 1)]; - } - - j = (int32_T)(100 * varargin_3_sizes[2]); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_3_data[(int32_T)(b_j - 1)]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ohlnbaaanoppohdb_cat.h b/gnc/matlab/code_generation/sharedutils/ohlnbaaanoppohdb_cat.h deleted file mode 100644 index 4fbcaa6486..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ohlnbaaanoppohdb_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: ohlnbaaanoppohdb_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Wed May 17 14:43:55 2017 -// -#ifndef SHARE_ohlnbaaanoppohdb_cat -#define SHARE_ohlnbaaanoppohdb_cat -#include "rtwtypes.h" - -extern void ohlnbaaanoppohdb_cat(const real32_T varargin_2_data[], const int32_T - varargin_2_sizes[3], const real32_T varargin_3_data[], const int32_T - varargin_3_sizes[3], real32_T y_data[], int32_T y_sizes[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophdbaimngdjphln_quat_rotation_vec.cpp b/gnc/matlab/code_generation/sharedutils/ophdbaimngdjphln_quat_rotation_vec.cpp deleted file mode 100644 index 8de8555cd8..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophdbaimngdjphln_quat_rotation_vec.cpp +++ /dev/null @@ -1,136 +0,0 @@ -// -// File: ophdbaimngdjphln_quat_rotation_vec.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Dec 4 08:33:06 2017 -// -#include "rtwtypes.h" -#include "ophdbaimngdjphln_quat_rotation_vec.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void ophdbaimngdjphln_quat_rotation_vec(real_T vector[3], const real32_T Q[4], - real32_T vec_out[3]) -{ - real32_T qOut_idx_3; - real32_T qOut_idx_1; - real32_T qOut_idx_2; - real32_T qOut_idx_0; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // - // Q = q_a2b, rotation represents a vector rotating from frame b2a - // q_out = quat_rotation(vector, Q2) - // - // rotates a row-vector of 3-vectors by a row-vector of quaternion Q2 - // - // From: Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra - // Equation below is from Eq. 77, with the the quaternions inverted because - // our quat multiplication convention has the reverse order from the - // mulitplication used in the paper - // - // Check to see if the vector is a single row - // Vec_out = Q * [V 0]' * Q^-1 - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - qOut_idx_0 = (0.0F * Q[0] + Q[3] * (real32_T)vector[0]) + (Q[1] * (real32_T) - vector[2] - Q[2] * (real32_T)vector[1]); - qOut_idx_1 = (0.0F * Q[1] + Q[3] * (real32_T)vector[1]) + (Q[2] * (real32_T) - vector[0] - Q[0] * (real32_T)vector[2]); - qOut_idx_2 = (0.0F * Q[2] + Q[3] * (real32_T)vector[2]) + (Q[0] * (real32_T) - vector[1] - Q[1] * (real32_T)vector[0]); - qOut_idx_3 = Q[3] * 0.0F - ((Q[0] * (real32_T)vector[0] + Q[1] * (real32_T) - vector[1]) + Q[2] * (real32_T)vector[2]); - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - vec_out[0] = (Q[3] * qOut_idx_0 + qOut_idx_3 * -Q[0]) + (qOut_idx_1 * -Q[2] - - qOut_idx_2 * -Q[1]); - vec_out[1] = (Q[3] * qOut_idx_1 + qOut_idx_3 * -Q[1]) + (qOut_idx_2 * -Q[0] - - qOut_idx_0 * -Q[2]); - vec_out[2] = (Q[3] * qOut_idx_2 + qOut_idx_3 * -Q[2]) + (qOut_idx_0 * -Q[1] - - qOut_idx_1 * -Q[0]); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophdbaimngdjphln_quat_rotation_vec.h b/gnc/matlab/code_generation/sharedutils/ophdbaimngdjphln_quat_rotation_vec.h deleted file mode 100644 index 7755ed1ba3..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophdbaimngdjphln_quat_rotation_vec.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ophdbaimngdjphln_quat_rotation_vec.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Dec 4 08:33:06 2017 -// -#ifndef SHARE_ophdbaimngdjphln_quat_rotation_vec -#define SHARE_ophdbaimngdjphln_quat_rotation_vec -#include "rtwtypes.h" - -extern void ophdbaimngdjphln_quat_rotation_vec(real_T vector[3], const real32_T - Q[4], real32_T vec_out[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophdecbiohdblnoh_xzlarf.cpp b/gnc/matlab/code_generation/sharedutils/ophdecbiohdblnoh_xzlarf.cpp deleted file mode 100644 index 854ecb3c07..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophdecbiohdblnoh_xzlarf.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// File: ophdecbiohdblnoh_xzlarf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "imopjmglppphcbai_xgemv.h" -#include "knohaiececjedbai_ilazlc.h" -#include "ohlfohlnbiekcjmg_xgerc.h" -#include "ophdecbiohdblnoh_xzlarf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void ophdecbiohdblnoh_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T tau, - real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work_data[]) -{ - int32_T lastv; - int32_T lastc; - if (tau != 0.0F) { - lastv = m; - lastc = (int32_T)(iv0 + m); - while ((lastv > 0) && (C_data[(int32_T)(lastc - 2)] == 0.0F)) { - lastv--; - lastc--; - } - - lastc = knohaiececjedbai_ilazlc(lastv, n, C_data, ic0, ldc); - } else { - lastv = 0; - lastc = 0; - } - - if (lastv > 0) { - imopjmglppphcbai_xgemv(lastv, lastc, C_data, ic0, ldc, C_data, iv0, - work_data); - ohlfohlnbiekcjmg_xgerc(lastv, lastc, -tau, iv0, work_data, C_data, ic0, ldc); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophdecbiohdblnoh_xzlarf.h b/gnc/matlab/code_generation/sharedutils/ophdecbiohdblnoh_xzlarf.h deleted file mode 100644 index 1a5c7dde53..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophdecbiohdblnoh_xzlarf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ophdecbiohdblnoh_xzlarf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ophdecbiohdblnoh_xzlarf -#define SHARE_ophdecbiohdblnoh_xzlarf -#include "rtwtypes.h" - -extern void ophdecbiohdblnoh_xzlarf(int32_T m, int32_T n, int32_T iv0, real32_T - tau, real32_T C_data[], int32_T ic0, int32_T ldc, real32_T work_data[]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophdimgdcjekekfc_xrot.cpp b/gnc/matlab/code_generation/sharedutils/ophdimgdcjekekfc_xrot.cpp deleted file mode 100644 index af73d2cff9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophdimgdcjekekfc_xrot.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: ophdimgdcjekekfc_xrot.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "ophdimgdcjekekfc_xrot.h" - -// Function for MATLAB Function: '/MATLAB Function' -void ophdimgdcjekekfc_xrot(real32_T x[36], int32_T ix0, int32_T iy0, real32_T c, - real32_T s) -{ - int32_T ix; - int32_T iy; - real32_T temp; - int32_T k; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k < 12; k++) { - temp = c * x[ix] + s * x[iy]; - x[iy] = c * x[iy] - s * x[ix]; - x[ix] = temp; - iy++; - ix++; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophdimgdcjekekfc_xrot.h b/gnc/matlab/code_generation/sharedutils/ophdimgdcjekekfc_xrot.h deleted file mode 100644 index 42115185ed..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophdimgdcjekekfc_xrot.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ophdimgdcjekekfc_xrot.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_ophdimgdcjekekfc_xrot -#define SHARE_ophdimgdcjekekfc_xrot -#include "rtwtypes.h" - -extern void ophdimgdcjekekfc_xrot(real32_T x[36], int32_T ix0, int32_T iy0, - real32_T c, real32_T s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophdjekfieknglfk_xswap.cpp b/gnc/matlab/code_generation/sharedutils/ophdjekfieknglfk_xswap.cpp deleted file mode 100644 index 5ba555de60..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophdjekfieknglfk_xswap.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: ophdjekfieknglfk_xswap.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ophdjekfieknglfk_xswap.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void ophdjekfieknglfk_xswap(int32_T n, real32_T x_data[], int32_T ix0, int32_T - iy0) -{ - int32_T ix; - int32_T iy; - real32_T temp; - int32_T k; - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 1; k <= n; k++) { - temp = x_data[ix]; - x_data[ix] = x_data[iy]; - x_data[iy] = temp; - ix++; - iy++; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophdjekfieknglfk_xswap.h b/gnc/matlab/code_generation/sharedutils/ophdjekfieknglfk_xswap.h deleted file mode 100644 index d61e06912c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophdjekfieknglfk_xswap.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ophdjekfieknglfk_xswap.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ophdjekfieknglfk_xswap -#define SHARE_ophdjekfieknglfk_xswap -#include "rtwtypes.h" - -extern void ophdjekfieknglfk_xswap(int32_T n, real32_T x_data[], int32_T ix0, - int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophlaaaiophdbaai_xscal.cpp b/gnc/matlab/code_generation/sharedutils/ophlaaaiophdbaai_xscal.cpp deleted file mode 100644 index 9066171f38..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophlaaaiophdbaai_xscal.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// File: ophlaaaiophdbaai_xscal.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ophlaaaiophdbaai_xscal.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void ophlaaaiophdbaai_xscal(real32_T a, real32_T x[9], int32_T ix0) -{ - int32_T k; - for (k = ix0; k <= (int32_T)(ix0 + 2); k++) { - x[(int32_T)(k - 1)] *= a; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophlaaaiophdbaai_xscal.h b/gnc/matlab/code_generation/sharedutils/ophlaaaiophdbaai_xscal.h deleted file mode 100644 index 8b8a010e92..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophlaaaiophdbaai_xscal.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: ophlaaaiophdbaai_xscal.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ophlaaaiophdbaai_xscal -#define SHARE_ophlaaaiophdbaai_xscal -#include "rtwtypes.h" - -extern void ophlaaaiophdbaai_xscal(real32_T a, real32_T x[9], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophlcjmgkfcbmohl_nullAssignment.cpp b/gnc/matlab/code_generation/sharedutils/ophlcjmgkfcbmohl_nullAssignment.cpp deleted file mode 100644 index 5cf4e81220..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophlcjmgkfcbmohl_nullAssignment.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// -// File: ophlcjmgkfcbmohl_nullAssignment.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ophlcjmgkfcbmohl_nullAssignment.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void ophlcjmgkfcbmohl_nullAssignment(real_T x_data[], int32_T *x_sizes, const - boolean_T idx_data[], const int32_T idx_sizes) -{ - int32_T nxin; - int32_T nxout; - int32_T k0; - int32_T k; - real_T x_data_0[100]; - nxin = *x_sizes; - nxout = 0; - for (k0 = 1; k0 <= idx_sizes; k0++) { - nxout += (int32_T)idx_data[(int32_T)(k0 - 1)]; - } - - nxout = (int32_T)(*x_sizes - nxout); - k0 = -1; - for (k = 1; k <= nxin; k++) { - if ((k > idx_sizes) || (!idx_data[(int32_T)(k - 1)])) { - k0++; - x_data[k0] = x_data[(int32_T)(k - 1)]; - } - } - - if (1 > nxout) { - nxout = 0; - } - - for (k0 = 0; k0 <= (int32_T)(nxout - 1); k0++) { - x_data_0[k0] = x_data[k0]; - } - - *x_sizes = nxout; - for (k0 = 0; k0 <= (int32_T)(nxout - 1); k0++) { - x_data[k0] = x_data_0[k0]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophlcjmgkfcbmohl_nullAssignment.h b/gnc/matlab/code_generation/sharedutils/ophlcjmgkfcbmohl_nullAssignment.h deleted file mode 100644 index 9d35926c0a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophlcjmgkfcbmohl_nullAssignment.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ophlcjmgkfcbmohl_nullAssignment.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ophlcjmgkfcbmohl_nullAssignment -#define SHARE_ophlcjmgkfcbmohl_nullAssignment -#include "rtwtypes.h" - -extern void ophlcjmgkfcbmohl_nullAssignment(real_T x_data[], int32_T *x_sizes, - const boolean_T idx_data[], const int32_T idx_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophlngdbgdjmgdje_cat.cpp b/gnc/matlab/code_generation/sharedutils/ophlngdbgdjmgdje_cat.cpp deleted file mode 100644 index b38857c18e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophlngdbgdjmgdje_cat.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// -// File: ophlngdbgdjmgdje_cat.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "ophlngdbgdjmgdje_cat.h" - -// Function for MATLAB Function: '/generate_output' -void ophlngdbgdjmgdje_cat(const real32_T varargin_2_data[], const int32_T - varargin_2_sizes[3], const real32_T varargin_3_data[], const int32_T - varargin_3_sizes[3], real32_T y_data[], int32_T y_sizes[3]) -{ - int32_T iy; - int32_T j; - int32_T b_j; - y_sizes[0] = 50; - y_sizes[1] = 2; - y_sizes[2] = 16; - iy = -1; - for (j = 0; j < 100; j++) { - iy++; - y_data[iy] = 0.0F; - } - - j = (int32_T)(100 * varargin_2_sizes[2]); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_2_data[(int32_T)(b_j - 1)]; - } - - j = (int32_T)(100 * varargin_3_sizes[2]); - for (b_j = 1; b_j <= j; b_j++) { - iy++; - y_data[iy] = varargin_3_data[(int32_T)(b_j - 1)]; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ophlngdbgdjmgdje_cat.h b/gnc/matlab/code_generation/sharedutils/ophlngdbgdjmgdje_cat.h deleted file mode 100644 index fa40d2df56..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ophlngdbgdjmgdje_cat.h +++ /dev/null @@ -1,24 +0,0 @@ -// -// File: ophlngdbgdjmgdje_cat.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_ophlngdbgdjmgdje_cat -#define SHARE_ophlngdbgdjmgdje_cat -#include "rtwtypes.h" - -extern void ophlngdbgdjmgdje_cat(const real32_T varargin_2_data[], const int32_T - varargin_2_sizes[3], const real32_T varargin_3_data[], const int32_T - varargin_3_sizes[3], real32_T y_data[], int32_T y_sizes[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/opphjmohhlnodjmo_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/opphjmohhlnodjmo_xaxpy.cpp deleted file mode 100644 index cb85016838..0000000000 --- a/gnc/matlab/code_generation/sharedutils/opphjmohhlnodjmo_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: opphjmohhlnodjmo_xaxpy.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "opphjmohhlnodjmo_xaxpy.h" - -// Function for MATLAB Function: '/MATLAB Function' -void opphjmohhlnodjmo_xaxpy(int32_T n, real32_T a, const real32_T x[12], int32_T - ix0, real32_T y[72], int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * x[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/opphjmohhlnodjmo_xaxpy.h b/gnc/matlab/code_generation/sharedutils/opphjmohhlnodjmo_xaxpy.h deleted file mode 100644 index 6af56a7270..0000000000 --- a/gnc/matlab/code_generation/sharedutils/opphjmohhlnodjmo_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: opphjmohhlnodjmo_xaxpy.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_opphjmohhlnodjmo_xaxpy -#define SHARE_opphjmohhlnodjmo_xaxpy -#include "rtwtypes.h" - -extern void opphjmohhlnodjmo_xaxpy(int32_T n, real32_T a, const real32_T x[12], - int32_T ix0, real32_T y[72], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/opppmglfglfkdbaa_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/opppmglfglfkdbaa_xaxpy.cpp deleted file mode 100644 index 1e9dc8c170..0000000000 --- a/gnc/matlab/code_generation/sharedutils/opppmglfglfkdbaa_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: opppmglfglfkdbaa_xaxpy.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "opppmglfglfkdbaa_xaxpy.h" - -// Function for MATLAB Function: '/MATLAB Function' -void opppmglfglfkdbaa_xaxpy(int32_T n, real32_T a, const real32_T x[36], int32_T - ix0, real32_T y[12], int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * x[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/opppmglfglfkdbaa_xaxpy.h b/gnc/matlab/code_generation/sharedutils/opppmglfglfkdbaa_xaxpy.h deleted file mode 100644 index c5a838bce5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/opppmglfglfkdbaa_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: opppmglfglfkdbaa_xaxpy.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_opppmglfglfkdbaa_xaxpy -#define SHARE_opppmglfglfkdbaa_xaxpy -#include "rtwtypes.h" - -extern void opppmglfglfkdbaa_xaxpy(int32_T n, real32_T a, const real32_T x[36], - int32_T ix0, real32_T y[12], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/opppnohdmglfjekf_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/opppnohdmglfjekf_xaxpy.cpp deleted file mode 100644 index c24e6ff6bf..0000000000 --- a/gnc/matlab/code_generation/sharedutils/opppnohdmglfjekf_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: opppnohdmglfjekf_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "opppnohdmglfjekf_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void opppnohdmglfjekf_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T y_data[], - int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y_data[iy] += a * y_data[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/opppnohdmglfjekf_xaxpy.h b/gnc/matlab/code_generation/sharedutils/opppnohdmglfjekf_xaxpy.h deleted file mode 100644 index 983e0e246e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/opppnohdmglfjekf_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: opppnohdmglfjekf_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_opppnohdmglfjekf_xaxpy -#define SHARE_opppnohdmglfjekf_xaxpy -#include "rtwtypes.h" - -extern void opppnohdmglfjekf_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T - y_data[], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/phdjbaaiglngbaaa_xzlarfg.cpp b/gnc/matlab/code_generation/sharedutils/phdjbaaiglngbaaa_xzlarfg.cpp deleted file mode 100644 index 30f5c15040..0000000000 --- a/gnc/matlab/code_generation/sharedutils/phdjbaaiglngbaaa_xzlarfg.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -// File: phdjbaaiglngbaaa_xzlarfg.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "mglnnophaaimmgln_xnrm2.h" -#include "rt_hypotf_snf.h" -#include "phdjbaaiglngbaaa_xzlarfg.h" - -// Function for MATLAB Function: '/Compute Residual and H' -real32_T phdjbaaiglngbaaa_xzlarfg(int32_T n, real32_T *alpha1, real32_T x_data[], - int32_T ix0) -{ - real32_T tau; - real32_T xnorm; - int32_T knt; - int32_T b_k; - int32_T c_k; - tau = 0.0F; - if (!(n <= 0)) { - xnorm = mglnnophaaimmgln_xnrm2((int32_T)(n - 1), x_data, ix0); - if (xnorm != 0.0F) { - xnorm = rt_hypotf_snf(*alpha1, xnorm); - if (*alpha1 >= 0.0F) { - xnorm = -xnorm; - } - - if ((real32_T)fabs((real_T)xnorm) < 9.86076132E-32F) { - knt = 0; - do { - knt++; - b_k = (int32_T)((int32_T)(ix0 + n) - 2); - for (c_k = ix0; c_k <= b_k; c_k++) { - x_data[(int32_T)(c_k - 1)] *= 1.01412048E+31F; - } - - xnorm *= 1.01412048E+31F; - *alpha1 *= 1.01412048E+31F; - } while (!((real32_T)fabs((real_T)xnorm) >= 9.86076132E-32F)); - - xnorm = rt_hypotf_snf(*alpha1, mglnnophaaimmgln_xnrm2((int32_T)(n - 1), - x_data, ix0)); - if (*alpha1 >= 0.0F) { - xnorm = -xnorm; - } - - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - b_k = (int32_T)((int32_T)(ix0 + n) - 2); - for (c_k = ix0; c_k <= b_k; c_k++) { - x_data[(int32_T)(c_k - 1)] *= *alpha1; - } - - for (b_k = 1; b_k <= knt; b_k++) { - xnorm *= 9.86076132E-32F; - } - - *alpha1 = xnorm; - } else { - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - knt = (int32_T)((int32_T)(ix0 + n) - 2); - for (b_k = ix0; b_k <= knt; b_k++) { - x_data[(int32_T)(b_k - 1)] *= *alpha1; - } - - *alpha1 = xnorm; - } - } - } - - return tau; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/phdjbaaiglngbaaa_xzlarfg.h b/gnc/matlab/code_generation/sharedutils/phdjbaaiglngbaaa_xzlarfg.h deleted file mode 100644 index 3db4f62003..0000000000 --- a/gnc/matlab/code_generation/sharedutils/phdjbaaiglngbaaa_xzlarfg.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: phdjbaaiglngbaaa_xzlarfg.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_phdjbaaiglngbaaa_xzlarfg -#define SHARE_phdjbaaiglngbaaa_xzlarfg -#include "rtwtypes.h" - -extern real32_T phdjbaaiglngbaaa_xzlarfg(int32_T n, real32_T *alpha1, real32_T - x_data[], int32_T ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/phlfglfcdbaajmgd_xswap.cpp b/gnc/matlab/code_generation/sharedutils/phlfglfcdbaajmgd_xswap.cpp deleted file mode 100644 index c45d5f9802..0000000000 --- a/gnc/matlab/code_generation/sharedutils/phlfglfcdbaajmgd_xswap.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// -// File: phlfglfcdbaajmgd_xswap.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "phlfglfcdbaajmgd_xswap.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void phlfglfcdbaajmgd_xswap(real32_T x_data[]) -{ - real32_T temp; - temp = x_data[0]; - x_data[0] = x_data[3]; - x_data[3] = temp; - temp = x_data[1]; - x_data[1] = x_data[4]; - x_data[4] = temp; - temp = x_data[2]; - x_data[2] = x_data[5]; - x_data[5] = temp; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/phlfglfcdbaajmgd_xswap.h b/gnc/matlab/code_generation/sharedutils/phlfglfcdbaajmgd_xswap.h deleted file mode 100644 index ea979449e6..0000000000 --- a/gnc/matlab/code_generation/sharedutils/phlfglfcdbaajmgd_xswap.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: phlfglfcdbaajmgd_xswap.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_phlfglfcdbaajmgd_xswap -#define SHARE_phlfglfcdbaajmgd_xswap -#include "rtwtypes.h" - -extern void phlfglfcdbaajmgd_xswap(real32_T x_data[]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/phlniekfphlndjmo_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/phlniekfphlndjmo_xaxpy.cpp deleted file mode 100644 index a53f560e99..0000000000 --- a/gnc/matlab/code_generation/sharedutils/phlniekfphlndjmo_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: phlniekfphlndjmo_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "phlniekfphlndjmo_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void phlniekfphlndjmo_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T y[16], - int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!((n < 1) || (a == 0.0F))) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y[iy] += a * y[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/phlniekfphlndjmo_xaxpy.h b/gnc/matlab/code_generation/sharedutils/phlniekfphlndjmo_xaxpy.h deleted file mode 100644 index 464736d5a1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/phlniekfphlndjmo_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: phlniekfphlndjmo_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_phlniekfphlndjmo_xaxpy -#define SHARE_phlniekfphlndjmo_xaxpy -#include "rtwtypes.h" - -extern void phlniekfphlndjmo_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T - y[16], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/phlnlfkfphdjohln_merge.cpp b/gnc/matlab/code_generation/sharedutils/phlnlfkfphdjohln_merge.cpp deleted file mode 100644 index 0cf85c2c6c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/phlnlfkfphdjohln_merge.cpp +++ /dev/null @@ -1,68 +0,0 @@ -// -// File: phlnlfkfphdjohln_merge.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "phlnlfkfphdjohln_merge.h" - -// Function for MATLAB Function: '/generate_output' -void phlnlfkfphdjohln_merge(int32_T idx[50], real32_T x[50], int32_T offset, - int32_T np, int32_T nq, int32_T iwork[50], real32_T xwork[50]) -{ - int32_T n; - int32_T q; - int32_T qend; - int32_T iout; - int32_T exitg1; - if (nq != 0) { - n = (int32_T)(np + nq); - for (q = 0; (int32_T)(q + 1) <= n; q++) { - iwork[q] = idx[(int32_T)(offset + q)]; - xwork[q] = x[(int32_T)(offset + q)]; - } - - n = 0; - q = np; - qend = (int32_T)(np + nq); - iout = (int32_T)(offset - 1); - do { - exitg1 = 0; - iout++; - if (xwork[n] <= xwork[q]) { - idx[iout] = iwork[n]; - x[iout] = xwork[n]; - if ((int32_T)(n + 1) < np) { - n++; - } else { - exitg1 = 1; - } - } else { - idx[iout] = iwork[q]; - x[iout] = xwork[q]; - if ((int32_T)(q + 1) < qend) { - q++; - } else { - q = (int32_T)((int32_T)(iout - n) + 1); - while ((int32_T)(n + 1) <= np) { - idx[(int32_T)(q + n)] = iwork[n]; - x[(int32_T)(q + n)] = xwork[n]; - n++; - } - - exitg1 = 1; - } - } - } while (exitg1 == 0); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/phlnlfkfphdjohln_merge.h b/gnc/matlab/code_generation/sharedutils/phlnlfkfphdjohln_merge.h deleted file mode 100644 index 698196440d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/phlnlfkfphdjohln_merge.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: phlnlfkfphdjohln_merge.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_phlnlfkfphdjohln_merge -#define SHARE_phlnlfkfphdjohln_merge -#include "rtwtypes.h" - -extern void phlnlfkfphdjohln_merge(int32_T idx[50], real32_T x[50], int32_T - offset, int32_T np, int32_T nq, int32_T iwork[50], real32_T xwork[50]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/plook_u32f_binckpan.cpp b/gnc/matlab/code_generation/sharedutils/plook_u32f_binckpan.cpp deleted file mode 100644 index 7cda8e23f1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/plook_u32f_binckpan.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// File: plook_u32f_binckpan.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "binsearch_u32f_prevIdx.h" -#include "plook_u32f_binckpan.h" - -uint32_T plook_u32f_binckpan(real32_T u, const real32_T bp[], uint32_T maxIndex, - uint32_T *prevIndex) -{ - uint32_T bpIndex; - - // Prelookup - Index only - // Index Search method: 'binary' - // Interpolation method: 'Use nearest' - // Extrapolation method: 'Clip' - // Use previous index: 'on' - // Use last breakpoint for index at or above upper limit: 'on' - // Remove protection against out-of-range input in generated code: 'off' - - if (u <= bp[0U]) { - bpIndex = 0U; - } else if (u < bp[maxIndex]) { - bpIndex = binsearch_u32f_prevIdx(u, bp, *prevIndex, maxIndex); - if (bp[(uint32_T)(bpIndex + 1U)] - u <= u - bp[bpIndex]) { - bpIndex++; - } - } else { - bpIndex = maxIndex; - } - - *prevIndex = bpIndex; - return bpIndex; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/plook_u32f_binckpan.h b/gnc/matlab/code_generation/sharedutils/plook_u32f_binckpan.h deleted file mode 100644 index 2c82ae818a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/plook_u32f_binckpan.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: plook_u32f_binckpan.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_plook_u32f_binckpan -#define SHARE_plook_u32f_binckpan -#include "rtwtypes.h" - -extern uint32_T plook_u32f_binckpan(real32_T u, const real32_T bp[], uint32_T - maxIndex, uint32_T *prevIndex); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/pphdglfkecjmjekf_xrot.cpp b/gnc/matlab/code_generation/sharedutils/pphdglfkecjmjekf_xrot.cpp deleted file mode 100644 index b0ef05dd09..0000000000 --- a/gnc/matlab/code_generation/sharedutils/pphdglfkecjmjekf_xrot.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// -// File: pphdglfkecjmjekf_xrot.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "pphdglfkecjmjekf_xrot.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void pphdglfkecjmjekf_xrot(int32_T n, real32_T x_data[], int32_T ix0, int32_T - iy0, real32_T c, real32_T s) -{ - int32_T ix; - int32_T iy; - real32_T temp; - int32_T k; - if (!(n < 1)) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 1; k <= n; k++) { - temp = c * x_data[ix] + s * x_data[iy]; - x_data[iy] = c * x_data[iy] - s * x_data[ix]; - x_data[ix] = temp; - iy++; - ix++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/pphdglfkecjmjekf_xrot.h b/gnc/matlab/code_generation/sharedutils/pphdglfkecjmjekf_xrot.h deleted file mode 100644 index 221ae45e0f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/pphdglfkecjmjekf_xrot.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: pphdglfkecjmjekf_xrot.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_pphdglfkecjmjekf_xrot -#define SHARE_pphdglfkecjmjekf_xrot -#include "rtwtypes.h" - -extern void pphdglfkecjmjekf_xrot(int32_T n, real32_T x_data[], int32_T ix0, - int32_T iy0, real32_T c, real32_T s); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/pphlopppohlnphdb_pinv.cpp b/gnc/matlab/code_generation/sharedutils/pphlopppohlnphdb_pinv.cpp deleted file mode 100644 index 83bfca8ea4..0000000000 --- a/gnc/matlab/code_generation/sharedutils/pphlopppohlnphdb_pinv.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// -// File: pphlopppohlnphdb_pinv.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "lnohcjmoimglcbai_svd.h" -#include "pphlopppohlnphdb_pinv.h" - -// Function for MATLAB Function: '/MATLAB Function' -void pphlopppohlnphdb_pinv(const real32_T A[36], real32_T X[36]) -{ - real32_T b_X[36]; - real32_T b_A[36]; - real32_T V[9]; - int32_T vcol; - real32_T tol; - int32_T j; - real32_T U[36]; - real32_T S[9]; - boolean_T b_p; - int32_T ar; - int32_T ia; - int32_T b; - int32_T ib; - int32_T b_ic; - int32_T r; - for (r = 0; r < 3; r++) { - for (vcol = 0; vcol < 12; vcol++) { - b_A[(int32_T)(vcol + (int32_T)(12 * r))] = A[(int32_T)((int32_T)(3 * vcol) - + r)]; - } - } - - b_p = true; - for (r = 0; r < 36; r++) { - b_X[r] = 0.0F; - if (b_p && ((!rtIsInfF(b_A[r])) && (!rtIsNaNF(b_A[r])))) { - } else { - b_p = false; - } - } - - if (b_p) { - lnohcjmoimglcbai_svd(b_A, U, S, V); - } else { - for (r = 0; r < 36; r++) { - U[r] = (rtNaNF); - } - - for (r = 0; r < 9; r++) { - S[r] = 0.0F; - V[r] = (rtNaNF); - } - - S[0] = (rtNaNF); - S[4] = (rtNaNF); - S[8] = (rtNaNF); - } - - tol = 12.0F * S[0] * 1.1920929E-7F; - r = 0; - vcol = 0; - while (((int32_T)(vcol + 1) < 4) && (S[(int32_T)((int32_T)(3 * vcol) + vcol)] > - tol)) { - r++; - vcol++; - } - - if (r > 0) { - vcol = 0; - for (j = 0; (int32_T)(j + 1) <= r; j++) { - tol = 1.0F / S[(int32_T)((int32_T)(3 * j) + j)]; - for (ar = vcol; (int32_T)(ar + 1) <= (int32_T)(vcol + 3); ar++) { - V[ar] *= tol; - } - - vcol += 3; - } - - for (vcol = 0; vcol <= 34; vcol += 3) { - for (j = vcol; (int32_T)(j + 1) <= (int32_T)(vcol + 3); j++) { - b_X[j] = 0.0F; - } - } - - vcol = -1; - for (j = 0; j <= 34; j += 3) { - ar = -1; - vcol++; - b = (int32_T)((int32_T)((int32_T)((int32_T)(r - 1) * 12) + vcol) + 1); - for (ib = vcol; (int32_T)(ib + 1) <= b; ib += 12) { - if (U[ib] != 0.0F) { - ia = ar; - for (b_ic = j; (int32_T)(b_ic + 1) <= (int32_T)(j + 3); b_ic++) { - ia++; - b_X[b_ic] += U[ib] * V[ia]; - } - } - - ar += 3; - } - } - } - - for (r = 0; r < 3; r++) { - for (vcol = 0; vcol < 12; vcol++) { - X[(int32_T)(vcol + (int32_T)(12 * r))] = b_X[(int32_T)((int32_T)(3 * vcol) - + r)]; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/pphlopppohlnphdb_pinv.h b/gnc/matlab/code_generation/sharedutils/pphlopppohlnphdb_pinv.h deleted file mode 100644 index 243580f9b7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/pphlopppohlnphdb_pinv.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: pphlopppohlnphdb_pinv.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_pphlopppohlnphdb_pinv -#define SHARE_pphlopppohlnphdb_pinv -#include "rtwtypes.h" - -extern void pphlopppohlnphdb_pinv(const real32_T A[36], real32_T X[36]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/pphlpphdeknodjek_quat_rotation_vec.cpp b/gnc/matlab/code_generation/sharedutils/pphlpphdeknodjek_quat_rotation_vec.cpp deleted file mode 100644 index 7fef04d82f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/pphlpphdeknodjek_quat_rotation_vec.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// -// File: pphlpphdeknodjek_quat_rotation_vec.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#include "rtwtypes.h" -#include "aaaahdbaecbaiecj_quatmult.h" -#include "pphlpphdeknodjek_quat_rotation_vec.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void pphlpphdeknodjek_quat_rotation_vec(real_T vector[3], const real32_T Q[4], - real32_T vec_out[3]) -{ - real32_T V[4]; - real32_T Q_0[4]; - real32_T tmp[4]; - - // - // Q = q_a2b, rotation represents a vector rotating from frame b2a - // q_out = quat_rotation(vector, Q2) - // - // rotates a row-vector of 3-vectors by a row-vector of quaternion Q2 - // - // From: Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra - // Equation below is from Eq. 77, with the the quaternions inverted because - // our quat multiplication convention has the reverse order from the - // mulitplication used in the paper - // - // Check to see if the vector is a single row - // Vec_out = Q * [V 0]' * Q^-1 - // Quaternion Multiplication: - // Uses Hamilton's convention where the rotation order is left to right, - // q1*q2 corresponds to the first rotation q1, followed by the second - // rotation q2. - // - // Fundamentals of Spacecraft Attitude Determination and Control, - // F. Landis Markley and John L. Crassidis - // Equation: 2.82b - // - // Jesse C. Fusco jesse.c.fusco@nasa.gov - V[0] = (0.0F * Q[0] + Q[3] * (real32_T)vector[0]) + (Q[1] * (real32_T)vector[2] - - Q[2] * (real32_T)vector[1]); - V[1] = (0.0F * Q[1] + Q[3] * (real32_T)vector[1]) + (Q[2] * (real32_T)vector[0] - - Q[0] * (real32_T)vector[2]); - V[2] = (0.0F * Q[2] + Q[3] * (real32_T)vector[2]) + (Q[0] * (real32_T)vector[1] - - Q[1] * (real32_T)vector[0]); - V[3] = Q[3] * 0.0F - ((Q[0] * (real32_T)vector[0] + Q[1] * (real32_T)vector[1]) - + Q[2] * (real32_T)vector[2]); - Q_0[0] = -Q[0]; - Q_0[1] = -Q[1]; - Q_0[2] = -Q[2]; - Q_0[3] = Q[3]; - aaaahdbaecbaiecj_quatmult(V, Q_0, tmp); - vec_out[0] = tmp[0]; - vec_out[1] = tmp[1]; - vec_out[2] = tmp[2]; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/pphlpphdeknodjek_quat_rotation_vec.h b/gnc/matlab/code_generation/sharedutils/pphlpphdeknodjek_quat_rotation_vec.h deleted file mode 100644 index b2278c03c0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/pphlpphdeknodjek_quat_rotation_vec.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: pphlpphdeknodjek_quat_rotation_vec.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1139 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Aug 22 16:52:08 2017 -// -#ifndef SHARE_pphlpphdeknodjek_quat_rotation_vec -#define SHARE_pphlpphdeknodjek_quat_rotation_vec -#include "rtwtypes.h" - -extern void pphlpphdeknodjek_quat_rotation_vec(real_T vector[3], const real32_T - Q[4], real32_T vec_out[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppphhdjekfkfdjmg_xgeqrf.cpp b/gnc/matlab/code_generation/sharedutils/ppphhdjekfkfdjmg_xgeqrf.cpp deleted file mode 100644 index f4afd92e8a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppphhdjekfkfdjmg_xgeqrf.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// -// File: ppphhdjekfkfdjmg_xgeqrf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "jecjfcjmbaaaaiek_xzlarfg.h" -#include "nohlfkfkhlfcdbai_xzlarf.h" -#include "ppphhdjekfkfdjmg_xgeqrf.h" - -// Function for MATLAB Function: '/Compute Residual and H' -void ppphhdjekfkfdjmg_xgeqrf(real32_T A_data[], int32_T A_sizes[2], real32_T - tau_data[], int32_T *tau_sizes) -{ - int32_T m; - int32_T n; - int32_T mn; - int32_T i_i; - int32_T mmi; - real32_T b; - real32_T b_atmp; - int32_T loop_ub; - real32_T work_data[100]; - int32_T work_sizes; - int8_T c_idx_0; - m = A_sizes[0]; - n = A_sizes[1]; - work_sizes = A_sizes[0]; - mn = A_sizes[1]; - if (work_sizes <= mn) { - mn = work_sizes; - } - - *tau_sizes = (int8_T)mn; - if (!((A_sizes[0] == 0) || (A_sizes[1] == 0))) { - c_idx_0 = (int8_T)A_sizes[1]; - work_sizes = c_idx_0; - loop_ub = c_idx_0; - for (i_i = 0; i_i < loop_ub; i_i++) { - work_data[i_i] = 0.0F; - } - - for (loop_ub = 0; loop_ub + 1 <= mn; loop_ub++) { - i_i = loop_ub * m + loop_ub; - mmi = m - loop_ub; - if (loop_ub + 1 < m) { - b_atmp = A_data[i_i]; - b = jecjfcjmbaaaaiek_xzlarfg(mmi, &b_atmp, A_data, i_i + 2); - tau_data[loop_ub] = b; - A_data[i_i] = b_atmp; - } else { - tau_data[loop_ub] = 0.0F; - } - - if (loop_ub + 1 < n) { - b_atmp = A_data[i_i]; - A_data[i_i] = 1.0F; - nohlfkfkhlfcdbai_xzlarf(mmi, (n - loop_ub) - 1, i_i + 1, - tau_data[loop_ub], A_data, (loop_ub + (loop_ub + 1) * m) + 1, m, - work_data); - A_data[i_i] = b_atmp; - } - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppphhdjekfkfdjmg_xgeqrf.h b/gnc/matlab/code_generation/sharedutils/ppphhdjekfkfdjmg_xgeqrf.h deleted file mode 100644 index a1f3724dc5..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppphhdjekfkfdjmg_xgeqrf.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ppphhdjekfkfdjmg_xgeqrf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_ppphhdjekfkfdjmg_xgeqrf -#define SHARE_ppphhdjekfkfdjmg_xgeqrf -#include "rtwtypes.h" - -extern void ppphhdjekfkfdjmg_xgeqrf(real32_T A_data[], int32_T A_sizes[2], - real32_T tau_data[], int32_T *tau_sizes); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppppaaaacbimpphd_ilazlc.cpp b/gnc/matlab/code_generation/sharedutils/ppppaaaacbimpphd_ilazlc.cpp deleted file mode 100644 index 282bd5babf..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppppaaaacbimpphd_ilazlc.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: ppppaaaacbimpphd_ilazlc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ppppaaaacbimpphd_ilazlc.h" - -// Function for MATLAB Function: '/Compute Residual and H' -int32_T ppppaaaacbimpphd_ilazlc(int32_T m, int32_T n, const real32_T A_data[], - int32_T ia0, int32_T lda) -{ - int32_T j; - int32_T coltop; - int32_T ia; - int32_T exitg1; - boolean_T exitg2; - j = n; - exitg2 = false; - while ((!exitg2) && (j > 0)) { - coltop = (int32_T)((int32_T)((int32_T)(j - 1) * lda) + ia0); - ia = coltop; - do { - exitg1 = 0; - if (ia <= (int32_T)((int32_T)(coltop + m) - 1)) { - if (A_data[(int32_T)(ia - 1)] != 0.0F) { - exitg1 = 1; - } else { - ia++; - } - } else { - j--; - exitg1 = 2; - } - } while (exitg1 == 0); - - if (exitg1 == 1) { - exitg2 = true; - } - } - - return j; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppppaaaacbimpphd_ilazlc.h b/gnc/matlab/code_generation/sharedutils/ppppaaaacbimpphd_ilazlc.h deleted file mode 100644 index 7a3da348d7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppppaaaacbimpphd_ilazlc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ppppaaaacbimpphd_ilazlc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ppppaaaacbimpphd_ilazlc -#define SHARE_ppppaaaacbimpphd_ilazlc -#include "rtwtypes.h" - -extern int32_T ppppaaaacbimpphd_ilazlc(int32_T m, int32_T n, const real32_T - A_data[], int32_T ia0, int32_T lda); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppppfkfkiekfimop_xnrm2.cpp b/gnc/matlab/code_generation/sharedutils/ppppfkfkiekfimop_xnrm2.cpp deleted file mode 100644 index 662ae7741d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppppfkfkiekfimop_xnrm2.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: ppppfkfkiekfimop_xnrm2.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include -#include "ppppfkfkiekfimop_xnrm2.h" - -// Function for MATLAB Function: '/MATLAB Function' -real32_T ppppfkfkiekfimop_xnrm2(int32_T n, const real32_T x[72], int32_T ix0) -{ - real32_T y; - real32_T scale; - int32_T kend; - real32_T absxk; - real32_T t; - int32_T k; - y = 0.0F; - if (!(n < 1)) { - if (n == 1) { - y = (real32_T)fabs((real_T)x[(int32_T)(ix0 - 1)]); - } else { - scale = 1.17549435E-38F; - kend = (int32_T)((int32_T)(ix0 + n) - 1); - for (k = ix0; k <= kend; k++) { - absxk = (real32_T)fabs((real_T)x[(int32_T)(k - 1)]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0F; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - y = scale * (real32_T)sqrt((real_T)y); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppppfkfkiekfimop_xnrm2.h b/gnc/matlab/code_generation/sharedutils/ppppfkfkiekfimop_xnrm2.h deleted file mode 100644 index 3c7595836f..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppppfkfkiekfimop_xnrm2.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ppppfkfkiekfimop_xnrm2.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_ppppfkfkiekfimop_xnrm2 -#define SHARE_ppppfkfkiekfimop_xnrm2 -#include "rtwtypes.h" - -extern real32_T ppppfkfkiekfimop_xnrm2(int32_T n, const real32_T x[72], int32_T - ix0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppppjmohbaaigdje_xaxpy.cpp b/gnc/matlab/code_generation/sharedutils/ppppjmohbaaigdje_xaxpy.cpp deleted file mode 100644 index db8d411457..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppppjmohbaaigdje_xaxpy.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -// File: ppppjmohbaaigdje_xaxpy.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "ppppjmohbaaigdje_xaxpy.h" - -// Function for MATLAB Function: '/compute_of_global_points' -void ppppjmohbaaigdje_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T y_data[], - int32_T iy0) -{ - int32_T ix; - int32_T iy; - int32_T k; - if (!(a == 0.0F)) { - ix = (int32_T)(ix0 - 1); - iy = (int32_T)(iy0 - 1); - for (k = 0; k <= (int32_T)(n - 1); k++) { - y_data[iy] += a * y_data[ix]; - ix++; - iy++; - } - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppppjmohbaaigdje_xaxpy.h b/gnc/matlab/code_generation/sharedutils/ppppjmohbaaigdje_xaxpy.h deleted file mode 100644 index c2647623a1..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppppjmohbaaigdje_xaxpy.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ppppjmohbaaigdje_xaxpy.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_ppppjmohbaaigdje_xaxpy -#define SHARE_ppppjmohbaaigdje_xaxpy -#include "rtwtypes.h" - -extern void ppppjmohbaaigdje_xaxpy(int32_T n, real32_T a, int32_T ix0, real32_T - y_data[], int32_T iy0); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppppmophjmopekno_ilazlc.cpp b/gnc/matlab/code_generation/sharedutils/ppppmophjmopekno_ilazlc.cpp deleted file mode 100644 index e6e8519f4d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppppmophjmopekno_ilazlc.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// File: ppppmophjmopekno_ilazlc.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#include "rtwtypes.h" -#include "ppppmophjmopekno_ilazlc.h" - -// Function for MATLAB Function: '/Compute Residual and H' -int32_T ppppmophjmopekno_ilazlc(int32_T m, int32_T n, const real32_T A_data[], - int32_T ia0, int32_T lda) -{ - int32_T j; - int32_T coltop; - int32_T ia; - int32_T exitg1; - boolean_T exitg2; - j = n; - exitg2 = false; - while ((!exitg2) && (j > 0)) { - coltop = (j - 1) * lda + ia0; - ia = coltop; - do { - exitg1 = 0; - if (ia <= (coltop + m) - 1) { - if (A_data[ia - 1] != 0.0F) { - exitg1 = 1; - } else { - ia++; - } - } else { - j--; - exitg1 = 2; - } - } while (exitg1 == 0); - - if (exitg1 == 1) { - exitg2 = true; - } - } - - return j; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/ppppmophjmopekno_ilazlc.h b/gnc/matlab/code_generation/sharedutils/ppppmophjmopekno_ilazlc.h deleted file mode 100644 index b1844d89ab..0000000000 --- a/gnc/matlab/code_generation/sharedutils/ppppmophjmopekno_ilazlc.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: ppppmophjmopekno_ilazlc.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1137 -// Simulink Coder version : 8.9 (R2015b) 13-Aug-2015 -// C/C++ source code generated on : Wed May 3 15:13:05 2017 -// -#ifndef SHARE_ppppmophjmopekno_ilazlc -#define SHARE_ppppmophjmopekno_ilazlc -#include "rtwtypes.h" - -extern int32_T ppppmophjmopekno_ilazlc(int32_T m, int32_T n, const real32_T - A_data[], int32_T ia0, int32_T lda); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rtGetInf.cpp b/gnc/matlab/code_generation/sharedutils/rtGetInf.cpp deleted file mode 100644 index 893251d955..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rtGetInf.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// -// File: rtGetInf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// - -// -// Abstract: -// Function to initialize non-finite, Inf - -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -extern "C" { - // - // Initialize rtInf needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetInf(void) - { - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; - } - - // - // Initialize rtInfF needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetInfF(void) - { - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; - } - - // - // Initialize rtMinusInf needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetMinusInf(void) - { - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; - } - - // - // Initialize rtMinusInfF needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetMinusInfF(void) - { - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; - } -} -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rtGetInf.h b/gnc/matlab/code_generation/sharedutils/rtGetInf.h deleted file mode 100644 index 333899d5f7..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rtGetInf.h +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: rtGetInf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef RTW_HEADER_rtGetInf_h_ -#define RTW_HEADER_rtGetInf_h_ -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" -#ifdef __cplusplus - -extern "C" { - -#endif - - extern real_T rtGetInf(void); - extern real32_T rtGetInfF(void); - extern real_T rtGetMinusInf(void); - extern real32_T rtGetMinusInfF(void); - -#ifdef __cplusplus - -} // extern "C" -#endif -#endif // RTW_HEADER_rtGetInf_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rtGetNaN.cpp b/gnc/matlab/code_generation/sharedutils/rtGetNaN.cpp deleted file mode 100644 index 74b192e44a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rtGetNaN.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// -// File: rtGetNaN.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// - -// -// Abstract: -// Function to initialize non-finite, NaN - -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -extern "C" { - // - // Initialize rtNaN needed by the generated code. - // NaN is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetNaN(void) - { - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; - } - - // - // Initialize rtNaNF needed by the generated code. - // NaN is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetNaNF(void) - { - IEEESingle nanF = { { 0 } }; - - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; - } -} -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rtGetNaN.h b/gnc/matlab/code_generation/sharedutils/rtGetNaN.h deleted file mode 100644 index 508d95fb5a..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rtGetNaN.h +++ /dev/null @@ -1,34 +0,0 @@ -// -// File: rtGetNaN.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef RTW_HEADER_rtGetNaN_h_ -#define RTW_HEADER_rtGetNaN_h_ -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" -#ifdef __cplusplus - -extern "C" { - -#endif - - extern real_T rtGetNaN(void); - extern real32_T rtGetNaNF(void); - -#ifdef __cplusplus - -} // extern "C" -#endif -#endif // RTW_HEADER_rtGetNaN_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_hypotf_snf.cpp b/gnc/matlab/code_generation/sharedutils/rt_hypotf_snf.cpp deleted file mode 100644 index 8433c1f8da..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_hypotf_snf.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// File: rt_hypotf_snf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "rt_nonfinite.h" -#include "rt_hypotf_snf.h" - -real32_T rt_hypotf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T a; - a = (real32_T)fabs((real_T)u0); - y = (real32_T)fabs((real_T)u1); - if (a < y) { - a /= y; - y *= (real32_T)sqrt((real_T)(a * a + 1.0F)); - } else if (a > y) { - y /= a; - y = (real32_T)sqrt((real_T)(y * y + 1.0F)) * a; - } else { - if (!rtIsNaNF(y)) { - y = a * 1.41421354F; - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_hypotf_snf.h b/gnc/matlab/code_generation/sharedutils/rt_hypotf_snf.h deleted file mode 100644 index d5051ec406..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_hypotf_snf.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: rt_hypotf_snf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_rt_hypotf_snf -#define SHARE_rt_hypotf_snf -#include "rtwtypes.h" - -extern real32_T rt_hypotf_snf(real32_T u0, real32_T u1); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_mldivide_U1f3x3_U2f_XeZWzB4d.cpp b/gnc/matlab/code_generation/sharedutils/rt_mldivide_U1f3x3_U2f_XeZWzB4d.cpp deleted file mode 100644 index c3bb49ace9..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_mldivide_U1f3x3_U2f_XeZWzB4d.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// -// File: rt_mldivide_U1f3x3_U2f_XeZWzB4d.cpp -// -// Code generated for Simulink model 'ctl_controller0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:40 2018 -// -#include "rtwtypes.h" -#include -#include "rt_mldivide_U1f3x3_U2f_XeZWzB4d.h" - -void rt_mldivide_U1f3x3_U2f_XeZWzB4d(const real32_T u0[9], const real32_T u1[3], - real32_T y[3]) -{ - real32_T A[9]; - int32_T r1; - int32_T r2; - int32_T r3; - real32_T maxval; - real32_T a21; - int32_T rtemp; - for (r1 = 0; r1 < 9; r1++) { - A[r1] = u0[r1]; - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs((real_T)u0[0]); - a21 = (real32_T)fabs((real_T)u0[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs((real_T)u0[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - A[r2] = u0[r2] / u0[r1]; - A[r3] /= A[r1]; - A[(int32_T)(3 + r2)] -= A[(int32_T)(3 + r1)] * A[r2]; - A[(int32_T)(3 + r3)] -= A[(int32_T)(3 + r1)] * A[r3]; - A[(int32_T)(6 + r2)] -= A[(int32_T)(6 + r1)] * A[r2]; - A[(int32_T)(6 + r3)] -= A[(int32_T)(6 + r1)] * A[r3]; - if ((real32_T)fabs((real_T)A[(int32_T)(3 + r3)]) > (real32_T)fabs((real_T)A - [(int32_T)(3 + r2)])) { - rtemp = (int32_T)(r2 + 1); - r2 = r3; - r3 = (int32_T)(rtemp - 1); - } - - A[(int32_T)(3 + r3)] /= A[(int32_T)(3 + r2)]; - A[(int32_T)(6 + r3)] -= A[(int32_T)(3 + r3)] * A[(int32_T)(6 + r2)]; - maxval = u1[r2] - u1[r1] * A[r2]; - a21 = ((u1[r3] - u1[r1] * A[r3]) - A[(int32_T)(3 + r3)] * maxval) / A[(int32_T) - (6 + r3)]; - maxval -= A[(int32_T)(6 + r2)] * a21; - maxval /= A[(int32_T)(3 + r2)]; - y[0] = ((u1[r1] - A[(int32_T)(6 + r1)] * a21) - A[(int32_T)(3 + r1)] * maxval) - / A[r1]; - y[1] = maxval; - y[2] = a21; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_mldivide_U1f3x3_U2f_XeZWzB4d.h b/gnc/matlab/code_generation/sharedutils/rt_mldivide_U1f3x3_U2f_XeZWzB4d.h deleted file mode 100644 index dcbc4dd5cc..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_mldivide_U1f3x3_U2f_XeZWzB4d.h +++ /dev/null @@ -1,23 +0,0 @@ -// -// File: rt_mldivide_U1f3x3_U2f_XeZWzB4d.h -// -// Code generated for Simulink model 'ctl_controller0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:40 2018 -// -#ifndef SHARE_rt_mldivide_U1f3x3_U2f_XeZWzB4d -#define SHARE_rt_mldivide_U1f3x3_U2f_XeZWzB4d -#include "rtwtypes.h" - -extern void rt_mldivide_U1f3x3_U2f_XeZWzB4d(const real32_T u0[9], const real32_T - u1[3], real32_T y[3]); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_nonfinite.cpp b/gnc/matlab/code_generation/sharedutils/rt_nonfinite.cpp deleted file mode 100644 index 5ecdbf2818..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_nonfinite.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// -// File: rt_nonfinite.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// - -// -// Abstract: -// Function to initialize non-finites, -// (Inf, NaN and -Inf). - -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -extern "C" { - real_T rtInf; - real_T rtMinusInf; - real_T rtNaN; - real32_T rtInfF; - real32_T rtMinusInfF; - real32_T rtNaNF; -} - extern "C" -{ - // - // Initialize the rtInf, rtMinusInf, and rtNaN needed by the - // generated code. NaN is initialized as non-signaling. Assumes IEEE. - // - void rt_InitInfAndNaN(size_t realSize) - { - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); - } - - // Test if value is infinite - boolean_T rtIsInf(real_T value) - { - return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); - } - - // Test if single-precision value is infinite - boolean_T rtIsInfF(real32_T value) - { - return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); - } - - // Test if value is not a number - boolean_T rtIsNaN(real_T value) - { - return (boolean_T)((value!=value) ? 1U : 0U); - } - - // Test if single-precision value is not a number - boolean_T rtIsNaNF(real32_T value) - { - return (boolean_T)(((value!=value) ? 1U : 0U)); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_nonfinite.h b/gnc/matlab/code_generation/sharedutils/rt_nonfinite.h deleted file mode 100644 index 51922dc599..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_nonfinite.h +++ /dev/null @@ -1,62 +0,0 @@ -// -// File: rt_nonfinite.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef RTW_HEADER_rt_nonfinite_h_ -#define RTW_HEADER_rt_nonfinite_h_ -#include -#include "rtwtypes.h" -#ifdef __cplusplus - -extern "C" { - -#endif - - extern real_T rtInf; - extern real_T rtMinusInf; - extern real_T rtNaN; - extern real32_T rtInfF; - extern real32_T rtMinusInfF; - extern real32_T rtNaNF; - extern void rt_InitInfAndNaN(size_t realSize); - extern boolean_T rtIsInf(real_T value); - extern boolean_T rtIsInfF(real32_T value); - extern boolean_T rtIsNaN(real_T value); - extern boolean_T rtIsNaNF(real32_T value); - typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; - } BigEndianIEEEDouble; - - typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; - } LittleEndianIEEEDouble; - - typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; - } IEEESingle; - -#ifdef __cplusplus - -} // extern "C" -#endif -#endif // RTW_HEADER_rt_nonfinite_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_nrand_Upu32_Yd_f_pw_snf.cpp b/gnc/matlab/code_generation/sharedutils/rt_nrand_Upu32_Yd_f_pw_snf.cpp deleted file mode 100644 index 6b28f80f0e..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_nrand_Upu32_Yd_f_pw_snf.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: rt_nrand_Upu32_Yd_f_pw_snf.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include -#include "rt_urand_Upu32_Yd_f_pw_snf.h" -#include "rt_nrand_Upu32_Yd_f_pw_snf.h" - -real_T rt_nrand_Upu32_Yd_f_pw_snf(uint32_T *u) -{ - real_T y; - real_T sr; - real_T si; - - // Normal (Gaussian) random number generator - do { - sr = 2.0 * rt_urand_Upu32_Yd_f_pw_snf(u) - 1.0; - si = 2.0 * rt_urand_Upu32_Yd_f_pw_snf(u) - 1.0; - si = sr * sr + si * si; - } while (si > 1.0); - - y = sqrt(-2.0 * log(si) / si) * sr; - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_nrand_Upu32_Yd_f_pw_snf.h b/gnc/matlab/code_generation/sharedutils/rt_nrand_Upu32_Yd_f_pw_snf.h deleted file mode 100644 index 94c4f87829..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_nrand_Upu32_Yd_f_pw_snf.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: rt_nrand_Upu32_Yd_f_pw_snf.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_rt_nrand_Upu32_Yd_f_pw_snf -#define SHARE_rt_nrand_Upu32_Yd_f_pw_snf -#include "rtwtypes.h" - -extern real_T rt_nrand_Upu32_Yd_f_pw_snf(uint32_T *u); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_powf_snf.cpp b/gnc/matlab/code_generation/sharedutils/rt_powf_snf.cpp deleted file mode 100644 index 0c7339dace..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_powf_snf.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// -// File: rt_powf_snf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "rtGetInf.h" -#include -#include "rt_powf_snf.h" - -real32_T rt_powf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T tmp; - real32_T tmp_0; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = (rtNaNF); - } else { - tmp = (real32_T)fabs((real_T)u0); - tmp_0 = (real32_T)fabs((real_T)u1); - if (rtIsInfF(u1)) { - if (tmp == 1.0F) { - y = (rtNaNF); - } else if (tmp > 1.0F) { - if (u1 > 0.0F) { - y = (rtInfF); - } else { - y = 0.0F; - } - } else if (u1 > 0.0F) { - y = 0.0F; - } else { - y = (rtInfF); - } - } else if (tmp_0 == 0.0F) { - y = 1.0F; - } else if (tmp_0 == 1.0F) { - if (u1 > 0.0F) { - y = u0; - } else { - y = 1.0F / u0; - } - } else if (u1 == 2.0F) { - y = u0 * u0; - } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { - y = (real32_T)sqrt((real_T)u0); - } else if ((u0 < 0.0F) && (u1 > (real32_T)floor((real_T)u1))) { - y = (rtNaNF); - } else { - y = (real32_T)pow((real_T)u0, (real_T)u1); - } - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_powf_snf.h b/gnc/matlab/code_generation/sharedutils/rt_powf_snf.h deleted file mode 100644 index 9a94ad67f0..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_powf_snf.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: rt_powf_snf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_rt_powf_snf -#define SHARE_rt_powf_snf -#include "rtwtypes.h" - -extern real32_T rt_powf_snf(real32_T u0, real32_T u1); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_roundd_snf.cpp b/gnc/matlab/code_generation/sharedutils/rt_roundd_snf.cpp deleted file mode 100644 index 38059afe4c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_roundd_snf.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: rt_roundd_snf.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#include "rtwtypes.h" -#include -#include "rt_roundd_snf.h" - -real_T rt_roundd_snf(real_T u) -{ - real_T y; - if (fabs(u) < 4.503599627370496E+15) { - if (u >= 0.5) { - y = floor(u + 0.5); - } else if (u > -0.5) { - y = u * 0.0; - } else { - y = ceil(u - 0.5); - } - } else { - y = u; - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_roundd_snf.h b/gnc/matlab/code_generation/sharedutils/rt_roundd_snf.h deleted file mode 100644 index 30b702148d..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_roundd_snf.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: rt_roundd_snf.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef SHARE_rt_roundd_snf -#define SHARE_rt_roundd_snf -#include "rtwtypes.h" - -extern real_T rt_roundd_snf(real_T u); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_roundf_snf.cpp b/gnc/matlab/code_generation/sharedutils/rt_roundf_snf.cpp deleted file mode 100644 index e145a6bc8c..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_roundf_snf.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// File: rt_roundf_snf.cpp -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#include "rtwtypes.h" -#include -#include "rt_roundf_snf.h" - -real32_T rt_roundf_snf(real32_T u) -{ - real32_T y; - if ((real32_T)fabs((real_T)u) < 8.388608E+6F) { - if (u >= 0.5F) { - y = (real32_T)floor((real_T)(u + 0.5F)); - } else if (u > -0.5F) { - y = u * 0.0F; - } else { - y = (real32_T)ceil((real_T)(u - 0.5F)); - } - } else { - y = u; - } - - return y; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_roundf_snf.h b/gnc/matlab/code_generation/sharedutils/rt_roundf_snf.h deleted file mode 100644 index db7106c267..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_roundf_snf.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: rt_roundf_snf.h -// -// Code generated for Simulink model 'fam_force_allocation_module'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:07:06 2018 -// -#ifndef SHARE_rt_roundf_snf -#define SHARE_rt_roundf_snf -#include "rtwtypes.h" - -extern real32_T rt_roundf_snf(real32_T u); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_urand_Upu32_Yd_f_pw_snf.cpp b/gnc/matlab/code_generation/sharedutils/rt_urand_Upu32_Yd_f_pw_snf.cpp deleted file mode 100644 index 9b9b54ef26..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_urand_Upu32_Yd_f_pw_snf.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// -// File: rt_urand_Upu32_Yd_f_pw_snf.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#include "rtwtypes.h" -#include "rt_urand_Upu32_Yd_f_pw_snf.h" - -real_T rt_urand_Upu32_Yd_f_pw_snf(uint32_T *u) -{ - uint32_T lo; - uint32_T hi; - - // Uniform random number generator (random number between 0 and 1) - - // #define IA 16807 magic multiplier = 7^5 - // #define IM 2147483647 modulus = 2^31-1 - // #define IQ 127773 IM div IA - // #define IR 2836 IM modulo IA - // #define S 4.656612875245797e-10 reciprocal of 2^31-1 - // test = IA * (seed % IQ) - IR * (seed/IQ) - // seed = test < 0 ? (test + IM) : test - // return (seed*S) - - lo = (uint32_T)(*u % 127773U * 16807U); - hi = (uint32_T)((uint32_T)(*u / 127773U) * 2836U); - if (lo < hi) { - *u = (uint32_T)(2147483647U - (uint32_T)(hi - lo)); - } else { - *u = (uint32_T)(lo - hi); - } - - return (real_T)*u * 4.6566128752457969E-10; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rt_urand_Upu32_Yd_f_pw_snf.h b/gnc/matlab/code_generation/sharedutils/rt_urand_Upu32_Yd_f_pw_snf.h deleted file mode 100644 index 4aa16d0326..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rt_urand_Upu32_Yd_f_pw_snf.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// File: rt_urand_Upu32_Yd_f_pw_snf.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:08:00 2018 -// -#ifndef SHARE_rt_urand_Upu32_Yd_f_pw_snf -#define SHARE_rt_urand_Upu32_Yd_f_pw_snf -#include "rtwtypes.h" - -extern real_T rt_urand_Upu32_Yd_f_pw_snf(uint32_T *u); - -#endif - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rtwtypes.h b/gnc/matlab/code_generation/sharedutils/rtwtypes.h deleted file mode 100644 index ac6a76a758..0000000000 --- a/gnc/matlab/code_generation/sharedutils/rtwtypes.h +++ /dev/null @@ -1,192 +0,0 @@ -// -// File: rtwtypes.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Tue Oct 16 10:06:07 2018 -// -#ifndef RTWTYPES_H -#define RTWTYPES_H - -// Logical type definitions -#if (!defined(__cplusplus)) -# ifndef false -# define false (0U) -# endif - -# ifndef true -# define true (1U) -# endif -#endif - -//=======================================================================* -// Target hardware information -// Device type: 32-bit Generic -// Number of bits: char: 8 short: 16 int: 32 -// long: 32 -// native word size: 32 -// Byte ordering: Unspecified -// Signed integer division rounds to: Undefined -// Shift right on a signed integer as arithmetic shift: on -// ======================================================================= -#ifdef PORTABLE_WORDSIZES // PORTABLE_WORDSIZES defined - -//=======================================================================* -// Host information -// Number of bits: char: 8 short: 16 int: 32 -// long: 64 -// long long: 64 -// native word size: 64 -// ======================================================================= - -//=======================================================================* -// Fixed width word size data types: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// real32_T, real64_T - 32 and 64 bit floating point numbers * -// ======================================================================= -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -//===========================================================================* -// Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * -// real_T, time_T, ulong_T. * -// =========================================================================== -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned int ulong_T; -typedef char char_T; -typedef unsigned char uchar_T; -typedef char_T byte_T; - -#else // PORTABLE_WORDSIZES not defined - -//=======================================================================* -// Fixed width word size data types: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// real32_T, real64_T - 32 and 64 bit floating point numbers * -// ======================================================================= -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -//===========================================================================* -// Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * -// real_T, time_T, ulong_T. * -// =========================================================================== -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef unsigned char uchar_T; -typedef char_T byte_T; - -#endif // PORTABLE_WORDSIZES - -//===========================================================================* -// Complex number type definitions * -// =========================================================================== -#define CREAL_T - -typedef struct { - real32_T re; - real32_T im; -} creal32_T; - -typedef struct { - real64_T re; - real64_T im; -} creal64_T; - -typedef struct { - real_T re; - real_T im; -} creal_T; - -#define CINT8_T - -typedef struct { - int8_T re; - int8_T im; -} cint8_T; - -#define CUINT8_T - -typedef struct { - uint8_T re; - uint8_T im; -} cuint8_T; - -#define CINT16_T - -typedef struct { - int16_T re; - int16_T im; -} cint16_T; - -#define CUINT16_T - -typedef struct { - uint16_T re; - uint16_T im; -} cuint16_T; - -#define CINT32_T - -typedef struct { - int32_T re; - int32_T im; -} cint32_T; - -#define CUINT32_T - -typedef struct { - uint32_T re; - uint32_T im; -} cuint32_T; - -//=======================================================================* -// Min and Max: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// ======================================================================= -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255U)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535U)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) - -// Block D-Work pointer type -typedef void * pointer_T; - -#endif // RTWTYPES_H - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sharedutils/rtwtypeschksum.mat b/gnc/matlab/code_generation/sharedutils/rtwtypeschksum.mat deleted file mode 100644 index 6864266856..0000000000 Binary files a/gnc/matlab/code_generation/sharedutils/rtwtypeschksum.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/sharedutils/tflSUInfo.mat b/gnc/matlab/code_generation/sharedutils/tflSUInfo.mat deleted file mode 100644 index dd1bf6f91b..0000000000 Binary files a/gnc/matlab/code_generation/sharedutils/tflSUInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/blower_aerodynamics.cpp b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/blower_aerodynamics.cpp deleted file mode 100644 index b85f10ab58..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/blower_aerodynamics.cpp +++ /dev/null @@ -1,303 +0,0 @@ -// -// File: blower_aerodynamics.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:45:47 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "blower_aerodynamics.h" - -// Include model header file for global data -#include "sim_model_lib0.h" -#include "sim_model_lib0_private.h" - -// System initialize for atomic system: '/blower_aerodynamics' -void si_blower_aerodynamics_Init(DW_blower_aerodynamics_sim_mo_T *localDW, - P_blower_aerodynamics_sim_mod_T *localP, real_T rtp_noz_randn_seed) -{ - uint32_T tseed; - int32_T r; - int32_T t; - real_T y1; - int32_T i; - for (i = 0; i < 6; i++) { - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - localDW->DiscreteTimeIntegrator_DSTATE[i] = - localP->DiscreteTimeIntegrator_IC; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(rtp_noz_randn_seed + rtCP_pooled1[i]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * localP->random_noise_StdDev[i] + - localP->random_noise_Mean; - localDW->NextOutput[i] = y1; - localDW->RandSeed[i] = tseed; - - // End of InitializeConditions for RandomNumber: '/random_noise' - } -} - -// Output and update for atomic system: '/blower_aerodynamics' -void sim_mod_blower_aerodynamics(real32_T rtu_rotor_speed, const real32_T - rtu_nozzle_areas[6], B_blower_aerodynamics_sim_mod_T *localB, - DW_blower_aerodynamics_sim_mo_T *localDW, P_blower_aerodynamics_sim_mod_T - *localP, real32_T rtp_imp_zero_thrust_area, real32_T - rtp_imp_zero_thrust_area_error, real32_T rtp_noise_on_flag, const real32_T - rtp_noz_cd[6], const real32_T rtp_noz_cd_error[6], const real32_T - rtp_imp_cdp_lookup[334], const real32_T rtp_imp_area_lookup[334], real32_T - rtp_imp_diameter, real32_T rtp_const_air_den, real32_T rtp_noz_thrust_sf, - const real_T rtp_noz_thrust_noise_feedback[6]) -{ - real32_T rtb_walking_bias[6]; - real32_T rtb_Product2_g[6]; - real32_T rtb_Cdp; - int32_T i; - for (i = 0; i < 6; i++) { - // Sum: '/Add3' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant8' - // Gain: '/Gain1' - - rtb_Cdp = rtp_noise_on_flag * rtp_noz_cd_error[i] + rtp_noz_cd[i]; - - // Product: '/Product2' - rtb_Product2_g[i] = rtb_Cdp * rtu_nozzle_areas[i]; - - // Sum: '/Add3' - rtb_walking_bias[i] = rtb_Cdp; - } - - // Sum: '/Sum of Elements1' - rtb_Cdp = rtb_Product2_g[0]; - for (i = 0; i < 5; i++) { - rtb_Cdp += rtb_Product2_g[(int32_T)(i + 1)]; - } - - // End of Sum: '/Sum of Elements1' - - // Sum: '/Add' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant6' - // Gain: '/Gain2' - - rtb_Cdp += rtp_noise_on_flag * rtp_imp_zero_thrust_area_error + - rtp_imp_zero_thrust_area; - - // Lookup_n-D: '/bpm_Cdp_lookup' - rtb_Cdp = look1_iflf_binlxpw(rtb_Cdp, rtp_imp_area_lookup, rtp_imp_cdp_lookup, - 333U); - - // Product: '/Product3' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - - rtb_Cdp = rtu_rotor_speed * rtu_rotor_speed * rtb_Cdp * rtp_imp_diameter * - rtp_imp_diameter * rtp_const_air_den; - - // Constant: '/Constant7' - localB->Constant7 = localP->Constant7_Value; - for (i = 0; i < 6; i++) { - // Sum: '/Add1' incorporates: - // Constant: '/Constant4' - // Constant: '/Constant9' - // DataTypeConversion: '/Data Type Conversion' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Product: '/Product1' - // Product: '/Product6' - // Product: '/Product7' - - localB->Add1[i] = localP->Constant4_Value * rtb_walking_bias[i] * - rtb_walking_bias[i] * rtb_Cdp * rtu_nozzle_areas[i] * rtp_noz_thrust_sf + - (real32_T)localDW->DiscreteTimeIntegrator_DSTATE[i]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain3' - // RandomNumber: '/random_noise' - // Sum: '/Sum' - - localDW->DiscreteTimeIntegrator_DSTATE[i] += (localDW->NextOutput[i] - - rtp_noz_thrust_noise_feedback[i] * localDW-> - DiscreteTimeIntegrator_DSTATE[i]) * localP->DiscreteTimeIntegrator_gainval; - - // Update for RandomNumber: '/random_noise' - localDW->NextOutput[i] = rt_nrand_Upu32_Yd_f_pw_snf(&localDW->RandSeed[i]) * - localP->random_noise_StdDev[i] + localP->random_noise_Mean; - } -} - -// Termination for atomic system: '/blower_aerodynamics' -void si_blower_aerodynamics_Term(void) -{ -} - -// System initialize for atomic system: '/blower_aerodynamics' -void blower_aerodynamics_d_Init(DW_blower_aerodynamics_sim__c_T *localDW, - P_blower_aerodynamics_sim_m_m_T *localP, real_T rtp_noz_randn_seed) -{ - uint32_T tseed; - int32_T r; - int32_T t; - real_T y1; - int32_T i; - for (i = 0; i < 6; i++) { - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - localDW->DiscreteTimeIntegrator_DSTATE[i] = - localP->DiscreteTimeIntegrator_IC; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(rtp_noz_randn_seed + rtCP_pooled1[i]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * localP->random_noise_StdDev[i] + - localP->random_noise_Mean; - localDW->NextOutput[i] = y1; - localDW->RandSeed[i] = tseed; - - // End of InitializeConditions for RandomNumber: '/random_noise' - } -} - -// Output and update for atomic system: '/blower_aerodynamics' -void sim_m_blower_aerodynamics_j(real32_T rtu_rotor_speed, const real32_T - rtu_nozzle_areas[6], B_blower_aerodynamics_sim_m_f_T *localB, - DW_blower_aerodynamics_sim__c_T *localDW, P_blower_aerodynamics_sim_m_m_T - *localP, real32_T rtp_imp_zero_thrust_area, real32_T - rtp_imp_zero_thrust_area_error, real32_T rtp_noise_on_flag, const real32_T - rtp_noz_cd[6], const real32_T rtp_noz_cd_error[6], const real32_T - rtp_imp_cdp_lookup[334], const real32_T rtp_imp_area_lookup[334], real32_T - rtp_imp_diameter, real32_T rtp_const_air_den, real32_T rtp_noz_thrust_sf, - const real_T rtp_noz_thrust_noise_feedback[6]) -{ - real32_T rtb_walking_bias[6]; - real32_T rtb_Product2_h[6]; - real32_T rtb_Cdp; - int32_T i; - for (i = 0; i < 6; i++) { - // Sum: '/Add3' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant8' - // Gain: '/Gain1' - - rtb_Cdp = rtp_noise_on_flag * rtp_noz_cd_error[i] + rtp_noz_cd[i]; - - // Product: '/Product2' - rtb_Product2_h[i] = rtb_Cdp * rtu_nozzle_areas[i]; - - // Sum: '/Add3' - rtb_walking_bias[i] = rtb_Cdp; - } - - // Sum: '/Sum of Elements1' - rtb_Cdp = rtb_Product2_h[0]; - for (i = 0; i < 5; i++) { - rtb_Cdp += rtb_Product2_h[(int32_T)(i + 1)]; - } - - // End of Sum: '/Sum of Elements1' - - // Sum: '/Add' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant6' - // Gain: '/Gain2' - - rtb_Cdp += rtp_noise_on_flag * rtp_imp_zero_thrust_area_error + - rtp_imp_zero_thrust_area; - - // Lookup_n-D: '/bpm_Cdp_lookup' - rtb_Cdp = look1_iflf_binlxpw(rtb_Cdp, rtp_imp_area_lookup, rtp_imp_cdp_lookup, - 333U); - - // Product: '/Product3' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - - rtb_Cdp = rtu_rotor_speed * rtu_rotor_speed * rtb_Cdp * rtp_imp_diameter * - rtp_imp_diameter * rtp_const_air_den; - - // Constant: '/Constant7' - localB->Constant7 = localP->Constant7_Value; - for (i = 0; i < 6; i++) { - // Sum: '/Add1' incorporates: - // Constant: '/Constant4' - // Constant: '/Constant9' - // DataTypeConversion: '/Data Type Conversion' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Product: '/Product1' - // Product: '/Product6' - // Product: '/Product7' - - localB->Add1[i] = localP->Constant4_Value * rtb_walking_bias[i] * - rtb_walking_bias[i] * rtb_Cdp * rtu_nozzle_areas[i] * rtp_noz_thrust_sf + - (real32_T)localDW->DiscreteTimeIntegrator_DSTATE[i]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain3' - // RandomNumber: '/random_noise' - // Sum: '/Sum' - - localDW->DiscreteTimeIntegrator_DSTATE[i] += (localDW->NextOutput[i] - - rtp_noz_thrust_noise_feedback[i] * localDW-> - DiscreteTimeIntegrator_DSTATE[i]) * localP->DiscreteTimeIntegrator_gainval; - - // Update for RandomNumber: '/random_noise' - localDW->NextOutput[i] = rt_nrand_Upu32_Yd_f_pw_snf(&localDW->RandSeed[i]) * - localP->random_noise_StdDev[i] + localP->random_noise_Mean; - } -} - -// Termination for atomic system: '/blower_aerodynamics' -void blower_aerodynamics_f_Term(void) -{ -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/blower_aerodynamics.h b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/blower_aerodynamics.h deleted file mode 100644 index 9f81fb7521..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/blower_aerodynamics.h +++ /dev/null @@ -1,128 +0,0 @@ -// -// File: blower_aerodynamics.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:45:47 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_blower_aerodynamics_h_ -#define RTW_HEADER_blower_aerodynamics_h_ -#include -#ifndef sim_model_lib0_COMMON_INCLUDES_ -# define sim_model_lib0_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#endif // sim_model_lib0_COMMON_INCLUDES_ - -#include "sim_model_lib0_types.h" -#include "look1_iflf_binlxpw.h" -#include "rt_nonfinite.h" -#include "rt_nrand_Upu32_Yd_f_pw_snf.h" - -// Block signals for system '/blower_aerodynamics' -typedef struct { - real32_T Add1[6]; // '/Add1' - real32_T Constant7; // '/Constant7' -} B_blower_aerodynamics_sim_mod_T; - -// Block states (auto storage) for system '/blower_aerodynamics' -typedef struct { - real_T DiscreteTimeIntegrator_DSTATE[6];// '/Discrete-Time Integrator' - real_T NextOutput[6]; // '/random_noise' - uint32_T RandSeed[6]; // '/random_noise' -} DW_blower_aerodynamics_sim_mo_T; - -// Block signals for system '/blower_aerodynamics' -typedef struct { - real32_T Add1[6]; // '/Add1' - real32_T Constant7; // '/Constant7' -} B_blower_aerodynamics_sim_m_f_T; - -// Block states (auto storage) for system '/blower_aerodynamics' -typedef struct { - real_T DiscreteTimeIntegrator_DSTATE[6];// '/Discrete-Time Integrator' - real_T NextOutput[6]; // '/random_noise' - uint32_T RandSeed[6]; // '/random_noise' -} DW_blower_aerodynamics_sim__c_T; - -// Parameters for system: '/blower_aerodynamics' -struct P_blower_aerodynamics_sim_mod_T_ { - real_T DiscreteTimeIntegrator_gainval;// Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - real_T DiscreteTimeIntegrator_IC; // Expression: 0 - // Referenced by: '/Discrete-Time Integrator' - - real_T random_noise_Mean; // Expression: 0 - // Referenced by: '/random_noise' - - real_T random_noise_StdDev[6]; // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - real32_T Constant4_Value; // Expression: single(2) - // Referenced by: '/Constant4' - - real32_T Constant7_Value; // Expression: single(0) - // Referenced by: '/Constant7' - -}; - -// Parameters for system: '/blower_aerodynamics' -struct P_blower_aerodynamics_sim_m_m_T_ { - real_T DiscreteTimeIntegrator_gainval;// Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - real_T DiscreteTimeIntegrator_IC; // Expression: 0 - // Referenced by: '/Discrete-Time Integrator' - - real_T random_noise_Mean; // Expression: 0 - // Referenced by: '/random_noise' - - real_T random_noise_StdDev[6]; // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - real32_T Constant4_Value; // Expression: single(2) - // Referenced by: '/Constant4' - - real32_T Constant7_Value; // Expression: single(0) - // Referenced by: '/Constant7' - -}; - -void si_blower_aerodynamics_Init(DW_blower_aerodynamics_sim_mo_T *localDW, - P_blower_aerodynamics_sim_mod_T *localP, real_T rtp_noz_randn_seed); -void sim_mod_blower_aerodynamics(real32_T rtu_rotor_speed, const real32_T - rtu_nozzle_areas[6], B_blower_aerodynamics_sim_mod_T *localB, - DW_blower_aerodynamics_sim_mo_T *localDW, P_blower_aerodynamics_sim_mod_T - *localP, real32_T rtp_imp_zero_thrust_area, real32_T - rtp_imp_zero_thrust_area_error, real32_T rtp_noise_on_flag, const real32_T - rtp_noz_cd[6], const real32_T rtp_noz_cd_error[6], const real32_T - rtp_imp_cdp_lookup[334], const real32_T rtp_imp_area_lookup[334], real32_T - rtp_imp_diameter, real32_T rtp_const_air_den, real32_T rtp_noz_thrust_sf, - const real_T rtp_noz_thrust_noise_feedback[6]); -void blower_aerodynamics_d_Init(DW_blower_aerodynamics_sim__c_T *localDW, - P_blower_aerodynamics_sim_m_m_T *localP, real_T rtp_noz_randn_seed); -void sim_m_blower_aerodynamics_j(real32_T rtu_rotor_speed, const real32_T - rtu_nozzle_areas[6], B_blower_aerodynamics_sim_m_f_T *localB, - DW_blower_aerodynamics_sim__c_T *localDW, P_blower_aerodynamics_sim_m_m_T - *localP, real32_T rtp_imp_zero_thrust_area, real32_T - rtp_imp_zero_thrust_area_error, real32_T rtp_noise_on_flag, const real32_T - rtp_noz_cd[6], const real32_T rtp_noz_cd_error[6], const real32_T - rtp_imp_cdp_lookup[334], const real32_T rtp_imp_area_lookup[334], real32_T - rtp_imp_diameter, real32_T rtp_const_air_den, real32_T rtp_noz_thrust_sf, - const real_T rtp_noz_thrust_noise_feedback[6]); - -#endif // RTW_HEADER_blower_aerodynamics_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/buildInfo.mat b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/buildInfo.mat deleted file mode 100644 index 12aca96395..0000000000 Binary files a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/codeInfo.mat b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/codeInfo.mat deleted file mode 100644 index 23e2e8807e..0000000000 Binary files a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/codeInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/defines.txt b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/defines.txt deleted file mode 100644 index 13eb57b846..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/defines.txt +++ /dev/null @@ -1,15 +0,0 @@ -RT_MALLOC -MODEL=sim_model_lib0 -NUMST=5 -NCSTATES=0 -HAVESTDIO -UNIX -ONESTEPFCN=1 -TERMFCN=1 -MAT_FILE=0 -MULTI_INSTANCE_CODE=1 -INTEGER_CODE=0 -MT=1 -CLASSIC_INTERFACE=0 -ALLOCATIONFCN=1 -TID01EQ=0 diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/ert_main.cpp b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/ert_main.cpp deleted file mode 100644 index 38c9c40af2..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/ert_main.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// -// File: ert_main.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:45:47 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include -#include // This ert_main.c example uses printf/fflush -#include "sim_model_lib0.h" // Model's header file -#include "rtwtypes.h" - -// '/act_msg' -static act_msg sim_model_lib0_U_act_msg_l; - -// '/cmc_msg_in' -static cmc_msg sim_model_lib0_U_cmc_msg_in; - -// '/cvs_optical_flow_msg' -static cvs_optical_flow_msg sim_model_lib0_Y_cvs_optical_flow_msg_n; - -// '/cvs_handrail_msg' -static cvs_handrail_msg sim_model_lib0_Y_cvs_handrail_msg_h; - -// '/cmc_msg' -static cmc_msg sim_model_lib0_Y_cmc_msg_c; - -// '/imu_msg' -static imu_msg sim_model_lib0_Y_imu_msg_o; - -// '/env_msg' -static env_msg sim_model_lib0_Y_env_msg_i; - -// '/bpm_msg' -static bpm_msg sim_model_lib0_Y_bpm_msg_h; - -// '/cvs_registration_pulse' -static cvs_registration_pulse sim_model_lib0_Y_cvs_registration_pulse_d; - -// '/cvs_landmark_msg' -static cvs_landmark_msg sim_model_lib0_Y_cvs_landmark_msg_n; - -// '/cvs_ar_tag_msg' -static cvs_landmark_msg sim_model_lib0_Y_cvs_ar_tag_msg; - -// '/ex_time_msg' -static ex_time_msg sim_model_lib0_Y_ex_time_msg_m; -const char *RT_MEMORY_ALLOCATION_ERROR = "memory allocation error"; - -// -// Associating rt_OneStep with a real-time clock or interrupt service routine -// is what makes the generated code "real-time". The function rt_OneStep is -// always associated with the base rate of the model. Subrates are managed -// by the base rate from inside the generated code. Enabling/disabling -// interrupts and floating point context switches are target specific. This -// example code indicates where these should take place relative to executing -// the generated code step function. Overrun behavior should be tailored to -// your application needs. This example simply sets an error status in the -// real-time model and returns from rt_OneStep. -// -void rt_OneStep(RT_MODEL_sim_model_lib0_T *const sim_model_lib0_M); -void rt_OneStep(RT_MODEL_sim_model_lib0_T *const sim_model_lib0_M) -{ - static boolean_T OverrunFlags[5] = { 0, 0, 0, 0, 0 }; - - static boolean_T eventFlags[5] = { 0, 0, 0, 0, 0 };// Model has 5 rates - - static int_T taskCounter[5] = { 0, 0, 0, 0, 0 }; - - int_T i; - - // Disable interrupts here - - // Check base rate for overrun - if (OverrunFlags[0]) { - rtmSetErrorStatus(sim_model_lib0_M, "Overrun"); - return; - } - - OverrunFlags[0] = true; - - // Save FPU context here (if necessary) - // Re-enable timer or interrupt here - - // - // For a bare-board target (i.e., no operating system), the - // following code checks whether any subrate overruns, - // and also sets the rates that need to run this time step. - - for (i = 1; i < 5; i++) { - if (taskCounter[i] == 0) { - if (eventFlags[i]) { - OverrunFlags[0] = false; - OverrunFlags[i] = true; - - // Sampling too fast - rtmSetErrorStatus(sim_model_lib0_M, "Overrun"); - return; - } - - eventFlags[i] = true; - } - } - - taskCounter[1]++; - if (taskCounter[1] == 10) { - taskCounter[1]= 0; - } - - taskCounter[2]++; - if (taskCounter[2] == 13) { - taskCounter[2]= 0; - } - - taskCounter[3]++; - if (taskCounter[3] == 17) { - taskCounter[3]= 0; - } - - taskCounter[4]++; - if (taskCounter[4] == 21) { - taskCounter[4]= 0; - } - - // Set model inputs associated with base rate here - - // Step the model for base rate - sim_model_lib0_step0(sim_model_lib0_M, &sim_model_lib0_U_act_msg_l, - &sim_model_lib0_U_cmc_msg_in, - &sim_model_lib0_Y_cvs_optical_flow_msg_n, - &sim_model_lib0_Y_cvs_handrail_msg_h, - &sim_model_lib0_Y_cmc_msg_c, &sim_model_lib0_Y_imu_msg_o, - &sim_model_lib0_Y_env_msg_i, &sim_model_lib0_Y_bpm_msg_h, - &sim_model_lib0_Y_cvs_registration_pulse_d, - &sim_model_lib0_Y_cvs_landmark_msg_n, - &sim_model_lib0_Y_cvs_ar_tag_msg, - &sim_model_lib0_Y_ex_time_msg_m); - - // Get model outputs here - - // Indicate task for base rate complete - OverrunFlags[0] = false; - - // Step the model for any subrate - for (i = 1; i < 5; i++) { - // If task "i" is running, don't run any lower priority task - if (OverrunFlags[i]) { - return; - } - - if (eventFlags[i]) { - OverrunFlags[i] = true; - - // Set model inputs associated with subrates here - - // Step the model for subrate "i" - switch (i) { - case 1 : - sim_model_lib0_step1(sim_model_lib0_M); - - // Get model outputs here - break; - - case 2 : - sim_model_lib0_step2(sim_model_lib0_M); - - // Get model outputs here - break; - - case 3 : - sim_model_lib0_step3(sim_model_lib0_M); - - // Get model outputs here - break; - - case 4 : - sim_model_lib0_step4(sim_model_lib0_M); - - // Get model outputs here - break; - - default : - break; - } - - // Indicate task complete for sample time "i" - OverrunFlags[i] = false; - eventFlags[i] = false; - } - } - - // Disable interrupts here - // Restore FPU context here (if necessary) - // Enable interrupts here -} - -// -// The example "main" function illustrates what is required by your -// application code to initialize, execute, and terminate the generated code. -// Attaching rt_OneStep to a real-time clock is target specific. This example -// illustrates how you do this relative to initializing the model. -// -int_T main(int_T argc, const char *argv[]) -{ - RT_MODEL_sim_model_lib0_T *sim_model_lib0_M; - - // Unused arguments - (void)(argc); - (void)(argv); - - // Allocate model data - sim_model_lib0_M = sim_model_lib0(&sim_model_lib0_U_act_msg_l, - &sim_model_lib0_U_cmc_msg_in, &sim_model_lib0_Y_cvs_optical_flow_msg_n, - &sim_model_lib0_Y_cvs_handrail_msg_h, &sim_model_lib0_Y_cmc_msg_c, - &sim_model_lib0_Y_imu_msg_o, &sim_model_lib0_Y_env_msg_i, - &sim_model_lib0_Y_bpm_msg_h, &sim_model_lib0_Y_cvs_registration_pulse_d, - &sim_model_lib0_Y_cvs_landmark_msg_n, &sim_model_lib0_Y_cvs_ar_tag_msg, - &sim_model_lib0_Y_ex_time_msg_m); - if (sim_model_lib0_M == NULL) { - (void)fprintf(stderr,"Memory allocation error during model " - "registration"); - return(1); - } - - if (rtmGetErrorStatus(sim_model_lib0_M) != NULL) { - (void)fprintf(stderr,"Error during model registration: %s\n", - rtmGetErrorStatus(sim_model_lib0_M)); - - // Disable rt_OneStep() here - - // Terminate model - sim_model_lib0_terminate(sim_model_lib0_M); - return(1); - } - - // Initialize model - sim_model_lib0_initialize(sim_model_lib0_M, &sim_model_lib0_U_act_msg_l, - &sim_model_lib0_U_cmc_msg_in, &sim_model_lib0_Y_cvs_optical_flow_msg_n, - &sim_model_lib0_Y_cvs_handrail_msg_h, &sim_model_lib0_Y_cmc_msg_c, - &sim_model_lib0_Y_imu_msg_o, &sim_model_lib0_Y_env_msg_i, - &sim_model_lib0_Y_bpm_msg_h, &sim_model_lib0_Y_cvs_registration_pulse_d, - &sim_model_lib0_Y_cvs_landmark_msg_n, &sim_model_lib0_Y_cvs_ar_tag_msg, - &sim_model_lib0_Y_ex_time_msg_m); - - // Attach rt_OneStep to a timer or interrupt service routine with - // period 0.016 seconds (the model's base sample time) here. The - // call syntax for rt_OneStep is - // - // rt_OneStep(sim_model_lib0_M); - - printf("Warning: The simulation will run forever. " - "Generated ERT main won't simulate model step behavior. " - "To change this behavior select the 'MAT-file logging' option.\n"); - fflush((NULL)); - while (rtmGetErrorStatus(sim_model_lib0_M) == (NULL)) { - // Perform other application tasks here - } - - // Disable rt_OneStep() here - - // Terminate model - sim_model_lib0_terminate(sim_model_lib0_M); - return 0; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/modelsources.txt b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/modelsources.txt deleted file mode 100644 index eaaf927b5a..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/modelsources.txt +++ /dev/null @@ -1 +0,0 @@ - sim_model_lib0.cpp blower_aerodynamics.cpp diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/rtw_proj.tmw b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/rtw_proj.tmw deleted file mode 100644 index 828794e7de..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/rtw_proj.tmw +++ /dev/null @@ -1,4 +0,0 @@ -Simulink Coder project for sim_model_lib0 using . MATLAB root = /usr/local/MATLAB/R2016b. SimStruct date: 22-Jul-2016 13:46:14 -This file is generated by Simulink Coder for use by the make utility -to determine when to rebuild objects when the name of the current Simulink Coder project changes. -The rtwinfomat located at: ../slprj/ert/sim_model_lib0/tmwinternal/binfo.mat diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0.cpp b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0.cpp deleted file mode 100644 index 74022b4e16..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0.cpp +++ /dev/null @@ -1,12265 +0,0 @@ -// -// File: sim_model_lib0.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:45:47 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "sim_model_lib0.h" -#include "sim_model_lib0_private.h" - -const env_msg sim_model_lib0_rtZenv_msg = { - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ECI_B - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // Q_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ECI_B - - { - 0.0F, 0.0F, 0.0F } - , // alpha_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // fan_torques_B - - { - 0.0F, 0.0F, 0.0F } - // fan_forces_B -} ; // env_msg ground - -const ex_time_msg sim_model_lib0_rtZex_time_msg = { - 0U, // timestamp_sec - 0U // timestamp_nsec -} ; // ex_time_msg ground - -const cvs_landmark_msg sim_model_lib0_rtZcvs_landmark_msg = { - 0U, // cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cvs_landmarks - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cvs_observations - - { - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U } - // cvs_valid_flag -} ; // cvs_landmark_msg ground - -const cvs_optical_flow_msg sim_model_lib0_rtZcvs_optical_flow_msg = { - 0U, // cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F } - , // cvs_observations - - { - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U } - , // cvs_valid_flag - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - // cvs_id_tag -} ; // cvs_optical_flow_msg ground - -const cvs_handrail_msg sim_model_lib0_rtZcvs_handrail_msg = { - 0U, // cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cvs_landmarks - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cvs_observations - - { - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U } - , // cvs_valid_flag - 0U, // cvs_3d_knowledge_flag - - { - 0.0F, 0.0F, 0.0F } - , // cvs_handrail_local_pos - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // cvs_handrail_local_quat - 0U // cvs_handrail_update_global_pose_flag -} ; // cvs_handrail_msg ground - -const cvs_registration_pulse sim_model_lib0_rtZcvs_registration_pulse = { - 0U, // cvs_ar_tag_pulse - 0U, // cvs_landmark_pulse - 0U, // cvs_optical_flow_pulse - 0U // cvs_handrail_pulse -} ; // cvs_registration_pulse ground - -const bpm_msg sim_model_lib0_rtZbpm_msg = { - 0U, // bpm_timestamp_sec - 0U, // bpm_timestamp_nsec - - { - 0.0F, 0.0F } - , // bpm_motor_curr - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // bpm_servo_curr - - { - 0.0F, 0.0F, 0.0F } - , // bpm_torque_B - - { - 0.0F, 0.0F, 0.0F } - , // bpm_force_B - - { - 0.0F, 0.0F } - , // bpm_motor_speed - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - // bpm_nozzle_theta -} ; // bpm_msg ground - -const imu_msg sim_model_lib0_rtZimu_msg = { - 0U, // imu_timestamp_sec - 0U, // imu_timestamp_nsec - - { - 0.0F, 0.0F, 0.0F } - , // imu_A_B_ECI_sensor - - { - 0.0F, 0.0F, 0.0F } - , // imu_accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // imu_omega_B_ECI_sensor - - { - 0.0F, 0.0F, 0.0F } - , // imu_gyro_bias - 0U, // imu_validity_flag - 0U // imu_sat_flag -} ; // imu_msg ground - -// Forward declaration for local functions -static void sim_model__format_AR_tag_output(const real32_T points_in_iss[108], - const real32_T points_in_cam[72], boolean_T valid_in[36], real32_T - points_out_iss[150], real32_T points_out_cam[100], real32_T valid_out[50]); -static void sim_model_lib0_rand(real_T varargin_2, real_T r_data[], int32_T - r_sizes[2], DW_sim_model_lib0_T *sim_model_lib0_DW); -static void sim_model_lib0_randperm(real_T n, real_T p_data[], int32_T p_sizes[2], - DW_sim_model_lib0_T *sim_model_lib0_DW); -static void sim_mode_format_handrail_output(const real32_T points_in_iss[1512], - const real32_T points_in_cam[1512], const boolean_T valid_in[504], real32_T - points_out_iss[150], real32_T points_out_cam[150], real32_T valid_out[50], - B_sim_model_lib0_T *sim_model_lib0_B, DW_sim_model_lib0_T *sim_model_lib0_DW); -static void sim__pphdimglohlnoppp_mergesort(int32_T idx_data[], const real32_T - x_data[], const int32_T x_sizes[2], const int32_T dir[2], int32_T n, - B_sim_model_lib0_T *sim_model_lib0_B); -static void sim_mo_aaieppphglfcglng_sortIdx(const real32_T x_data[], const - int32_T x_sizes[2], const int32_T col[2], int32_T idx_data[], int32_T - *idx_sizes, B_sim_model_lib0_T *sim_model_lib0_B); -static void hdjmcjmophlfkngl_apply_row_perm(real32_T y_data[], int32_T y_sizes[2], - const int32_T idx_data[], B_sim_model_lib0_T *sim_model_lib0_B); -static void sim_m_mglflfcbkfkfbiec_sortrows(real32_T y_data[], int32_T y_sizes[2], - real_T ndx_data[], int32_T *ndx_sizes, B_sim_model_lib0_T *sim_model_lib0_B); -static void sim__gdbacbaajekfglng_mergesort(int32_T idx_data[], const int32_T - x_data[], int32_T n, B_sim_model_lib0_T *sim_model_lib0_B); -static void sim_mo_dbaaphlnfkfkphdb_sortIdx(const int32_T x_data[], const - int32_T x_sizes, int32_T idx_data[], int32_T *idx_sizes, B_sim_model_lib0_T - *sim_model_lib0_B); -static void sim_fkfchlfkgdbimgdj_do_vectors(const real32_T a[50], const real32_T - b_data[], const int32_T b_sizes, real32_T c_data[], int32_T *c_sizes, int32_T - ia_data[], int32_T *ia_sizes, int32_T ib_data[], int32_T *ib_sizes, - B_sim_model_lib0_T *sim_model_lib0_B); -static void sim_model_lib0_rand_n(real_T varargin_2, real_T r_data[], int32_T - r_sizes[2], DW_sim_model_lib0_T *sim_model_lib0_DW); -static void sim__aiekglfcjekfdbai_mergesort(int32_T idx_data[], const real_T - x_data[], int32_T n, B_sim_model_lib0_T *sim_model_lib0_B); -static void sim_mo_cbimppphbaaaaiec_sortIdx(const real_T x_data[], const int32_T - x_sizes[2], int32_T idx_data[], int32_T idx_sizes[2], B_sim_model_lib0_T - *sim_model_lib0_B); -static void sim_model_lib0_randperm_i(real_T n, real_T p_data[], int32_T - p_sizes[2], B_sim_model_lib0_T *sim_model_lib0_B, DW_sim_model_lib0_T - *sim_model_lib0_DW); -static void sim_mo_lfcjjmgdkfkniekn_sortIdx(const real32_T x[17264], const - int32_T col[2], int32_T idx[8632], B_sim_model_lib0_T *sim_model_lib0_B); -static void imglgdjmcjmomgdj_apply_row_perm(real32_T y[17264], const int32_T - idx[8632], B_sim_model_lib0_T *sim_model_lib0_B); -static void sim_m_nopphdbaaiecfkng_sortrows(real32_T y[17264], real_T ndx[8632], - B_sim_model_lib0_T *sim_model_lib0_B); -static void sim_model_lib0_rand_o(real_T varargin_2, real_T r_data[], int32_T - r_sizes[2], DW_sim_model_lib0_T *sim_model_lib0_DW); -static void sim__kfkflfcjohdbglno_mergesort(int32_T idx_data[], const real_T - x_data[], int32_T n, B_sim_model_lib0_T *sim_model_lib0_B); -static void sim_mo_bimgaiekaimglfkf_sortIdx(const real_T x_data[], const int32_T - x_sizes[2], int32_T idx_data[], int32_T idx_sizes[2], B_sim_model_lib0_T - *sim_model_lib0_B); -static void sim_model_lib0_randperm_e(real_T n, real_T p_data[], int32_T - p_sizes[2], B_sim_model_lib0_T *sim_model_lib0_B, DW_sim_model_lib0_T - *sim_model_lib0_DW); -static void sim_mode_format_landmark_output(const real32_T points_in_iss[25896], - const real32_T points_in_cam[17264], const boolean_T valid_in[8632], real32_T - points_out_iss[150], real32_T points_out_cam[100], real32_T valid_out[50], - B_sim_model_lib0_T *sim_model_lib0_B, DW_sim_model_lib0_T *sim_model_lib0_DW); -const act_msg sim_model_lib0_rtZact_msg = { 0U,// act_timestamp_sec - 0U, // act_timestamp_nsec - { 0U, 0U }, // act_impeller_speed_cmd - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// act_servo_pwm_cmd - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// act_nozzle_theta - { 0.0F, 0.0F, 0.0F }, // act_predicted_force_B - { 0.0F, 0.0F, 0.0F } // act_predicted_torque_B -}; - -const cmc_msg sim_model_lib0_rtZcmc_msg = { { 0U,// timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmc_state_cmd_a - { 0U, // timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmc_state_cmd_b - 0U, // cmc_mode_cmd - 0U, // speed_gain_cmd - 0U, // localization_mode_cmd - { 0.0F, 0.0F, 0.0F }, // att_kp - { 0.0F, 0.0F, 0.0F }, // att_ki - { 0.0F, 0.0F, 0.0F }, // omega_kd - { 0.0F, 0.0F, 0.0F }, // pos_kp - { 0.0F, 0.0F, 0.0F }, // pos_ki - { 0.0F, 0.0F, 0.0F }, // vel_kd - { 0.0F, 0.0F, 0.0F }, // center_of_mass - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// inertia_matrix - 0.0F // mass -}; - -// -// Output and update for action system: -// '/Normalize' -// '/Normalize' -// -void sim_model_lib0_Normalize(const real32_T rtu_Vec[4], real32_T rtu_Magnitude, - real32_T rty_Normalized_Vec[4]) -{ - // Product: '/Divide' - rty_Normalized_Vec[0] = rtu_Vec[0] / rtu_Magnitude; - rty_Normalized_Vec[1] = rtu_Vec[1] / rtu_Magnitude; - rty_Normalized_Vec[2] = rtu_Vec[2] / rtu_Magnitude; - rty_Normalized_Vec[3] = rtu_Vec[3] / rtu_Magnitude; -} - -// -// Termination for action system: -// '/Normalize' -// '/Normalize' -// -void sim_model_li_Normalize_Term(void) -{ -} - -// -// Output and update for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void sim_model_lib0_Normalize_e(const real32_T rtu_Vec[3], real32_T - rtu_Magnitude, real32_T rty_Normalized_Vec[3]) -{ - // Product: '/Divide' - rty_Normalized_Vec[0] = rtu_Vec[0] / rtu_Magnitude; - rty_Normalized_Vec[1] = rtu_Vec[1] / rtu_Magnitude; - rty_Normalized_Vec[2] = rtu_Vec[2] / rtu_Magnitude; -} - -// -// Termination for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void sim_model__Normalize_m_Term(void) -{ -} - -// -// System initialize for iterator system: -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// -void pinhole_projection_mod_Init(int32_T NumIters, - DW_pinhole_projection_model_s_T localDW[], P_pinhole_projection_model_si_T - *localP, real_T rtp_pixel_noise_var, real_T rtp_pixel_noise_seed) -{ - // local scratch DWork variables - int32_T ForEach_itr; - uint32_T tseed; - int32_T r; - int32_T t; - real_T tmp; - for (ForEach_itr = 0; ForEach_itr < NumIters; ForEach_itr++) { - // InitializeConditions for RandomNumber: '/pixel_noise' - tmp = floor(rtp_pixel_noise_seed + 1.0); - if (rtIsNaN(tmp) || rtIsInf(tmp)) { - tmp = 0.0; - } else { - tmp = fmod(tmp, 4.294967296E+9); - } - - tseed = tmp < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-tmp : (uint32_T) - tmp; - r = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - localDW[ForEach_itr].CoreSubsys.RandSeed = tseed; - localDW[ForEach_itr].CoreSubsys.NextOutput = rt_nrand_Upu32_Yd_f_pw_snf - (&localDW[ForEach_itr].CoreSubsys.RandSeed) * sqrt(rtp_pixel_noise_var) + - localP->CoreSubsys.pixel_noise_Mean; - - // End of InitializeConditions for RandomNumber: '/pixel_noise' - - // InitializeConditions for RandomNumber: '/pixel_noise1' - tmp = floor(rtp_pixel_noise_seed + 2.0); - if (rtIsNaN(tmp) || rtIsInf(tmp)) { - tmp = 0.0; - } else { - tmp = fmod(tmp, 4.294967296E+9); - } - - tseed = tmp < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-tmp : (uint32_T) - tmp; - r = (int32_T)(uint32_T)(tseed >> 16U); - t = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)t) + - (uint32_T)r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - localDW[ForEach_itr].CoreSubsys.RandSeed_k = tseed; - localDW[ForEach_itr].CoreSubsys.NextOutput_n = rt_nrand_Upu32_Yd_f_pw_snf - (&localDW[ForEach_itr].CoreSubsys.RandSeed_k) * sqrt(rtp_pixel_noise_var) - + localP->CoreSubsys.pixel_noise1_Mean; - - // End of InitializeConditions for RandomNumber: '/pixel_noise1' - } -} - -// -// Start for iterator system: -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// -void pinhole_projection_mo_Start(int32_T NumIters, - DW_pinhole_projection_model_s_T localDW[]) -{ - // local scratch DWork variables - int32_T ForEach_itr; - for (ForEach_itr = 0; ForEach_itr < NumIters; ForEach_itr++) { - localDW[ForEach_itr].CoreSubsys.NextOutput = 0.0; - localDW[ForEach_itr].CoreSubsys.NextOutput_n = 0.0; - } -} - -// -// Output and update for iterator system: -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// -void si_pinhole_projection_model(int32_T NumIters, const real32_T - rtu_P_points_cam_cam[], real32_T rty_P_points_2D_cam[], boolean_T - rty_points_in_FOV_flag[], DW_pinhole_projection_model_s_T localDW[], - P_pinhole_projection_model_si_T *localP, uint8_T rtp_noise_on_flag, real32_T - rtp_cam_focal_length_Y, real32_T rtp_cam_focal_length_X, real_T - rtp_pixel_noise_var, real32_T rtp_cam_num_X_pixels, real32_T - rtp_cam_num_Y_pixels, real32_T rtp_cam_min_dist, real32_T rtp_cam_max_dist, - const real32_T rtp_cam_pointing[3]) -{ - // local scratch DWork variables - int32_T ForEach_itr; - real32_T rtb_DotProduct1; - real32_T rtb_Switch_j_idx_0; - real32_T rtb_Switch_j_idx_1; - - // Outputs for Iterator SubSystem: '/pinhole_projection_model' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr = 0; ForEach_itr < NumIters; ForEach_itr++) { - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - // Constant: '/Constant4' - // DataTypeConversion: '/Data Type Conversion1' - // DataTypeConversion: '/Data Type Conversion2' - // ForEachSliceSelector: '/ImpSel_InsertedFor_P_points_cam_cam_at_outport_0' - // Gain: '/Gain' - // Gain: '/Gain1' - // Product: '/Product1' - // Product: '/Product2' - // RandomNumber: '/pixel_noise' - // RandomNumber: '/pixel_noise1' - // Sum: '/Sum1' - // Sum: '/Sum3' - - if (rtu_P_points_cam_cam[(int32_T)((int32_T)(NumIters << 1) + ForEach_itr)] > - localP->CoreSubsys.Switch_Threshold) { - rtb_Switch_j_idx_0 = rtp_cam_focal_length_X * - rtu_P_points_cam_cam[ForEach_itr] / rtu_P_points_cam_cam[(int32_T) - ((int32_T)(NumIters << 1) + ForEach_itr)] + (real32_T)rtp_noise_on_flag * - (real32_T)localDW[ForEach_itr].CoreSubsys.NextOutput; - rtb_Switch_j_idx_1 = 1.0F / rtu_P_points_cam_cam[(int32_T)((int32_T) - (NumIters << 1) + ForEach_itr)] * rtu_P_points_cam_cam[(int32_T) - (NumIters + ForEach_itr)] * rtp_cam_focal_length_Y + (real32_T) - rtp_noise_on_flag * (real32_T)localDW[ForEach_itr]. - CoreSubsys.NextOutput_n; - } else { - rtb_Switch_j_idx_0 = localP->CoreSubsys.Constant_Value[0]; - rtb_Switch_j_idx_1 = localP->CoreSubsys.Constant_Value[1]; - } - - // End of Switch: '/Switch' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - // ForEachSliceSelector: '/ImpSel_InsertedFor_P_points_cam_cam_at_outport_0' - - rtb_DotProduct1 = (real32_T)sqrt((real_T)((rtu_P_points_cam_cam[(int32_T) - (NumIters + ForEach_itr)] * rtu_P_points_cam_cam[(int32_T)(NumIters + - ForEach_itr)] + rtu_P_points_cam_cam[ForEach_itr] * - rtu_P_points_cam_cam[ForEach_itr]) + rtu_P_points_cam_cam[(int32_T) - ((int32_T)(NumIters << 1) + ForEach_itr)] * rtu_P_points_cam_cam[(int32_T) - ((int32_T)(NumIters << 1) + ForEach_itr)])); - - // Update for RandomNumber: '/pixel_noise' - localDW[ForEach_itr].CoreSubsys.NextOutput = rt_nrand_Upu32_Yd_f_pw_snf - (&localDW[ForEach_itr].CoreSubsys.RandSeed) * sqrt(rtp_pixel_noise_var) + - localP->CoreSubsys.pixel_noise_Mean; - - // Update for RandomNumber: '/pixel_noise1' - localDW[ForEach_itr].CoreSubsys.NextOutput_n = rt_nrand_Upu32_Yd_f_pw_snf - (&localDW[ForEach_itr].CoreSubsys.RandSeed_k) * sqrt(rtp_pixel_noise_var) - + localP->CoreSubsys.pixel_noise1_Mean; - - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_points_in_FOV_flag_at_inport_0' incorporates: - // Abs: '/Abs' - // Abs: '/Abs1' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Constant: '/Constant4' - // Constant: '/Constant' - // DotProduct: '/Dot Product1' - // ForEachSliceSelector: '/ImpSel_InsertedFor_P_points_cam_cam_at_outport_0' - // Logic: '/Logical Operator' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Relational Operator3' - // RelationalOperator: '/Compare' - - rty_points_in_FOV_flag[ForEach_itr] = (((real32_T)fabs((real_T) - rtb_Switch_j_idx_0) <= rtp_cam_num_X_pixels / 2.0F) && ((real32_T)fabs - ((real_T)rtb_Switch_j_idx_1) <= rtp_cam_num_Y_pixels / 2.0F) && - (rtb_DotProduct1 >= rtp_cam_min_dist) && (rtb_DotProduct1 <= - rtp_cam_max_dist) && ((rtu_P_points_cam_cam[(int32_T)(NumIters + - ForEach_itr)] * rtp_cam_pointing[1] + rtu_P_points_cam_cam[ForEach_itr] * - rtp_cam_pointing[0]) + rtu_P_points_cam_cam[(int32_T)((int32_T)(NumIters << - 1) + ForEach_itr)] * rtp_cam_pointing[2] > - localP->CoreSubsys.Constant_Value_n)); - - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_P_points_2D_cam_at_inport_0' - rty_P_points_2D_cam[ForEach_itr] = rtb_Switch_j_idx_0; - rty_P_points_2D_cam[(int32_T)(NumIters + ForEach_itr)] = rtb_Switch_j_idx_1; - } - - // End of Outputs for SubSystem: '/pinhole_projection_model' -} - -// -// Termination for iterator system: -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// '/pinhole_projection_model' -// -void pinhole_projection_mod_Term(void) -{ -} - -// -// System initialize for enable system: -// '/latch_nozzle_thrust_matricies' -// '/latch_nozzle_thrust_matricies' -// -void latch_nozzle_thrust_ma_Init(B_latch_nozzle_thrust_matrici_T *localB, - P_latch_nozzle_thrust_matrici_T *localP) -{ - int32_T i; - for (i = 0; i < 18; i++) { - // SystemInitialize for Outport: '/thrust2torque_B' - localB->OutportBufferForthrust2torque_B[i] = localP->thrust2torque_B_Y0; - - // SystemInitialize for Outport: '/thrust2force_B' - localB->OutportBufferForthrust2force_B[i] = localP->thrust2force_B_Y0; - } -} - -// -// Output and update for enable system: -// '/latch_nozzle_thrust_matricies' -// '/latch_nozzle_thrust_matricies' -// -void latch_nozzle_thrust_matrici(boolean_T rtu_Enable, const real32_T - rtu_veh_cm[3], B_latch_nozzle_thrust_matrici_T *localB, - P_latch_nozzle_thrust_matrici_T *localP, const real32_T - rtp_nozzle_orientation_offset[24], const real32_T rtp_P_nozzle_B_B[18], const - real32_T rtp_nozzle_orientation_B[18], const real32_T rtp_P_nozzle_B_B_offset - [18], const real_T rtp_P_CG_B_B_error[3], real32_T rtp_noise_on_flag) -{ - // local scratch DWork variables - int32_T ForEach_itr; - real32_T nozzle_moment_arm[18]; - real_T b[18]; - int32_T ibmat; - int32_T itilerow; - real32_T c[18]; - real32_T rtb_Assignment_h2[9]; - real32_T rtb_Sum_bc; - real32_T rtb_ImpAsg_InsertedFor_rotated_[18]; - real_T rtb_Sum2_c[3]; - int32_T jcol; - real32_T tmp[9]; - real32_T rtb_Switch_e_0[9]; - real32_T rtb_Assignment_a[9]; - const real32_T *rtb_Switch_e_1; - - // Outputs for Enabled SubSystem: '/latch_nozzle_thrust_matricies' incorporates: - // EnablePort: '/Enable' - - if (rtu_Enable) { - // Switch: '/Switch' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant5' - // Constant: '/Constant8' - - if (rtp_noise_on_flag > localP->Switch_Threshold) { - rtb_Switch_e_1 = &rtp_nozzle_orientation_offset[0]; - } else { - rtb_Switch_e_1 = &localP->Constant2_Value[0]; - } - - // End of Switch: '/Switch' - - // Outputs for Iterator SubSystem: '/For Each Subsystem' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr = 0; ForEach_itr < 6; ForEach_itr++) { - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_bc = rtb_Switch_e_1[(int32_T)(18 + ForEach_itr)] * rtb_Switch_e_1 - [(int32_T)(18 + ForEach_itr)] * localP->CoreSubsys.Gain_Gain - (real32_T) - localP->CoreSubsys.Constant1_Value; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (jcol = 0; jcol < 9; jcol++) { - rtb_Assignment_h2[jcol] = (real32_T)localP-> - CoreSubsys.Constant2_Value[jcol]; - } - - rtb_Assignment_h2[0] = rtb_Sum_bc; - rtb_Assignment_h2[4] = rtb_Sum_bc; - rtb_Assignment_h2[8] = rtb_Sum_bc; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - - rtb_Sum_bc = rtb_Switch_e_1[(int32_T)(18 + ForEach_itr)] * - localP->CoreSubsys.Gain1_Gain; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp[0] = (real32_T)localP->CoreSubsys.Constant3_Value; - tmp[1] = rtb_Switch_e_1[(int32_T)(12 + ForEach_itr)]; - tmp[2] = rtb_Switch_e_1[(int32_T)(6 + ForEach_itr)] * - localP->CoreSubsys.Gain_Gain_k; - tmp[3] = rtb_Switch_e_1[(int32_T)(12 + ForEach_itr)] * - localP->CoreSubsys.Gain1_Gain_f; - tmp[4] = (real32_T)localP->CoreSubsys.Constant3_Value; - tmp[5] = rtb_Switch_e_1[ForEach_itr]; - tmp[6] = rtb_Switch_e_1[(int32_T)(ForEach_itr + 6)]; - tmp[7] = localP->CoreSubsys.Gain2_Gain * rtb_Switch_e_1[ForEach_itr]; - tmp[8] = (real32_T)localP->CoreSubsys.Constant3_Value; - - // Product: '/Product1' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_misalignment_quats_at_outport_0' - // Gain: '/Gain2' - - for (jcol = 0; jcol < 3; jcol++) { - rtb_Switch_e_0[jcol] = rtb_Switch_e_1[(int32_T)((int32_T)(6 * jcol) + - ForEach_itr)] * rtb_Switch_e_1[ForEach_itr]; - rtb_Switch_e_0[(int32_T)(jcol + 3)] = rtb_Switch_e_1[(int32_T)((int32_T) - (6 * jcol) + ForEach_itr)] * rtb_Switch_e_1[(int32_T)(ForEach_itr + 6)]; - rtb_Switch_e_0[(int32_T)(jcol + 6)] = rtb_Switch_e_1[(int32_T)((int32_T) - (6 * jcol) + ForEach_itr)] * rtb_Switch_e_1[(int32_T)(ForEach_itr + 12)]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (jcol = 0; jcol < 3; jcol++) { - rtb_Assignment_a[(int32_T)(3 * jcol)] = (rtb_Assignment_h2[(int32_T)(3 * - jcol)] - tmp[(int32_T)(3 * jcol)] * rtb_Sum_bc) + rtb_Switch_e_0 - [(int32_T)(3 * jcol)] * localP->CoreSubsys.Gain2_Gain_k; - rtb_Assignment_a[(int32_T)(1 + (int32_T)(3 * jcol))] = - (rtb_Assignment_h2[(int32_T)((int32_T)(3 * jcol) + 1)] - tmp[(int32_T) - ((int32_T)(3 * jcol) + 1)] * rtb_Sum_bc) + rtb_Switch_e_0[(int32_T) - ((int32_T)(3 * jcol) + 1)] * localP->CoreSubsys.Gain2_Gain_k; - rtb_Assignment_a[(int32_T)(2 + (int32_T)(3 * jcol))] = - (rtb_Assignment_h2[(int32_T)((int32_T)(3 * jcol) + 2)] - tmp[(int32_T) - ((int32_T)(3 * jcol) + 2)] * rtb_Sum_bc) + rtb_Switch_e_0[(int32_T) - ((int32_T)(3 * jcol) + 2)] * localP->CoreSubsys.Gain2_Gain_k; - } - - // End of Sum: '/Sum1' - for (jcol = 0; jcol < 3; jcol++) { - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_rotated_vectors_at_inport_0' incorporates: - // Constant: '/Constant3' - // ForEachSliceSelector: '/ImpSel_InsertedFor_unit_vectors_at_outport_0' - // Product: '/Product' - - rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(ForEach_itr + (int32_T)(6 * - jcol))] = rtb_Assignment_a[(int32_T)(jcol + 6)] * - rtp_nozzle_orientation_B[(int32_T)(ForEach_itr + 12)] + - (rtb_Assignment_a[(int32_T)(jcol + 3)] * rtp_nozzle_orientation_B - [(int32_T)(ForEach_itr + 6)] + rtb_Assignment_a[jcol] * - rtp_nozzle_orientation_B[ForEach_itr]); - } - } - - // End of Outputs for SubSystem: '/For Each Subsystem' - // MATLAB Function 'bpm_blower_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/MATLAB Function': ':1' - // NOTE: if number of nozzles change, must change this hard coded '6' below - // ':1:5' - for (jcol = 0; jcol < 3; jcol++) { - // Sum: '/Sum2' incorporates: - // Constant: '/Constant7' - // Gain: '/Gain2' - - rtb_Sum2_c[jcol] = (real_T)rtp_noise_on_flag * rtp_P_CG_B_B_error[jcol] + - (real_T)rtu_veh_cm[jcol]; - - // MATLAB Function: '/MATLAB Function' - ibmat = (int32_T)(jcol * 6); - for (itilerow = 0; itilerow < 6; itilerow++) { - b[(int32_T)(ibmat + itilerow)] = rtb_Sum2_c[jcol]; - } - } - - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant4' - // Gain: '/Gain' - // Sum: '/Sum' - - for (jcol = 0; jcol < 18; jcol++) { - nozzle_moment_arm[jcol] = (rtp_noise_on_flag * - rtp_P_nozzle_B_B_offset[jcol] + rtp_P_nozzle_B_B[jcol]) - (real32_T) - b[jcol]; - } - - // [m] Distance from nozzles to CG - // ':1:7' - // [-] Converts a nozzle thrust into resulting body torques - // ':1:8' - // [-] Converts a nozzle thrust into resulting body forces - for (jcol = 0; jcol < 6; jcol++) { - c[jcol] = nozzle_moment_arm[(int32_T)(jcol + 6)] * - rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(jcol + 12)] - - nozzle_moment_arm[(int32_T)(jcol + 12)] * - rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(jcol + 6)]; - c[(int32_T)(jcol + 6)] = nozzle_moment_arm[(int32_T)(jcol + 12)] * - rtb_ImpAsg_InsertedFor_rotated_[jcol] - rtb_ImpAsg_InsertedFor_rotated_ - [(int32_T)(jcol + 12)] * nozzle_moment_arm[jcol]; - c[(int32_T)(jcol + 12)] = rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(jcol + - 6)] * nozzle_moment_arm[jcol] - nozzle_moment_arm[(int32_T)(jcol + 6)] * - rtb_ImpAsg_InsertedFor_rotated_[jcol]; - - // SignalConversion: '/OutportBufferForthrust2force_B' - localB->OutportBufferForthrust2force_B[(int32_T)(3 * jcol)] = - -rtb_ImpAsg_InsertedFor_rotated_[jcol]; - localB->OutportBufferForthrust2force_B[(int32_T)(1 + (int32_T)(3 * jcol))] - = -rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(jcol + 6)]; - localB->OutportBufferForthrust2force_B[(int32_T)(2 + (int32_T)(3 * jcol))] - = -rtb_ImpAsg_InsertedFor_rotated_[(int32_T)(jcol + 12)]; - - // SignalConversion: '/OutportBufferForthrust2torque_B' - localB->OutportBufferForthrust2torque_B[(int32_T)(3 * jcol)] = -c[jcol]; - localB->OutportBufferForthrust2torque_B[(int32_T)(1 + (int32_T)(3 * jcol))] - = -c[(int32_T)(jcol + 6)]; - localB->OutportBufferForthrust2torque_B[(int32_T)(2 + (int32_T)(3 * jcol))] - = -c[(int32_T)(jcol + 12)]; - } - } - - // End of Outputs for SubSystem: '/latch_nozzle_thrust_matricies' -} - -// -// Termination for enable system: -// '/latch_nozzle_thrust_matricies' -// '/latch_nozzle_thrust_matricies' -// -void latch_nozzle_thrust_ma_Term(void) -{ -} - -// -// Output and update for atomic system: -// '/calc_nozzle_area' -// '/calc_nozzle_area' -// -void sim_model__calc_nozzle_area(const real32_T rtu_servo_theta[6], - B_calc_nozzle_area_sim_model__T *localB, real32_T rtp_noz_flap_count, real32_T - rtp_noz_min_theta, real32_T rtp_noz_flap_length, real32_T - rtp_noz_intake_height, const real32_T rtp_noz_width[6], real32_T - rtp_noz_gear_ratio) -{ - real32_T y; - int32_T i; - - // Gain: '/Gain12' - y = 1.0F / rtp_noz_gear_ratio; - for (i = 0; i < 6; i++) { - localB->Gain12[i] = y * rtu_servo_theta[i]; - - // Product: '/Product2' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant4' - // Constant: '/Constant5' - // Constant: '/Constant6' - // Product: '/Product1' - // Sum: '/Subtract' - // Sum: '/Subtract1' - // Trigonometry: '/Trigonometric Function' - - localB->Product2[i] = (rtp_noz_intake_height - (real32_T)cos((real_T) - (localB->Gain12[i] + rtp_noz_min_theta)) * rtp_noz_flap_length) * - rtp_noz_width[i] * rtp_noz_flap_count; - } - - // End of Gain: '/Gain12' -} - -// -// Termination for atomic system: -// '/calc_nozzle_area' -// '/calc_nozzle_area' -// -void sim_m_calc_nozzle_area_Term(void) -{ -} - -// -// Output and update for atomic system: -// '/dc_motor_model' -// '/dc_motor_model' -// -void sim_model_li_dc_motor_model(real32_T rtu_ctl_voltage, real32_T - rtu_curr_rotor_speed, B_dc_motor_model_sim_model_li_T *localB, real32_T - rtp_motor_kn, real32_T rtp_motor_r, real32_T rtp_motor_kt, real32_T - rtp_motor_friction_coeff) -{ - // Gain: '/Gain5' incorporates: - // Gain: '/Gain4' - // Sum: '/Add' - - localB->current = (rtu_ctl_voltage - 1.0F / rtp_motor_kn * - rtu_curr_rotor_speed) * (1.0F / rtp_motor_r); - - // Sum: '/Add1' incorporates: - // Gain: '/Gain1' - // Gain: '/Gain6' - - localB->output_torque = rtp_motor_kt * localB->current - - rtp_motor_friction_coeff * rtu_curr_rotor_speed; -} - -// -// Termination for atomic system: -// '/dc_motor_model' -// '/dc_motor_model' -// -void sim_mod_dc_motor_model_Term(void) -{ -} - -// -// System initialize for atomic system: -// '/speed_controller' -// '/speed_controller' -// -void sim_m_speed_controller_Init(DW_speed_controller_sim_model_T *localDW, - P_speed_controller_sim_model__T *localP) -{ - // InitializeConditions for RateLimiter: '/Rate Limiter' - localDW->PrevY = localP->RateLimiter_IC; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE = localP->Integrator_IC; - - // InitializeConditions for DiscreteIntegrator: '/Filter' - localDW->Filter_DSTATE = localP->Filter_IC; -} - -// -// Output and update for atomic system: -// '/speed_controller' -// '/speed_controller' -// -void sim_model__speed_controller(real32_T rtu_battery_voltage, uint8_T - rtu_speed_cmd, real32_T rtu_speed_curr, B_speed_controller_sim_model__T - *localB, DW_speed_controller_sim_model_T *localDW, - P_speed_controller_sim_model__T *localP, real32_T rtp_ctl_pwm2speed, real32_T - rtp_ctl_speed_filt_num, real32_T rtp_ctl_speed_cilt_den, real32_T rtp_ctl_kp, - real32_T rtp_ctl_kd, real32_T rtp_ctl_filt_n, real32_T rtp_ctl_max_voltage, - real32_T rtp_ctl_ki) -{ - real32_T rtb_IntegralGain; - real32_T rtb_Sum_k; - real32_T rtb_SignPreIntegrator; - real32_T rtb_FilterCoefficient; - boolean_T rtb_NotEqual; - real32_T rtb_Gain_pl; - int8_T rtb_SignPreIntegrator_0; - - // Product: '/Divide' incorporates: - // Constant: '/Constant' - - rtb_IntegralGain = (real32_T)rtu_speed_cmd * rtp_ctl_pwm2speed; - - // RateLimiter: '/Rate Limiter' - rtb_FilterCoefficient = rtb_IntegralGain - localDW->PrevY; - if (rtb_FilterCoefficient > localP->RateLimiter_RisingLim) { - rtb_IntegralGain = localDW->PrevY + localP->RateLimiter_RisingLim; - } else { - if (rtb_FilterCoefficient < localP->RateLimiter_FallingLim) { - rtb_IntegralGain = localDW->PrevY + localP->RateLimiter_FallingLim; - } - } - - localDW->PrevY = rtb_IntegralGain; - - // End of RateLimiter: '/Rate Limiter' - - // Sum: '/Sum' incorporates: - // DiscreteTransferFcn: '/Discrete Transfer Fcn1' - - rtb_IntegralGain -= rtu_speed_curr * rtp_ctl_speed_filt_num / - rtp_ctl_speed_cilt_den; - - // Gain: '/Filter Coefficient' incorporates: - // DiscreteIntegrator: '/Filter' - // Gain: '/Derivative Gain' - // Sum: '/SumD' - - rtb_FilterCoefficient = (rtp_ctl_kd * rtb_IntegralGain - - localDW->Filter_DSTATE) * rtp_ctl_filt_n; - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Integrator' - // Gain: '/Proportional Gain' - - rtb_Sum_k = (rtp_ctl_kp * rtb_IntegralGain + localDW->Integrator_DSTATE) + - rtb_FilterCoefficient; - - // DeadZone: '/DeadZone' - if (rtb_Sum_k > rtp_ctl_max_voltage) { - rtb_SignPreIntegrator = rtb_Sum_k - rtp_ctl_max_voltage; - } else if (rtb_Sum_k >= localP->DiscretePIDController_LowerSatu) { - rtb_SignPreIntegrator = 0.0F; - } else { - rtb_SignPreIntegrator = rtb_Sum_k - localP->DiscretePIDController_LowerSatu; - } - - // End of DeadZone: '/DeadZone' - - // RelationalOperator: '/NotEqual' incorporates: - // Gain: '/ZeroGain' - - rtb_NotEqual = (localP->ZeroGain_Gain * rtb_Sum_k != rtb_SignPreIntegrator); - - // Signum: '/SignDeltaU' - if (rtb_SignPreIntegrator < 0.0F) { - rtb_SignPreIntegrator = -1.0F; - } else if (rtb_SignPreIntegrator > 0.0F) { - rtb_SignPreIntegrator = 1.0F; - } else { - if (rtb_SignPreIntegrator == 0.0F) { - rtb_SignPreIntegrator = 0.0F; - } - } - - // End of Signum: '/SignDeltaU' - - // Gain: '/Integral Gain' - rtb_IntegralGain *= rtp_ctl_ki; - - // Saturate: '/Saturate' - if (rtb_Sum_k > rtp_ctl_max_voltage) { - rtb_Sum_k = rtp_ctl_max_voltage; - } else { - if (rtb_Sum_k < localP->DiscretePIDController_LowerSatu) { - rtb_Sum_k = localP->DiscretePIDController_LowerSatu; - } - } - - // End of Saturate: '/Saturate' - - // Switch: '/Switch2' incorporates: - // RelationalOperator: '/LowerRelop1' - - if (rtb_Sum_k > rtu_battery_voltage) { - localB->Switch2 = rtu_battery_voltage; - } else { - // Gain: '/Gain' - rtb_Gain_pl = localP->Gain_Gain * rtu_battery_voltage; - - // Switch: '/Switch' incorporates: - // RelationalOperator: '/UpperRelop' - - if (rtb_Sum_k < rtb_Gain_pl) { - localB->Switch2 = rtb_Gain_pl; - } else { - localB->Switch2 = rtb_Sum_k; - } - - // End of Switch: '/Switch' - } - - // End of Switch: '/Switch2' - - // Signum: '/SignPreIntegrator' - if (rtb_IntegralGain < 0.0F) { - // DataTypeConversion: '/DataTypeConv2' - rtb_Sum_k = -1.0F; - } else if (rtb_IntegralGain > 0.0F) { - // DataTypeConversion: '/DataTypeConv2' - rtb_Sum_k = 1.0F; - } else if (rtb_IntegralGain == 0.0F) { - // DataTypeConversion: '/DataTypeConv2' - rtb_Sum_k = 0.0F; - } else { - // DataTypeConversion: '/DataTypeConv2' - rtb_Sum_k = rtb_IntegralGain; - } - - // End of Signum: '/SignPreIntegrator' - - // DataTypeConversion: '/DataTypeConv2' - if (rtIsNaNF(rtb_Sum_k) || rtIsInfF(rtb_Sum_k)) { - rtb_Sum_k = 0.0F; - } else { - rtb_Sum_k = (real32_T)fmod((real_T)rtb_Sum_k, (real_T)256.0F); - } - - // DataTypeConversion: '/DataTypeConv1' - if (rtb_SignPreIntegrator < 128.0F) { - rtb_SignPreIntegrator_0 = (int8_T)rtb_SignPreIntegrator; - } else { - rtb_SignPreIntegrator_0 = MAX_int8_T; - } - - // End of DataTypeConversion: '/DataTypeConv1' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/DataTypeConv2' - // Logic: '/AND' - // RelationalOperator: '/Equal' - - if (rtb_NotEqual && ((rtb_Sum_k < 0.0F ? (int32_T)(int8_T)(int32_T)-(int32_T) - (int8_T)(uint8_T)-rtb_Sum_k : (int32_T)(int8_T)(uint8_T) - rtb_Sum_k) == (int32_T)rtb_SignPreIntegrator_0)) { - rtb_IntegralGain = localP->Constant_Value; - } - - // End of Switch: '/Switch' - - // Update for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE += localP->Integrator_gainval * rtb_IntegralGain; - - // Update for DiscreteIntegrator: '/Filter' - localDW->Filter_DSTATE += localP->Filter_gainval * rtb_FilterCoefficient; -} - -// -// Termination for atomic system: -// '/speed_controller' -// '/speed_controller' -// -void sim_m_speed_controller_Term(void) -{ -} - -// -// System initialize for atomic system: -// '/servo_model' -// '/servo_model' -// -void sim_model__servo_model_Init(DW_servo_model_sim_model_lib0_T *localDW, - P_servo_model_sim_model_lib0_T *localP, real32_T rtp_servo_min_theta, real32_T - rtp_motor_gear_ratio) -{ - real32_T y; - int32_T i; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator4' - y = rtp_servo_min_theta / rtp_motor_gear_ratio; - for (i = 0; i < 6; i++) { - localDW->DiscreteTimeIntegrator4_DSTATE[i] = y; - - // InitializeConditions for Backlash: '/Backlash1' - localDW->PrevY[i] = localP->Backlash1_InitialOutput; - - // InitializeConditions for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE[i] = localP->Integrator_IC; - - // InitializeConditions for DiscreteIntegrator: '/Filter' - localDW->Filter_DSTATE[i] = localP->Filter_IC; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator3' - localDW->DiscreteTimeIntegrator3_DSTATE[i] = - localP->DiscreteTimeIntegrator3_IC; - } - - // End of InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator4' -} - -// -// Output and update for atomic system: -// '/servo_model' -// '/servo_model' -// -void sim_model_lib0_servo_model(const real32_T rtu_command_PWM[6], - B_servo_model_sim_model_lib0_T *localB, DW_servo_model_sim_model_lib0_T - *localDW, P_servo_model_sim_model_lib0_T *localP, real32_T - rtp_servo_pwm2angle_bias, real32_T rtp_servo_pwm2angle, real32_T - rtp_servo_min_theta, real32_T rtp_motor_gear_ratio, real32_T - rtp_servo_max_theta, real32_T rtp_motor_backlash_deadband, real32_T - rtp_servo_ctl_kp, real32_T rtp_servo_ctl_kd, real32_T rtp_servo_ctl_filt_n, - real32_T rtp_servo_max_voltage, real32_T rtp_motor_k, real32_T rtp_motor_r, - real32_T rtp_motor_friction_coeff, real32_T rtp_motor_gear_box_inertia, - real32_T rtp_servo_ctl_ki) -{ - real32_T halfDeadBand; - real32_T y; - real32_T rtb_IntegralGain[6]; - real32_T rtb_SignDeltaU[6]; - real32_T rtb_FilterCoefficient[6]; - boolean_T rtb_NotEqual[6]; - real32_T tmp; - real32_T tmp_0; - real32_T tmp_1; - int32_T i; - real32_T rtb_IntegralGain_c; - int8_T rtb_SignDeltaU_0; - - // Backlash: '/Backlash1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator4' - // Gain: '/Filter Coefficient' - // Sum: '/Sum1' - - halfDeadBand = rtp_motor_backlash_deadband / 2.0F; - for (i = 0; i < 6; i++) { - if (localDW->DiscreteTimeIntegrator4_DSTATE[i] < localDW->PrevY[i] - - halfDeadBand) { - localB->Backlash1[i] = localDW->DiscreteTimeIntegrator4_DSTATE[i] + - halfDeadBand; - } else if (localDW->DiscreteTimeIntegrator4_DSTATE[i] <= localDW->PrevY[i] + - halfDeadBand) { - localB->Backlash1[i] = localDW->PrevY[i]; - } else { - localB->Backlash1[i] = localDW->DiscreteTimeIntegrator4_DSTATE[i] - - halfDeadBand; - } - - // Gain: '/Gain1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator4' - - localB->Gain1[i] = rtp_motor_gear_ratio * localB->Backlash1[i]; - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant' - // Gain: '/Gain' - // Sum: '/Sum' - - rtb_IntegralGain_c = (rtu_command_PWM[i] + rtp_servo_pwm2angle_bias) * - rtp_servo_pwm2angle - localB->Gain1[i]; - - // Gain: '/Filter Coefficient' incorporates: - // DiscreteIntegrator: '/Filter' - // Gain: '/Derivative Gain' - // Sum: '/SumD' - - y = (rtp_servo_ctl_kd * rtb_IntegralGain_c - localDW->Filter_DSTATE[i]) * - rtp_servo_ctl_filt_n; - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Integrator' - // Gain: '/Proportional Gain' - - rtb_SignDeltaU[i] = (rtp_servo_ctl_kp * rtb_IntegralGain_c + - localDW->Integrator_DSTATE[i]) + y; - rtb_IntegralGain[i] = rtb_IntegralGain_c; - rtb_FilterCoefficient[i] = y; - } - - // End of Backlash: '/Backlash1' - - // Gain: '/Gain5' incorporates: - // DeadZone: '/DeadZone' - // DiscreteIntegrator: '/Discrete-Time Integrator3' - // Gain: '/Gain7' - // Gain: '/Integral Gain' - // Sum: '/Add3' - - halfDeadBand = 1.0F / rtp_motor_r; - for (i = 0; i < 6; i++) { - // Saturate: '/Saturate' - if (rtb_SignDeltaU[i] > rtp_servo_max_voltage) { - y = rtp_servo_max_voltage; - } else if (rtb_SignDeltaU[i] < localP->DiscretePIDController_LowerSatu) { - y = localP->DiscretePIDController_LowerSatu; - } else { - y = rtb_SignDeltaU[i]; - } - - localB->current[i] = (y - rtp_motor_k * - localDW->DiscreteTimeIntegrator3_DSTATE[i]) * - halfDeadBand; - - // DeadZone: '/DeadZone' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - // Gain: '/Gain7' - // Saturate: '/Saturate' - // Sum: '/Add3' - - if (rtb_SignDeltaU[i] > rtp_servo_max_voltage) { - y = rtb_SignDeltaU[i] - rtp_servo_max_voltage; - } else if (rtb_SignDeltaU[i] >= localP->DiscretePIDController_LowerSatu) { - y = 0.0F; - } else { - y = rtb_SignDeltaU[i] - localP->DiscretePIDController_LowerSatu; - } - - // RelationalOperator: '/NotEqual' incorporates: - // Gain: '/ZeroGain' - // Saturate: '/Saturate' - - rtb_NotEqual[i] = (localP->ZeroGain_Gain * rtb_SignDeltaU[i] != y); - - // Signum: '/SignDeltaU' - if (y < 0.0F) { - y = -1.0F; - } else if (y > 0.0F) { - y = 1.0F; - } else { - if (y == 0.0F) { - y = 0.0F; - } - } - - // End of Signum: '/SignDeltaU' - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator4' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - - localDW->DiscreteTimeIntegrator4_DSTATE[i] += - localP->DiscreteTimeIntegrator4_gainval * - localDW->DiscreteTimeIntegrator3_DSTATE[i]; - rtb_SignDeltaU[i] = y; - rtb_IntegralGain[i] *= rtp_servo_ctl_ki; - } - - // End of Gain: '/Gain5' - - // Gain: '/Gain6' - halfDeadBand = 1.0F / rtp_motor_gear_box_inertia; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator4' - y = rtp_servo_max_theta / rtp_motor_gear_ratio; - tmp = rtp_servo_min_theta / rtp_motor_gear_ratio; - tmp_0 = rtp_servo_max_theta / rtp_motor_gear_ratio; - tmp_1 = rtp_servo_min_theta / rtp_motor_gear_ratio; - for (i = 0; i < 6; i++) { - if (localDW->DiscreteTimeIntegrator4_DSTATE[i] >= y) { - localDW->DiscreteTimeIntegrator4_DSTATE[i] = tmp_0; - } else { - if (localDW->DiscreteTimeIntegrator4_DSTATE[i] <= tmp) { - localDW->DiscreteTimeIntegrator4_DSTATE[i] = tmp_1; - } - } - - // Update for Backlash: '/Backlash1' - localDW->PrevY[i] = localB->Backlash1[i]; - - // Update for DiscreteIntegrator: '/Integrator' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/DataTypeConv1' - // DataTypeConversion: '/DataTypeConv2' - // Logic: '/AND' - // RelationalOperator: '/Equal' - // Signum: '/SignPreIntegrator' - // Switch: '/Switch' - - if (rtb_IntegralGain[i] < 0.0F) { - rtb_IntegralGain_c = -1.0F; - } else if (rtb_IntegralGain[i] > 0.0F) { - rtb_IntegralGain_c = 1.0F; - } else if (rtb_IntegralGain[i] == 0.0F) { - rtb_IntegralGain_c = 0.0F; - } else { - rtb_IntegralGain_c = rtb_IntegralGain[i]; - } - - rtb_IntegralGain_c = (real32_T)floor((real_T)rtb_IntegralGain_c); - if (rtIsNaNF(rtb_IntegralGain_c) || rtIsInfF(rtb_IntegralGain_c)) { - rtb_IntegralGain_c = 0.0F; - } else { - rtb_IntegralGain_c = (real32_T)fmod((real_T)rtb_IntegralGain_c, (real_T) - 256.0F); - } - - if (rtb_SignDeltaU[i] < 128.0F) { - if (rtb_SignDeltaU[i] >= -128.0F) { - rtb_SignDeltaU_0 = (int8_T)rtb_SignDeltaU[i]; - } else { - rtb_SignDeltaU_0 = MIN_int8_T; - } - } else { - rtb_SignDeltaU_0 = MAX_int8_T; - } - - if (rtb_NotEqual[i] && ((rtb_IntegralGain_c < 0.0F ? (int32_T)(int8_T) - (int32_T)-(int32_T)(int8_T)(uint8_T)-rtb_IntegralGain_c : (int32_T) - (int8_T)(uint8_T)rtb_IntegralGain_c) == (int32_T)rtb_SignDeltaU_0)) { - rtb_IntegralGain_c = localP->Constant_Value; - } else { - rtb_IntegralGain_c = rtb_IntegralGain[i]; - } - - localDW->Integrator_DSTATE[i] += localP->Integrator_gainval * - rtb_IntegralGain_c; - - // End of Update for DiscreteIntegrator: '/Integrator' - - // Update for DiscreteIntegrator: '/Filter' - localDW->Filter_DSTATE[i] += localP->Filter_gainval * - rtb_FilterCoefficient[i]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator3' - // Gain: '/Gain11' - // Gain: '/Gain6' - // Gain: '/Gain9' - // Sum: '/Add1' - - localDW->DiscreteTimeIntegrator3_DSTATE[i] += (rtp_motor_k * localB-> - current[i] - rtp_motor_friction_coeff * - localDW->DiscreteTimeIntegrator3_DSTATE[i]) * halfDeadBand * - localP->DiscreteTimeIntegrator3_gainval; - } -} - -// -// Termination for atomic system: -// '/servo_model' -// '/servo_model' -// -void sim_model__servo_model_Term(void) -{ -} - -// Function for MATLAB Function: '/generate_output' -static void sim_model__format_AR_tag_output(const real32_T points_in_iss[108], - const real32_T points_in_cam[72], boolean_T valid_in[36], real32_T - points_out_iss[150], real32_T points_out_cam[100], real32_T valid_out[50]) -{ - int32_T b_i; - int32_T c_i; - int32_T d_i; - int32_T i; - boolean_T tmp; - int8_T b_data[36]; - int32_T b_sizes; - real32_T points_in_iss_data[258]; - real32_T points_in_cam_data[172]; - int8_T valid_in_data[86]; - int32_T points_in_iss_sizes_idx_0; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // format_AR_tag_output.m - // inputs: - // num_pts_out = Required number of points to be reported - // points_in iss = ALL [x,y,z] position of points in the ISS frame - // [NOTE: for AR tags, these are grouped in 4's. first 4 = 1 tag, second 4 = another tag] - // points_in_cam = ALL [u,v] pixel location of points in the camera - // valid_in = indicates which points are valid - // - // outputs: - // points_out_iss = output [x,y,z] position of points in the ISS frame - // points_out_cam = output [u,v] pixel location of points in the camera - // valid_out = indicates which values of the output are valid! - // initialize the outputs - for (b_i = 0; b_i < 9; b_i++) { - c_i = (int32_T)(b_i << 2); - tmp = kfcjlnglmgdbgdjm_all((boolean_T *)&valid_in[c_i]); - valid_in[c_i] = tmp; - valid_in[(int32_T)(1 + c_i)] = tmp; - valid_in[(int32_T)(2 + c_i)] = tmp; - valid_in[(int32_T)(3 + c_i)] = tmp; - - // all 4 tags must be valid, or they are all invalid - } - - // find how many pts are valid, and determine how that compares to required number - b_i = 0; - for (c_i = 0; c_i < 36; c_i++) { - if (valid_in[c_i]) { - b_i++; - } - } - - // if we need more points, back fill with zeros, and zero the valid cooresponding valid flag - c_i = 0; - for (d_i = 0; d_i < 36; d_i++) { - if (valid_in[d_i]) { - c_i++; - } - } - - b_sizes = c_i; - c_i = 0; - for (d_i = 0; d_i < 36; d_i++) { - if (valid_in[d_i]) { - b_data[c_i] = (int8_T)(int32_T)(d_i + 1); - c_i++; - } - } - - d_i = (int32_T)(50.0F - (real32_T)b_i); - points_in_iss_sizes_idx_0 = (int32_T)(b_sizes + d_i); - for (c_i = 0; c_i < 3; c_i++) { - for (i = 0; i <= (int32_T)(b_sizes - 1); i++) { - points_in_iss_data[(int32_T)(i + (int32_T)(points_in_iss_sizes_idx_0 * c_i))] - = points_in_iss[(int32_T)((int32_T)((int32_T)(36 * c_i) + (int32_T) - b_data[i]) - 1)]; - } - } - - for (c_i = 0; c_i < 3; c_i++) { - for (i = 0; i <= (int32_T)(d_i - 1); i++) { - points_in_iss_data[(int32_T)((int32_T)(i + b_sizes) + (int32_T) - (points_in_iss_sizes_idx_0 * c_i))] = 0.0F; - } - } - - for (c_i = 0; c_i < 3; c_i++) { - memcpy(&points_out_iss[(int32_T)(c_i * 50)], &points_in_iss_data[(int32_T) - (c_i * 50)], (uint32_T)(50U * sizeof(real32_T))); - } - - c_i = 0; - for (d_i = 0; d_i < 36; d_i++) { - if (valid_in[d_i]) { - c_i++; - } - } - - b_sizes = c_i; - c_i = 0; - for (d_i = 0; d_i < 36; d_i++) { - if (valid_in[d_i]) { - b_data[c_i] = (int8_T)(int32_T)(d_i + 1); - c_i++; - } - } - - d_i = (int32_T)(50.0F - (real32_T)b_i); - points_in_iss_sizes_idx_0 = (int32_T)(b_sizes + d_i); - for (c_i = 0; c_i <= (int32_T)(b_sizes - 1); c_i++) { - points_in_cam_data[c_i] = points_in_cam[(int32_T)((int32_T)b_data[c_i] - 1)]; - } - - for (c_i = 0; c_i <= (int32_T)(b_sizes - 1); c_i++) { - points_in_cam_data[(int32_T)(c_i + points_in_iss_sizes_idx_0)] = - points_in_cam[(int32_T)((int32_T)b_data[c_i] + 35)]; - } - - for (c_i = 0; c_i <= (int32_T)(d_i - 1); c_i++) { - points_in_cam_data[(int32_T)(c_i + b_sizes)] = 0.0F; - } - - for (c_i = 0; c_i <= (int32_T)(d_i - 1); c_i++) { - points_in_cam_data[(int32_T)((int32_T)(c_i + b_sizes) + - points_in_iss_sizes_idx_0)] = 0.0F; - } - - for (c_i = 0; c_i < 2; c_i++) { - memcpy(&points_out_cam[(int32_T)(c_i * 50)], &points_in_cam_data[(int32_T) - (c_i * 50)], (uint32_T)(50U * sizeof(real32_T))); - } - - c_i = 0; - for (d_i = 0; d_i < 36; d_i++) { - if (valid_in[d_i]) { - c_i++; - } - } - - b_sizes = c_i; - c_i = 0; - for (d_i = 0; d_i < 36; d_i++) { - if (valid_in[d_i]) { - b_data[c_i] = (int8_T)(int32_T)(d_i + 1); - c_i++; - } - } - - for (c_i = 0; c_i <= (int32_T)(b_sizes - 1); c_i++) { - valid_in_data[c_i] = (int8_T)valid_in[(int32_T)((int32_T)b_data[c_i] - 1)]; - } - - b_i = (int32_T)(50.0F - (real32_T)b_i); - for (c_i = 0; c_i <= (int32_T)(b_i - 1); c_i++) { - valid_in_data[(int32_T)(c_i + b_sizes)] = 0; - } - - for (c_i = 0; c_i < 50; c_i++) { - valid_out[c_i] = (real32_T)valid_in_data[c_i]; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_model_lib0_rand(real_T varargin_2, real_T r_data[], int32_T - r_sizes[2], DW_sim_model_lib0_T *sim_model_lib0_DW) -{ - real_T b; - int32_T k; - r_sizes[0] = 1; - r_sizes[1] = (int32_T)varargin_2; - for (k = 0; k <= (int32_T)(r_sizes[1] - 1); k++) { - b = aimgnophaiekjecb_eml_rand_mt19937ar(sim_model_lib0_DW->state_m); - r_data[k] = b; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_model_lib0_randperm(real_T n, real_T p_data[], int32_T p_sizes[2], - DW_sim_model_lib0_T *sim_model_lib0_DW) -{ - int32_T i; - int32_T tmp_data[504]; - int32_T tmp_sizes[2]; - int32_T p_idx_1; - int32_T p_idx_0; - sim_model_lib0_rand(n, p_data, p_sizes, sim_model_lib0_DW); - baaiimglmglniekf_sortIdx(p_data, p_sizes, tmp_data, tmp_sizes); - p_idx_0 = p_sizes[0]; - p_idx_1 = p_sizes[1]; - p_sizes[0] = 1; - for (i = 0; i <= (int32_T)(p_idx_1 - 1); i++) { - p_data[i] = (real_T)tmp_data[(int32_T)(p_idx_0 * i)]; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_mode_format_handrail_output(const real32_T points_in_iss[1512], - const real32_T points_in_cam[1512], const boolean_T valid_in[504], real32_T - points_out_iss[150], real32_T points_out_cam[150], real32_T valid_out[50], - B_sim_model_lib0_T *sim_model_lib0_B, DW_sim_model_lib0_T *sim_model_lib0_DW) -{ - int32_T trueCount; - int32_T i; - real_T random_order_data[504]; - int32_T random_order_sizes[2]; - int16_T e_data[504]; - int32_T e_sizes; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // format_handrail_output.m - // inputs: - // num_pts_out = Required number of points to be reported - // points_in_iss = ALL [x,y,z] position of points in the ISS frame - // points_in_cam = ALL [u,v] pixel location of points in the camera - // valid_in = indicates which points are valid - // - // outputs: - // points_out_iss = output [x,y,z] position of points in the ISS frame - // points_out_cam = output [x,y,z] pixel location of points in the camera frame - // valid_out = indicates which values of the output are valid! - // initialize the outputs - memset(&points_out_iss[0], 0, (uint32_T)(150U * sizeof(real32_T))); - memset(&points_out_cam[0], 0, (uint32_T)(150U * sizeof(real32_T))); - memset(&valid_out[0], 0, (uint32_T)(50U * sizeof(real32_T))); - - // find how many pts are valid, and determine how that compares to required number - trueCount = 0; - for (i = 0; i < 504; i++) { - if (valid_in[i]) { - trueCount++; - } - } - - if (50.0F - (real32_T)trueCount >= 0.0F) { - // if exactly the right ammount or need more, just report what we got! (rest will be zeros) - i = 0; - for (trueCount = 0; trueCount < 504; trueCount++) { - if (valid_in[trueCount]) { - i++; - } - } - - e_sizes = i; - i = 0; - for (trueCount = 0; trueCount < 504; trueCount++) { - if (valid_in[trueCount]) { - e_data[i] = (int16_T)(int32_T)(trueCount + 1); - i++; - } - } - - for (i = 0; i < 3; i++) { - for (trueCount = 0; trueCount <= (int32_T)(e_sizes - 1); trueCount++) { - points_out_iss[(int32_T)(trueCount + (int32_T)(50 * i))] = - points_in_iss[(int32_T)((int32_T)((int32_T)(504 * i) + (int32_T) - e_data[trueCount]) - 1)]; - } - } - - i = 0; - for (trueCount = 0; trueCount < 504; trueCount++) { - if (valid_in[trueCount]) { - i++; - } - } - - e_sizes = i; - i = 0; - for (trueCount = 0; trueCount < 504; trueCount++) { - if (valid_in[trueCount]) { - e_data[i] = (int16_T)(int32_T)(trueCount + 1); - i++; - } - } - - for (i = 0; i < 3; i++) { - for (trueCount = 0; trueCount <= (int32_T)(e_sizes - 1); trueCount++) { - points_out_cam[(int32_T)(trueCount + (int32_T)(50 * i))] = - points_in_cam[(int32_T)((int32_T)((int32_T)(504 * i) + (int32_T) - e_data[trueCount]) - 1)]; - } - } - - i = 0; - for (trueCount = 0; trueCount < 504; trueCount++) { - if (valid_in[trueCount]) { - i++; - } - } - - e_sizes = i; - i = 0; - for (trueCount = 0; trueCount < 504; trueCount++) { - if (valid_in[trueCount]) { - e_data[i] = (int16_T)(int32_T)(trueCount + 1); - i++; - } - } - - for (i = 0; i <= (int32_T)(e_sizes - 1); i++) { - valid_out[i] = (real32_T)valid_in[(int32_T)((int32_T)e_data[i] - 1)]; - } - } else { - // if we have too many points, take a random sample - trueCount = 0; - for (i = 0; i < 504; i++) { - if (valid_in[i]) { - trueCount++; - } - } - - if (trueCount == 0) { - trueCount = 0; - } else { - if (!(trueCount >= 3)) { - trueCount = 3; - } - } - - sim_model_lib0_randperm((real_T)trueCount, random_order_data, - random_order_sizes, sim_model_lib0_DW); - trueCount = 0; - for (i = 0; i < 504; i++) { - if (valid_in[i]) { - trueCount++; - } - } - - e_sizes = trueCount; - trueCount = 0; - for (i = 0; i < 504; i++) { - if (valid_in[i]) { - e_data[trueCount] = (int16_T)(int32_T)(i + 1); - trueCount++; - } - } - - for (i = 0; i < 3; i++) { - for (trueCount = 0; trueCount <= (int32_T)(e_sizes - 1); trueCount++) { - sim_model_lib0_B->points_in_iss_data[(int32_T)(trueCount + (int32_T) - (e_sizes * i))] = points_in_iss[(int32_T)((int32_T)((int32_T)(504 * i) - + (int32_T)e_data[trueCount]) - 1)]; - } - } - - for (i = 0; i < 3; i++) { - for (trueCount = 0; trueCount < 50; trueCount++) { - points_out_iss[(int32_T)(trueCount + (int32_T)(50 * i))] = - sim_model_lib0_B->points_in_iss_data[(int32_T)((int32_T)((int32_T) - (e_sizes * i) + (int32_T)random_order_data[trueCount]) - 1)]; - } - } - - trueCount = 0; - for (i = 0; i < 504; i++) { - if (valid_in[i]) { - trueCount++; - } - } - - e_sizes = trueCount; - trueCount = 0; - for (i = 0; i < 504; i++) { - if (valid_in[i]) { - e_data[trueCount] = (int16_T)(int32_T)(i + 1); - trueCount++; - } - } - - for (i = 0; i < 3; i++) { - for (trueCount = 0; trueCount <= (int32_T)(e_sizes - 1); trueCount++) { - sim_model_lib0_B->points_in_iss_data[(int32_T)(trueCount + (int32_T) - (e_sizes * i))] = points_in_cam[(int32_T)((int32_T)((int32_T)(504 * i) - + (int32_T)e_data[trueCount]) - 1)]; - } - } - - for (i = 0; i < 3; i++) { - for (trueCount = 0; trueCount < 50; trueCount++) { - points_out_cam[(int32_T)(trueCount + (int32_T)(50 * i))] = - sim_model_lib0_B->points_in_iss_data[(int32_T)((int32_T)((int32_T) - (e_sizes * i) + (int32_T)random_order_data[trueCount]) - 1)]; - } - } - - for (i = 0; i < 50; i++) { - valid_out[i] = 1.0F; - } - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim__pphdimglohlnoppp_mergesort(int32_T idx_data[], const real32_T - x_data[], const int32_T x_sizes[2], const int32_T dir[2], int32_T n, - B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T k; - int32_T i; - int32_T i2; - int32_T j; - int32_T pEnd; - int32_T p; - int32_T q; - int32_T qEnd; - int32_T kEnd; - for (i = 1; i <= (int32_T)(n - 1); i += 2) { - if (dbiekfcbglfkkfcb_sortLE(x_data, x_sizes, dir, i, (int32_T)(i + 1))) { - idx_data[(int32_T)(i - 1)] = i; - idx_data[i] = (int32_T)(i + 1); - } else { - idx_data[(int32_T)(i - 1)] = (int32_T)(i + 1); - idx_data[i] = i; - } - } - - if ((int32_T)(n & 1) != 0) { - idx_data[(int32_T)(n - 1)] = n; - } - - i = 2; - while (i < n) { - i2 = (int32_T)(i << 1); - j = 1; - pEnd = (int32_T)(1 + i); - while (pEnd < (int32_T)(n + 1)) { - p = j; - q = pEnd; - qEnd = (int32_T)(j + i2); - if (qEnd > (int32_T)(n + 1)) { - qEnd = (int32_T)(n + 1); - } - - k = 0; - kEnd = (int32_T)(qEnd - j); - while ((int32_T)(k + 1) <= kEnd) { - if (dbiekfcbglfkkfcb_sortLE(x_data, x_sizes, dir, idx_data[(int32_T)(p - - 1)], idx_data[(int32_T)(q - 1)])) { - sim_model_lib0_B->iwork_data_n[k] = idx_data[(int32_T)(p - 1)]; - p++; - if (p == pEnd) { - while (q < qEnd) { - k++; - sim_model_lib0_B->iwork_data_n[k] = idx_data[(int32_T)(q - 1)]; - q++; - } - } - } else { - sim_model_lib0_B->iwork_data_n[k] = idx_data[(int32_T)(q - 1)]; - q++; - if (q == qEnd) { - while (p < pEnd) { - k++; - sim_model_lib0_B->iwork_data_n[k] = idx_data[(int32_T)(p - 1)]; - p++; - } - } - } - - k++; - } - - for (pEnd = 0; (int32_T)(pEnd + 1) <= kEnd; pEnd++) { - idx_data[(int32_T)((int32_T)(j + pEnd) - 1)] = - sim_model_lib0_B->iwork_data_n[pEnd]; - } - - j = qEnd; - pEnd = (int32_T)(qEnd + i); - } - - i = i2; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_mo_aaieppphglfcglng_sortIdx(const real32_T x_data[], const - int32_T x_sizes[2], const int32_T col[2], int32_T idx_data[], int32_T - *idx_sizes, B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T i; - int32_T loop_ub; - *idx_sizes = x_sizes[0]; - loop_ub = x_sizes[0]; - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - idx_data[i] = 0; - } - - sim__pphdimglohlnoppp_mergesort(idx_data, x_data, x_sizes, col, x_sizes[0], - sim_model_lib0_B); -} - -// Function for MATLAB Function: '/generate_output' -static void hdjmcjmophlfkngl_apply_row_perm(real32_T y_data[], int32_T y_sizes[2], - const int32_T idx_data[], B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T m; - int32_T i; - m = y_sizes[0]; - for (i = 0; (int32_T)(i + 1) <= m; i++) { - sim_model_lib0_B->ycol_data[i] = y_data[(int32_T)(idx_data[i] - 1)]; - } - - for (i = 0; (int32_T)(i + 1) <= m; i++) { - y_data[i] = sim_model_lib0_B->ycol_data[i]; - } - - for (i = 0; (int32_T)(i + 1) <= m; i++) { - sim_model_lib0_B->ycol_data[i] = y_data[(int32_T)((int32_T)(idx_data[i] + - y_sizes[0]) - 1)]; - } - - for (i = 0; (int32_T)(i + 1) <= m; i++) { - y_data[(int32_T)(i + y_sizes[0])] = sim_model_lib0_B->ycol_data[i]; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_m_mglflfcbkfkfbiec_sortrows(real32_T y_data[], int32_T y_sizes[2], - real_T ndx_data[], int32_T *ndx_sizes, B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T m; - int32_T col[2]; - int32_T idx_sizes; - m = y_sizes[0]; - *ndx_sizes = y_sizes[0]; - col[0] = 1; - col[1] = 2; - sim_mo_aaieppphglfcglng_sortIdx(y_data, y_sizes, col, - sim_model_lib0_B->idx_data_m, &idx_sizes, sim_model_lib0_B); - hdjmcjmophlfkngl_apply_row_perm(y_data, y_sizes, sim_model_lib0_B->idx_data_m, - sim_model_lib0_B); - for (idx_sizes = 0; (int32_T)(idx_sizes + 1) <= m; idx_sizes++) { - ndx_data[idx_sizes] = (real_T)sim_model_lib0_B->idx_data_m[idx_sizes]; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim__gdbacbaajekfglng_mergesort(int32_T idx_data[], const int32_T - x_data[], int32_T n, B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T k; - int32_T i; - int32_T i2; - int32_T j; - int32_T pEnd; - int32_T p; - int32_T q; - int32_T qEnd; - int32_T kEnd; - for (i = 1; i <= (int32_T)(n - 1); i += 2) { - if (x_data[(int32_T)(i - 1)] <= x_data[i]) { - idx_data[(int32_T)(i - 1)] = i; - idx_data[i] = (int32_T)(i + 1); - } else { - idx_data[(int32_T)(i - 1)] = (int32_T)(i + 1); - idx_data[i] = i; - } - } - - if ((int32_T)(n & 1) != 0) { - idx_data[(int32_T)(n - 1)] = n; - } - - i = 2; - while (i < n) { - i2 = (int32_T)(i << 1); - j = 1; - pEnd = (int32_T)(1 + i); - while (pEnd < (int32_T)(n + 1)) { - p = j; - q = pEnd; - qEnd = (int32_T)(j + i2); - if (qEnd > (int32_T)(n + 1)) { - qEnd = (int32_T)(n + 1); - } - - k = 0; - kEnd = (int32_T)(qEnd - j); - while ((int32_T)(k + 1) <= kEnd) { - if (x_data[(int32_T)(idx_data[(int32_T)(p - 1)] - 1)] <= x_data[(int32_T) - (idx_data[(int32_T)(q - 1)] - 1)]) { - sim_model_lib0_B->iwork_data_p[k] = idx_data[(int32_T)(p - 1)]; - p++; - if (p == pEnd) { - while (q < qEnd) { - k++; - sim_model_lib0_B->iwork_data_p[k] = idx_data[(int32_T)(q - 1)]; - q++; - } - } - } else { - sim_model_lib0_B->iwork_data_p[k] = idx_data[(int32_T)(q - 1)]; - q++; - if (q == qEnd) { - while (p < pEnd) { - k++; - sim_model_lib0_B->iwork_data_p[k] = idx_data[(int32_T)(p - 1)]; - p++; - } - } - } - - k++; - } - - for (pEnd = 0; (int32_T)(pEnd + 1) <= kEnd; pEnd++) { - idx_data[(int32_T)((int32_T)(j + pEnd) - 1)] = - sim_model_lib0_B->iwork_data_p[pEnd]; - } - - j = qEnd; - pEnd = (int32_T)(qEnd + i); - } - - i = i2; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_mo_dbaaphlnfkfkphdb_sortIdx(const int32_T x_data[], const - int32_T x_sizes, int32_T idx_data[], int32_T *idx_sizes, B_sim_model_lib0_T - *sim_model_lib0_B) -{ - int32_T i; - int32_T loop_ub; - int16_T b_x_idx_0; - b_x_idx_0 = (int16_T)x_sizes; - *idx_sizes = (int32_T)b_x_idx_0; - loop_ub = (int32_T)b_x_idx_0; - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - idx_data[i] = 0; - } - - if (x_sizes != 0) { - sim__gdbacbaajekfglng_mergesort(idx_data, x_data, x_sizes, sim_model_lib0_B); - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_fkfchlfkgdbimgdj_do_vectors(const real32_T a[50], const real32_T - b_data[], const int32_T b_sizes, real32_T c_data[], int32_T *c_sizes, int32_T - ia_data[], int32_T *ia_sizes, int32_T ib_data[], int32_T *ib_sizes, - B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T nc; - int32_T nib; - int32_T iafirst; - int32_T ialast; - int32_T ibfirst; - int32_T iblast; - real32_T bk; - int32_T c_ialast; - int32_T c_iblast; - real32_T b_bk; - boolean_T p; - real32_T absxk; - int32_T exponent; - int32_T ia_data_0[50]; - *ib_sizes = b_sizes; - nc = -1; - *ia_sizes = 0; - nib = 0; - iafirst = 0; - ialast = 1; - ibfirst = 0; - iblast = 1; - while ((ialast <= 50) && (iblast <= b_sizes)) { - c_ialast = ialast; - bk = kngdbaaaiecjbiec_skip_to_last_equal_value(&c_ialast, a); - ialast = c_ialast; - c_iblast = iblast; - b_bk = mgdjimgdopphcjek_skip_to_last_equal_value(&c_iblast, b_data, b_sizes); - iblast = c_iblast; - absxk = (real32_T)fabs((real_T)(b_bk / 2.0F)); - if ((!rtIsInfF(absxk)) && (!rtIsNaNF(absxk))) { - if (absxk <= 1.17549435E-38F) { - absxk = 1.4013E-45F; - } else { - frexp((real_T)absxk, &exponent); - absxk = (real32_T)ldexp((real_T)1.0F, (int32_T)(exponent - 24)); - } - } else { - absxk = (rtNaNF); - } - - if (((real32_T)fabs((real_T)(b_bk - bk)) < absxk) || (rtIsInfF(bk) && - rtIsInfF(b_bk) && ((bk > 0.0F) == (b_bk > 0.0F)))) { - p = true; - } else { - p = false; - } - - if (p) { - ialast = (int32_T)(c_ialast + 1); - iafirst = c_ialast; - iblast = (int32_T)(c_iblast + 1); - ibfirst = c_iblast; - } else { - if ((bk < b_bk) || rtIsNaNF(b_bk)) { - p = true; - } - - if (p) { - nc++; - (*ia_sizes)++; - c_data[nc] = bk; - ia_data[(int32_T)(*ia_sizes - 1)] = (int32_T)(iafirst + 1); - ialast = (int32_T)(c_ialast + 1); - iafirst = c_ialast; - } else { - nc++; - nib++; - c_data[nc] = b_bk; - ib_data[(int32_T)(nib - 1)] = (int32_T)(ibfirst + 1); - iblast = (int32_T)(c_iblast + 1); - ibfirst = c_iblast; - } - } - } - - while (ialast <= 50) { - iafirst = ialast; - bk = kngdbaaaiecjbiec_skip_to_last_equal_value(&iafirst, a); - nc++; - (*ia_sizes)++; - c_data[nc] = bk; - ia_data[(int32_T)(*ia_sizes - 1)] = ialast; - ialast = (int32_T)(iafirst + 1); - } - - while (iblast <= b_sizes) { - ialast = iblast; - bk = mgdjimgdopphcjek_skip_to_last_equal_value(&ialast, b_data, b_sizes); - nc++; - nib++; - c_data[nc] = bk; - ib_data[(int32_T)(nib - 1)] = iblast; - iblast = (int32_T)(ialast + 1); - } - - if (1 > *ia_sizes) { - *ia_sizes = 0; - } - - for (iafirst = 0; iafirst <= (int32_T)(*ia_sizes - 1); iafirst++) { - ia_data_0[iafirst] = ia_data[iafirst]; - } - - for (iafirst = 0; iafirst <= (int32_T)(*ia_sizes - 1); iafirst++) { - ia_data[iafirst] = ia_data_0[iafirst]; - } - - if (b_sizes > 0) { - if (1 > nib) { - nib = 0; - } - - for (iafirst = 0; iafirst <= (int32_T)(nib - 1); iafirst++) { - sim_model_lib0_B->ib_data[iafirst] = ib_data[iafirst]; - } - - *ib_sizes = nib; - for (iafirst = 0; iafirst <= (int32_T)(nib - 1); iafirst++) { - ib_data[iafirst] = sim_model_lib0_B->ib_data[iafirst]; - } - } - - if (1 > (int32_T)(nc + 1)) { - *c_sizes = 0; - } else { - *c_sizes = (int32_T)(nc + 1); - } - - for (iafirst = 0; iafirst <= (int32_T)(*c_sizes - 1); iafirst++) { - sim_model_lib0_B->c_data[iafirst] = c_data[iafirst]; - } - - for (iafirst = 0; iafirst <= (int32_T)(*c_sizes - 1); iafirst++) { - c_data[iafirst] = sim_model_lib0_B->c_data[iafirst]; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_model_lib0_rand_n(real_T varargin_2, real_T r_data[], int32_T - r_sizes[2], DW_sim_model_lib0_T *sim_model_lib0_DW) -{ - real_T b; - int32_T k; - r_sizes[0] = 1; - r_sizes[1] = (int32_T)varargin_2; - for (k = 0; k <= (int32_T)(r_sizes[1] - 1); k++) { - b = aimgnophaiekjecb_eml_rand_mt19937ar(sim_model_lib0_DW->state); - r_data[k] = b; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim__aiekglfcjekfdbai_mergesort(int32_T idx_data[], const real_T - x_data[], int32_T n, B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T k; - int32_T i; - int32_T i2; - int32_T j; - int32_T pEnd; - int32_T p; - int32_T q; - int32_T qEnd; - int32_T kEnd; - for (i = 1; i <= (int32_T)(n - 1); i += 2) { - if (x_data[(int32_T)(i - 1)] <= x_data[i]) { - idx_data[(int32_T)(i - 1)] = i; - idx_data[i] = (int32_T)(i + 1); - } else { - idx_data[(int32_T)(i - 1)] = (int32_T)(i + 1); - idx_data[i] = i; - } - } - - if ((int32_T)(n & 1) != 0) { - idx_data[(int32_T)(n - 1)] = n; - } - - i = 2; - while (i < n) { - i2 = (int32_T)(i << 1); - j = 1; - pEnd = (int32_T)(1 + i); - while (pEnd < (int32_T)(n + 1)) { - p = j; - q = pEnd; - qEnd = (int32_T)(j + i2); - if (qEnd > (int32_T)(n + 1)) { - qEnd = (int32_T)(n + 1); - } - - k = 0; - kEnd = (int32_T)(qEnd - j); - while ((int32_T)(k + 1) <= kEnd) { - if (x_data[(int32_T)(idx_data[(int32_T)(p - 1)] - 1)] <= x_data[(int32_T) - (idx_data[(int32_T)(q - 1)] - 1)]) { - sim_model_lib0_B->iwork_data_l[k] = idx_data[(int32_T)(p - 1)]; - p++; - if (p == pEnd) { - while (q < qEnd) { - k++; - sim_model_lib0_B->iwork_data_l[k] = idx_data[(int32_T)(q - 1)]; - q++; - } - } - } else { - sim_model_lib0_B->iwork_data_l[k] = idx_data[(int32_T)(q - 1)]; - q++; - if (q == qEnd) { - while (p < pEnd) { - k++; - sim_model_lib0_B->iwork_data_l[k] = idx_data[(int32_T)(p - 1)]; - p++; - } - } - } - - k++; - } - - for (pEnd = 0; (int32_T)(pEnd + 1) <= kEnd; pEnd++) { - idx_data[(int32_T)((int32_T)(j + pEnd) - 1)] = - sim_model_lib0_B->iwork_data_l[pEnd]; - } - - j = qEnd; - pEnd = (int32_T)(qEnd + i); - } - - i = i2; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_mo_cbimppphbaaaaiec_sortIdx(const real_T x_data[], const int32_T - x_sizes[2], int32_T idx_data[], int32_T idx_sizes[2], B_sim_model_lib0_T - *sim_model_lib0_B) -{ - int32_T i; - int32_T loop_ub; - idx_sizes[0] = 1; - idx_sizes[1] = x_sizes[1]; - loop_ub = x_sizes[1]; - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - idx_data[i] = 0; - } - - sim__aiekglfcjekfdbai_mergesort(idx_data, x_data, x_sizes[1], sim_model_lib0_B); -} - -// Function for MATLAB Function: '/generate_output' -static void sim_model_lib0_randperm_i(real_T n, real_T p_data[], int32_T - p_sizes[2], B_sim_model_lib0_T *sim_model_lib0_B, DW_sim_model_lib0_T - *sim_model_lib0_DW) -{ - int32_T i; - int32_T idx_sizes[2]; - int32_T p_idx_1; - int32_T p_idx_0; - sim_model_lib0_rand_n(n, p_data, p_sizes, sim_model_lib0_DW); - sim_mo_cbimppphbaaaaiec_sortIdx(p_data, p_sizes, sim_model_lib0_B->idx_data_g, - idx_sizes, sim_model_lib0_B); - p_idx_0 = p_sizes[0]; - p_idx_1 = p_sizes[1]; - p_sizes[0] = 1; - for (i = 0; i <= (int32_T)(p_idx_1 - 1); i++) { - p_data[i] = (real_T)sim_model_lib0_B->idx_data_g[(int32_T)(p_idx_0 * i)]; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_mo_lfcjjmgdkfkniekn_sortIdx(const real32_T x[17264], const - int32_T col[2], int32_T idx[8632], B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T k; - int32_T i; - int32_T i2; - int32_T j; - int32_T pEnd; - int32_T p; - int32_T q; - int32_T qEnd; - int32_T kEnd; - for (i = 0; i <= 8631; i += 2) { - if (mohdcjmgnohlknoh_sortLE(x, col, (int32_T)(i + 1), (int32_T)(i + 2))) { - idx[i] = (int32_T)(i + 1); - idx[(int32_T)(i + 1)] = (int32_T)(i + 2); - } else { - idx[i] = (int32_T)(i + 2); - idx[(int32_T)(i + 1)] = (int32_T)(i + 1); - } - } - - i = 2; - while (i < 8632) { - i2 = (int32_T)(i << 1); - j = 1; - pEnd = (int32_T)(1 + i); - while (pEnd < 8633) { - p = j; - q = pEnd; - qEnd = (int32_T)(j + i2); - if (qEnd > 8633) { - qEnd = 8633; - } - - k = 0; - kEnd = (int32_T)(qEnd - j); - while ((int32_T)(k + 1) <= kEnd) { - if (mohdcjmgnohlknoh_sortLE(x, col, idx[(int32_T)(p - 1)], idx[(int32_T) - (q - 1)])) { - sim_model_lib0_B->iwork[k] = idx[(int32_T)(p - 1)]; - p++; - if (p == pEnd) { - while (q < qEnd) { - k++; - sim_model_lib0_B->iwork[k] = idx[(int32_T)(q - 1)]; - q++; - } - } - } else { - sim_model_lib0_B->iwork[k] = idx[(int32_T)(q - 1)]; - q++; - if (q == qEnd) { - while (p < pEnd) { - k++; - sim_model_lib0_B->iwork[k] = idx[(int32_T)(p - 1)]; - p++; - } - } - } - - k++; - } - - for (pEnd = 0; (int32_T)(pEnd + 1) <= kEnd; pEnd++) { - idx[(int32_T)((int32_T)(j + pEnd) - 1)] = sim_model_lib0_B->iwork[pEnd]; - } - - j = qEnd; - pEnd = (int32_T)(qEnd + i); - } - - i = i2; - } -} - -// Function for MATLAB Function: '/generate_output' -static void imglgdjmcjmomgdj_apply_row_perm(real32_T y[17264], const int32_T - idx[8632], B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T j; - int32_T i; - for (j = 0; j < 2; j++) { - for (i = 0; i < 8632; i++) { - sim_model_lib0_B->ycol[i] = y[(int32_T)((int32_T)((int32_T)(8632 * j) + - idx[i]) - 1)]; - } - - memcpy(&y[(int32_T)(j * 8632)], &sim_model_lib0_B->ycol[0], (uint32_T)(8632U - * sizeof(real32_T))); - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_m_nopphdbaaiecfkng_sortrows(real32_T y[17264], real_T ndx[8632], - B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T col[2]; - int32_T k; - col[0] = 1; - col[1] = 2; - sim_mo_lfcjjmgdkfkniekn_sortIdx(y, col, sim_model_lib0_B->idx_p, - sim_model_lib0_B); - imglgdjmcjmomgdj_apply_row_perm(y, sim_model_lib0_B->idx_p, sim_model_lib0_B); - for (k = 0; k < 8632; k++) { - ndx[k] = (real_T)sim_model_lib0_B->idx_p[k]; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_model_lib0_rand_o(real_T varargin_2, real_T r_data[], int32_T - r_sizes[2], DW_sim_model_lib0_T *sim_model_lib0_DW) -{ - real_T b; - int32_T k; - r_sizes[0] = 1; - r_sizes[1] = (int32_T)varargin_2; - for (k = 0; k <= (int32_T)(r_sizes[1] - 1); k++) { - b = aimgnophaiekjecb_eml_rand_mt19937ar(sim_model_lib0_DW->state_d); - r_data[k] = b; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim__kfkflfcjohdbglno_mergesort(int32_T idx_data[], const real_T - x_data[], int32_T n, B_sim_model_lib0_T *sim_model_lib0_B) -{ - int32_T k; - int32_T i; - int32_T i2; - int32_T j; - int32_T pEnd; - int32_T p; - int32_T q; - int32_T qEnd; - int32_T kEnd; - for (i = 1; i <= (int32_T)(n - 1); i += 2) { - if (x_data[(int32_T)(i - 1)] <= x_data[i]) { - idx_data[(int32_T)(i - 1)] = i; - idx_data[i] = (int32_T)(i + 1); - } else { - idx_data[(int32_T)(i - 1)] = (int32_T)(i + 1); - idx_data[i] = i; - } - } - - if ((int32_T)(n & 1) != 0) { - idx_data[(int32_T)(n - 1)] = n; - } - - i = 2; - while (i < n) { - i2 = (int32_T)(i << 1); - j = 1; - pEnd = (int32_T)(1 + i); - while (pEnd < (int32_T)(n + 1)) { - p = j; - q = pEnd; - qEnd = (int32_T)(j + i2); - if (qEnd > (int32_T)(n + 1)) { - qEnd = (int32_T)(n + 1); - } - - k = 0; - kEnd = (int32_T)(qEnd - j); - while ((int32_T)(k + 1) <= kEnd) { - if (x_data[(int32_T)(idx_data[(int32_T)(p - 1)] - 1)] <= x_data[(int32_T) - (idx_data[(int32_T)(q - 1)] - 1)]) { - sim_model_lib0_B->iwork_data[k] = idx_data[(int32_T)(p - 1)]; - p++; - if (p == pEnd) { - while (q < qEnd) { - k++; - sim_model_lib0_B->iwork_data[k] = idx_data[(int32_T)(q - 1)]; - q++; - } - } - } else { - sim_model_lib0_B->iwork_data[k] = idx_data[(int32_T)(q - 1)]; - q++; - if (q == qEnd) { - while (p < pEnd) { - k++; - sim_model_lib0_B->iwork_data[k] = idx_data[(int32_T)(p - 1)]; - p++; - } - } - } - - k++; - } - - for (pEnd = 0; (int32_T)(pEnd + 1) <= kEnd; pEnd++) { - idx_data[(int32_T)((int32_T)(j + pEnd) - 1)] = - sim_model_lib0_B->iwork_data[pEnd]; - } - - j = qEnd; - pEnd = (int32_T)(qEnd + i); - } - - i = i2; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_mo_bimgaiekaimglfkf_sortIdx(const real_T x_data[], const int32_T - x_sizes[2], int32_T idx_data[], int32_T idx_sizes[2], B_sim_model_lib0_T - *sim_model_lib0_B) -{ - int32_T i; - int32_T loop_ub; - idx_sizes[0] = 1; - idx_sizes[1] = x_sizes[1]; - loop_ub = x_sizes[1]; - for (i = 0; i <= (int32_T)(loop_ub - 1); i++) { - idx_data[i] = 0; - } - - if (x_sizes[1] != 0) { - sim__kfkflfcjohdbglno_mergesort(idx_data, x_data, x_sizes[1], - sim_model_lib0_B); - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_model_lib0_randperm_e(real_T n, real_T p_data[], int32_T - p_sizes[2], B_sim_model_lib0_T *sim_model_lib0_B, DW_sim_model_lib0_T - *sim_model_lib0_DW) -{ - int32_T i; - int32_T idx_sizes[2]; - int32_T p_idx_1; - int32_T p_idx_0; - sim_model_lib0_rand_o(n, p_data, p_sizes, sim_model_lib0_DW); - sim_mo_bimgaiekaimglfkf_sortIdx(p_data, p_sizes, sim_model_lib0_B->idx_data, - idx_sizes, sim_model_lib0_B); - p_idx_0 = p_sizes[0]; - p_idx_1 = p_sizes[1]; - p_sizes[0] = 1; - for (i = 0; i <= (int32_T)(p_idx_1 - 1); i++) { - p_data[i] = (real_T)sim_model_lib0_B->idx_data[(int32_T)(p_idx_0 * i)]; - } -} - -// Function for MATLAB Function: '/generate_output' -static void sim_mode_format_landmark_output(const real32_T points_in_iss[25896], - const real32_T points_in_cam[17264], const boolean_T valid_in[8632], real32_T - points_out_iss[150], real32_T points_out_cam[100], real32_T valid_out[50], - B_sim_model_lib0_T *sim_model_lib0_B, DW_sim_model_lib0_T *sim_model_lib0_DW) -{ - int32_T nb; - int32_T k; - int32_T k0; - int32_T loop_ub; - int32_T i; - int32_T random_order_sizes[2]; - int32_T unique_rows_sizes; - int32_T c_valid_in_sizes; - int32_T b_points_in_iss_sizes_idx_0; - int32_T b_points_in_iss_sizes_idx_0_0; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // format_landmark_output.m - // inputs: - // num_pts_out = Required number of points to be reported - // points_in iss = ALL [x,y,z] position of points in the ISS frame - // points_in_cam = ALL [u,v] pixel location of points in the camera - // valid_in = indicates which points are valid - // - // outputs: - // points_out_iss = output [x,y,z] position of points in the ISS frame - // points_out_cam = output [u,v] pixel location of points in the camera - // valid_out = indicates which values of the output are valid! - // initialize the outputs - // Find unique 2D points in the cam can be thought of as either: - // a) points blocked by other points are not visible to the camera, despite technically being in the FOV of the camera - // b) Only can have 1 point reported per pixel location - memcpy(&sim_model_lib0_B->b_b[0], &points_in_cam[0], (uint32_T)(17264U * - sizeof(real32_T))); - sim_m_nopphdbaaiecfkng_sortrows(sim_model_lib0_B->b_b, sim_model_lib0_B->idx, - sim_model_lib0_B); - random_order_sizes[0] = 8632; - random_order_sizes[1] = 2; - nb = 0; - k = 1; - while (k <= 8632) { - k0 = k; - do { - k++; - } while (!((k > 8632) || ohdbiekfdjechlno_rows_differ(sim_model_lib0_B->b_b, - random_order_sizes, k0, k))); - - nb++; - sim_model_lib0_B->b_b[(int32_T)(nb - 1)] = sim_model_lib0_B->b_b[(int32_T) - (k0 - 1)]; - sim_model_lib0_B->b_b[(int32_T)(nb + 8631)] = sim_model_lib0_B->b_b[(int32_T) - (k0 + 8631)]; - sim_model_lib0_B->idx[(int32_T)(nb - 1)] = sim_model_lib0_B->idx[(int32_T) - (k0 - 1)]; - } - - if (1 > nb) { - loop_ub = 0; - } else { - loop_ub = nb; - } - - for (k = 0; k <= (int32_T)(loop_ub - 1); k++) { - sim_model_lib0_B->b_data_c[k] = sim_model_lib0_B->b_b[k]; - } - - for (k = 0; k <= (int32_T)(loop_ub - 1); k++) { - sim_model_lib0_B->b_data_c[(int32_T)(k + loop_ub)] = sim_model_lib0_B->b_b - [(int32_T)(k + 8632)]; - } - - for (k = 0; k <= (int32_T)(loop_ub - 1); k++) { - sim_model_lib0_B->b_b[k] = sim_model_lib0_B->b_data_c[k]; - } - - for (k = 0; k <= (int32_T)(loop_ub - 1); k++) { - sim_model_lib0_B->b_b[(int32_T)(k + loop_ub)] = sim_model_lib0_B->b_data_c - [(int32_T)(k + loop_ub)]; - } - - for (k = 0; (int32_T)(k + 1) <= nb; k++) { - sim_model_lib0_B->indx_data[k] = (int32_T)sim_model_lib0_B->idx[k]; - } - - unique_rows_sizes = (int32_T)(nb - 1); - for (k = 0; k <= (int32_T)(nb - 1); k++) { - sim_model_lib0_B->unique_rows_data[k] = sim_model_lib0_B->indx_data[k]; - } - - b_points_in_iss_sizes_idx_0 = nb; - for (k = 0; k < 3; k++) { - for (i = 0; i <= (int32_T)(nb - 1); i++) { - sim_model_lib0_B->b_points_in_iss_data_c[(int32_T)(i + (int32_T)(nb * k))] - = points_in_iss[(int32_T)((int32_T)((int32_T)(8632 * k) + - sim_model_lib0_B->indx_data[i]) - 1)]; - } - } - - for (k = 0; k <= (int32_T)(nb - 1); k++) { - sim_model_lib0_B->b_valid_in_data_j[k] = valid_in[(int32_T) - (sim_model_lib0_B->indx_data[k] - 1)]; - } - - // find how many pts are valid, and determine how that compares to required number - c_valid_in_sizes = (int32_T)(nb - 1); - nb = 0; - for (k = 0; k <= c_valid_in_sizes; k++) { - if (valid_in[(int32_T)(sim_model_lib0_B->indx_data[k] - 1)]) { - nb++; - } - } - - if (50.0F - (real32_T)nb == 0.0F) { - // if exactly the right ammount, just report them! - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - k++; - } - } - - c_valid_in_sizes = k; - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - sim_model_lib0_B->indx_data[k] = (int32_T)(k0 + 1); - k++; - } - } - - for (k = 0; k < 3; k++) { - for (i = 0; i <= (int32_T)(c_valid_in_sizes - 1); i++) { - sim_model_lib0_B->b_points_in_iss_data_k[(int32_T)(i + (int32_T) - (c_valid_in_sizes * k))] = sim_model_lib0_B->b_points_in_iss_data_c - [(int32_T)((int32_T)((int32_T)(b_points_in_iss_sizes_idx_0 * k) + - sim_model_lib0_B->indx_data[i]) - 1)]; - } - } - - for (k = 0; k < 3; k++) { - memcpy(&points_out_iss[(int32_T)(k * 50)], - &sim_model_lib0_B->b_points_in_iss_data_k[(int32_T)(k * 50)], - (uint32_T)(50U * sizeof(real32_T))); - } - - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - k++; - } - } - - c_valid_in_sizes = k; - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - sim_model_lib0_B->indx_data[k] = (int32_T)(k0 + 1); - k++; - } - } - - for (k = 0; k <= (int32_T)(c_valid_in_sizes - 1); k++) { - sim_model_lib0_B->b_data_c[k] = sim_model_lib0_B->b_b[(int32_T) - (sim_model_lib0_B->indx_data[k] - 1)]; - } - - for (k = 0; k <= (int32_T)(c_valid_in_sizes - 1); k++) { - sim_model_lib0_B->b_data_c[(int32_T)(k + c_valid_in_sizes)] = - sim_model_lib0_B->b_b[(int32_T)((int32_T)(sim_model_lib0_B->indx_data[k] - + loop_ub) - 1)]; - } - - for (k = 0; k < 2; k++) { - memcpy(&points_out_cam[(int32_T)(k * 50)], &sim_model_lib0_B->b_data_c - [(int32_T)(k * 50)], (uint32_T)(50U * sizeof(real32_T))); - } - - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - sim_model_lib0_B->indx_data[k] = (int32_T)(k0 + 1); - k++; - } - } - - for (k = 0; k < 50; k++) { - valid_out[k] = (real32_T)sim_model_lib0_B->b_valid_in_data_j[(int32_T) - (sim_model_lib0_B->indx_data[k] - 1)]; - } - } else if (50.0F - (real32_T)nb > 0.0F) { - // if we need more points, back fill with zeros, and zero the valid cooresponding valid flag - k0 = 0; - for (k = 0; k <= unique_rows_sizes; k++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k] - 1)]) { - k0++; - } - } - - c_valid_in_sizes = k0; - k0 = 0; - for (k = 0; k <= unique_rows_sizes; k++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k] - 1)]) { - sim_model_lib0_B->indx_data[k0] = (int32_T)(k + 1); - k0++; - } - } - - k0 = (int32_T)(50.0F - (real32_T)nb); - b_points_in_iss_sizes_idx_0_0 = (int32_T)(c_valid_in_sizes + k0); - for (k = 0; k < 3; k++) { - for (i = 0; i <= (int32_T)(c_valid_in_sizes - 1); i++) { - sim_model_lib0_B->b_points_in_iss_data[(int32_T)(i + (int32_T) - (b_points_in_iss_sizes_idx_0_0 * k))] = - sim_model_lib0_B->b_points_in_iss_data_c[(int32_T)((int32_T)((int32_T) - (b_points_in_iss_sizes_idx_0 * k) + sim_model_lib0_B->indx_data[i]) - - 1)]; - } - } - - for (k = 0; k < 3; k++) { - for (i = 0; i <= (int32_T)(k0 - 1); i++) { - sim_model_lib0_B->b_points_in_iss_data[(int32_T)((int32_T)(i + - c_valid_in_sizes) + (int32_T)(b_points_in_iss_sizes_idx_0_0 * k))] = - 0.0F; - } - } - - for (k = 0; k < 3; k++) { - memcpy(&points_out_iss[(int32_T)(k * 50)], - &sim_model_lib0_B->b_points_in_iss_data[(int32_T)(k * 50)], - (uint32_T)(50U * sizeof(real32_T))); - } - - k0 = 0; - for (k = 0; k <= unique_rows_sizes; k++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k] - 1)]) { - k0++; - } - } - - c_valid_in_sizes = k0; - k0 = 0; - for (k = 0; k <= unique_rows_sizes; k++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k] - 1)]) { - sim_model_lib0_B->indx_data[k0] = (int32_T)(k + 1); - k0++; - } - } - - k0 = (int32_T)(50.0F - (real32_T)nb); - b_points_in_iss_sizes_idx_0 = (int32_T)(c_valid_in_sizes + k0); - for (k = 0; k <= (int32_T)(c_valid_in_sizes - 1); k++) { - sim_model_lib0_B->b_data[k] = sim_model_lib0_B->b_b[(int32_T) - (sim_model_lib0_B->indx_data[k] - 1)]; - } - - for (k = 0; k <= (int32_T)(c_valid_in_sizes - 1); k++) { - sim_model_lib0_B->b_data[(int32_T)(k + b_points_in_iss_sizes_idx_0)] = - sim_model_lib0_B->b_b[(int32_T)((int32_T)(sim_model_lib0_B->indx_data[k] - + loop_ub) - 1)]; - } - - for (k = 0; k <= (int32_T)(k0 - 1); k++) { - sim_model_lib0_B->b_data[(int32_T)(k + c_valid_in_sizes)] = 0.0F; - } - - for (k = 0; k <= (int32_T)(k0 - 1); k++) { - sim_model_lib0_B->b_data[(int32_T)((int32_T)(k + c_valid_in_sizes) + - b_points_in_iss_sizes_idx_0)] = 0.0F; - } - - for (k = 0; k < 2; k++) { - memcpy(&points_out_cam[(int32_T)(k * 50)], &sim_model_lib0_B->b_data - [(int32_T)(k * 50)], (uint32_T)(50U * sizeof(real32_T))); - } - - k0 = 0; - for (k = 0; k <= unique_rows_sizes; k++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k] - 1)]) { - k0++; - } - } - - c_valid_in_sizes = k0; - k0 = 0; - for (k = 0; k <= unique_rows_sizes; k++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k] - 1)]) { - sim_model_lib0_B->indx_data[k0] = (int32_T)(k + 1); - k0++; - } - } - - for (k = 0; k <= (int32_T)(c_valid_in_sizes - 1); k++) { - sim_model_lib0_B->b_valid_in_data[k] = (int8_T) - sim_model_lib0_B->b_valid_in_data_j[(int32_T) - (sim_model_lib0_B->indx_data[k] - 1)]; - } - - loop_ub = (int32_T)(50.0F - (real32_T)nb); - for (k = 0; k <= (int32_T)(loop_ub - 1); k++) { - sim_model_lib0_B->b_valid_in_data[(int32_T)(k + c_valid_in_sizes)] = 0; - } - - for (k = 0; k < 50; k++) { - valid_out[k] = (real32_T)sim_model_lib0_B->b_valid_in_data[k]; - } - } else { - // if we have too many points, take a random sample - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - k++; - } - } - - if (k == 0) { - k = 0; - } else { - if (!(k >= 3)) { - k = 3; - } - } - - sim_model_lib0_randperm_e((real_T)k, sim_model_lib0_B->idx, - random_order_sizes, sim_model_lib0_B, sim_model_lib0_DW); - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - k++; - } - } - - c_valid_in_sizes = k; - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - sim_model_lib0_B->indx_data[k] = (int32_T)(k0 + 1); - k++; - } - } - - for (k = 0; k < 3; k++) { - for (i = 0; i <= (int32_T)(c_valid_in_sizes - 1); i++) { - sim_model_lib0_B->b_points_in_iss_data_k[(int32_T)(i + (int32_T) - (c_valid_in_sizes * k))] = sim_model_lib0_B->b_points_in_iss_data_c - [(int32_T)((int32_T)((int32_T)(b_points_in_iss_sizes_idx_0 * k) + - sim_model_lib0_B->indx_data[i]) - 1)]; - } - } - - for (k = 0; k < 3; k++) { - for (i = 0; i < 50; i++) { - points_out_iss[(int32_T)(i + (int32_T)(50 * k))] = - sim_model_lib0_B->b_points_in_iss_data_k[(int32_T)((int32_T)((int32_T) - (c_valid_in_sizes * k) + (int32_T)sim_model_lib0_B->idx[i]) - 1)]; - } - } - - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - k++; - } - } - - c_valid_in_sizes = k; - k = 0; - for (k0 = 0; k0 <= unique_rows_sizes; k0++) { - if (valid_in[(int32_T)(sim_model_lib0_B->unique_rows_data[k0] - 1)]) { - sim_model_lib0_B->indx_data[k] = (int32_T)(k0 + 1); - k++; - } - } - - for (k = 0; k <= (int32_T)(c_valid_in_sizes - 1); k++) { - sim_model_lib0_B->b_data_c[k] = sim_model_lib0_B->b_b[(int32_T) - (sim_model_lib0_B->indx_data[k] - 1)]; - } - - for (k = 0; k <= (int32_T)(c_valid_in_sizes - 1); k++) { - sim_model_lib0_B->b_data_c[(int32_T)(k + c_valid_in_sizes)] = - sim_model_lib0_B->b_b[(int32_T)((int32_T)(sim_model_lib0_B->indx_data[k] - + loop_ub) - 1)]; - } - - for (k = 0; k < 2; k++) { - for (i = 0; i < 50; i++) { - points_out_cam[(int32_T)(i + (int32_T)(50 * k))] = - sim_model_lib0_B->b_data_c[(int32_T)((int32_T)((int32_T) - (c_valid_in_sizes * k) + (int32_T)sim_model_lib0_B->idx[i]) - 1)]; - } - } - - for (nb = 0; nb < 50; nb++) { - valid_out[nb] = 1.0F; - } - } -} - -// Model step function for TID0 -void sim_model_lib0_step0(RT_MODEL_sim_model_lib0_T *const sim_model_lib0_M, - act_msg *sim_model_lib0_U_act_msg_l, cmc_msg *sim_model_lib0_U_cmc_msg_in, - cvs_optical_flow_msg *sim_model_lib0_Y_cvs_optical_flow_msg_n, - cvs_handrail_msg *sim_model_lib0_Y_cvs_handrail_msg_h, cmc_msg - *sim_model_lib0_Y_cmc_msg_c, imu_msg *sim_model_lib0_Y_imu_msg_o, env_msg - *sim_model_lib0_Y_env_msg_i, bpm_msg *sim_model_lib0_Y_bpm_msg_h, - cvs_registration_pulse *sim_model_lib0_Y_cvs_registration_pulse_d, - cvs_landmark_msg *sim_model_lib0_Y_cvs_landmark_msg_n, cvs_landmark_msg - *sim_model_lib0_Y_cvs_ar_tag_msg, ex_time_msg *sim_model_lib0_Y_ex_time_msg_m) - // Sample time: [0.016s, 0.0s] -{ - P_sim_model_lib0_T *sim_model_lib0_P = ((P_sim_model_lib0_T *) - sim_model_lib0_M->defaultParam); - B_sim_model_lib0_T *sim_model_lib0_B = ((B_sim_model_lib0_T *) - sim_model_lib0_M->blockIO); - DW_sim_model_lib0_T *sim_model_lib0_DW = ((DW_sim_model_lib0_T *) - sim_model_lib0_M->dwork); - real32_T rtb_P_B_ISS_SS[3]; - real32_T rtb_Switch_al[19]; - real32_T rtb_V_B_ISS_ISS[3]; - real32_T rtb_A_B_ISS_ISS[3]; - real32_T rtb_omega_B_ISS_B[3]; - real32_T rtb_alpha_B_ECI_B[3]; - uint8_T rtb_Bias; - real32_T rtb_imu_gyro_bias[3]; - real32_T rtb_bpm_force_B[3]; - real32_T rtb_bpm_torque_B[3]; - real_T rtb_Product_l[4]; - real_T rtb_Assignment_jy[9]; - real_T rtb_MatrixConcatenate[16]; - real_T rtb_Assignment[9]; - real_T rtb_Sum; - real32_T rtb_Sqrt_m; - real32_T rtb_Product_e[4]; - real32_T rtb_Merge_kq[4]; - real32_T rtb_Merge_m[4]; - real32_T rtb_Assignment_m[9]; - real32_T rtb_Elementproduct_c[6]; - uint32_T rtb_Switch1; - uint32_T rtb_Switch_g; - real32_T rtb_bpm_servo_curr[12]; - real32_T rtb_bpm_nozzle_theta[12]; - real32_T rtb_VectorConcatenate_d[16]; - real32_T rtb_Product_p; - real32_T rtb_TrigonometricFunction1_c; - real32_T rtb_Assignment_ok[16]; - real32_T rtb_Delay1_bpm_torque_B[3]; - real32_T rtb_Merge_o[3]; - real32_T rtb_Sum2_f[3]; - real_T rtb_Add3_a[3]; - real_T rtb_Add3[3]; - real_T rtb_random_noise[3]; - real_T rtb_linearaccelbias; - real_T rtb_P_sensor_CG_B[3]; - boolean_T rtb_LogicalOperator2[12]; - int32_T i; - real32_T tmp[3]; - int32_T i_0; - boolean_T tmp_0; - real_T rtb_Product_p_0[9]; - real32_T tmp_1[9]; - real32_T rtb_Merge_b[9]; - real32_T rtb_Assignment_i[9]; - real32_T tmp_2[9]; - real32_T tmp_3[9]; - real32_T tmp_4[9]; - real32_T tmp_5[9]; - real_T rtb_Assignment_3[9]; - real_T tmp_6[9]; - real_T tmp_7[9]; - real_T rtb_Assignment_c[9]; - real32_T tmp_8[9]; - real32_T tmp_9[9]; - real32_T tmp_a[3]; - real32_T tmp_b[3]; - real32_T rtb_bpm_torque_B_c; - real32_T rtb_Merge_c; - real32_T rtb_V_B_ISS_ISS_h; - real_T rtb_Product; - real32_T rtb_bpm_motor_curr_idx_0; - uint32_T rtb_Switch1_p_idx_0; - uint32_T rtb_Switch1_p_idx_1; - real_T rtb_DataTypeConversion5_idx_1; - real_T rtb_DataTypeConversion5_idx_2; - real_T rtb_DataTypeConversion5_idx_0; - real32_T rtb_Delay1_bpm_force_B_idx_1; - real32_T rtb_Delay1_bpm_force_B_idx_2; - real_T y; - real_T y_0; - - // Update the flag to indicate when data transfers from - // Sample time: [0.016s, 0.0s] to Sample time: [0.16s, 0.0s] - (sim_model_lib0_M->Timing.RateInteraction.TID0_1)++; - if ((sim_model_lib0_M->Timing.RateInteraction.TID0_1) > 9) { - sim_model_lib0_M->Timing.RateInteraction.TID0_1 = 0; - } - - // Update the flag to indicate when data transfers from - // Sample time: [0.016s, 0.0s] to Sample time: [0.20800000000000002s, 0.0s] - (sim_model_lib0_M->Timing.RateInteraction.TID0_2)++; - if ((sim_model_lib0_M->Timing.RateInteraction.TID0_2) > 12) { - sim_model_lib0_M->Timing.RateInteraction.TID0_2 = 0; - } - - // Update the flag to indicate when data transfers from - // Sample time: [0.016s, 0.0s] to Sample time: [0.272s, 0.0s] - (sim_model_lib0_M->Timing.RateInteraction.TID0_3)++; - if ((sim_model_lib0_M->Timing.RateInteraction.TID0_3) > 16) { - sim_model_lib0_M->Timing.RateInteraction.TID0_3 = 0; - } - - // Update the flag to indicate when data transfers from - // Sample time: [0.016s, 0.0s] to Sample time: [0.336s, 0.0s] - (sim_model_lib0_M->Timing.RateInteraction.TID0_4)++; - if ((sim_model_lib0_M->Timing.RateInteraction.TID0_4) > 20) { - sim_model_lib0_M->Timing.RateInteraction.TID0_4 = 0; - } - - // RateTransition: '/Rate Transition3' - if (sim_model_lib0_M->Timing.RateInteraction.TID0_2 == 1) { - sim_model_lib0_B->RateTransition3 = - sim_model_lib0_DW->RateTransition3_Buffer0; - - // RateTransition: '/Rate Transition1' - sim_model_lib0_B->RateTransition1 = - sim_model_lib0_DW->RateTransition1_Buffer0; - - // RateTransition: '/Rate Transition5' - memcpy(&sim_model_lib0_B->RateTransition5[0], - &sim_model_lib0_DW->RateTransition5_Buffer0[0], (uint32_T)(150U * - sizeof(real32_T))); - - // RateTransition: '/Rate Transition9' - memcpy(&sim_model_lib0_B->RateTransition9[0], - &sim_model_lib0_DW->RateTransition9_Buffer0[0], (uint32_T)(150U * - sizeof(real32_T))); - - // RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_B->RateTransition7[i] = - sim_model_lib0_DW->RateTransition7_Buffer0[i]; - } - - // End of RateTransition: '/Rate Transition7' - } - - // End of RateTransition: '/Rate Transition3' - - // SignalConversion: '/ConcatBufferAtVector Concatenate5In1' incorporates: - // Constant: '/Constant1' - // Selector: '/select_current_command' - // UnitDelay: '/Unit Delay' - - rtb_omega_B_ISS_B[0] = sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 155)]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate5In2' incorporates: - // Constant: '/Constant1' - // Selector: '/select_current_command' - // UnitDelay: '/Unit Delay' - - rtb_omega_B_ISS_B[1] = sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 167)]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate5In3' incorporates: - // Constant: '/Constant1' - // Selector: '/select_current_command' - // UnitDelay: '/Unit Delay' - - rtb_omega_B_ISS_B[2] = sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 179)]; - - // Bias: '/Bias' incorporates: - // UnitDelay: '/Unit Delay' - - rtb_Bias = (uint8_T)(uint32_T)((uint32_T)sim_model_lib0_DW->UnitDelay_DSTATE_i - + (uint32_T)sim_model_lib0_P->Bias_Bias); - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // RelationalOperator: '/Relational Operator1' - // Selector: '/select_next_command' - // Selector: '/select_next_time' - // Switch: '/Switch' - - if ((real32_T)rtb_Bias <= sim_model_lib0_P->mlp_num_commands) { - rtb_Switch1_p_idx_0 = sim_model_lib0_P->mlp_command_times[(int32_T)((int32_T) - rtb_Bias - 1)]; - rtb_Switch1_p_idx_1 = sim_model_lib0_P->mlp_command_times[(int32_T)((int32_T) - rtb_Bias + 11)]; - for (i_0 = 0; i_0 < 19; i_0++) { - rtb_Switch_al[i_0] = sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - ((int32_T)(12 * i_0) + (int32_T)rtb_Bias) - 1)]; - } - } else { - rtb_Switch1_p_idx_0 = sim_model_lib0_P->Constant2_Value_p1[0]; - rtb_Switch1_p_idx_1 = sim_model_lib0_P->Constant2_Value_p1[1]; - memcpy(&rtb_Switch_al[0], &sim_model_lib0_P->mlp_dummy_state_cmd[0], - (uint32_T)(19U * sizeof(real32_T))); - } - - // End of Switch: '/Switch1' - - // SignalConversion: '/ConcatBufferAtVector Concatenate9In1' - rtb_Merge_o[0] = rtb_Switch_al[6]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate9In2' - rtb_Merge_o[1] = rtb_Switch_al[7]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate9In3' - rtb_Merge_o[2] = rtb_Switch_al[8]; - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // RelationalOperator: '/Compare' - // Sum: '/Sum1' - // Sum: '/Sum2' - // Switch: '/Switch' - // UnitDelay: '/Unit Delay' - // UnitDelay: '/Unit Delay1' - - if (sim_model_lib0_DW->UnitDelay_DSTATE >= - sim_model_lib0_P->CompareToConstant_const_d) { - rtb_Switch1 = (uint32_T)(sim_model_lib0_P->Constant2_Value_jp + - sim_model_lib0_DW->UnitDelay1_DSTATE); - rtb_Switch_g = (uint32_T)(sim_model_lib0_DW->UnitDelay_DSTATE - - sim_model_lib0_P->Constant1_Value_hc); - } else { - rtb_Switch1 = sim_model_lib0_DW->UnitDelay1_DSTATE; - rtb_Switch_g = sim_model_lib0_DW->UnitDelay_DSTATE; - } - - // End of Switch: '/Switch1' - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant8' - // Constant: '/Constant2' - // DataTypeConversion: '/Data Type Conversion1' - - memcpy(&rtb_Assignment_jy[0], &sim_model_lib0_P->Constant2_Value_i[0], - (uint32_T)(9U * sizeof(real_T))); - rtb_Assignment_jy[0] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[3]; - rtb_Assignment_jy[4] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[3]; - rtb_Assignment_jy[8] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant8' - // Constant: '/Constant3' - // DataTypeConversion: '/Data Type Conversion1' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Product_p_0[0] = sim_model_lib0_P->Constant3_Value; - rtb_Product_p_0[1] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[2]; - rtb_Product_p_0[2] = sim_model_lib0_P->Gain_Gain_p * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[1]; - rtb_Product_p_0[3] = sim_model_lib0_P->Gain1_Gain * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[2]; - rtb_Product_p_0[4] = sim_model_lib0_P->Constant3_Value; - rtb_Product_p_0[5] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[0]; - rtb_Product_p_0[6] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[1]; - rtb_Product_p_0[7] = sim_model_lib0_P->Gain2_Gain * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[0]; - rtb_Product_p_0[8] = sim_model_lib0_P->Constant3_Value; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Constant: '/Constant8' - // DataTypeConversion: '/Data Type Conversion1' - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_MatrixConcatenate[(int32_T)(i_0 << 2)] = rtb_Assignment_jy[(int32_T)(3 * - i_0)] + rtb_Product_p_0[(int32_T)(3 * i_0)]; - rtb_MatrixConcatenate[(int32_T)(1 + (int32_T)(i_0 << 2))] = - rtb_Assignment_jy[(int32_T)((int32_T)(3 * i_0) + 1)] + rtb_Product_p_0 - [(int32_T)((int32_T)(3 * i_0) + 1)]; - rtb_MatrixConcatenate[(int32_T)(2 + (int32_T)(i_0 << 2))] = - rtb_Assignment_jy[(int32_T)((int32_T)(3 * i_0) + 2)] + rtb_Product_p_0 - [(int32_T)((int32_T)(3 * i_0) + 2)]; - } - - rtb_MatrixConcatenate[3] = sim_model_lib0_P->Gain1_Gain_a * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[0]; - rtb_MatrixConcatenate[7] = sim_model_lib0_P->Gain1_Gain_a * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[1]; - rtb_MatrixConcatenate[11] = sim_model_lib0_P->Gain1_Gain_a * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Switch: '/Switch3' incorporates: - // Constant: '/Constant3' - - tmp_0 = ((int32_T)sim_model_lib0_P->tun_epson_report_truth != 0); - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant8' - // DataTypeConversion: '/Data Type Conversion1' - - rtb_MatrixConcatenate[12] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[0]; - - // Product: '/Product' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant13' - // Switch: '/Switch3' - - if (tmp_0) { - rtb_Sum = sim_model_lib0_P->Constant1_Value_o[0]; - rtb_DataTypeConversion5_idx_0 = sim_model_lib0_P->Constant1_Value_o[1]; - } else { - rtb_Sum = sim_model_lib0_P->epson_Q_B2accel_error[0]; - rtb_DataTypeConversion5_idx_0 = sim_model_lib0_P->epson_Q_B2accel_error[1]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant8' - // DataTypeConversion: '/Data Type Conversion1' - - rtb_MatrixConcatenate[13] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[1]; - rtb_MatrixConcatenate[14] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[2]; - - // Product: '/Product' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant13' - // Switch: '/Switch3' - - if (tmp_0) { - rtb_DataTypeConversion5_idx_1 = sim_model_lib0_P->Constant1_Value_o[2]; - rtb_DataTypeConversion5_idx_2 = sim_model_lib0_P->Constant1_Value_o[3]; - } else { - rtb_DataTypeConversion5_idx_1 = sim_model_lib0_P->epson_Q_B2accel_error[2]; - rtb_DataTypeConversion5_idx_2 = sim_model_lib0_P->epson_Q_B2accel_error[3]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant8' - // DataTypeConversion: '/Data Type Conversion1' - - rtb_MatrixConcatenate[15] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[3]; - - // Product: '/Product' - for (i_0 = 0; i_0 < 4; i_0++) { - rtb_linearaccelbias = rtb_MatrixConcatenate[(int32_T)(i_0 + 12)] * - rtb_DataTypeConversion5_idx_2 + (rtb_MatrixConcatenate[(int32_T)(i_0 + 8)] - * rtb_DataTypeConversion5_idx_1 + (rtb_MatrixConcatenate[(int32_T)(i_0 + 4)] - * rtb_DataTypeConversion5_idx_0 + rtb_MatrixConcatenate[i_0] * rtb_Sum)); - rtb_Product_l[i_0] = rtb_linearaccelbias; - } - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum = rtb_Product_l[3] * rtb_Product_l[3] * sim_model_lib0_P->Gain_Gain_f - - sim_model_lib0_P->Constant1_Value_c; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - - memcpy(&rtb_Assignment[0], &sim_model_lib0_P->Constant2_Value_m[0], (uint32_T) - (9U * sizeof(real_T))); - rtb_Assignment[0] = rtb_Sum; - rtb_Assignment[4] = rtb_Sum; - rtb_Assignment[8] = rtb_Sum; - - // Gain: '/Gain1' - rtb_Sum = sim_model_lib0_P->Gain1_Gain_d * rtb_Product_l[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Assignment_3[0] = sim_model_lib0_P->Constant3_Value_n; - rtb_Assignment_3[1] = rtb_Product_l[2]; - rtb_Assignment_3[2] = sim_model_lib0_P->Gain_Gain_fd * rtb_Product_l[1]; - rtb_Assignment_3[3] = sim_model_lib0_P->Gain1_Gain_l * rtb_Product_l[2]; - rtb_Assignment_3[4] = sim_model_lib0_P->Constant3_Value_n; - rtb_Assignment_3[5] = rtb_Product_l[0]; - rtb_Assignment_3[6] = rtb_Product_l[1]; - rtb_Assignment_3[7] = sim_model_lib0_P->Gain2_Gain_a * rtb_Product_l[0]; - rtb_Assignment_3[8] = sim_model_lib0_P->Constant3_Value_n; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Product_p_0[i_0] = rtb_Product_l[i_0] * rtb_Product_l[0]; - rtb_Product_p_0[(int32_T)(i_0 + 3)] = rtb_Product_l[i_0] * rtb_Product_l[1]; - rtb_Product_p_0[(int32_T)(i_0 + 6)] = rtb_Product_l[i_0] * rtb_Product_l[2]; - } - - // End of Product: '/Product1' - - // DotProduct: '/Dot Product' - rtb_Sqrt_m = 0.0F; - for (i_0 = 0; i_0 < 3; i_0++) { - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - - rtb_Assignment_jy[(int32_T)(3 * i_0)] = (rtb_Assignment[(int32_T)(3 * i_0)] - - rtb_Assignment_3[(int32_T)(3 * i_0)] * rtb_Sum) + rtb_Product_p_0 - [(int32_T)(3 * i_0)] * sim_model_lib0_P->Gain2_Gain_d; - rtb_Assignment_jy[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 1)] - rtb_Assignment_3[(int32_T)((int32_T) - (3 * i_0) + 1)] * rtb_Sum) + rtb_Product_p_0[(int32_T)((int32_T)(3 * i_0) - + 1)] * sim_model_lib0_P->Gain2_Gain_d; - rtb_Assignment_jy[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 2)] - rtb_Assignment_3[(int32_T)((int32_T) - (3 * i_0) + 2)] * rtb_Sum) + rtb_Product_p_0[(int32_T)((int32_T)(3 * i_0) - + 2)] * sim_model_lib0_P->Gain2_Gain_d; - - // DotProduct: '/Dot Product' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_Sqrt_m += sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[i_0] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[i_0]; - } - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Sqrt_m = (real32_T)sqrt((real_T)rtb_Sqrt_m); - - // If: '/If' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - - if (rtb_Sqrt_m == sim_model_lib0_P->CompareToConstant1_const) { - // Outputs for IfAction SubSystem: '/Stopped' incorporates: - // ActionPort: '/Action Port' - - // SignalConversion: '/OutportBufferForTheta' incorporates: - // Constant: '/Constant' - - memcpy(&rtb_VectorConcatenate_d[0], &sim_model_lib0_P->Constant_Value_c[0], - (uint32_T)(sizeof(real32_T) << 4U)); - - // End of Outputs for SubSystem: '/Stopped' - } else if (!(rtb_Sqrt_m <= sim_model_lib0_P->CompareToConstant_const)) { - // Outputs for IfAction SubSystem: '/slow_speed_approx' incorporates: - // ActionPort: '/Action Port' - - // DataTypeConversion: '/Data Type Conversion' incorporates: - // Gain: '/Gain' - // SampleTimeMath: '/Weighted Sample Time Math' - // - // About '/Weighted Sample Time Math': - // y = K where K = ( w * Ts ) - - rtb_Sqrt_m = (real32_T)(sim_model_lib0_P->Gain_Gain * - sim_model_lib0_P->WeightedSampleTimeMath_WtEt); - rtb_VectorConcatenate_d[0] = sim_model_lib0_P->Constant3_Value_g2; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant3' - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[1] = sim_model_lib0_P->Gain_Gain_dy * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - - // Gain: '/Gain1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[3] = sim_model_lib0_P->Gain1_Gain_ow * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[4] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - rtb_VectorConcatenate_d[5] = sim_model_lib0_P->Constant2_Value_l; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant2' - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[6] = sim_model_lib0_P->Gain2_Gain_oe * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - - // Gain: '/Gain3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[7] = sim_model_lib0_P->Gain3_Gain * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - - // Gain: '/Gain4' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[8] = sim_model_lib0_P->Gain4_Gain * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[9] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - rtb_VectorConcatenate_d[10] = sim_model_lib0_P->Constant1_Value_p4; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[11] = sim_model_lib0_P->Gain5_Gain * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[12] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[13] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[14] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - rtb_VectorConcatenate_d[15] = sim_model_lib0_P->Constant_Value_d; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Product: '/Product' - - for (i_0 = 0; i_0 < 16; i_0++) { - rtb_VectorConcatenate_d[i_0] = rtb_Sqrt_m * rtb_VectorConcatenate_d[i_0] + - sim_model_lib0_P->Constant_Value_p[i_0]; - } - - // End of Sum: '/Sum' - // End of Outputs for SubSystem: '/slow_speed_approx' - } else { - // Outputs for IfAction SubSystem: '/normal_speed' incorporates: - // ActionPort: '/Action Port' - - // Product: '/Product' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Gain: '/Gain' - // SampleTimeMath: '/Weighted Sample Time Math' - // - // About '/Weighted Sample Time Math': - // y = K where K = ( w * Ts ) - - rtb_Product_p = (real32_T)(sim_model_lib0_P->Gain_Gain_j * - sim_model_lib0_P->WeightedSampleTimeMath_WtEt_o) * rtb_Sqrt_m; - - // Trigonometry: '/Trigonometric Function1' - rtb_TrigonometricFunction1_c = (real32_T)cos((real_T)rtb_Product_p); - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 16; i++) { - rtb_Assignment_ok[i] = (real32_T)sim_model_lib0_P->Constant2_Value[i]; - } - - rtb_Assignment_ok[0] = rtb_TrigonometricFunction1_c; - rtb_Assignment_ok[5] = rtb_TrigonometricFunction1_c; - rtb_Assignment_ok[10] = rtb_TrigonometricFunction1_c; - rtb_Assignment_ok[15] = rtb_TrigonometricFunction1_c; - - // End of Assignment: '/Assignment' - - // Trigonometry: '/Trigonometric Function' - rtb_Product_p = (real32_T)sin((real_T)rtb_Product_p); - rtb_VectorConcatenate_d[0] = sim_model_lib0_P->Constant3_Value_pa; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant3' - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[1] = sim_model_lib0_P->Gain_Gain_g * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - - // Gain: '/Gain1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[3] = sim_model_lib0_P->Gain1_Gain_c * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[4] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - rtb_VectorConcatenate_d[5] = sim_model_lib0_P->Constant2_Value_dl; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant2' - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[6] = sim_model_lib0_P->Gain2_Gain_l * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - - // Gain: '/Gain3' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[7] = sim_model_lib0_P->Gain3_Gain_i * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - - // Gain: '/Gain4' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[8] = sim_model_lib0_P->Gain4_Gain_f * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[9] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - rtb_VectorConcatenate_d[10] = sim_model_lib0_P->Constant1_Value_of; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[11] = sim_model_lib0_P->Gain5_Gain_l * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[12] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[13] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_VectorConcatenate_d[14] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - rtb_VectorConcatenate_d[15] = sim_model_lib0_P->Constant_Value_f; - - // Product: '/Divide' incorporates: - // Constant: '/Constant' - - rtb_Sqrt_m = 1.0F / rtb_Sqrt_m * rtb_Product_p; - - // Sum: '/Sum' incorporates: - // Product: '/Divide' - - for (i_0 = 0; i_0 < 16; i_0++) { - rtb_VectorConcatenate_d[i_0] = rtb_Sqrt_m * rtb_VectorConcatenate_d[i_0] + - rtb_Assignment_ok[i_0]; - } - - // End of Sum: '/Sum' - // End of Outputs for SubSystem: '/normal_speed' - } - - // End of If: '/If' - - // Delay: '/Delay' incorporates: - // Constant: '/Constant1' - - if ((int32_T)sim_model_lib0_DW->icLoad != 0) { - sim_model_lib0_DW->Delay_DSTATE[0] = sim_model_lib0_P->tun_ini_Q_ISS2B[0]; - sim_model_lib0_DW->Delay_DSTATE[1] = sim_model_lib0_P->tun_ini_Q_ISS2B[1]; - sim_model_lib0_DW->Delay_DSTATE[2] = sim_model_lib0_P->tun_ini_Q_ISS2B[2]; - sim_model_lib0_DW->Delay_DSTATE[3] = sim_model_lib0_P->tun_ini_Q_ISS2B[3]; - } - - // DotProduct: '/Dot Product' - rtb_Sqrt_m = 0.0F; - for (i_0 = 0; i_0 < 4; i_0++) { - // Product: '/Product' incorporates: - // Delay: '/Delay' - - rtb_TrigonometricFunction1_c = rtb_VectorConcatenate_d[(int32_T)(i_0 + 12)] * - sim_model_lib0_DW->Delay_DSTATE[3] + (rtb_VectorConcatenate_d[(int32_T) - (i_0 + 8)] * sim_model_lib0_DW->Delay_DSTATE[2] + - (rtb_VectorConcatenate_d[(int32_T)(i_0 + 4)] * - sim_model_lib0_DW->Delay_DSTATE[1] + rtb_VectorConcatenate_d[i_0] * - sim_model_lib0_DW->Delay_DSTATE[0])); - - // DotProduct: '/Dot Product' - rtb_Sqrt_m += rtb_TrigonometricFunction1_c * rtb_TrigonometricFunction1_c; - - // Product: '/Product' - rtb_Product_e[i_0] = rtb_TrigonometricFunction1_c; - } - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Sqrt_m = (real32_T)sqrt((real_T)rtb_Sqrt_m); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Sqrt_m > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - sim_model_lib0_Normalize(rtb_Product_e, rtb_Sqrt_m, rtb_Merge_kq); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge_kq[0] = rtb_Product_e[0]; - rtb_Merge_kq[1] = rtb_Product_e[1]; - rtb_Merge_kq[2] = rtb_Product_e[2]; - rtb_Merge_kq[3] = rtb_Product_e[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Merge_kq[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - // Product: '/Product' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - - rtb_Product_e[0] = rtb_Merge_kq[0] * (real32_T) - sim_model_lib0_P->Constant1_Value; - rtb_Product_e[1] = rtb_Merge_kq[1] * (real32_T) - sim_model_lib0_P->Constant1_Value; - rtb_Product_e[2] = rtb_Merge_kq[2] * (real32_T) - sim_model_lib0_P->Constant1_Value; - rtb_Product_e[3] = rtb_Merge_kq[3] * (real32_T) - sim_model_lib0_P->Constant1_Value; - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product_e[0] = rtb_Merge_kq[0]; - rtb_Product_e[1] = rtb_Merge_kq[1]; - rtb_Product_e[2] = rtb_Merge_kq[2]; - rtb_Product_e[3] = rtb_Merge_kq[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Sqrt_m = (real32_T)sqrt((real_T)(((rtb_Product_e[0] * rtb_Product_e[0] + - rtb_Product_e[1] * rtb_Product_e[1]) + rtb_Product_e[2] * rtb_Product_e[2]) - + rtb_Product_e[3] * rtb_Product_e[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Sqrt_m > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - sim_model_lib0_Normalize(rtb_Product_e, rtb_Sqrt_m, rtb_Merge_m); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge_m[0] = rtb_Product_e[0]; - rtb_Merge_m[1] = rtb_Product_e[1]; - rtb_Merge_m[2] = rtb_Product_e[2]; - rtb_Merge_m[3] = rtb_Product_e[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sqrt_m = rtb_Merge_m[3] * rtb_Merge_m[3] * sim_model_lib0_P->Gain_Gain_m - - (real32_T)sim_model_lib0_P->Constant1_Value_oo; - for (i = 0; i < 9; i++) { - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment_m[i] = (real32_T)sim_model_lib0_P->Constant2_Value_m1[i]; - - // Outport: '/cmc_msg' incorporates: - // Inport: '/cmc_msg_in' - // SignalConversion: '/Signal Conversion' - - sim_model_lib0_Y_cmc_msg_c->inertia_matrix[i] = - sim_model_lib0_U_cmc_msg_in->inertia_matrix[i]; - } - - // Assignment: '/Assignment' - rtb_Assignment_m[0] = rtb_Sqrt_m; - - // Delay: '/Delay1' - rtb_Delay1_bpm_torque_B[0] = sim_model_lib0_DW->Delay1_DSTATE.bpm_torque_B[0]; - rtb_TrigonometricFunction1_c = sim_model_lib0_DW->Delay1_DSTATE.bpm_force_B[0]; - - // Outport: '/cmc_msg' incorporates: - // Constant: '/Constant1' - // Selector: '/select_current_command' - // SignalConversion: '/ConcatBufferAtVector Concatenate1In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate2In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate3In1' - // UnitDelay: '/Unit Delay' - - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.P_B_ISS_ISS[0] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i - 1)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.V_B_ISS_ISS[0] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 35)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.A_B_ISS_ISS[0] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 71)]; - - // Assignment: '/Assignment' - rtb_Assignment_m[4] = rtb_Sqrt_m; - - // Delay: '/Delay1' - rtb_Delay1_bpm_torque_B[1] = sim_model_lib0_DW->Delay1_DSTATE.bpm_torque_B[1]; - rtb_Delay1_bpm_force_B_idx_1 = sim_model_lib0_DW->Delay1_DSTATE.bpm_force_B[1]; - - // Outport: '/cmc_msg' incorporates: - // Constant: '/Constant1' - // Selector: '/select_current_command' - // SignalConversion: '/ConcatBufferAtVector Concatenate1In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate2In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate3In2' - // UnitDelay: '/Unit Delay' - - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.P_B_ISS_ISS[1] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 11)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.V_B_ISS_ISS[1] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 47)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.A_B_ISS_ISS[1] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 83)]; - - // Assignment: '/Assignment' - rtb_Assignment_m[8] = rtb_Sqrt_m; - - // Delay: '/Delay1' - rtb_Delay1_bpm_torque_B[2] = sim_model_lib0_DW->Delay1_DSTATE.bpm_torque_B[2]; - rtb_Delay1_bpm_force_B_idx_2 = sim_model_lib0_DW->Delay1_DSTATE.bpm_force_B[2]; - - // Outport: '/cmc_msg' incorporates: - // Constant: '/Constant1' - // Selector: '/select_current_command' - // SignalConversion: '/ConcatBufferAtVector Concatenate1In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate2In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate3In3' - // UnitDelay: '/Unit Delay' - - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.P_B_ISS_ISS[2] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 23)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.V_B_ISS_ISS[2] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 59)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.A_B_ISS_ISS[2] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 95)]; - - // Gain: '/Gain1' - rtb_Sqrt_m = sim_model_lib0_P->Gain1_Gain_oi * rtb_Merge_m[3]; - - // Outport: '/cmc_msg' incorporates: - // BusCreator: '/BusConversion_InsertedFor_Bus Assignment_at_inport_1' - // BusCreator: '/BusConversion_InsertedFor_Bus Assignment_at_inport_2' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant4' - // Constant: '/Constant6' - // Constant: '/Constant8' - // Inport: '/cmc_msg_in' - // Selector: '/select_current_command' - // Selector: '/select_current_command1' - // Selector: '/select_current_command2' - // Selector: '/select_current_command' - // Selector: '/select_current_time' - // SignalConversion: '/Signal Conversion' - // SignalConversion: '/ConcatBufferAtVector Concatenate10In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate10In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate10In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate10In4' - // SignalConversion: '/ConcatBufferAtVector Concatenate11In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate11In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate11In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate12In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate12In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate12In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate4In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate4In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate4In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate4In4' - // SignalConversion: '/ConcatBufferAtVector Concatenate5In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate5In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate5In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate6In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate6In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate6In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate7In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate7In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate7In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate8In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate8In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate8In3' - // SignalConversion: '/ConcatBufferAtVector Concatenate9In1' - // SignalConversion: '/ConcatBufferAtVector Concatenate9In2' - // SignalConversion: '/ConcatBufferAtVector Concatenate9In3' - // UnitDelay: '/Unit Delay' - // UnitDelay: '/Unit Delay' - // UnitDelay: '/Unit Delay' - // UnitDelay: '/Unit Delay' - - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.timestamp_sec = - sim_model_lib0_P->mlp_command_times[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i - 1)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.timestamp_nsec = - sim_model_lib0_P->mlp_command_times[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 11)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.quat_ISS2B[0] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 107)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.quat_ISS2B[1] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 119)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.quat_ISS2B[2] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 131)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.quat_ISS2B[3] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 143)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.timestamp_sec = - rtb_Switch1_p_idx_0; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.timestamp_nsec = - rtb_Switch1_p_idx_1; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.omega_B_ISS_B[0] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 155)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.alpha_B_ISS_B[0] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 191)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.P_B_ISS_ISS[0] = rtb_Switch_al[0]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.V_B_ISS_ISS[0] = rtb_Switch_al[3]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.A_B_ISS_ISS[0] = rtb_Switch_al[6]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.omega_B_ISS_B[1] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 167)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.alpha_B_ISS_B[1] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 203)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.P_B_ISS_ISS[1] = rtb_Switch_al[1]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.V_B_ISS_ISS[1] = rtb_Switch_al[4]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.A_B_ISS_ISS[1] = rtb_Switch_al[7]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.omega_B_ISS_B[2] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 179)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_a.alpha_B_ISS_B[2] = - sim_model_lib0_P->mlp_command_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_i + 215)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.P_B_ISS_ISS[2] = rtb_Switch_al[2]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.V_B_ISS_ISS[2] = rtb_Switch_al[5]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.A_B_ISS_ISS[2] = rtb_Switch_al[8]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.quat_ISS2B[0] = rtb_Switch_al[9]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.quat_ISS2B[1] = rtb_Switch_al[10]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.quat_ISS2B[2] = rtb_Switch_al[11]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.quat_ISS2B[3] = rtb_Switch_al[12]; - sim_model_lib0_Y_cmc_msg_c->cmc_mode_cmd = sim_model_lib0_P-> - mlp_mode_cmd_list[(int32_T)((int32_T)sim_model_lib0_DW->UnitDelay_DSTATE_if - - 1)]; - sim_model_lib0_Y_cmc_msg_c->speed_gain_cmd = - sim_model_lib0_P->mlp_speed_gain_cmd_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_h - 1)]; - sim_model_lib0_Y_cmc_msg_c->localization_mode_cmd = - sim_model_lib0_P->mlp_loc_mode_cmd_list[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_ht - 1)]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.omega_B_ISS_B[0] = rtb_Switch_al - [13]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.alpha_B_ISS_B[0] = rtb_Switch_al - [16]; - sim_model_lib0_Y_cmc_msg_c->att_kp[0] = sim_model_lib0_U_cmc_msg_in->att_kp[0]; - sim_model_lib0_Y_cmc_msg_c->att_ki[0] = sim_model_lib0_U_cmc_msg_in->att_ki[0]; - sim_model_lib0_Y_cmc_msg_c->omega_kd[0] = - sim_model_lib0_U_cmc_msg_in->omega_kd[0]; - sim_model_lib0_Y_cmc_msg_c->pos_kp[0] = sim_model_lib0_U_cmc_msg_in->pos_kp[0]; - sim_model_lib0_Y_cmc_msg_c->pos_ki[0] = sim_model_lib0_U_cmc_msg_in->pos_ki[0]; - sim_model_lib0_Y_cmc_msg_c->vel_kd[0] = sim_model_lib0_U_cmc_msg_in->vel_kd[0]; - sim_model_lib0_Y_cmc_msg_c->center_of_mass[0] = - sim_model_lib0_U_cmc_msg_in->center_of_mass[0]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.omega_B_ISS_B[1] = rtb_Switch_al - [14]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.alpha_B_ISS_B[1] = rtb_Switch_al - [17]; - sim_model_lib0_Y_cmc_msg_c->att_kp[1] = sim_model_lib0_U_cmc_msg_in->att_kp[1]; - sim_model_lib0_Y_cmc_msg_c->att_ki[1] = sim_model_lib0_U_cmc_msg_in->att_ki[1]; - sim_model_lib0_Y_cmc_msg_c->omega_kd[1] = - sim_model_lib0_U_cmc_msg_in->omega_kd[1]; - sim_model_lib0_Y_cmc_msg_c->pos_kp[1] = sim_model_lib0_U_cmc_msg_in->pos_kp[1]; - sim_model_lib0_Y_cmc_msg_c->pos_ki[1] = sim_model_lib0_U_cmc_msg_in->pos_ki[1]; - sim_model_lib0_Y_cmc_msg_c->vel_kd[1] = sim_model_lib0_U_cmc_msg_in->vel_kd[1]; - sim_model_lib0_Y_cmc_msg_c->center_of_mass[1] = - sim_model_lib0_U_cmc_msg_in->center_of_mass[1]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.omega_B_ISS_B[2] = rtb_Switch_al - [15]; - sim_model_lib0_Y_cmc_msg_c->cmc_state_cmd_b.alpha_B_ISS_B[2] = rtb_Switch_al - [18]; - sim_model_lib0_Y_cmc_msg_c->att_kp[2] = sim_model_lib0_U_cmc_msg_in->att_kp[2]; - sim_model_lib0_Y_cmc_msg_c->att_ki[2] = sim_model_lib0_U_cmc_msg_in->att_ki[2]; - sim_model_lib0_Y_cmc_msg_c->omega_kd[2] = - sim_model_lib0_U_cmc_msg_in->omega_kd[2]; - sim_model_lib0_Y_cmc_msg_c->pos_kp[2] = sim_model_lib0_U_cmc_msg_in->pos_kp[2]; - sim_model_lib0_Y_cmc_msg_c->pos_ki[2] = sim_model_lib0_U_cmc_msg_in->pos_ki[2]; - sim_model_lib0_Y_cmc_msg_c->vel_kd[2] = sim_model_lib0_U_cmc_msg_in->vel_kd[2]; - sim_model_lib0_Y_cmc_msg_c->center_of_mass[2] = - sim_model_lib0_U_cmc_msg_in->center_of_mass[2]; - sim_model_lib0_Y_cmc_msg_c->mass = sim_model_lib0_U_cmc_msg_in->mass; - - // Sqrt: '/Sqrt' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DotProduct: '/Dot Product' - - rtb_Product_p = (real32_T)sqrt((real_T) - ((sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] + - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1]) + - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Inport: '/In1' - - if ((real_T)rtb_Product_p > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - sim_model_lib0_Normalize_e - (sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d, rtb_Product_p, - rtb_Merge_o); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge_o[0] = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0]; - rtb_Merge_o[1] = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1]; - rtb_Merge_o[2] = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant6' - // Constant: '/Constant7' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain' - // Product: '/Divide' - // Sum: '/Sum' - - if ((int32_T)sim_model_lib0_P->tun_env_drag_disturb_on != 0) { - // Switch: '/Switch' incorporates: - // Constant: '/Constant9' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DotProduct: '/Dot Product' - // Product: '/Product' - // RelationalOperator: '/Relational Operator' - // Sqrt: '/Sqrt' - - if ((real32_T)sqrt((real_T) - ((sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] + - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1]) - + sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2])) < - sim_model_lib0_P->env_max_ext_air_vel) { - rtb_Merge_o[0] = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0]; - rtb_Merge_o[1] = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1]; - rtb_Merge_o[2] = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2]; - } else { - rtb_Merge_o[0] *= sim_model_lib0_P->env_max_ext_air_vel; - rtb_Merge_o[1] *= sim_model_lib0_P->env_max_ext_air_vel; - rtb_Merge_o[2] *= sim_model_lib0_P->env_max_ext_air_vel; - } - - // End of Switch: '/Switch' - - // Sum: '/Sum' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_Merge_c = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0] - - rtb_Merge_o[0]; - - // Product: '/Divide' - if (rtb_Merge_c < 0.0F) { - rtb_Product_p = -1.0F; - } else if (rtb_Merge_c > 0.0F) { - rtb_Product_p = 1.0F; - } else if (rtb_Merge_c == 0.0F) { - rtb_Product_p = 0.0F; - } else { - rtb_Product_p = rtb_Merge_c; - } - - rtb_Merge_c = sim_model_lib0_P->Gain_Gain_go * rtb_Product_p * rtb_Merge_c * - rtb_Merge_c * sim_model_lib0_P->env_avg_drag_coeff; - rtb_Merge_o[0] = rtb_Merge_c; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain' - // Product: '/Divide' - - rtb_Merge_c = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1] - - rtb_Merge_o[1]; - - // Product: '/Divide' - if (rtb_Merge_c < 0.0F) { - rtb_Product_p = -1.0F; - } else if (rtb_Merge_c > 0.0F) { - rtb_Product_p = 1.0F; - } else if (rtb_Merge_c == 0.0F) { - rtb_Product_p = 0.0F; - } else { - rtb_Product_p = rtb_Merge_c; - } - - rtb_Merge_c = sim_model_lib0_P->Gain_Gain_go * rtb_Product_p * rtb_Merge_c * - rtb_Merge_c * sim_model_lib0_P->env_avg_drag_coeff; - rtb_Merge_o[1] = rtb_Merge_c; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain' - // Product: '/Divide' - - rtb_Merge_c = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2] - - rtb_Merge_o[2]; - - // Product: '/Divide' - if (rtb_Merge_c < 0.0F) { - rtb_Product_p = -1.0F; - } else if (rtb_Merge_c > 0.0F) { - rtb_Product_p = 1.0F; - } else if (rtb_Merge_c == 0.0F) { - rtb_Product_p = 0.0F; - } else { - rtb_Product_p = rtb_Merge_c; - } - - rtb_Merge_c = sim_model_lib0_P->Gain_Gain_go * rtb_Product_p * rtb_Merge_c * - rtb_Merge_c * sim_model_lib0_P->env_avg_drag_coeff; - rtb_Merge_o[2] = rtb_Merge_c; - } else { - rtb_Merge_o[0] = sim_model_lib0_P->Constant7_Value[0]; - rtb_Merge_o[1] = sim_model_lib0_P->Constant7_Value[1]; - rtb_Merge_o[2] = sim_model_lib0_P->Constant7_Value[2]; - } - - // End of Switch: '/Switch1' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant' - // Inport: '/cmc_msg_in' - // SignalConversion: '/Signal Conversion' - - rtb_Product_p = sim_model_lib0_P->tun_mass_error + - sim_model_lib0_U_cmc_msg_in->mass; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_1[0] = (real32_T)sim_model_lib0_P->Constant3_Value_k; - tmp_1[1] = rtb_Merge_m[2]; - tmp_1[2] = sim_model_lib0_P->Gain_Gain_j1 * rtb_Merge_m[1]; - tmp_1[3] = sim_model_lib0_P->Gain1_Gain_e * rtb_Merge_m[2]; - tmp_1[4] = (real32_T)sim_model_lib0_P->Constant3_Value_k; - tmp_1[5] = rtb_Merge_m[0]; - tmp_1[6] = rtb_Merge_m[1]; - tmp_1[7] = sim_model_lib0_P->Gain2_Gain_p * rtb_Merge_m[0]; - tmp_1[8] = (real32_T)sim_model_lib0_P->Constant3_Value_k; - - // Math: '/Math Function' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Merge_b[i_0] = rtb_Merge_m[0] * rtb_Merge_m[i_0]; - rtb_Merge_b[(int32_T)(i_0 + 3)] = rtb_Merge_m[1] * rtb_Merge_m[i_0]; - rtb_Merge_b[(int32_T)(i_0 + 6)] = rtb_Merge_m[2] * rtb_Merge_m[i_0]; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_i[(int32_T)(3 * i_0)] = (rtb_Assignment_m[i_0] - rtb_Sqrt_m * - tmp_1[i_0]) + rtb_Merge_b[(int32_T)(3 * i_0)] * - sim_model_lib0_P->Gain2_Gain_h; - rtb_Assignment_i[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment_m - [(int32_T)(i_0 + 3)] - tmp_1[(int32_T)(i_0 + 3)] * rtb_Sqrt_m) + - rtb_Merge_b[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_h; - rtb_Assignment_i[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment_m - [(int32_T)(i_0 + 6)] - tmp_1[(int32_T)(i_0 + 6)] * rtb_Sqrt_m) + - rtb_Merge_b[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_h; - } - - // End of Sum: '/Sum1' - - // Product: '/Element product' incorporates: - // Constant: '/Constant1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - - rtb_Elementproduct_c[0] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[1] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2]; - rtb_Elementproduct_c[1] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[2] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0]; - rtb_Elementproduct_c[2] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[0] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1]; - rtb_Elementproduct_c[3] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[2] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1]; - rtb_Elementproduct_c[4] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[0] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2]; - rtb_Elementproduct_c[5] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[1] * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0]; - for (i = 0; i < 3; i++) { - // Product: '/Product1' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant3' - // Constant: '/Constant5' - // Product: '/Product' - // Product: '/Divide1' - // Sum: '/Sum1' - // Sum: '/Add1' - // Switch: '/Switch' - - if (sim_model_lib0_P->env_grav_disturb_on) { - rtb_Sqrt_m = rtb_Product_p * sim_model_lib0_P->const_gravity_local[i]; - } else { - rtb_Sqrt_m = sim_model_lib0_P->Constant5_Value_av[i]; - } - - rtb_bpm_force_B[i] = (((rtb_Assignment_i[(int32_T)(i + 3)] * - rtb_Delay1_bpm_force_B_idx_1 + rtb_Assignment_i[i] * - rtb_TrigonometricFunction1_c) + rtb_Assignment_i[(int32_T)(i + 6)] * - rtb_Delay1_bpm_force_B_idx_2) + (rtb_Merge_o[i] + rtb_Sqrt_m)) / - rtb_Product_p; - - // End of Product: '/Product1' - - // Product: '/Product3' incorporates: - // Constant: '/Constant10' - // Sum: '/Add3' - - rtb_P_B_ISS_SS[i] = (rtb_Elementproduct_c[i] - rtb_Elementproduct_c[(int32_T) - (i + 3)]) * sim_model_lib0_P->Constant10_Value_a; - } - - // Sum: '/Add3' incorporates: - // Constant: '/Constant1' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Product: '/Element product' - - rtb_Merge_o[0] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[1] * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2] - - sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[2] * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1]; - rtb_Merge_o[1] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[2] * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0] - - sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[0] * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2]; - rtb_Merge_o[2] = sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[0] * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1] - - sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[1] * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0]; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant1' - // Product: '/Element product' - // Sum: '/Add3' - // Sum: '/Add' - - rtb_P_B_ISS_SS[0] = ((rtb_bpm_force_B[0] - rtb_P_B_ISS_SS[0]) - - (sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[1] * - rtb_Merge_o[2] - - sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[2] * - rtb_Merge_o[1])) * - sim_model_lib0_P->tun_env_accel_dof_gain[0]; - rtb_P_B_ISS_SS[1] = ((rtb_bpm_force_B[1] - rtb_P_B_ISS_SS[1]) - - (sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[2] * - rtb_Merge_o[0] - - sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[0] * - rtb_Merge_o[2])) * - sim_model_lib0_P->tun_env_accel_dof_gain[1]; - rtb_Merge_c = ((rtb_bpm_force_B[2] - rtb_P_B_ISS_SS[2]) - - (sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[0] * rtb_Merge_o[1] - - sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[1] * - rtb_Merge_o[0])) * sim_model_lib0_P->tun_env_accel_dof_gain[2]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sqrt_m = rtb_Merge_m[3] * rtb_Merge_m[3] * sim_model_lib0_P->Gain_Gain_b - - (real32_T)sim_model_lib0_P->Constant1_Value_i; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_m[i] = (real32_T)sim_model_lib0_P->Constant2_Value_d[i]; - } - - rtb_Assignment_m[0] = rtb_Sqrt_m; - rtb_Assignment_m[4] = rtb_Sqrt_m; - rtb_Assignment_m[8] = rtb_Sqrt_m; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Product_p = sim_model_lib0_P->Gain1_Gain_m * rtb_Merge_m[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_2[0] = (real32_T)sim_model_lib0_P->Constant3_Value_a; - tmp_2[1] = rtb_Merge_m[2]; - tmp_2[2] = sim_model_lib0_P->Gain_Gain_la * rtb_Merge_m[1]; - tmp_2[3] = sim_model_lib0_P->Gain1_Gain_hb * rtb_Merge_m[2]; - tmp_2[4] = (real32_T)sim_model_lib0_P->Constant3_Value_a; - tmp_2[5] = rtb_Merge_m[0]; - tmp_2[6] = rtb_Merge_m[1]; - tmp_2[7] = sim_model_lib0_P->Gain2_Gain_gu * rtb_Merge_m[0]; - tmp_2[8] = (real32_T)sim_model_lib0_P->Constant3_Value_a; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Merge_b[i_0] = rtb_Merge_m[i_0] * rtb_Merge_m[0]; - rtb_Merge_b[(int32_T)(i_0 + 3)] = rtb_Merge_m[i_0] * rtb_Merge_m[1]; - rtb_Merge_b[(int32_T)(i_0 + 6)] = rtb_Merge_m[i_0] * rtb_Merge_m[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_i[(int32_T)(3 * i_0)] = (rtb_Assignment_m[(int32_T)(3 * i_0)] - - tmp_2[(int32_T)(3 * i_0)] * rtb_Product_p) + rtb_Merge_b[(int32_T)(3 * - i_0)] * sim_model_lib0_P->Gain2_Gain_lk; - rtb_Assignment_i[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment_m - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_2[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Product_p) + rtb_Merge_b[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_lk; - rtb_Assignment_i[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment_m - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_2[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Product_p) + rtb_Merge_b[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_lk; - } - - // End of Sum: '/Sum1' - - // Product: '/Product' - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Merge_o[i_0] = rtb_Assignment_i[(int32_T)(i_0 + 6)] * rtb_Merge_c + - (rtb_Assignment_i[(int32_T)(i_0 + 3)] * rtb_P_B_ISS_SS[1] + - rtb_Assignment_i[i_0] * rtb_P_B_ISS_SS[0]); - } - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sqrt_m = rtb_Merge_m[3] * rtb_Merge_m[3] * sim_model_lib0_P->Gain_Gain_h - - (real32_T)sim_model_lib0_P->Constant1_Value_c4; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_m[i] = (real32_T)sim_model_lib0_P->Constant2_Value_p[i]; - } - - rtb_Assignment_m[0] = rtb_Sqrt_m; - rtb_Assignment_m[4] = rtb_Sqrt_m; - rtb_Assignment_m[8] = rtb_Sqrt_m; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Product_p = sim_model_lib0_P->Gain1_Gain_al * rtb_Merge_m[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_3[0] = (real32_T)sim_model_lib0_P->Constant3_Value_b; - tmp_3[1] = rtb_Merge_m[2]; - tmp_3[2] = sim_model_lib0_P->Gain_Gain_kk * rtb_Merge_m[1]; - tmp_3[3] = sim_model_lib0_P->Gain1_Gain_aj * rtb_Merge_m[2]; - tmp_3[4] = (real32_T)sim_model_lib0_P->Constant3_Value_b; - tmp_3[5] = rtb_Merge_m[0]; - tmp_3[6] = rtb_Merge_m[1]; - tmp_3[7] = sim_model_lib0_P->Gain2_Gain_m * rtb_Merge_m[0]; - tmp_3[8] = (real32_T)sim_model_lib0_P->Constant3_Value_b; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Merge_b[i_0] = rtb_Merge_m[i_0] * rtb_Merge_m[0]; - rtb_Merge_b[(int32_T)(i_0 + 3)] = rtb_Merge_m[i_0] * rtb_Merge_m[1]; - rtb_Merge_b[(int32_T)(i_0 + 6)] = rtb_Merge_m[i_0] * rtb_Merge_m[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_i[(int32_T)(3 * i_0)] = (rtb_Assignment_m[(int32_T)(3 * i_0)] - - tmp_3[(int32_T)(3 * i_0)] * rtb_Product_p) + rtb_Merge_b[(int32_T)(3 * - i_0)] * sim_model_lib0_P->Gain2_Gain_h4; - rtb_Assignment_i[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment_m - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_3[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Product_p) + rtb_Merge_b[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_h4; - rtb_Assignment_i[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment_m - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_3[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Product_p) + rtb_Merge_b[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_h4; - } - - // End of Sum: '/Sum1' - - // Product: '/Product' - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_imu_gyro_bias[i_0] = rtb_Assignment_i[(int32_T)(i_0 + 6)] * - rtb_bpm_force_B[2] + (rtb_Assignment_i[(int32_T)(i_0 + 3)] * - rtb_bpm_force_B[1] + rtb_Assignment_i[i_0] * rtb_bpm_force_B[0]); - } - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sqrt_m = rtb_Merge_m[3] * rtb_Merge_m[3] * sim_model_lib0_P->Gain_Gain_c - - (real32_T)sim_model_lib0_P->Constant1_Value_o5; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_m[i] = (real32_T)sim_model_lib0_P->Constant2_Value_c[i]; - } - - rtb_Assignment_m[0] = rtb_Sqrt_m; - rtb_Assignment_m[4] = rtb_Sqrt_m; - rtb_Assignment_m[8] = rtb_Sqrt_m; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Product_p = sim_model_lib0_P->Gain1_Gain_de * rtb_Merge_m[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_4[0] = (real32_T)sim_model_lib0_P->Constant3_Value_j; - tmp_4[1] = rtb_Merge_m[2]; - tmp_4[2] = sim_model_lib0_P->Gain_Gain_g5 * rtb_Merge_m[1]; - tmp_4[3] = sim_model_lib0_P->Gain1_Gain_k * rtb_Merge_m[2]; - tmp_4[4] = (real32_T)sim_model_lib0_P->Constant3_Value_j; - tmp_4[5] = rtb_Merge_m[0]; - tmp_4[6] = rtb_Merge_m[1]; - tmp_4[7] = sim_model_lib0_P->Gain2_Gain_ku * rtb_Merge_m[0]; - tmp_4[8] = (real32_T)sim_model_lib0_P->Constant3_Value_j; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Merge_b[i_0] = rtb_Merge_m[i_0] * rtb_Merge_m[0]; - rtb_Merge_b[(int32_T)(i_0 + 3)] = rtb_Merge_m[i_0] * rtb_Merge_m[1]; - rtb_Merge_b[(int32_T)(i_0 + 6)] = rtb_Merge_m[i_0] * rtb_Merge_m[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_i[(int32_T)(3 * i_0)] = (rtb_Assignment_m[(int32_T)(3 * i_0)] - - tmp_4[(int32_T)(3 * i_0)] * rtb_Product_p) + rtb_Merge_b[(int32_T)(3 * - i_0)] * sim_model_lib0_P->Gain2_Gain_ax; - rtb_Assignment_i[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment_m - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_4[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Product_p) + rtb_Merge_b[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_ax; - rtb_Assignment_i[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment_m - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_4[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Product_p) + rtb_Merge_b[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_ax; - } - - // End of Sum: '/Sum1' - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant4' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_bpm_force_B[i_0] = ((rtb_Assignment_i[(int32_T)(i_0 + 3)] * - sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[1] + rtb_Assignment_i[i_0] * - sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[0]) + rtb_Assignment_i - [(int32_T)(i_0 + 6)] * sim_model_lib0_P->tun_iss_omega_ISS_ECI_ISS[2]) + - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[i_0]; - } - - // End of Sum: '/Sum1' - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant1' - // Inport: '/cmc_msg_in' - // SignalConversion: '/Signal Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_m[i] = sim_model_lib0_U_cmc_msg_in->inertia_matrix[i] + - sim_model_lib0_P->tun_inertia_error_mat[i]; - } - - // End of Sum: '/Sum1' - - // Switch: '/Switch2' incorporates: - // Constant: '/Constant10' - // Constant: '/Constant11' - // Constant: '/Constant8' - // Gain: '/Gain2' - // Product: '/Divide3' - // Sum: '/Sum2' - - if ((int32_T)sim_model_lib0_P->tun_env_drag_disturb_on != 0) { - // Sum: '/Sum2' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - rtb_Sqrt_m = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0] - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[0]; - - // Product: '/Divide3' - if (rtb_Sqrt_m < 0.0F) { - rtb_Product_p = -1.0F; - } else if (rtb_Sqrt_m > 0.0F) { - rtb_Product_p = 1.0F; - } else if (rtb_Sqrt_m == 0.0F) { - rtb_Product_p = 0.0F; - } else { - rtb_Product_p = rtb_Sqrt_m; - } - - rtb_Sqrt_m = (real32_T)((real_T)(sim_model_lib0_P->Gain2_Gain_f * - rtb_Product_p * rtb_Sqrt_m * rtb_Sqrt_m) * - sim_model_lib0_P->env_rotational_drag_coeff); - rtb_Sum2_f[0] = rtb_Sqrt_m; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant11' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain2' - // Product: '/Divide3' - - rtb_Sqrt_m = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1] - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[1]; - - // Product: '/Divide3' - if (rtb_Sqrt_m < 0.0F) { - rtb_Product_p = -1.0F; - } else if (rtb_Sqrt_m > 0.0F) { - rtb_Product_p = 1.0F; - } else if (rtb_Sqrt_m == 0.0F) { - rtb_Product_p = 0.0F; - } else { - rtb_Product_p = rtb_Sqrt_m; - } - - rtb_Sqrt_m = (real32_T)((real_T)(sim_model_lib0_P->Gain2_Gain_f * - rtb_Product_p * rtb_Sqrt_m * rtb_Sqrt_m) * - sim_model_lib0_P->env_rotational_drag_coeff); - rtb_Sum2_f[1] = rtb_Sqrt_m; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant11' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain2' - // Product: '/Divide3' - - rtb_Sqrt_m = sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2] - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[2]; - - // Product: '/Divide3' - if (rtb_Sqrt_m < 0.0F) { - rtb_Product_p = -1.0F; - } else if (rtb_Sqrt_m > 0.0F) { - rtb_Product_p = 1.0F; - } else if (rtb_Sqrt_m == 0.0F) { - rtb_Product_p = 0.0F; - } else { - rtb_Product_p = rtb_Sqrt_m; - } - - rtb_Sqrt_m = (real32_T)((real_T)(sim_model_lib0_P->Gain2_Gain_f * - rtb_Product_p * rtb_Sqrt_m * rtb_Sqrt_m) * - sim_model_lib0_P->env_rotational_drag_coeff); - rtb_Sum2_f[2] = rtb_Sqrt_m; - } else { - rtb_Sum2_f[0] = sim_model_lib0_P->Constant10_Value_g[0]; - rtb_Sum2_f[1] = sim_model_lib0_P->Constant10_Value_g[1]; - rtb_Sum2_f[2] = sim_model_lib0_P->Constant10_Value_g[2]; - } - - // End of Switch: '/Switch2' - - // Product: '/Product2' - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_alpha_B_ECI_B[i_0] = rtb_Assignment_m[(int32_T)(i_0 + 6)] * - rtb_bpm_force_B[2] + (rtb_Assignment_m[(int32_T)(i_0 + 3)] * - rtb_bpm_force_B[1] + rtb_Assignment_m[i_0] * rtb_bpm_force_B[0]); - } - - // End of Product: '/Product2' - - // SignalConversion: '/TmpSignal ConversionAtProductInport1' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Product: '/Product' - - tmp_5[0] = (real32_T)sim_model_lib0_P->Constant3_Value_e; - tmp_5[1] = rtb_alpha_B_ECI_B[2]; - tmp_5[2] = sim_model_lib0_P->Gain_Gain_hs * rtb_alpha_B_ECI_B[1]; - tmp_5[3] = sim_model_lib0_P->Gain1_Gain_ep * rtb_alpha_B_ECI_B[2]; - tmp_5[4] = (real32_T)sim_model_lib0_P->Constant3_Value_e; - tmp_5[5] = rtb_alpha_B_ECI_B[0]; - tmp_5[6] = rtb_alpha_B_ECI_B[1]; - tmp_5[7] = sim_model_lib0_P->Gain2_Gain_c * rtb_alpha_B_ECI_B[0]; - tmp_5[8] = (real32_T)sim_model_lib0_P->Constant3_Value_e; - - // Sum: '/Sum' incorporates: - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Sum2_f[i_0] = ((tmp_5[(int32_T)(i_0 + 3)] * rtb_bpm_force_B[1] + - tmp_5[i_0] * rtb_bpm_force_B[0]) + tmp_5[(int32_T)(i_0 + - 6)] * rtb_bpm_force_B[2]) + (rtb_Delay1_bpm_torque_B[i_0] + rtb_Sum2_f[i_0]); - } - - // End of Sum: '/Sum' - - // Product: '/Divide' - rt_mldivide_U1f3x3_U2f_XeZWzB4d(rtb_Assignment_m, rtb_Sum2_f, tmp); - - // Gain: '/Gain' - rtb_alpha_B_ECI_B[0] = sim_model_lib0_P->tun_env_alpha_dof_gain[0] * tmp[0]; - - // BusCreator: '/Bus Creator' - rtb_Sum2_f[0] = rtb_bpm_force_B[0]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant' - // Inport: '/cmc_msg_in' - // SignalConversion: '/Signal Conversion' - - rtb_V_B_ISS_ISS_h = sim_model_lib0_U_cmc_msg_in->center_of_mass[0] + - sim_model_lib0_P->tun_sim_cg_error[0]; - - // Sum: '/Subtract' incorporates: - // Constant: '/ 1' - // Constant: '/ 2' - // DataTypeConversion: '/Data Type Conversion6' - // Gain: '/Gain' - - rtb_P_sensor_CG_B[0] = ((real_T)sim_model_lib0_P->tun_epson_report_truth * - sim_model_lib0_P->epson_P_sensor_B_B_error[0] + (real_T) - sim_model_lib0_P->tun_abp_p_imu_body_body[0]) - (real_T)rtb_V_B_ISS_ISS_h; - - // Sum: '/Sum' - rtb_V_B_ISS_ISS[0] = rtb_V_B_ISS_ISS_h; - - // Gain: '/Gain' - rtb_alpha_B_ECI_B[1] = sim_model_lib0_P->tun_env_alpha_dof_gain[1] * tmp[1]; - - // BusCreator: '/Bus Creator' - rtb_Sum2_f[1] = rtb_bpm_force_B[1]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant' - // Inport: '/cmc_msg_in' - // SignalConversion: '/Signal Conversion' - - rtb_V_B_ISS_ISS_h = sim_model_lib0_U_cmc_msg_in->center_of_mass[1] + - sim_model_lib0_P->tun_sim_cg_error[1]; - - // Sum: '/Subtract' incorporates: - // Constant: '/ 1' - // Constant: '/ 2' - // DataTypeConversion: '/Data Type Conversion6' - // Gain: '/Gain' - - rtb_P_sensor_CG_B[1] = ((real_T)sim_model_lib0_P->tun_epson_report_truth * - sim_model_lib0_P->epson_P_sensor_B_B_error[1] + (real_T) - sim_model_lib0_P->tun_abp_p_imu_body_body[1]) - (real_T)rtb_V_B_ISS_ISS_h; - - // Sum: '/Sum' - rtb_V_B_ISS_ISS[1] = rtb_V_B_ISS_ISS_h; - - // Gain: '/Gain' - rtb_alpha_B_ECI_B[2] = sim_model_lib0_P->tun_env_alpha_dof_gain[2] * tmp[2]; - - // BusCreator: '/Bus Creator' - rtb_Sum2_f[2] = rtb_bpm_force_B[2]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant' - // Inport: '/cmc_msg_in' - // SignalConversion: '/Signal Conversion' - - rtb_V_B_ISS_ISS_h = sim_model_lib0_U_cmc_msg_in->center_of_mass[2] + - sim_model_lib0_P->tun_sim_cg_error[2]; - - // Sum: '/Subtract' incorporates: - // Constant: '/ 1' - // Constant: '/ 2' - // DataTypeConversion: '/Data Type Conversion6' - // Gain: '/Gain' - - rtb_P_sensor_CG_B[2] = ((real_T)sim_model_lib0_P->tun_epson_report_truth * - sim_model_lib0_P->epson_P_sensor_B_B_error[2] + (real_T) - sim_model_lib0_P->tun_abp_p_imu_body_body[2]) - (real_T)rtb_V_B_ISS_ISS_h; - - // Sum: '/Sum' - rtb_V_B_ISS_ISS[2] = rtb_V_B_ISS_ISS_h; - - // Switch: '/Switch1' incorporates: - // BusCreator: '/Bus Creator' - // Constant: '/Constant4' - // Constant: '/Constant5' - // DataTypeConversion: '/Data Type Conversion5' - // Product: '/Element product' - // Sum: '/Add1' - // Sum: '/Add3' - - if (sim_model_lib0_P->epson_no_rot_effects != 0.0) { - rtb_Add3[0] = sim_model_lib0_P->Constant5_Value; - rtb_Add3[1] = sim_model_lib0_P->Constant5_Value; - rtb_Add3[2] = sim_model_lib0_P->Constant5_Value; - } else { - // Sum: '/Add3' incorporates: - // BusCreator: '/Bus Creator' - // DataTypeConversion: '/Data Type Conversion3' - // Product: '/Element product' - - rtb_Add3[0] = (real_T)rtb_alpha_B_ECI_B[1] * rtb_P_sensor_CG_B[2] - (real_T) - rtb_alpha_B_ECI_B[2] * rtb_P_sensor_CG_B[1]; - rtb_Add3[1] = (real_T)rtb_alpha_B_ECI_B[2] * rtb_P_sensor_CG_B[0] - (real_T) - rtb_alpha_B_ECI_B[0] * rtb_P_sensor_CG_B[2]; - rtb_Add3[2] = (real_T)rtb_alpha_B_ECI_B[0] * rtb_P_sensor_CG_B[1] - (real_T) - rtb_alpha_B_ECI_B[1] * rtb_P_sensor_CG_B[0]; - - // Sum: '/Add3' incorporates: - // BusCreator: '/Bus Creator' - // DataTypeConversion: '/Data Type Conversion5' - // Product: '/Element product' - - rtb_Add3_a[0] = (real_T)rtb_bpm_force_B[1] * rtb_P_sensor_CG_B[2] - (real_T) - rtb_bpm_force_B[2] * rtb_P_sensor_CG_B[1]; - rtb_Add3_a[1] = (real_T)rtb_bpm_force_B[2] * rtb_P_sensor_CG_B[0] - (real_T) - rtb_bpm_force_B[0] * rtb_P_sensor_CG_B[2]; - rtb_Add3_a[2] = (real_T)rtb_bpm_force_B[0] * rtb_P_sensor_CG_B[1] - (real_T) - rtb_bpm_force_B[1] * rtb_P_sensor_CG_B[0]; - rtb_Add3[0] += (real_T)rtb_bpm_force_B[1] * rtb_Add3_a[2] - (real_T) - rtb_bpm_force_B[2] * rtb_Add3_a[1]; - rtb_Add3[1] += (real_T)rtb_bpm_force_B[2] * rtb_Add3_a[0] - (real_T) - rtb_bpm_force_B[0] * rtb_Add3_a[2]; - rtb_Add3[2] += (real_T)rtb_bpm_force_B[0] * rtb_Add3_a[1] - (real_T) - rtb_bpm_force_B[1] * rtb_Add3_a[0]; - } - - // End of Switch: '/Switch1' - - // DataTypeConversion: '/Data Type Conversion2' incorporates: - // BusCreator: '/Bus Creator' - - rtb_Product_l[0] = (real_T)rtb_Merge_m[0]; - rtb_Product_l[1] = (real_T)rtb_Merge_m[1]; - rtb_Product_l[2] = (real_T)rtb_Merge_m[2]; - rtb_Product_l[3] = (real_T)rtb_Merge_m[3]; - - // Sum: '/Sum' incorporates: - // BusCreator: '/Bus Creator' - // Constant: '/Constant1' - // DataTypeConversion: '/Data Type Conversion2' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum = (real_T)rtb_Merge_m[3] * (real_T)rtb_Merge_m[3] * - sim_model_lib0_P->Gain_Gain_e - sim_model_lib0_P->Constant1_Value_b; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - - memcpy(&rtb_Assignment[0], &sim_model_lib0_P->Constant2_Value_cr[0], (uint32_T) - (9U * sizeof(real_T))); - rtb_Assignment[0] = rtb_Sum; - rtb_Assignment[4] = rtb_Sum; - rtb_Assignment[8] = rtb_Sum; - - // Gain: '/Gain1' incorporates: - // BusCreator: '/Bus Creator' - // DataTypeConversion: '/Data Type Conversion2' - - rtb_Sum = sim_model_lib0_P->Gain1_Gain_o * (real_T)rtb_Merge_m[3]; - - // Product: '/Product' incorporates: - // BusCreator: '/Bus Creator' - // Constant: '/Constant3' - // DataTypeConversion: '/Data Type Conversion2' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Assignment_c[0] = sim_model_lib0_P->Constant3_Value_d; - rtb_Assignment_c[1] = (real_T)rtb_Merge_m[2]; - rtb_Assignment_c[2] = sim_model_lib0_P->Gain_Gain_l * (real_T)rtb_Merge_m[1]; - rtb_Assignment_c[3] = sim_model_lib0_P->Gain1_Gain_oh * (real_T)rtb_Merge_m[2]; - rtb_Assignment_c[4] = sim_model_lib0_P->Constant3_Value_d; - rtb_Assignment_c[5] = (real_T)rtb_Merge_m[0]; - rtb_Assignment_c[6] = (real_T)rtb_Merge_m[1]; - rtb_Assignment_c[7] = sim_model_lib0_P->Gain2_Gain_o * (real_T)rtb_Merge_m[0]; - rtb_Assignment_c[8] = sim_model_lib0_P->Constant3_Value_d; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Product_p_0[i_0] = rtb_Product_l[i_0] * rtb_Product_l[0]; - rtb_Product_p_0[(int32_T)(i_0 + 3)] = rtb_Product_l[i_0] * rtb_Product_l[1]; - rtb_Product_p_0[(int32_T)(i_0 + 6)] = rtb_Product_l[i_0] * rtb_Product_l[2]; - } - - // End of Product: '/Product1' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant6' - - tmp_0 = (sim_model_lib0_P->tun_ase_gravity_removal > - sim_model_lib0_P->Switch_Threshold_p); - for (i_0 = 0; i_0 < 3; i_0++) { - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - rtb_Assignment_3[(int32_T)(3 * i_0)] = (rtb_Assignment[(int32_T)(3 * i_0)] - - rtb_Assignment_c[(int32_T)(3 * i_0)] * rtb_Sum) + rtb_Product_p_0[(int32_T) - (3 * i_0)] * sim_model_lib0_P->Gain2_Gain_ar; - rtb_Assignment_3[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 1)] - rtb_Assignment_c[(int32_T)((int32_T) - (3 * i_0) + 1)] * rtb_Sum) + rtb_Product_p_0[(int32_T)((int32_T)(3 * i_0) - + 1)] * sim_model_lib0_P->Gain2_Gain_ar; - rtb_Assignment_3[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 2)] - rtb_Assignment_c[(int32_T)((int32_T) - (3 * i_0) + 2)] * rtb_Sum) + rtb_Product_p_0[(int32_T)((int32_T)(3 * i_0) - + 2)] * sim_model_lib0_P->Gain2_Gain_ar; - - // Sum: '/Subtract1' incorporates: - // BusCreator: '/Bus Creator' - // Constant: '/Constant4' - // Constant: '/Constant7' - // DataTypeConversion: '/Data Type Conversion4' - // DataTypeConversion: '/Data Type Conversion9' - // Product: '/Product' - // Switch: '/Switch' - - if (tmp_0) { - rtb_Product = (real_T)sim_model_lib0_P->tun_ase_gravity_accel[i_0]; - } else { - rtb_Product = (real_T)sim_model_lib0_P->Constant7_Value_j[i_0]; - } - - rtb_Add3_a[i_0] = (real_T)rtb_imu_gyro_bias[i_0] + rtb_Product; - - // End of Sum: '/Subtract1' - } - - // Sum: '/Add2' incorporates: - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_P_sensor_CG_B[i_0] = ((rtb_Assignment_3[(int32_T)(i_0 + 3)] * - rtb_Add3_a[1] + rtb_Assignment_3[i_0] * rtb_Add3_a[0]) + rtb_Assignment_3 - [(int32_T)(i_0 + 6)] * rtb_Add3_a[2]) + rtb_Add3[i_0]; - } - - // End of Sum: '/Add2' - - // Sum: '/Subtract' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // DataTypeConversion: '/Data Type Conversion7' - - rtb_Sum = (real_T)sim_model_lib0_P->Constant_Value_g - - sim_model_lib0_P->Constant_Value; - for (i = 0; i < 3; i++) { - // RandomNumber: '/random_noise' - rtb_random_noise[i] = sim_model_lib0_DW->NextOutput[i]; - - // Sum: '/Sum6' incorporates: - // Constant: '/Constant2' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // DiscreteIntegrator: '/Discrete-Time Integrator2' - // Product: '/Divide4' - - rtb_Add3_a[i] = (rtb_Sum * sim_model_lib0_P->epson_accel_temp_bias_coeff[i] - + sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTATE[i]) + - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[i]; - - // Product: '/Product' - rtb_Add3[i] = rtb_Assignment_jy[(int32_T)(i + 6)] * rtb_P_sensor_CG_B[2] + - (rtb_Assignment_jy[(int32_T)(i + 3)] * rtb_P_sensor_CG_B[1] + - rtb_Assignment_jy[i] * rtb_P_sensor_CG_B[0]); - } - - // Switch: '/Switch' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant11' - // Constant: '/Constant3' - // Constant: '/Constant6' - // Gain: '/Gain4' - // Gain: '/scale_factor' - // Product: '/Divide' - // Product: '/Product2' - // Sum: '/Sum2' - - if ((int32_T)sim_model_lib0_P->tun_epson_report_truth == 0) { - // Gain: '/Gain4' - rtb_Sum = 1.0 / sqrt(sim_model_lib0_P->astrobee_time_step_size); - - // Gain: '/scale_factor' - rtb_Product = sim_model_lib0_P->epson_accel_sf_coef * rtb_Add3[0]; - rtb_Product = ((sin(rtb_Product * sim_model_lib0_P->Constant1_Value_p / - sim_model_lib0_P->epson_accel_upper_sat) * - sim_model_lib0_P->epson_accel_nonlinearity_coeff + - rtb_Product) + rtb_Sum * rtb_random_noise[0]) + rtb_Add3_a[0]; - rtb_Add3[0] = rtb_Product; - - // Gain: '/scale_factor' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant11' - // Constant: '/Constant6' - // Gain: '/Gain4' - // Product: '/Divide' - // Product: '/Product2' - // Sum: '/Sum2' - - rtb_Product = sim_model_lib0_P->epson_accel_sf_coef * rtb_Add3[1]; - rtb_Product = ((sin(rtb_Product * sim_model_lib0_P->Constant1_Value_p / - sim_model_lib0_P->epson_accel_upper_sat) * - sim_model_lib0_P->epson_accel_nonlinearity_coeff + - rtb_Product) + rtb_Sum * rtb_random_noise[1]) + rtb_Add3_a[1]; - rtb_Add3[1] = rtb_Product; - - // Gain: '/scale_factor' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant11' - // Constant: '/Constant6' - // Gain: '/Gain4' - // Product: '/Divide' - // Product: '/Product2' - // Sum: '/Sum2' - - rtb_Product = sim_model_lib0_P->epson_accel_sf_coef * rtb_Add3[2]; - rtb_Product = ((sin(rtb_Product * sim_model_lib0_P->Constant1_Value_p / - sim_model_lib0_P->epson_accel_upper_sat) * - sim_model_lib0_P->epson_accel_nonlinearity_coeff + - rtb_Product) + rtb_Sum * rtb_random_noise[2]) + rtb_Add3_a[2]; - rtb_Add3[2] = rtb_Product; - } - - // End of Switch: '/Switch' - - // Quantizer: '/Quantizer' incorporates: - // DiscreteTransferFcn: '/Discrete Transfer Fcn1' - - rtb_Product = rt_roundd_snf(rtb_Add3[0] * - sim_model_lib0_P->epson_accel_filt_num / - sim_model_lib0_P->epson_accel_filt_den / - sim_model_lib0_P->epson_accel_resolution) * - sim_model_lib0_P->epson_accel_resolution; - - // Saturate: '/Saturation' - if (rtb_Product > sim_model_lib0_P->epson_accel_upper_sat) { - y = sim_model_lib0_P->epson_accel_upper_sat; - } else if (rtb_Product < sim_model_lib0_P->epson_accel_lower_sat) { - y = sim_model_lib0_P->epson_accel_lower_sat; - } else { - y = rtb_Product; - } - - // Quantizer: '/Quantizer' incorporates: - // DiscreteTransferFcn: '/Discrete Transfer Fcn1' - - rtb_Product = rt_roundd_snf(rtb_Add3[1] * - sim_model_lib0_P->epson_accel_filt_num / - sim_model_lib0_P->epson_accel_filt_den / - sim_model_lib0_P->epson_accel_resolution) * - sim_model_lib0_P->epson_accel_resolution; - - // Saturate: '/Saturation' - if (rtb_Product > sim_model_lib0_P->epson_accel_upper_sat) { - y_0 = sim_model_lib0_P->epson_accel_upper_sat; - } else if (rtb_Product < sim_model_lib0_P->epson_accel_lower_sat) { - y_0 = sim_model_lib0_P->epson_accel_lower_sat; - } else { - y_0 = rtb_Product; - } - - // Quantizer: '/Quantizer' incorporates: - // DiscreteTransferFcn: '/Discrete Transfer Fcn1' - - rtb_Product = rt_roundd_snf(rtb_Add3[2] * - sim_model_lib0_P->epson_accel_filt_num / - sim_model_lib0_P->epson_accel_filt_den / - sim_model_lib0_P->epson_accel_resolution) * - sim_model_lib0_P->epson_accel_resolution; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant2' - // DataTypeConversion: '/Data Type Conversion2' - - memcpy(&rtb_Assignment_jy[0], &sim_model_lib0_P->Constant2_Value_n[0], - (uint32_T)(9U * sizeof(real_T))); - rtb_Assignment_jy[0] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[3]; - rtb_Assignment_jy[4] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[3]; - rtb_Assignment_jy[8] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant3' - // DataTypeConversion: '/Data Type Conversion2' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_6[0] = sim_model_lib0_P->Constant3_Value_i; - tmp_6[1] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[2]; - tmp_6[2] = sim_model_lib0_P->Gain_Gain_d * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[1]; - tmp_6[3] = sim_model_lib0_P->Gain1_Gain_j * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[2]; - tmp_6[4] = sim_model_lib0_P->Constant3_Value_i; - tmp_6[5] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[0]; - tmp_6[6] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[1]; - tmp_6[7] = sim_model_lib0_P->Gain2_Gain_k * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[0]; - tmp_6[8] = sim_model_lib0_P->Constant3_Value_i; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Data Type Conversion2' - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_MatrixConcatenate[(int32_T)(i_0 << 2)] = rtb_Assignment_jy[(int32_T)(3 * - i_0)] + tmp_6[(int32_T)(3 * i_0)]; - rtb_MatrixConcatenate[(int32_T)(1 + (int32_T)(i_0 << 2))] = - rtb_Assignment_jy[(int32_T)((int32_T)(3 * i_0) + 1)] + tmp_6[(int32_T) - ((int32_T)(3 * i_0) + 1)]; - rtb_MatrixConcatenate[(int32_T)(2 + (int32_T)(i_0 << 2))] = - rtb_Assignment_jy[(int32_T)((int32_T)(3 * i_0) + 2)] + tmp_6[(int32_T) - ((int32_T)(3 * i_0) + 2)]; - } - - rtb_MatrixConcatenate[3] = sim_model_lib0_P->Gain1_Gain_h * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[0]; - rtb_MatrixConcatenate[7] = sim_model_lib0_P->Gain1_Gain_h * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[1]; - rtb_MatrixConcatenate[11] = sim_model_lib0_P->Gain1_Gain_h * (real_T) - sim_model_lib0_P->tun_abp_quat_body2imu[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Switch: '/Switch4' incorporates: - // Constant: '/Constant11' - - tmp_0 = ((int32_T)sim_model_lib0_P->tun_epson_report_truth != 0); - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Data Type Conversion2' - - rtb_MatrixConcatenate[12] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[0]; - - // Product: '/Product' incorporates: - // Constant: '/Constant10' - // Constant: '/Constant9' - // Switch: '/Switch4' - - if (tmp_0) { - rtb_Sum = sim_model_lib0_P->Constant10_Value[0]; - rtb_DataTypeConversion5_idx_0 = sim_model_lib0_P->Constant10_Value[1]; - } else { - rtb_Sum = sim_model_lib0_P->epson_Q_B2gyro_error[0]; - rtb_DataTypeConversion5_idx_0 = sim_model_lib0_P->epson_Q_B2gyro_error[1]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Data Type Conversion2' - - rtb_MatrixConcatenate[13] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[1]; - rtb_MatrixConcatenate[14] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[2]; - - // Product: '/Product' incorporates: - // Constant: '/Constant10' - // Constant: '/Constant9' - // Switch: '/Switch4' - - if (tmp_0) { - rtb_DataTypeConversion5_idx_1 = sim_model_lib0_P->Constant10_Value[2]; - rtb_DataTypeConversion5_idx_2 = sim_model_lib0_P->Constant10_Value[3]; - } else { - rtb_DataTypeConversion5_idx_1 = sim_model_lib0_P->epson_Q_B2gyro_error[2]; - rtb_DataTypeConversion5_idx_2 = sim_model_lib0_P->epson_Q_B2gyro_error[3]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Data Type Conversion2' - - rtb_MatrixConcatenate[15] = (real_T)sim_model_lib0_P->tun_abp_quat_body2imu[3]; - - // Product: '/Product' - for (i_0 = 0; i_0 < 4; i_0++) { - rtb_linearaccelbias = rtb_MatrixConcatenate[(int32_T)(i_0 + 12)] * - rtb_DataTypeConversion5_idx_2 + (rtb_MatrixConcatenate[(int32_T)(i_0 + 8)] - * rtb_DataTypeConversion5_idx_1 + (rtb_MatrixConcatenate[(int32_T)(i_0 + 4)] - * rtb_DataTypeConversion5_idx_0 + rtb_MatrixConcatenate[i_0] * rtb_Sum)); - rtb_Product_l[i_0] = rtb_linearaccelbias; - } - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum = rtb_Product_l[3] * rtb_Product_l[3] * sim_model_lib0_P->Gain_Gain_ee - - sim_model_lib0_P->Constant1_Value_k; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - - memcpy(&rtb_Assignment_jy[0], &sim_model_lib0_P->Constant2_Value_dr[0], - (uint32_T)(9U * sizeof(real_T))); - rtb_Assignment_jy[0] = rtb_Sum; - rtb_Assignment_jy[4] = rtb_Sum; - rtb_Assignment_jy[8] = rtb_Sum; - - // Gain: '/Gain1' - rtb_Sum = sim_model_lib0_P->Gain1_Gain_dy * rtb_Product_l[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_7[0] = sim_model_lib0_P->Constant3_Value_iy; - tmp_7[1] = rtb_Product_l[2]; - tmp_7[2] = sim_model_lib0_P->Gain_Gain_k * rtb_Product_l[1]; - tmp_7[3] = sim_model_lib0_P->Gain1_Gain_p * rtb_Product_l[2]; - tmp_7[4] = sim_model_lib0_P->Constant3_Value_iy; - tmp_7[5] = rtb_Product_l[0]; - tmp_7[6] = rtb_Product_l[1]; - tmp_7[7] = sim_model_lib0_P->Gain2_Gain_dz * rtb_Product_l[0]; - tmp_7[8] = sim_model_lib0_P->Constant3_Value_iy; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Product_p_0[i_0] = rtb_Product_l[i_0] * rtb_Product_l[0]; - rtb_Product_p_0[(int32_T)(i_0 + 3)] = rtb_Product_l[i_0] * rtb_Product_l[1]; - rtb_Product_p_0[(int32_T)(i_0 + 6)] = rtb_Product_l[i_0] * rtb_Product_l[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_c[(int32_T)(3 * i_0)] = (rtb_Assignment_jy[(int32_T)(3 * i_0)] - - tmp_7[(int32_T)(3 * i_0)] * rtb_Sum) + rtb_Product_p_0[(int32_T)(3 * i_0)] - * sim_model_lib0_P->Gain2_Gain_g; - rtb_Assignment_c[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment_jy - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_7[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Sum) + rtb_Product_p_0[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_g; - rtb_Assignment_c[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment_jy - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_7[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Sum) + rtb_Product_p_0[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_g; - } - - // End of Sum: '/Sum1' - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant4' - // DataTypeConversion: '/Data Type Conversion7' - - rtb_Sum = (real_T)sim_model_lib0_P->Constant_Value_g - - sim_model_lib0_P->Constant4_Value; - for (i = 0; i < 3; i++) { - // RandomNumber: '/random_noise' - rtb_random_noise[i] = sim_model_lib0_DW->NextOutput_e[i]; - - // Product: '/Product' incorporates: - // BusCreator: '/Bus Creator' - // DataTypeConversion: '/Data Type Conversion5' - - rtb_Add3[i] = rtb_Assignment_c[(int32_T)(i + 6)] * (real_T)rtb_bpm_force_B[2] - + (rtb_Assignment_c[(int32_T)(i + 3)] * (real_T)rtb_bpm_force_B[1] + - rtb_Assignment_c[i] * (real_T)rtb_bpm_force_B[0]); - } - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant8' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // DiscreteIntegrator: '/Discrete-Time Integrator2' - // Product: '/Divide2' - - rtb_DataTypeConversion5_idx_0 = (rtb_Sum * - sim_model_lib0_P->epson_gyro_temp_bias_coeff[0] + - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[0]) + - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[0]; - rtb_DataTypeConversion5_idx_1 = (rtb_Sum * - sim_model_lib0_P->epson_gyro_temp_bias_coeff[1] + - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[1]) + - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[1]; - rtb_DataTypeConversion5_idx_2 = (rtb_Sum * - sim_model_lib0_P->epson_gyro_temp_bias_coeff[2] + - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[2]) + - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[2]; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant5' - // Constant: '/Constant6' - // Constant: '/Constant9' - // Gain: '/Gain3' - // Gain: '/scale_factor' - // Product: '/Divide1' - // Product: '/Product2' - // Sum: '/Sum1' - - if ((int32_T)sim_model_lib0_P->tun_epson_report_truth == 0) { - // Gain: '/Gain3' - rtb_Sum = 1.0 / sqrt(sim_model_lib0_P->astrobee_time_step_size); - rtb_Add3[0] *= sim_model_lib0_P->epson_gyro_sf_coef; - rtb_Add3[1] *= sim_model_lib0_P->epson_gyro_sf_coef; - rtb_Add3[2] *= sim_model_lib0_P->epson_gyro_sf_coef; - - // Product: '/Divide6' incorporates: - // Constant: '/Constant3' - // DotProduct: '/Dot Product1' - // Gain: '/scale_factor' - // Sqrt: '/Sqrt' - - rtb_linearaccelbias = sqrt((rtb_P_sensor_CG_B[0] * rtb_P_sensor_CG_B[0] + - rtb_P_sensor_CG_B[1] * rtb_P_sensor_CG_B[1]) + rtb_P_sensor_CG_B[2] * - rtb_P_sensor_CG_B[2]) * - sim_model_lib0_P->epson_gyro_linear_accel_bias_coeff; - rtb_Add3[0] = (((sin(rtb_Add3[0] * sim_model_lib0_P->Constant5_Value_a / - sim_model_lib0_P->epson_gyro_upper_sat) * - sim_model_lib0_P->epson_gyro_nonlinearity_coeff + rtb_Add3 - [0]) + rtb_linearaccelbias) + rtb_Sum * rtb_random_noise[0]) - + rtb_DataTypeConversion5_idx_0; - rtb_Add3[1] = (((sin(rtb_Add3[1] * sim_model_lib0_P->Constant5_Value_a / - sim_model_lib0_P->epson_gyro_upper_sat) * - sim_model_lib0_P->epson_gyro_nonlinearity_coeff + rtb_Add3 - [1]) + rtb_linearaccelbias) + rtb_Sum * rtb_random_noise[1]) - + rtb_DataTypeConversion5_idx_1; - rtb_Add3[2] = (((sin(rtb_Add3[2] * sim_model_lib0_P->Constant5_Value_a / - sim_model_lib0_P->epson_gyro_upper_sat) * - sim_model_lib0_P->epson_gyro_nonlinearity_coeff + rtb_Add3 - [2]) + rtb_linearaccelbias) + rtb_Sum * rtb_random_noise[2]) - + rtb_DataTypeConversion5_idx_2; - } - - // End of Switch: '/Switch' - - // BusCreator: '/Bus Creator' incorporates: - // BusCreator: '/Bus Creator1' - // Constant: '/Constant1' - // Constant: '/Constant2' - - sim_model_lib0_Y_imu_msg_o->imu_timestamp_sec = rtb_Switch1; - sim_model_lib0_Y_imu_msg_o->imu_timestamp_nsec = rtb_Switch_g; - sim_model_lib0_Y_imu_msg_o->imu_validity_flag = - sim_model_lib0_P->Constant1_Value_ol; - sim_model_lib0_Y_imu_msg_o->imu_sat_flag = - sim_model_lib0_P->Constant2_Value_cc; - - // Quantizer: '/Quantizer1' incorporates: - // DiscreteTransferFcn: '/Discrete Transfer Fcn' - - rtb_Sum = rt_roundd_snf(rtb_Add3[0] * sim_model_lib0_P->epson_gyro_filt_num / - sim_model_lib0_P->epson_gyro_filt_den / - sim_model_lib0_P->epson_gyro_resolution) * - sim_model_lib0_P->epson_gyro_resolution; - - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // DataTypeConversion: '/Data Type Conversion8' - // Delay: '/Delay2' - // Saturate: '/Saturation' - - sim_model_lib0_Y_imu_msg_o->imu_A_B_ECI_sensor[0] = (real32_T)y; - sim_model_lib0_Y_imu_msg_o->imu_accel_bias[0] = (real32_T)rtb_Add3_a[0]; - - // Saturate: '/Saturation1' - if (rtb_Sum > sim_model_lib0_P->epson_gyro_upper_sat) { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[0] = (real32_T) - sim_model_lib0_P->epson_gyro_upper_sat; - } else if (rtb_Sum < sim_model_lib0_P->epson_gyro_lower_sat) { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[0] = (real32_T) - sim_model_lib0_P->epson_gyro_lower_sat; - } else { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[0] = (real32_T)rtb_Sum; - } - - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion10' - // Delay: '/Delay1' - - sim_model_lib0_Y_imu_msg_o->imu_gyro_bias[0] = (real32_T) - rtb_DataTypeConversion5_idx_0; - - // Outport: '/env_msg' incorporates: - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - sim_model_lib0_Y_env_msg_i->P_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0]; - sim_model_lib0_Y_env_msg_i->V_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0]; - sim_model_lib0_Y_env_msg_i->A_B_ISS_ISS[0] = rtb_P_B_ISS_SS[0]; - sim_model_lib0_Y_env_msg_i->A_B_ISS_B[0] = rtb_Merge_o[0]; - sim_model_lib0_Y_env_msg_i->A_B_ECI_B[0] = rtb_imu_gyro_bias[0]; - - // Quantizer: '/Quantizer1' incorporates: - // DiscreteTransferFcn: '/Discrete Transfer Fcn' - - rtb_Sum = rt_roundd_snf(rtb_Add3[1] * sim_model_lib0_P->epson_gyro_filt_num / - sim_model_lib0_P->epson_gyro_filt_den / - sim_model_lib0_P->epson_gyro_resolution) * - sim_model_lib0_P->epson_gyro_resolution; - - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // DataTypeConversion: '/Data Type Conversion8' - // Delay: '/Delay2' - // Saturate: '/Saturation' - - sim_model_lib0_Y_imu_msg_o->imu_A_B_ECI_sensor[1] = (real32_T)y_0; - sim_model_lib0_Y_imu_msg_o->imu_accel_bias[1] = (real32_T)rtb_Add3_a[1]; - - // Saturate: '/Saturation1' - if (rtb_Sum > sim_model_lib0_P->epson_gyro_upper_sat) { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[1] = (real32_T) - sim_model_lib0_P->epson_gyro_upper_sat; - } else if (rtb_Sum < sim_model_lib0_P->epson_gyro_lower_sat) { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[1] = (real32_T) - sim_model_lib0_P->epson_gyro_lower_sat; - } else { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[1] = (real32_T)rtb_Sum; - } - - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion10' - // Delay: '/Delay1' - - sim_model_lib0_Y_imu_msg_o->imu_gyro_bias[1] = (real32_T) - rtb_DataTypeConversion5_idx_1; - - // Outport: '/env_msg' incorporates: - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - sim_model_lib0_Y_env_msg_i->P_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1]; - sim_model_lib0_Y_env_msg_i->V_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1]; - sim_model_lib0_Y_env_msg_i->A_B_ISS_ISS[1] = rtb_P_B_ISS_SS[1]; - sim_model_lib0_Y_env_msg_i->A_B_ISS_B[1] = rtb_Merge_o[1]; - sim_model_lib0_Y_env_msg_i->A_B_ECI_B[1] = rtb_imu_gyro_bias[1]; - - // Quantizer: '/Quantizer1' incorporates: - // DiscreteTransferFcn: '/Discrete Transfer Fcn' - - rtb_Sum = rt_roundd_snf(rtb_Add3[2] * sim_model_lib0_P->epson_gyro_filt_num / - sim_model_lib0_P->epson_gyro_filt_den / - sim_model_lib0_P->epson_gyro_resolution) * - sim_model_lib0_P->epson_gyro_resolution; - - // Saturate: '/Saturation' - if (rtb_Product > sim_model_lib0_P->epson_accel_upper_sat) { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion' - - sim_model_lib0_Y_imu_msg_o->imu_A_B_ECI_sensor[2] = (real32_T) - sim_model_lib0_P->epson_accel_upper_sat; - } else if (rtb_Product < sim_model_lib0_P->epson_accel_lower_sat) { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion' - - sim_model_lib0_Y_imu_msg_o->imu_A_B_ECI_sensor[2] = (real32_T) - sim_model_lib0_P->epson_accel_lower_sat; - } else { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion' - - sim_model_lib0_Y_imu_msg_o->imu_A_B_ECI_sensor[2] = (real32_T)rtb_Product; - } - - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion8' - // Delay: '/Delay2' - - sim_model_lib0_Y_imu_msg_o->imu_accel_bias[2] = (real32_T)rtb_Add3_a[2]; - - // Saturate: '/Saturation1' - if (rtb_Sum > sim_model_lib0_P->epson_gyro_upper_sat) { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[2] = (real32_T) - sim_model_lib0_P->epson_gyro_upper_sat; - } else if (rtb_Sum < sim_model_lib0_P->epson_gyro_lower_sat) { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[2] = (real32_T) - sim_model_lib0_P->epson_gyro_lower_sat; - } else { - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - sim_model_lib0_Y_imu_msg_o->imu_omega_B_ECI_sensor[2] = (real32_T)rtb_Sum; - } - - // BusCreator: '/Bus Creator' incorporates: - // DataTypeConversion: '/Data Type Conversion10' - // Delay: '/Delay1' - - sim_model_lib0_Y_imu_msg_o->imu_gyro_bias[2] = (real32_T) - rtb_DataTypeConversion5_idx_2; - - // Outport: '/env_msg' incorporates: - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - sim_model_lib0_Y_env_msg_i->P_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2]; - sim_model_lib0_Y_env_msg_i->V_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2]; - sim_model_lib0_Y_env_msg_i->A_B_ISS_ISS[2] = rtb_Merge_c; - sim_model_lib0_Y_env_msg_i->A_B_ISS_B[2] = rtb_Merge_o[2]; - sim_model_lib0_Y_env_msg_i->A_B_ECI_B[2] = rtb_imu_gyro_bias[2]; - sim_model_lib0_Y_env_msg_i->Q_ISS2B[0] = rtb_Merge_m[0]; - sim_model_lib0_Y_env_msg_i->Q_ISS2B[1] = rtb_Merge_m[1]; - sim_model_lib0_Y_env_msg_i->Q_ISS2B[2] = rtb_Merge_m[2]; - sim_model_lib0_Y_env_msg_i->Q_ISS2B[3] = rtb_Merge_m[3]; - sim_model_lib0_Y_env_msg_i->omega_B_ISS_B[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - sim_model_lib0_Y_env_msg_i->omega_B_ECI_B[0] = rtb_bpm_force_B[0]; - sim_model_lib0_Y_env_msg_i->alpha_B_ISS_B[0] = rtb_alpha_B_ECI_B[0]; - sim_model_lib0_Y_env_msg_i->fan_torques_B[0] = rtb_Delay1_bpm_torque_B[0]; - sim_model_lib0_Y_env_msg_i->fan_forces_B[0] = rtb_TrigonometricFunction1_c; - sim_model_lib0_Y_env_msg_i->omega_B_ISS_B[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - sim_model_lib0_Y_env_msg_i->omega_B_ECI_B[1] = rtb_bpm_force_B[1]; - sim_model_lib0_Y_env_msg_i->alpha_B_ISS_B[1] = rtb_alpha_B_ECI_B[1]; - sim_model_lib0_Y_env_msg_i->fan_torques_B[1] = rtb_Delay1_bpm_torque_B[1]; - sim_model_lib0_Y_env_msg_i->fan_forces_B[1] = rtb_Delay1_bpm_force_B_idx_1; - sim_model_lib0_Y_env_msg_i->omega_B_ISS_B[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - sim_model_lib0_Y_env_msg_i->omega_B_ECI_B[2] = rtb_bpm_force_B[2]; - sim_model_lib0_Y_env_msg_i->alpha_B_ISS_B[2] = rtb_alpha_B_ECI_B[2]; - sim_model_lib0_Y_env_msg_i->fan_torques_B[2] = rtb_Delay1_bpm_torque_B[2]; - sim_model_lib0_Y_env_msg_i->fan_forces_B[2] = rtb_Delay1_bpm_force_B_idx_2; - - // Outputs for Atomic SubSystem: '/speed_controller' - - // Constant: '/Constant1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Inport: '/act_msg' - - sim_model__speed_controller(sim_model_lib0_P->Constant1_Value_fq, - sim_model_lib0_U_act_msg_l->act_impeller_speed_cmd[0], - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTAT_dp, - &sim_model_lib0_B->speed_controller, &sim_model_lib0_DW->speed_controller, - (P_speed_controller_sim_model__T *)&sim_model_lib0_P->speed_controller, - sim_model_lib0_P->bpm_blower_1_propulsion_module_, - sim_model_lib0_P->bpm_imp_speed_filt_num, - sim_model_lib0_P->bpm_imp_speed_filt_den, sim_model_lib0_P->bpm_imp_ctl_kp, - sim_model_lib0_P->bpm_imp_ctl_kd, sim_model_lib0_P->bpm_imp_ctl_filt_n, - sim_model_lib0_P->bpm_imp_max_voltage, sim_model_lib0_P->bpm_imp_ctl_ki); - - // End of Outputs for SubSystem: '/speed_controller' - - // Outputs for Atomic SubSystem: '/dc_motor_model' - - // DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_li_dc_motor_model(sim_model_lib0_B->speed_controller.Switch2, - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTAT_dp, - &sim_model_lib0_B->dc_motor_model, sim_model_lib0_P->bpm_imp_motor_speed_k, - sim_model_lib0_P->bpm_imp_motor_r, sim_model_lib0_P->bpm_imp_motor_torque_k, - sim_model_lib0_P->bpm_imp_motor_friction_coeff); - - // End of Outputs for SubSystem: '/dc_motor_model' - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn1' - rtb_bpm_motor_curr_idx_0 = sim_model_lib0_B->dc_motor_model.current; - - // Outputs for Atomic SubSystem: '/speed_controller' - - // Constant: '/Constant2' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Inport: '/act_msg' - - sim_model__speed_controller(sim_model_lib0_P->Constant2_Value_cw, - sim_model_lib0_U_act_msg_l->act_impeller_speed_cmd[1], - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_e, - &sim_model_lib0_B->speed_controller_c, - &sim_model_lib0_DW->speed_controller_c, (P_speed_controller_sim_model__T *) - &sim_model_lib0_P->speed_controller_c, - sim_model_lib0_P->bpm_blower_2_propulsion_module_, - sim_model_lib0_P->bpm_imp_speed_filt_num, - sim_model_lib0_P->bpm_imp_speed_filt_den, sim_model_lib0_P->bpm_imp_ctl_kp, - sim_model_lib0_P->bpm_imp_ctl_kd, sim_model_lib0_P->bpm_imp_ctl_filt_n, - sim_model_lib0_P->bpm_imp_max_voltage, sim_model_lib0_P->bpm_imp_ctl_ki); - - // End of Outputs for SubSystem: '/speed_controller' - - // Outputs for Atomic SubSystem: '/dc_motor_model' - - // DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_li_dc_motor_model(sim_model_lib0_B->speed_controller_c.Switch2, - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_e, - &sim_model_lib0_B->dc_motor_model_g, sim_model_lib0_P->bpm_imp_motor_speed_k, - sim_model_lib0_P->bpm_imp_motor_r, sim_model_lib0_P->bpm_imp_motor_torque_k, - sim_model_lib0_P->bpm_imp_motor_friction_coeff); - - // End of Outputs for SubSystem: '/dc_motor_model' - - // Outputs for Atomic SubSystem: '/servo_model' - - // Inport: '/act_msg' - sim_model_lib0_servo_model((real32_T *) - &sim_model_lib0_U_act_msg_l->act_servo_pwm_cmd[0], - &sim_model_lib0_B->servo_model, &sim_model_lib0_DW->servo_model, - (P_servo_model_sim_model_lib0_T *)&sim_model_lib0_P->servo_model, - sim_model_lib0_P->bpm_servo_pwm2angle_bias, - sim_model_lib0_P->bpm_servo_pwm2angle, (real32_T) - sim_model_lib0_P->bpm_servo_min_theta, - sim_model_lib0_P->bpm_servo_motor_gear_ratio, - sim_model_lib0_P->bpm_servo_max_theta, - sim_model_lib0_P->bpm_servo_motor_backlash_deadband, - sim_model_lib0_P->bpm_servo_ctl_kp, sim_model_lib0_P->bpm_servo_ctl_kd, - sim_model_lib0_P->bpm_servo_ctl_filt_n, - sim_model_lib0_P->bpm_servo_max_voltage, sim_model_lib0_P->bpm_servo_motor_k, - sim_model_lib0_P->bpm_servo_motor_r, - sim_model_lib0_P->bpm_servo_motor_friction_coeff, - sim_model_lib0_P->bpm_servo_motor_gear_box_inertia, - sim_model_lib0_P->bpm_servo_ctl_ki); - - // End of Outputs for SubSystem: '/servo_model' - - // SignalConversion: '/ConcatBufferAtVector Concatenate1In1' - for (i = 0; i < 6; i++) { - rtb_bpm_servo_curr[i] = sim_model_lib0_B->servo_model.current[i]; - } - - // End of SignalConversion: '/ConcatBufferAtVector Concatenate1In1' - - // Outputs for Atomic SubSystem: '/servo_model' - - // Inport: '/act_msg' - sim_model_lib0_servo_model((real32_T *) - &sim_model_lib0_U_act_msg_l->act_servo_pwm_cmd[6], - &sim_model_lib0_B->servo_model_f, &sim_model_lib0_DW->servo_model_f, - (P_servo_model_sim_model_lib0_T *)&sim_model_lib0_P->servo_model_f, - sim_model_lib0_P->bpm_servo_pwm2angle_bias, - sim_model_lib0_P->bpm_servo_pwm2angle, (real32_T) - sim_model_lib0_P->bpm_servo_min_theta, - sim_model_lib0_P->bpm_servo_motor_gear_ratio, - sim_model_lib0_P->bpm_servo_max_theta, - sim_model_lib0_P->bpm_servo_motor_backlash_deadband, - sim_model_lib0_P->bpm_servo_ctl_kp, sim_model_lib0_P->bpm_servo_ctl_kd, - sim_model_lib0_P->bpm_servo_ctl_filt_n, - sim_model_lib0_P->bpm_servo_max_voltage, sim_model_lib0_P->bpm_servo_motor_k, - sim_model_lib0_P->bpm_servo_motor_r, - sim_model_lib0_P->bpm_servo_motor_friction_coeff, - sim_model_lib0_P->bpm_servo_motor_gear_box_inertia, - sim_model_lib0_P->bpm_servo_ctl_ki); - - // End of Outputs for SubSystem: '/servo_model' - - // SignalConversion: '/ConcatBufferAtVector Concatenate1In2' - for (i = 0; i < 6; i++) { - rtb_bpm_servo_curr[(int32_T)(i + 6)] = - sim_model_lib0_B->servo_model_f.current[i]; - } - - // End of SignalConversion: '/ConcatBufferAtVector Concatenate1In2' - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_bpm_torque_B_c = sim_model_lib0_P->tun_bpm_noise_on_flag * - sim_model_lib0_P->bmp_PM1_impeller_orientation_error[0] + - sim_model_lib0_P->abp_pm1_impeller_orientation[0]; - - // DotProduct: '/Dot Product' - rtb_Sqrt_m = rtb_bpm_torque_B_c * rtb_bpm_torque_B_c; - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_bpm_torque_B[0] = rtb_bpm_torque_B_c; - rtb_bpm_torque_B_c = sim_model_lib0_P->tun_bpm_noise_on_flag * - sim_model_lib0_P->bmp_PM1_impeller_orientation_error[1] + - sim_model_lib0_P->abp_pm1_impeller_orientation[1]; - - // DotProduct: '/Dot Product' - rtb_Sqrt_m += rtb_bpm_torque_B_c * rtb_bpm_torque_B_c; - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_bpm_torque_B[1] = rtb_bpm_torque_B_c; - rtb_bpm_torque_B_c = sim_model_lib0_P->tun_bpm_noise_on_flag * - sim_model_lib0_P->bmp_PM1_impeller_orientation_error[2] + - sim_model_lib0_P->abp_pm1_impeller_orientation[2]; - - // DotProduct: '/Dot Product' - rtb_Sqrt_m += rtb_bpm_torque_B_c * rtb_bpm_torque_B_c; - - // Sum: '/Add1' - rtb_bpm_torque_B[2] = rtb_bpm_torque_B_c; - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Sqrt_m = (real32_T)sqrt((real_T)rtb_Sqrt_m); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Sqrt_m > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - sim_model_lib0_Normalize_e(rtb_bpm_torque_B, rtb_Sqrt_m, rtb_omega_B_ISS_B); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_omega_B_ISS_B[0] = rtb_bpm_torque_B[0]; - rtb_omega_B_ISS_B[1] = rtb_bpm_torque_B[1]; - rtb_omega_B_ISS_B[2] = rtb_bpm_torque_B_c; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Gain: '/Gain1' - rtb_Product_p = sim_model_lib0_P->Gain1_Gain_k3 * - sim_model_lib0_B->dc_motor_model.output_torque; - - // Product: '/Product' - rtb_bpm_torque_B[0] = rtb_omega_B_ISS_B[0] * rtb_Product_p; - rtb_bpm_torque_B[1] = rtb_omega_B_ISS_B[1] * rtb_Product_p; - rtb_bpm_torque_B[2] = rtb_omega_B_ISS_B[2] * rtb_Product_p; - - // Product: '/Product1' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant4' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain1' - // Sum: '/Add3' - - rtb_Product_p = (sim_model_lib0_P->tun_bpm_noise_on_flag * - sim_model_lib0_P->bpm_impeller_inertia_error + - sim_model_lib0_P->bpm_impeller_inertia) * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTAT_dp; - - // Outputs for Enabled SubSystem: '/latch_nozzle_thrust_matricies' - - // UnitDelay: '/Unit Delay' - latch_nozzle_thrust_matrici(sim_model_lib0_DW->UnitDelay_DSTATE_m, - rtb_V_B_ISS_ISS, &sim_model_lib0_B->latch_nozzle_thrust_matricies, - (P_latch_nozzle_thrust_matrici_T *) - &sim_model_lib0_P->latch_nozzle_thrust_matricies, - sim_model_lib0_P->bpm_PM1_Q_nozzle2misaligned, - sim_model_lib0_P->abp_PM1_P_nozzle_B_B, - sim_model_lib0_P->abp_PM1_nozzle_orientations, - sim_model_lib0_P->bpm_PM1_P_nozzle_B_B_error, - sim_model_lib0_P->abp_P_CG_B_B_error, - sim_model_lib0_P->tun_bpm_noise_on_flag); - - // End of Outputs for SubSystem: '/latch_nozzle_thrust_matricies' - - // Outputs for Atomic SubSystem: '/calc_nozzle_area' - sim_model__calc_nozzle_area(sim_model_lib0_B->servo_model.Gain1, - &sim_model_lib0_B->calc_nozzle_area, sim_model_lib0_P->abp_nozzle_flap_count, - sim_model_lib0_P->abp_nozzle_min_open_angle, - sim_model_lib0_P->abp_nozzle_flap_length, - sim_model_lib0_P->abp_nozzle_intake_height, - sim_model_lib0_P->abp_PM1_nozzle_widths, - sim_model_lib0_P->abp_nozzle_gear_ratio); - - // End of Outputs for SubSystem: '/calc_nozzle_area' - - // Outputs for Atomic SubSystem: '/blower_aerodynamics' - - // DiscreteIntegrator: '/Discrete-Time Integrator' - sim_mod_blower_aerodynamics(sim_model_lib0_DW->DiscreteTimeIntegrator_DSTAT_dp, - sim_model_lib0_B->calc_nozzle_area.Product2, - &sim_model_lib0_B->blower_aerodynamics, - &sim_model_lib0_DW->blower_aerodynamics, (P_blower_aerodynamics_sim_mod_T *) - &sim_model_lib0_P->blower_aerodynamics, - sim_model_lib0_P->abp_pm1_zero_thrust_area, - sim_model_lib0_P->bpm_PM1_zero_thrust_area_error, - sim_model_lib0_P->tun_bpm_noise_on_flag, - sim_model_lib0_P->abp_PM1_discharge_coeff, - sim_model_lib0_P->bpm_PM1_nozzle_discharge_coeff_error, - sim_model_lib0_P->bpm_lookup_Cdp_data, - sim_model_lib0_P->bpm_lookup_totalarea_breakpoints, - sim_model_lib0_P->abp_impeller_diameter, sim_model_lib0_P->const_air_density, - sim_model_lib0_P->tun_bpm_PM1_thrust_error_sf, - sim_model_lib0_P->bpm_PM1_nozzle_noise_feedback_gain); - - // End of Outputs for SubSystem: '/blower_aerodynamics' - - // SignalConversion: '/TmpSignal ConversionAtProductInport1' incorporates: - // BusCreator: '/Bus Creator' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Product: '/Product' - - tmp_8[0] = (real32_T)sim_model_lib0_P->Constant3_Value_ao; - tmp_8[1] = rtb_bpm_force_B[2]; - tmp_8[2] = sim_model_lib0_P->Gain_Gain_i * rtb_bpm_force_B[1]; - tmp_8[3] = sim_model_lib0_P->Gain1_Gain_me * rtb_bpm_force_B[2]; - tmp_8[4] = (real32_T)sim_model_lib0_P->Constant3_Value_ao; - tmp_8[5] = rtb_bpm_force_B[0]; - tmp_8[6] = rtb_bpm_force_B[1]; - tmp_8[7] = sim_model_lib0_P->Gain2_Gain_b * rtb_bpm_force_B[0]; - tmp_8[8] = (real32_T)sim_model_lib0_P->Constant3_Value_ao; - - // DotProduct: '/Dot Product' - rtb_Sqrt_m = 0.0F; - for (i = 0; i < 3; i++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - tmp[i] = 0.0F; - for (i_0 = 0; i_0 < 6; i_0++) { - tmp[i] += - sim_model_lib0_B->latch_nozzle_thrust_matricies.OutportBufferForthrust2torque_B - [(int32_T)((int32_T)(3 * i_0) + i)] * - sim_model_lib0_B->blower_aerodynamics.Add1[i_0]; - } - - // End of Product: '/Product4' - - // Sum: '/Add2' incorporates: - // Product: '/Product2' - // Product: '/Product' - - rtb_A_B_ISS_ISS[i] = (((rtb_omega_B_ISS_B[0] * rtb_Product_p * tmp_8[i] + - tmp_8[(int32_T)(i + 3)] * (rtb_omega_B_ISS_B[1] * rtb_Product_p)) + tmp_8 - [(int32_T)(i + 6)] * (rtb_omega_B_ISS_B[2] * rtb_Product_p)) + - rtb_bpm_torque_B[i]) + tmp[i]; - - // Sum: '/Add1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Gain: '/Gain' - - rtb_bpm_torque_B_c = sim_model_lib0_P->tun_bpm_noise_on_flag * - sim_model_lib0_P->bmp_PM2_impeller_orientation_error[i] + - sim_model_lib0_P->abp_pm2_impeller_orientation[i]; - - // DotProduct: '/Dot Product' - rtb_Sqrt_m += rtb_bpm_torque_B_c * rtb_bpm_torque_B_c; - - // Sum: '/Add1' - rtb_bpm_torque_B[i] = rtb_bpm_torque_B_c; - } - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Sqrt_m = (real32_T)sqrt((real_T)rtb_Sqrt_m); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Sqrt_m > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - sim_model_lib0_Normalize_e(rtb_bpm_torque_B, rtb_Sqrt_m, rtb_omega_B_ISS_B); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_omega_B_ISS_B[0] = rtb_bpm_torque_B[0]; - rtb_omega_B_ISS_B[1] = rtb_bpm_torque_B[1]; - rtb_omega_B_ISS_B[2] = rtb_bpm_torque_B[2]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Gain: '/Gain1' - rtb_Product_p = sim_model_lib0_P->Gain1_Gain_hf * - sim_model_lib0_B->dc_motor_model_g.output_torque; - - // Product: '/Product' - rtb_bpm_torque_B[0] = rtb_omega_B_ISS_B[0] * rtb_Product_p; - rtb_bpm_torque_B[1] = rtb_omega_B_ISS_B[1] * rtb_Product_p; - rtb_bpm_torque_B[2] = rtb_omega_B_ISS_B[2] * rtb_Product_p; - - // Product: '/Product1' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant4' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // Gain: '/Gain1' - // Sum: '/Add3' - - rtb_Product_p = (sim_model_lib0_P->tun_bpm_noise_on_flag * - sim_model_lib0_P->bpm_impeller_inertia_error + - sim_model_lib0_P->bpm_impeller_inertia) * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_e; - - // Outputs for Enabled SubSystem: '/latch_nozzle_thrust_matricies' - - // UnitDelay: '/Unit Delay' - latch_nozzle_thrust_matrici(sim_model_lib0_DW->UnitDelay_DSTATE_c, - rtb_V_B_ISS_ISS, &sim_model_lib0_B->latch_nozzle_thrust_matricies_p, - (P_latch_nozzle_thrust_matrici_T *) - &sim_model_lib0_P->latch_nozzle_thrust_matricies_p, - sim_model_lib0_P->bpm_PM2_Q_nozzle2misaligned, - sim_model_lib0_P->abp_PM2_P_nozzle_B_B, - sim_model_lib0_P->abp_PM2_nozzle_orientations, - sim_model_lib0_P->bpm_PM2_P_nozzle_B_B_error, - sim_model_lib0_P->abp_P_CG_B_B_error, - sim_model_lib0_P->tun_bpm_noise_on_flag); - - // End of Outputs for SubSystem: '/latch_nozzle_thrust_matricies' - - // Outputs for Atomic SubSystem: '/calc_nozzle_area' - sim_model__calc_nozzle_area(sim_model_lib0_B->servo_model_f.Gain1, - &sim_model_lib0_B->calc_nozzle_area_c, - sim_model_lib0_P->abp_nozzle_flap_count, - sim_model_lib0_P->abp_nozzle_min_open_angle, - sim_model_lib0_P->abp_nozzle_flap_length, - sim_model_lib0_P->abp_nozzle_intake_height, - sim_model_lib0_P->abp_PM2_nozzle_widths, - sim_model_lib0_P->abp_nozzle_gear_ratio); - - // End of Outputs for SubSystem: '/calc_nozzle_area' - - // Outputs for Atomic SubSystem: '/blower_aerodynamics' - - // DiscreteIntegrator: '/Discrete-Time Integrator' - sim_m_blower_aerodynamics_j(sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_e, - sim_model_lib0_B->calc_nozzle_area_c.Product2, - &sim_model_lib0_B->blower_aerodynamics_j, - &sim_model_lib0_DW->blower_aerodynamics_j, (P_blower_aerodynamics_sim_m_m_T *) - &sim_model_lib0_P->blower_aerodynamics_j, - sim_model_lib0_P->abp_pm2_zero_thrust_area, - sim_model_lib0_P->bpm_PM2_zero_thrust_area_error, - sim_model_lib0_P->tun_bpm_noise_on_flag, - sim_model_lib0_P->abp_PM2_discharge_coeff, - sim_model_lib0_P->bpm_PM2_nozzle_discharge_coeff_error, - sim_model_lib0_P->bpm_lookup_Cdp_data, - sim_model_lib0_P->bpm_lookup_totalarea_breakpoints, - sim_model_lib0_P->abp_impeller_diameter, sim_model_lib0_P->const_air_density, - sim_model_lib0_P->tun_bpm_PM2_thrust_error_sf, - sim_model_lib0_P->bpm_PM2_nozzle_noise_feedback_gain); - - // End of Outputs for SubSystem: '/blower_aerodynamics' - - // SignalConversion: '/TmpSignal ConversionAtProductInport1' incorporates: - // BusCreator: '/Bus Creator' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Product: '/Product' - - tmp_9[0] = (real32_T)sim_model_lib0_P->Constant3_Value_c; - tmp_9[1] = rtb_bpm_force_B[2]; - tmp_9[2] = sim_model_lib0_P->Gain_Gain_ib * rtb_bpm_force_B[1]; - tmp_9[3] = sim_model_lib0_P->Gain1_Gain_ak * rtb_bpm_force_B[2]; - tmp_9[4] = (real32_T)sim_model_lib0_P->Constant3_Value_c; - tmp_9[5] = rtb_bpm_force_B[0]; - tmp_9[6] = rtb_bpm_force_B[1]; - tmp_9[7] = sim_model_lib0_P->Gain2_Gain_al * rtb_bpm_force_B[0]; - tmp_9[8] = (real32_T)sim_model_lib0_P->Constant3_Value_c; - for (i_0 = 0; i_0 < 3; i_0++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - tmp[i_0] = 0.0F; - - // Product: '/Product3' incorporates: - // Sum: '/Add1' - - tmp_a[i_0] = 0.0F; - - // Product: '/Product3' incorporates: - // Sum: '/Add1' - - tmp_b[i_0] = 0.0F; - for (i = 0; i < 6; i++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - tmp[i_0] += - sim_model_lib0_B->latch_nozzle_thrust_matricies_p.OutportBufferForthrust2torque_B - [(int32_T)((int32_T)(3 * i) + i_0)] * - sim_model_lib0_B->blower_aerodynamics_j.Add1[i]; - - // Product: '/Product3' incorporates: - // Sum: '/Add1' - - tmp_a[i_0] += - sim_model_lib0_B->latch_nozzle_thrust_matricies.OutportBufferForthrust2force_B - [(int32_T)((int32_T)(3 * i) + i_0)] * - sim_model_lib0_B->blower_aerodynamics.Add1[i]; - - // Product: '/Product3' incorporates: - // Sum: '/Add1' - - tmp_b[i_0] += - sim_model_lib0_B->latch_nozzle_thrust_matricies_p.OutportBufferForthrust2force_B - [(int32_T)((int32_T)(3 * i) + i_0)] * - sim_model_lib0_B->blower_aerodynamics_j.Add1[i]; - } - - // Sum: '/Add1' - rtb_bpm_force_B[i_0] = tmp_a[i_0] + tmp_b[i_0]; - - // Sum: '/Add' incorporates: - // Product: '/Product2' - // Product: '/Product' - // Sum: '/Add2' - - rtb_bpm_torque_B[i_0] = ((((rtb_omega_B_ISS_B[0] * rtb_Product_p * tmp_9[i_0] - + tmp_9[(int32_T)(i_0 + 3)] * (rtb_omega_B_ISS_B[1] * rtb_Product_p)) + - tmp_9[(int32_T)(i_0 + 6)] * (rtb_omega_B_ISS_B[2] * rtb_Product_p)) + - rtb_bpm_torque_B[i_0]) + tmp[i_0]) + rtb_A_B_ISS_ISS[i_0]; - } - - for (i = 0; i < 6; i++) { - // SignalConversion: '/ConcatBufferAtVector Concatenate5In1' - rtb_bpm_nozzle_theta[i] = sim_model_lib0_B->calc_nozzle_area.Gain12[i]; - - // SignalConversion: '/ConcatBufferAtVector Concatenate5In2' - rtb_bpm_nozzle_theta[(int32_T)(i + 6)] = - sim_model_lib0_B->calc_nozzle_area_c.Gain12[i]; - } - - // Outport: '/bpm_msg' incorporates: - // BusCreator: '/Bus Creator1' - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn2' - - sim_model_lib0_Y_bpm_msg_h->bpm_timestamp_sec = rtb_Switch1; - sim_model_lib0_Y_bpm_msg_h->bpm_timestamp_nsec = rtb_Switch_g; - sim_model_lib0_Y_bpm_msg_h->bpm_motor_curr[0] = rtb_bpm_motor_curr_idx_0; - sim_model_lib0_Y_bpm_msg_h->bpm_motor_curr[1] = - sim_model_lib0_B->dc_motor_model_g.current; - sim_model_lib0_Y_bpm_msg_h->bpm_torque_B[0] = rtb_bpm_torque_B[0]; - sim_model_lib0_Y_bpm_msg_h->bpm_force_B[0] = rtb_bpm_force_B[0]; - sim_model_lib0_Y_bpm_msg_h->bpm_torque_B[1] = rtb_bpm_torque_B[1]; - sim_model_lib0_Y_bpm_msg_h->bpm_force_B[1] = rtb_bpm_force_B[1]; - sim_model_lib0_Y_bpm_msg_h->bpm_torque_B[2] = rtb_bpm_torque_B[2]; - sim_model_lib0_Y_bpm_msg_h->bpm_force_B[2] = rtb_bpm_force_B[2]; - sim_model_lib0_Y_bpm_msg_h->bpm_motor_speed[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTAT_dp; - sim_model_lib0_Y_bpm_msg_h->bpm_motor_speed[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_e; - for (i = 0; i < 12; i++) { - sim_model_lib0_Y_bpm_msg_h->bpm_servo_curr[i] = rtb_bpm_servo_curr[i]; - sim_model_lib0_Y_bpm_msg_h->bpm_nozzle_theta[i] = rtb_bpm_nozzle_theta[i]; - } - - // End of Outport: '/bpm_msg' - - // DiscretePulseGenerator: '/AR_pulse_gen' - rtb_Sum = ((real_T)sim_model_lib0_DW->clockTickCounter < - sim_model_lib0_P->AR_pulse_gen_Duty) && - (sim_model_lib0_DW->clockTickCounter >= 0) ? - sim_model_lib0_P->AR_pulse_gen_Amp : 0.0; - if ((real_T)sim_model_lib0_DW->clockTickCounter >= floor - (sim_model_lib0_P->cvs_AR_process_time / - sim_model_lib0_P->astrobee_time_step_size) - 1.0) { - sim_model_lib0_DW->clockTickCounter = 0; - } else { - sim_model_lib0_DW->clockTickCounter++; - } - - // End of DiscretePulseGenerator: '/AR_pulse_gen' - - // DataTypeConversion: '/Data Type Conversion2' - rtb_Product = floor(rtb_Sum); - if (rtIsNaN(rtb_Product) || rtIsInf(rtb_Product)) { - rtb_Product = 0.0; - } else { - rtb_Product = fmod(rtb_Product, 256.0); - } - - // DiscretePulseGenerator: '/landmark_pulse_gen' - rtb_Sum = ((real_T)sim_model_lib0_DW->clockTickCounter_c < - sim_model_lib0_P->landmark_pulse_gen_Duty) && - (sim_model_lib0_DW->clockTickCounter_c >= 0) ? - sim_model_lib0_P->landmark_pulse_gen_Amp : 0.0; - if ((real_T)sim_model_lib0_DW->clockTickCounter_c >= floor - (sim_model_lib0_P->cvs_landmark_process_time / - sim_model_lib0_P->astrobee_time_step_size) - 1.0) { - sim_model_lib0_DW->clockTickCounter_c = 0; - } else { - sim_model_lib0_DW->clockTickCounter_c++; - } - - // End of DiscretePulseGenerator: '/landmark_pulse_gen' - - // DataTypeConversion: '/Data Type Conversion1' - y = floor(rtb_Sum); - if (rtIsNaN(y) || rtIsInf(y)) { - y = 0.0; - } else { - y = fmod(y, 256.0); - } - - // RateTransition: '/Rate Transition10' - if (sim_model_lib0_M->Timing.RateInteraction.TID0_3 == 1) { - sim_model_lib0_B->RateTransition10 = - sim_model_lib0_DW->RateTransition10_Buffer0; - - // RateTransition: '/Rate Transition3' - sim_model_lib0_B->RateTransition3_j = - sim_model_lib0_DW->RateTransition3_Buffer0_g; - - // RateTransition: '/Rate Transition1' - sim_model_lib0_B->RateTransition1_j = - sim_model_lib0_DW->RateTransition1_Buffer0_b; - - // RateTransition: '/Rate Transition6' - memcpy(&sim_model_lib0_B->RateTransition6[0], - &sim_model_lib0_DW->RateTransition6_Buffer0[0], (uint32_T)(1600U * - sizeof(real32_T))); - - // RateTransition: '/Rate Transition7' - memcpy(&sim_model_lib0_B->RateTransition7_f[0], - &sim_model_lib0_DW->RateTransition7_Buffer0_a[0], (uint32_T)(800U * - sizeof(uint8_T))); - - // RateTransition: '/Rate Transition8' - memcpy(&sim_model_lib0_B->RateTransition8[0], - &sim_model_lib0_DW->RateTransition8_Buffer0[0], (uint32_T)(50U * - sizeof(real32_T))); - } - - // End of RateTransition: '/Rate Transition10' - - // RateTransition: '/Rate Transition3' - if (sim_model_lib0_M->Timing.RateInteraction.TID0_4 == 1) { - sim_model_lib0_B->RateTransition3_o = - sim_model_lib0_DW->RateTransition3_Buffer0_o; - - // RateTransition: '/Rate Transition1' - sim_model_lib0_B->RateTransition1_a = - sim_model_lib0_DW->RateTransition1_Buffer0_l; - - // RateTransition: '/Rate Transition5' - memcpy(&sim_model_lib0_B->RateTransition5_b[0], - &sim_model_lib0_DW->RateTransition5_Buffer0_i[0], (uint32_T)(150U * - sizeof(real32_T))); - - // RateTransition: '/Rate Transition6' - memcpy(&sim_model_lib0_B->RateTransition6_n[0], - &sim_model_lib0_DW->RateTransition6_Buffer0_h[0], (uint32_T)(100U * - sizeof(real32_T))); - - // RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_B->RateTransition7_g[i] = - sim_model_lib0_DW->RateTransition7_Buffer0_an[i]; - } - - // End of RateTransition: '/Rate Transition7' - } - - // End of RateTransition: '/Rate Transition3' - - // BusCreator: '/bus_creator1' incorporates: - // Constant: '/cvs_3d_knowledge_flag' - // Constant: '/cvs_handrail_local_pos' - // Constant: '/cvs_handrail_local_quat' - // Constant: '/cvs_handrail_update_global_pose_flag' - - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_timestamp_sec = - sim_model_lib0_B->RateTransition3; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_timestamp_nsec = - sim_model_lib0_B->RateTransition1; - memcpy(&sim_model_lib0_Y_cvs_handrail_msg_h->cvs_landmarks[0], - &sim_model_lib0_B->RateTransition5[0], (uint32_T)(150U * sizeof - (real32_T))); - memcpy(&sim_model_lib0_Y_cvs_handrail_msg_h->cvs_observations[0], - &sim_model_lib0_B->RateTransition9[0], (uint32_T)(150U * sizeof - (real32_T))); - for (i = 0; i < 50; i++) { - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_valid_flag[i] = - sim_model_lib0_B->RateTransition7[i]; - } - - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_3d_knowledge_flag = - sim_model_lib0_P->cvs_3d_knowledge_flag_Value; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_handrail_local_pos[0] = - sim_model_lib0_P->cvs_handrail_local_pos_Value[0]; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_handrail_local_pos[1] = - sim_model_lib0_P->cvs_handrail_local_pos_Value[1]; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_handrail_local_pos[2] = - sim_model_lib0_P->cvs_handrail_local_pos_Value[2]; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_handrail_local_quat[0] = - sim_model_lib0_P->cvs_handrail_local_quat_Value[0]; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_handrail_local_quat[1] = - sim_model_lib0_P->cvs_handrail_local_quat_Value[1]; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_handrail_local_quat[2] = - sim_model_lib0_P->cvs_handrail_local_quat_Value[2]; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_handrail_local_quat[3] = - sim_model_lib0_P->cvs_handrail_local_quat_Value[3]; - sim_model_lib0_Y_cvs_handrail_msg_h->cvs_handrail_update_global_pose_flag = - sim_model_lib0_P->cvs_handrail_update_global_pose; - - // End of BusCreator: '/bus_creator1' - - // Outport: '/cvs_landmark_msg' incorporates: - // BusCreator: '/Bus Creator6' - - sim_model_lib0_Y_cvs_landmark_msg_n->cvs_timestamp_sec = - sim_model_lib0_B->RateTransition3_o; - sim_model_lib0_Y_cvs_landmark_msg_n->cvs_timestamp_nsec = - sim_model_lib0_B->RateTransition1_a; - memcpy(&sim_model_lib0_Y_cvs_landmark_msg_n->cvs_landmarks[0], - &sim_model_lib0_B->RateTransition5_b[0], (uint32_T)(150U * sizeof - (real32_T))); - memcpy(&sim_model_lib0_Y_cvs_landmark_msg_n->cvs_observations[0], - &sim_model_lib0_B->RateTransition6_n[0], (uint32_T)(100U * sizeof - (real32_T))); - for (i = 0; i < 50; i++) { - sim_model_lib0_Y_cvs_landmark_msg_n->cvs_valid_flag[i] = - sim_model_lib0_B->RateTransition7_g[i]; - } - - // End of Outport: '/cvs_landmark_msg' - - // RateTransition: '/Rate Transition3' - if (sim_model_lib0_M->Timing.RateInteraction.TID0_1 == 1) { - sim_model_lib0_B->RateTransition3_e = - sim_model_lib0_DW->RateTransition3_Buffer0_n; - - // RateTransition: '/Rate Transition1' - sim_model_lib0_B->RateTransition1_b = - sim_model_lib0_DW->RateTransition1_Buffer0_i; - - // RateTransition: '/Rate Transition5' - memcpy(&sim_model_lib0_B->RateTransition5_p[0], - &sim_model_lib0_DW->RateTransition5_Buffer0_f[0], (uint32_T)(150U * - sizeof(real32_T))); - - // RateTransition: '/Rate Transition6' - memcpy(&sim_model_lib0_B->RateTransition6_p[0], - &sim_model_lib0_DW->RateTransition6_Buffer0_e[0], (uint32_T)(100U * - sizeof(real32_T))); - - // RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_B->RateTransition7_k[i] = - sim_model_lib0_DW->RateTransition7_Buffer0_j[i]; - } - - // End of RateTransition: '/Rate Transition7' - } - - // End of RateTransition: '/Rate Transition3' - - // BusCreator: '/Bus Creator2' - sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_timestamp_sec = - sim_model_lib0_B->RateTransition3_j; - sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_timestamp_nsec = - sim_model_lib0_B->RateTransition1_j; - memcpy(&sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_observations[0], - &sim_model_lib0_B->RateTransition6[0], (uint32_T)(1600U * sizeof - (real32_T))); - memcpy(&sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_valid_flag[0], - &sim_model_lib0_B->RateTransition7_f[0], (uint32_T)(800U * sizeof - (uint8_T))); - memcpy(&sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_id_tag[0], - &sim_model_lib0_B->RateTransition8[0], (uint32_T)(50U * sizeof(real32_T))); - - // DiscretePulseGenerator: '/handrail_pulse_gen' - rtb_Sum = ((real_T)sim_model_lib0_DW->clockTickCounter_n < - sim_model_lib0_P->handrail_pulse_gen_Duty) && - (sim_model_lib0_DW->clockTickCounter_n >= 0) ? - sim_model_lib0_P->handrail_pulse_gen_Amp : 0.0; - if ((real_T)sim_model_lib0_DW->clockTickCounter_n >= floor - (sim_model_lib0_P->cvs_handrail_process_time / - sim_model_lib0_P->astrobee_time_step_size) - 1.0) { - sim_model_lib0_DW->clockTickCounter_n = 0; - } else { - sim_model_lib0_DW->clockTickCounter_n++; - } - - // End of DiscretePulseGenerator: '/handrail_pulse_gen' - - // DataTypeConversion: '/Data Type Conversion3' - y_0 = floor(rtb_Sum); - if (rtIsNaN(y_0) || rtIsInf(y_0)) { - y_0 = 0.0; - } else { - y_0 = fmod(y_0, 256.0); - } - - // Outport: '/cvs_registration_pulse' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // DataTypeConversion: '/Data Type Conversion2' - - sim_model_lib0_Y_cvs_registration_pulse_d->cvs_ar_tag_pulse = (uint8_T) - (rtb_Product < 0.0 ? (int32_T)(uint8_T)(int32_T)-(int32_T)(int8_T)(uint8_T) - -rtb_Product : (int32_T)(uint8_T)rtb_Product); - sim_model_lib0_Y_cvs_registration_pulse_d->cvs_landmark_pulse = (uint8_T)(y < - 0.0 ? (int32_T)(uint8_T)(int32_T)-(int32_T)(int8_T)(uint8_T)-y : (int32_T) - (uint8_T)y); - - // Switch: '/Switch3' incorporates: - // Logic: '/Logical Operator' - // RelationalOperator: '/FixPt Relational Operator' - // RelationalOperator: '/FixPt Relational Operator' - // UnitDelay: '/Delay Input1' - // UnitDelay: '/Delay Input1' - - if ((sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_timestamp_sec != - sim_model_lib0_DW->DelayInput1_DSTATE_p) || - (sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_timestamp_nsec != - sim_model_lib0_DW->DelayInput1_DSTATE_l)) { - // Outport: '/cvs_registration_pulse' - sim_model_lib0_Y_cvs_registration_pulse_d->cvs_optical_flow_pulse = - sim_model_lib0_B->RateTransition10; - } else { - // DataTypeConversion: '/Data Type Conversion' incorporates: - // Constant: '/Constant1' - - rtb_Product = floor(sim_model_lib0_P->Constant1_Value_ot); - if (rtIsNaN(rtb_Product) || rtIsInf(rtb_Product)) { - rtb_Product = 0.0; - } else { - rtb_Product = fmod(rtb_Product, 256.0); - } - - // Outport: '/cvs_registration_pulse' incorporates: - // DataTypeConversion: '/Data Type Conversion' - - sim_model_lib0_Y_cvs_registration_pulse_d->cvs_optical_flow_pulse = (uint8_T) - (rtb_Product < 0.0 ? (int32_T)(uint8_T)(int32_T)-(int32_T)(int8_T)(uint8_T) - -rtb_Product : (int32_T)(uint8_T)rtb_Product); - } - - // End of Switch: '/Switch3' - - // Outport: '/cvs_registration_pulse' incorporates: - // DataTypeConversion: '/Data Type Conversion3' - - sim_model_lib0_Y_cvs_registration_pulse_d->cvs_handrail_pulse = (uint8_T)(y_0 < - 0.0 ? (int32_T)(uint8_T)(int32_T)-(int32_T)(int8_T)(uint8_T)-y_0 : (int32_T) - (uint8_T)y_0); - - // Outport: '/cvs_ar_tag_msg' incorporates: - // BusCreator: '/Bus Creator1' - - sim_model_lib0_Y_cvs_ar_tag_msg->cvs_timestamp_sec = - sim_model_lib0_B->RateTransition3_e; - sim_model_lib0_Y_cvs_ar_tag_msg->cvs_timestamp_nsec = - sim_model_lib0_B->RateTransition1_b; - memcpy(&sim_model_lib0_Y_cvs_ar_tag_msg->cvs_landmarks[0], - &sim_model_lib0_B->RateTransition5_p[0], (uint32_T)(150U * sizeof - (real32_T))); - memcpy(&sim_model_lib0_Y_cvs_ar_tag_msg->cvs_observations[0], - &sim_model_lib0_B->RateTransition6_p[0], (uint32_T)(100U * sizeof - (real32_T))); - for (i = 0; i < 50; i++) { - sim_model_lib0_Y_cvs_ar_tag_msg->cvs_valid_flag[i] = - sim_model_lib0_B->RateTransition7_k[i]; - } - - // End of Outport: '/cvs_ar_tag_msg' - - // Outport: '/ex_time_msg' incorporates: - // BusCreator: '/Bus Creator1' - - sim_model_lib0_Y_ex_time_msg_m->timestamp_sec = rtb_Switch1; - sim_model_lib0_Y_ex_time_msg_m->timestamp_nsec = rtb_Switch_g; - - // Logic: '/Logical Operator2' incorporates: - // BusCreator: '/Bus Creator1' - // Constant: '/Constant2' - // Constant: '/Constant' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator1' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Compare' - // Selector: '/Select_col_1' - // Selector: '/Select_col_2' - - for (i_0 = 0; i_0 < 12; i_0++) { - rtb_LogicalOperator2[i_0] = ((sim_model_lib0_P->mlp_command_times[i_0] != - sim_model_lib0_P->Constant_Value_n) && - ((sim_model_lib0_P->mlp_command_times[i_0] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_command_times[i_0] == rtb_Switch1) && - (rtb_Switch_g >= sim_model_lib0_P->mlp_command_times[(int32_T)(12 + i_0)])))); - } - - // End of Logic: '/Logical Operator2' - - // Sum: '/Sum1' - rtb_Switch1_p_idx_0 = (uint32_T)rtb_LogicalOperator2[0]; - for (i_0 = 0; i_0 < 11; i_0++) { - rtb_Switch1_p_idx_0 += (uint32_T)rtb_LogicalOperator2[(int32_T)(i_0 + 1)]; - } - - rtb_Bias = (uint8_T)rtb_Switch1_p_idx_0; - - // End of Sum: '/Sum1' - - // Saturate: '/Saturation' - if (rtb_Bias > sim_model_lib0_P->Saturation_UpperSat) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_i = - sim_model_lib0_P->Saturation_UpperSat; - } else if (rtb_Bias < sim_model_lib0_P->Saturation_LowerSat) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_i = - sim_model_lib0_P->Saturation_LowerSat; - } else { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_i = rtb_Bias; - } - - // End of Saturate: '/Saturation' - - // Sum: '/Sum1' incorporates: - // BusCreator: '/Bus Creator1' - // Constant: '/Constant3' - // Constant: '/Constant' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Compare' - // Selector: '/Select_col_1' - // Selector: '/Select_col_2' - - rtb_Switch1_p_idx_0 = (uint32_T)((uint32_T)((uint32_T) - ((sim_model_lib0_P->mlp_mode_cmd_times[0] != - sim_model_lib0_P->Constant_Value_j) && - ((sim_model_lib0_P->mlp_mode_cmd_times[0] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_mode_cmd_times[0] == rtb_Switch1) && (rtb_Switch_g - >= sim_model_lib0_P->mlp_mode_cmd_times[3])))) + (uint32_T) - ((sim_model_lib0_P->mlp_mode_cmd_times[1] != - sim_model_lib0_P->Constant_Value_j) && - ((sim_model_lib0_P->mlp_mode_cmd_times[1] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_mode_cmd_times[1] == rtb_Switch1) && (rtb_Switch_g - >= sim_model_lib0_P->mlp_mode_cmd_times[4]))))) + (uint32_T) - ((sim_model_lib0_P->mlp_mode_cmd_times[2] != - sim_model_lib0_P->Constant_Value_j) && - ((sim_model_lib0_P->mlp_mode_cmd_times[2] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_mode_cmd_times[2] == rtb_Switch1) && (rtb_Switch_g - >= sim_model_lib0_P->mlp_mode_cmd_times[5]))))); - - // Saturate: '/Saturation' incorporates: - // Sum: '/Sum1' - - if ((uint8_T)rtb_Switch1_p_idx_0 > sim_model_lib0_P->Saturation_UpperSat_b) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_if = - sim_model_lib0_P->Saturation_UpperSat_b; - } else if ((uint8_T)rtb_Switch1_p_idx_0 < - sim_model_lib0_P->Saturation_LowerSat_e) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_if = - sim_model_lib0_P->Saturation_LowerSat_e; - } else { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_if = (uint8_T)rtb_Switch1_p_idx_0; - } - - // End of Saturate: '/Saturation' - - // Sum: '/Sum1' incorporates: - // BusCreator: '/Bus Creator1' - // Constant: '/Constant5' - // Constant: '/Constant' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Compare' - // Selector: '/Select_col_1' - // Selector: '/Select_col_2' - - rtb_Switch1_p_idx_0 = (uint32_T)((uint32_T)((uint32_T) - ((sim_model_lib0_P->mlp_loc_mode_cmd_times[0] != - sim_model_lib0_P->Constant_Value_m) && - ((sim_model_lib0_P->mlp_loc_mode_cmd_times[0] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_loc_mode_cmd_times[0] == rtb_Switch1) && - (rtb_Switch_g >= sim_model_lib0_P->mlp_loc_mode_cmd_times[3])))) + - (uint32_T)((sim_model_lib0_P->mlp_loc_mode_cmd_times[1] != - sim_model_lib0_P->Constant_Value_m) && - ((sim_model_lib0_P->mlp_loc_mode_cmd_times[1] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_loc_mode_cmd_times[1] == rtb_Switch1) && - (rtb_Switch_g >= sim_model_lib0_P->mlp_loc_mode_cmd_times[4]))))) - + (uint32_T)((sim_model_lib0_P->mlp_loc_mode_cmd_times[2] != - sim_model_lib0_P->Constant_Value_m) && - ((sim_model_lib0_P->mlp_loc_mode_cmd_times[2] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_loc_mode_cmd_times[2] == rtb_Switch1) && - (rtb_Switch_g >= sim_model_lib0_P->mlp_loc_mode_cmd_times[5]))))); - - // Saturate: '/Saturation' incorporates: - // Sum: '/Sum1' - - if ((uint8_T)rtb_Switch1_p_idx_0 > sim_model_lib0_P->Saturation_UpperSat_d) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_ht = - sim_model_lib0_P->Saturation_UpperSat_d; - } else if ((uint8_T)rtb_Switch1_p_idx_0 < - sim_model_lib0_P->Saturation_LowerSat_j) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_ht = - sim_model_lib0_P->Saturation_LowerSat_j; - } else { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_ht = (uint8_T)rtb_Switch1_p_idx_0; - } - - // End of Saturate: '/Saturation' - - // Sum: '/Sum1' incorporates: - // BusCreator: '/Bus Creator1' - // Constant: '/Constant7' - // Constant: '/Constant' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Compare' - // Selector: '/Select_col_1' - // Selector: '/Select_col_2' - - rtb_Switch1_p_idx_0 = (uint32_T)((uint32_T)((uint32_T) - ((sim_model_lib0_P->mlp_speed_gain_cmd_times[0] != - sim_model_lib0_P->Constant_Value_nc) && - ((sim_model_lib0_P->mlp_speed_gain_cmd_times[0] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_speed_gain_cmd_times[0] == rtb_Switch1) && - (rtb_Switch_g >= sim_model_lib0_P->mlp_speed_gain_cmd_times[3])))) + - (uint32_T)((sim_model_lib0_P->mlp_speed_gain_cmd_times[1] != - sim_model_lib0_P->Constant_Value_nc) && - ((sim_model_lib0_P->mlp_speed_gain_cmd_times[1] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_speed_gain_cmd_times[1] == rtb_Switch1) && - (rtb_Switch_g >= sim_model_lib0_P->mlp_speed_gain_cmd_times[4]))))) - + (uint32_T)((sim_model_lib0_P->mlp_speed_gain_cmd_times[2] != - sim_model_lib0_P->Constant_Value_nc) && - ((sim_model_lib0_P->mlp_speed_gain_cmd_times[2] < rtb_Switch1) || - ((sim_model_lib0_P->mlp_speed_gain_cmd_times[2] == rtb_Switch1) - && (rtb_Switch_g >= - sim_model_lib0_P->mlp_speed_gain_cmd_times[5]))))); - - // RateTransition: '/Rate Transition2' incorporates: - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - if (sim_model_lib0_M->Timing.RateInteraction.TID0_1 == 1) { - sim_model_lib0_B->RateTransition2.P_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0]; - sim_model_lib0_B->RateTransition2.V_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0]; - sim_model_lib0_B->RateTransition2.A_B_ISS_ISS[0] = rtb_P_B_ISS_SS[0]; - sim_model_lib0_B->RateTransition2.A_B_ISS_B[0] = rtb_Merge_o[0]; - sim_model_lib0_B->RateTransition2.A_B_ECI_B[0] = rtb_imu_gyro_bias[0]; - sim_model_lib0_B->RateTransition2.P_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1]; - sim_model_lib0_B->RateTransition2.V_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1]; - sim_model_lib0_B->RateTransition2.A_B_ISS_ISS[1] = rtb_P_B_ISS_SS[1]; - sim_model_lib0_B->RateTransition2.A_B_ISS_B[1] = rtb_Merge_o[1]; - sim_model_lib0_B->RateTransition2.A_B_ECI_B[1] = rtb_imu_gyro_bias[1]; - sim_model_lib0_B->RateTransition2.P_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2]; - sim_model_lib0_B->RateTransition2.V_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2]; - sim_model_lib0_B->RateTransition2.A_B_ISS_ISS[2] = rtb_Merge_c; - sim_model_lib0_B->RateTransition2.A_B_ISS_B[2] = rtb_Merge_o[2]; - sim_model_lib0_B->RateTransition2.A_B_ECI_B[2] = rtb_imu_gyro_bias[2]; - sim_model_lib0_B->RateTransition2.Q_ISS2B[0] = rtb_Merge_m[0]; - sim_model_lib0_B->RateTransition2.Q_ISS2B[1] = rtb_Merge_m[1]; - sim_model_lib0_B->RateTransition2.Q_ISS2B[2] = rtb_Merge_m[2]; - sim_model_lib0_B->RateTransition2.Q_ISS2B[3] = rtb_Merge_m[3]; - sim_model_lib0_B->RateTransition2.omega_B_ISS_B[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - sim_model_lib0_B->RateTransition2.omega_B_ECI_B[0] = rtb_Sum2_f[0]; - sim_model_lib0_B->RateTransition2.alpha_B_ISS_B[0] = rtb_alpha_B_ECI_B[0]; - sim_model_lib0_B->RateTransition2.fan_torques_B[0] = - rtb_Delay1_bpm_torque_B[0]; - sim_model_lib0_B->RateTransition2.fan_forces_B[0] = - rtb_TrigonometricFunction1_c; - sim_model_lib0_B->RateTransition2.omega_B_ISS_B[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - sim_model_lib0_B->RateTransition2.omega_B_ECI_B[1] = rtb_Sum2_f[1]; - sim_model_lib0_B->RateTransition2.alpha_B_ISS_B[1] = rtb_alpha_B_ECI_B[1]; - sim_model_lib0_B->RateTransition2.fan_torques_B[1] = - rtb_Delay1_bpm_torque_B[1]; - sim_model_lib0_B->RateTransition2.fan_forces_B[1] = - rtb_Delay1_bpm_force_B_idx_1; - sim_model_lib0_B->RateTransition2.omega_B_ISS_B[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - sim_model_lib0_B->RateTransition2.omega_B_ECI_B[2] = rtb_Sum2_f[2]; - sim_model_lib0_B->RateTransition2.alpha_B_ISS_B[2] = rtb_alpha_B_ECI_B[2]; - sim_model_lib0_B->RateTransition2.fan_torques_B[2] = - rtb_Delay1_bpm_torque_B[2]; - sim_model_lib0_B->RateTransition2.fan_forces_B[2] = - rtb_Delay1_bpm_force_B_idx_2; - - // RateTransition: '/Rate Transition4' incorporates: - // BusCreator: '/Bus Creator' - // BusCreator: '/Bus Creator1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - sim_model_lib0_B->RateTransition4.timestamp_sec = rtb_Switch1; - sim_model_lib0_B->RateTransition4.timestamp_nsec = rtb_Switch_g; - } - - // End of RateTransition: '/Rate Transition2' - - // RateTransition: '/Rate Transition2' incorporates: - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - if (sim_model_lib0_M->Timing.RateInteraction.TID0_2 == 1) { - sim_model_lib0_B->RateTransition2_o.P_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0]; - sim_model_lib0_B->RateTransition2_o.V_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0]; - sim_model_lib0_B->RateTransition2_o.A_B_ISS_ISS[0] = rtb_P_B_ISS_SS[0]; - sim_model_lib0_B->RateTransition2_o.A_B_ISS_B[0] = rtb_Merge_o[0]; - sim_model_lib0_B->RateTransition2_o.A_B_ECI_B[0] = rtb_imu_gyro_bias[0]; - sim_model_lib0_B->RateTransition2_o.P_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1]; - sim_model_lib0_B->RateTransition2_o.V_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1]; - sim_model_lib0_B->RateTransition2_o.A_B_ISS_ISS[1] = rtb_P_B_ISS_SS[1]; - sim_model_lib0_B->RateTransition2_o.A_B_ISS_B[1] = rtb_Merge_o[1]; - sim_model_lib0_B->RateTransition2_o.A_B_ECI_B[1] = rtb_imu_gyro_bias[1]; - sim_model_lib0_B->RateTransition2_o.P_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2]; - sim_model_lib0_B->RateTransition2_o.V_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2]; - sim_model_lib0_B->RateTransition2_o.A_B_ISS_ISS[2] = rtb_Merge_c; - sim_model_lib0_B->RateTransition2_o.A_B_ISS_B[2] = rtb_Merge_o[2]; - sim_model_lib0_B->RateTransition2_o.A_B_ECI_B[2] = rtb_imu_gyro_bias[2]; - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[0] = rtb_Merge_m[0]; - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[1] = rtb_Merge_m[1]; - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[2] = rtb_Merge_m[2]; - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[3] = rtb_Merge_m[3]; - sim_model_lib0_B->RateTransition2_o.omega_B_ISS_B[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - sim_model_lib0_B->RateTransition2_o.omega_B_ECI_B[0] = rtb_Sum2_f[0]; - sim_model_lib0_B->RateTransition2_o.alpha_B_ISS_B[0] = rtb_alpha_B_ECI_B[0]; - sim_model_lib0_B->RateTransition2_o.fan_torques_B[0] = - rtb_Delay1_bpm_torque_B[0]; - sim_model_lib0_B->RateTransition2_o.fan_forces_B[0] = - rtb_TrigonometricFunction1_c; - sim_model_lib0_B->RateTransition2_o.omega_B_ISS_B[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - sim_model_lib0_B->RateTransition2_o.omega_B_ECI_B[1] = rtb_Sum2_f[1]; - sim_model_lib0_B->RateTransition2_o.alpha_B_ISS_B[1] = rtb_alpha_B_ECI_B[1]; - sim_model_lib0_B->RateTransition2_o.fan_torques_B[1] = - rtb_Delay1_bpm_torque_B[1]; - sim_model_lib0_B->RateTransition2_o.fan_forces_B[1] = - rtb_Delay1_bpm_force_B_idx_1; - sim_model_lib0_B->RateTransition2_o.omega_B_ISS_B[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - sim_model_lib0_B->RateTransition2_o.omega_B_ECI_B[2] = rtb_Sum2_f[2]; - sim_model_lib0_B->RateTransition2_o.alpha_B_ISS_B[2] = rtb_alpha_B_ECI_B[2]; - sim_model_lib0_B->RateTransition2_o.fan_torques_B[2] = - rtb_Delay1_bpm_torque_B[2]; - sim_model_lib0_B->RateTransition2_o.fan_forces_B[2] = - rtb_Delay1_bpm_force_B_idx_2; - - // RateTransition: '/Rate Transition4' incorporates: - // BusCreator: '/Bus Creator' - // BusCreator: '/Bus Creator1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - sim_model_lib0_B->RateTransition4_m.timestamp_sec = rtb_Switch1; - sim_model_lib0_B->RateTransition4_m.timestamp_nsec = rtb_Switch_g; - } - - // End of RateTransition: '/Rate Transition2' - - // RateTransition: '/Rate Transition2' incorporates: - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - if (sim_model_lib0_M->Timing.RateInteraction.TID0_4 == 1) { - sim_model_lib0_B->RateTransition2_j.P_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0]; - sim_model_lib0_B->RateTransition2_j.V_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0]; - sim_model_lib0_B->RateTransition2_j.A_B_ISS_ISS[0] = rtb_P_B_ISS_SS[0]; - sim_model_lib0_B->RateTransition2_j.A_B_ISS_B[0] = rtb_Merge_o[0]; - sim_model_lib0_B->RateTransition2_j.A_B_ECI_B[0] = rtb_imu_gyro_bias[0]; - sim_model_lib0_B->RateTransition2_j.P_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1]; - sim_model_lib0_B->RateTransition2_j.V_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1]; - sim_model_lib0_B->RateTransition2_j.A_B_ISS_ISS[1] = rtb_P_B_ISS_SS[1]; - sim_model_lib0_B->RateTransition2_j.A_B_ISS_B[1] = rtb_Merge_o[1]; - sim_model_lib0_B->RateTransition2_j.A_B_ECI_B[1] = rtb_imu_gyro_bias[1]; - sim_model_lib0_B->RateTransition2_j.P_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2]; - sim_model_lib0_B->RateTransition2_j.V_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2]; - sim_model_lib0_B->RateTransition2_j.A_B_ISS_ISS[2] = rtb_Merge_c; - sim_model_lib0_B->RateTransition2_j.A_B_ISS_B[2] = rtb_Merge_o[2]; - sim_model_lib0_B->RateTransition2_j.A_B_ECI_B[2] = rtb_imu_gyro_bias[2]; - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[0] = rtb_Merge_m[0]; - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[1] = rtb_Merge_m[1]; - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[2] = rtb_Merge_m[2]; - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[3] = rtb_Merge_m[3]; - sim_model_lib0_B->RateTransition2_j.omega_B_ISS_B[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - sim_model_lib0_B->RateTransition2_j.omega_B_ECI_B[0] = rtb_Sum2_f[0]; - sim_model_lib0_B->RateTransition2_j.alpha_B_ISS_B[0] = rtb_alpha_B_ECI_B[0]; - sim_model_lib0_B->RateTransition2_j.fan_torques_B[0] = - rtb_Delay1_bpm_torque_B[0]; - sim_model_lib0_B->RateTransition2_j.fan_forces_B[0] = - rtb_TrigonometricFunction1_c; - sim_model_lib0_B->RateTransition2_j.omega_B_ISS_B[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - sim_model_lib0_B->RateTransition2_j.omega_B_ECI_B[1] = rtb_Sum2_f[1]; - sim_model_lib0_B->RateTransition2_j.alpha_B_ISS_B[1] = rtb_alpha_B_ECI_B[1]; - sim_model_lib0_B->RateTransition2_j.fan_torques_B[1] = - rtb_Delay1_bpm_torque_B[1]; - sim_model_lib0_B->RateTransition2_j.fan_forces_B[1] = - rtb_Delay1_bpm_force_B_idx_1; - sim_model_lib0_B->RateTransition2_j.omega_B_ISS_B[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - sim_model_lib0_B->RateTransition2_j.omega_B_ECI_B[2] = rtb_Sum2_f[2]; - sim_model_lib0_B->RateTransition2_j.alpha_B_ISS_B[2] = rtb_alpha_B_ECI_B[2]; - sim_model_lib0_B->RateTransition2_j.fan_torques_B[2] = - rtb_Delay1_bpm_torque_B[2]; - sim_model_lib0_B->RateTransition2_j.fan_forces_B[2] = - rtb_Delay1_bpm_force_B_idx_2; - - // RateTransition: '/Rate Transition4' incorporates: - // BusCreator: '/Bus Creator' - // BusCreator: '/Bus Creator1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - sim_model_lib0_B->RateTransition4_e.timestamp_sec = rtb_Switch1; - sim_model_lib0_B->RateTransition4_e.timestamp_nsec = rtb_Switch_g; - } - - // End of RateTransition: '/Rate Transition2' - - // RateTransition: '/Rate Transition2' incorporates: - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - if (sim_model_lib0_M->Timing.RateInteraction.TID0_3 == 1) { - sim_model_lib0_B->RateTransition2_b.P_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0]; - sim_model_lib0_B->RateTransition2_b.V_B_ISS_ISS[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0]; - sim_model_lib0_B->RateTransition2_b.A_B_ISS_ISS[0] = rtb_P_B_ISS_SS[0]; - sim_model_lib0_B->RateTransition2_b.A_B_ISS_B[0] = rtb_Merge_o[0]; - sim_model_lib0_B->RateTransition2_b.A_B_ECI_B[0] = rtb_imu_gyro_bias[0]; - sim_model_lib0_B->RateTransition2_b.P_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1]; - sim_model_lib0_B->RateTransition2_b.V_B_ISS_ISS[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1]; - sim_model_lib0_B->RateTransition2_b.A_B_ISS_ISS[1] = rtb_P_B_ISS_SS[1]; - sim_model_lib0_B->RateTransition2_b.A_B_ISS_B[1] = rtb_Merge_o[1]; - sim_model_lib0_B->RateTransition2_b.A_B_ECI_B[1] = rtb_imu_gyro_bias[1]; - sim_model_lib0_B->RateTransition2_b.P_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2]; - sim_model_lib0_B->RateTransition2_b.V_B_ISS_ISS[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2]; - sim_model_lib0_B->RateTransition2_b.A_B_ISS_ISS[2] = rtb_Merge_c; - sim_model_lib0_B->RateTransition2_b.A_B_ISS_B[2] = rtb_Merge_o[2]; - sim_model_lib0_B->RateTransition2_b.A_B_ECI_B[2] = rtb_imu_gyro_bias[2]; - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[0] = rtb_Merge_m[0]; - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[1] = rtb_Merge_m[1]; - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[2] = rtb_Merge_m[2]; - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[3] = rtb_Merge_m[3]; - sim_model_lib0_B->RateTransition2_b.omega_B_ISS_B[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0]; - sim_model_lib0_B->RateTransition2_b.omega_B_ECI_B[0] = rtb_Sum2_f[0]; - sim_model_lib0_B->RateTransition2_b.alpha_B_ISS_B[0] = rtb_alpha_B_ECI_B[0]; - sim_model_lib0_B->RateTransition2_b.fan_torques_B[0] = - rtb_Delay1_bpm_torque_B[0]; - sim_model_lib0_B->RateTransition2_b.fan_forces_B[0] = - rtb_TrigonometricFunction1_c; - sim_model_lib0_B->RateTransition2_b.omega_B_ISS_B[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1]; - sim_model_lib0_B->RateTransition2_b.omega_B_ECI_B[1] = rtb_Sum2_f[1]; - sim_model_lib0_B->RateTransition2_b.alpha_B_ISS_B[1] = rtb_alpha_B_ECI_B[1]; - sim_model_lib0_B->RateTransition2_b.fan_torques_B[1] = - rtb_Delay1_bpm_torque_B[1]; - sim_model_lib0_B->RateTransition2_b.fan_forces_B[1] = - rtb_Delay1_bpm_force_B_idx_1; - sim_model_lib0_B->RateTransition2_b.omega_B_ISS_B[2] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2]; - sim_model_lib0_B->RateTransition2_b.omega_B_ECI_B[2] = rtb_Sum2_f[2]; - sim_model_lib0_B->RateTransition2_b.alpha_B_ISS_B[2] = rtb_alpha_B_ECI_B[2]; - sim_model_lib0_B->RateTransition2_b.fan_torques_B[2] = - rtb_Delay1_bpm_torque_B[2]; - sim_model_lib0_B->RateTransition2_b.fan_forces_B[2] = - rtb_Delay1_bpm_force_B_idx_2; - - // RateTransition: '/Rate Transition4' incorporates: - // BusCreator: '/Bus Creator' - // BusCreator: '/Bus Creator1' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator1' - - sim_model_lib0_B->RateTransition4_f.timestamp_sec = rtb_Switch1; - sim_model_lib0_B->RateTransition4_f.timestamp_nsec = rtb_Switch_g; - } - - // End of RateTransition: '/Rate Transition2' - - // Sum: '/Add' - rtb_Sqrt_m = sim_model_lib0_B->dc_motor_model.output_torque - - sim_model_lib0_B->blower_aerodynamics.Constant7; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - // Gain: '/Gain' - // Gain: '/Gain2' - - if (sim_model_lib0_P->tun_bpm_noise_on_flag > - sim_model_lib0_P->Switch_Threshold) { - rtb_Sqrt_m *= 1.0F / sim_model_lib0_P->bpm_impeller_inertia; - } else { - rtb_Sqrt_m *= 1.0F / (sim_model_lib0_P->bpm_impeller_inertia + - sim_model_lib0_P->bpm_impeller_inertia_error); - } - - // End of Switch: '/Switch' - - // Sum: '/Add' - rtb_Product_p = sim_model_lib0_B->dc_motor_model_g.output_torque - - sim_model_lib0_B->blower_aerodynamics_j.Constant7; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - // Gain: '/Gain' - // Gain: '/Gain2' - - if (sim_model_lib0_P->tun_bpm_noise_on_flag > - sim_model_lib0_P->Switch_Threshold_o) { - rtb_Product_p *= 1.0F / sim_model_lib0_P->bpm_impeller_inertia; - } else { - rtb_Product_p *= 1.0F / (sim_model_lib0_P->bpm_impeller_inertia + - sim_model_lib0_P->bpm_impeller_inertia_error); - } - - // End of Switch: '/Switch' - - // Gain: '/Gain1' incorporates: - // RandomNumber: '/random_noise1' - - rtb_Sum = 1.0 / sqrt(sim_model_lib0_P->astrobee_time_step_size); - rtb_random_noise[0] = rtb_Sum * sim_model_lib0_DW->NextOutput_i[0]; - rtb_random_noise[1] = rtb_Sum * sim_model_lib0_DW->NextOutput_i[1]; - rtb_random_noise[2] = rtb_Sum * sim_model_lib0_DW->NextOutput_i[2]; - - // Gain: '/Gain4' - rtb_Sum = 1.0 / sqrt(sim_model_lib0_P->astrobee_time_step_size); - - // Saturate: '/Saturation' incorporates: - // Sum: '/Sum1' - - if ((uint8_T)rtb_Switch1_p_idx_0 > sim_model_lib0_P->Saturation_UpperSat_f) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_h = - sim_model_lib0_P->Saturation_UpperSat_f; - } else if ((uint8_T)rtb_Switch1_p_idx_0 < - sim_model_lib0_P->Saturation_LowerSat_k) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_h = - sim_model_lib0_P->Saturation_LowerSat_k; - } else { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_h = (uint8_T)rtb_Switch1_p_idx_0; - } - - // End of Saturate: '/Saturation' - - // Update for UnitDelay: '/Unit Delay1' - sim_model_lib0_DW->UnitDelay1_DSTATE = rtb_Switch1; - - // Constant: '/Constant' - rtb_Product = 1.0E+9 * sim_model_lib0_P->astrobee_time_step_size; - y = fabs(rtb_Product); - if (y < 4.503599627370496E+15) { - if (y >= 0.5) { - rtb_Product = floor(rtb_Product + 0.5); - } else { - rtb_Product *= 0.0; - } - } - - if (rtb_Product < 4.294967296E+9) { - if (rtb_Product >= 0.0) { - rtb_Switch1_p_idx_0 = (uint32_T)rtb_Product; - } else { - rtb_Switch1_p_idx_0 = 0U; - } - } else { - rtb_Switch1_p_idx_0 = MAX_uint32_T; - } - - // End of Constant: '/Constant' - - // Update for UnitDelay: '/Unit Delay' incorporates: - // Sum: '/Sum' - - sim_model_lib0_DW->UnitDelay_DSTATE = (uint32_T)(rtb_Switch1_p_idx_0 + - rtb_Switch_g); - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0] += - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_i * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainval * rtb_P_B_ISS_SS[0]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainva_p * rtb_alpha_B_ECI_B[0]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1] += - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_i * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainval * rtb_P_B_ISS_SS[1]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainva_p * rtb_alpha_B_ECI_B[1]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2] += - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_i * - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainval * rtb_Merge_c; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainva_p * rtb_alpha_B_ECI_B[2]; - - // Update for Delay: '/Delay' - sim_model_lib0_DW->icLoad = 0U; - sim_model_lib0_DW->Delay_DSTATE[0] = rtb_Merge_kq[0]; - sim_model_lib0_DW->Delay_DSTATE[1] = rtb_Merge_kq[1]; - sim_model_lib0_DW->Delay_DSTATE[2] = rtb_Merge_kq[2]; - sim_model_lib0_DW->Delay_DSTATE[3] = rtb_Merge_kq[3]; - - // Update for Delay: '/Delay1' incorporates: - // BusCreator: '/Bus Creator1' - // BusCreator: '/Bus Creator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // DiscreteIntegrator: '/Discrete-Time Integrator' - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn2' - - sim_model_lib0_DW->Delay1_DSTATE.bpm_timestamp_sec = rtb_Switch1; - sim_model_lib0_DW->Delay1_DSTATE.bpm_timestamp_nsec = rtb_Switch_g; - sim_model_lib0_DW->Delay1_DSTATE.bpm_motor_curr[0] = rtb_bpm_motor_curr_idx_0; - sim_model_lib0_DW->Delay1_DSTATE.bpm_motor_curr[1] = - sim_model_lib0_B->dc_motor_model_g.current; - sim_model_lib0_DW->Delay1_DSTATE.bpm_torque_B[0] = rtb_bpm_torque_B[0]; - sim_model_lib0_DW->Delay1_DSTATE.bpm_force_B[0] = rtb_bpm_force_B[0]; - sim_model_lib0_DW->Delay1_DSTATE.bpm_torque_B[1] = rtb_bpm_torque_B[1]; - sim_model_lib0_DW->Delay1_DSTATE.bpm_force_B[1] = rtb_bpm_force_B[1]; - sim_model_lib0_DW->Delay1_DSTATE.bpm_torque_B[2] = rtb_bpm_torque_B[2]; - sim_model_lib0_DW->Delay1_DSTATE.bpm_force_B[2] = rtb_bpm_force_B[2]; - sim_model_lib0_DW->Delay1_DSTATE.bpm_motor_speed[0] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTAT_dp; - sim_model_lib0_DW->Delay1_DSTATE.bpm_motor_speed[1] = - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_e; - for (i = 0; i < 12; i++) { - sim_model_lib0_DW->Delay1_DSTATE.bpm_servo_curr[i] = rtb_bpm_servo_curr[i]; - sim_model_lib0_DW->Delay1_DSTATE.bpm_nozzle_theta[i] = - rtb_bpm_nozzle_theta[i]; - } - - // End of Update for Delay: '/Delay1' - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // RandomNumber: '/Random Number' - - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainva_j * (real32_T) - sim_model_lib0_DW->NextOutput_l[0]; - if (sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] >= - sim_model_lib0_P->env_max_ext_air_vel) { - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] = - sim_model_lib0_P->env_max_ext_air_vel; - } else { - if (sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] <= - -(sim_model_lib0_P->env_max_ext_air_vel + 2.22044605E-16F)) { - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] = - -(sim_model_lib0_P->env_max_ext_air_vel + 2.22044605E-16F); - } - } - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // RandomNumber: '/Random Number1' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[0] += - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_a * (real32_T) - sim_model_lib0_DW->NextOutput_f[0]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // RandomNumber: '/Random Number' - - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainva_j * (real32_T) - sim_model_lib0_DW->NextOutput_l[1]; - if (sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1] >= - sim_model_lib0_P->env_max_ext_air_vel) { - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1] = - sim_model_lib0_P->env_max_ext_air_vel; - } else { - if (sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1] <= - -(sim_model_lib0_P->env_max_ext_air_vel + 2.22044605E-16F)) { - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1] = - -(sim_model_lib0_P->env_max_ext_air_vel + 2.22044605E-16F); - } - } - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // RandomNumber: '/Random Number1' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[1] += - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_a * (real32_T) - sim_model_lib0_DW->NextOutput_f[1]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // RandomNumber: '/Random Number' - - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2] += - sim_model_lib0_P->DiscreteTimeIntegrator_gainva_j * (real32_T) - sim_model_lib0_DW->NextOutput_l[2]; - if (sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2] >= - sim_model_lib0_P->env_max_ext_air_vel) { - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2] = - sim_model_lib0_P->env_max_ext_air_vel; - } else { - if (sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2] <= - -(sim_model_lib0_P->env_max_ext_air_vel + 2.22044605E-16F)) { - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2] = - -(sim_model_lib0_P->env_max_ext_air_vel + 2.22044605E-16F); - } - } - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // RandomNumber: '/Random Number1' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[2] += - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_a * (real32_T) - sim_model_lib0_DW->NextOutput_f[2]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTAT_dp += - sim_model_lib0_P->DiscreteTimeIntegrator_gainv_j1 * rtb_Sqrt_m; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_e += - sim_model_lib0_P->DiscreteTimeIntegrator_gainva_o * rtb_Product_p; - - // Update for UnitDelay: '/Unit Delay' incorporates: - // Logic: '/Logical Operator' - // RelationalOperator: '/FixPt Relational Operator' - // UnitDelay: '/Delay Input1' - - sim_model_lib0_DW->UnitDelay_DSTATE_m = ((rtb_V_B_ISS_ISS[0] != - sim_model_lib0_DW->DelayInput1_DSTATE[0]) || (rtb_V_B_ISS_ISS[1] != - sim_model_lib0_DW->DelayInput1_DSTATE[1]) || (rtb_V_B_ISS_ISS_h != - sim_model_lib0_DW->DelayInput1_DSTATE[2])); - - // Update for UnitDelay: '/Unit Delay' incorporates: - // Logic: '/Logical Operator' - // RelationalOperator: '/FixPt Relational Operator' - // UnitDelay: '/Delay Input1' - - sim_model_lib0_DW->UnitDelay_DSTATE_c = ((rtb_V_B_ISS_ISS[0] != - sim_model_lib0_DW->DelayInput1_DSTATE_m[0]) || (rtb_V_B_ISS_ISS[1] != - sim_model_lib0_DW->DelayInput1_DSTATE_m[1]) || (rtb_V_B_ISS_ISS_h != - sim_model_lib0_DW->DelayInput1_DSTATE_m[2])); - - // Update for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_p = - sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_timestamp_sec; - - // Update for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_l = - sim_model_lib0_Y_cvs_optical_flow_msg_n->cvs_timestamp_nsec; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' - if (sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[0] >= - sim_model_lib0_P->env_max_ext_air_omega) { - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[0] = - sim_model_lib0_P->env_max_ext_air_omega; - } else { - if (sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[0] <= - -(sim_model_lib0_P->env_max_ext_air_omega + 2.22044605E-16F)) { - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[0] = - -(sim_model_lib0_P->env_max_ext_air_omega + 2.22044605E-16F); - } - } - - // Update for RandomNumber: '/random_noise' - sim_model_lib0_DW->NextOutput[0] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed[0]) * sim_model_lib0_P->random_noise_StdDev + - sim_model_lib0_P->random_noise_Mean; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTATE[0] += - sim_model_lib0_P->DiscreteTimeIntegrator2_gainval * rtb_random_noise[0]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain2' - // RandomNumber: '/random_noise2' - // Sum: '/Sum' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[0] += - (sim_model_lib0_DW->NextOutput_j[0] - - sim_model_lib0_P->epson_accel_markov_tau * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[0]) * - sim_model_lib0_P->DiscreteTimeIntegrator1_gainval; - - // Update for RandomNumber: '/random_noise' - sim_model_lib0_DW->NextOutput_e[0] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_k[0]) * - sim_model_lib0_P->random_noise_StdDev_c + - sim_model_lib0_P->random_noise_Mean_k; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator2' incorporates: - // Gain: '/Gain4' - // RandomNumber: '/random_noise1' - - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[0] += rtb_Sum * - sim_model_lib0_DW->NextOutput_h[0] * - sim_model_lib0_P->DiscreteTimeIntegrator2_gainv_k; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain2' - // RandomNumber: '/random_noise2' - // Sum: '/Sum' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[0] += - (sim_model_lib0_DW->NextOutput_p[0] - - sim_model_lib0_P->epson_gyro_markov_tau * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[0]) * - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_l; - - // Update for RandomNumber: '/Random Number' - sim_model_lib0_DW->NextOutput_l[0] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_i[0]) * sqrt - (sim_model_lib0_P->env_ext_air_vel_variance[0]) + - sim_model_lib0_P->RandomNumber_Mean; - - // Update for RandomNumber: '/Random Number1' - sim_model_lib0_DW->NextOutput_f[0] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_m[0]) * sqrt - (sim_model_lib0_P->env_ext_air_omega_variance[0]) + - sim_model_lib0_P->RandomNumber1_Mean; - - // Update for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE[0] = rtb_V_B_ISS_ISS[0]; - - // Update for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_m[0] = rtb_V_B_ISS_ISS[0]; - - // Update for RandomNumber: '/random_noise1' - sim_model_lib0_DW->NextOutput_i[0] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_ip[0]) * - sim_model_lib0_P->random_noise1_StdDev + - sim_model_lib0_P->random_noise1_Mean; - - // Update for RandomNumber: '/random_noise2' - sim_model_lib0_DW->NextOutput_j[0] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_e[0]) * sim_model_lib0_P->random_noise2_StdDev - + sim_model_lib0_P->random_noise2_Mean; - - // Update for RandomNumber: '/random_noise1' - sim_model_lib0_DW->NextOutput_h[0] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_p[0]) * - sim_model_lib0_P->random_noise1_StdDev_l + - sim_model_lib0_P->random_noise1_Mean_d; - - // Update for RandomNumber: '/random_noise2' - sim_model_lib0_DW->NextOutput_p[0] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_n[0]) * - sim_model_lib0_P->random_noise2_StdDev_k + - sim_model_lib0_P->random_noise2_Mean_f; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' - if (sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[1] >= - sim_model_lib0_P->env_max_ext_air_omega) { - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[1] = - sim_model_lib0_P->env_max_ext_air_omega; - } else { - if (sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[1] <= - -(sim_model_lib0_P->env_max_ext_air_omega + 2.22044605E-16F)) { - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[1] = - -(sim_model_lib0_P->env_max_ext_air_omega + 2.22044605E-16F); - } - } - - // Update for RandomNumber: '/random_noise' - sim_model_lib0_DW->NextOutput[1] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed[1]) * sim_model_lib0_P->random_noise_StdDev + - sim_model_lib0_P->random_noise_Mean; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTATE[1] += - sim_model_lib0_P->DiscreteTimeIntegrator2_gainval * rtb_random_noise[1]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain2' - // RandomNumber: '/random_noise2' - // Sum: '/Sum' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[1] += - (sim_model_lib0_DW->NextOutput_j[1] - - sim_model_lib0_P->epson_accel_markov_tau * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[1]) * - sim_model_lib0_P->DiscreteTimeIntegrator1_gainval; - - // Update for RandomNumber: '/random_noise' - sim_model_lib0_DW->NextOutput_e[1] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_k[1]) * - sim_model_lib0_P->random_noise_StdDev_c + - sim_model_lib0_P->random_noise_Mean_k; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator2' incorporates: - // Gain: '/Gain4' - // RandomNumber: '/random_noise1' - - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[1] += rtb_Sum * - sim_model_lib0_DW->NextOutput_h[1] * - sim_model_lib0_P->DiscreteTimeIntegrator2_gainv_k; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain2' - // RandomNumber: '/random_noise2' - // Sum: '/Sum' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[1] += - (sim_model_lib0_DW->NextOutput_p[1] - - sim_model_lib0_P->epson_gyro_markov_tau * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[1]) * - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_l; - - // Update for RandomNumber: '/Random Number' - sim_model_lib0_DW->NextOutput_l[1] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_i[1]) * sqrt - (sim_model_lib0_P->env_ext_air_vel_variance[1]) + - sim_model_lib0_P->RandomNumber_Mean; - - // Update for RandomNumber: '/Random Number1' - sim_model_lib0_DW->NextOutput_f[1] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_m[1]) * sqrt - (sim_model_lib0_P->env_ext_air_omega_variance[1]) + - sim_model_lib0_P->RandomNumber1_Mean; - - // Update for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE[1] = rtb_V_B_ISS_ISS[1]; - - // Update for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_m[1] = rtb_V_B_ISS_ISS[1]; - - // Update for RandomNumber: '/random_noise1' - sim_model_lib0_DW->NextOutput_i[1] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_ip[1]) * - sim_model_lib0_P->random_noise1_StdDev + - sim_model_lib0_P->random_noise1_Mean; - - // Update for RandomNumber: '/random_noise2' - sim_model_lib0_DW->NextOutput_j[1] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_e[1]) * sim_model_lib0_P->random_noise2_StdDev - + sim_model_lib0_P->random_noise2_Mean; - - // Update for RandomNumber: '/random_noise1' - sim_model_lib0_DW->NextOutput_h[1] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_p[1]) * - sim_model_lib0_P->random_noise1_StdDev_l + - sim_model_lib0_P->random_noise1_Mean_d; - - // Update for RandomNumber: '/random_noise2' - sim_model_lib0_DW->NextOutput_p[1] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_n[1]) * - sim_model_lib0_P->random_noise2_StdDev_k + - sim_model_lib0_P->random_noise2_Mean_f; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' - if (sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[2] >= - sim_model_lib0_P->env_max_ext_air_omega) { - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[2] = - sim_model_lib0_P->env_max_ext_air_omega; - } else { - if (sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[2] <= - -(sim_model_lib0_P->env_max_ext_air_omega + 2.22044605E-16F)) { - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[2] = - -(sim_model_lib0_P->env_max_ext_air_omega + 2.22044605E-16F); - } - } - - // Update for RandomNumber: '/random_noise' - sim_model_lib0_DW->NextOutput[2] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed[2]) * sim_model_lib0_P->random_noise_StdDev + - sim_model_lib0_P->random_noise_Mean; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTATE[2] += - sim_model_lib0_P->DiscreteTimeIntegrator2_gainval * rtb_random_noise[2]; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain2' - // RandomNumber: '/random_noise2' - // Sum: '/Sum' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[2] += - (sim_model_lib0_DW->NextOutput_j[2] - - sim_model_lib0_P->epson_accel_markov_tau * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[2]) * - sim_model_lib0_P->DiscreteTimeIntegrator1_gainval; - - // Update for RandomNumber: '/random_noise' - sim_model_lib0_DW->NextOutput_e[2] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_k[2]) * - sim_model_lib0_P->random_noise_StdDev_c + - sim_model_lib0_P->random_noise_Mean_k; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator2' incorporates: - // Gain: '/Gain4' - // RandomNumber: '/random_noise1' - - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[2] += rtb_Sum * - sim_model_lib0_DW->NextOutput_h[2] * - sim_model_lib0_P->DiscreteTimeIntegrator2_gainv_k; - - // Update for DiscreteIntegrator: '/Discrete-Time Integrator1' incorporates: - // DiscreteIntegrator: '/Discrete-Time Integrator1' - // Gain: '/Gain2' - // RandomNumber: '/random_noise2' - // Sum: '/Sum' - - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[2] += - (sim_model_lib0_DW->NextOutput_p[2] - - sim_model_lib0_P->epson_gyro_markov_tau * - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[2]) * - sim_model_lib0_P->DiscreteTimeIntegrator1_gainv_l; - - // Update for RandomNumber: '/Random Number' - sim_model_lib0_DW->NextOutput_l[2] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_i[2]) * sqrt - (sim_model_lib0_P->env_ext_air_vel_variance[2]) + - sim_model_lib0_P->RandomNumber_Mean; - - // Update for RandomNumber: '/Random Number1' - sim_model_lib0_DW->NextOutput_f[2] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_m[2]) * sqrt - (sim_model_lib0_P->env_ext_air_omega_variance[2]) + - sim_model_lib0_P->RandomNumber1_Mean; - - // Update for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE[2] = rtb_V_B_ISS_ISS_h; - - // Update for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_m[2] = rtb_V_B_ISS_ISS_h; - - // Update for RandomNumber: '/random_noise1' - sim_model_lib0_DW->NextOutput_i[2] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_ip[2]) * - sim_model_lib0_P->random_noise1_StdDev + - sim_model_lib0_P->random_noise1_Mean; - - // Update for RandomNumber: '/random_noise2' - sim_model_lib0_DW->NextOutput_j[2] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_e[2]) * sim_model_lib0_P->random_noise2_StdDev - + sim_model_lib0_P->random_noise2_Mean; - - // Update for RandomNumber: '/random_noise1' - sim_model_lib0_DW->NextOutput_h[2] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_p[2]) * - sim_model_lib0_P->random_noise1_StdDev_l + - sim_model_lib0_P->random_noise1_Mean_d; - - // Update for RandomNumber: '/random_noise2' - sim_model_lib0_DW->NextOutput_p[2] = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_n[2]) * - sim_model_lib0_P->random_noise2_StdDev_k + - sim_model_lib0_P->random_noise2_Mean_f; -} - -// Model step function for TID1 -void sim_model_lib0_step1(RT_MODEL_sim_model_lib0_T *const sim_model_lib0_M) // Sample time: [0.16s, 0.0s] -{ - P_sim_model_lib0_T *sim_model_lib0_P = ((P_sim_model_lib0_T *) - sim_model_lib0_M->defaultParam); - B_sim_model_lib0_T *sim_model_lib0_B = ((B_sim_model_lib0_T *) - sim_model_lib0_M->blockIO); - DW_sim_model_lib0_T *sim_model_lib0_DW = ((DW_sim_model_lib0_T *) - sim_model_lib0_M->dwork); - - // local block i/o variables - real32_T rtb_ImpAsg_InsertedFor_P_points[72]; - boolean_T rtb_ImpAsg_InsertedFor_points_i[36]; - real32_T rtb_Conversion_my[9]; - real32_T rtb_Subtract1[3]; - real32_T rtb_MatrixConcatenate[16]; - real32_T rtb_Product[4]; - real32_T rtb_Sum; - real32_T rtb_Assignment[9]; - real32_T rtb_Sum_od; - real32_T rtb_P_point_B_iss[108]; - real32_T rtb_valid_out[50]; - int8_T rtb_DataTypeConversion4[50]; - real32_T rtb_Add_b[108]; - real32_T rtb_points_out_iss[150]; - real32_T rtb_points_out_2D_cam[100]; - int32_T i; - boolean_T tmp[36]; - int32_T i_0; - boolean_T tmp_0; - real32_T tmp_1[9]; - real32_T rtb_Assignment_h4[9]; - real32_T tmp_2[9]; - real32_T rtb_Product_o3[9]; - real32_T rtb_Add_c[108]; - real32_T tmp_3; - real32_T tmp_4; - real32_T rtb_Product_b; - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Conversion_my[i] = (real32_T)sim_model_lib0_P->Constant2_Value_n1[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant3' - - rtb_Conversion_my[0] = sim_model_lib0_P->tun_abp_q_body2dockcam[3]; - rtb_Conversion_my[4] = sim_model_lib0_P->tun_abp_q_body2dockcam[3]; - rtb_Conversion_my[8] = sim_model_lib0_P->tun_abp_q_body2dockcam[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Product_o3[0] = (real32_T)sim_model_lib0_P->Constant3_Value_eu; - rtb_Product_o3[1] = sim_model_lib0_P->tun_abp_q_body2dockcam[2]; - rtb_Product_o3[2] = sim_model_lib0_P->Gain_Gain_o * - sim_model_lib0_P->tun_abp_q_body2dockcam[1]; - rtb_Product_o3[3] = sim_model_lib0_P->Gain1_Gain_mr * - sim_model_lib0_P->tun_abp_q_body2dockcam[2]; - rtb_Product_o3[4] = (real32_T)sim_model_lib0_P->Constant3_Value_eu; - rtb_Product_o3[5] = sim_model_lib0_P->tun_abp_q_body2dockcam[0]; - rtb_Product_o3[6] = sim_model_lib0_P->tun_abp_q_body2dockcam[1]; - rtb_Product_o3[7] = sim_model_lib0_P->Gain2_Gain_og * - sim_model_lib0_P->tun_abp_q_body2dockcam[0]; - rtb_Product_o3[8] = (real32_T)sim_model_lib0_P->Constant3_Value_eu; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Constant: '/Constant3' - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_MatrixConcatenate[(int32_T)(i_0 << 2)] = rtb_Conversion_my[(int32_T)(3 * - i_0)] + rtb_Product_o3[(int32_T)(3 * i_0)]; - rtb_MatrixConcatenate[(int32_T)(1 + (int32_T)(i_0 << 2))] = - rtb_Conversion_my[(int32_T)((int32_T)(3 * i_0) + 1)] + rtb_Product_o3 - [(int32_T)((int32_T)(3 * i_0) + 1)]; - rtb_MatrixConcatenate[(int32_T)(2 + (int32_T)(i_0 << 2))] = - rtb_Conversion_my[(int32_T)((int32_T)(3 * i_0) + 2)] + rtb_Product_o3 - [(int32_T)((int32_T)(3 * i_0) + 2)]; - } - - rtb_MatrixConcatenate[3] = sim_model_lib0_P->Gain1_Gain_cz * - sim_model_lib0_P->tun_abp_q_body2dockcam[0]; - rtb_MatrixConcatenate[7] = sim_model_lib0_P->Gain1_Gain_cz * - sim_model_lib0_P->tun_abp_q_body2dockcam[1]; - rtb_MatrixConcatenate[11] = sim_model_lib0_P->Gain1_Gain_cz * - sim_model_lib0_P->tun_abp_q_body2dockcam[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - - tmp_0 = ((int32_T)sim_model_lib0_P->tun_cvs_noise_on != 0); - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[12] = sim_model_lib0_P->tun_abp_q_body2dockcam[0]; - - // Product: '/Product' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Switch: '/Switch' - - if (tmp_0) { - rtb_Sum = sim_model_lib0_P->cvs_dockcam_Q_B2dockcam_error[0]; - rtb_Sum_od = sim_model_lib0_P->cvs_dockcam_Q_B2dockcam_error[1]; - } else { - rtb_Sum = sim_model_lib0_P->Constant7_Value_n[0]; - rtb_Sum_od = sim_model_lib0_P->Constant7_Value_n[1]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[13] = sim_model_lib0_P->tun_abp_q_body2dockcam[1]; - rtb_MatrixConcatenate[14] = sim_model_lib0_P->tun_abp_q_body2dockcam[2]; - - // Product: '/Product' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Switch: '/Switch' - - if (tmp_0) { - tmp_3 = sim_model_lib0_P->cvs_dockcam_Q_B2dockcam_error[2]; - tmp_4 = sim_model_lib0_P->cvs_dockcam_Q_B2dockcam_error[3]; - } else { - tmp_3 = sim_model_lib0_P->Constant7_Value_n[2]; - tmp_4 = sim_model_lib0_P->Constant7_Value_n[3]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[15] = sim_model_lib0_P->tun_abp_q_body2dockcam[3]; - - // Product: '/Product' - for (i_0 = 0; i_0 < 4; i_0++) { - rtb_Product_b = rtb_MatrixConcatenate[(int32_T)(i_0 + 12)] * tmp_4 + - (rtb_MatrixConcatenate[(int32_T)(i_0 + 8)] * tmp_3 + - (rtb_MatrixConcatenate[(int32_T)(i_0 + 4)] * rtb_Sum_od + - rtb_MatrixConcatenate[i_0] * rtb_Sum)); - rtb_Product[i_0] = rtb_Product_b; - } - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum = rtb_Product[3] * rtb_Product[3] * sim_model_lib0_P->Gain_Gain_oe - - (real32_T)sim_model_lib0_P->Constant1_Value_g; - for (i = 0; i < 9; i++) { - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Conversion_my[i] = (real32_T)sim_model_lib0_P->Constant2_Value_ci[i]; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment[i] = (real32_T)sim_model_lib0_P->Constant2_Value_a[i]; - } - - // Assignment: '/Assignment' - rtb_Conversion_my[0] = rtb_Sum; - rtb_Conversion_my[4] = rtb_Sum; - rtb_Conversion_my[8] = rtb_Sum; - - // Gain: '/Gain1' - rtb_Sum = sim_model_lib0_P->Gain1_Gain_mm * rtb_Product[3]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_od = sim_model_lib0_B->RateTransition2.Q_ISS2B[3] * - sim_model_lib0_B->RateTransition2.Q_ISS2B[3] * sim_model_lib0_P->Gain_Gain_a - - (real32_T)sim_model_lib0_P->Constant1_Value_m; - - // Assignment: '/Assignment' - rtb_Assignment[0] = rtb_Sum_od; - rtb_Assignment[4] = rtb_Sum_od; - rtb_Assignment[8] = rtb_Sum_od; - - // Gain: '/Gain1' - rtb_Sum_od = sim_model_lib0_P->Gain1_Gain_al2 * - sim_model_lib0_B->RateTransition2.Q_ISS2B[3]; - - // Sum: '/Add' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant4' - // Gain: '/Gain' - - for (i = 0; i < 108; i++) { - rtb_Add_b[i] = (real32_T)sim_model_lib0_P->tun_cvs_noise_on * - sim_model_lib0_P->cvs_AR_map_error[i] + sim_model_lib0_P->cvs_AR_map_iss[i]; - } - - // End of Sum: '/Add' - for (i_0 = 0; i_0 < 36; i_0++) { - // Sum: '/Subtract' incorporates: - // Selector: '/Select_X' - - rtb_P_point_B_iss[i_0] = rtb_Add_b[i_0] - - sim_model_lib0_B->RateTransition2.P_B_ISS_ISS[0]; - - // Sum: '/Subtract2' incorporates: - // Selector: '/Select_Y' - - rtb_P_point_B_iss[(int32_T)(36 + i_0)] = rtb_Add_b[(int32_T)(36 + i_0)] - - sim_model_lib0_B->RateTransition2.P_B_ISS_ISS[1]; - - // Sum: '/Subtract3' incorporates: - // Selector: '/Select_Z' - - rtb_P_point_B_iss[(int32_T)(72 + i_0)] = rtb_Add_b[(int32_T)(72 + i_0)] - - sim_model_lib0_B->RateTransition2.P_B_ISS_ISS[2]; - } - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_1[0] = (real32_T)sim_model_lib0_P->Constant3_Value_az; - tmp_1[1] = sim_model_lib0_B->RateTransition2.Q_ISS2B[2]; - tmp_1[2] = sim_model_lib0_P->Gain_Gain_od * - sim_model_lib0_B->RateTransition2.Q_ISS2B[1]; - tmp_1[3] = sim_model_lib0_P->Gain1_Gain_f * - sim_model_lib0_B->RateTransition2.Q_ISS2B[2]; - tmp_1[4] = (real32_T)sim_model_lib0_P->Constant3_Value_az; - tmp_1[5] = sim_model_lib0_B->RateTransition2.Q_ISS2B[0]; - tmp_1[6] = sim_model_lib0_B->RateTransition2.Q_ISS2B[1]; - tmp_1[7] = sim_model_lib0_P->Gain2_Gain_hy * - sim_model_lib0_B->RateTransition2.Q_ISS2B[0]; - tmp_1[8] = (real32_T)sim_model_lib0_P->Constant3_Value_az; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Product_o3[i_0] = sim_model_lib0_B->RateTransition2.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2.Q_ISS2B[0]; - rtb_Product_o3[(int32_T)(i_0 + 3)] = - sim_model_lib0_B->RateTransition2.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2.Q_ISS2B[1]; - rtb_Product_o3[(int32_T)(i_0 + 6)] = - sim_model_lib0_B->RateTransition2.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2.Q_ISS2B[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_h4[(int32_T)(3 * i_0)] = (rtb_Assignment[(int32_T)(3 * i_0)] - - tmp_1[(int32_T)(3 * i_0)] * rtb_Sum_od) + rtb_Product_o3[(int32_T)(3 * - i_0)] * sim_model_lib0_P->Gain2_Gain_lw; - rtb_Assignment_h4[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_1[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Sum_od) + rtb_Product_o3[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_lw; - rtb_Assignment_h4[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_1[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Sum_od) + rtb_Product_o3[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_lw; - } - - // End of Sum: '/Sum1' - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_2[0] = (real32_T)sim_model_lib0_P->Constant3_Value_p; - tmp_2[1] = rtb_Product[2]; - tmp_2[2] = sim_model_lib0_P->Gain_Gain_ee4 * rtb_Product[1]; - tmp_2[3] = sim_model_lib0_P->Gain1_Gain_g * rtb_Product[2]; - tmp_2[4] = (real32_T)sim_model_lib0_P->Constant3_Value_p; - tmp_2[5] = rtb_Product[0]; - tmp_2[6] = rtb_Product[1]; - tmp_2[7] = sim_model_lib0_P->Gain2_Gain_l0 * rtb_Product[0]; - tmp_2[8] = (real32_T)sim_model_lib0_P->Constant3_Value_p; - for (i = 0; i < 3; i++) { - // Product: '/Product' incorporates: - // Math: '/Math Function' - - for (i_0 = 0; i_0 < 36; i_0++) { - rtb_Add_b[(int32_T)(i + (int32_T)(3 * i_0))] = 0.0F; - rtb_Add_b[(int32_T)(i + (int32_T)(3 * i_0))] += rtb_Assignment_h4[i] * - rtb_P_point_B_iss[i_0]; - rtb_Add_b[(int32_T)(i + (int32_T)(3 * i_0))] += rtb_Assignment_h4[(int32_T) - (i + 3)] * rtb_P_point_B_iss[(int32_T)(i_0 + 36)]; - rtb_Add_b[(int32_T)(i + (int32_T)(3 * i_0))] += rtb_Assignment_h4[(int32_T) - (i + 6)] * rtb_P_point_B_iss[(int32_T)(i_0 + 72)]; - } - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Gain: '/Gain1' - - rtb_Subtract1[i] = sim_model_lib0_P->tun_abp_p_dockcam_body_body_sim[i] - - (real32_T)sim_model_lib0_P->tun_cvs_noise_on * - sim_model_lib0_P->cvs_dockcam_P_B_B_error[i]; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - rtb_Product_o3[i] = rtb_Product[i] * rtb_Product[0]; - rtb_Product_o3[(int32_T)(i + 3)] = rtb_Product[i] * rtb_Product[1]; - rtb_Product_o3[(int32_T)(i + 6)] = rtb_Product[i] * rtb_Product[2]; - } - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product1' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_h4[(int32_T)(3 * i_0)] = (rtb_Conversion_my[(int32_T)(3 * i_0)] - - tmp_2[(int32_T)(3 * i_0)] * rtb_Sum) + rtb_Product_o3[(int32_T)(3 * i_0)] - * sim_model_lib0_P->Gain2_Gain_fv; - rtb_Assignment_h4[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Conversion_my - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_2[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Sum) + rtb_Product_o3[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_fv; - rtb_Assignment_h4[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Conversion_my - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_2[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Sum) + rtb_Product_o3[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_fv; - } - - // End of Sum: '/Sum1' - for (i_0 = 0; i_0 < 36; i_0++) { - // Concatenate: '/Matrix Concatenate1' incorporates: - // Product: '/Product1' - // Selector: '/Select_X1' - // Selector: '/Select_Y1' - // Selector: '/Select_Z1' - // Sum: '/Subtract4' - // Sum: '/Subtract5' - // Sum: '/Subtract6' - - rtb_Add_c[(int32_T)(3 * i_0)] = rtb_Add_b[(int32_T)(3 * i_0)] - - rtb_Subtract1[0]; - rtb_Add_c[(int32_T)(1 + (int32_T)(3 * i_0))] = rtb_Add_b[(int32_T)((int32_T) - (3 * i_0) + 1)] - rtb_Subtract1[1]; - rtb_Add_c[(int32_T)(2 + (int32_T)(3 * i_0))] = rtb_Add_b[(int32_T)((int32_T) - (3 * i_0) + 2)] - rtb_Subtract1[2]; - - // Math: '/Math Function1' incorporates: - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_P_point_B_iss[(int32_T)(i_0 + (int32_T)(36 * i))] = 0.0F; - rtb_P_point_B_iss[(int32_T)(i_0 + (int32_T)(36 * i))] += rtb_Add_c - [(int32_T)(3 * i_0)] * rtb_Assignment_h4[i]; - rtb_P_point_B_iss[(int32_T)(i_0 + (int32_T)(36 * i))] += rtb_Add_c - [(int32_T)((int32_T)(3 * i_0) + 1)] * rtb_Assignment_h4[(int32_T)(i + 3)]; - rtb_P_point_B_iss[(int32_T)(i_0 + (int32_T)(36 * i))] += rtb_Add_c - [(int32_T)((int32_T)(3 * i_0) + 2)] * rtb_Assignment_h4[(int32_T)(i + 6)]; - } - - // End of Math: '/Math Function1' - } - - // Outputs for Iterator SubSystem: '/pinhole_projection_model' - si_pinhole_projection_model(36, rtb_P_point_B_iss, - rtb_ImpAsg_InsertedFor_P_points, rtb_ImpAsg_InsertedFor_points_i, - sim_model_lib0_DW->pinhole_projection_model, - (P_pinhole_projection_model_si_T *) - &sim_model_lib0_P->pinhole_projection_model, - sim_model_lib0_P->tun_cvs_noise_on, - sim_model_lib0_P->tun_cvs_dockcam_focal_length_Y, - sim_model_lib0_P->tun_cvs_dockcam_focal_length_X, - sim_model_lib0_P->cvs_AR_pixel_noise, - sim_model_lib0_P->tun_cvs_dockcam_num_pixels_X, - sim_model_lib0_P->tun_cvs_dockcam_num_pixels_Y, - sim_model_lib0_P->cvs_dockcam_min_dist, - sim_model_lib0_P->cvs_dockcam_max_dist, - sim_model_lib0_P->cvs_dockcam_pointing); - - // End of Outputs for SubSystem: '/pinhole_projection_model' - - // MATLAB Function: '/generate_output' incorporates: - // Constant: '/Constant3' - - // MATLAB Function 'image_processing/generate_output': ':1' - // Inputs: - // Process type: [Parameter] - // 1=Landmarks, 2=AR Tags, 3=Optical Flow, 4=Handrail - // num_pts_out = Required number of points to be reported [Parameter] - // P_point_iss_iss = ALL [x,y,z] position of points in the ISS frame **Landmark only** - // P_point_cam_cam = ALL [x,y,z] position of points in the CAM frame **Handrail only** - // P_points_2D_cam = ALL [u,v] pixel location of points in the camera **Landmark and OF only** - // valid_flag = indicates which points are valid - // num_hist_kept = Required number of histories kept for Optical Flow [Parameter] **OF only** - // flow_ids = ID tag for each optical flow point [Parameter] **OF only** - // - // Outputs: - // points_out_iss = output [x,y,z] position of points in the ISS frame **This output unused by optical flow** - // points_out_2D_cam = output [u,v] pixel location of points in the camera **This output unused by handrail** - // points_out_cam = output [x,y,z] position of points in the CAM frame **This output unused by OF and landmarks** - // valid_out = indicates which values of the output are valid! **This output unused by landmarks** - // ids_out = optical flow IDs - // registration_id = visual odometery registration pulse ID **ONLY used by OF** - // AR tags - // ':1:34' - for (i_0 = 0; i_0 < 36; i_0++) { - tmp[i_0] = rtb_ImpAsg_InsertedFor_points_i[i_0]; - } - - sim_model__format_AR_tag_output(sim_model_lib0_P->cvs_AR_map_iss, - rtb_ImpAsg_InsertedFor_P_points, tmp, rtb_points_out_iss, - rtb_points_out_2D_cam, rtb_valid_out); - - // End of MATLAB Function: '/generate_output' - - // DataTypeConversion: '/Data Type Conversion4' incorporates: - // Constant: '/Constant3' - // Logic: '/Logical Operator' - // Selector: '/select_current_command' - // UnitDelay: '/Unit Delay' - - // ':1:34' - // ':1:35' - // ':1:36' - // ':1:37' - // end of function - for (i = 0; i < 50; i++) { - rtb_DataTypeConversion4[i] = (int8_T)((rtb_valid_out[i] != 0.0F) && - sim_model_lib0_P->cvs_AR_valid_mask[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_hj - 1)]); - } - - // End of DataTypeConversion: '/Data Type Conversion4' - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Compare' - // Selector: '/Select_col_1' - // Selector: '/Select_col_2' - - i_0 = (int32_T)(uint32_T)((uint32_T)((uint32_T) - ((sim_model_lib0_P->cvs_AR_valid_times[0] != - sim_model_lib0_P->Constant_Value_e) && - ((sim_model_lib0_P->cvs_AR_valid_times[0] < - sim_model_lib0_B->RateTransition4.timestamp_sec) || - ((sim_model_lib0_P->cvs_AR_valid_times[0] == - sim_model_lib0_B->RateTransition4.timestamp_sec) && - (sim_model_lib0_B->RateTransition4.timestamp_nsec >= - sim_model_lib0_P->cvs_AR_valid_times[3])))) + (uint32_T) - ((sim_model_lib0_P->cvs_AR_valid_times[1] != - sim_model_lib0_P->Constant_Value_e) && - ((sim_model_lib0_P->cvs_AR_valid_times[1] < - sim_model_lib0_B->RateTransition4.timestamp_sec) || - ((sim_model_lib0_P->cvs_AR_valid_times[1] == - sim_model_lib0_B->RateTransition4.timestamp_sec) && - (sim_model_lib0_B->RateTransition4.timestamp_nsec >= - sim_model_lib0_P->cvs_AR_valid_times[4]))))) + (uint32_T) - ((sim_model_lib0_P->cvs_AR_valid_times[2] != - sim_model_lib0_P->Constant_Value_e) && - ((sim_model_lib0_P->cvs_AR_valid_times[2] < - sim_model_lib0_B->RateTransition4.timestamp_sec) || - ((sim_model_lib0_P->cvs_AR_valid_times[2] == - sim_model_lib0_B->RateTransition4.timestamp_sec) && - (sim_model_lib0_B->RateTransition4.timestamp_nsec >= - sim_model_lib0_P->cvs_AR_valid_times[5]))))); - - // Update for RateTransition: '/Rate Transition3' - sim_model_lib0_DW->RateTransition3_Buffer0_n = - sim_model_lib0_B->RateTransition4.timestamp_sec; - - // Update for RateTransition: '/Rate Transition1' - sim_model_lib0_DW->RateTransition1_Buffer0_i = - sim_model_lib0_B->RateTransition4.timestamp_nsec; - - // Update for RateTransition: '/Rate Transition5' - memcpy(&sim_model_lib0_DW->RateTransition5_Buffer0_f[0], &rtb_points_out_iss[0], - (uint32_T)(150U * sizeof(real32_T))); - - // Update for RateTransition: '/Rate Transition6' - memcpy(&sim_model_lib0_DW->RateTransition6_Buffer0_e[0], - &rtb_points_out_2D_cam[0], (uint32_T)(100U * sizeof(real32_T))); - - // Update for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_DW->RateTransition7_Buffer0_j[i] = (uint8_T) - rtb_DataTypeConversion4[i]; - } - - // End of Update for RateTransition: '/Rate Transition7' - - // Update for RandomNumber: '/pixel_noise' - rt_nrand_Upu32_Yd_f_pw_snf(&sim_model_lib0_DW->RandSeed_mb); - - // Saturate: '/Saturation' incorporates: - // Sum: '/Sum1' - - if ((uint8_T)i_0 > sim_model_lib0_P->Saturation_UpperSat_i) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_hj = - sim_model_lib0_P->Saturation_UpperSat_i; - } else if ((uint8_T)i_0 < sim_model_lib0_P->Saturation_LowerSat_b) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_hj = - sim_model_lib0_P->Saturation_LowerSat_b; - } else { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_hj = (uint8_T)i_0; - } - - // End of Saturate: '/Saturation' -} - -// Model step function for TID2 -void sim_model_lib0_step2(RT_MODEL_sim_model_lib0_T *const sim_model_lib0_M) // Sample time: [0.20800000000000002s, 0.0s] -{ - P_sim_model_lib0_T *sim_model_lib0_P = ((P_sim_model_lib0_T *) - sim_model_lib0_M->defaultParam); - B_sim_model_lib0_T *sim_model_lib0_B = ((B_sim_model_lib0_T *) - sim_model_lib0_M->blockIO); - DW_sim_model_lib0_T *sim_model_lib0_DW = ((DW_sim_model_lib0_T *) - sim_model_lib0_M->dwork); - real32_T rtb_Conversion_i[9]; - real32_T rtb_Subtract1[3]; - real32_T rtb_MatrixConcatenate[16]; - real32_T rtb_Product[4]; - real32_T rtb_Sum; - real32_T rtb_Assignment[9]; - real32_T rtb_Sum_as; - int8_T rtb_DataTypeConversion4[50]; - real32_T valid_out_i[50]; - real32_T ImpAsg_InsertedFor_P_points_2_g[1008]; - int32_T i; - int32_T i_0; - boolean_T tmp; - real32_T tmp_0[9]; - real32_T rtb_Assignment_in[9]; - real32_T tmp_1[9]; - real32_T rtb_Product_h[9]; - real32_T tmp_2; - real32_T tmp_3; - real32_T rtb_Product_b; - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Conversion_i[i] = (real32_T)sim_model_lib0_P->Constant2_Value_nv[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant3' - - rtb_Conversion_i[0] = sim_model_lib0_P->tun_abp_q_body2perchcam[3]; - rtb_Conversion_i[4] = sim_model_lib0_P->tun_abp_q_body2perchcam[3]; - rtb_Conversion_i[8] = sim_model_lib0_P->tun_abp_q_body2perchcam[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Product_h[0] = (real32_T)sim_model_lib0_P->Constant3_Value_h; - rtb_Product_h[1] = sim_model_lib0_P->tun_abp_q_body2perchcam[2]; - rtb_Product_h[2] = sim_model_lib0_P->Gain_Gain_op * - sim_model_lib0_P->tun_abp_q_body2perchcam[1]; - rtb_Product_h[3] = sim_model_lib0_P->Gain1_Gain_da * - sim_model_lib0_P->tun_abp_q_body2perchcam[2]; - rtb_Product_h[4] = (real32_T)sim_model_lib0_P->Constant3_Value_h; - rtb_Product_h[5] = sim_model_lib0_P->tun_abp_q_body2perchcam[0]; - rtb_Product_h[6] = sim_model_lib0_P->tun_abp_q_body2perchcam[1]; - rtb_Product_h[7] = sim_model_lib0_P->Gain2_Gain_fs * - sim_model_lib0_P->tun_abp_q_body2perchcam[0]; - rtb_Product_h[8] = (real32_T)sim_model_lib0_P->Constant3_Value_h; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Constant: '/Constant3' - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_MatrixConcatenate[(int32_T)(i_0 << 2)] = rtb_Conversion_i[(int32_T)(3 * - i_0)] + rtb_Product_h[(int32_T)(3 * i_0)]; - rtb_MatrixConcatenate[(int32_T)(1 + (int32_T)(i_0 << 2))] = - rtb_Conversion_i[(int32_T)((int32_T)(3 * i_0) + 1)] + rtb_Product_h - [(int32_T)((int32_T)(3 * i_0) + 1)]; - rtb_MatrixConcatenate[(int32_T)(2 + (int32_T)(i_0 << 2))] = - rtb_Conversion_i[(int32_T)((int32_T)(3 * i_0) + 2)] + rtb_Product_h - [(int32_T)((int32_T)(3 * i_0) + 2)]; - } - - rtb_MatrixConcatenate[3] = sim_model_lib0_P->Gain1_Gain_jq * - sim_model_lib0_P->tun_abp_q_body2perchcam[0]; - rtb_MatrixConcatenate[7] = sim_model_lib0_P->Gain1_Gain_jq * - sim_model_lib0_P->tun_abp_q_body2perchcam[1]; - rtb_MatrixConcatenate[11] = sim_model_lib0_P->Gain1_Gain_jq * - sim_model_lib0_P->tun_abp_q_body2perchcam[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - - tmp = ((int32_T)sim_model_lib0_P->tun_cvs_noise_on != 0); - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[12] = sim_model_lib0_P->tun_abp_q_body2perchcam[0]; - - // Product: '/Product' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Switch: '/Switch' - - if (tmp) { - rtb_Sum = sim_model_lib0_P->cvs_perchcam_Q_B2perchcam_error[0]; - rtb_Sum_as = sim_model_lib0_P->cvs_perchcam_Q_B2perchcam_error[1]; - } else { - rtb_Sum = sim_model_lib0_P->Constant7_Value_h[0]; - rtb_Sum_as = sim_model_lib0_P->Constant7_Value_h[1]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[13] = sim_model_lib0_P->tun_abp_q_body2perchcam[1]; - rtb_MatrixConcatenate[14] = sim_model_lib0_P->tun_abp_q_body2perchcam[2]; - - // Product: '/Product' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Switch: '/Switch' - - if (tmp) { - tmp_2 = sim_model_lib0_P->cvs_perchcam_Q_B2perchcam_error[2]; - tmp_3 = sim_model_lib0_P->cvs_perchcam_Q_B2perchcam_error[3]; - } else { - tmp_2 = sim_model_lib0_P->Constant7_Value_h[2]; - tmp_3 = sim_model_lib0_P->Constant7_Value_h[3]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[15] = sim_model_lib0_P->tun_abp_q_body2perchcam[3]; - - // Product: '/Product' - for (i_0 = 0; i_0 < 4; i_0++) { - rtb_Product_b = rtb_MatrixConcatenate[(int32_T)(i_0 + 12)] * tmp_3 + - (rtb_MatrixConcatenate[(int32_T)(i_0 + 8)] * tmp_2 + - (rtb_MatrixConcatenate[(int32_T)(i_0 + 4)] * rtb_Sum_as + - rtb_MatrixConcatenate[i_0] * rtb_Sum)); - rtb_Product[i_0] = rtb_Product_b; - } - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum = rtb_Product[3] * rtb_Product[3] * sim_model_lib0_P->Gain_Gain_pb - - (real32_T)sim_model_lib0_P->Constant1_Value_oc; - for (i = 0; i < 9; i++) { - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Conversion_i[i] = (real32_T)sim_model_lib0_P->Constant2_Value_ab[i]; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment[i] = (real32_T)sim_model_lib0_P->Constant2_Value_o[i]; - } - - // Assignment: '/Assignment' - rtb_Conversion_i[0] = rtb_Sum; - rtb_Conversion_i[4] = rtb_Sum; - rtb_Conversion_i[8] = rtb_Sum; - - // Gain: '/Gain1' - rtb_Sum = sim_model_lib0_P->Gain1_Gain_i * rtb_Product[3]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_as = sim_model_lib0_B->RateTransition2_o.Q_ISS2B[3] * - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[3] * - sim_model_lib0_P->Gain_Gain_cl - (real32_T) - sim_model_lib0_P->Constant1_Value_e; - - // Assignment: '/Assignment' - rtb_Assignment[0] = rtb_Sum_as; - rtb_Assignment[4] = rtb_Sum_as; - rtb_Assignment[8] = rtb_Sum_as; - - // Gain: '/Gain1' - rtb_Sum_as = sim_model_lib0_P->Gain1_Gain_ap * - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[3]; - - // Sum: '/Add' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant4' - // Gain: '/Gain' - - for (i = 0; i < 1512; i++) { - sim_model_lib0_B->Add_l[i] = (real32_T)sim_model_lib0_P->tun_cvs_noise_on * - sim_model_lib0_P->cvs_handrail_map_error[i] + - sim_model_lib0_P->cvs_handrail_map_iss[i]; - } - - // End of Sum: '/Add' - for (i_0 = 0; i_0 < 504; i_0++) { - // Sum: '/Subtract' incorporates: - // Selector: '/Select_X' - - sim_model_lib0_B->P_point_B_iss_p[i_0] = sim_model_lib0_B->Add_l[i_0] - - sim_model_lib0_B->RateTransition2_o.P_B_ISS_ISS[0]; - - // Sum: '/Subtract2' incorporates: - // Selector: '/Select_Y' - - sim_model_lib0_B->P_point_B_iss_p[(int32_T)(504 + i_0)] = - sim_model_lib0_B->Add_l[(int32_T)(504 + i_0)] - - sim_model_lib0_B->RateTransition2_o.P_B_ISS_ISS[1]; - - // Sum: '/Subtract3' incorporates: - // Selector: '/Select_Z' - - sim_model_lib0_B->P_point_B_iss_p[(int32_T)(1008 + i_0)] = - sim_model_lib0_B->Add_l[(int32_T)(1008 + i_0)] - - sim_model_lib0_B->RateTransition2_o.P_B_ISS_ISS[2]; - } - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_0[0] = (real32_T)sim_model_lib0_P->Constant3_Value_j0; - tmp_0[1] = sim_model_lib0_B->RateTransition2_o.Q_ISS2B[2]; - tmp_0[2] = sim_model_lib0_P->Gain_Gain_mu * - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[1]; - tmp_0[3] = sim_model_lib0_P->Gain1_Gain_d3 * - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[2]; - tmp_0[4] = (real32_T)sim_model_lib0_P->Constant3_Value_j0; - tmp_0[5] = sim_model_lib0_B->RateTransition2_o.Q_ISS2B[0]; - tmp_0[6] = sim_model_lib0_B->RateTransition2_o.Q_ISS2B[1]; - tmp_0[7] = sim_model_lib0_P->Gain2_Gain_e * - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[0]; - tmp_0[8] = (real32_T)sim_model_lib0_P->Constant3_Value_j0; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Product_h[i_0] = sim_model_lib0_B->RateTransition2_o.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[0]; - rtb_Product_h[(int32_T)(i_0 + 3)] = - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[1]; - rtb_Product_h[(int32_T)(i_0 + 6)] = - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2_o.Q_ISS2B[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_in[(int32_T)(3 * i_0)] = (rtb_Assignment[(int32_T)(3 * i_0)] - - tmp_0[(int32_T)(3 * i_0)] * rtb_Sum_as) + rtb_Product_h[(int32_T)(3 * - i_0)] * sim_model_lib0_P->Gain2_Gain_hf; - rtb_Assignment_in[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_0[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Sum_as) + rtb_Product_h[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_hf; - rtb_Assignment_in[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_0[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Sum_as) + rtb_Product_h[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_hf; - } - - // End of Sum: '/Sum1' - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_1[0] = (real32_T)sim_model_lib0_P->Constant3_Value_f; - tmp_1[1] = rtb_Product[2]; - tmp_1[2] = sim_model_lib0_P->Gain_Gain_hw * rtb_Product[1]; - tmp_1[3] = sim_model_lib0_P->Gain1_Gain_dx * rtb_Product[2]; - tmp_1[4] = (real32_T)sim_model_lib0_P->Constant3_Value_f; - tmp_1[5] = rtb_Product[0]; - tmp_1[6] = rtb_Product[1]; - tmp_1[7] = sim_model_lib0_P->Gain2_Gain_j * rtb_Product[0]; - tmp_1[8] = (real32_T)sim_model_lib0_P->Constant3_Value_f; - for (i = 0; i < 3; i++) { - // Product: '/Product' incorporates: - // Math: '/Math Function' - - for (i_0 = 0; i_0 < 504; i_0++) { - sim_model_lib0_B->Add_l[(int32_T)(i + (int32_T)(3 * i_0))] = 0.0F; - sim_model_lib0_B->Add_l[(int32_T)(i + (int32_T)(3 * i_0))] += - rtb_Assignment_in[i] * sim_model_lib0_B->P_point_B_iss_p[i_0]; - sim_model_lib0_B->Add_l[(int32_T)(i + (int32_T)(3 * i_0))] += - rtb_Assignment_in[(int32_T)(i + 3)] * sim_model_lib0_B->P_point_B_iss_p - [(int32_T)(i_0 + 504)]; - sim_model_lib0_B->Add_l[(int32_T)(i + (int32_T)(3 * i_0))] += - rtb_Assignment_in[(int32_T)(i + 6)] * sim_model_lib0_B->P_point_B_iss_p - [(int32_T)(i_0 + 1008)]; - } - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Gain: '/Gain1' - - rtb_Subtract1[i] = sim_model_lib0_P->tun_abp_p_perchcam_body_body_sim[i] - - (real32_T)sim_model_lib0_P->tun_cvs_noise_on * - sim_model_lib0_P->cvs_perchcam_P_B_B_error[i]; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - rtb_Product_h[i] = rtb_Product[i] * rtb_Product[0]; - rtb_Product_h[(int32_T)(i + 3)] = rtb_Product[i] * rtb_Product[1]; - rtb_Product_h[(int32_T)(i + 6)] = rtb_Product[i] * rtb_Product[2]; - } - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product1' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_in[(int32_T)(3 * i_0)] = (rtb_Conversion_i[(int32_T)(3 * i_0)] - - tmp_1[(int32_T)(3 * i_0)] * rtb_Sum) + rtb_Product_h[(int32_T)(3 * i_0)] - * sim_model_lib0_P->Gain2_Gain_dy; - rtb_Assignment_in[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Conversion_i - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_1[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Sum) + rtb_Product_h[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_dy; - rtb_Assignment_in[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Conversion_i - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_1[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Sum) + rtb_Product_h[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_dy; - } - - // End of Sum: '/Sum1' - for (i_0 = 0; i_0 < 504; i_0++) { - // Concatenate: '/Matrix Concatenate1' incorporates: - // Product: '/Product1' - // Selector: '/Select_X1' - // Selector: '/Select_Y1' - // Selector: '/Select_Z1' - // Sum: '/Subtract4' - // Sum: '/Subtract5' - // Sum: '/Subtract6' - - sim_model_lib0_B->rtb_Add_l_g[(int32_T)(3 * i_0)] = sim_model_lib0_B->Add_l - [(int32_T)(3 * i_0)] - rtb_Subtract1[0]; - sim_model_lib0_B->rtb_Add_l_g[(int32_T)(1 + (int32_T)(3 * i_0))] = - sim_model_lib0_B->Add_l[(int32_T)((int32_T)(3 * i_0) + 1)] - - rtb_Subtract1[1]; - sim_model_lib0_B->rtb_Add_l_g[(int32_T)(2 + (int32_T)(3 * i_0))] = - sim_model_lib0_B->Add_l[(int32_T)((int32_T)(3 * i_0) + 2)] - - rtb_Subtract1[2]; - - // Math: '/Math Function1' incorporates: - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - sim_model_lib0_B->P_point_B_iss_p[(int32_T)(i_0 + (int32_T)(504 * i))] = - 0.0F; - sim_model_lib0_B->P_point_B_iss_p[(int32_T)(i_0 + (int32_T)(504 * i))] += - sim_model_lib0_B->rtb_Add_l_g[(int32_T)(3 * i_0)] * rtb_Assignment_in[i]; - sim_model_lib0_B->P_point_B_iss_p[(int32_T)(i_0 + (int32_T)(504 * i))] += - sim_model_lib0_B->rtb_Add_l_g[(int32_T)((int32_T)(3 * i_0) + 1)] * - rtb_Assignment_in[(int32_T)(i + 3)]; - sim_model_lib0_B->P_point_B_iss_p[(int32_T)(i_0 + (int32_T)(504 * i))] += - sim_model_lib0_B->rtb_Add_l_g[(int32_T)((int32_T)(3 * i_0) + 2)] * - rtb_Assignment_in[(int32_T)(i + 6)]; - } - - // End of Math: '/Math Function1' - } - - // Gain: '/Gain' incorporates: - // DataTypeConversion: '/Data Type Conversion2' - // RandomNumber: '/pixel_noise' - - rtb_Sum = (real32_T)sim_model_lib0_P->tun_cvs_noise_on * (real32_T) - sim_model_lib0_DW->NextOutput_o; - - // Outputs for Iterator SubSystem: '/pinhole_projection_model' - si_pinhole_projection_model(504, sim_model_lib0_B->P_point_B_iss_p, - ImpAsg_InsertedFor_P_points_2_g, - sim_model_lib0_B->ImpAsg_InsertedFor_points_in__n, - sim_model_lib0_DW->pinhole_projection_model_i, - (P_pinhole_projection_model_si_T *) - &sim_model_lib0_P->pinhole_projection_model_i, - sim_model_lib0_P->tun_cvs_noise_on, - sim_model_lib0_P->tun_cvs_perchcam_focal_length_Y, - sim_model_lib0_P->tun_cvs_perchcam_focal_length_X, - sim_model_lib0_P->handrail_image_processing_pixel, - sim_model_lib0_P->tun_cvs_perchcam_num_pixels_X, - sim_model_lib0_P->tun_cvs_perchcam_num_pixels_Y, - sim_model_lib0_P->cvs_perchcam_min_dist, - sim_model_lib0_P->cvs_perchcam_max_dist, - sim_model_lib0_P->cvs_perchcam_pointing); - - // End of Outputs for SubSystem: '/pinhole_projection_model' - - // Sum: '/Sum1' - // MATLAB Function 'image_processing/generate_output': ':1' - // Inputs: - // Process type: [Parameter] - // 1=Landmarks, 2=AR Tags, 3=Optical Flow, 4=Handrail - // num_pts_out = Required number of points to be reported [Parameter] - // P_point_iss_iss = ALL [x,y,z] position of points in the ISS frame **Landmark only** - // P_point_cam_cam = ALL [x,y,z] position of points in the CAM frame **Handrail only** - // P_points_2D_cam = ALL [u,v] pixel location of points in the camera **Landmark and OF only** - // valid_flag = indicates which points are valid - // num_hist_kept = Required number of histories kept for Optical Flow [Parameter] **OF only** - // flow_ids = ID tag for each optical flow point [Parameter] **OF only** - // - // Outputs: - // points_out_iss = output [x,y,z] position of points in the ISS frame **This output unused by optical flow** - // points_out_2D_cam = output [u,v] pixel location of points in the camera **This output unused by handrail** - // points_out_cam = output [x,y,z] position of points in the CAM frame **This output unused by OF and landmarks** - // valid_out = indicates which values of the output are valid! **This output unused by landmarks** - // ids_out = optical flow IDs - // registration_id = visual odometery registration pulse ID **ONLY used by OF** - // handrail - // ':1:25' - for (i_0 = 0; i_0 < 1512; i_0++) { - sim_model_lib0_B->Add_l[i_0] = sim_model_lib0_B->P_point_B_iss_p[i_0] + - rtb_Sum; - } - - // End of Sum: '/Sum1' - - // MATLAB Function: '/generate_output' incorporates: - // Constant: '/Constant3' - - sim_mode_format_handrail_output(sim_model_lib0_P->cvs_handrail_map_iss, - sim_model_lib0_B->Add_l, sim_model_lib0_B->ImpAsg_InsertedFor_points_in__n, - sim_model_lib0_B->points_out_iss_h, sim_model_lib0_B->points_out_3D_cam_n, - valid_out_i, sim_model_lib0_B, sim_model_lib0_DW); - - // DataTypeConversion: '/Data Type Conversion4' incorporates: - // Constant: '/Constant3' - // Logic: '/Logical Operator' - // Selector: '/select_current_command' - // UnitDelay: '/Unit Delay' - - // ':1:25' - // ':1:26' - // ':1:27' - // ':1:28' - // end of function - for (i = 0; i < 50; i++) { - rtb_DataTypeConversion4[i] = (int8_T)((valid_out_i[i] != 0.0F) && - sim_model_lib0_P->cvs_handrail_valid_mask[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_g - 1)]); - } - - // End of DataTypeConversion: '/Data Type Conversion4' - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Compare' - // Selector: '/Select_col_1' - // Selector: '/Select_col_2' - - i_0 = (int32_T)(uint32_T)((uint32_T)((uint32_T) - ((sim_model_lib0_P->cvs_handrail_valid_times[0] != - sim_model_lib0_P->Constant_Value_c0) && - ((sim_model_lib0_P->cvs_handrail_valid_times[0] < - sim_model_lib0_B->RateTransition4_m.timestamp_sec) || - ((sim_model_lib0_P->cvs_handrail_valid_times[0] == - sim_model_lib0_B->RateTransition4_m.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_m.timestamp_nsec >= - sim_model_lib0_P->cvs_handrail_valid_times[3])))) + (uint32_T) - ((sim_model_lib0_P->cvs_handrail_valid_times[1] != - sim_model_lib0_P->Constant_Value_c0) && - ((sim_model_lib0_P->cvs_handrail_valid_times[1] < - sim_model_lib0_B->RateTransition4_m.timestamp_sec) || - ((sim_model_lib0_P->cvs_handrail_valid_times[1] == - sim_model_lib0_B->RateTransition4_m.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_m.timestamp_nsec >= - sim_model_lib0_P->cvs_handrail_valid_times[4]))))) + (uint32_T) - ((sim_model_lib0_P->cvs_handrail_valid_times[2] != - sim_model_lib0_P->Constant_Value_c0) && - ((sim_model_lib0_P->cvs_handrail_valid_times[2] < - sim_model_lib0_B->RateTransition4_m.timestamp_sec) || - ((sim_model_lib0_P->cvs_handrail_valid_times[2] == - sim_model_lib0_B->RateTransition4_m.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_m.timestamp_nsec >= - sim_model_lib0_P->cvs_handrail_valid_times[5]))))); - - // Update for RateTransition: '/Rate Transition3' - sim_model_lib0_DW->RateTransition3_Buffer0 = - sim_model_lib0_B->RateTransition4_m.timestamp_sec; - - // Update for RateTransition: '/Rate Transition1' - sim_model_lib0_DW->RateTransition1_Buffer0 = - sim_model_lib0_B->RateTransition4_m.timestamp_nsec; - - // Update for RateTransition: '/Rate Transition5' - memcpy(&sim_model_lib0_DW->RateTransition5_Buffer0[0], - &sim_model_lib0_B->points_out_iss_h[0], (uint32_T)(150U * sizeof - (real32_T))); - - // Update for RateTransition: '/Rate Transition9' - memcpy(&sim_model_lib0_DW->RateTransition9_Buffer0[0], - &sim_model_lib0_B->points_out_3D_cam_n[0], (uint32_T)(150U * sizeof - (real32_T))); - - // Update for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_DW->RateTransition7_Buffer0[i] = (uint8_T) - rtb_DataTypeConversion4[i]; - } - - // End of Update for RateTransition: '/Rate Transition7' - - // Update for RandomNumber: '/pixel_noise' - sim_model_lib0_DW->NextOutput_o = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_d) * sqrt - (sim_model_lib0_P->cvs_handrail_noise_var) + - sim_model_lib0_P->pixel_noise_Mean_a; - - // Saturate: '/Saturation' incorporates: - // Sum: '/Sum1' - - if ((uint8_T)i_0 > sim_model_lib0_P->Saturation_UpperSat_n) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_g = - sim_model_lib0_P->Saturation_UpperSat_n; - } else if ((uint8_T)i_0 < sim_model_lib0_P->Saturation_LowerSat_f) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_g = - sim_model_lib0_P->Saturation_LowerSat_f; - } else { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_g = (uint8_T)i_0; - } - - // End of Saturate: '/Saturation' -} - -// Model step function for TID3 -void sim_model_lib0_step3(RT_MODEL_sim_model_lib0_T *const sim_model_lib0_M) // Sample time: [0.272s, 0.0s] -{ - P_sim_model_lib0_T *sim_model_lib0_P = ((P_sim_model_lib0_T *) - sim_model_lib0_M->defaultParam); - B_sim_model_lib0_T *sim_model_lib0_B = ((B_sim_model_lib0_T *) - sim_model_lib0_M->blockIO); - DW_sim_model_lib0_T *sim_model_lib0_DW = ((DW_sim_model_lib0_T *) - sim_model_lib0_M->dwork); - boolean_T replace_these_points[50]; - real_T num_points_needed; - real_T num_points_to_use; - int32_T nb; - int32_T khi; - static const int8_T l[4] = { 2, 10, 13, 15 }; - - real32_T rtb_Conversion_h[9]; - real32_T rtb_Subtract1[3]; - real32_T rtb_MatrixConcatenate[16]; - real32_T rtb_Product[4]; - real32_T rtb_Sum; - real32_T rtb_Assignment[9]; - real32_T rtb_Sum_g5; - uint8_T valid_out_h[800]; - int32_T i; - boolean_T tmp; - real32_T tmp_0[9]; - real32_T rtb_Assignment_pv[9]; - real32_T tmp_1[9]; - real32_T rtb_Product_f[9]; - int32_T loop_ub; - int32_T valid_points_in_cam_sizes[2]; - real32_T new_ids_data[50]; - uint8_T new_valid_flags_data[800]; - int32_T random_order_sizes[2]; - int32_T shuffled_ids_sizes; - int8_T d_data[50]; - int32_T indx_sizes; - int32_T r_sizes; - int32_T ia_data[50]; - int32_T ib_data[50]; - uint8_T tmp_data[50]; - uint8_T tmp_data_0[750]; - int32_T tmp_sizes[2]; - real32_T valid_points_in_cam_data[100]; - int32_T valid_points_in_cam_sizes_0[2]; - int32_T tmp_sizes_0[3]; - uint8_T tmp_data_1[450]; - uint8_T tmp_data_2[300]; - int32_T tmp_sizes_1[2]; - int32_T tmp_sizes_2[2]; - real32_T tmp_data_3[900]; - int32_T tmp_sizes_3[3]; - real32_T tmp_data_4[600]; - int32_T tmp_sizes_4[3]; - int32_T tmp_sizes_5[3]; - uint8_T tmp_data_5[600]; - uint8_T tmp_data_6[150]; - real32_T tmp_data_7[300]; - int8_T tmp_data_8[50]; - real32_T tmp_2; - real32_T tmp_3; - int32_T new_valid_flags_sizes_idx_0; - real32_T rtb_Product_e; - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Conversion_h[i] = (real32_T)sim_model_lib0_P->Constant2_Value_d4[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant3' - - rtb_Conversion_h[0] = sim_model_lib0_P->tun_abp_q_body2navcam[3]; - rtb_Conversion_h[4] = sim_model_lib0_P->tun_abp_q_body2navcam[3]; - rtb_Conversion_h[8] = sim_model_lib0_P->tun_abp_q_body2navcam[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Product_f[0] = (real32_T)sim_model_lib0_P->Constant3_Value_g; - rtb_Product_f[1] = sim_model_lib0_P->tun_abp_q_body2navcam[2]; - rtb_Product_f[2] = sim_model_lib0_P->Gain_Gain_mw * - sim_model_lib0_P->tun_abp_q_body2navcam[1]; - rtb_Product_f[3] = sim_model_lib0_P->Gain1_Gain_b * - sim_model_lib0_P->tun_abp_q_body2navcam[2]; - rtb_Product_f[4] = (real32_T)sim_model_lib0_P->Constant3_Value_g; - rtb_Product_f[5] = sim_model_lib0_P->tun_abp_q_body2navcam[0]; - rtb_Product_f[6] = sim_model_lib0_P->tun_abp_q_body2navcam[1]; - rtb_Product_f[7] = sim_model_lib0_P->Gain2_Gain_ch * - sim_model_lib0_P->tun_abp_q_body2navcam[0]; - rtb_Product_f[8] = (real32_T)sim_model_lib0_P->Constant3_Value_g; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Constant: '/Constant3' - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (khi = 0; khi < 3; khi++) { - rtb_MatrixConcatenate[(int32_T)(khi << 2)] = rtb_Conversion_h[(int32_T)(3 * - khi)] + rtb_Product_f[(int32_T)(3 * khi)]; - rtb_MatrixConcatenate[(int32_T)(1 + (int32_T)(khi << 2))] = - rtb_Conversion_h[(int32_T)((int32_T)(3 * khi) + 1)] + rtb_Product_f - [(int32_T)((int32_T)(3 * khi) + 1)]; - rtb_MatrixConcatenate[(int32_T)(2 + (int32_T)(khi << 2))] = - rtb_Conversion_h[(int32_T)((int32_T)(3 * khi) + 2)] + rtb_Product_f - [(int32_T)((int32_T)(3 * khi) + 2)]; - } - - rtb_MatrixConcatenate[3] = sim_model_lib0_P->Gain1_Gain_ll * - sim_model_lib0_P->tun_abp_q_body2navcam[0]; - rtb_MatrixConcatenate[7] = sim_model_lib0_P->Gain1_Gain_ll * - sim_model_lib0_P->tun_abp_q_body2navcam[1]; - rtb_MatrixConcatenate[11] = sim_model_lib0_P->Gain1_Gain_ll * - sim_model_lib0_P->tun_abp_q_body2navcam[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - - tmp = ((int32_T)sim_model_lib0_P->tun_cvs_noise_on != 0); - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[12] = sim_model_lib0_P->tun_abp_q_body2navcam[0]; - - // Product: '/Product' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Switch: '/Switch' - - if (tmp) { - rtb_Sum = sim_model_lib0_P->cvs_navcam_Q_B2navcan_error[0]; - rtb_Sum_g5 = sim_model_lib0_P->cvs_navcam_Q_B2navcan_error[1]; - } else { - rtb_Sum = sim_model_lib0_P->Constant7_Value_m[0]; - rtb_Sum_g5 = sim_model_lib0_P->Constant7_Value_m[1]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[13] = sim_model_lib0_P->tun_abp_q_body2navcam[1]; - rtb_MatrixConcatenate[14] = sim_model_lib0_P->tun_abp_q_body2navcam[2]; - - // Product: '/Product' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Switch: '/Switch' - - if (tmp) { - tmp_3 = sim_model_lib0_P->cvs_navcam_Q_B2navcan_error[2]; - tmp_2 = sim_model_lib0_P->cvs_navcam_Q_B2navcan_error[3]; - } else { - tmp_3 = sim_model_lib0_P->Constant7_Value_m[2]; - tmp_2 = sim_model_lib0_P->Constant7_Value_m[3]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[15] = sim_model_lib0_P->tun_abp_q_body2navcam[3]; - - // Product: '/Product' - for (khi = 0; khi < 4; khi++) { - rtb_Product_e = rtb_MatrixConcatenate[(int32_T)(khi + 12)] * tmp_2 + - (rtb_MatrixConcatenate[(int32_T)(khi + 8)] * tmp_3 + - (rtb_MatrixConcatenate[(int32_T)(khi + 4)] * rtb_Sum_g5 + - rtb_MatrixConcatenate[khi] * rtb_Sum)); - rtb_Product[khi] = rtb_Product_e; - } - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum = rtb_Product[3] * rtb_Product[3] * sim_model_lib0_P->Gain_Gain_ci - - (real32_T)sim_model_lib0_P->Constant1_Value_h; - for (i = 0; i < 9; i++) { - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Conversion_h[i] = (real32_T)sim_model_lib0_P->Constant2_Value_kp[i]; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment[i] = (real32_T)sim_model_lib0_P->Constant2_Value_e[i]; - } - - // Assignment: '/Assignment' - rtb_Conversion_h[0] = rtb_Sum; - rtb_Conversion_h[4] = rtb_Sum; - rtb_Conversion_h[8] = rtb_Sum; - - // Gain: '/Gain1' - rtb_Sum = sim_model_lib0_P->Gain1_Gain_fm * rtb_Product[3]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_g5 = sim_model_lib0_B->RateTransition2_b.Q_ISS2B[3] * - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[3] * - sim_model_lib0_P->Gain_Gain_fp - (real32_T) - sim_model_lib0_P->Constant1_Value_es; - - // Assignment: '/Assignment' - rtb_Assignment[0] = rtb_Sum_g5; - rtb_Assignment[4] = rtb_Sum_g5; - rtb_Assignment[8] = rtb_Sum_g5; - - // Gain: '/Gain1' - rtb_Sum_g5 = sim_model_lib0_P->Gain1_Gain_in * - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[3]; - - // Sum: '/Add' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant4' - // Gain: '/Gain' - - for (i = 0; i < 11937; i++) { - sim_model_lib0_B->Add_f[i] = (real32_T)sim_model_lib0_P->tun_cvs_noise_on * - sim_model_lib0_P->cvs_optflow_map_error[i] + - sim_model_lib0_P->cvs_optflow_map_iss[i]; - } - - // End of Sum: '/Add' - for (khi = 0; khi < 3979; khi++) { - // Sum: '/Subtract' incorporates: - // Selector: '/Select_X' - - sim_model_lib0_B->P_point_B_iss_k[khi] = sim_model_lib0_B->Add_f[khi] - - sim_model_lib0_B->RateTransition2_b.P_B_ISS_ISS[0]; - - // Sum: '/Subtract2' incorporates: - // Selector: '/Select_Y' - - sim_model_lib0_B->P_point_B_iss_k[(int32_T)(3979 + khi)] = - sim_model_lib0_B->Add_f[(int32_T)(3979 + khi)] - - sim_model_lib0_B->RateTransition2_b.P_B_ISS_ISS[1]; - - // Sum: '/Subtract3' incorporates: - // Selector: '/Select_Z' - - sim_model_lib0_B->P_point_B_iss_k[(int32_T)(7958 + khi)] = - sim_model_lib0_B->Add_f[(int32_T)(7958 + khi)] - - sim_model_lib0_B->RateTransition2_b.P_B_ISS_ISS[2]; - } - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_0[0] = (real32_T)sim_model_lib0_P->Constant3_Value_kj; - tmp_0[1] = sim_model_lib0_B->RateTransition2_b.Q_ISS2B[2]; - tmp_0[2] = sim_model_lib0_P->Gain_Gain_lw * - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[1]; - tmp_0[3] = sim_model_lib0_P->Gain1_Gain_jqn * - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[2]; - tmp_0[4] = (real32_T)sim_model_lib0_P->Constant3_Value_kj; - tmp_0[5] = sim_model_lib0_B->RateTransition2_b.Q_ISS2B[0]; - tmp_0[6] = sim_model_lib0_B->RateTransition2_b.Q_ISS2B[1]; - tmp_0[7] = sim_model_lib0_P->Gain2_Gain_jj * - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[0]; - tmp_0[8] = (real32_T)sim_model_lib0_P->Constant3_Value_kj; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (khi = 0; khi < 3; khi++) { - rtb_Product_f[khi] = sim_model_lib0_B->RateTransition2_b.Q_ISS2B[khi] * - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[0]; - rtb_Product_f[(int32_T)(khi + 3)] = - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[khi] * - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[1]; - rtb_Product_f[(int32_T)(khi + 6)] = - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[khi] * - sim_model_lib0_B->RateTransition2_b.Q_ISS2B[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (khi = 0; khi < 3; khi++) { - rtb_Assignment_pv[(int32_T)(3 * khi)] = (rtb_Assignment[(int32_T)(3 * khi)] - - tmp_0[(int32_T)(3 * khi)] * rtb_Sum_g5) + rtb_Product_f[(int32_T)(3 * - khi)] * sim_model_lib0_P->Gain2_Gain_dk; - rtb_Assignment_pv[(int32_T)(1 + (int32_T)(3 * khi))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * khi) + 1)] - tmp_0[(int32_T)((int32_T)(3 * khi) + - 1)] * rtb_Sum_g5) + rtb_Product_f[(int32_T)((int32_T)(3 * khi) + 1)] * - sim_model_lib0_P->Gain2_Gain_dk; - rtb_Assignment_pv[(int32_T)(2 + (int32_T)(3 * khi))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * khi) + 2)] - tmp_0[(int32_T)((int32_T)(3 * khi) + - 2)] * rtb_Sum_g5) + rtb_Product_f[(int32_T)((int32_T)(3 * khi) + 2)] * - sim_model_lib0_P->Gain2_Gain_dk; - } - - // End of Sum: '/Sum1' - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_1[0] = (real32_T)sim_model_lib0_P->Constant3_Value_c2; - tmp_1[1] = rtb_Product[2]; - tmp_1[2] = sim_model_lib0_P->Gain_Gain_d5 * rtb_Product[1]; - tmp_1[3] = sim_model_lib0_P->Gain1_Gain_hfz * rtb_Product[2]; - tmp_1[4] = (real32_T)sim_model_lib0_P->Constant3_Value_c2; - tmp_1[5] = rtb_Product[0]; - tmp_1[6] = rtb_Product[1]; - tmp_1[7] = sim_model_lib0_P->Gain2_Gain_ee * rtb_Product[0]; - tmp_1[8] = (real32_T)sim_model_lib0_P->Constant3_Value_c2; - for (i = 0; i < 3; i++) { - // Product: '/Product' incorporates: - // Math: '/Math Function' - - for (khi = 0; khi < 3979; khi++) { - sim_model_lib0_B->Add_f[(int32_T)(i + (int32_T)(3 * khi))] = 0.0F; - sim_model_lib0_B->Add_f[(int32_T)(i + (int32_T)(3 * khi))] += - rtb_Assignment_pv[i] * sim_model_lib0_B->P_point_B_iss_k[khi]; - sim_model_lib0_B->Add_f[(int32_T)(i + (int32_T)(3 * khi))] += - rtb_Assignment_pv[(int32_T)(i + 3)] * sim_model_lib0_B->P_point_B_iss_k - [(int32_T)(khi + 3979)]; - sim_model_lib0_B->Add_f[(int32_T)(i + (int32_T)(3 * khi))] += - rtb_Assignment_pv[(int32_T)(i + 6)] * sim_model_lib0_B->P_point_B_iss_k - [(int32_T)(khi + 7958)]; - } - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Gain: '/Gain1' - - rtb_Subtract1[i] = sim_model_lib0_P->tun_abp_p_navcam_body_body_sim[i] - - (real32_T)sim_model_lib0_P->tun_cvs_noise_on * - sim_model_lib0_P->cvs_navcam_P_B_B_error[i]; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - rtb_Product_f[i] = rtb_Product[i] * rtb_Product[0]; - rtb_Product_f[(int32_T)(i + 3)] = rtb_Product[i] * rtb_Product[1]; - rtb_Product_f[(int32_T)(i + 6)] = rtb_Product[i] * rtb_Product[2]; - } - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product1' - // Product: '/Product' - - for (khi = 0; khi < 3; khi++) { - rtb_Assignment_pv[(int32_T)(3 * khi)] = (rtb_Conversion_h[(int32_T)(3 * khi)] - - tmp_1[(int32_T)(3 * khi)] * rtb_Sum) + rtb_Product_f[(int32_T)(3 * khi)] - * sim_model_lib0_P->Gain2_Gain_he; - rtb_Assignment_pv[(int32_T)(1 + (int32_T)(3 * khi))] = (rtb_Conversion_h - [(int32_T)((int32_T)(3 * khi) + 1)] - tmp_1[(int32_T)((int32_T)(3 * khi) + - 1)] * rtb_Sum) + rtb_Product_f[(int32_T)((int32_T)(3 * khi) + 1)] * - sim_model_lib0_P->Gain2_Gain_he; - rtb_Assignment_pv[(int32_T)(2 + (int32_T)(3 * khi))] = (rtb_Conversion_h - [(int32_T)((int32_T)(3 * khi) + 2)] - tmp_1[(int32_T)((int32_T)(3 * khi) + - 2)] * rtb_Sum) + rtb_Product_f[(int32_T)((int32_T)(3 * khi) + 2)] * - sim_model_lib0_P->Gain2_Gain_he; - } - - // End of Sum: '/Sum1' - for (khi = 0; khi < 3979; khi++) { - // Concatenate: '/Matrix Concatenate1' incorporates: - // Product: '/Product1' - // Selector: '/Select_X1' - // Selector: '/Select_Y1' - // Selector: '/Select_Z1' - // Sum: '/Subtract4' - // Sum: '/Subtract5' - // Sum: '/Subtract6' - - sim_model_lib0_B->rtb_Add_f_b[(int32_T)(3 * khi)] = sim_model_lib0_B->Add_f - [(int32_T)(3 * khi)] - rtb_Subtract1[0]; - sim_model_lib0_B->rtb_Add_f_b[(int32_T)(1 + (int32_T)(3 * khi))] = - sim_model_lib0_B->Add_f[(int32_T)((int32_T)(3 * khi) + 1)] - - rtb_Subtract1[1]; - sim_model_lib0_B->rtb_Add_f_b[(int32_T)(2 + (int32_T)(3 * khi))] = - sim_model_lib0_B->Add_f[(int32_T)((int32_T)(3 * khi) + 2)] - - rtb_Subtract1[2]; - - // Math: '/Math Function1' incorporates: - // Product: '/Product1' - - for (indx_sizes = 0; indx_sizes < 3; indx_sizes++) { - sim_model_lib0_B->P_point_B_iss_k[(int32_T)(khi + (int32_T)(3979 * - indx_sizes))] = 0.0F; - sim_model_lib0_B->P_point_B_iss_k[(int32_T)(khi + (int32_T)(3979 * - indx_sizes))] += sim_model_lib0_B->rtb_Add_f_b[(int32_T)(3 * khi)] * - rtb_Assignment_pv[indx_sizes]; - sim_model_lib0_B->P_point_B_iss_k[(int32_T)(khi + (int32_T)(3979 * - indx_sizes))] += sim_model_lib0_B->rtb_Add_f_b[(int32_T)((int32_T)(3 * - khi) + 1)] * rtb_Assignment_pv[(int32_T)(indx_sizes + 3)]; - sim_model_lib0_B->P_point_B_iss_k[(int32_T)(khi + (int32_T)(3979 * - indx_sizes))] += sim_model_lib0_B->rtb_Add_f_b[(int32_T)((int32_T)(3 * - khi) + 2)] * rtb_Assignment_pv[(int32_T)(indx_sizes + 6)]; - } - - // End of Math: '/Math Function1' - } - - // Outputs for Iterator SubSystem: '/pinhole_projection_model' - si_pinhole_projection_model(3979, sim_model_lib0_B->P_point_B_iss_k, - sim_model_lib0_B->ImpAsg_InsertedFor_P_points_2D_, - sim_model_lib0_B->ImpAsg_InsertedFor_points_in_FO, - sim_model_lib0_DW->pinhole_projection_model_o, - (P_pinhole_projection_model_si_T *) - &sim_model_lib0_P->pinhole_projection_model_o, - sim_model_lib0_P->tun_cvs_noise_on, - sim_model_lib0_P->tun_cvs_navcam_focal_length_Y, - sim_model_lib0_P->tun_cvs_navcam_focal_length_X, - sim_model_lib0_P->optical_flow_image_processing_p, - sim_model_lib0_P->tun_cvs_navcam_num_pixels_X, - sim_model_lib0_P->tun_cvs_navcam_num_pixels_Y, - sim_model_lib0_P->cvs_navcam_min_dist, sim_model_lib0_P->cvs_navcam_max_dist, - sim_model_lib0_P->cvs_navcam_pointing); - - // End of Outputs for SubSystem: '/pinhole_projection_model' - - // MATLAB Function: '/generate_output' - // MATLAB Function 'image_processing/generate_output': ':1' - // Inputs: - // Process type: [Parameter] - // 1=Landmarks, 2=AR Tags, 3=Optical Flow, 4=Handrail - // num_pts_out = Required number of points to be reported [Parameter] - // P_point_iss_iss = ALL [x,y,z] position of points in the ISS frame **Landmark only** - // P_point_cam_cam = ALL [x,y,z] position of points in the CAM frame **Handrail only** - // P_points_2D_cam = ALL [u,v] pixel location of points in the camera **Landmark and OF only** - // valid_flag = indicates which points are valid - // num_hist_kept = Required number of histories kept for Optical Flow [Parameter] **OF only** - // flow_ids = ID tag for each optical flow point [Parameter] **OF only** - // - // Outputs: - // points_out_iss = output [x,y,z] position of points in the ISS frame **This output unused by optical flow** - // points_out_2D_cam = output [u,v] pixel location of points in the camera **This output unused by handrail** - // points_out_cam = output [x,y,z] position of points in the CAM frame **This output unused by OF and landmarks** - // valid_out = indicates which values of the output are valid! **This output unused by landmarks** - // ids_out = optical flow IDs - // registration_id = visual odometery registration pulse ID **ONLY used by OF** - // optical flow - // ':1:30' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // format_visOD_output.m - // inputs: - // num_pts_out = Required number of points to be reported - // points_in iss = ALL [x,y,z] position of points in the ISS frame - // points_in_cam = ALL [u,v] pixel location of points in the camera - // valid_in = indicates which points are valid - // num_augments = Required number of histories kept for Optical Flow (ASSUMED to be >=2) - // flow_ids = ID tag for each optical flow point (OF only) - // - // outputs: - // observati_out = output [u,v] pixel location of points in the camera - // valid_out = indicates which values of the output are valid! - // ids_out = optical flow IDs - // if number of points out changes, change this number. - // order of the replacement augmentations - // note: real FSW has a fancy way to determine the last augmentation to replace that changes between 13, 14 or 15. Here we simplify to just take 15 every time - i = 0; - - // Define Persistance Variables - // Either Initalize the variables (1st time only), or shift the history over - if (!sim_model_lib0_DW->int_id_hist_not_empty) { - sim_model_lib0_DW->int_id_hist_not_empty = true; - memset(&sim_model_lib0_DW->int_observations[0], 0, (uint32_T)(1600U * sizeof - (real32_T))); - memset(&sim_model_lib0_DW->int_valid_flag[0], 0, (uint32_T)(800U * sizeof - (uint8_T))); - sim_model_lib0_DW->int_registration_number = 1U; - sim_model_lib0_DW->int_initalization_complete = 0.0; - } else { - // until 16 augmentations have been captured, just keep incrementing number. Then after that, start rotating through the registration numbers - if (sim_model_lib0_DW->int_initalization_complete < 15.0) { - sim_model_lib0_DW->int_initalization_complete++; - memset(&valid_points_in_cam_data[0], 0, (uint32_T)(100U * sizeof(real32_T))); - for (khi = 0; khi < 2; khi++) { - for (indx_sizes = 0; indx_sizes < 15; indx_sizes++) { - memcpy(&sim_model_lib0_B->tmp_data_l[(int32_T)((int32_T)(khi * 50) + - (int32_T)(indx_sizes * 100))], - &sim_model_lib0_DW->int_observations[(int32_T)((int32_T)(khi * - 50) + (int32_T)(indx_sizes * 100))], (uint32_T)(50U * sizeof - (real32_T))); - } - } - - cbiephdblnopophl_cat(sim_model_lib0_B->tmp_data_l, - sim_model_lib0_DW->int_observations); - memset(&new_ids_data[0], 0, (uint32_T)(50U * sizeof(real32_T))); - for (khi = 0; khi < 15; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - tmp_data_0[(int32_T)(indx_sizes + (int32_T)(50 * khi))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)(50 * khi) + - indx_sizes)]; - } - } - - mglflfcbcbimecje_cat(tmp_data_0, sim_model_lib0_DW->int_valid_flag); - sim_model_lib0_DW->int_registration_number = 1U; - } else { - switch (sim_model_lib0_DW->int_registration_number) { - case 2U: - // the registration pulse should rotated between augmentations [0, 2, 6, 15] - sim_model_lib0_DW->int_registration_number = 10U; - - // remove the 3rd augmentation (zero based index) - tmp_sizes_3[0] = 50; - tmp_sizes_3[1] = 2; - tmp_sizes_3[2] = 9; - for (khi = 0; khi < 9; khi++) { - for (indx_sizes = 0; indx_sizes < 2; indx_sizes++) { - memcpy(&tmp_data_3[(int32_T)((int32_T)(khi * 100) + (int32_T) - (indx_sizes * 50))], &sim_model_lib0_DW->int_observations - [(int32_T)((int32_T)(khi * 100) + (int32_T)(indx_sizes * 50))], - (uint32_T)(50U * sizeof(real32_T))); - } - } - - tmp_sizes_4[0] = 50; - tmp_sizes_4[1] = 2; - tmp_sizes_4[2] = 6; - for (khi = 0; khi < 6; khi++) { - for (indx_sizes = 0; indx_sizes < 2; indx_sizes++) { - memcpy(&tmp_data_4[(int32_T)((int32_T)(khi * 100) + (int32_T) - (indx_sizes * 50))], &sim_model_lib0_DW->int_observations - [(int32_T)((int32_T)((int32_T)(khi * 100) + (int32_T) - (indx_sizes * 50)) + 1000)], (uint32_T)(50U * sizeof - (real32_T))); - } - } - - ophlngdbgdjmgdje_cat(tmp_data_3, tmp_sizes_3, tmp_data_4, tmp_sizes_4, - sim_model_lib0_B->tmp_data_d, tmp_sizes_5); - memcpy(&sim_model_lib0_DW->int_observations[0], - &sim_model_lib0_B->tmp_data_d[0], (uint32_T)(1600U * sizeof - (real32_T))); - random_order_sizes[0] = 50; - random_order_sizes[1] = 9; - for (khi = 0; khi < 9; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - tmp_data_1[(int32_T)(indx_sizes + (int32_T)(50 * khi))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)(50 * khi) + - indx_sizes)]; - } - } - - tmp_sizes_1[0] = 50; - tmp_sizes_1[1] = 6; - for (khi = 0; khi < 6; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - tmp_data_2[(int32_T)(indx_sizes + (int32_T)(50 * khi))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)((int32_T)(10 - + khi) * 50) + indx_sizes)]; - } - } - - dbiehlnglnohaaai_cat(tmp_data_1, random_order_sizes, tmp_data_2, - tmp_sizes_1, valid_out_h, tmp_sizes_2); - memcpy(&sim_model_lib0_DW->int_valid_flag[0], &valid_out_h[0], (uint32_T) - (800U * sizeof(uint8_T))); - break; - - case 10U: - sim_model_lib0_DW->int_registration_number = 13U; - - // remove the 7th augmentation (zero based index) - tmp_sizes_3[0] = 50; - tmp_sizes_3[1] = 2; - tmp_sizes_3[2] = 12; - for (khi = 0; khi < 12; khi++) { - for (indx_sizes = 0; indx_sizes < 2; indx_sizes++) { - memcpy(&sim_model_lib0_B->tmp_data_dh[(int32_T)((int32_T)(khi * 100) - + (int32_T)(indx_sizes * 50))], - &sim_model_lib0_DW->int_observations[(int32_T)((int32_T)(khi * - 100) + (int32_T)(indx_sizes * 50))], (uint32_T)(50U * - sizeof(real32_T))); - } - } - - tmp_sizes_4[0] = 50; - tmp_sizes_4[1] = 2; - tmp_sizes_4[2] = 3; - for (khi = 0; khi < 3; khi++) { - for (indx_sizes = 0; indx_sizes < 2; indx_sizes++) { - memcpy(&tmp_data_7[(int32_T)((int32_T)(khi * 100) + (int32_T) - (indx_sizes * 50))], &sim_model_lib0_DW->int_observations - [(int32_T)((int32_T)((int32_T)(khi * 100) + (int32_T) - (indx_sizes * 50)) + 1300)], (uint32_T)(50U * sizeof - (real32_T))); - } - } - - ophlngdbgdjmgdje_cat(sim_model_lib0_B->tmp_data_dh, tmp_sizes_3, - tmp_data_7, tmp_sizes_4, - sim_model_lib0_B->tmp_data_d, tmp_sizes_5); - memcpy(&sim_model_lib0_DW->int_observations[0], - &sim_model_lib0_B->tmp_data_d[0], (uint32_T)(1600U * sizeof - (real32_T))); - random_order_sizes[0] = 50; - random_order_sizes[1] = 12; - for (khi = 0; khi < 12; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - tmp_data_5[(int32_T)(indx_sizes + (int32_T)(50 * khi))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)(50 * khi) + - indx_sizes)]; - } - } - - tmp_sizes_1[0] = 50; - tmp_sizes_1[1] = 3; - for (khi = 0; khi < 3; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - tmp_data_6[(int32_T)(indx_sizes + (int32_T)(50 * khi))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)((int32_T)(13 - + khi) * 50) + indx_sizes)]; - } - } - - dbiehlnglnohaaai_cat(tmp_data_5, random_order_sizes, tmp_data_6, - tmp_sizes_1, valid_out_h, tmp_sizes_2); - memcpy(&sim_model_lib0_DW->int_valid_flag[0], &valid_out_h[0], (uint32_T) - (800U * sizeof(uint8_T))); - break; - - case 13U: - sim_model_lib0_DW->int_registration_number = 15U; - - // remove the last augmentation (zero based index) - memset(&valid_points_in_cam_data[0], 0, (uint32_T)(100U * sizeof - (real32_T))); - for (khi = 0; khi < 2; khi++) { - for (indx_sizes = 0; indx_sizes < 15; indx_sizes++) { - memcpy(&sim_model_lib0_B->tmp_data_l[(int32_T)((int32_T)(khi * 50) + - (int32_T)(indx_sizes * 100))], - &sim_model_lib0_DW->int_observations[(int32_T)((int32_T)(khi * - 50) + (int32_T)(indx_sizes * 100))], (uint32_T)(50U * - sizeof(real32_T))); - } - } - - cbiephdblnopophl_cat(sim_model_lib0_B->tmp_data_l, - sim_model_lib0_DW->int_observations); - memset(&new_ids_data[0], 0, (uint32_T)(50U * sizeof(real32_T))); - for (khi = 0; khi < 15; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - tmp_data_0[(int32_T)(indx_sizes + (int32_T)(50 * khi))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)(50 * khi) + - indx_sizes)]; - } - } - - mglflfcbcbimecje_cat(tmp_data_0, sim_model_lib0_DW->int_valid_flag); - break; - - default: - i = 1; - - // after capturing the last image, set flag to report - sim_model_lib0_DW->int_registration_number = 2U; - - // remove the first augmentation (zero based index) - memset(&valid_points_in_cam_data[0], 0, (uint32_T)(100U * sizeof - (real32_T))); - for (khi = 0; khi < 2; khi++) { - for (indx_sizes = 0; indx_sizes < 15; indx_sizes++) { - memcpy(&sim_model_lib0_B->tmp_data_l[(int32_T)((int32_T)(khi * 50) + - (int32_T)(indx_sizes * 100))], - &sim_model_lib0_DW->int_observations[(int32_T)((int32_T) - ((int32_T)(khi * 50) + (int32_T)(indx_sizes * 100)) + 100)], - (uint32_T)(50U * sizeof(real32_T))); - } - } - - cbiephdblnopophl_cat(sim_model_lib0_B->tmp_data_l, - sim_model_lib0_DW->int_observations); - memset(&new_ids_data[0], 0, (uint32_T)(50U * sizeof(real32_T))); - for (khi = 0; khi < 15; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - tmp_data_0[(int32_T)(indx_sizes + (int32_T)(50 * khi))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)((int32_T)(1 - + khi) * 50) + indx_sizes)]; - } - } - - mglflfcbcbimecje_cat(tmp_data_0, sim_model_lib0_DW->int_valid_flag); - break; - } - } - } - - // pull out the currently valid points as marked by the valid flag input - nb = 0; - for (khi = 0; khi < 3979; khi++) { - if (sim_model_lib0_B->ImpAsg_InsertedFor_points_in_FO[khi]) { - nb++; - } - } - - shuffled_ids_sizes = nb; - nb = 0; - for (khi = 0; khi < 3979; khi++) { - if (sim_model_lib0_B->ImpAsg_InsertedFor_points_in_FO[khi]) { - sim_model_lib0_B->shuffled_ids_data[nb] = (int32_T)(khi + 1); - nb++; - } - } - - nb = 0; - for (khi = 0; khi < 3979; khi++) { - if (sim_model_lib0_B->ImpAsg_InsertedFor_points_in_FO[khi]) { - nb++; - } - } - - indx_sizes = nb; - nb = 0; - for (khi = 0; khi < 3979; khi++) { - if (sim_model_lib0_B->ImpAsg_InsertedFor_points_in_FO[khi]) { - sim_model_lib0_B->indx_data_g[nb] = (int32_T)(khi + 1); - nb++; - } - } - - valid_points_in_cam_sizes[0] = indx_sizes; - valid_points_in_cam_sizes[1] = 2; - for (khi = 0; khi <= (int32_T)(indx_sizes - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data[khi] = - sim_model_lib0_B->ImpAsg_InsertedFor_P_points_2D_[(int32_T) - (sim_model_lib0_B->indx_data_g[khi] - 1)]; - } - - for (khi = 0; khi <= (int32_T)(indx_sizes - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)(khi + indx_sizes)] = - sim_model_lib0_B->ImpAsg_InsertedFor_P_points_2D_[(int32_T) - (sim_model_lib0_B->indx_data_g[khi] + 3978)]; - } - - // Find unique (and valid) 2D points in the cam frame. Can be thought of as either: - // a) points blocked by other points are not visible to the camera, despite technically being in the FOV of the camera - // b) Only can have 1 point reported per pixel location - if (indx_sizes != 0) { - sim_m_mglflfcbkfkfbiec_sortrows(sim_model_lib0_B->valid_points_in_cam_data, - valid_points_in_cam_sizes, sim_model_lib0_B->idx_data_c, &khi, - sim_model_lib0_B); - nb = 0; - khi = 1; - while (khi <= indx_sizes) { - r_sizes = khi; - do { - khi++; - } while (!((khi > indx_sizes) || fcbadjmgohlnpphl_rows_differ - (sim_model_lib0_B->valid_points_in_cam_data, - valid_points_in_cam_sizes, r_sizes, khi))); - - nb++; - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)(nb - 1)] = - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)(r_sizes - 1)]; - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)((int32_T)(nb + - valid_points_in_cam_sizes[0]) - 1)] = - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)((int32_T)(r_sizes + - valid_points_in_cam_sizes[0]) - 1)]; - sim_model_lib0_B->idx_data_c[(int32_T)(nb - 1)] = - sim_model_lib0_B->idx_data_c[(int32_T)(r_sizes - 1)]; - } - - if (1 > nb) { - loop_ub = 0; - } else { - loop_ub = nb; - } - - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data_f[khi] = - sim_model_lib0_B->valid_points_in_cam_data[khi]; - } - - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data_f[(int32_T)(khi + loop_ub)] = - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)(khi + - valid_points_in_cam_sizes[0])]; - } - - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data[khi] = - sim_model_lib0_B->valid_points_in_cam_data_f[khi]; - } - - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)(khi + loop_ub)] = - sim_model_lib0_B->valid_points_in_cam_data_f[(int32_T)(khi + loop_ub)]; - } - - indx_sizes = nb; - for (khi = 0; (int32_T)(khi + 1) <= nb; khi++) { - sim_model_lib0_B->indx_data_g[khi] = (int32_T)sim_model_lib0_B-> - idx_data_c[khi]; - } - - sim_mo_dbaaphlnfkfkphdb_sortIdx(sim_model_lib0_B->indx_data_g, nb, - sim_model_lib0_B->r_data, &r_sizes, sim_model_lib0_B); - for (khi = 0; khi <= (int32_T)(r_sizes - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data_f[khi] = - sim_model_lib0_B->valid_points_in_cam_data[(int32_T) - (sim_model_lib0_B->r_data[khi] - 1)]; - } - - for (khi = 0; khi <= (int32_T)(r_sizes - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data_f[(int32_T)(khi + r_sizes)] = - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)((int32_T) - (sim_model_lib0_B->r_data[khi] + loop_ub) - 1)]; - } - - valid_points_in_cam_sizes[0] = r_sizes; - for (khi = 0; khi <= (int32_T)(r_sizes - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data[khi] = - sim_model_lib0_B->valid_points_in_cam_data_f[khi]; - } - - for (khi = 0; khi <= (int32_T)(r_sizes - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)(khi + r_sizes)] = - sim_model_lib0_B->valid_points_in_cam_data_f[(int32_T)(khi + r_sizes)]; - } - - for (khi = 0; (int32_T)(khi + 1) <= nb; khi++) { - sim_model_lib0_B->indx_data_g[khi] = (int32_T)sim_model_lib0_B-> - idx_data_c[(int32_T)(sim_model_lib0_B->r_data[khi] - 1)]; - } - } - - // finds unique rows, and 'stable' preserves the current order instead of sorting - for (khi = 0; khi <= (int32_T)(shuffled_ids_sizes - 1); khi++) { - sim_model_lib0_B->tmp_data[khi] = (int16_T) - sim_model_lib0_B->shuffled_ids_data[khi]; - } - - for (khi = 0; khi <= (int32_T)(indx_sizes - 1); khi++) { - sim_model_lib0_B->valid_ids_data[khi] = (real32_T)sim_model_lib0_B-> - tmp_data[(int32_T)(sim_model_lib0_B->indx_data_g[khi] - 1)]; - } - - // If any historical valid points are currently valid, capture this info - hdbiaaaabiecmgdb_do_vectors(sim_model_lib0_DW->int_id_hist, - sim_model_lib0_B->valid_ids_data, indx_sizes, new_ids_data, &khi, ia_data, - &shuffled_ids_sizes, ib_data, &r_sizes); - if (!(shuffled_ids_sizes == 0)) { - for (khi = 0; khi <= (int32_T)(r_sizes - 1); khi++) { - sim_model_lib0_DW->int_observations[(int32_T)(ia_data[khi] - 1)] = - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)(ib_data[khi] - 1)]; - } - - for (khi = 0; khi <= (int32_T)(r_sizes - 1); khi++) { - sim_model_lib0_DW->int_observations[(int32_T)(ia_data[khi] + 49)] = - sim_model_lib0_B->valid_points_in_cam_data[(int32_T)((int32_T) - (ib_data[khi] + valid_points_in_cam_sizes[0]) - 1)]; - } - - for (khi = 0; khi <= (int32_T)(shuffled_ids_sizes - 1); khi++) { - sim_model_lib0_DW->int_valid_flag[(int32_T)(ia_data[khi] - 1)] = 1U; - } - } - - // Determine which points need to be replaced. Keep any points that are still valid this history, - for (khi = 0; khi < 50; khi++) { - replace_these_points[khi] = ((int32_T)sim_model_lib0_DW->int_valid_flag[khi] - == 0); - } - - num_points_needed = jecjcjmgfkfkophl_sum(replace_these_points); - if ((50.0 <= num_points_needed) || rtIsNaN(num_points_needed)) { - num_points_needed = 50.0; - } - - // Replace any points needed - if (num_points_needed > 0.0) { - // initialize new slots with zeros (since we may not fill it, want the remaining to be zeros already) - loop_ub = (int32_T)num_points_needed; - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - new_ids_data[khi] = 0.0F; - } - - r_sizes = (int32_T)num_points_needed; - loop_ub = (int32_T)((int32_T)num_points_needed << 5); - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - sim_model_lib0_B->new_observations_data[khi] = 0.0F; - } - - new_valid_flags_sizes_idx_0 = (int32_T)num_points_needed; - loop_ub = (int32_T)((int32_T)num_points_needed << 4); - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - new_valid_flags_data[khi] = 0U; - } - - // find the unused valid points (points not already in augmentations) - sim_fkfchlfkgdbimgdj_do_vectors(sim_model_lib0_DW->int_id_hist, - sim_model_lib0_B->valid_ids_data, indx_sizes, sim_model_lib0_B->b_c_data, - &khi, ia_data, &shuffled_ids_sizes, sim_model_lib0_B->indx_data_g, - &indx_sizes, sim_model_lib0_B); - for (khi = 0; khi <= (int32_T)(indx_sizes - 1); khi++) { - sim_model_lib0_B->idx_data_c[khi] = (real_T)sim_model_lib0_B-> - indx_data_g[khi]; - } - - if (indx_sizes > 0) { - // shuffle the points avaliable - sim_model_lib0_randperm_i((real_T)indx_sizes, - sim_model_lib0_B->random_order_data, random_order_sizes, - sim_model_lib0_B, sim_model_lib0_DW); - shuffled_ids_sizes = random_order_sizes[1]; - loop_ub = random_order_sizes[1]; - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - sim_model_lib0_B->shuffled_ids_data[khi] = (int32_T) - sim_model_lib0_B->idx_data_c[(int32_T)((int32_T) - sim_model_lib0_B->random_order_data[(int32_T)(random_order_sizes[0] * - khi)] - 1)]; - } - - // determine if num_points_avail>num_points needed, then verify its valid_ids_data - [(int32_T)(sim_model_lib0_B->shuffled_ids_data[(int32_T)((int32_T) - tmp_data_8[khi] - 1)] - 1)]; - } - - for (khi = 0; khi <= (int32_T)(shuffled_ids_sizes - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data_f[khi] = - sim_model_lib0_B->valid_points_in_cam_data[(int32_T) - (sim_model_lib0_B->shuffled_ids_data[khi] - 1)]; - } - - for (khi = 0; khi <= (int32_T)(shuffled_ids_sizes - 1); khi++) { - sim_model_lib0_B->valid_points_in_cam_data_f[(int32_T)(khi + - shuffled_ids_sizes)] = sim_model_lib0_B->valid_points_in_cam_data - [(int32_T)((int32_T)(sim_model_lib0_B->shuffled_ids_data[khi] + - valid_points_in_cam_sizes[0]) - 1)]; - } - - valid_points_in_cam_sizes_0[0] = (int32_T)num_points_to_use; - valid_points_in_cam_sizes_0[1] = 2; - loop_ub = (int32_T)num_points_to_use; - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - valid_points_in_cam_data[khi] = - sim_model_lib0_B->valid_points_in_cam_data_f[khi]; - } - - loop_ub = (int32_T)num_points_to_use; - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - valid_points_in_cam_data[(int32_T)(khi + valid_points_in_cam_sizes_0[0])] - = sim_model_lib0_B->valid_points_in_cam_data_f[(int32_T)(khi + - shuffled_ids_sizes)]; - } - - tmp_sizes_0[0] = (int32_T)num_points_to_use; - tmp_sizes_0[1] = 2; - tmp_sizes_0[2] = 15; - loop_ub = (int32_T)((int32_T)((int32_T)num_points_to_use << 1) * 15); - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - sim_model_lib0_B->tmp_data_l[khi] = 0.0F; - } - - mglncbaaiekflfcb_cat(valid_points_in_cam_data, valid_points_in_cam_sizes_0, - tmp_sizes_0, sim_model_lib0_B->tmp_data_d, - tmp_sizes_3); - for (khi = 0; khi < 16; khi++) { - loop_ub = tmp_sizes_3[0]; - for (indx_sizes = 0; indx_sizes <= (int32_T)(loop_ub - 1); indx_sizes++) - { - sim_model_lib0_B->new_observations_data[(int32_T)(indx_sizes + - (int32_T)((int32_T)(r_sizes << 1) * khi))] = - sim_model_lib0_B->tmp_data_d[(int32_T)((int32_T)((int32_T) - (tmp_sizes_3[0] * tmp_sizes_3[1]) * khi) + indx_sizes)]; - } - - loop_ub = tmp_sizes_3[0]; - for (indx_sizes = 0; indx_sizes <= (int32_T)(loop_ub - 1); indx_sizes++) - { - sim_model_lib0_B->new_observations_data[(int32_T)((int32_T)(indx_sizes - + r_sizes) + (int32_T)((int32_T)(r_sizes << 1) * khi))] = - sim_model_lib0_B->tmp_data_d[(int32_T)((int32_T)((int32_T)((int32_T) - (tmp_sizes_3[0] * tmp_sizes_3[1]) * khi) + indx_sizes) + - tmp_sizes_3[0])]; - } - } - - loop_ub = (int32_T)num_points_to_use; - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - tmp_data[khi] = 1U; - } - - tmp_sizes[0] = (int32_T)num_points_to_use; - tmp_sizes[1] = 15; - loop_ub = (int32_T)((int32_T)num_points_to_use * 15); - for (khi = 0; khi <= (int32_T)(loop_ub - 1); khi++) { - tmp_data_0[khi] = 0U; - } - - fkfkbiecjecjbaie_cat((int32_T)num_points_to_use, tmp_sizes, valid_out_h, - random_order_sizes); - for (khi = 0; khi < 16; khi++) { - loop_ub = random_order_sizes[0]; - for (indx_sizes = 0; indx_sizes <= (int32_T)(loop_ub - 1); indx_sizes++) - { - new_valid_flags_data[(int32_T)(indx_sizes + (int32_T) - (new_valid_flags_sizes_idx_0 * khi))] = valid_out_h[(int32_T) - ((int32_T)(random_order_sizes[0] * khi) + indx_sizes)]; - } - } - } - - nb = 0; - for (khi = 0; khi < 50; khi++) { - if (replace_these_points[khi]) { - sim_model_lib0_DW->int_id_hist[khi] = new_ids_data[nb]; - nb++; - } - } - - nb = 0; - for (khi = 0; khi < 50; khi++) { - if (replace_these_points[khi]) { - ia_data[nb] = (int32_T)(khi + 1); - nb++; - } - } - - for (khi = 0; khi < 16; khi++) { - loop_ub = (int32_T)num_points_needed; - for (indx_sizes = 0; indx_sizes <= (int32_T)(loop_ub - 1); indx_sizes++) { - sim_model_lib0_DW->int_observations[(int32_T)((int32_T) - (ia_data[indx_sizes] + (int32_T)(100 * khi)) - 1)] = - sim_model_lib0_B->new_observations_data[(int32_T)((int32_T)((int32_T) - (r_sizes << 1) * khi) + indx_sizes)]; - } - - loop_ub = (int32_T)num_points_needed; - for (indx_sizes = 0; indx_sizes <= (int32_T)(loop_ub - 1); indx_sizes++) { - sim_model_lib0_DW->int_observations[(int32_T)((int32_T) - (ia_data[indx_sizes] + (int32_T)(100 * khi)) + 49)] = - sim_model_lib0_B->new_observations_data[(int32_T)((int32_T)((int32_T) - ((int32_T)(r_sizes << 1) * khi) + indx_sizes) + r_sizes)]; - } - } - - nb = 0; - for (khi = 0; khi < 50; khi++) { - if (replace_these_points[khi]) { - ia_data[nb] = (int32_T)(khi + 1); - nb++; - } - } - - for (khi = 0; khi < 16; khi++) { - loop_ub = (int32_T)num_points_needed; - for (indx_sizes = 0; indx_sizes <= (int32_T)(loop_ub - 1); indx_sizes++) { - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)(ia_data[indx_sizes] - + (int32_T)(50 * khi)) - 1)] = new_valid_flags_data[(int32_T)((int32_T) - (new_valid_flags_sizes_idx_0 * khi) + indx_sizes)]; - } - } - } - - // sort the outputs by ID number - hdjmknohknopimgl_sort(sim_model_lib0_DW->int_id_hist, ia_data); - for (khi = 0; khi < 16; khi++) { - for (indx_sizes = 0; indx_sizes < 2; indx_sizes++) { - for (shuffled_ids_sizes = 0; shuffled_ids_sizes < 50; shuffled_ids_sizes++) - { - sim_model_lib0_B->tmp_data_d[(int32_T)((int32_T)(shuffled_ids_sizes + - (int32_T)(50 * indx_sizes)) + (int32_T)(100 * khi))] = - sim_model_lib0_DW->int_observations[(int32_T)((int32_T)((int32_T) - ((int32_T)(50 * indx_sizes) + ia_data[shuffled_ids_sizes]) + (int32_T) - (100 * khi)) - 1)]; - } - } - } - - for (khi = 0; khi < 16; khi++) { - for (indx_sizes = 0; indx_sizes < 2; indx_sizes++) { - memcpy(&sim_model_lib0_DW->int_observations[(int32_T)((int32_T)(khi * 100) - + (int32_T)(indx_sizes * 50))], &sim_model_lib0_B->tmp_data_d - [(int32_T)((int32_T)(khi * 100) + (int32_T)(indx_sizes * 50))], - (uint32_T)(50U * sizeof(real32_T))); - } - - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - valid_out_h[(int32_T)(indx_sizes + (int32_T)(50 * khi))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)((int32_T)(50 * khi) - + ia_data[indx_sizes]) - 1)]; - } - } - - for (khi = 0; khi < 16; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - sim_model_lib0_DW->int_valid_flag[(int32_T)(indx_sizes + (int32_T)(50 * - khi))] = valid_out_h[(int32_T)((int32_T)(50 * khi) + indx_sizes)]; - } - } - - memset(&valid_out_h[0], 0, (uint32_T)(800U * sizeof(uint8_T))); - - // Determine which points to report - if (i != 0) { - // if we just replaced the last augmentation - for (khi = 0; khi < 4; khi++) { - for (indx_sizes = 0; indx_sizes < 50; indx_sizes++) { - valid_out_h[(int32_T)(indx_sizes + (int32_T)(50 * (int32_T)l[khi]))] = - sim_model_lib0_DW->int_valid_flag[(int32_T)((int32_T)(50 * (int32_T) - l[khi]) + indx_sizes)]; - } - } - } - - // DataTypeConversion: '/Data Type Conversion4' incorporates: - // Constant: '/Constant3' - // Logic: '/Logical Operator' - // Selector: '/select_current_command' - // UnitDelay: '/Unit Delay' - - // else send all zeros - // populate outputs - // ':1:30' - // ':1:31' - // landmarks NOT populated/used by optical flow - // ':1:32' - // cam points not populated by OF - // end of function - for (khi = 0; khi < 800; khi++) { - valid_out_h[khi] = (uint8_T)(((int32_T)valid_out_h[khi] != 0) && - sim_model_lib0_P->cvs_optflow_valid_mask[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_it - 1)]); - } - - // End of DataTypeConversion: '/Data Type Conversion4' - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Compare' - // Selector: '/Select_col_1' - // Selector: '/Select_col_2' - - khi = (int32_T)(uint32_T)((uint32_T)((uint32_T) - ((sim_model_lib0_P->cvs_optflow_valid_times[0] != - sim_model_lib0_P->Constant_Value_mi) && - ((sim_model_lib0_P->cvs_optflow_valid_times[0] < - sim_model_lib0_B->RateTransition4_f.timestamp_sec) || - ((sim_model_lib0_P->cvs_optflow_valid_times[0] == - sim_model_lib0_B->RateTransition4_f.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_f.timestamp_nsec >= - sim_model_lib0_P->cvs_optflow_valid_times[3])))) + (uint32_T) - ((sim_model_lib0_P->cvs_optflow_valid_times[1] != - sim_model_lib0_P->Constant_Value_mi) && - ((sim_model_lib0_P->cvs_optflow_valid_times[1] < - sim_model_lib0_B->RateTransition4_f.timestamp_sec) || - ((sim_model_lib0_P->cvs_optflow_valid_times[1] == - sim_model_lib0_B->RateTransition4_f.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_f.timestamp_nsec >= - sim_model_lib0_P->cvs_optflow_valid_times[4]))))) + (uint32_T) - ((sim_model_lib0_P->cvs_optflow_valid_times[2] != - sim_model_lib0_P->Constant_Value_mi) && - ((sim_model_lib0_P->cvs_optflow_valid_times[2] < - sim_model_lib0_B->RateTransition4_f.timestamp_sec) || - ((sim_model_lib0_P->cvs_optflow_valid_times[2] == - sim_model_lib0_B->RateTransition4_f.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_f.timestamp_nsec >= - sim_model_lib0_P->cvs_optflow_valid_times[5]))))); - - // Update for RateTransition: '/Rate Transition10' incorporates: - // MATLAB Function: '/generate_output' - - sim_model_lib0_DW->RateTransition10_Buffer0 = - sim_model_lib0_DW->int_registration_number; - - // Update for RateTransition: '/Rate Transition3' - sim_model_lib0_DW->RateTransition3_Buffer0_g = - sim_model_lib0_B->RateTransition4_f.timestamp_sec; - - // Update for RateTransition: '/Rate Transition1' - sim_model_lib0_DW->RateTransition1_Buffer0_b = - sim_model_lib0_B->RateTransition4_f.timestamp_nsec; - - // Update for RateTransition: '/Rate Transition6' incorporates: - // MATLAB Function: '/generate_output' - - memcpy(&sim_model_lib0_DW->RateTransition6_Buffer0[0], - &sim_model_lib0_DW->int_observations[0], (uint32_T)(1600U * sizeof - (real32_T))); - - // Update for RateTransition: '/Rate Transition7' - memcpy(&sim_model_lib0_DW->RateTransition7_Buffer0_a[0], &valid_out_h[0], - (uint32_T)(800U * sizeof(uint8_T))); - - // Update for RateTransition: '/Rate Transition8' incorporates: - // MATLAB Function: '/generate_output' - - memcpy(&sim_model_lib0_DW->RateTransition8_Buffer0[0], - &sim_model_lib0_DW->int_id_hist[0], (uint32_T)(50U * sizeof(real32_T))); - - // Update for RandomNumber: '/pixel_noise' - rt_nrand_Upu32_Yd_f_pw_snf(&sim_model_lib0_DW->RandSeed_o); - - // Saturate: '/Saturation' incorporates: - // Sum: '/Sum1' - - if ((uint8_T)khi > sim_model_lib0_P->Saturation_UpperSat_p) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_it = - sim_model_lib0_P->Saturation_UpperSat_p; - } else if ((uint8_T)khi < sim_model_lib0_P->Saturation_LowerSat_h) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_it = - sim_model_lib0_P->Saturation_LowerSat_h; - } else { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_it = (uint8_T)khi; - } - - // End of Saturate: '/Saturation' -} - -// Model step function for TID4 -void sim_model_lib0_step4(RT_MODEL_sim_model_lib0_T *const sim_model_lib0_M) // Sample time: [0.336s, 0.0s] -{ - P_sim_model_lib0_T *sim_model_lib0_P = ((P_sim_model_lib0_T *) - sim_model_lib0_M->defaultParam); - B_sim_model_lib0_T *sim_model_lib0_B = ((B_sim_model_lib0_T *) - sim_model_lib0_M->blockIO); - DW_sim_model_lib0_T *sim_model_lib0_DW = ((DW_sim_model_lib0_T *) - sim_model_lib0_M->dwork); - real32_T rtb_Conversion_oh[9]; - real32_T rtb_Subtract1[3]; - real32_T rtb_MatrixConcatenate[16]; - real32_T rtb_Product[4]; - real32_T rtb_Sum; - real32_T rtb_Assignment[9]; - real32_T rtb_Sum_o; - int8_T rtb_DataTypeConversion4[50]; - real32_T valid_out[50]; - int32_T i; - int32_T i_0; - boolean_T tmp; - real32_T tmp_0[9]; - real32_T rtb_Assignment_p[9]; - real32_T tmp_1[9]; - real32_T rtb_Product_m[9]; - real32_T tmp_2; - real32_T tmp_3; - real32_T rtb_Product_p; - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Conversion_oh[i] = (real32_T)sim_model_lib0_P->Constant2_Value_j[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant3' - - rtb_Conversion_oh[0] = sim_model_lib0_P->tun_abp_q_body2navcam[3]; - rtb_Conversion_oh[4] = sim_model_lib0_P->tun_abp_q_body2navcam[3]; - rtb_Conversion_oh[8] = sim_model_lib0_P->tun_abp_q_body2navcam[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Product_m[0] = (real32_T)sim_model_lib0_P->Constant3_Value_hp; - rtb_Product_m[1] = sim_model_lib0_P->tun_abp_q_body2navcam[2]; - rtb_Product_m[2] = sim_model_lib0_P->Gain_Gain_kc * - sim_model_lib0_P->tun_abp_q_body2navcam[1]; - rtb_Product_m[3] = sim_model_lib0_P->Gain1_Gain_pi * - sim_model_lib0_P->tun_abp_q_body2navcam[2]; - rtb_Product_m[4] = (real32_T)sim_model_lib0_P->Constant3_Value_hp; - rtb_Product_m[5] = sim_model_lib0_P->tun_abp_q_body2navcam[0]; - rtb_Product_m[6] = sim_model_lib0_P->tun_abp_q_body2navcam[1]; - rtb_Product_m[7] = sim_model_lib0_P->Gain2_Gain_pj * - sim_model_lib0_P->tun_abp_q_body2navcam[0]; - rtb_Product_m[8] = (real32_T)sim_model_lib0_P->Constant3_Value_hp; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Constant: '/Constant3' - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_MatrixConcatenate[(int32_T)(i_0 << 2)] = rtb_Conversion_oh[(int32_T)(3 * - i_0)] + rtb_Product_m[(int32_T)(3 * i_0)]; - rtb_MatrixConcatenate[(int32_T)(1 + (int32_T)(i_0 << 2))] = - rtb_Conversion_oh[(int32_T)((int32_T)(3 * i_0) + 1)] + rtb_Product_m - [(int32_T)((int32_T)(3 * i_0) + 1)]; - rtb_MatrixConcatenate[(int32_T)(2 + (int32_T)(i_0 << 2))] = - rtb_Conversion_oh[(int32_T)((int32_T)(3 * i_0) + 2)] + rtb_Product_m - [(int32_T)((int32_T)(3 * i_0) + 2)]; - } - - rtb_MatrixConcatenate[3] = sim_model_lib0_P->Gain1_Gain_hr * - sim_model_lib0_P->tun_abp_q_body2navcam[0]; - rtb_MatrixConcatenate[7] = sim_model_lib0_P->Gain1_Gain_hr * - sim_model_lib0_P->tun_abp_q_body2navcam[1]; - rtb_MatrixConcatenate[11] = sim_model_lib0_P->Gain1_Gain_hr * - sim_model_lib0_P->tun_abp_q_body2navcam[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - - tmp = ((int32_T)sim_model_lib0_P->tun_cvs_noise_on != 0); - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[12] = sim_model_lib0_P->tun_abp_q_body2navcam[0]; - - // Product: '/Product' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Switch: '/Switch' - - if (tmp) { - rtb_Sum = sim_model_lib0_P->cvs_navcam_Q_B2navcan_error[0]; - rtb_Sum_o = sim_model_lib0_P->cvs_navcam_Q_B2navcan_error[1]; - } else { - rtb_Sum = sim_model_lib0_P->Constant7_Value_b[0]; - rtb_Sum_o = sim_model_lib0_P->Constant7_Value_b[1]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[13] = sim_model_lib0_P->tun_abp_q_body2navcam[1]; - rtb_MatrixConcatenate[14] = sim_model_lib0_P->tun_abp_q_body2navcam[2]; - - // Product: '/Product' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Switch: '/Switch' - - if (tmp) { - tmp_2 = sim_model_lib0_P->cvs_navcam_Q_B2navcan_error[2]; - tmp_3 = sim_model_lib0_P->cvs_navcam_Q_B2navcan_error[3]; - } else { - tmp_2 = sim_model_lib0_P->Constant7_Value_b[2]; - tmp_3 = sim_model_lib0_P->Constant7_Value_b[3]; - } - - // Reshape: '/Reshape1' incorporates: - // Constant: '/Constant3' - - rtb_MatrixConcatenate[15] = sim_model_lib0_P->tun_abp_q_body2navcam[3]; - - // Product: '/Product' - for (i_0 = 0; i_0 < 4; i_0++) { - rtb_Product_p = rtb_MatrixConcatenate[(int32_T)(i_0 + 12)] * tmp_3 + - (rtb_MatrixConcatenate[(int32_T)(i_0 + 8)] * tmp_2 + - (rtb_MatrixConcatenate[(int32_T)(i_0 + 4)] * rtb_Sum_o + - rtb_MatrixConcatenate[i_0] * rtb_Sum)); - rtb_Product[i_0] = rtb_Product_p; - } - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum = rtb_Product[3] * rtb_Product[3] * sim_model_lib0_P->Gain_Gain_cy - - (real32_T)sim_model_lib0_P->Constant1_Value_f; - for (i = 0; i < 9; i++) { - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Conversion_oh[i] = (real32_T)sim_model_lib0_P->Constant2_Value_id[i]; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment[i] = (real32_T)sim_model_lib0_P->Constant2_Value_k[i]; - } - - // Assignment: '/Assignment' - rtb_Conversion_oh[0] = rtb_Sum; - rtb_Conversion_oh[4] = rtb_Sum; - rtb_Conversion_oh[8] = rtb_Sum; - - // Gain: '/Gain1' - rtb_Sum = sim_model_lib0_P->Gain1_Gain_l0 * rtb_Product[3]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_o = sim_model_lib0_B->RateTransition2_j.Q_ISS2B[3] * - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[3] * - sim_model_lib0_P->Gain_Gain_bvs - (real32_T) - sim_model_lib0_P->Constant1_Value_l; - - // Assignment: '/Assignment' - rtb_Assignment[0] = rtb_Sum_o; - rtb_Assignment[4] = rtb_Sum_o; - rtb_Assignment[8] = rtb_Sum_o; - - // Gain: '/Gain1' - rtb_Sum_o = sim_model_lib0_P->Gain1_Gain_hd * - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[3]; - - // Sum: '/Add' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant4' - // Gain: '/Gain' - - for (i = 0; i < 25896; i++) { - sim_model_lib0_B->Add_o[i] = (real32_T)sim_model_lib0_P->tun_cvs_noise_on * - sim_model_lib0_P->cvs_landmark_map_error[i] + - sim_model_lib0_P->cvs_landmark_map_iss[i]; - } - - // End of Sum: '/Add' - for (i_0 = 0; i_0 < 8632; i_0++) { - // Sum: '/Subtract' incorporates: - // Selector: '/Select_X' - - sim_model_lib0_B->P_point_B_iss_p4[i_0] = sim_model_lib0_B->Add_o[i_0] - - sim_model_lib0_B->RateTransition2_j.P_B_ISS_ISS[0]; - - // Sum: '/Subtract2' incorporates: - // Selector: '/Select_Y' - - sim_model_lib0_B->P_point_B_iss_p4[(int32_T)(8632 + i_0)] = - sim_model_lib0_B->Add_o[(int32_T)(8632 + i_0)] - - sim_model_lib0_B->RateTransition2_j.P_B_ISS_ISS[1]; - - // Sum: '/Subtract3' incorporates: - // Selector: '/Select_Z' - - sim_model_lib0_B->P_point_B_iss_p4[(int32_T)(17264 + i_0)] = - sim_model_lib0_B->Add_o[(int32_T)(17264 + i_0)] - - sim_model_lib0_B->RateTransition2_j.P_B_ISS_ISS[2]; - } - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_0[0] = (real32_T)sim_model_lib0_P->Constant3_Value_p2; - tmp_0[1] = sim_model_lib0_B->RateTransition2_j.Q_ISS2B[2]; - tmp_0[2] = sim_model_lib0_P->Gain_Gain_jw * - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[1]; - tmp_0[3] = sim_model_lib0_P->Gain1_Gain_om * - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[2]; - tmp_0[4] = (real32_T)sim_model_lib0_P->Constant3_Value_p2; - tmp_0[5] = sim_model_lib0_B->RateTransition2_j.Q_ISS2B[0]; - tmp_0[6] = sim_model_lib0_B->RateTransition2_j.Q_ISS2B[1]; - tmp_0[7] = sim_model_lib0_P->Gain2_Gain_pf * - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[0]; - tmp_0[8] = (real32_T)sim_model_lib0_P->Constant3_Value_p2; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Product_m[i_0] = sim_model_lib0_B->RateTransition2_j.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[0]; - rtb_Product_m[(int32_T)(i_0 + 3)] = - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[1]; - rtb_Product_m[(int32_T)(i_0 + 6)] = - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[i_0] * - sim_model_lib0_B->RateTransition2_j.Q_ISS2B[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_p[(int32_T)(3 * i_0)] = (rtb_Assignment[(int32_T)(3 * i_0)] - - tmp_0[(int32_T)(3 * i_0)] * rtb_Sum_o) + rtb_Product_m[(int32_T)(3 * i_0)] - * sim_model_lib0_P->Gain2_Gain_n; - rtb_Assignment_p[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_0[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Sum_o) + rtb_Product_m[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_n; - rtb_Assignment_p[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Assignment - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_0[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Sum_o) + rtb_Product_m[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_n; - } - - // End of Sum: '/Sum1' - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_1[0] = (real32_T)sim_model_lib0_P->Constant3_Value_o; - tmp_1[1] = rtb_Product[2]; - tmp_1[2] = sim_model_lib0_P->Gain_Gain_bv * rtb_Product[1]; - tmp_1[3] = sim_model_lib0_P->Gain1_Gain_m5 * rtb_Product[2]; - tmp_1[4] = (real32_T)sim_model_lib0_P->Constant3_Value_o; - tmp_1[5] = rtb_Product[0]; - tmp_1[6] = rtb_Product[1]; - tmp_1[7] = sim_model_lib0_P->Gain2_Gain_l1 * rtb_Product[0]; - tmp_1[8] = (real32_T)sim_model_lib0_P->Constant3_Value_o; - for (i = 0; i < 3; i++) { - // Product: '/Product' incorporates: - // Math: '/Math Function' - - for (i_0 = 0; i_0 < 8632; i_0++) { - sim_model_lib0_B->Add_o[(int32_T)(i + (int32_T)(3 * i_0))] = 0.0F; - sim_model_lib0_B->Add_o[(int32_T)(i + (int32_T)(3 * i_0))] += - rtb_Assignment_p[i] * sim_model_lib0_B->P_point_B_iss_p4[i_0]; - sim_model_lib0_B->Add_o[(int32_T)(i + (int32_T)(3 * i_0))] += - rtb_Assignment_p[(int32_T)(i + 3)] * sim_model_lib0_B->P_point_B_iss_p4 - [(int32_T)(i_0 + 8632)]; - sim_model_lib0_B->Add_o[(int32_T)(i + (int32_T)(3 * i_0))] += - rtb_Assignment_p[(int32_T)(i + 6)] * sim_model_lib0_B->P_point_B_iss_p4 - [(int32_T)(i_0 + 17264)]; - } - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Gain: '/Gain1' - - rtb_Subtract1[i] = sim_model_lib0_P->tun_abp_p_navcam_body_body_sim[i] - - (real32_T)sim_model_lib0_P->tun_cvs_noise_on * - sim_model_lib0_P->cvs_navcam_P_B_B_error[i]; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - rtb_Product_m[i] = rtb_Product[i] * rtb_Product[0]; - rtb_Product_m[(int32_T)(i + 3)] = rtb_Product[i] * rtb_Product[1]; - rtb_Product_m[(int32_T)(i + 6)] = rtb_Product[i] * rtb_Product[2]; - } - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product1' - // Product: '/Product' - - for (i_0 = 0; i_0 < 3; i_0++) { - rtb_Assignment_p[(int32_T)(3 * i_0)] = (rtb_Conversion_oh[(int32_T)(3 * i_0)] - - tmp_1[(int32_T)(3 * i_0)] * rtb_Sum) + rtb_Product_m[(int32_T)(3 * i_0)] - * sim_model_lib0_P->Gain2_Gain_hs; - rtb_Assignment_p[(int32_T)(1 + (int32_T)(3 * i_0))] = (rtb_Conversion_oh - [(int32_T)((int32_T)(3 * i_0) + 1)] - tmp_1[(int32_T)((int32_T)(3 * i_0) + - 1)] * rtb_Sum) + rtb_Product_m[(int32_T)((int32_T)(3 * i_0) + 1)] * - sim_model_lib0_P->Gain2_Gain_hs; - rtb_Assignment_p[(int32_T)(2 + (int32_T)(3 * i_0))] = (rtb_Conversion_oh - [(int32_T)((int32_T)(3 * i_0) + 2)] - tmp_1[(int32_T)((int32_T)(3 * i_0) + - 2)] * rtb_Sum) + rtb_Product_m[(int32_T)((int32_T)(3 * i_0) + 2)] * - sim_model_lib0_P->Gain2_Gain_hs; - } - - // End of Sum: '/Sum1' - for (i_0 = 0; i_0 < 8632; i_0++) { - // Concatenate: '/Matrix Concatenate1' incorporates: - // Product: '/Product1' - // Selector: '/Select_X1' - // Selector: '/Select_Y1' - // Selector: '/Select_Z1' - // Sum: '/Subtract4' - // Sum: '/Subtract5' - // Sum: '/Subtract6' - - sim_model_lib0_B->rtb_Add_o_m[(int32_T)(3 * i_0)] = sim_model_lib0_B->Add_o - [(int32_T)(3 * i_0)] - rtb_Subtract1[0]; - sim_model_lib0_B->rtb_Add_o_m[(int32_T)(1 + (int32_T)(3 * i_0))] = - sim_model_lib0_B->Add_o[(int32_T)((int32_T)(3 * i_0) + 1)] - - rtb_Subtract1[1]; - sim_model_lib0_B->rtb_Add_o_m[(int32_T)(2 + (int32_T)(3 * i_0))] = - sim_model_lib0_B->Add_o[(int32_T)((int32_T)(3 * i_0) + 2)] - - rtb_Subtract1[2]; - - // Math: '/Math Function1' incorporates: - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - sim_model_lib0_B->P_point_B_iss_p4[(int32_T)(i_0 + (int32_T)(8632 * i))] = - 0.0F; - sim_model_lib0_B->P_point_B_iss_p4[(int32_T)(i_0 + (int32_T)(8632 * i))] += - sim_model_lib0_B->rtb_Add_o_m[(int32_T)(3 * i_0)] * rtb_Assignment_p[i]; - sim_model_lib0_B->P_point_B_iss_p4[(int32_T)(i_0 + (int32_T)(8632 * i))] += - sim_model_lib0_B->rtb_Add_o_m[(int32_T)((int32_T)(3 * i_0) + 1)] * - rtb_Assignment_p[(int32_T)(i + 3)]; - sim_model_lib0_B->P_point_B_iss_p4[(int32_T)(i_0 + (int32_T)(8632 * i))] += - sim_model_lib0_B->rtb_Add_o_m[(int32_T)((int32_T)(3 * i_0) + 2)] * - rtb_Assignment_p[(int32_T)(i + 6)]; - } - - // End of Math: '/Math Function1' - } - - // Outputs for Iterator SubSystem: '/pinhole_projection_model' - si_pinhole_projection_model(8632, sim_model_lib0_B->P_point_B_iss_p4, - sim_model_lib0_B->ImpAsg_InsertedFor_P_points_2_f, - sim_model_lib0_B->ImpAsg_InsertedFor_points_in__p, - sim_model_lib0_DW->pinhole_projection_model_id, - (P_pinhole_projection_model_si_T *) - &sim_model_lib0_P->pinhole_projection_model_id, - sim_model_lib0_P->tun_cvs_noise_on, - sim_model_lib0_P->tun_cvs_navcam_focal_length_Y, - sim_model_lib0_P->tun_cvs_navcam_focal_length_X, - sim_model_lib0_P->landmark_image_processing_pixel, - sim_model_lib0_P->tun_cvs_navcam_num_pixels_X, - sim_model_lib0_P->tun_cvs_navcam_num_pixels_Y, - sim_model_lib0_P->cvs_navcam_min_dist, sim_model_lib0_P->cvs_navcam_max_dist, - sim_model_lib0_P->cvs_navcam_pointing); - - // End of Outputs for SubSystem: '/pinhole_projection_model' - - // MATLAB Function: '/generate_output' incorporates: - // Constant: '/Constant3' - - // MATLAB Function 'image_processing/generate_output': ':1' - // Inputs: - // Process type: [Parameter] - // 1=Landmarks, 2=AR Tags, 3=Optical Flow, 4=Handrail - // num_pts_out = Required number of points to be reported [Parameter] - // P_point_iss_iss = ALL [x,y,z] position of points in the ISS frame **Landmark only** - // P_point_cam_cam = ALL [x,y,z] position of points in the CAM frame **Handrail only** - // P_points_2D_cam = ALL [u,v] pixel location of points in the camera **Landmark and OF only** - // valid_flag = indicates which points are valid - // num_hist_kept = Required number of histories kept for Optical Flow [Parameter] **OF only** - // flow_ids = ID tag for each optical flow point [Parameter] **OF only** - // - // Outputs: - // points_out_iss = output [x,y,z] position of points in the ISS frame **This output unused by optical flow** - // points_out_2D_cam = output [u,v] pixel location of points in the camera **This output unused by handrail** - // points_out_cam = output [x,y,z] position of points in the CAM frame **This output unused by OF and landmarks** - // valid_out = indicates which values of the output are valid! **This output unused by landmarks** - // ids_out = optical flow IDs - // registration_id = visual odometery registration pulse ID **ONLY used by OF** - // default to landmarks - // ':1:39' - sim_mode_format_landmark_output(sim_model_lib0_P->cvs_landmark_map_iss, - sim_model_lib0_B->ImpAsg_InsertedFor_P_points_2_f, - sim_model_lib0_B->ImpAsg_InsertedFor_points_in__p, - sim_model_lib0_B->points_out_iss_j, sim_model_lib0_B->points_out_2D_cam_g, - valid_out, sim_model_lib0_B, sim_model_lib0_DW); - - // DataTypeConversion: '/Data Type Conversion4' incorporates: - // Constant: '/Constant3' - // Logic: '/Logical Operator' - // Selector: '/select_current_command' - // UnitDelay: '/Unit Delay' - - // ':1:39' - // ':1:40' - // ID tag only populated/used in optical flow - // ':1:41' - // cam points not populated by landmarks - // ':1:42' - // end of function - for (i = 0; i < 50; i++) { - rtb_DataTypeConversion4[i] = (int8_T)((valid_out[i] != 0.0F) && - sim_model_lib0_P->cvs_landmark_valid_mask[(int32_T)((int32_T) - sim_model_lib0_DW->UnitDelay_DSTATE_a - 1)]); - } - - // End of DataTypeConversion: '/Data Type Conversion4' - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Relational Operator1' - // RelationalOperator: '/Relational Operator2' - // RelationalOperator: '/Compare' - // Selector: '/Select_col_1' - // Selector: '/Select_col_2' - - i_0 = (int32_T)(uint32_T)((uint32_T)((uint32_T) - ((sim_model_lib0_P->cvs_landmark_valid_times[0] != - sim_model_lib0_P->Constant_Value_eq) && - ((sim_model_lib0_P->cvs_landmark_valid_times[0] < - sim_model_lib0_B->RateTransition4_e.timestamp_sec) || - ((sim_model_lib0_P->cvs_landmark_valid_times[0] == - sim_model_lib0_B->RateTransition4_e.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_e.timestamp_nsec >= - sim_model_lib0_P->cvs_landmark_valid_times[3])))) + (uint32_T) - ((sim_model_lib0_P->cvs_landmark_valid_times[1] != - sim_model_lib0_P->Constant_Value_eq) && - ((sim_model_lib0_P->cvs_landmark_valid_times[1] < - sim_model_lib0_B->RateTransition4_e.timestamp_sec) || - ((sim_model_lib0_P->cvs_landmark_valid_times[1] == - sim_model_lib0_B->RateTransition4_e.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_e.timestamp_nsec >= - sim_model_lib0_P->cvs_landmark_valid_times[4]))))) + (uint32_T) - ((sim_model_lib0_P->cvs_landmark_valid_times[2] != - sim_model_lib0_P->Constant_Value_eq) && - ((sim_model_lib0_P->cvs_landmark_valid_times[2] < - sim_model_lib0_B->RateTransition4_e.timestamp_sec) || - ((sim_model_lib0_P->cvs_landmark_valid_times[2] == - sim_model_lib0_B->RateTransition4_e.timestamp_sec) && - (sim_model_lib0_B->RateTransition4_e.timestamp_nsec >= - sim_model_lib0_P->cvs_landmark_valid_times[5]))))); - - // Update for RateTransition: '/Rate Transition3' - sim_model_lib0_DW->RateTransition3_Buffer0_o = - sim_model_lib0_B->RateTransition4_e.timestamp_sec; - - // Update for RateTransition: '/Rate Transition1' - sim_model_lib0_DW->RateTransition1_Buffer0_l = - sim_model_lib0_B->RateTransition4_e.timestamp_nsec; - - // Update for RateTransition: '/Rate Transition5' - memcpy(&sim_model_lib0_DW->RateTransition5_Buffer0_i[0], - &sim_model_lib0_B->points_out_iss_j[0], (uint32_T)(150U * sizeof - (real32_T))); - - // Update for RateTransition: '/Rate Transition6' - memcpy(&sim_model_lib0_DW->RateTransition6_Buffer0_h[0], - &sim_model_lib0_B->points_out_2D_cam_g[0], (uint32_T)(100U * sizeof - (real32_T))); - - // Update for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_DW->RateTransition7_Buffer0_an[i] = (uint8_T) - rtb_DataTypeConversion4[i]; - } - - // End of Update for RateTransition: '/Rate Transition7' - - // Update for RandomNumber: '/pixel_noise' - rt_nrand_Upu32_Yd_f_pw_snf(&sim_model_lib0_DW->RandSeed_mi); - - // Saturate: '/Saturation' incorporates: - // Sum: '/Sum1' - - if ((uint8_T)i_0 > sim_model_lib0_P->Saturation_UpperSat_k) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_a = - sim_model_lib0_P->Saturation_UpperSat_k; - } else if ((uint8_T)i_0 < sim_model_lib0_P->Saturation_LowerSat_a) { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_a = - sim_model_lib0_P->Saturation_LowerSat_a; - } else { - // Update for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_a = (uint8_T)i_0; - } - - // End of Saturate: '/Saturation' -} - -// Model initialize function -void sim_model_lib0_initialize(RT_MODEL_sim_model_lib0_T *const sim_model_lib0_M, - act_msg *sim_model_lib0_U_act_msg_l, cmc_msg *sim_model_lib0_U_cmc_msg_in, - cvs_optical_flow_msg *sim_model_lib0_Y_cvs_optical_flow_msg_n, - cvs_handrail_msg *sim_model_lib0_Y_cvs_handrail_msg_h, cmc_msg - *sim_model_lib0_Y_cmc_msg_c, imu_msg *sim_model_lib0_Y_imu_msg_o, env_msg - *sim_model_lib0_Y_env_msg_i, bpm_msg *sim_model_lib0_Y_bpm_msg_h, - cvs_registration_pulse *sim_model_lib0_Y_cvs_registration_pulse_d, - cvs_landmark_msg *sim_model_lib0_Y_cvs_landmark_msg_n, cvs_landmark_msg - *sim_model_lib0_Y_cvs_ar_tag_msg, ex_time_msg *sim_model_lib0_Y_ex_time_msg_m) -{ - P_sim_model_lib0_T *sim_model_lib0_P = ((P_sim_model_lib0_T *) - sim_model_lib0_M->defaultParam); - B_sim_model_lib0_T *sim_model_lib0_B = ((B_sim_model_lib0_T *) - sim_model_lib0_M->blockIO); - DW_sim_model_lib0_T *sim_model_lib0_DW = ((DW_sim_model_lib0_T *) - sim_model_lib0_M->dwork); - - { - uint32_T tseed; - uint32_T r; - real_T y1; - int32_T i; - real32_T Constant3_idx_2; - real32_T Constant12_idx_2; - real32_T Constant11_idx_2; - real32_T Constant3_idx_1; - real32_T Constant12_idx_1; - real32_T Constant11_idx_1; - real32_T Constant3_idx_0; - real32_T Constant12_idx_0; - real32_T Constant11_idx_0; - - // Start for RateTransition: '/Rate Transition3' - sim_model_lib0_B->RateTransition3 = sim_model_lib0_P->RateTransition3_X0; - - // Start for RateTransition: '/Rate Transition1' - sim_model_lib0_B->RateTransition1 = sim_model_lib0_P->RateTransition1_X0; - for (i = 0; i < 150; i++) { - // Start for RateTransition: '/Rate Transition5' - sim_model_lib0_B->RateTransition5[i] = - sim_model_lib0_P->RateTransition5_X0; - - // Start for RateTransition: '/Rate Transition9' - sim_model_lib0_B->RateTransition9[i] = - sim_model_lib0_P->RateTransition9_X0; - } - - // Start for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_B->RateTransition7[i] = - sim_model_lib0_P->RateTransition7_X0; - } - - // End of Start for RateTransition: '/Rate Transition7' - - // Start for Constant: '/Constant11' - Constant11_idx_0 = sim_model_lib0_P->tun_ini_P_B_ISS_ISS[0]; - - // Start for Constant: '/Constant12' - Constant12_idx_0 = sim_model_lib0_P->tun_ini_V_B_ISS_ISS[0]; - - // Start for Constant: '/Constant3' - Constant3_idx_0 = sim_model_lib0_P->tun_ini_omega_B_ISS_B[0]; - - // Start for Constant: '/Constant11' - Constant11_idx_1 = sim_model_lib0_P->tun_ini_P_B_ISS_ISS[1]; - - // Start for Constant: '/Constant12' - Constant12_idx_1 = sim_model_lib0_P->tun_ini_V_B_ISS_ISS[1]; - - // Start for Constant: '/Constant3' - Constant3_idx_1 = sim_model_lib0_P->tun_ini_omega_B_ISS_B[1]; - - // Start for Constant: '/Constant11' - Constant11_idx_2 = sim_model_lib0_P->tun_ini_P_B_ISS_ISS[2]; - - // Start for Constant: '/Constant12' - Constant12_idx_2 = sim_model_lib0_P->tun_ini_V_B_ISS_ISS[2]; - - // Start for Constant: '/Constant3' - Constant3_idx_2 = sim_model_lib0_P->tun_ini_omega_B_ISS_B[2]; - - // Start for RateTransition: '/Rate Transition10' - sim_model_lib0_B->RateTransition10 = sim_model_lib0_P->RateTransition10_X0; - - // Start for RateTransition: '/Rate Transition3' - sim_model_lib0_B->RateTransition3_j = sim_model_lib0_P->RateTransition3_X0_o; - - // Start for RateTransition: '/Rate Transition1' - sim_model_lib0_B->RateTransition1_j = sim_model_lib0_P->RateTransition1_X0_e; - - // Start for RateTransition: '/Rate Transition6' - for (i = 0; i < 1600; i++) { - sim_model_lib0_B->RateTransition6[i] = - sim_model_lib0_P->RateTransition6_X0; - } - - // End of Start for RateTransition: '/Rate Transition6' - - // Start for RateTransition: '/Rate Transition7' - for (i = 0; i < 800; i++) { - sim_model_lib0_B->RateTransition7_f[i] = - sim_model_lib0_P->RateTransition7_X0_m; - } - - // End of Start for RateTransition: '/Rate Transition7' - - // Start for RateTransition: '/Rate Transition8' - for (i = 0; i < 50; i++) { - sim_model_lib0_B->RateTransition8[i] = - sim_model_lib0_P->RateTransition8_X0; - } - - // End of Start for RateTransition: '/Rate Transition8' - - // Start for RateTransition: '/Rate Transition3' - sim_model_lib0_B->RateTransition3_o = sim_model_lib0_P->RateTransition3_X0_m; - - // Start for RateTransition: '/Rate Transition1' - sim_model_lib0_B->RateTransition1_a = - sim_model_lib0_P->RateTransition1_X0_eo; - - // Start for RateTransition: '/Rate Transition5' - for (i = 0; i < 150; i++) { - sim_model_lib0_B->RateTransition5_b[i] = - sim_model_lib0_P->RateTransition5_X0_c; - } - - // End of Start for RateTransition: '/Rate Transition5' - - // Start for RateTransition: '/Rate Transition6' - for (i = 0; i < 100; i++) { - sim_model_lib0_B->RateTransition6_n[i] = - sim_model_lib0_P->RateTransition6_X0_j; - } - - // End of Start for RateTransition: '/Rate Transition6' - - // Start for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_B->RateTransition7_g[i] = - sim_model_lib0_P->RateTransition7_X0_k; - } - - // End of Start for RateTransition: '/Rate Transition7' - - // Start for RateTransition: '/Rate Transition3' - sim_model_lib0_B->RateTransition3_e = sim_model_lib0_P->RateTransition3_X0_c; - - // Start for RateTransition: '/Rate Transition1' - sim_model_lib0_B->RateTransition1_b = sim_model_lib0_P->RateTransition1_X0_a; - - // Start for RateTransition: '/Rate Transition5' - for (i = 0; i < 150; i++) { - sim_model_lib0_B->RateTransition5_p[i] = - sim_model_lib0_P->RateTransition5_X0_o; - } - - // End of Start for RateTransition: '/Rate Transition5' - - // Start for RateTransition: '/Rate Transition6' - for (i = 0; i < 100; i++) { - sim_model_lib0_B->RateTransition6_p[i] = - sim_model_lib0_P->RateTransition6_X0_g; - } - - // End of Start for RateTransition: '/Rate Transition6' - - // Start for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_B->RateTransition7_k[i] = - sim_model_lib0_P->RateTransition7_X0_n; - } - - // End of Start for RateTransition: '/Rate Transition7' - - // Start for Iterator SubSystem: '/pinhole_projection_model' - pinhole_projection_mo_Start(36, sim_model_lib0_DW->pinhole_projection_model); - - // End of Start for SubSystem: '/pinhole_projection_model' - - // Start for Iterator SubSystem: '/pinhole_projection_model' - pinhole_projection_mo_Start(504, - sim_model_lib0_DW->pinhole_projection_model_i); - - // End of Start for SubSystem: '/pinhole_projection_model' - - // Start for Iterator SubSystem: '/pinhole_projection_model' - pinhole_projection_mo_Start(8632, - sim_model_lib0_DW->pinhole_projection_model_id); - - // End of Start for SubSystem: '/pinhole_projection_model' - - // Start for Iterator SubSystem: '/pinhole_projection_model' - pinhole_projection_mo_Start(3979, - sim_model_lib0_DW->pinhole_projection_model_o); - - // End of Start for SubSystem: '/pinhole_projection_model' - - // InitializeConditions for RateTransition: '/Rate Transition3' - sim_model_lib0_DW->RateTransition3_Buffer0 = - sim_model_lib0_P->RateTransition3_X0; - - // InitializeConditions for RateTransition: '/Rate Transition1' - sim_model_lib0_DW->RateTransition1_Buffer0 = - sim_model_lib0_P->RateTransition1_X0; - - // InitializeConditions for RateTransition: '/Rate Transition5' - for (i = 0; i < 150; i++) { - sim_model_lib0_DW->RateTransition5_Buffer0[i] = - sim_model_lib0_P->RateTransition5_X0; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition5' - - // InitializeConditions for RateTransition: '/Rate Transition9' - for (i = 0; i < 150; i++) { - sim_model_lib0_DW->RateTransition9_Buffer0[i] = - sim_model_lib0_P->RateTransition9_X0; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition9' - - // InitializeConditions for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_DW->RateTransition7_Buffer0[i] = - sim_model_lib0_P->RateTransition7_X0; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition7' - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_i = - sim_model_lib0_P->UnitDelay_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_if = - sim_model_lib0_P->UnitDelay_InitialCondition_c; - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_h = - sim_model_lib0_P->UnitDelay_InitialCondition_n; - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_ht = - sim_model_lib0_P->UnitDelay_InitialCondition_l; - - // InitializeConditions for UnitDelay: '/Unit Delay1' - sim_model_lib0_DW->UnitDelay1_DSTATE = sim_model_lib0_P->ini_time_seconds; - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE = sim_model_lib0_P->ini_time_nanoseconds; - - // InitializeConditions for Delay: '/Delay' - sim_model_lib0_DW->icLoad = 1U; - - // InitializeConditions for Delay: '/Delay1' - sim_model_lib0_DW->Delay1_DSTATE = sim_model_lib0_P->Delay1_InitialCondition; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[0] = Constant11_idx_0; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[0] = Constant12_idx_0; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[0] = Constant3_idx_0; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[0] = - sim_model_lib0_P->DiscreteTimeIntegrator_IC; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[0] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC_b; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[0]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise_StdDev + - sim_model_lib0_P->random_noise_Mean; - sim_model_lib0_DW->NextOutput[0] = y1; - sim_model_lib0_DW->RandSeed[0] = tseed; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTATE[0] = (real_T) - sim_model_lib0_P->epson_accel_bias_ic[0]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[0] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[1] = Constant11_idx_1; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[1] = Constant12_idx_1; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[1] = Constant3_idx_1; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[1] = - sim_model_lib0_P->DiscreteTimeIntegrator_IC; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[1] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC_b; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[1]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise_StdDev + - sim_model_lib0_P->random_noise_Mean; - sim_model_lib0_DW->NextOutput[1] = y1; - sim_model_lib0_DW->RandSeed[1] = tseed; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTATE[1] = (real_T) - sim_model_lib0_P->epson_accel_bias_ic[1]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[1] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_p[2] = Constant11_idx_2; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE[2] = Constant12_idx_2; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_b[2] = Constant3_idx_2; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_d[2] = - sim_model_lib0_P->DiscreteTimeIntegrator_IC; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_b[2] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC_b; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[2]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise_StdDev + - sim_model_lib0_P->random_noise_Mean; - sim_model_lib0_DW->NextOutput[2] = y1; - sim_model_lib0_DW->RandSeed[2] = tseed; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTATE[2] = (real_T) - sim_model_lib0_P->epson_accel_bias_ic[2]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTATE[2] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[0]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise_StdDev_c + - sim_model_lib0_P->random_noise_Mean_k; - sim_model_lib0_DW->NextOutput_e[0] = y1; - sim_model_lib0_DW->RandSeed_k[0] = tseed; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[0] = - sim_model_lib0_P->epson_gyro_bias_ic[0]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[0] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC_k; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[1]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise_StdDev_c + - sim_model_lib0_P->random_noise_Mean_k; - sim_model_lib0_DW->NextOutput_e[1] = y1; - sim_model_lib0_DW->RandSeed_k[1] = tseed; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[1] = - sim_model_lib0_P->epson_gyro_bias_ic[1]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[1] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC_k; - - // InitializeConditions for RandomNumber: '/random_noise' - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[2]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise_StdDev_c + - sim_model_lib0_P->random_noise_Mean_k; - sim_model_lib0_DW->NextOutput_e[2] = y1; - sim_model_lib0_DW->RandSeed_k[2] = tseed; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator2' - sim_model_lib0_DW->DiscreteTimeIntegrator2_DSTAT_j[2] = - sim_model_lib0_P->epson_gyro_bias_ic[2]; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator1' - sim_model_lib0_DW->DiscreteTimeIntegrator1_DSTAT_c[2] = - sim_model_lib0_P->DiscreteTimeIntegrator1_IC_k; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTAT_dp = - sim_model_lib0_P->bpm_impeller_init_speed; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - sim_model_lib0_DW->DiscreteTimeIntegrator_DSTATE_e = - sim_model_lib0_P->bpm_impeller_init_speed; - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_m = - sim_model_lib0_P->UnitDelay_InitialCondition_o; - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_c = - sim_model_lib0_P->UnitDelay_InitialCondition_gb; - - // InitializeConditions for DiscretePulseGenerator: '/AR_pulse_gen' - sim_model_lib0_DW->clockTickCounter = 0; - - // InitializeConditions for DiscretePulseGenerator: '/landmark_pulse_gen' - sim_model_lib0_DW->clockTickCounter_c = 0; - - // InitializeConditions for RateTransition: '/Rate Transition10' - sim_model_lib0_DW->RateTransition10_Buffer0 = - sim_model_lib0_P->RateTransition10_X0; - - // InitializeConditions for RateTransition: '/Rate Transition3' - sim_model_lib0_DW->RateTransition3_Buffer0_g = - sim_model_lib0_P->RateTransition3_X0_o; - - // InitializeConditions for RateTransition: '/Rate Transition1' - sim_model_lib0_DW->RateTransition1_Buffer0_b = - sim_model_lib0_P->RateTransition1_X0_e; - - // InitializeConditions for RateTransition: '/Rate Transition6' - for (i = 0; i < 1600; i++) { - sim_model_lib0_DW->RateTransition6_Buffer0[i] = - sim_model_lib0_P->RateTransition6_X0; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition6' - - // InitializeConditions for RateTransition: '/Rate Transition7' - for (i = 0; i < 800; i++) { - sim_model_lib0_DW->RateTransition7_Buffer0_a[i] = - sim_model_lib0_P->RateTransition7_X0_m; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition7' - - // InitializeConditions for RateTransition: '/Rate Transition8' - for (i = 0; i < 50; i++) { - sim_model_lib0_DW->RateTransition8_Buffer0[i] = - sim_model_lib0_P->RateTransition8_X0; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition8' - - // InitializeConditions for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_p = - sim_model_lib0_P->DetectChange_vinit_j; - - // InitializeConditions for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_l = - sim_model_lib0_P->DetectChange1_vinit; - - // InitializeConditions for RateTransition: '/Rate Transition3' - sim_model_lib0_DW->RateTransition3_Buffer0_o = - sim_model_lib0_P->RateTransition3_X0_m; - - // InitializeConditions for RateTransition: '/Rate Transition1' - sim_model_lib0_DW->RateTransition1_Buffer0_l = - sim_model_lib0_P->RateTransition1_X0_eo; - - // InitializeConditions for RateTransition: '/Rate Transition5' - for (i = 0; i < 150; i++) { - sim_model_lib0_DW->RateTransition5_Buffer0_i[i] = - sim_model_lib0_P->RateTransition5_X0_c; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition5' - - // InitializeConditions for RateTransition: '/Rate Transition6' - for (i = 0; i < 100; i++) { - sim_model_lib0_DW->RateTransition6_Buffer0_h[i] = - sim_model_lib0_P->RateTransition6_X0_j; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition6' - - // InitializeConditions for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_DW->RateTransition7_Buffer0_an[i] = - sim_model_lib0_P->RateTransition7_X0_k; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition7' - - // InitializeConditions for RateTransition: '/Rate Transition3' - sim_model_lib0_DW->RateTransition3_Buffer0_n = - sim_model_lib0_P->RateTransition3_X0_c; - - // InitializeConditions for RateTransition: '/Rate Transition1' - sim_model_lib0_DW->RateTransition1_Buffer0_i = - sim_model_lib0_P->RateTransition1_X0_a; - - // InitializeConditions for RateTransition: '/Rate Transition5' - for (i = 0; i < 150; i++) { - sim_model_lib0_DW->RateTransition5_Buffer0_f[i] = - sim_model_lib0_P->RateTransition5_X0_o; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition5' - - // InitializeConditions for RateTransition: '/Rate Transition6' - for (i = 0; i < 100; i++) { - sim_model_lib0_DW->RateTransition6_Buffer0_e[i] = - sim_model_lib0_P->RateTransition6_X0_g; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition6' - - // InitializeConditions for RateTransition: '/Rate Transition7' - for (i = 0; i < 50; i++) { - sim_model_lib0_DW->RateTransition7_Buffer0_j[i] = - sim_model_lib0_P->RateTransition7_X0_n; - } - - // End of InitializeConditions for RateTransition: '/Rate Transition7' - - // InitializeConditions for DiscretePulseGenerator: '/handrail_pulse_gen' - sim_model_lib0_DW->clockTickCounter_n = 0; - - // InitializeConditions for RandomNumber: '/Random Number' - y1 = floor(sim_model_lib0_P->env_ext_air_vel_seed[0]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * sqrt - (sim_model_lib0_P->env_ext_air_vel_variance[0]) + - sim_model_lib0_P->RandomNumber_Mean; - sim_model_lib0_DW->NextOutput_l[0] = y1; - sim_model_lib0_DW->RandSeed_i[0] = tseed; - y1 = floor(sim_model_lib0_P->env_ext_air_vel_seed[1]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * sqrt - (sim_model_lib0_P->env_ext_air_vel_variance[1]) + - sim_model_lib0_P->RandomNumber_Mean; - sim_model_lib0_DW->NextOutput_l[1] = y1; - sim_model_lib0_DW->RandSeed_i[1] = tseed; - y1 = floor(sim_model_lib0_P->env_ext_air_vel_seed[2]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * sqrt - (sim_model_lib0_P->env_ext_air_vel_variance[2]) + - sim_model_lib0_P->RandomNumber_Mean; - sim_model_lib0_DW->NextOutput_l[2] = y1; - sim_model_lib0_DW->RandSeed_i[2] = tseed; - - // End of InitializeConditions for RandomNumber: '/Random Number' - - // InitializeConditions for RandomNumber: '/Random Number1' - y1 = floor(sim_model_lib0_P->env_ext_air_vel_seed[0]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * sqrt - (sim_model_lib0_P->env_ext_air_omega_variance[0]) + - sim_model_lib0_P->RandomNumber1_Mean; - sim_model_lib0_DW->NextOutput_f[0] = y1; - sim_model_lib0_DW->RandSeed_m[0] = tseed; - y1 = floor(sim_model_lib0_P->env_ext_air_vel_seed[1]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * sqrt - (sim_model_lib0_P->env_ext_air_omega_variance[1]) + - sim_model_lib0_P->RandomNumber1_Mean; - sim_model_lib0_DW->NextOutput_f[1] = y1; - sim_model_lib0_DW->RandSeed_m[1] = tseed; - y1 = floor(sim_model_lib0_P->env_ext_air_vel_seed[2]); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * sqrt - (sim_model_lib0_P->env_ext_air_omega_variance[2]) + - sim_model_lib0_P->RandomNumber1_Mean; - sim_model_lib0_DW->NextOutput_f[2] = y1; - sim_model_lib0_DW->RandSeed_m[2] = tseed; - - // End of InitializeConditions for RandomNumber: '/Random Number1' - - // InitializeConditions for RandomNumber: '/pixel_noise' - y1 = floor(sim_model_lib0_P->ARtag_image_processing_handrail); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - sim_model_lib0_DW->RandSeed_mb = tseed; - rt_nrand_Upu32_Yd_f_pw_snf(&sim_model_lib0_DW->RandSeed_mb); - - // End of InitializeConditions for RandomNumber: '/pixel_noise' - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_hj = - sim_model_lib0_P->UnitDelay_InitialCondition_d; - - // InitializeConditions for RandomNumber: '/pixel_noise' - y1 = floor(sim_model_lib0_P->cvs_noise_seed * 8.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - sim_model_lib0_DW->RandSeed_d = tseed; - sim_model_lib0_DW->NextOutput_o = rt_nrand_Upu32_Yd_f_pw_snf - (&sim_model_lib0_DW->RandSeed_d) * sqrt - (sim_model_lib0_P->cvs_handrail_noise_var) + - sim_model_lib0_P->pixel_noise_Mean_a; - - // End of InitializeConditions for RandomNumber: '/pixel_noise' - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_g = - sim_model_lib0_P->UnitDelay_InitialCondition_m; - - // InitializeConditions for RandomNumber: '/pixel_noise' - y1 = floor(sim_model_lib0_P->landmark_image_processing_handr); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - sim_model_lib0_DW->RandSeed_mi = tseed; - rt_nrand_Upu32_Yd_f_pw_snf(&sim_model_lib0_DW->RandSeed_mi); - - // End of InitializeConditions for RandomNumber: '/pixel_noise' - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_a = - sim_model_lib0_P->UnitDelay_InitialCondition_g; - - // InitializeConditions for RandomNumber: '/pixel_noise' - y1 = floor(sim_model_lib0_P->optical_flow_image_processing_h); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - sim_model_lib0_DW->RandSeed_o = tseed; - rt_nrand_Upu32_Yd_f_pw_snf(&sim_model_lib0_DW->RandSeed_o); - - // End of InitializeConditions for RandomNumber: '/pixel_noise' - - // InitializeConditions for UnitDelay: '/Unit Delay' - sim_model_lib0_DW->UnitDelay_DSTATE_it = - sim_model_lib0_P->UnitDelay_InitialCondition_a; - - // InitializeConditions for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE[0] = - sim_model_lib0_P->DetectChange_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_m[0] = - sim_model_lib0_P->DetectChange_vinit_o; - - // InitializeConditions for RandomNumber: '/random_noise1' - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[0] + 1.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise1_StdDev + - sim_model_lib0_P->random_noise1_Mean; - sim_model_lib0_DW->NextOutput_i[0] = y1; - sim_model_lib0_DW->RandSeed_ip[0] = tseed; - - // InitializeConditions for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE[1] = - sim_model_lib0_P->DetectChange_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_m[1] = - sim_model_lib0_P->DetectChange_vinit_o; - - // InitializeConditions for RandomNumber: '/random_noise1' - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[1] + 1.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise1_StdDev + - sim_model_lib0_P->random_noise1_Mean; - sim_model_lib0_DW->NextOutput_i[1] = y1; - sim_model_lib0_DW->RandSeed_ip[1] = tseed; - - // InitializeConditions for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE[2] = - sim_model_lib0_P->DetectChange_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - sim_model_lib0_DW->DelayInput1_DSTATE_m[2] = - sim_model_lib0_P->DetectChange_vinit_o; - - // InitializeConditions for RandomNumber: '/random_noise1' - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[2] + 1.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise1_StdDev + - sim_model_lib0_P->random_noise1_Mean; - sim_model_lib0_DW->NextOutput_i[2] = y1; - sim_model_lib0_DW->RandSeed_ip[2] = tseed; - - // InitializeConditions for RandomNumber: '/random_noise2' - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[0] + 2.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise2_StdDev + - sim_model_lib0_P->random_noise2_Mean; - sim_model_lib0_DW->NextOutput_j[0] = y1; - sim_model_lib0_DW->RandSeed_e[0] = tseed; - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[1] + 2.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise2_StdDev + - sim_model_lib0_P->random_noise2_Mean; - sim_model_lib0_DW->NextOutput_j[1] = y1; - sim_model_lib0_DW->RandSeed_e[1] = tseed; - y1 = floor(sim_model_lib0_P->epson_accel_noise_seed[2] + 2.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise2_StdDev + - sim_model_lib0_P->random_noise2_Mean; - sim_model_lib0_DW->NextOutput_j[2] = y1; - sim_model_lib0_DW->RandSeed_e[2] = tseed; - - // End of InitializeConditions for RandomNumber: '/random_noise2' - - // InitializeConditions for RandomNumber: '/random_noise1' - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[0] + 1.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise1_StdDev_l + - sim_model_lib0_P->random_noise1_Mean_d; - sim_model_lib0_DW->NextOutput_h[0] = y1; - sim_model_lib0_DW->RandSeed_p[0] = tseed; - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[1] + 1.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise1_StdDev_l + - sim_model_lib0_P->random_noise1_Mean_d; - sim_model_lib0_DW->NextOutput_h[1] = y1; - sim_model_lib0_DW->RandSeed_p[1] = tseed; - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[2] + 1.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise1_StdDev_l + - sim_model_lib0_P->random_noise1_Mean_d; - sim_model_lib0_DW->NextOutput_h[2] = y1; - sim_model_lib0_DW->RandSeed_p[2] = tseed; - - // End of InitializeConditions for RandomNumber: '/random_noise1' - - // InitializeConditions for RandomNumber: '/random_noise2' - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[0] + 2.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise2_StdDev_k + - sim_model_lib0_P->random_noise2_Mean_f; - sim_model_lib0_DW->NextOutput_p[0] = y1; - sim_model_lib0_DW->RandSeed_n[0] = tseed; - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[1] + 2.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise2_StdDev_k + - sim_model_lib0_P->random_noise2_Mean_f; - sim_model_lib0_DW->NextOutput_p[1] = y1; - sim_model_lib0_DW->RandSeed_n[1] = tseed; - y1 = floor(sim_model_lib0_P->epson_gyro_noise_seed[2] + 2.0); - if (rtIsNaN(y1) || rtIsInf(y1)) { - y1 = 0.0; - } else { - y1 = fmod(y1, 4.294967296E+9); - } - - tseed = y1 < 0.0 ? (uint32_T)(int32_T)-(int32_T)(uint32_T)-y1 : (uint32_T)y1; - r = (uint32_T)(tseed >> 16U); - i = (int32_T)(uint32_T)(tseed & 32768U); - tseed = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(tseed - - (uint32_T)(r << 16U)) + (uint32_T)i) << 16U) + (uint32_T)i) + r); - if (tseed < 1U) { - tseed = 1144108930U; - } else { - if (tseed > 2147483646U) { - tseed = 2147483646U; - } - } - - y1 = rt_nrand_Upu32_Yd_f_pw_snf(&tseed) * - sim_model_lib0_P->random_noise2_StdDev_k + - sim_model_lib0_P->random_noise2_Mean_f; - sim_model_lib0_DW->NextOutput_p[2] = y1; - sim_model_lib0_DW->RandSeed_n[2] = tseed; - - // End of InitializeConditions for RandomNumber: '/random_noise2' - - // SystemInitialize for Atomic SubSystem: '/speed_controller' - sim_m_speed_controller_Init(&sim_model_lib0_DW->speed_controller, - (P_speed_controller_sim_model__T *)&sim_model_lib0_P->speed_controller); - - // End of SystemInitialize for SubSystem: '/speed_controller' - - // SystemInitialize for Atomic SubSystem: '/speed_controller' - sim_m_speed_controller_Init(&sim_model_lib0_DW->speed_controller_c, - (P_speed_controller_sim_model__T *)&sim_model_lib0_P->speed_controller_c); - - // End of SystemInitialize for SubSystem: '/speed_controller' - - // SystemInitialize for Atomic SubSystem: '/servo_model' - sim_model__servo_model_Init(&sim_model_lib0_DW->servo_model, - (P_servo_model_sim_model_lib0_T *)&sim_model_lib0_P->servo_model, - (real32_T)sim_model_lib0_P->bpm_servo_min_theta, - sim_model_lib0_P->bpm_servo_motor_gear_ratio); - - // End of SystemInitialize for SubSystem: '/servo_model' - - // SystemInitialize for Atomic SubSystem: '/servo_model' - sim_model__servo_model_Init(&sim_model_lib0_DW->servo_model_f, - (P_servo_model_sim_model_lib0_T *)&sim_model_lib0_P->servo_model_f, - (real32_T)sim_model_lib0_P->bpm_servo_min_theta, - sim_model_lib0_P->bpm_servo_motor_gear_ratio); - - // End of SystemInitialize for SubSystem: '/servo_model' - - // SystemInitialize for Enabled SubSystem: '/latch_nozzle_thrust_matricies' - latch_nozzle_thrust_ma_Init(&sim_model_lib0_B->latch_nozzle_thrust_matricies, - (P_latch_nozzle_thrust_matrici_T *) - &sim_model_lib0_P->latch_nozzle_thrust_matricies); - - // End of SystemInitialize for SubSystem: '/latch_nozzle_thrust_matricies' - - // SystemInitialize for Atomic SubSystem: '/blower_aerodynamics' - si_blower_aerodynamics_Init(&sim_model_lib0_DW->blower_aerodynamics, - (P_blower_aerodynamics_sim_mod_T *)&sim_model_lib0_P->blower_aerodynamics, - sim_model_lib0_P->bpm_PM1_randn_noise_seed); - - // End of SystemInitialize for SubSystem: '/blower_aerodynamics' - - // SystemInitialize for Enabled SubSystem: '/latch_nozzle_thrust_matricies' - latch_nozzle_thrust_ma_Init - (&sim_model_lib0_B->latch_nozzle_thrust_matricies_p, - (P_latch_nozzle_thrust_matrici_T *) - &sim_model_lib0_P->latch_nozzle_thrust_matricies_p); - - // End of SystemInitialize for SubSystem: '/latch_nozzle_thrust_matricies' - - // SystemInitialize for Atomic SubSystem: '/blower_aerodynamics' - blower_aerodynamics_d_Init(&sim_model_lib0_DW->blower_aerodynamics_j, - (P_blower_aerodynamics_sim_m_m_T *) - &sim_model_lib0_P->blower_aerodynamics_j, - sim_model_lib0_P->bpm_PM2_randn_noise_seed); - - // End of SystemInitialize for SubSystem: '/blower_aerodynamics' - - // SystemInitialize for Iterator SubSystem: '/pinhole_projection_model' - pinhole_projection_mod_Init(36, sim_model_lib0_DW->pinhole_projection_model, - (P_pinhole_projection_model_si_T *) - &sim_model_lib0_P->pinhole_projection_model, - sim_model_lib0_P->cvs_AR_pixel_noise, sim_model_lib0_P->cvs_noise_seed * - 4.0); - - // End of SystemInitialize for SubSystem: '/pinhole_projection_model' - - // SystemInitialize for Iterator SubSystem: '/pinhole_projection_model' - pinhole_projection_mod_Init(504, - sim_model_lib0_DW->pinhole_projection_model_i, - (P_pinhole_projection_model_si_T *) - &sim_model_lib0_P->pinhole_projection_model_i, - sim_model_lib0_P->handrail_image_processing_pixel, - sim_model_lib0_P->cvs_noise_seed * 7.0); - - // End of SystemInitialize for SubSystem: '/pinhole_projection_model' - - // SystemInitialize for MATLAB Function: '/generate_output' - memset(&sim_model_lib0_DW->state_m[0], 0, (uint32_T)(625U * sizeof(uint32_T))); - r = 5489U; - sim_model_lib0_DW->state_m[0] = 5489U; - for (i = 0; i < 623; i++) { - r = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(r >> 30U) ^ r) * - 1812433253U) + (uint32_T)i) + 1U); - sim_model_lib0_DW->state_m[(int32_T)(i + 1)] = r; - } - - sim_model_lib0_DW->state_m[624] = 624U; - - // End of SystemInitialize for MATLAB Function: '/generate_output' - - // SystemInitialize for Iterator SubSystem: '/pinhole_projection_model' - pinhole_projection_mod_Init(8632, - sim_model_lib0_DW->pinhole_projection_model_id, - (P_pinhole_projection_model_si_T *) - &sim_model_lib0_P->pinhole_projection_model_id, - sim_model_lib0_P->landmark_image_processing_pixel, - sim_model_lib0_P->cvs_noise_seed * 3.0); - - // End of SystemInitialize for SubSystem: '/pinhole_projection_model' - - // SystemInitialize for MATLAB Function: '/generate_output' - memset(&sim_model_lib0_DW->state_d[0], 0, (uint32_T)(625U * sizeof(uint32_T))); - r = 5489U; - sim_model_lib0_DW->state_d[0] = 5489U; - for (i = 0; i < 623; i++) { - r = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(r >> 30U) ^ r) * - 1812433253U) + (uint32_T)i) + 1U); - sim_model_lib0_DW->state_d[(int32_T)(i + 1)] = r; - } - - sim_model_lib0_DW->state_d[624] = 624U; - - // End of SystemInitialize for MATLAB Function: '/generate_output' - - // SystemInitialize for Iterator SubSystem: '/pinhole_projection_model' - pinhole_projection_mod_Init(3979, - sim_model_lib0_DW->pinhole_projection_model_o, - (P_pinhole_projection_model_si_T *) - &sim_model_lib0_P->pinhole_projection_model_o, - sim_model_lib0_P->optical_flow_image_processing_p, - sim_model_lib0_P->cvs_noise_seed * 5.0); - - // End of SystemInitialize for SubSystem: '/pinhole_projection_model' - - // SystemInitialize for MATLAB Function: '/generate_output' - sim_model_lib0_DW->int_id_hist_not_empty = false; - memset(&sim_model_lib0_DW->int_id_hist[0], 0, (uint32_T)(50U * sizeof - (real32_T))); - memset(&sim_model_lib0_DW->state[0], 0, (uint32_T)(625U * sizeof(uint32_T))); - r = 5489U; - sim_model_lib0_DW->state[0] = 5489U; - for (i = 0; i < 623; i++) { - r = (uint32_T)((uint32_T)((uint32_T)((uint32_T)((uint32_T)(r >> 30U) ^ r) * - 1812433253U) + (uint32_T)i) + 1U); - sim_model_lib0_DW->state[(int32_T)(i + 1)] = r; - } - - sim_model_lib0_DW->state[624] = 624U; - - // End of SystemInitialize for MATLAB Function: '/generate_output' - } -} - -// Model terminate function -void sim_model_lib0_terminate(RT_MODEL_sim_model_lib0_T * sim_model_lib0_M) -{ - // model code - rt_FREE(sim_model_lib0_M->blockIO); - if (sim_model_lib0_M->paramIsMalloced) { - rt_FREE(sim_model_lib0_M->defaultParam); - } - - rt_FREE(sim_model_lib0_M->dwork); - rt_FREE(sim_model_lib0_M); -} - -// Model data allocation function -RT_MODEL_sim_model_lib0_T *sim_model_lib0(act_msg *sim_model_lib0_U_act_msg_l, - cmc_msg *sim_model_lib0_U_cmc_msg_in, cvs_optical_flow_msg - *sim_model_lib0_Y_cvs_optical_flow_msg_n, cvs_handrail_msg - *sim_model_lib0_Y_cvs_handrail_msg_h, cmc_msg *sim_model_lib0_Y_cmc_msg_c, - imu_msg *sim_model_lib0_Y_imu_msg_o, env_msg *sim_model_lib0_Y_env_msg_i, - bpm_msg *sim_model_lib0_Y_bpm_msg_h, cvs_registration_pulse - *sim_model_lib0_Y_cvs_registration_pulse_d, cvs_landmark_msg - *sim_model_lib0_Y_cvs_landmark_msg_n, cvs_landmark_msg - *sim_model_lib0_Y_cvs_ar_tag_msg, ex_time_msg *sim_model_lib0_Y_ex_time_msg_m) -{ - RT_MODEL_sim_model_lib0_T *sim_model_lib0_M; - sim_model_lib0_M = (RT_MODEL_sim_model_lib0_T *) malloc(sizeof - (RT_MODEL_sim_model_lib0_T)); - if (sim_model_lib0_M == NULL) { - return NULL; - } - - (void) memset((char *)sim_model_lib0_M, 0, - sizeof(RT_MODEL_sim_model_lib0_T)); - - // block I/O - { - B_sim_model_lib0_T *b = (B_sim_model_lib0_T *) malloc(sizeof - (B_sim_model_lib0_T)); - rt_VALIDATE_MEMORY(sim_model_lib0_M,b); - sim_model_lib0_M->blockIO = (b); - } - - // parameters - { - P_sim_model_lib0_T *p; - static int_T pSeen = 0; - - // only malloc on multiple model instantiation - if (pSeen == 1 ) { - p = (P_sim_model_lib0_T *) malloc(sizeof(P_sim_model_lib0_T)); - rt_VALIDATE_MEMORY(sim_model_lib0_M,p); - (void) memcpy(p, &sim_model_lib0_P, - sizeof(P_sim_model_lib0_T)); - sim_model_lib0_M->paramIsMalloced = (true); - } else { - p = &sim_model_lib0_P; - sim_model_lib0_M->paramIsMalloced = (false); - pSeen = 1; - } - - sim_model_lib0_M->defaultParam = (p); - } - - // states (dwork) - { - DW_sim_model_lib0_T *dwork = (DW_sim_model_lib0_T *) malloc(sizeof - (DW_sim_model_lib0_T)); - rt_VALIDATE_MEMORY(sim_model_lib0_M,dwork); - sim_model_lib0_M->dwork = (dwork); - } - - { - P_sim_model_lib0_T *sim_model_lib0_P = ((P_sim_model_lib0_T *) - sim_model_lib0_M->defaultParam); - B_sim_model_lib0_T *sim_model_lib0_B = ((B_sim_model_lib0_T *) - sim_model_lib0_M->blockIO); - DW_sim_model_lib0_T *sim_model_lib0_DW = ((DW_sim_model_lib0_T *) - sim_model_lib0_M->dwork); - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // block I/O - (void) memset(((void *) sim_model_lib0_B), 0, - sizeof(B_sim_model_lib0_T)); - - // states (dwork) - (void) memset((void *)sim_model_lib0_DW, 0, - sizeof(DW_sim_model_lib0_T)); - - // external inputs - *sim_model_lib0_U_act_msg_l = sim_model_lib0_rtZact_msg; - *sim_model_lib0_U_cmc_msg_in = sim_model_lib0_rtZcmc_msg; - - // external outputs - (*sim_model_lib0_Y_cvs_optical_flow_msg_n) = - sim_model_lib0_rtZcvs_optical_flow_msg; - (*sim_model_lib0_Y_cvs_handrail_msg_h) = sim_model_lib0_rtZcvs_handrail_msg; - (*sim_model_lib0_Y_cmc_msg_c) = sim_model_lib0_rtZcmc_msg; - (*sim_model_lib0_Y_imu_msg_o) = sim_model_lib0_rtZimu_msg; - (*sim_model_lib0_Y_env_msg_i) = sim_model_lib0_rtZenv_msg; - (*sim_model_lib0_Y_bpm_msg_h) = sim_model_lib0_rtZbpm_msg; - (*sim_model_lib0_Y_cvs_registration_pulse_d) = - sim_model_lib0_rtZcvs_registration_pulse; - (*sim_model_lib0_Y_cvs_landmark_msg_n) = sim_model_lib0_rtZcvs_landmark_msg; - (*sim_model_lib0_Y_cvs_ar_tag_msg) = sim_model_lib0_rtZcvs_landmark_msg; - (*sim_model_lib0_Y_ex_time_msg_m) = sim_model_lib0_rtZex_time_msg; - } - - return sim_model_lib0_M; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0.h b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0.h deleted file mode 100644 index 6f4ecb0cf9..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0.h +++ /dev/null @@ -1,2793 +0,0 @@ -// -// File: sim_model_lib0.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:45:47 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_sim_model_lib0_h_ -#define RTW_HEADER_sim_model_lib0_h_ -#include -#include -#include -#ifndef sim_model_lib0_COMMON_INCLUDES_ -# define sim_model_lib0_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#endif // sim_model_lib0_COMMON_INCLUDES_ - -#include "sim_model_lib0_types.h" - -// Child system includes -#include "blower_aerodynamics.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "aimgnophaiekjecb_eml_rand_mt19937ar.h" -#include "baaiimglmglniekf_sortIdx.h" -#include "cbiephdblnopophl_cat.h" -#include "dbiehlnglnohaaai_cat.h" -#include "dbiekfcbglfkkfcb_sortLE.h" -#include "fcbadjmgohlnpphl_rows_differ.h" -#include "fkfkbiecjecjbaie_cat.h" -#include "hdbiaaaabiecmgdb_do_vectors.h" -#include "hdjmknohknopimgl_sort.h" -#include "jecjcjmgfkfkophl_sum.h" -#include "kfcjlnglmgdbgdjm_all.h" -#include "kngdbaaaiecjbiec_skip_to_last_equal_value.h" -#include "mgdjimgdopphcjek_skip_to_last_equal_value.h" -#include "mglflfcbcbimecje_cat.h" -#include "mglncbaaiekflfcb_cat.h" -#include "mohdcjmgnohlknoh_sortLE.h" -#include "ohdbiekfdjechlno_rows_differ.h" -#include "ophlngdbgdjmgdje_cat.h" -#include "rt_mldivide_U1f3x3_U2f_XeZWzB4d.h" -#include "rt_nrand_Upu32_Yd_f_pw_snf.h" -#include "rt_roundd_snf.h" -#include "rtGetInf.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -# define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -# define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Block states (auto storage) for system '/CoreSubsys' -typedef struct { - real_T NextOutput; // '/pixel_noise' - real_T NextOutput_n; // '/pixel_noise1' - uint32_T RandSeed; // '/pixel_noise' - uint32_T RandSeed_k; // '/pixel_noise1' -} DW_CoreSubsys_sim_model_lib0_T; - -// Block states (auto storage) for system '/pinhole_projection_model' -typedef struct { - DW_CoreSubsys_sim_model_lib0_T CoreSubsys;// '/CoreSubsys' -} DW_pinhole_projection_model_s_T; - -// Block signals for system '/latch_nozzle_thrust_matricies' -typedef struct { - real32_T OutportBufferForthrust2force_B[18];// '/MATLAB Function' - real32_T OutportBufferForthrust2torque_B[18];// '/MATLAB Function' -} B_latch_nozzle_thrust_matrici_T; - -// Block signals for system '/calc_nozzle_area' -typedef struct { - real32_T Gain12[6]; // '/Gain12' - real32_T Product2[6]; // '/Product2' -} B_calc_nozzle_area_sim_model__T; - -// Block signals for system '/dc_motor_model' -typedef struct { - real32_T current; // '/Gain5' - real32_T output_torque; // '/Add1' -} B_dc_motor_model_sim_model_li_T; - -// Block signals for system '/speed_controller' -typedef struct { - real32_T Switch2; // '/Switch2' -} B_speed_controller_sim_model__T; - -// Block states (auto storage) for system '/speed_controller' -typedef struct { - real32_T Integrator_DSTATE; // '/Integrator' - real32_T Filter_DSTATE; // '/Filter' - real32_T PrevY; // '/Rate Limiter' -} DW_speed_controller_sim_model_T; - -// Block signals for system '/servo_model' -typedef struct { - real32_T Backlash1[6]; // '/Backlash1' - real32_T Gain1[6]; // '/Gain1' - real32_T current[6]; // '/Gain5' -} B_servo_model_sim_model_lib0_T; - -// Block states (auto storage) for system '/servo_model' -typedef struct { - real32_T DiscreteTimeIntegrator4_DSTATE[6];// '/Discrete-Time Integrator4' - real32_T Integrator_DSTATE[6]; // '/Integrator' - real32_T Filter_DSTATE[6]; // '/Filter' - real32_T DiscreteTimeIntegrator3_DSTATE[6];// '/Discrete-Time Integrator3' - real32_T PrevY[6]; // '/Backlash1' -} DW_servo_model_sim_model_lib0_T; - -// Block signals (auto storage) -typedef struct { - real32_T b_points_in_iss_data[26046]; - real32_T P_point_B_iss_p4[25896]; // '/Matrix Concatenate' - real32_T Add_o[25896]; // '/Add' - real32_T rtb_Add_o_m[25896]; - real32_T b_points_in_iss_data_c[25896]; - real32_T b_points_in_iss_data_k[25896]; - real32_T b_data[17364]; - real32_T b_b[17264]; - real32_T b_data_c[17264]; - real_T idx[8632]; - real32_T P_point_B_iss_k[11937]; // '/Matrix Concatenate' - real32_T Add_f[11937]; // '/Add' - real32_T rtb_Add_f_b[11937]; - real32_T ycol[8632]; - int32_T unique_rows_data[8632]; - int32_T indx_data[8632]; - int32_T iwork[8632]; - int32_T idx_data[8632]; - int32_T idx_p[8632]; - int32_T iwork_data[8632]; - real_T random_order_data[3979]; - real_T idx_data_c[3979]; - real32_T valid_points_in_cam_data[7958]; - real32_T valid_points_in_cam_data_f[7958]; - real32_T b_c_data[4029]; - real32_T c_data[4029]; - int32_T shuffled_ids_data[3979]; - int32_T indx_data_g[3979]; - int32_T r_data[3979]; - int32_T ib_data[3979]; - int32_T idx_data_g[3979]; - int32_T idx_data_m[3979]; - int32_T iwork_data_n[3979]; - int32_T iwork_data_p[3979]; - int32_T iwork_data_l[3979]; - real32_T valid_ids_data[3979]; - real32_T ycol_data[3979]; - int8_T b_valid_in_data[8682]; - boolean_T b_valid_in_data_j[8632]; - int16_T tmp_data[3979]; - real32_T points_out_2D_cam_e[1600]; // '/generate_output' - real32_T new_observations_data[1600]; - real32_T tmp_data_d[1600]; - real32_T P_point_B_iss_p[1512]; // '/Matrix Concatenate' - real32_T Add_l[1512]; // '/Add' - real32_T rtb_Add_l_g[1512]; - real32_T points_in_iss_data[1512]; - real32_T tmp_data_l[1500]; - real32_T tmp_data_dh[1200]; - env_msg RateTransition2; // '/Rate Transition2' - env_msg RateTransition2_o; // '/Rate Transition2' - env_msg RateTransition2_j; // '/Rate Transition2' - env_msg RateTransition2_b; // '/Rate Transition2' - ex_time_msg RateTransition4; // '/Rate Transition4' - ex_time_msg RateTransition4_m; // '/Rate Transition4' - ex_time_msg RateTransition4_e; // '/Rate Transition4' - ex_time_msg RateTransition4_f; // '/Rate Transition4' - real32_T RateTransition5[150]; // '/Rate Transition5' - real32_T RateTransition9[150]; // '/Rate Transition9' - real32_T RateTransition6[1600]; // '/Rate Transition6' - real32_T RateTransition8[50]; // '/Rate Transition8' - real32_T RateTransition5_b[150]; // '/Rate Transition5' - real32_T RateTransition6_n[100]; // '/Rate Transition6' - real32_T RateTransition5_p[150]; // '/Rate Transition5' - real32_T RateTransition6_p[100]; // '/Rate Transition6' - real32_T id_tag_m[50]; // '/generate_output' - real32_T ImpAsg_InsertedFor_P_points_2D_[7958];// '/pinhole_camera_projection' - real32_T points_out_iss_j[150]; // '/generate_output' - real32_T points_out_2D_cam_g[100]; // '/generate_output' - real32_T ImpAsg_InsertedFor_P_points_2_f[17264];// '/pinhole_camera_projection' - real32_T points_out_iss_h[150]; // '/generate_output' - real32_T points_out_3D_cam_n[150]; // '/generate_output' - uint32_T RateTransition3; // '/Rate Transition3' - uint32_T RateTransition1; // '/Rate Transition1' - uint32_T RateTransition3_j; // '/Rate Transition3' - uint32_T RateTransition1_j; // '/Rate Transition1' - uint32_T RateTransition3_o; // '/Rate Transition3' - uint32_T RateTransition1_a; // '/Rate Transition1' - uint32_T RateTransition3_e; // '/Rate Transition3' - uint32_T RateTransition1_b; // '/Rate Transition1' - uint8_T RateTransition7[50]; // '/Rate Transition7' - uint8_T RateTransition10; // '/Rate Transition10' - uint8_T RateTransition7_f[800]; // '/Rate Transition7' - uint8_T RateTransition7_g[50]; // '/Rate Transition7' - uint8_T RateTransition7_k[50]; // '/Rate Transition7' - uint8_T registration_id_h; // '/generate_output' - boolean_T ImpAsg_InsertedFor_points_in_FO[3979];// '/determine_points_in_FOV' - boolean_T ImpAsg_InsertedFor_points_in__p[8632];// '/determine_points_in_FOV' - boolean_T ImpAsg_InsertedFor_points_in__n[504];// '/determine_points_in_FOV' - B_servo_model_sim_model_lib0_T servo_model_f;// '/servo_model' - B_speed_controller_sim_model__T speed_controller_c;// '/speed_controller' - B_dc_motor_model_sim_model_li_T dc_motor_model_g;// '/dc_motor_model' - B_calc_nozzle_area_sim_model__T calc_nozzle_area_c;// '/calc_nozzle_area' - B_latch_nozzle_thrust_matrici_T latch_nozzle_thrust_matricies_p;// '/latch_nozzle_thrust_matricies' - B_blower_aerodynamics_sim_m_f_T blower_aerodynamics_j;// '/blower_aerodynamics' - B_servo_model_sim_model_lib0_T servo_model;// '/servo_model' - B_speed_controller_sim_model__T speed_controller;// '/speed_controller' - B_dc_motor_model_sim_model_li_T dc_motor_model;// '/dc_motor_model' - B_calc_nozzle_area_sim_model__T calc_nozzle_area;// '/calc_nozzle_area' - B_latch_nozzle_thrust_matrici_T latch_nozzle_thrust_matricies;// '/latch_nozzle_thrust_matricies' - B_blower_aerodynamics_sim_mod_T blower_aerodynamics;// '/blower_aerodynamics' -} B_sim_model_lib0_T; - -// Block states (auto storage) for system '' -typedef struct { - bpm_msg Delay1_DSTATE; // '/Delay1' - real_T DiscreteTimeIntegrator2_DSTATE[3];// '/Discrete-Time Integrator2' - real_T DiscreteTimeIntegrator1_DSTATE[3];// '/Discrete-Time Integrator1' - real_T DiscreteTimeIntegrator2_DSTAT_j[3];// '/Discrete-Time Integrator2' - real_T DiscreteTimeIntegrator1_DSTAT_c[3];// '/Discrete-Time Integrator1' - real_T NextOutput[3]; // '/random_noise' - real_T NextOutput_e[3]; // '/random_noise' - real_T NextOutput_l[3]; // '/Random Number' - real_T NextOutput_f[3]; // '/Random Number1' - real_T NextOutput_o; // '/pixel_noise' - real_T NextOutput_i[3]; // '/random_noise1' - real_T NextOutput_j[3]; // '/random_noise2' - real_T NextOutput_h[3]; // '/random_noise1' - real_T NextOutput_p[3]; // '/random_noise2' - real_T int_initalization_complete; // '/generate_output' - real32_T DiscreteTimeIntegrator1_DSTAT_p[3];// '/Discrete-Time Integrator1' - real32_T DiscreteTimeIntegrator_DSTATE[3];// '/Discrete-Time Integrator' - real32_T DiscreteTimeIntegrator_DSTATE_b[3];// '/Discrete-Time Integrator' - real32_T Delay_DSTATE[4]; // '/Delay' - real32_T DiscreteTimeIntegrator_DSTATE_d[3];// '/Discrete-Time Integrator' - real32_T DiscreteTimeIntegrator1_DSTAT_b[3];// '/Discrete-Time Integrator1' - real32_T DiscreteTimeIntegrator_DSTAT_dp;// '/Discrete-Time Integrator' - real32_T DiscreteTimeIntegrator_DSTATE_e;// '/Discrete-Time Integrator' - real32_T DelayInput1_DSTATE[3]; // '/Delay Input1' - real32_T DelayInput1_DSTATE_m[3]; // '/Delay Input1' - uint32_T UnitDelay1_DSTATE; // '/Unit Delay1' - uint32_T UnitDelay_DSTATE; // '/Unit Delay' - uint32_T DelayInput1_DSTATE_p; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_l; // '/Delay Input1' - real32_T RateTransition5_Buffer0[150];// '/Rate Transition5' - real32_T RateTransition9_Buffer0[150];// '/Rate Transition9' - real32_T RateTransition6_Buffer0[1600];// '/Rate Transition6' - real32_T RateTransition8_Buffer0[50];// '/Rate Transition8' - real32_T RateTransition5_Buffer0_i[150];// '/Rate Transition5' - real32_T RateTransition6_Buffer0_h[100];// '/Rate Transition6' - real32_T RateTransition5_Buffer0_f[150];// '/Rate Transition5' - real32_T RateTransition6_Buffer0_e[100];// '/Rate Transition6' - real32_T int_id_hist[50]; // '/generate_output' - real32_T int_observations[1600]; // '/generate_output' - int32_T clockTickCounter; // '/AR_pulse_gen' - int32_T clockTickCounter_c; // '/landmark_pulse_gen' - int32_T clockTickCounter_n; // '/handrail_pulse_gen' - uint32_T RateTransition3_Buffer0; // '/Rate Transition3' - uint32_T RateTransition1_Buffer0; // '/Rate Transition1' - uint32_T RandSeed[3]; // '/random_noise' - uint32_T RandSeed_k[3]; // '/random_noise' - uint32_T RateTransition3_Buffer0_g; // '/Rate Transition3' - uint32_T RateTransition1_Buffer0_b; // '/Rate Transition1' - uint32_T RateTransition3_Buffer0_o; // '/Rate Transition3' - uint32_T RateTransition1_Buffer0_l; // '/Rate Transition1' - uint32_T RateTransition3_Buffer0_n; // '/Rate Transition3' - uint32_T RateTransition1_Buffer0_i; // '/Rate Transition1' - uint32_T RandSeed_i[3]; // '/Random Number' - uint32_T RandSeed_m[3]; // '/Random Number1' - uint32_T RandSeed_mb; // '/pixel_noise' - uint32_T RandSeed_d; // '/pixel_noise' - uint32_T RandSeed_mi; // '/pixel_noise' - uint32_T RandSeed_o; // '/pixel_noise' - uint32_T RandSeed_ip[3]; // '/random_noise1' - uint32_T RandSeed_e[3]; // '/random_noise2' - uint32_T RandSeed_p[3]; // '/random_noise1' - uint32_T RandSeed_n[3]; // '/random_noise2' - uint32_T state[625]; // '/generate_output' - uint32_T state_d[625]; // '/generate_output' - uint32_T state_m[625]; // '/generate_output' - uint8_T UnitDelay_DSTATE_i; // '/Unit Delay' - uint8_T UnitDelay_DSTATE_if; // '/Unit Delay' - uint8_T UnitDelay_DSTATE_h; // '/Unit Delay' - uint8_T UnitDelay_DSTATE_ht; // '/Unit Delay' - uint8_T UnitDelay_DSTATE_hj; // '/Unit Delay' - uint8_T UnitDelay_DSTATE_g; // '/Unit Delay' - uint8_T UnitDelay_DSTATE_a; // '/Unit Delay' - uint8_T UnitDelay_DSTATE_it; // '/Unit Delay' - boolean_T UnitDelay_DSTATE_m; // '/Unit Delay' - boolean_T UnitDelay_DSTATE_c; // '/Unit Delay' - uint8_T RateTransition7_Buffer0[50]; // '/Rate Transition7' - uint8_T icLoad; // '/Delay' - uint8_T RateTransition10_Buffer0; // '/Rate Transition10' - uint8_T RateTransition7_Buffer0_a[800];// '/Rate Transition7' - uint8_T RateTransition7_Buffer0_an[50];// '/Rate Transition7' - uint8_T RateTransition7_Buffer0_j[50];// '/Rate Transition7' - uint8_T int_valid_flag[800]; // '/generate_output' - uint8_T int_registration_number; // '/generate_output' - boolean_T int_id_hist_not_empty; // '/generate_output' - DW_servo_model_sim_model_lib0_T servo_model_f;// '/servo_model' - DW_speed_controller_sim_model_T speed_controller_c;// '/speed_controller' - DW_blower_aerodynamics_sim__c_T blower_aerodynamics_j;// '/blower_aerodynamics' - DW_servo_model_sim_model_lib0_T servo_model;// '/servo_model' - DW_speed_controller_sim_model_T speed_controller;// '/speed_controller' - DW_blower_aerodynamics_sim_mo_T blower_aerodynamics;// '/blower_aerodynamics' - DW_pinhole_projection_model_s_T pinhole_projection_model_o[3979];// '/pinhole_projection_model' - DW_pinhole_projection_model_s_T pinhole_projection_model_id[8632];// '/pinhole_projection_model' - DW_pinhole_projection_model_s_T pinhole_projection_model_i[504];// '/pinhole_projection_model' - DW_pinhole_projection_model_s_T pinhole_projection_model[36];// '/pinhole_projection_model' -} DW_sim_model_lib0_T; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_sim_model_lib0_T_ { - real_T pixel_noise_Mean; // Expression: 0 - // Referenced by: '/pixel_noise' - - real_T pixel_noise1_Mean; // Expression: 0 - // Referenced by: '/pixel_noise1' - - real32_T Constant_Value[2]; // Expression: single([0;0]) - // Referenced by: '/Constant' - - real32_T Switch_Threshold; // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - real32_T Constant_Value_n; // Computed Parameter: Constant_Value_n - // Referenced by: '/Constant' - -}; - -// Parameters for system: '/pinhole_projection_model' -struct P_pinhole_projection_model_si_T_ { - P_CoreSubsys_sim_model_lib0_T CoreSubsys;// '/CoreSubsys' -}; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_sim_model_lib0_a_T_ { - real_T Constant2_Value[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value; // Expression: 0 - // Referenced by: '/Constant3' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T Gain1_Gain; // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - real32_T Gain_Gain_k; // Computed Parameter: Gain_Gain_k - // Referenced by: '/Gain' - - real32_T Gain1_Gain_f; // Computed Parameter: Gain1_Gain_f - // Referenced by: '/Gain1' - - real32_T Gain2_Gain; // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_k; // Computed Parameter: Gain2_Gain_k - // Referenced by: '/Gain2' - -}; - -// Parameters for system: '/latch_nozzle_thrust_matricies' -struct P_latch_nozzle_thrust_matrici_T_ { - real32_T Constant2_Value[24]; // Expression: [zeros(6,3,'single'), ones(6,1,'single')] - // Referenced by: '/Constant2' - - real32_T thrust2torque_B_Y0; // Computed Parameter: thrust2torque_B_Y0 - // Referenced by: '/thrust2torque_B' - - real32_T thrust2force_B_Y0; // Computed Parameter: thrust2force_B_Y0 - // Referenced by: '/thrust2force_B' - - real32_T Switch_Threshold; // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - P_CoreSubsys_sim_model_lib0_a_T CoreSubsys;// '/CoreSubsys' -}; - -// Parameters for system: '/speed_controller' -struct P_speed_controller_sim_model__T_ { - real32_T DiscretePIDController_LowerSatu;// Mask Parameter: DiscretePIDController_LowerSatu - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T DiscreteTransferFcn1_InitialSta;// Computed Parameter: DiscreteTransferFcn1_InitialSta - // Referenced by: '/Discrete Transfer Fcn1' - - real32_T RateLimiter_RisingLim; // Computed Parameter: RateLimiter_RisingLim - // Referenced by: '/Rate Limiter' - - real32_T RateLimiter_FallingLim; // Computed Parameter: RateLimiter_FallingLim - // Referenced by: '/Rate Limiter' - - real32_T RateLimiter_IC; // Computed Parameter: RateLimiter_IC - // Referenced by: '/Rate Limiter' - - real32_T Integrator_gainval; // Computed Parameter: Integrator_gainval - // Referenced by: '/Integrator' - - real32_T Integrator_IC; // Computed Parameter: Integrator_IC - // Referenced by: '/Integrator' - - real32_T Filter_gainval; // Computed Parameter: Filter_gainval - // Referenced by: '/Filter' - - real32_T Filter_IC; // Computed Parameter: Filter_IC - // Referenced by: '/Filter' - - real32_T ZeroGain_Gain; // Computed Parameter: ZeroGain_Gain - // Referenced by: '/ZeroGain' - - real32_T Constant_Value; // Computed Parameter: Constant_Value - // Referenced by: '/Constant' - -}; - -// Parameters for system: '/servo_model' -struct P_servo_model_sim_model_lib0_T_ { - real32_T DiscretePIDController_LowerSatu;// Mask Parameter: DiscretePIDController_LowerSatu - // Referenced by: - // '/Saturate' - // '/DeadZone' - - real32_T DiscreteTimeIntegrator4_gainval;// Computed Parameter: DiscreteTimeIntegrator4_gainval - // Referenced by: '/Discrete-Time Integrator4' - - real32_T Backlash1_InitialOutput; // Computed Parameter: Backlash1_InitialOutput - // Referenced by: '/Backlash1' - - real32_T Integrator_gainval; // Computed Parameter: Integrator_gainval - // Referenced by: '/Integrator' - - real32_T Integrator_IC; // Computed Parameter: Integrator_IC - // Referenced by: '/Integrator' - - real32_T Filter_gainval; // Computed Parameter: Filter_gainval - // Referenced by: '/Filter' - - real32_T Filter_IC; // Computed Parameter: Filter_IC - // Referenced by: '/Filter' - - real32_T DiscreteTimeIntegrator3_gainval;// Computed Parameter: DiscreteTimeIntegrator3_gainval - // Referenced by: '/Discrete-Time Integrator3' - - real32_T DiscreteTimeIntegrator3_IC; // Expression: single(0) - // Referenced by: '/Discrete-Time Integrator3' - - real32_T ZeroGain_Gain; // Computed Parameter: ZeroGain_Gain - // Referenced by: '/ZeroGain' - - real32_T Constant_Value; // Computed Parameter: Constant_Value - // Referenced by: '/Constant' - -}; - -// Parameters (auto storage) -struct P_sim_model_lib0_T_ { - real_T abp_P_CG_B_B_error[3]; // Variable: abp_P_CG_B_B_error - // Referenced by: - // '/latch_nozzle_thrust_matricies' - // '/latch_nozzle_thrust_matricies' - - real_T astrobee_time_step_size; // Variable: astrobee_time_step_size - // Referenced by: - // '/Constant' - // '/AR_pulse_gen' - // '/handrail_pulse_gen' - // '/landmark_pulse_gen' - // '/Gain1' - // '/Gain4' - // '/Gain3' - // '/Gain4' - - real_T bpm_PM1_nozzle_noise_feedback_gain[6];// Variable: bpm_PM1_nozzle_noise_feedback_gain - // Referenced by: '/blower_aerodynamics' - - real_T bpm_PM1_randn_noise_seed; // Variable: bpm_PM1_randn_noise_seed - // Referenced by: '/blower_aerodynamics' - - real_T bpm_PM2_nozzle_noise_feedback_gain[6];// Variable: bpm_PM2_nozzle_noise_feedback_gain - // Referenced by: '/blower_aerodynamics' - - real_T bpm_PM2_randn_noise_seed; // Variable: bpm_PM2_randn_noise_seed - // Referenced by: '/blower_aerodynamics' - - real_T bpm_servo_min_theta; // Variable: bpm_servo_min_theta - // Referenced by: - // '/servo_model' - // '/servo_model' - - real_T cvs_AR_pixel_noise; // Variable: cvs_AR_pixel_noise - // Referenced by: '/pinhole_projection_model' - - real_T cvs_AR_process_time; // Variable: cvs_AR_process_time - // Referenced by: '/AR_pulse_gen' - - real_T cvs_handrail_noise_var; // Variable: cvs_handrail_noise_var - // Referenced by: '/pixel_noise' - - real_T cvs_handrail_process_time; // Variable: cvs_handrail_process_time - // Referenced by: '/handrail_pulse_gen' - - real_T cvs_landmark_process_time; // Variable: cvs_landmark_process_time - // Referenced by: '/landmark_pulse_gen' - - real_T cvs_noise_seed; // Variable: cvs_noise_seed - // Referenced by: - // '/pinhole_projection_model' - // '/pinhole_projection_model' - // '/pixel_noise' - // '/pinhole_projection_model' - // '/pinhole_projection_model' - - real_T env_ext_air_omega_variance[3];// Variable: env_ext_air_omega_variance - // Referenced by: '/Random Number1' - - real_T env_ext_air_vel_seed[3]; // Variable: env_ext_air_vel_seed - // Referenced by: - // '/Random Number' - // '/Random Number1' - - real_T env_ext_air_vel_variance[3]; // Variable: env_ext_air_vel_variance - // Referenced by: '/Random Number' - - real_T env_rotational_drag_coeff; // Variable: env_rotational_drag_coeff - // Referenced by: '/Constant11' - - real_T epson_P_sensor_B_B_error[3]; // Variable: epson_P_sensor_B_B_error - // Referenced by: '/ 2' - - real_T epson_Q_B2accel_error[4]; // Variable: epson_Q_B2accel_error - // Referenced by: '/Constant13' - - real_T epson_Q_B2gyro_error[4]; // Variable: epson_Q_B2gyro_error - // Referenced by: '/Constant9' - - real_T epson_accel_filt_den; // Variable: epson_accel_filt_den - // Referenced by: '/Discrete Transfer Fcn1' - - real_T epson_accel_filt_num; // Variable: epson_accel_filt_num - // Referenced by: '/Discrete Transfer Fcn1' - - real_T epson_accel_lower_sat; // Variable: epson_accel_lower_sat - // Referenced by: '/Saturation' - - real_T epson_accel_markov_tau; // Variable: epson_accel_markov_tau - // Referenced by: '/Gain2' - - real_T epson_accel_noise_seed[3]; // Variable: epson_accel_noise_seed - // Referenced by: - // '/random_noise' - // '/random_noise1' - // '/random_noise2' - - real_T epson_accel_nonlinearity_coeff;// Variable: epson_accel_nonlinearity_coeff - // Referenced by: '/Constant6' - - real_T epson_accel_resolution; // Variable: epson_accel_resolution - // Referenced by: '/Quantizer' - - real_T epson_accel_sf_coef; // Variable: epson_accel_sf_coef - // Referenced by: '/scale_factor' - - real_T epson_accel_temp_bias_coeff[3];// Variable: epson_accel_temp_bias_coeff - // Referenced by: '/Constant2' - - real_T epson_accel_upper_sat; // Variable: epson_accel_upper_sat - // Referenced by: - // '/Constant11' - // '/Saturation' - - real_T epson_gyro_bias_ic[3]; // Variable: epson_gyro_bias_ic - // Referenced by: '/Discrete-Time Integrator2' - - real_T epson_gyro_filt_den; // Variable: epson_gyro_filt_den - // Referenced by: '/Discrete Transfer Fcn' - - real_T epson_gyro_filt_num; // Variable: epson_gyro_filt_num - // Referenced by: '/Discrete Transfer Fcn' - - real_T epson_gyro_linear_accel_bias_coeff;// Variable: epson_gyro_linear_accel_bias_coeff - // Referenced by: '/Constant3' - - real_T epson_gyro_lower_sat; // Variable: epson_gyro_lower_sat - // Referenced by: '/Saturation1' - - real_T epson_gyro_markov_tau; // Variable: epson_gyro_markov_tau - // Referenced by: '/Gain2' - - real_T epson_gyro_noise_seed[3]; // Variable: epson_gyro_noise_seed - // Referenced by: - // '/random_noise' - // '/random_noise1' - // '/random_noise2' - - real_T epson_gyro_nonlinearity_coeff;// Variable: epson_gyro_nonlinearity_coeff - // Referenced by: '/Constant9' - - real_T epson_gyro_resolution; // Variable: epson_gyro_resolution - // Referenced by: '/Quantizer1' - - real_T epson_gyro_sf_coef; // Variable: epson_gyro_sf_coef - // Referenced by: '/scale_factor' - - real_T epson_gyro_temp_bias_coeff[3];// Variable: epson_gyro_temp_bias_coeff - // Referenced by: '/Constant8' - - real_T epson_gyro_upper_sat; // Variable: epson_gyro_upper_sat - // Referenced by: - // '/Constant6' - // '/Saturation1' - - real_T epson_no_rot_effects; // Variable: epson_no_rot_effects - // Referenced by: '/Constant4' - - real32_T abp_PM1_P_nozzle_B_B[18]; // Variable: abp_PM1_P_nozzle_B_B - // Referenced by: '/latch_nozzle_thrust_matricies' - - real32_T abp_PM1_discharge_coeff[6]; // Variable: abp_PM1_discharge_coeff - // Referenced by: '/blower_aerodynamics' - - real32_T abp_PM1_nozzle_orientations[18];// Variable: abp_PM1_nozzle_orientations - // Referenced by: '/latch_nozzle_thrust_matricies' - - real32_T abp_PM1_nozzle_widths[6]; // Variable: abp_PM1_nozzle_widths - // Referenced by: '/calc_nozzle_area' - - real32_T abp_PM2_P_nozzle_B_B[18]; // Variable: abp_PM2_P_nozzle_B_B - // Referenced by: '/latch_nozzle_thrust_matricies' - - real32_T abp_PM2_discharge_coeff[6]; // Variable: abp_PM2_discharge_coeff - // Referenced by: '/blower_aerodynamics' - - real32_T abp_PM2_nozzle_orientations[18];// Variable: abp_PM2_nozzle_orientations - // Referenced by: '/latch_nozzle_thrust_matricies' - - real32_T abp_PM2_nozzle_widths[6]; // Variable: abp_PM2_nozzle_widths - // Referenced by: '/calc_nozzle_area' - - real32_T abp_impeller_diameter; // Variable: abp_impeller_diameter - // Referenced by: - // '/blower_aerodynamics' - // '/blower_aerodynamics' - - real32_T abp_nozzle_flap_count; // Variable: abp_nozzle_flap_count - // Referenced by: - // '/calc_nozzle_area' - // '/calc_nozzle_area' - - real32_T abp_nozzle_flap_length; // Variable: abp_nozzle_flap_length - // Referenced by: - // '/calc_nozzle_area' - // '/calc_nozzle_area' - - real32_T abp_nozzle_gear_ratio; // Variable: abp_nozzle_gear_ratio - // Referenced by: - // '/calc_nozzle_area' - // '/calc_nozzle_area' - - real32_T abp_nozzle_intake_height; // Variable: abp_nozzle_intake_height - // Referenced by: - // '/calc_nozzle_area' - // '/calc_nozzle_area' - - real32_T abp_nozzle_min_open_angle; // Variable: abp_nozzle_min_open_angle - // Referenced by: - // '/calc_nozzle_area' - // '/calc_nozzle_area' - - real32_T abp_pm1_impeller_orientation[3];// Variable: abp_pm1_impeller_orientation - // Referenced by: '/Constant1' - - real32_T abp_pm1_zero_thrust_area; // Variable: abp_pm1_zero_thrust_area - // Referenced by: '/blower_aerodynamics' - - real32_T abp_pm2_impeller_orientation[3];// Variable: abp_pm2_impeller_orientation - // Referenced by: '/Constant1' - - real32_T abp_pm2_zero_thrust_area; // Variable: abp_pm2_zero_thrust_area - // Referenced by: '/blower_aerodynamics' - - real32_T bmp_PM1_impeller_orientation_error[3];// Variable: bmp_PM1_impeller_orientation_error - // Referenced by: '/Constant3' - - real32_T bmp_PM2_impeller_orientation_error[3];// Variable: bmp_PM2_impeller_orientation_error - // Referenced by: '/Constant3' - - real32_T bpm_PM1_P_nozzle_B_B_error[18];// Variable: bpm_PM1_P_nozzle_B_B_error - // Referenced by: '/latch_nozzle_thrust_matricies' - - real32_T bpm_PM1_Q_nozzle2misaligned[24];// Variable: bpm_PM1_Q_nozzle2misaligned - // Referenced by: '/latch_nozzle_thrust_matricies' - - real32_T bpm_PM1_nozzle_discharge_coeff_error[6];// Variable: bpm_PM1_nozzle_discharge_coeff_error - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_PM1_zero_thrust_area_error;// Variable: bpm_PM1_zero_thrust_area_error - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_PM2_P_nozzle_B_B_error[18];// Variable: bpm_PM2_P_nozzle_B_B_error - // Referenced by: '/latch_nozzle_thrust_matricies' - - real32_T bpm_PM2_Q_nozzle2misaligned[24];// Variable: bpm_PM2_Q_nozzle2misaligned - // Referenced by: '/latch_nozzle_thrust_matricies' - - real32_T bpm_PM2_nozzle_discharge_coeff_error[6];// Variable: bpm_PM2_nozzle_discharge_coeff_error - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_PM2_zero_thrust_area_error;// Variable: bpm_PM2_zero_thrust_area_error - // Referenced by: '/blower_aerodynamics' - - real32_T bpm_imp_ctl_filt_n; // Variable: bpm_imp_ctl_filt_n - // Referenced by: - // '/speed_controller' - // '/speed_controller' - - real32_T bpm_imp_ctl_kd; // Variable: bpm_imp_ctl_kd - // Referenced by: - // '/speed_controller' - // '/speed_controller' - - real32_T bpm_imp_ctl_ki; // Variable: bpm_imp_ctl_ki - // Referenced by: - // '/speed_controller' - // '/speed_controller' - - real32_T bpm_imp_ctl_kp; // Variable: bpm_imp_ctl_kp - // Referenced by: - // '/speed_controller' - // '/speed_controller' - - real32_T bpm_imp_max_voltage; // Variable: bpm_imp_max_voltage - // Referenced by: - // '/speed_controller' - // '/speed_controller' - - real32_T bpm_imp_motor_friction_coeff;// Variable: bpm_imp_motor_friction_coeff - // Referenced by: - // '/dc_motor_model' - // '/dc_motor_model' - - real32_T bpm_imp_motor_r; // Variable: bpm_imp_motor_r - // Referenced by: - // '/dc_motor_model' - // '/dc_motor_model' - - real32_T bpm_imp_motor_speed_k; // Variable: bpm_imp_motor_speed_k - // Referenced by: - // '/dc_motor_model' - // '/dc_motor_model' - - real32_T bpm_imp_motor_torque_k; // Variable: bpm_imp_motor_torque_k - // Referenced by: - // '/dc_motor_model' - // '/dc_motor_model' - - real32_T bpm_imp_speed_filt_den; // Variable: bpm_imp_speed_filt_den - // Referenced by: - // '/speed_controller' - // '/speed_controller' - - real32_T bpm_imp_speed_filt_num; // Variable: bpm_imp_speed_filt_num - // Referenced by: - // '/speed_controller' - // '/speed_controller' - - real32_T bpm_impeller_inertia; // Variable: bpm_impeller_inertia - // Referenced by: - // '/Constant2' - // '/Gain' - // '/Gain2' - // '/Constant2' - // '/Gain' - // '/Gain2' - - real32_T bpm_impeller_inertia_error; // Variable: bpm_impeller_inertia_error - // Referenced by: - // '/Constant4' - // '/Gain2' - // '/Constant4' - // '/Gain2' - - real32_T bpm_impeller_init_speed; // Variable: bpm_impeller_init_speed - // Referenced by: - // '/Discrete-Time Integrator' - // '/Discrete-Time Integrator' - - real32_T bpm_lookup_Cdp_data[334]; // Variable: bpm_lookup_Cdp_data - // Referenced by: - // '/blower_aerodynamics' - // '/blower_aerodynamics' - - real32_T bpm_lookup_totalarea_breakpoints[334];// Variable: bpm_lookup_totalarea_breakpoints - // Referenced by: - // '/blower_aerodynamics' - // '/blower_aerodynamics' - - real32_T bpm_servo_ctl_filt_n; // Variable: bpm_servo_ctl_filt_n - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_ctl_kd; // Variable: bpm_servo_ctl_kd - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_ctl_ki; // Variable: bpm_servo_ctl_ki - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_ctl_kp; // Variable: bpm_servo_ctl_kp - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_max_theta; // Variable: bpm_servo_max_theta - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_max_voltage; // Variable: bpm_servo_max_voltage - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_motor_backlash_deadband;// Variable: bpm_servo_motor_backlash_deadband - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_motor_friction_coeff;// Variable: bpm_servo_motor_friction_coeff - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_motor_gear_box_inertia;// Variable: bpm_servo_motor_gear_box_inertia - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_motor_gear_ratio; // Variable: bpm_servo_motor_gear_ratio - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_motor_k; // Variable: bpm_servo_motor_k - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_motor_r; // Variable: bpm_servo_motor_r - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_pwm2angle; // Variable: bpm_servo_pwm2angle - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T bpm_servo_pwm2angle_bias; // Variable: bpm_servo_pwm2angle_bias - // Referenced by: - // '/servo_model' - // '/servo_model' - - real32_T const_air_density; // Variable: const_air_density - // Referenced by: - // '/blower_aerodynamics' - // '/blower_aerodynamics' - - real32_T const_gravity_local[3]; // Variable: const_gravity_local - // Referenced by: '/Constant2' - - real32_T cvs_AR_map_error[108]; // Variable: cvs_AR_map_error - // Referenced by: '/Constant4' - - real32_T cvs_AR_map_iss[108]; // Variable: cvs_AR_map_iss - // Referenced by: '/Constant3' - - real32_T cvs_dockcam_P_B_B_error[3]; // Variable: cvs_dockcam_P_B_B_error - // Referenced by: '/Constant1' - - real32_T cvs_dockcam_Q_B2dockcam_error[4];// Variable: cvs_dockcam_Q_B2dockcam_error - // Referenced by: '/Constant6' - - real32_T cvs_dockcam_max_dist; // Variable: cvs_dockcam_max_dist - // Referenced by: '/pinhole_projection_model' - - real32_T cvs_dockcam_min_dist; // Variable: cvs_dockcam_min_dist - // Referenced by: '/pinhole_projection_model' - - real32_T cvs_dockcam_pointing[3]; // Variable: cvs_dockcam_pointing - // Referenced by: '/pinhole_projection_model' - - real32_T cvs_handrail_map_error[1512];// Variable: cvs_handrail_map_error - // Referenced by: '/Constant4' - - real32_T cvs_handrail_map_iss[1512]; // Variable: cvs_handrail_map_iss - // Referenced by: '/Constant3' - - real32_T cvs_landmark_map_error[25896];// Variable: cvs_landmark_map_error - // Referenced by: '/Constant4' - - real32_T cvs_landmark_map_iss[25896];// Variable: cvs_landmark_map_iss - // Referenced by: '/Constant3' - - real32_T cvs_navcam_P_B_B_error[3]; // Variable: cvs_navcam_P_B_B_error - // Referenced by: - // '/Constant1' - // '/Constant1' - - real32_T cvs_navcam_Q_B2navcan_error[4];// Variable: cvs_navcam_Q_B2navcan_error - // Referenced by: - // '/Constant6' - // '/Constant6' - - real32_T cvs_navcam_max_dist; // Variable: cvs_navcam_max_dist - // Referenced by: - // '/pinhole_projection_model' - // '/pinhole_projection_model' - - real32_T cvs_navcam_min_dist; // Variable: cvs_navcam_min_dist - // Referenced by: - // '/pinhole_projection_model' - // '/pinhole_projection_model' - - real32_T cvs_navcam_pointing[3]; // Variable: cvs_navcam_pointing - // Referenced by: - // '/pinhole_projection_model' - // '/pinhole_projection_model' - - real32_T cvs_optflow_map_error[11937];// Variable: cvs_optflow_map_error - // Referenced by: '/Constant4' - - real32_T cvs_optflow_map_iss[11937]; // Variable: cvs_optflow_map_iss - // Referenced by: '/Constant3' - - real32_T cvs_perchcam_P_B_B_error[3];// Variable: cvs_perchcam_P_B_B_error - // Referenced by: '/Constant1' - - real32_T cvs_perchcam_Q_B2perchcam_error[4];// Variable: cvs_perchcam_Q_B2perchcam_error - // Referenced by: '/Constant6' - - real32_T cvs_perchcam_max_dist; // Variable: cvs_perchcam_max_dist - // Referenced by: '/pinhole_projection_model' - - real32_T cvs_perchcam_min_dist; // Variable: cvs_perchcam_min_dist - // Referenced by: '/pinhole_projection_model' - - real32_T cvs_perchcam_pointing[3]; // Variable: cvs_perchcam_pointing - // Referenced by: '/pinhole_projection_model' - - real32_T env_avg_drag_coeff; // Variable: env_avg_drag_coeff - // Referenced by: '/Constant1' - - real32_T env_max_ext_air_omega; // Variable: env_max_ext_air_omega - // Referenced by: '/Discrete-Time Integrator1' - - real32_T env_max_ext_air_vel; // Variable: env_max_ext_air_vel - // Referenced by: - // '/Constant9' - // '/Discrete-Time Integrator' - - real32_T epson_accel_bias_ic[3]; // Variable: epson_accel_bias_ic - // Referenced by: '/Discrete-Time Integrator2' - - real32_T mlp_command_list[228]; // Variable: mlp_command_list - // Referenced by: '/Constant1' - - real32_T mlp_dummy_state_cmd[19]; // Variable: mlp_dummy_state_cmd - // Referenced by: '/Constant' - - real32_T mlp_num_commands; // Variable: mlp_num_commands - // Referenced by: '/Constant1' - - real32_T tun_abp_p_dockcam_body_body_sim[3];// Variable: tun_abp_p_dockcam_body_body_sim - // Referenced by: '/Constant2' - - real32_T tun_abp_p_imu_body_body[3]; // Variable: tun_abp_p_imu_body_body - // Referenced by: '/ 1' - - real32_T tun_abp_p_navcam_body_body_sim[3];// Variable: tun_abp_p_navcam_body_body_sim - // Referenced by: - // '/Constant2' - // '/Constant2' - - real32_T tun_abp_p_perchcam_body_body_sim[3];// Variable: tun_abp_p_perchcam_body_body_sim - // Referenced by: '/Constant2' - - real32_T tun_abp_q_body2dockcam[4]; // Variable: tun_abp_q_body2dockcam - // Referenced by: '/Constant3' - - real32_T tun_abp_q_body2navcam[4]; // Variable: tun_abp_q_body2navcam - // Referenced by: - // '/Constant3' - // '/Constant3' - - real32_T tun_abp_q_body2perchcam[4]; // Variable: tun_abp_q_body2perchcam - // Referenced by: '/Constant3' - - real32_T tun_abp_quat_body2imu[4]; // Variable: tun_abp_quat_body2imu - // Referenced by: - // '/Constant2' - // '/Constant8' - - real32_T tun_ase_gravity_accel[3]; // Variable: tun_ase_gravity_accel - // Referenced by: '/Constant4' - - real32_T tun_bpm_PM1_thrust_error_sf;// Variable: tun_bpm_PM1_thrust_error_sf - // Referenced by: '/blower_aerodynamics' - - real32_T tun_bpm_PM2_thrust_error_sf;// Variable: tun_bpm_PM2_thrust_error_sf - // Referenced by: '/blower_aerodynamics' - - real32_T tun_bpm_noise_on_flag; // Variable: tun_bpm_noise_on_flag - // Referenced by: - // '/blower_aerodynamics' - // '/blower_aerodynamics' - // '/latch_nozzle_thrust_matricies' - // '/Gain' - // '/Gain1' - // '/Constant' - // '/latch_nozzle_thrust_matricies' - // '/Gain' - // '/Gain1' - // '/Constant' - - real32_T tun_cvs_dockcam_focal_length_X;// Variable: tun_cvs_dockcam_focal_length_X - // Referenced by: '/pinhole_projection_model' - - real32_T tun_cvs_dockcam_focal_length_Y;// Variable: tun_cvs_dockcam_focal_length_Y - // Referenced by: '/pinhole_projection_model' - - real32_T tun_cvs_dockcam_num_pixels_X;// Variable: tun_cvs_dockcam_num_pixels_X - // Referenced by: '/pinhole_projection_model' - - real32_T tun_cvs_dockcam_num_pixels_Y;// Variable: tun_cvs_dockcam_num_pixels_Y - // Referenced by: '/pinhole_projection_model' - - real32_T tun_cvs_navcam_focal_length_X;// Variable: tun_cvs_navcam_focal_length_X - // Referenced by: - // '/pinhole_projection_model' - // '/pinhole_projection_model' - - real32_T tun_cvs_navcam_focal_length_Y;// Variable: tun_cvs_navcam_focal_length_Y - // Referenced by: - // '/pinhole_projection_model' - // '/pinhole_projection_model' - - real32_T tun_cvs_navcam_num_pixels_X;// Variable: tun_cvs_navcam_num_pixels_X - // Referenced by: - // '/pinhole_projection_model' - // '/pinhole_projection_model' - - real32_T tun_cvs_navcam_num_pixels_Y;// Variable: tun_cvs_navcam_num_pixels_Y - // Referenced by: - // '/pinhole_projection_model' - // '/pinhole_projection_model' - - real32_T tun_cvs_perchcam_focal_length_X;// Variable: tun_cvs_perchcam_focal_length_X - // Referenced by: '/pinhole_projection_model' - - real32_T tun_cvs_perchcam_focal_length_Y;// Variable: tun_cvs_perchcam_focal_length_Y - // Referenced by: '/pinhole_projection_model' - - real32_T tun_cvs_perchcam_num_pixels_X;// Variable: tun_cvs_perchcam_num_pixels_X - // Referenced by: '/pinhole_projection_model' - - real32_T tun_cvs_perchcam_num_pixels_Y;// Variable: tun_cvs_perchcam_num_pixels_Y - // Referenced by: '/pinhole_projection_model' - - real32_T tun_env_accel_dof_gain[3]; // Variable: tun_env_accel_dof_gain - // Referenced by: '/Gain' - - real32_T tun_env_alpha_dof_gain[3]; // Variable: tun_env_alpha_dof_gain - // Referenced by: '/Gain' - - real32_T tun_inertia_error_mat[9]; // Variable: tun_inertia_error_mat - // Referenced by: '/Constant1' - - real32_T tun_ini_P_B_ISS_ISS[3]; // Variable: tun_ini_P_B_ISS_ISS - // Referenced by: '/Constant11' - - real32_T tun_ini_Q_ISS2B[4]; // Variable: tun_ini_Q_ISS2B - // Referenced by: '/Constant1' - - real32_T tun_ini_V_B_ISS_ISS[3]; // Variable: tun_ini_V_B_ISS_ISS - // Referenced by: '/Constant12' - - real32_T tun_ini_omega_B_ISS_B[3]; // Variable: tun_ini_omega_B_ISS_B - // Referenced by: '/Constant3' - - real32_T tun_iss_omega_ISS_ECI_ISS[3];// Variable: tun_iss_omega_ISS_ECI_ISS - // Referenced by: - // '/Constant4' - // '/Constant1' - - real32_T tun_mass_error; // Variable: tun_mass_error - // Referenced by: '/Constant' - - real32_T tun_sim_cg_error[3]; // Variable: tun_sim_cg_error - // Referenced by: '/Constant' - - uint32_T cvs_AR_valid_times[6]; // Variable: cvs_AR_valid_times - // Referenced by: '/Constant1' - - uint32_T cvs_handrail_valid_times[6];// Variable: cvs_handrail_valid_times - // Referenced by: '/Constant1' - - uint32_T cvs_landmark_valid_times[6];// Variable: cvs_landmark_valid_times - // Referenced by: '/Constant1' - - uint32_T cvs_optflow_valid_times[6]; // Variable: cvs_optflow_valid_times - // Referenced by: '/Constant1' - - uint32_T ini_time_nanoseconds; // Variable: ini_time_nanoseconds - // Referenced by: '/Unit Delay' - - uint32_T ini_time_seconds; // Variable: ini_time_seconds - // Referenced by: '/Unit Delay1' - - uint32_T mlp_command_times[24]; // Variable: mlp_command_times - // Referenced by: '/Constant2' - - uint32_T mlp_loc_mode_cmd_times[6]; // Variable: mlp_loc_mode_cmd_times - // Referenced by: '/Constant5' - - uint32_T mlp_mode_cmd_times[6]; // Variable: mlp_mode_cmd_times - // Referenced by: '/Constant3' - - uint32_T mlp_speed_gain_cmd_times[6];// Variable: mlp_speed_gain_cmd_times - // Referenced by: '/Constant7' - - uint8_T mlp_loc_mode_cmd_list[3]; // Variable: mlp_loc_mode_cmd_list - // Referenced by: '/Constant6' - - uint8_T mlp_mode_cmd_list[3]; // Variable: mlp_mode_cmd_list - // Referenced by: '/Constant4' - - uint8_T mlp_speed_gain_cmd_list[3]; // Variable: mlp_speed_gain_cmd_list - // Referenced by: '/Constant8' - - uint8_T tun_ase_gravity_removal; // Variable: tun_ase_gravity_removal - // Referenced by: '/Constant6' - - uint8_T tun_cvs_noise_on; // Variable: tun_cvs_noise_on - // Referenced by: - // '/pinhole_projection_model' - // '/Gain' - // '/pinhole_projection_model' - // '/Gain' - // '/pinhole_projection_model' - // '/Gain' - // '/pinhole_projection_model' - // '/Gain' - // '/Constant' - // '/Gain' - // '/Gain1' - // '/Constant' - // '/Gain' - // '/Gain1' - // '/Constant' - // '/Gain' - // '/Gain1' - // '/Constant' - // '/Gain' - // '/Gain1' - - uint8_T tun_env_drag_disturb_on; // Variable: tun_env_drag_disturb_on - // Referenced by: - // '/Constant6' - // '/Constant8' - - uint8_T tun_epson_report_truth; // Variable: tun_epson_report_truth - // Referenced by: - // '/Constant3' - // '/Constant1' - // '/Constant11' - // '/Constant3' - // '/Gain' - - boolean_T cvs_AR_valid_mask[3]; // Variable: cvs_AR_valid_mask - // Referenced by: '/Constant3' - - boolean_T cvs_handrail_valid_mask[3];// Variable: cvs_handrail_valid_mask - // Referenced by: '/Constant3' - - boolean_T cvs_landmark_valid_mask[3];// Variable: cvs_landmark_valid_mask - // Referenced by: '/Constant3' - - boolean_T cvs_optflow_valid_mask[3]; // Variable: cvs_optflow_valid_mask - // Referenced by: '/Constant3' - - boolean_T env_grav_disturb_on; // Variable: env_grav_disturb_on - // Referenced by: '/Constant3' - - real_T ARtag_image_processing_handrail;// Mask Parameter: ARtag_image_processing_handrail - // Referenced by: '/pixel_noise' - - real_T landmark_image_processing_handr;// Mask Parameter: landmark_image_processing_handr - // Referenced by: '/pixel_noise' - - real_T optical_flow_image_processing_h;// Mask Parameter: optical_flow_image_processing_h - // Referenced by: '/pixel_noise' - - real_T ARtag_image_processing_handra_k;// Mask Parameter: ARtag_image_processing_handra_k - // Referenced by: '/pixel_noise' - - real_T landmark_image_processing_han_h;// Mask Parameter: landmark_image_processing_han_h - // Referenced by: '/pixel_noise' - - real_T optical_flow_image_processing_b;// Mask Parameter: optical_flow_image_processing_b - // Referenced by: '/pixel_noise' - - real_T handrail_image_processing_pixel;// Mask Parameter: handrail_image_processing_pixel - // Referenced by: '/pinhole_projection_model' - - real_T landmark_image_processing_pixel;// Mask Parameter: landmark_image_processing_pixel - // Referenced by: '/pinhole_projection_model' - - real_T optical_flow_image_processing_p;// Mask Parameter: optical_flow_image_processing_p - // Referenced by: '/pinhole_projection_model' - - real32_T CompareToConstant1_const; // Mask Parameter: CompareToConstant1_const - // Referenced by: '/Constant' - - real32_T CompareToConstant_const; // Mask Parameter: CompareToConstant_const - // Referenced by: '/Constant' - - real32_T bpm_blower_1_propulsion_module_;// Mask Parameter: bpm_blower_1_propulsion_module_ - // Referenced by: '/speed_controller' - - real32_T bpm_blower_2_propulsion_module_;// Mask Parameter: bpm_blower_2_propulsion_module_ - // Referenced by: '/speed_controller' - - real32_T DetectChange_vinit; // Mask Parameter: DetectChange_vinit - // Referenced by: '/Delay Input1' - - real32_T DetectChange_vinit_o; // Mask Parameter: DetectChange_vinit_o - // Referenced by: '/Delay Input1' - - uint32_T CompareToConstant_const_d; // Mask Parameter: CompareToConstant_const_d - // Referenced by: '/Constant' - - uint32_T DetectChange_vinit_j; // Mask Parameter: DetectChange_vinit_j - // Referenced by: '/Delay Input1' - - uint32_T DetectChange1_vinit; // Mask Parameter: DetectChange1_vinit - // Referenced by: '/Delay Input1' - - bpm_msg Delay1_InitialCondition; // Computed Parameter: Delay1_InitialCondition - // Referenced by: '/Delay1' - - real_T Constant1_Value; // Expression: -1 - // Referenced by: '/Constant1' - - real_T WeightedSampleTimeMath_WtEt; // Computed Parameter: WeightedSampleTimeMath_WtEt - // Referenced by: '/Weighted Sample Time Math' - - real_T Gain_Gain; // Expression: .5 - // Referenced by: '/Gain' - - real_T Constant2_Value[16]; // Expression: zeros(16,1) - // Referenced by: '/Constant2' - - real_T WeightedSampleTimeMath_WtEt_o;// Computed Parameter: WeightedSampleTimeMath_WtEt_o - // Referenced by: '/Weighted Sample Time Math' - - real_T Gain_Gain_j; // Expression: .5 - // Referenced by: '/Gain' - - real_T Constant2_Value_m[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant2_Value_i[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Gain_Gain_p; // Expression: -1 - // Referenced by: '/Gain' - - real_T Gain1_Gain; // Expression: -1 - // Referenced by: '/Gain1' - - real_T Gain2_Gain; // Expression: -1 - // Referenced by: '/Gain2' - - real_T Gain1_Gain_a; // Expression: -1 - // Referenced by: '/Gain1' - - real_T Constant1_Value_o[4]; // Expression: [0 0 0 1] - // Referenced by: '/Constant1' - - real_T Gain_Gain_f; // Expression: 2 - // Referenced by: '/Gain' - - real_T Constant1_Value_c; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Gain1_Gain_d; // Expression: 2 - // Referenced by: '/Gain1' - - real_T Constant3_Value_n; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Gain_Gain_fd; // Expression: -1 - // Referenced by: '/Gain' - - real_T Gain1_Gain_l; // Expression: -1 - // Referenced by: '/Gain1' - - real_T Gain2_Gain_a; // Expression: -1 - // Referenced by: '/Gain2' - - real_T Gain2_Gain_d; // Expression: 2 - // Referenced by: '/Gain2' - - real_T Constant5_Value; // Expression: 0 - // Referenced by: '/Constant5' - - real_T Constant2_Value_m1[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_oo; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_k; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_d[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_i; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_a; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_p[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_c4; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_b; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_c[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_o5; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_j; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant3_Value_e; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_cr[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Gain_Gain_e; // Expression: 2 - // Referenced by: '/Gain' - - real_T Constant1_Value_b; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Gain1_Gain_o; // Expression: 2 - // Referenced by: '/Gain1' - - real_T Constant3_Value_d; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Gain_Gain_l; // Expression: -1 - // Referenced by: '/Gain' - - real_T Gain1_Gain_oh; // Expression: -1 - // Referenced by: '/Gain1' - - real_T Gain2_Gain_o; // Expression: -1 - // Referenced by: '/Gain2' - - real_T Gain2_Gain_ar; // Expression: 2 - // Referenced by: '/Gain2' - - real_T Constant1_Value_p; // Expression: 2*pi - // Referenced by: '/Constant1' - - real_T random_noise_Mean; // Expression: 0 - // Referenced by: '/random_noise' - - real_T random_noise_StdDev; // Computed Parameter: random_noise_StdDev - // Referenced by: '/random_noise' - - real_T Constant_Value; // Expression: 25 - // Referenced by: '/Constant' - - real_T DiscreteTimeIntegrator2_gainval;// Computed Parameter: DiscreteTimeIntegrator2_gainval - // Referenced by: '/Discrete-Time Integrator2' - - real_T DiscreteTimeIntegrator1_gainval;// Computed Parameter: DiscreteTimeIntegrator1_gainval - // Referenced by: '/Discrete-Time Integrator1' - - real_T DiscreteTimeIntegrator1_IC; // Expression: 0 - // Referenced by: '/Discrete-Time Integrator1' - - real_T DiscreteTransferFcn1_InitialSta;// Expression: 0 - // Referenced by: '/Discrete Transfer Fcn1' - - real_T Delay2_InitialCondition; // Expression: 0.0 - // Referenced by: '/Delay2' - - real_T Delay2_InitialCondition_o; // Expression: 0.0 - // Referenced by: '/Delay2' - - real_T Constant2_Value_dr[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant2_Value_n[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_i; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Gain_Gain_d; // Expression: -1 - // Referenced by: '/Gain' - - real_T Gain1_Gain_j; // Expression: -1 - // Referenced by: '/Gain1' - - real_T Gain2_Gain_k; // Expression: -1 - // Referenced by: '/Gain2' - - real_T Gain1_Gain_h; // Expression: -1 - // Referenced by: '/Gain1' - - real_T Constant10_Value[4]; // Expression: [0 0 0 1] - // Referenced by: '/Constant10' - - real_T Gain_Gain_ee; // Expression: 2 - // Referenced by: '/Gain' - - real_T Constant1_Value_k; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Gain1_Gain_dy; // Expression: 2 - // Referenced by: '/Gain1' - - real_T Constant3_Value_iy; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Gain_Gain_k; // Expression: -1 - // Referenced by: '/Gain' - - real_T Gain1_Gain_p; // Expression: -1 - // Referenced by: '/Gain1' - - real_T Gain2_Gain_dz; // Expression: -1 - // Referenced by: '/Gain2' - - real_T Gain2_Gain_g; // Expression: 2 - // Referenced by: '/Gain2' - - real_T Constant5_Value_a; // Expression: 2*pi - // Referenced by: '/Constant5' - - real_T random_noise_Mean_k; // Expression: 0 - // Referenced by: '/random_noise' - - real_T random_noise_StdDev_c; // Computed Parameter: random_noise_StdDev_c - // Referenced by: '/random_noise' - - real_T Constant4_Value; // Expression: 25 - // Referenced by: '/Constant4' - - real_T DiscreteTimeIntegrator2_gainv_k;// Computed Parameter: DiscreteTimeIntegrator2_gainv_k - // Referenced by: '/Discrete-Time Integrator2' - - real_T DiscreteTimeIntegrator1_gainv_l;// Computed Parameter: DiscreteTimeIntegrator1_gainv_l - // Referenced by: '/Discrete-Time Integrator1' - - real_T DiscreteTimeIntegrator1_IC_k; // Expression: 0 - // Referenced by: '/Discrete-Time Integrator1' - - real_T DiscreteTransferFcn_InitialStat;// Expression: 0 - // Referenced by: '/Discrete Transfer Fcn' - - real_T Delay1_InitialCondition_m; // Expression: 0.0 - // Referenced by: '/Delay1' - - real_T Delay1_InitialCondition_b; // Expression: 0.0 - // Referenced by: '/Delay1' - - real_T Constant3_Value_ao; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant3_Value_c; // Expression: 0 - // Referenced by: '/Constant3' - - real_T AR_pulse_gen_Amp; // Expression: 1 - // Referenced by: '/AR_pulse_gen' - - real_T AR_pulse_gen_Duty; // Expression: 1 - // Referenced by: '/AR_pulse_gen' - - real_T AR_pulse_gen_PhaseDelay; // Expression: 0 - // Referenced by: '/AR_pulse_gen' - - real_T landmark_pulse_gen_Amp; // Expression: 1 - // Referenced by: '/landmark_pulse_gen' - - real_T landmark_pulse_gen_Duty; // Expression: 1 - // Referenced by: '/landmark_pulse_gen' - - real_T landmark_pulse_gen_PhaseDelay;// Expression: 0 - // Referenced by: '/landmark_pulse_gen' - - real_T Constant1_Value_ot; // Expression: 0 - // Referenced by: '/Constant1' - - real_T handrail_pulse_gen_Amp; // Expression: 1 - // Referenced by: '/handrail_pulse_gen' - - real_T handrail_pulse_gen_Duty; // Expression: 1 - // Referenced by: '/handrail_pulse_gen' - - real_T handrail_pulse_gen_PhaseDelay;// Expression: 0 - // Referenced by: '/handrail_pulse_gen' - - real_T RandomNumber_Mean; // Expression: 0 - // Referenced by: '/Random Number' - - real_T RandomNumber1_Mean; // Expression: 0 - // Referenced by: '/Random Number1' - - real_T Constant2_Value_ci[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant2_Value_n1[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_eu; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant1_Value_g; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_p; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_a[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_m; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_az; // Expression: 0 - // Referenced by: '/Constant3' - - real_T pixel_noise_Mean; // Expression: 0 - // Referenced by: '/pixel_noise' - - real_T Constant2_Value_ab[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant2_Value_nv[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_h; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant1_Value_oc; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_f; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_o[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_e; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_j0; // Expression: 0 - // Referenced by: '/Constant3' - - real_T pixel_noise_Mean_a; // Expression: 0 - // Referenced by: '/pixel_noise' - - real_T Constant2_Value_id[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant2_Value_j[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_hp; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant1_Value_f; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_o; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_k[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_l; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_p2; // Expression: 0 - // Referenced by: '/Constant3' - - real_T pixel_noise_Mean_n; // Expression: 0 - // Referenced by: '/pixel_noise' - - real_T Constant2_Value_kp[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant2_Value_d4[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_g; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant1_Value_h; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_c2; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_e[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_es; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_kj; // Expression: 0 - // Referenced by: '/Constant3' - - real_T pixel_noise_Mean_d; // Expression: 0 - // Referenced by: '/pixel_noise' - - real_T random_noise1_Mean; // Expression: 0 - // Referenced by: '/random_noise1' - - real_T random_noise1_StdDev; // Computed Parameter: random_noise1_StdDev - // Referenced by: '/random_noise1' - - real_T random_noise2_Mean; // Expression: 0 - // Referenced by: '/random_noise2' - - real_T random_noise2_StdDev; // Computed Parameter: random_noise2_StdDev - // Referenced by: '/random_noise2' - - real_T random_noise1_Mean_d; // Expression: 0 - // Referenced by: '/random_noise1' - - real_T random_noise1_StdDev_l; // Computed Parameter: random_noise1_StdDev_l - // Referenced by: '/random_noise1' - - real_T random_noise2_Mean_f; // Expression: 0 - // Referenced by: '/random_noise2' - - real_T random_noise2_StdDev_k; // Computed Parameter: random_noise2_StdDev_k - // Referenced by: '/random_noise2' - - real32_T Constant_Value_c[16]; // Computed Parameter: Constant_Value_c - // Referenced by: '/Constant' - - real32_T Constant_Value_p[16]; // Computed Parameter: Constant_Value_p - // Referenced by: '/Constant' - - real32_T Constant3_Value_g2; // Computed Parameter: Constant3_Value_g2 - // Referenced by: '/Constant3' - - real32_T Gain_Gain_dy; // Computed Parameter: Gain_Gain_dy - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ow; // Computed Parameter: Gain1_Gain_ow - // Referenced by: '/Gain1' - - real32_T Constant2_Value_l; // Computed Parameter: Constant2_Value_l - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_oe; // Computed Parameter: Gain2_Gain_oe - // Referenced by: '/Gain2' - - real32_T Gain3_Gain; // Computed Parameter: Gain3_Gain - // Referenced by: '/Gain3' - - real32_T Gain4_Gain; // Computed Parameter: Gain4_Gain - // Referenced by: '/Gain4' - - real32_T Constant1_Value_p4; // Computed Parameter: Constant1_Value_p4 - // Referenced by: '/Constant1' - - real32_T Gain5_Gain; // Computed Parameter: Gain5_Gain - // Referenced by: '/Gain5' - - real32_T Constant_Value_d; // Computed Parameter: Constant_Value_d - // Referenced by: '/Constant' - - real32_T Constant3_Value_pa; // Computed Parameter: Constant3_Value_pa - // Referenced by: '/Constant3' - - real32_T Gain_Gain_g; // Computed Parameter: Gain_Gain_g - // Referenced by: '/Gain' - - real32_T Gain1_Gain_c; // Computed Parameter: Gain1_Gain_c - // Referenced by: '/Gain1' - - real32_T Constant2_Value_dl; // Computed Parameter: Constant2_Value_dl - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_l; // Computed Parameter: Gain2_Gain_l - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_i; // Computed Parameter: Gain3_Gain_i - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_f; // Computed Parameter: Gain4_Gain_f - // Referenced by: '/Gain4' - - real32_T Constant1_Value_of; // Computed Parameter: Constant1_Value_of - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_l; // Computed Parameter: Gain5_Gain_l - // Referenced by: '/Gain5' - - real32_T Constant_Value_f; // Computed Parameter: Constant_Value_f - // Referenced by: '/Constant' - - real32_T Gain2_Gain_f; // Computed Parameter: Gain2_Gain_f - // Referenced by: '/Gain2' - - real32_T Gain_Gain_go; // Computed Parameter: Gain_Gain_go - // Referenced by: '/Gain' - - real32_T RateTransition5_X0; // Computed Parameter: RateTransition5_X0 - // Referenced by: '/Rate Transition5' - - real32_T RateTransition9_X0; // Computed Parameter: RateTransition9_X0 - // Referenced by: '/Rate Transition9' - - real32_T DiscreteTimeIntegrator1_gainv_i;// Computed Parameter: DiscreteTimeIntegrator1_gainv_i - // Referenced by: '/Discrete-Time Integrator1' - - real32_T DiscreteTimeIntegrator_gainval;// Computed Parameter: DiscreteTimeIntegrator_gainval - // Referenced by: '/Discrete-Time Integrator' - - real32_T DiscreteTimeIntegrator_gainva_p;// Computed Parameter: DiscreteTimeIntegrator_gainva_p - // Referenced by: '/Discrete-Time Integrator' - - real32_T Gain_Gain_m; // Computed Parameter: Gain_Gain_m - // Referenced by: '/Gain' - - real32_T Gain1_Gain_oi; // Computed Parameter: Gain1_Gain_oi - // Referenced by: '/Gain1' - - real32_T Gain_Gain_j1; // Computed Parameter: Gain_Gain_j1 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_e; // Computed Parameter: Gain1_Gain_e - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_p; // Computed Parameter: Gain2_Gain_p - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_h; // Computed Parameter: Gain2_Gain_h - // Referenced by: '/Gain2' - - real32_T DiscreteTimeIntegrator_gainva_j;// Computed Parameter: DiscreteTimeIntegrator_gainva_j - // Referenced by: '/Discrete-Time Integrator' - - real32_T DiscreteTimeIntegrator_IC; // Computed Parameter: DiscreteTimeIntegrator_IC - // Referenced by: '/Discrete-Time Integrator' - - real32_T Constant7_Value[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant7' - - real32_T Constant5_Value_av[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant5' - - real32_T Constant10_Value_a; // Expression: single(2) - // Referenced by: '/Constant10' - - real32_T Gain_Gain_b; // Computed Parameter: Gain_Gain_b - // Referenced by: '/Gain' - - real32_T Gain1_Gain_m; // Computed Parameter: Gain1_Gain_m - // Referenced by: '/Gain1' - - real32_T Gain_Gain_la; // Computed Parameter: Gain_Gain_la - // Referenced by: '/Gain' - - real32_T Gain1_Gain_hb; // Computed Parameter: Gain1_Gain_hb - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_gu; // Computed Parameter: Gain2_Gain_gu - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_lk; // Computed Parameter: Gain2_Gain_lk - // Referenced by: '/Gain2' - - real32_T Gain_Gain_h; // Computed Parameter: Gain_Gain_h - // Referenced by: '/Gain' - - real32_T Gain1_Gain_al; // Computed Parameter: Gain1_Gain_al - // Referenced by: '/Gain1' - - real32_T Gain_Gain_kk; // Computed Parameter: Gain_Gain_kk - // Referenced by: '/Gain' - - real32_T Gain1_Gain_aj; // Computed Parameter: Gain1_Gain_aj - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_m; // Computed Parameter: Gain2_Gain_m - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_h4; // Computed Parameter: Gain2_Gain_h4 - // Referenced by: '/Gain2' - - real32_T Gain_Gain_c; // Computed Parameter: Gain_Gain_c - // Referenced by: '/Gain' - - real32_T Gain1_Gain_de; // Computed Parameter: Gain1_Gain_de - // Referenced by: '/Gain1' - - real32_T Gain_Gain_g5; // Computed Parameter: Gain_Gain_g5 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_k; // Computed Parameter: Gain1_Gain_k - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_ku; // Computed Parameter: Gain2_Gain_ku - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_ax; // Computed Parameter: Gain2_Gain_ax - // Referenced by: '/Gain2' - - real32_T DiscreteTimeIntegrator1_gainv_a;// Computed Parameter: DiscreteTimeIntegrator1_gainv_a - // Referenced by: '/Discrete-Time Integrator1' - - real32_T DiscreteTimeIntegrator1_IC_b;// Computed Parameter: DiscreteTimeIntegrator1_IC_b - // Referenced by: '/Discrete-Time Integrator1' - - real32_T Constant10_Value_g[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant10' - - real32_T Gain_Gain_hs; // Computed Parameter: Gain_Gain_hs - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ep; // Computed Parameter: Gain1_Gain_ep - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_c; // Computed Parameter: Gain2_Gain_c - // Referenced by: '/Gain2' - - real32_T Constant7_Value_j[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant7' - - real32_T Constant_Value_g; // Expression: single(25) - // Referenced by: '/Constant' - - real32_T Constant1_Value_fq; // Expression: single(14) - // Referenced by: '/Constant1' - - real32_T DiscreteTimeIntegrator_gainv_j1;// Computed Parameter: DiscreteTimeIntegrator_gainv_j1 - // Referenced by: '/Discrete-Time Integrator' - - real32_T Constant2_Value_cw; // Expression: single(14) - // Referenced by: '/Constant2' - - real32_T DiscreteTimeIntegrator_gainva_o;// Computed Parameter: DiscreteTimeIntegrator_gainva_o - // Referenced by: '/Discrete-Time Integrator' - - real32_T Gain1_Gain_k3; // Computed Parameter: Gain1_Gain_k3 - // Referenced by: '/Gain1' - - real32_T Gain_Gain_i; // Computed Parameter: Gain_Gain_i - // Referenced by: '/Gain' - - real32_T Gain1_Gain_me; // Computed Parameter: Gain1_Gain_me - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_b; // Computed Parameter: Gain2_Gain_b - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_hf; // Computed Parameter: Gain1_Gain_hf - // Referenced by: '/Gain1' - - real32_T Gain_Gain_ib; // Computed Parameter: Gain_Gain_ib - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ak; // Computed Parameter: Gain1_Gain_ak - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_al; // Computed Parameter: Gain2_Gain_al - // Referenced by: '/Gain2' - - real32_T RateTransition6_X0; // Computed Parameter: RateTransition6_X0 - // Referenced by: '/Rate Transition6' - - real32_T RateTransition8_X0; // Computed Parameter: RateTransition8_X0 - // Referenced by: '/Rate Transition8' - - real32_T RateTransition5_X0_c; // Computed Parameter: RateTransition5_X0_c - // Referenced by: '/Rate Transition5' - - real32_T RateTransition6_X0_j; // Computed Parameter: RateTransition6_X0_j - // Referenced by: '/Rate Transition6' - - real32_T cvs_handrail_local_pos_Value[3];// Expression: single([0 0 0]) - // Referenced by: '/cvs_handrail_local_pos' - - real32_T cvs_handrail_local_quat_Value[4];// Expression: single([0 0 0 1]) - // Referenced by: '/cvs_handrail_local_quat' - - real32_T RateTransition5_X0_o; // Computed Parameter: RateTransition5_X0_o - // Referenced by: '/Rate Transition5' - - real32_T RateTransition6_X0_g; // Computed Parameter: RateTransition6_X0_g - // Referenced by: '/Rate Transition6' - - real32_T Gain_Gain_o; // Computed Parameter: Gain_Gain_o - // Referenced by: '/Gain' - - real32_T Gain1_Gain_mr; // Computed Parameter: Gain1_Gain_mr - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_og; // Computed Parameter: Gain2_Gain_og - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_cz; // Computed Parameter: Gain1_Gain_cz - // Referenced by: '/Gain1' - - real32_T Constant7_Value_n[4]; // Expression: single([0 0 0 1]) - // Referenced by: '/Constant7' - - real32_T Gain_Gain_oe; // Computed Parameter: Gain_Gain_oe - // Referenced by: '/Gain' - - real32_T Gain1_Gain_mm; // Computed Parameter: Gain1_Gain_mm - // Referenced by: '/Gain1' - - real32_T Gain_Gain_ee4; // Computed Parameter: Gain_Gain_ee4 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_g; // Computed Parameter: Gain1_Gain_g - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_l0; // Computed Parameter: Gain2_Gain_l0 - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_fv; // Computed Parameter: Gain2_Gain_fv - // Referenced by: '/Gain2' - - real32_T Gain_Gain_a; // Computed Parameter: Gain_Gain_a - // Referenced by: '/Gain' - - real32_T Gain1_Gain_al2; // Computed Parameter: Gain1_Gain_al2 - // Referenced by: '/Gain1' - - real32_T Gain_Gain_od; // Computed Parameter: Gain_Gain_od - // Referenced by: '/Gain' - - real32_T Gain1_Gain_f; // Computed Parameter: Gain1_Gain_f - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_hy; // Computed Parameter: Gain2_Gain_hy - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_lw; // Computed Parameter: Gain2_Gain_lw - // Referenced by: '/Gain2' - - real32_T Gain_Gain_op; // Computed Parameter: Gain_Gain_op - // Referenced by: '/Gain' - - real32_T Gain1_Gain_da; // Computed Parameter: Gain1_Gain_da - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_fs; // Computed Parameter: Gain2_Gain_fs - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_jq; // Computed Parameter: Gain1_Gain_jq - // Referenced by: '/Gain1' - - real32_T Constant7_Value_h[4]; // Expression: single([0 0 0 1]) - // Referenced by: '/Constant7' - - real32_T Gain_Gain_pb; // Computed Parameter: Gain_Gain_pb - // Referenced by: '/Gain' - - real32_T Gain1_Gain_i; // Computed Parameter: Gain1_Gain_i - // Referenced by: '/Gain1' - - real32_T Gain_Gain_hw; // Computed Parameter: Gain_Gain_hw - // Referenced by: '/Gain' - - real32_T Gain1_Gain_dx; // Computed Parameter: Gain1_Gain_dx - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_j; // Computed Parameter: Gain2_Gain_j - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_dy; // Computed Parameter: Gain2_Gain_dy - // Referenced by: '/Gain2' - - real32_T Gain_Gain_cl; // Computed Parameter: Gain_Gain_cl - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ap; // Computed Parameter: Gain1_Gain_ap - // Referenced by: '/Gain1' - - real32_T Gain_Gain_mu; // Computed Parameter: Gain_Gain_mu - // Referenced by: '/Gain' - - real32_T Gain1_Gain_d3; // Computed Parameter: Gain1_Gain_d3 - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_e; // Computed Parameter: Gain2_Gain_e - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_hf; // Computed Parameter: Gain2_Gain_hf - // Referenced by: '/Gain2' - - real32_T Gain_Gain_kc; // Computed Parameter: Gain_Gain_kc - // Referenced by: '/Gain' - - real32_T Gain1_Gain_pi; // Computed Parameter: Gain1_Gain_pi - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_pj; // Computed Parameter: Gain2_Gain_pj - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_hr; // Computed Parameter: Gain1_Gain_hr - // Referenced by: '/Gain1' - - real32_T Constant7_Value_b[4]; // Expression: single([0 0 0 1]) - // Referenced by: '/Constant7' - - real32_T Gain_Gain_cy; // Computed Parameter: Gain_Gain_cy - // Referenced by: '/Gain' - - real32_T Gain1_Gain_l0; // Computed Parameter: Gain1_Gain_l0 - // Referenced by: '/Gain1' - - real32_T Gain_Gain_bv; // Computed Parameter: Gain_Gain_bv - // Referenced by: '/Gain' - - real32_T Gain1_Gain_m5; // Computed Parameter: Gain1_Gain_m5 - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_l1; // Computed Parameter: Gain2_Gain_l1 - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_hs; // Computed Parameter: Gain2_Gain_hs - // Referenced by: '/Gain2' - - real32_T Gain_Gain_bvs; // Computed Parameter: Gain_Gain_bvs - // Referenced by: '/Gain' - - real32_T Gain1_Gain_hd; // Computed Parameter: Gain1_Gain_hd - // Referenced by: '/Gain1' - - real32_T Gain_Gain_jw; // Computed Parameter: Gain_Gain_jw - // Referenced by: '/Gain' - - real32_T Gain1_Gain_om; // Computed Parameter: Gain1_Gain_om - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_pf; // Computed Parameter: Gain2_Gain_pf - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_n; // Computed Parameter: Gain2_Gain_n - // Referenced by: '/Gain2' - - real32_T Gain_Gain_mw; // Computed Parameter: Gain_Gain_mw - // Referenced by: '/Gain' - - real32_T Gain1_Gain_b; // Computed Parameter: Gain1_Gain_b - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_ch; // Computed Parameter: Gain2_Gain_ch - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_ll; // Computed Parameter: Gain1_Gain_ll - // Referenced by: '/Gain1' - - real32_T Constant7_Value_m[4]; // Expression: single([0 0 0 1]) - // Referenced by: '/Constant7' - - real32_T Gain_Gain_ci; // Computed Parameter: Gain_Gain_ci - // Referenced by: '/Gain' - - real32_T Gain1_Gain_fm; // Computed Parameter: Gain1_Gain_fm - // Referenced by: '/Gain1' - - real32_T Gain_Gain_d5; // Computed Parameter: Gain_Gain_d5 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_hfz; // Computed Parameter: Gain1_Gain_hfz - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_ee; // Computed Parameter: Gain2_Gain_ee - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_he; // Computed Parameter: Gain2_Gain_he - // Referenced by: '/Gain2' - - real32_T Gain_Gain_fp; // Computed Parameter: Gain_Gain_fp - // Referenced by: '/Gain' - - real32_T Gain1_Gain_in; // Computed Parameter: Gain1_Gain_in - // Referenced by: '/Gain1' - - real32_T Gain_Gain_lw; // Computed Parameter: Gain_Gain_lw - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jqn; // Computed Parameter: Gain1_Gain_jqn - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_jj; // Computed Parameter: Gain2_Gain_jj - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_dk; // Computed Parameter: Gain2_Gain_dk - // Referenced by: '/Gain2' - - real32_T Switch_Threshold; // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - real32_T Switch_Threshold_o; // Computed Parameter: Switch_Threshold_o - // Referenced by: '/Switch' - - uint32_T RateTransition3_X0; // Computed Parameter: RateTransition3_X0 - // Referenced by: '/Rate Transition3' - - uint32_T RateTransition1_X0; // Computed Parameter: RateTransition1_X0 - // Referenced by: '/Rate Transition1' - - uint32_T Constant2_Value_p1[2]; // Expression: uint32([2^31 2^31]) - // Referenced by: '/Constant2' - - uint32_T Constant2_Value_jp; // Expression: uint32(1) - // Referenced by: '/Constant2' - - uint32_T Constant1_Value_hc; // Expression: uint32(1E9) - // Referenced by: '/Constant1' - - uint32_T Delay_DelayLength; // Computed Parameter: Delay_DelayLength - // Referenced by: '/Delay' - - uint32_T Delay1_DelayLength; // Computed Parameter: Delay1_DelayLength - // Referenced by: '/Delay1' - - uint32_T RateTransition3_X0_o; // Computed Parameter: RateTransition3_X0_o - // Referenced by: '/Rate Transition3' - - uint32_T RateTransition1_X0_e; // Computed Parameter: RateTransition1_X0_e - // Referenced by: '/Rate Transition1' - - uint32_T RateTransition3_X0_m; // Computed Parameter: RateTransition3_X0_m - // Referenced by: '/Rate Transition3' - - uint32_T RateTransition1_X0_eo; // Computed Parameter: RateTransition1_X0_eo - // Referenced by: '/Rate Transition1' - - uint32_T RateTransition3_X0_c; // Computed Parameter: RateTransition3_X0_c - // Referenced by: '/Rate Transition3' - - uint32_T RateTransition1_X0_a; // Computed Parameter: RateTransition1_X0_a - // Referenced by: '/Rate Transition1' - - uint32_T Constant_Value_n; // Computed Parameter: Constant_Value_n - // Referenced by: '/Constant' - - uint32_T Constant_Value_j; // Computed Parameter: Constant_Value_j - // Referenced by: '/Constant' - - uint32_T Constant_Value_m; // Computed Parameter: Constant_Value_m - // Referenced by: '/Constant' - - uint32_T Constant_Value_nc; // Computed Parameter: Constant_Value_nc - // Referenced by: '/Constant' - - uint32_T Constant_Value_e; // Computed Parameter: Constant_Value_e - // Referenced by: '/Constant' - - uint32_T Constant_Value_c0; // Computed Parameter: Constant_Value_c0 - // Referenced by: '/Constant' - - uint32_T Constant_Value_eq; // Computed Parameter: Constant_Value_eq - // Referenced by: '/Constant' - - uint32_T Constant_Value_mi; // Computed Parameter: Constant_Value_mi - // Referenced by: '/Constant' - - uint8_T RateTransition7_X0; // Computed Parameter: RateTransition7_X0 - // Referenced by: '/Rate Transition7' - - uint8_T UnitDelay_InitialCondition; // Computed Parameter: UnitDelay_InitialCondition - // Referenced by: '/Unit Delay' - - uint8_T Bias_Bias; // Computed Parameter: Bias_Bias - // Referenced by: '/Bias' - - uint8_T UnitDelay_InitialCondition_c;// Computed Parameter: UnitDelay_InitialCondition_c - // Referenced by: '/Unit Delay' - - uint8_T UnitDelay_InitialCondition_n;// Computed Parameter: UnitDelay_InitialCondition_n - // Referenced by: '/Unit Delay' - - uint8_T UnitDelay_InitialCondition_l;// Computed Parameter: UnitDelay_InitialCondition_l - // Referenced by: '/Unit Delay' - - uint8_T Switch_Threshold_p; // Computed Parameter: Switch_Threshold_p - // Referenced by: '/Switch' - - uint8_T Delay2_DelayLength; // Computed Parameter: Delay2_DelayLength - // Referenced by: '/Delay2' - - uint8_T Delay2_DelayLength_h; // Computed Parameter: Delay2_DelayLength_h - // Referenced by: '/Delay2' - - uint8_T Delay1_DelayLength_b; // Computed Parameter: Delay1_DelayLength_b - // Referenced by: '/Delay1' - - uint8_T Delay1_DelayLength_n; // Computed Parameter: Delay1_DelayLength_n - // Referenced by: '/Delay1' - - uint8_T Constant1_Value_ol; // Expression: uint8(1) - // Referenced by: '/Constant1' - - uint8_T Constant2_Value_cc; // Expression: uint8(0) - // Referenced by: '/Constant2' - - uint8_T RateTransition10_X0; // Computed Parameter: RateTransition10_X0 - // Referenced by: '/Rate Transition10' - - uint8_T RateTransition7_X0_m; // Computed Parameter: RateTransition7_X0_m - // Referenced by: '/Rate Transition7' - - uint8_T RateTransition7_X0_k; // Computed Parameter: RateTransition7_X0_k - // Referenced by: '/Rate Transition7' - - uint8_T cvs_3d_knowledge_flag_Value; // Expression: uint8(0) - // Referenced by: '/cvs_3d_knowledge_flag' - - uint8_T cvs_handrail_update_global_pose;// Expression: uint8(0) - // Referenced by: '/cvs_handrail_update_global_pose_flag' - - uint8_T RateTransition7_X0_n; // Computed Parameter: RateTransition7_X0_n - // Referenced by: '/Rate Transition7' - - uint8_T Saturation_UpperSat; // Expression: mlp_num_commands - // Referenced by: '/Saturation' - - uint8_T Saturation_LowerSat; // Computed Parameter: Saturation_LowerSat - // Referenced by: '/Saturation' - - uint8_T Saturation_UpperSat_b; // Expression: mlp_num_mode_cmds - // Referenced by: '/Saturation' - - uint8_T Saturation_LowerSat_e; // Computed Parameter: Saturation_LowerSat_e - // Referenced by: '/Saturation' - - uint8_T Saturation_UpperSat_d; // Expression: mlp_num_mode_cmds - // Referenced by: '/Saturation' - - uint8_T Saturation_LowerSat_j; // Computed Parameter: Saturation_LowerSat_j - // Referenced by: '/Saturation' - - uint8_T Saturation_UpperSat_f; // Expression: mlp_num_mode_cmds - // Referenced by: '/Saturation' - - uint8_T Saturation_LowerSat_k; // Computed Parameter: Saturation_LowerSat_k - // Referenced by: '/Saturation' - - uint8_T UnitDelay_InitialCondition_d;// Computed Parameter: UnitDelay_InitialCondition_d - // Referenced by: '/Unit Delay' - - uint8_T Saturation_UpperSat_i; // Computed Parameter: Saturation_UpperSat_i - // Referenced by: '/Saturation' - - uint8_T Saturation_LowerSat_b; // Computed Parameter: Saturation_LowerSat_b - // Referenced by: '/Saturation' - - uint8_T UnitDelay_InitialCondition_m;// Computed Parameter: UnitDelay_InitialCondition_m - // Referenced by: '/Unit Delay' - - uint8_T Saturation_UpperSat_n; // Computed Parameter: Saturation_UpperSat_n - // Referenced by: '/Saturation' - - uint8_T Saturation_LowerSat_f; // Computed Parameter: Saturation_LowerSat_f - // Referenced by: '/Saturation' - - uint8_T UnitDelay_InitialCondition_g;// Computed Parameter: UnitDelay_InitialCondition_g - // Referenced by: '/Unit Delay' - - uint8_T Saturation_UpperSat_k; // Computed Parameter: Saturation_UpperSat_k - // Referenced by: '/Saturation' - - uint8_T Saturation_LowerSat_a; // Computed Parameter: Saturation_LowerSat_a - // Referenced by: '/Saturation' - - uint8_T UnitDelay_InitialCondition_a;// Computed Parameter: UnitDelay_InitialCondition_a - // Referenced by: '/Unit Delay' - - uint8_T Saturation_UpperSat_p; // Computed Parameter: Saturation_UpperSat_p - // Referenced by: '/Saturation' - - uint8_T Saturation_LowerSat_h; // Computed Parameter: Saturation_LowerSat_h - // Referenced by: '/Saturation' - - boolean_T UnitDelay_InitialCondition_o;// Computed Parameter: UnitDelay_InitialCondition_o - // Referenced by: '/Unit Delay' - - boolean_T UnitDelay_InitialCondition_gb;// Computed Parameter: UnitDelay_InitialCondition_gb - // Referenced by: '/Unit Delay' - - P_servo_model_sim_model_lib0_T servo_model_f;// '/servo_model' - P_speed_controller_sim_model__T speed_controller_c;// '/speed_controller' - P_latch_nozzle_thrust_matrici_T latch_nozzle_thrust_matricies_p;// '/latch_nozzle_thrust_matricies' - P_blower_aerodynamics_sim_m_m_T blower_aerodynamics_j;// '/blower_aerodynamics' - P_servo_model_sim_model_lib0_T servo_model;// '/servo_model' - P_speed_controller_sim_model__T speed_controller;// '/speed_controller' - P_latch_nozzle_thrust_matrici_T latch_nozzle_thrust_matricies;// '/latch_nozzle_thrust_matricies' - P_blower_aerodynamics_sim_mod_T blower_aerodynamics;// '/blower_aerodynamics' - P_pinhole_projection_model_si_T pinhole_projection_model_o;// '/pinhole_projection_model' - P_pinhole_projection_model_si_T pinhole_projection_model_id;// '/pinhole_projection_model' - P_pinhole_projection_model_si_T pinhole_projection_model_i;// '/pinhole_projection_model' - P_pinhole_projection_model_si_T pinhole_projection_model;// '/pinhole_projection_model' -}; - -// Real-time Model Data Structure -struct tag_RTM_sim_model_lib0_T { - const char_T * volatile errorStatus; - B_sim_model_lib0_T *blockIO; - P_sim_model_lib0_T *defaultParam; - boolean_T paramIsMalloced; - DW_sim_model_lib0_T *dwork; - - // - // Timing: - // The following substructure contains information regarding - // the timing information for the model. - - struct { - struct { - uint8_T TID0_1; - uint8_T TID0_2; - uint8_T TID0_3; - uint8_T TID0_4; - } RateInteraction; - } Timing; -}; - -#ifdef __cplusplus - -extern "C" { - -#endif - -#ifdef __cplusplus - -} -#endif - -// External data declarations for dependent source files -extern const act_msg sim_model_lib0_rtZact_msg;// act_msg ground -extern const cmc_msg sim_model_lib0_rtZcmc_msg;// cmc_msg ground -extern const env_msg sim_model_lib0_rtZenv_msg;// env_msg ground -extern const ex_time_msg sim_model_lib0_rtZex_time_msg;// ex_time_msg ground -extern const cvs_landmark_msg sim_model_lib0_rtZcvs_landmark_msg;// cvs_landmark_msg ground -extern const cvs_optical_flow_msg sim_model_lib0_rtZcvs_optical_flow_msg;// cvs_optical_flow_msg ground -extern const cvs_handrail_msg sim_model_lib0_rtZcvs_handrail_msg;// cvs_handrail_msg ground -extern const cvs_registration_pulse sim_model_lib0_rtZcvs_registration_pulse;// cvs_registration_pulse ground -extern const bpm_msg sim_model_lib0_rtZbpm_msg;// bpm_msg ground -extern const imu_msg sim_model_lib0_rtZimu_msg;// imu_msg ground - -#ifdef __cplusplus - -extern "C" { - -#endif - - extern const char *RT_MEMORY_ALLOCATION_ERROR; - -#ifdef __cplusplus - -} -#endif - -extern P_sim_model_lib0_T sim_model_lib0_P;// parameters - -#ifdef __cplusplus - -extern "C" { - -#endif - - // Model entry point functions - extern RT_MODEL_sim_model_lib0_T *sim_model_lib0(act_msg - *sim_model_lib0_U_act_msg_l, cmc_msg *sim_model_lib0_U_cmc_msg_in, - cvs_optical_flow_msg *sim_model_lib0_Y_cvs_optical_flow_msg_n, - cvs_handrail_msg *sim_model_lib0_Y_cvs_handrail_msg_h, cmc_msg - *sim_model_lib0_Y_cmc_msg_c, imu_msg *sim_model_lib0_Y_imu_msg_o, env_msg - *sim_model_lib0_Y_env_msg_i, bpm_msg *sim_model_lib0_Y_bpm_msg_h, - cvs_registration_pulse *sim_model_lib0_Y_cvs_registration_pulse_d, - cvs_landmark_msg *sim_model_lib0_Y_cvs_landmark_msg_n, cvs_landmark_msg - *sim_model_lib0_Y_cvs_ar_tag_msg, ex_time_msg - *sim_model_lib0_Y_ex_time_msg_m); - extern void sim_model_lib0_initialize(RT_MODEL_sim_model_lib0_T *const - sim_model_lib0_M, act_msg *sim_model_lib0_U_act_msg_l, cmc_msg - *sim_model_lib0_U_cmc_msg_in, cvs_optical_flow_msg - *sim_model_lib0_Y_cvs_optical_flow_msg_n, cvs_handrail_msg - *sim_model_lib0_Y_cvs_handrail_msg_h, cmc_msg *sim_model_lib0_Y_cmc_msg_c, - imu_msg *sim_model_lib0_Y_imu_msg_o, env_msg *sim_model_lib0_Y_env_msg_i, - bpm_msg *sim_model_lib0_Y_bpm_msg_h, cvs_registration_pulse - *sim_model_lib0_Y_cvs_registration_pulse_d, cvs_landmark_msg - *sim_model_lib0_Y_cvs_landmark_msg_n, cvs_landmark_msg - *sim_model_lib0_Y_cvs_ar_tag_msg, ex_time_msg - *sim_model_lib0_Y_ex_time_msg_m); - extern void sim_model_lib0_step0(RT_MODEL_sim_model_lib0_T *const - sim_model_lib0_M, act_msg *sim_model_lib0_U_act_msg_l, cmc_msg - *sim_model_lib0_U_cmc_msg_in, cvs_optical_flow_msg - *sim_model_lib0_Y_cvs_optical_flow_msg_n, cvs_handrail_msg - *sim_model_lib0_Y_cvs_handrail_msg_h, cmc_msg *sim_model_lib0_Y_cmc_msg_c, - imu_msg *sim_model_lib0_Y_imu_msg_o, env_msg *sim_model_lib0_Y_env_msg_i, - bpm_msg *sim_model_lib0_Y_bpm_msg_h, cvs_registration_pulse - *sim_model_lib0_Y_cvs_registration_pulse_d, cvs_landmark_msg - *sim_model_lib0_Y_cvs_landmark_msg_n, cvs_landmark_msg - *sim_model_lib0_Y_cvs_ar_tag_msg, ex_time_msg - *sim_model_lib0_Y_ex_time_msg_m); - extern void sim_model_lib0_step1(RT_MODEL_sim_model_lib0_T *const - sim_model_lib0_M); - extern void sim_model_lib0_step2(RT_MODEL_sim_model_lib0_T *const - sim_model_lib0_M); - extern void sim_model_lib0_step3(RT_MODEL_sim_model_lib0_T *const - sim_model_lib0_M); - extern void sim_model_lib0_step4(RT_MODEL_sim_model_lib0_T *const - sim_model_lib0_M); - extern void sim_model_lib0_terminate(RT_MODEL_sim_model_lib0_T - * sim_model_lib0_M); - -#ifdef __cplusplus - -} -#endif - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Note that this particular code originates from a subsystem build, -// and has its own system numbers different from the parent model. -// Refer to the system hierarchy for this subsystem below, and use the -// MATLAB hilite_system command to trace the generated code back -// to the parent model. For example, -// -// hilite_system('astrobee/sim_model_lib') - opens subsystem astrobee/sim_model_lib -// hilite_system('astrobee/sim_model_lib/Kp') - opens and selects block Kp -// -// Here is the system hierarchy for this model -// -// '' : 'astrobee' -// '' : 'astrobee/sim_model_lib' -// '' : 'astrobee/sim_model_lib/env_environment_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics' -// '' : 'astrobee/sim_model_lib/env_environment_model/disturbance_model' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics' -// '' : 'astrobee/sim_model_lib/env_environment_model/tsm_time_simulation_module' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/cross_product1' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/rotate_vec_A2B' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/cross_product1/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/cross_product1/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/Compare To Constant' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/Compare To Constant1' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/Stopped' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/normal_speed' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/slow_speed_approx' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/vector_magnitude' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/vector_normalize' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/normal_speed/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/normal_speed/create_omega_matrix' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/slow_speed_approx/create_omega_matrix' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/vector_normalize/No-op' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/vector_normalize/Normalize' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/quaternion_propogator/vector_normalize/vector_magnitude' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/env_environment_model/attitude_dynamics/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/disturbance_model/saturate_vector_mag' -// '' : 'astrobee/sim_model_lib/env_environment_model/disturbance_model/saturate_vector_mag/vector_magnitude1' -// '' : 'astrobee/sim_model_lib/env_environment_model/disturbance_model/saturate_vector_mag/vector_normalize' -// '' : 'astrobee/sim_model_lib/env_environment_model/disturbance_model/saturate_vector_mag/vector_normalize/No-op' -// '' : 'astrobee/sim_model_lib/env_environment_model/disturbance_model/saturate_vector_mag/vector_normalize/Normalize' -// '' : 'astrobee/sim_model_lib/env_environment_model/disturbance_model/saturate_vector_mag/vector_normalize/vector_magnitude' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/Cross Product' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/Cross Product2' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/Cross Product3' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B1' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_B2A' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B1/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B1/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B1/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B1/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_A2B1/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/env_environment_model/translational_dynamics/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/env_environment_model/tsm_time_simulation_module/Compare To Constant' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/determine_cmd_idx' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/determine_cmd_idx1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/determine_cmd_idx2' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/determine_cmd_idx3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/select_cmd' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/determine_cmd_idx/Compare To Zero3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/determine_cmd_idx1/Compare To Zero3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/determine_cmd_idx2/Compare To Zero3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cmc_command_and_mode_control/determine_cmd_idx3/Compare To Zero3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/generate_reg_pulse' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/apply_valid_flag_mask' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/generate_output' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/apply_valid_flag_mask/Compare To Zero3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/pinhole_projection_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/pinhole_projection_model/determine_points_in_FOV' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/pinhole_projection_model/pinhole_camera_projection' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/ARtag_image_processing/camera_model/pinhole_projection_model/determine_points_in_FOV/Compare To Zero' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/generate_reg_pulse/Compare To Constant' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/generate_reg_pulse/Compare To Constant1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/generate_reg_pulse/Detect Change' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/generate_reg_pulse/Detect Change1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/apply_valid_flag_mask' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/generate_output' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/apply_valid_flag_mask/Compare To Zero3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/pinhole_projection_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/pinhole_projection_model/determine_points_in_FOV' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/pinhole_projection_model/pinhole_camera_projection' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/handrail_image_processing/camera_model/pinhole_projection_model/determine_points_in_FOV/Compare To Zero' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/apply_valid_flag_mask' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/generate_output' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/apply_valid_flag_mask/Compare To Zero3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/pinhole_projection_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/pinhole_projection_model/determine_points_in_FOV' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/pinhole_projection_model/pinhole_camera_projection' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/landmark_image_processing/camera_model/pinhole_projection_model/determine_points_in_FOV/Compare To Zero' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/apply_valid_flag_mask' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/generate_output' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/apply_valid_flag_mask/Compare To Zero3' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/pinhole_projection_model' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/convert_points_to_cam_frame/quaternion_to_DCM1/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/pinhole_projection_model/determine_points_in_FOV' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/pinhole_projection_model/pinhole_camera_projection' -// '' : 'astrobee/sim_model_lib/mlp_mid_level_processor_model/cvs_computer_vision_system/optical_flow_image_processing/camera_model/pinhole_projection_model/determine_points_in_FOV/Compare To Zero' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_aerodynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/calc_nozzle_area' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/motor_rotational_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/speed_sensor' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/Detect Change' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/cross_product' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/vector_normalize' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/cross_product/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/cross_product/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/MATLAB Function' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/vector_normalize/No-op' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/vector_normalize/Normalize' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/blower_body_dynamics/vector_normalize/vector_magnitude' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/dc_motor_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/speed_controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/speed_controller/Discrete PID Controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/speed_controller/Saturation Dynamic' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/impeller_model/speed_controller/Discrete PID Controller/Clamping circuit' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model/dc_motor_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model/servo_controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model/servo_controller/Discrete PID Controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module/servo_model/servo_controller/Discrete PID Controller/Clamping circuit' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_aerodynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/calc_nozzle_area' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/motor_rotational_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/speed_sensor' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/Detect Change' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/cross_product' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/vector_normalize' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/cross_product/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/cross_product/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/MATLAB Function' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/latch_nozzle_thrust_matricies/For Each Subsystem/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/vector_normalize/No-op' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/vector_normalize/Normalize' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/blower_body_dynamics/vector_normalize/vector_magnitude' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/dc_motor_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/speed_controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/speed_controller/Discrete PID Controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/speed_controller/Saturation Dynamic' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/impeller_model/speed_controller/Discrete PID Controller/Clamping circuit' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model/dc_motor_dynamics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model/servo_controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model/servo_controller/Discrete PID Controller' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_2_propulsion_module/servo_model/servo_controller/Discrete PID Controller/Clamping circuit' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/accelerometer_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/gyroscope_model' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_hardware' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Cross Product' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Cross Product1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Cross Product2' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B2' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication1/Quaternion Xi' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication1/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication1/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/Quaternion_Multiplication1/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B1/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B1/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B1/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B1/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B1/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B2/quaternion_to_DCM' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B2/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B2/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B2/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/sim_model_lib/veh_vehicle_model/epson_imu_model/imu_model_core/imu_kinematics/rotate_vec_A2B2/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' - -#endif // RTW_HEADER_sim_model_lib0_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_data.cpp b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_data.cpp deleted file mode 100644 index 48b941a2ea..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_data.cpp +++ /dev/null @@ -1,15820 +0,0 @@ -// -// File: sim_model_lib0_data.cpp -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:45:47 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "sim_model_lib0.h" -#include "sim_model_lib0_private.h" - -// Block parameters (auto storage) -P_sim_model_lib0_T sim_model_lib0_P = { - { 0.0, 0.0, 0.0 }, - 0.016, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 254.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 255.0, - 0.0, - 0.38490017945975047, - 0.16, - 0.0001, - 0.20800000000000002, - 0.336, - 123.0, - - { 0.01, 0.01, 0.01 }, - - { 1.0, 2.0, 3.0 }, - - { 0.01, 0.01, 0.01 }, - 0.001, - - { 0.01, 0.01, 0.01 }, - - { 0.0052365876279462167, 0.0052867112495294279, 0.0052365628192932723, - 0.99995860276514381 }, - - { 0.0058467547473392972, 0.0058982406607320766, 0.0058466827131893623, - 0.99994841992839 }, - 1.0, - 1.0, - -29.419949999999996, - 0.08, - - { 4.0, 5.0, 6.0 }, - 0.001, - 0.0012258312499999998, - 1.0, - - { 0.00019613299999999997, 0.00019613299999999997, 0.00019613299999999997 }, - 29.419949999999996, - - { 0.0017453292519943296, 0.0017453292519943296, 0.0017453292519943296 }, - 1.0, - 1.0, - 1.7797405352432585E-5, - -2.6179938779914944, - 0.04, - - { 1.0, 2.0, 3.0 }, - 0.001, - 8.7266462599716482E-5, - 1.0, - - { 1.7453292519943296E-5, 1.7453292519943296E-5, 1.7453292519943296E-5 }, - 2.6179938779914944, - 1.0, - - { 0.1524F, -0.1524F, 0.0718819946F, -0.0718819946F, -0.067564F, 0.067564F, - 0.101854004F, 0.101854004F, 0.1524F, 0.1524F, 0.101854004F, 0.101854004F, - -0.0396239981F, 0.0396239981F, 0.0718819946F, -0.0718819946F, 0.1524F, - -0.1524F }, - - { 0.914971054F, 0.755778253F, 0.940762937F, 0.792109787F, 0.9240188F, - 0.930319786F }, - - { 1.0F, -1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, -1.0F }, - - { 0.127F, 0.127F, 0.07112F, 0.07112F, 0.07112F, 0.07112F }, - - { -0.1524F, 0.1524F, -0.0718819946F, 0.0718819946F, 0.067564F, -0.067564F, - -0.101854004F, -0.101854004F, -0.1524F, -0.1524F, -0.101854004F, - -0.101854004F, -0.0396239981F, 0.0396239981F, 0.0718819946F, -0.0718819946F, - 0.1524F, -0.1524F }, - - { 0.947114F, 0.764468908F, 1.0F, 0.904809415F, 0.936555624F, 0.893794775F }, - - { -1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, -1.0F, -1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, -1.0F }, - - { 0.127F, 0.127F, 0.07112F, 0.07112F, 0.07112F, 0.07112F }, - 0.1397F, - 2.0F, - 0.0135966204F, - 0.5F, - 0.01309116F, - 0.273667634F, - - { 0.0F, 1.0F, 0.0F }, - 0.0044667F, - - { 0.0F, -1.0F, 0.0F }, - 0.0042273F, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, -0.0F, - 0.0F, -0.0F, -0.0F, -0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - 0.0F, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, -0.0F, - -0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - 0.0F, - 1.0F, - 0.05F, - 0.1F, - 0.2F, - 16.6F, - 8.00571161E-6F, - 1.2F, - 39.1651878F, - 0.0255F, - 1.0F, - 1.0F, - 0.001F, - 0.0F, - 0.0F, - - { 0.0825F, 0.082625635F, 0.08274699F, 0.0828641355F, 0.082977131F, - 0.083086051F, 0.0831909552F, 0.083291918F, 0.083389F, 0.083482258F, - 0.0835717544F, 0.0836575627F, 0.0837397277F, 0.083818309F, 0.0838933736F, - 0.0839649737F, 0.0840331689F, 0.0840980038F, 0.0841595381F, 0.0842178315F, - 0.0842729211F, 0.084324874F, 0.0843737274F, 0.0844195336F, 0.0844623446F, - 0.0845022127F, 0.0845391676F, 0.0845732689F, 0.0846045539F, 0.0846330673F, - 0.0846588612F, 0.0846819654F, 0.0847024247F, 0.0847202763F, 0.0847355723F, - 0.0847483352F, 0.0847586095F, 0.0847664326F, 0.0847718418F, 0.0847748667F, - 0.0847755447F, 0.084773913F, 0.08477F, 0.0847638398F, 0.0847554579F, - 0.084744893F, 0.0847321674F, 0.0847173184F, 0.0847003683F, 0.0846813396F, - 0.0846602693F, 0.0846371725F, 0.0846120864F, 0.0845850259F, 0.0845560208F, - 0.0845250934F, 0.0844922587F, 0.0844575465F, 0.0844209716F, 0.0843825564F, - 0.0843423232F, 0.0843002871F, 0.0842564702F, 0.084210895F, 0.0841635615F, - 0.0841145F, 0.0840637162F, 0.0840112418F, 0.0839570761F, 0.083901234F, - 0.0838437378F, 0.0837845877F, 0.0837238058F, 0.0836614F, 0.0835973844F, - 0.0835317597F, 0.083464548F, 0.0833957493F, 0.0833253786F, 0.0832534432F, - 0.0831799433F, 0.0831048936F, 0.0830283F, 0.0829501674F, 0.0828705F, - 0.0827893F, 0.0827065781F, 0.0826223418F, 0.0825365856F, 0.0824493095F, - 0.0823605284F, 0.0822702423F, 0.0821784437F, 0.08208514F, 0.0819903314F, - 0.0818940252F, 0.0817962065F, 0.0816968903F, 0.081596069F, 0.0814937353F, - 0.081389904F, 0.0812845528F, 0.0811777F, 0.0810693204F, 0.0809594318F, - 0.0808480158F, 0.0807350799F, 0.0806206167F, 0.0805046186F, 0.0803870782F, - 0.080268F, 0.0801473707F, 0.0800251886F, 0.0799014494F, 0.0797761381F, - 0.0796492621F, 0.0795208F, 0.0793907568F, 0.0792591125F, 0.0791258737F, - 0.0789910182F, 0.0788545534F, 0.0787164569F, 0.0785767287F, 0.0784353614F, - 0.0782923326F, 0.0781476498F, 0.0780012906F, 0.077853255F, 0.0777035281F, - 0.0775521F, 0.0773989633F, 0.0772441104F, 0.0770875216F, 0.0769291893F, - 0.0767691135F, 0.0766072646F, 0.0764436498F, 0.0762782469F, 0.076111041F, - 0.0759420395F, 0.0757712126F, 0.0755985528F, 0.0754240528F, 0.0752477F, - 0.0750694722F, 0.0748893768F, 0.0747073889F, 0.0745235F, 0.0743377F, - 0.0741499662F, 0.0739603F, 0.0737686828F, 0.0735751F, 0.0733795539F, - 0.0731820166F, 0.0729824826F, 0.072780937F, 0.0725773647F, 0.072371766F, - 0.0721641108F, 0.0719544F, 0.071742624F, 0.0715287626F, 0.0713128075F, - 0.0710947439F, 0.0708745569F, 0.0706522539F, 0.0704278F, 0.0702012F, - 0.0699724406F, 0.0697415F, 0.0695083737F, 0.0692730546F, 0.0690355226F, - 0.0687957704F, 0.0685537905F, 0.0683095828F, 0.0680631176F, 0.0678143948F, - 0.0675634F, 0.0673101321F, 0.0670545697F, 0.0667967126F, 0.0665365383F, - 0.0662740618F, 0.0660092533F, 0.0657421201F, 0.0654726326F, 0.0652008057F, - 0.0649266243F, 0.0646500662F, 0.0643711537F, 0.0640898496F, 0.0638061538F, - 0.0635200813F, 0.0632316F, 0.0629407242F, 0.0626474321F, 0.0623517223F, - 0.0620536F, 0.0617530458F, 0.0614500828F, 0.0611446686F, 0.0608368181F, - 0.0605265312F, 0.0602138F, 0.0598986298F, 0.0595810153F, 0.0592609458F, - 0.0589384325F, 0.0586134642F, 0.0582860447F, 0.0579561703F, 0.0576238409F, - 0.0572890751F, 0.0569518507F, 0.0566121712F, 0.0562700555F, 0.0559254885F, - 0.0555784889F, 0.0552290566F, 0.054877162F, 0.0545228682F, 0.0541661382F, - 0.0538069829F, 0.0534454137F, 0.0530814491F, 0.0527150668F, 0.0523463078F, - 0.0519751534F, 0.0516016223F, 0.0512257218F, 0.0508474559F, 0.0504668579F, - 0.0500839F, 0.0496986173F, 0.0493110269F, 0.0489211343F, 0.0485289469F, - 0.0481344871F, 0.0477377549F, 0.04733878F, 0.0469375588F, 0.0465341397F, - 0.0461285152F, 0.0457207039F, 0.0453107134F, 0.0448985882F, 0.0444843248F, - 0.0440679714F, 0.0436495095F, 0.0432290025F, 0.0428064205F, 0.0423818417F, - 0.0419552475F, 0.0415266976F, 0.0410961919F, 0.0406637266F, 0.0402293913F, - 0.0397931822F, 0.0393551365F, 0.038915243F, 0.0384735875F, 0.0380301885F, - 0.0375850387F, 0.0371381938F, 0.0366896801F, 0.0362395607F, 0.0357878208F, - 0.0353345275F, 0.0348797F, 0.0344234072F, 0.0339656323F, 0.033506453F, - 0.033045914F, 0.032584019F, 0.0321208276F, 0.0316563807F, 0.0311907455F, - 0.0307239257F, 0.0302559771F, 0.0297869369F, 0.0293168686F, 0.0288458094F, - 0.0283738412F, 0.0279009566F, 0.0274272151F, 0.0269527063F, 0.0264774524F, - 0.0260014758F, 0.0255248956F, 0.0250477232F, 0.0245700441F, 0.0240918733F, - 0.0236133039F, 0.0231343843F, 0.0226551369F, 0.0221756734F, 0.0216960311F, - 0.0212163F, 0.0207364894F, 0.0202567242F, 0.0197770186F, 0.0192974396F, - 0.0188181363F, 0.0183390342F, 0.0178603418F, 0.0173820443F, 0.0169042759F, - 0.0164270438F, 0.0159504749F, 0.0154745989F, 0.01499952F, 0.0145253353F, - 0.0140520222F, 0.0135798045F, 0.0131086893F, 0.0126387365F, 0.0121700801F, - 0.0117027499F, 0.0112369023F, 0.0107725374F, 0.0103098117F, 0.0098488F, - 0.00938956812F, 0.00893222168F, 0.00847681239F, 0.00802352652F, - 0.00757240178F, 0.007123549F, 0.00667699846F, 0.00623294385F, 0.0057914448F - }, - - { 0.0F, 1.46138646E-5F, 2.92062887E-5F, 4.37784511E-5F, 5.83315159E-5F, - 7.28665837E-5F, 8.7384753E-5F, 0.000101887068F, 0.000116374569F, - 0.000130848246F, 0.000145309066F, 0.000159758F, 0.000174195899F, - 0.000188623759F, 0.000203042364F, 0.000217452631F, 0.000231855331F, - 0.000246251322F, 0.000260641362F, 0.000275026192F, 0.000289406598F, - 0.000303783338F, 0.00031815705F, 0.000332528463F, 0.000346898247F, - 0.000361267099F, 0.00037563566F, 0.000390004541F, 0.000404374383F, - 0.000418745767F, 0.000433119305F, 0.000447495579F, 0.000461875083F, - 0.000476258458F, 0.000490646285F, 0.000505038945F, 0.00051943725F, - 0.000533841376F, 0.000548252079F, 0.000562669651F, 0.00057709479F, - 0.000591527845F, 0.000605969399F, 0.000620419742F, 0.000634879572F, - 0.000649349065F, 0.000663828861F, 0.000678319368F, 0.000692821F, - 0.000707334315F, 0.000721859455F, 0.000736397109F, 0.000750947569F, - 0.0007655113F, 0.000780088594F, 0.000794680149F, 0.000809286F, - 0.000823906856F, 0.000838542939F, 0.000853194739F, 0.000867862604F, - 0.000882546941F, 0.000897248159F, 0.00091196649F, 0.000926702633F, - 0.000941456819F, 0.0009562294F, 0.00097102084F, 0.000985831604F, - 0.00100066199F, 0.00101551227F, 0.00103038305F, 0.0010452749F, - 0.00106018782F, 0.00107512227F, 0.00109007885F, 0.00110505789F, - 0.00112005987F, 0.00113508501F, 0.00115013402F, 0.00116520724F, - 0.0011803048F, 0.00119542738F, 0.00121057557F, 0.00122574961F, 0.00124095F, - 0.00125617709F, 0.00127143157F, 0.00128671364F, 0.001302024F, 0.00131736277F, - 0.00133273099F, 0.00134812866F, 0.00136355648F, 0.00137901504F, - 0.00139450456F, 0.00141002587F, 0.00142557954F, 0.0014411658F, - 0.00145678537F, 0.00147243857F, 0.00148812647F, 0.00150384917F, - 0.00151960761F, 0.00153540191F, 0.00155123312F, 0.0015671018F, - 0.00158300844F, 0.00159895339F, 0.00161493802F, 0.00163096236F, - 0.00164702744F, 0.0016631335F, 0.00167928194F, 0.00169547275F, - 0.00171170721F, 0.00172798557F, 0.00174430886F, 0.0017606779F, - 0.00177709304F, 0.00179355568F, 0.00181006629F, 0.00182662567F, - 0.00184323464F, 0.00185989426F, 0.00187660486F, 0.0018933682F, - 0.00191018439F, 0.00192705484F, 0.00194398011F, 0.00196096138F, - 0.00197799969F, 0.00199509552F, 0.00201225071F, 0.00202946551F, - 0.00204674155F, 0.00206407951F, 0.00208148058F, 0.00209894567F, - 0.00211647665F, 0.00213407329F, 0.00215173839F, 0.00216947193F, - 0.00218727579F, 0.00220515113F, 0.00222309888F, 0.00224112067F, - 0.0022592172F, 0.0022773908F, 0.00229564216F, 0.00231397292F, 0.00233238423F, - 0.00235087774F, 0.00236945506F, 0.00238811737F, 0.00240686629F, - 0.00242570369F, 0.00244463095F, 0.00246364973F, 0.00248276163F, - 0.00250196829F, 0.00252127182F, 0.00254067336F, 0.00256017526F, - 0.00257977913F, 0.00259948708F, 0.00261930074F, 0.00263922219F, - 0.00265925354F, 0.00267939596F, 0.00269965269F, 0.00272002514F, - 0.00274051586F, 0.00276112719F, 0.002781861F, 0.00280271959F, 0.00282370578F, - 0.00284482073F, 0.00286606839F, 0.00288745062F, 0.00290896976F, - 0.00293062883F, 0.00295243063F, 0.00297437701F, 0.00299647171F, - 0.00301871705F, 0.00304111606F, 0.00306367176F, 0.00308638718F, - 0.00310926535F, 0.0031323093F, 0.00315552298F, 0.00317890849F, - 0.00320247072F, 0.00322621223F, 0.00325013627F, 0.00327424728F, - 0.00329854828F, 0.00332304346F, 0.00334773702F, 0.0033726322F, - 0.00339773367F, 0.00342304516F, 0.00344857131F, 0.00347431679F, - 0.00350028556F, 0.00352648227F, 0.00355291134F, 0.00357957813F, - 0.00360648776F, 0.0036336449F, 0.00366105489F, 0.00368872308F, - 0.00371665484F, 0.00374485622F, 0.00377333187F, 0.00380208902F, - 0.00383113371F, 0.00386047084F, 0.00389010808F, 0.00392005173F, - 0.00395030761F, 0.00398088479F, 0.00401178841F, 0.0040430259F, - 0.00407460658F, 0.00410653651F, 0.00413882406F, 0.0041714781F, - 0.00420450559F, 0.00423791679F, 0.00427172F, 0.00430592475F, 0.00434054F, - 0.00437557558F, 0.00441104267F, 0.00444695121F, 0.00448331F, 0.00452013267F, - 0.00455742935F, 0.00459521171F, 0.00463349326F, 0.00467228517F, - 0.00471160142F, 0.00475145364F, 0.00479185674F, 0.00483282562F, - 0.00487437518F, 0.00491652032F, 0.00495927641F, 0.00500265928F, - 0.00504668895F, 0.00509137847F, 0.00513675157F, 0.0051828213F, - 0.00522961235F, 0.00527714146F, 0.00532543147F, 0.00537450658F, - 0.00542438496F, 0.00547509314F, 0.00552665396F, 0.0055790972F, - 0.00563244335F, 0.00568672409F, 0.00574196922F, 0.00579820713F, - 0.00585547043F, 0.00591378892F, 0.00597320078F, 0.00603374047F, - 0.00609544525F, 0.00615835097F, 0.00622250512F, 0.0062879459F, 0.006354718F, - 0.00642287126F, 0.00649245409F, 0.00656351773F, 0.00663611386F, - 0.00671030255F, 0.00678614574F, 0.00686370581F, 0.00694304705F, - 0.00702424115F, 0.00710735889F, 0.00719248503F, 0.0072797006F, - 0.00736908847F, 0.00746074459F, 0.00755477371F, 0.00765126897F, - 0.00775034819F, 0.00785212312F, 0.00795672741F, 0.00806429F, 0.00817495212F, - 0.00828887522F, 0.00840621162F, 0.00852714293F, 0.00865184888F, - 0.00878054276F, 0.00891342945F, 0.00905075204F, 0.00919276662F, - 0.0093397228F, 0.00949195493F, 0.00964974798F, 0.00981347449F, - 0.00998349674F, 0.0101602506F, 0.0103441793F, 0.0105357962F, 0.010735645F, - 0.0109443273F, 0.0111625558F, 0.0113910241F, 0.0116305985F, 0.0118822251F, - 0.0121469265F, 0.0124259172F, 0.0127205F, 0.0130322482F, 0.0133628659F, - 0.0137143685F, 0.0140890917F, 0.0144897308F, 0.0149194878F, 0.01538203F, - 0.0158818662F, 0.0164243504F, 0.0170160942F, 0.0176650118F, 0.0183811728F }, - 1.0F, - 0.8F, - 1.5F, - 5.0F, - 0.560512543F, - 6.0F, - 0.0174532924F, - 3.0E-7F, - 2.5E-8F, - 0.01F, - 0.00192866661F, - 4.0F, - 0.00219808845F, - 0.0F, - 1.2F, - - { 0.0F, 0.0F, -9.80665F }, - - { -0.00336788432F, 0.000756919035F, -0.000439121271F, 0.00220910879F, - 0.000223762705F, -8.29708297E-5F, -3.63285253E-5F, 0.000105756058F, - 0.00332015823F, 0.000429791224F, 0.00245890114F, 0.00298073539F, - 0.000612389238F, -0.00194776827F, 0.00455108238F, -0.000281568966F, - -0.000486866746F, -0.00318390783F, -0.000892865413F, 0.00181155652F, - 0.00125877827F, 0.000448705105F, -0.000457331684F, 0.000256639702F, - 0.000446673308F, 0.00213241787F, 0.00214878819F, -0.00250839931F, - 0.00199681171F, 0.000388764485F, -0.00123091496F, -0.00157174189F, - 0.00355977588F, -0.00146745425F, -0.00208738353F, -0.00095116609F, - -0.00105573563F, 0.00146875472F, 0.00213086791F, 0.00385333551F, - 0.00178576226F, 0.000591441349F, 0.00237607188F, -0.000467110745F, - -0.000632000912F, 0.000222286224F, 0.000852670055F, 0.00340030901F, - 0.00129333383F, 0.000313011667F, -0.000761072617F, 0.000568527903F, - -0.000191253755F, -0.00171094981F, -2.1555923E-5F, -0.000465813384F, - 3.45232252E-6F, -0.000633936841F, -0.00139456941F, -9.25707645E-5F, - -0.00208319793F, 0.00197498221F, -0.000540694746F, -0.00207471359F, - -0.000496756169F, -0.000511685212F, -0.00379107636F, -0.000457675749F, - 0.00226847478F, 0.00018942797F, -0.000577478786F, -0.00132975937F, - -0.00157581398F, -0.00122977782F, 0.00205743127F, 0.0013322077F, - -0.00221894821F, 0.00321273622F, 0.00102241652F, 0.00164932315F, - 0.000843955379F, -0.0012582097F, -0.00178798335F, 0.00109172985F, - -0.000130870481F, 0.000115722381F, -3.22697088E-5F, -0.00157068402F, - 0.000308997172F, -0.000345857552F, -0.000487587444F, 0.00311357295F, - 0.000574917241F, -0.0037129703F, 5.61115521E-5F, -0.000872521952F, - -0.000823775888F, -0.000608467613F, 0.00186711806F, 0.000142344928F, - 0.00301438128F, 0.000701116F, -0.00111390545F, -0.00562226679F, - 0.00218102406F, -0.00128370186F, 0.000453281449F, -0.00167139783F }, - - { -0.725585699F, -0.725585699F, -0.717133284F, -0.717133284F, -0.693466663F, - -0.693466663F, -0.685014307F, -0.685014307F, -0.698960721F, -0.698960721F, - -0.690508366F, -0.690508366F, -0.720091641F, -0.720091641F, -0.711639285F, - -0.711639285F, -0.698960721F, -0.698960721F, -0.682056F, -0.682056F, - -0.728544F, -0.728544F, -0.711639285F, -0.711639285F, -0.725585699F, - -0.725585699F, -0.708680928F, -0.708680928F, -0.701919079F, -0.701919079F, - -0.685014307F, -0.685014307F, -0.725585699F, -0.725585699F, -0.685014307F, - -0.685014307F, 0.2395F, 0.2195F, 0.2395F, 0.2195F, 0.4015F, 0.3815F, 0.4015F, - 0.3815F, 0.1595F, 0.1395F, 0.1595F, 0.1395F, 0.4815F, 0.4615F, 0.4815F, - 0.4615F, 0.2195F, 0.1795F, 0.2195F, 0.1795F, 0.4415F, 0.4015F, 0.4415F, - 0.4015F, 0.1795F, 0.1395F, 0.1795F, 0.1795F, 0.4815F, 0.4415F, 0.4815F, - 0.4415F, 0.3585F, 0.2625F, 0.3585F, 0.2625F, -0.881302774F, -0.881302774F, - -0.863176644F, -0.863176644F, -0.812423408F, -0.812423408F, -0.794297218F, - -0.794297218F, -0.824205399F, -0.824205399F, -0.806079209F, -0.806079209F, - -0.869520783F, -0.869520783F, -0.851394594F, -0.851394594F, -0.824205399F, - -0.824205399F, -0.787953079F, -0.787953079F, -0.887646914F, -0.887646914F, - -0.851394594F, -0.851394594F, -0.881302774F, -0.881302774F, -0.845050454F, - -0.845050454F, -0.830549538F, -0.830549538F, -0.794297218F, -0.794297218F, - -0.881302774F, -0.881302774F, -0.794297218F, -0.794297218F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 100.0F, - 0.2F, - - { 0.0F, 0.0F, 1.0F }, - - { 0.00606156467F, -0.00344005227F, 0.0180553533F, 0.00865349919F, - -0.00946311746F, 0.00381508283F, 0.0230191424F, -0.0109915743F, - -0.00266298559F, 0.00968829542F, -0.00444955798F, -0.00817996F, - 0.0140017457F, 0.0193218533F, 0.00649184827F, 0.0100369109F, - -0.000658180565F, 0.00428030826F, -0.00188001501F, -0.0079884883F, - 0.00217060745F, -0.0159873366F, -0.00880108681F, 0.00756484084F, - -0.000181183539F, 0.00159069872F, 0.00854385272F, -0.0107202362F, - -0.00947838742F, -0.0112478975F, -0.021161288F, -0.000621224637F, - 0.00430607283F, 0.0056569F, -0.010954448F, -0.00888787303F, -0.00565989409F, - -0.0129386876F, -0.00830491912F, -0.00792784616F, 0.000736621092F, - 0.00642236322F, -0.00844574813F, 0.0088067539F, 0.000518325833F, - 0.00280910637F, -0.00995289814F, -0.00908765383F, -0.00290838163F, - -0.00053331F, -0.00781478081F, -0.00413396955F, -0.0125545813F, - 0.0109629119F, 0.00474097859F, 0.00544589339F, 0.00474756816F, - 0.00138256664F, -0.000376353943F, -0.00471412204F, -0.0161388107F, - 0.00336641702F, -0.000364524632F, 0.00962826F, 0.0094805425F, -0.0162130371F, - -0.000283979112F, -0.00300397747F, 0.00543746026F, 0.00243261224F, - -0.000911900133F, 0.010635308F, 0.027149871F, 0.0112344008F, -0.0198854934F, - 0.0014425047F, -0.0133516127F, 0.0126589192F, -0.0137959486F, - -0.00353120686F, 0.00390082272F, -0.00288937846F, -0.00093180727F, - 0.00737880822F, -0.00614040811F, -0.00538029289F, 0.00769053213F, - -0.00182024552F, 0.00111518789F, 0.00457067601F, 0.0158360545F, - 0.00794899184F, -0.0142966565F, 0.00274946238F, -0.00187182857F, - -0.00897563249F, 0.00395730743F, -0.00754609844F, 0.00172936637F, - 0.00995319244F, 0.00707035139F, 0.00822883751F, -0.0126032289F, - -0.012225762F, -0.00386934658F, 0.0010382561F, -0.0209206752F, - 0.00634169858F, -0.00802259427F, 0.0110762063F, 0.00695273886F, - -0.00342374761F, -0.0028875533F, 0.00780988391F, -0.0154554751F, - 0.00684840046F, 0.0169624966F, 0.00630839F, 0.000114499271F, 0.00167108327F, - 0.0119722513F, -0.00800228212F, -0.00293646078F, -0.00591818F, - -0.00416881824F, 0.00362601876F, -0.0135433357F, -0.00540967472F, - -0.00251467153F, -0.00651993742F, -0.00680367928F, -0.00803572312F, - -0.00322321453F, -0.00297651975F, 0.00111299229F, -0.00423949352F, - -0.000393448019F, 0.00885926F, -0.00479869405F, -0.00883775F, 0.00893347431F, - -0.0242195893F, 0.00590894325F, -0.00390900159F, -0.00209364505F, - -0.0105465641F, -0.00374092301F, 0.0168847479F, -0.00713981362F, - -0.0100497194F, 0.00893209688F, 0.00666550547F, -0.00346075743F, - 0.00594909582F, -0.00183695066F, -0.00258302898F, 0.0169720799F, - -0.0200033709F, 0.00237349188F, -0.0148915658F, -0.00330648781F, - -0.0055645369F, -0.00485477084F, 0.0032390377F, -0.00715921959F, - 0.00780849671F, 0.00720657734F, 0.00495705381F, -0.0071813059F, - 0.0167638548F, 0.0143888285F, -9.3058894E-5F, -0.00257990044F, - -0.0159309544F, -0.00227520894F, -0.0104642604F, 0.00496402709F, - 0.000640567916F, -0.00948576909F, 0.0152633032F, -0.00935794599F, - -0.00339529244F, 0.00681086723F, -0.00272885687F, -0.0146949813F, - 0.00360509707F, -0.0131577589F, -0.0118649025F, -0.00906300452F, - 0.0131161278F, 0.00327090756F, -0.00252448977F, -0.0167778712F, - -0.00383156794F, -0.0184164457F, -0.000595535268F, -0.00755985035F, - 0.00254259026F, -0.00199769833F, 0.000300802611F, 0.00341809471F, - -0.000691933732F, 0.00623060158F, 0.00330770877F, 0.0189398248F, - 0.0151627026F, 0.00808206946F, -0.0162538774F, 0.00578208035F, - -0.000448677398F, 0.0105305575F, 0.00279524666F, -0.00336226076F, - -0.00473966F, -0.0101101073F, -0.0153058646F, -0.00135166582F, - -0.0201697927F, 7.47931408E-5F, -0.00278010522F, -0.00715761399F, - 0.00922386441F, -0.000479212F, -0.00602104561F, 0.00317661162F, - -0.026581673F, 0.002423506F, -0.00382629712F, -0.00632130587F, - -0.00102625112F, 0.00273787952F, 0.00736712059F, 0.000741527707F, - -0.0157483071F, 0.0015655352F, 0.00156827073F, 0.00737781962F, - -0.00552571705F, -0.0019597481F, 0.0113462955F, 0.00489395531F, - 0.00629872642F, -0.00441214209F, 0.0040208362F, -0.00449417392F, - 0.00756792864F, 0.00587344728F, 0.0041020168F, -0.00160966662F, - 0.0129942968F, 0.00389889441F, 0.00214125193F, -0.00666016247F, - 0.00950889476F, -0.00472422317F, -0.00434721634F, 0.0103281979F, - -0.00893547107F, 0.00852341577F, 0.00462331343F, -0.00540325651F, - 0.0114813596F, 0.0111236684F, 0.00907952804F, 0.00515274704F, 0.00535857677F, - 0.00436115451F, 0.000207766076F, 0.0154409138F, -0.00409035059F, - -0.00151193037F, -0.00680552749F, 0.00733996881F, -0.0110018421F, - -0.00332934083F, -0.00555744488F, 0.000243478338F, -0.00608506752F, - -0.0046153767F, -0.0146149239F, 0.00185615104F, -0.00276559079F, - -0.0132109933F, 0.0181835592F, -0.000313335127F, 0.000803823583F, - -0.0122778872F, 0.00218197796F, -0.00638071075F, -0.00225564511F, - -0.00166721409F, -0.025466809F, 0.00362761621F, -0.00541841146F, - -0.000565202849F, 0.00602215063F, -0.0062912209F, 0.00862479117F, - -0.00414668163F, 0.00076501217F, 0.00613231864F, -0.0234907698F, - -0.0118360054F, 0.00382524659F, -0.0101721827F, -0.0210124608F, - 0.00939702056F, 0.00574987149F, -0.00753985764F, 0.00585566601F, - 0.00932550617F, -0.0169944931F, 0.00992552377F, 0.00854495447F, - -0.0077906209F, 0.0072865095F, 0.00660369918F, 0.0034078462F, - -0.00257112039F, 0.00165885314F, -0.0153030707F, -0.00289050699F, - 0.0232428107F, 0.00926323608F, 0.0117034391F, 0.00488043344F, - 0.000615728728F, 0.0135925449F, -0.00550593203F, 0.0128099695F, - 0.0133386375F, 0.00577808032F, 0.00847864617F, 0.0128218029F, - -0.00515220501F, -0.00350694684F, 0.0136552F, -0.010827356F, -0.00852288865F, - 0.0073600281F, -0.00586959F, 0.0129327141F, 0.00129815657F, -0.0258395746F, - -0.00723262364F, -3.52038696E-5F, 0.00938207563F, -2.42339011E-5F, - -0.00285711442F, 0.000205813936F, -0.00520839682F, -0.00209792424F, - 0.000340064755F, -0.00267653237F, -2.68971016E-5F, 0.0174336322F, - 0.0072482042F, -0.0113640418F, 0.0108306017F, -0.000671644812F, - -0.0226518139F, 0.0241687447F, -0.0213565044F, 0.00316398893F, 0.0156784132F, - 0.020625731F, 0.0196231864F, 0.000465976977F, -0.0160134F, -0.00843973178F, - -0.00226906897F, 0.00441164896F, -0.0130004166F, 0.00411510328F, - 0.0104465839F, -0.0156636834F, -0.00455205375F, 0.0234368686F, - -0.00740067661F, -0.00240870682F, 0.00974404626F, -0.00602309173F, - -0.00910938438F, 0.0211451668F, -0.0173088796F, -0.0187027883F, - 0.00323439599F, -0.0107444283F, 0.00041848782F, 0.0148232104F, -0.00835143F, - -0.0003282947F, -0.0117340647F, -0.011861939F, -0.00237188162F, 0.00620691F, - -0.0128319766F, 0.0182411261F, 0.0125901653F, -0.0181230791F, 0.00425567478F, - 0.0118946601F, 0.0157129411F, 0.0125213526F, -0.00264000869F, -0.0015787615F, - -0.00300661498F, 0.00781005435F, -0.00415492104F, -0.00324162305F, - -5.05092976E-5F, -0.00113155378F, 0.00482564652F, -0.00132571557F, - -0.00535091152F, 0.00454125926F, -0.0069677392F, -0.00208899868F, - -0.00216207327F, -0.000280423323F, 0.00277421554F, -0.0134887584F, - -0.00829146337F, -0.00385464448F, -0.0211741943F, 0.0198880192F, - 0.00768465735F, -0.00886919443F, 0.02308584F, 0.0086709F, 0.0183024034F, - -0.00314779207F, -0.00411467487F, 0.00289760786F, 0.00429076049F, - 0.00544497697F, 0.000687598309F, 0.00999781F, -0.0152136525F, 0.00584037276F, - 0.00199156301F, 0.00309212925F, -0.00940055121F, 0.00845886115F, - 0.00298238732F, -0.0219619591F, 0.00107649015F, 0.00208459399F, - 0.0143202553F, 0.00645184703F, 0.0108736884F, 0.00174385612F, 0.00143806497F, - 0.00111533923F, 0.00942300633F, -0.00122628384F, -0.0100508211F, - -0.00181868498F, 0.0121630654F, -0.0136691025F, -0.00135642639F, - 0.0011069522F, -0.00434801914F, -0.0042508659F, 0.00350622204F, - 0.00553556858F, -0.0108376956F, 0.00217491901F, -0.000161719188F, - -0.0112593537F, -0.00640390115F, 0.0289371815F, -0.0162465591F, - -0.00144722522F, 0.0164335296F, -0.00666276086F, 0.00387572404F, - 0.0130085442F, -0.00431104423F, -0.00481119473F, -0.0049422523F, - -0.00570453145F, -0.000778472167F, 5.23201925E-5F, 0.00429674145F, - 0.00525222812F, -0.00493790908F, -0.018944405F, -0.0102667427F, - 0.0122618889F, -0.00485703489F, 0.0140237743F, 0.00284989667F, 0.0170525815F, - -0.0076654586F, -0.00294364337F, -0.0139437709F, -0.027684629F, - 0.0101535264F, 0.00328143453F, -0.00794723537F, 0.000876911858F, - -0.000598226732F, 0.00435226038F, 0.00660784869F, -0.0111084105F, - -0.00290883495F, 0.000746363658F, -0.000136318806F, -0.00678478042F, - 0.0016372595F, 0.00175219798F, 0.0197298713F, 0.00288926833F, 0.00420642085F, - -0.0116319694F, -0.00524374703F, 0.0124317901F, 0.0107039502F, - -0.0184392501F, 0.00933973119F, 0.00654625567F, -0.0156219918F, - 0.000233224331F, -0.00677607814F, -0.0133225853F, -0.00443376275F, - -0.00979377236F, -0.00443680165F, -0.000603302615F, 0.0172509179F, - -0.00745766237F, 0.00156663312F, 0.00545492722F, 0.0117284842F, - 0.0130755333F, 0.00992071256F, 0.0059167915F, 0.017799899F, -0.00433220621F, - -0.00013127533F, 0.00781681109F, -0.00134264771F, 0.0036925415F, - 0.00699291285F, 0.00337239867F, -5.20506956E-6F, 0.00221215072F, - -0.00169512036F, 0.00279290532F, 0.00790651143F, -0.00014080145F, - 0.000713506946F, -0.00699186185F, -0.00877050124F, 0.00488128047F, - -0.00463150442F, 0.0078700576F, -0.0136306109F, 0.00517367572F, - -0.00632078247F, -0.00992224459F, 0.00324682775F, -0.00613524392F, - -0.00456635188F, 0.0102063352F, -0.0212193336F, -0.0144760925F, - -0.0026682287F, 0.00613824045F, 0.0012808023F, 0.00570368487F, 0.0089703612F, - -0.00236674375F, -0.00195455505F, -0.00934492331F, 0.00950122252F, - 0.0155558912F, 0.0036439281F, 0.00715691689F, -0.000444967038F, - 0.00633150293F, 0.00912440754F, 0.00340665621F, -0.00357529684F, - 0.012186504F, -0.00412787963F, 0.00743196951F, 0.00932896882F, - -0.0106556686F, -0.0170328692F, 0.00165477069F, -0.00758263282F, - -0.00431907596F, -0.00512616F, 0.00161715399F, -0.00204595947F, 0.00826144F, - 0.00699883699F, -0.00315183122F, 0.013904037F, 0.0163200535F, -0.0121856015F, - -0.000735311F, 0.00454972498F, -0.00865622703F, -0.0138298227F, - -0.000394707487F, 0.00390612544F, -0.00384827843F, -0.00676562265F, - 0.00556194503F, 0.00263215299F, 0.0112350425F, -0.0079720756F, - 0.00624962756F, 0.00398215931F, -0.0263605341F, 0.00382214226F, - -0.000164813799F, -0.00426447345F, -0.0109346863F, -0.00379145588F, - 0.00378801185F, 0.0176146217F, 0.0110308863F, 0.00412995461F, 0.0101969345F, - -0.00723832194F, -0.0218386892F, 0.00363201462F, -0.0123484759F, - 0.00273140939F, 0.00996249F, -0.0127519369F, -0.00622306392F, 0.00381385116F, - -0.00675543211F, -0.0119581763F, 0.00699661206F, 0.00650731288F, - 0.00725589041F, -0.000300940796F, -0.0123206014F, -0.0153628429F, - 0.00343260425F, -0.00180567987F, 0.00860201847F, 0.0136477537F, - 0.0119351195F, 0.000432604604F, -0.00468939263F, -0.00372709986F, - 0.00287470571F, 0.0133960508F, -0.00824329F, -0.00624222541F, 0.00481390255F, - -0.0227996446F, -0.00156518444F, -0.0105533889F, -0.00496784737F, - 0.00269284379F, -0.000114698596F, 0.0168459285F, 0.00565707218F, - 0.0114758676F, 0.00458923308F, -0.00677418848F, 0.00959109329F, - 0.00394871086F, -0.00150114263F, 0.00584908249F, 0.00865442678F, - 0.000883616158F, 0.000934448966F, 0.016941905F, -0.0131530724F, - 0.00264091138F, -0.00163345481F, -0.00897766929F, -0.0207896121F, - -1.75315745E-5F, 0.00308867404F, 0.0169646069F, 0.0172869042F, - -0.00407053716F, -0.000221079681F, -0.000187136538F, 0.000154477835F, - -0.0127898753F, -0.0263119228F, -0.0164325F, -0.000548108947F, - 0.00105730596F, -0.0170398764F, -0.00450286F, 0.0157485157F, -0.00427580671F, - 0.00530213583F, -0.00379790575F, -0.00740397256F, 0.000548614422F, - -0.000521130743F, 0.0171797946F, 0.0154786007F, 0.000454373861F, - 0.00851280149F, 0.00339569338F, -0.000801311398F, -0.0123865828F, - 0.00653325114F, -0.00634922273F, 0.00239180611F, 0.00624982268F, - 0.00658280449F, 0.0184252877F, 0.00570788421F, 0.0130226798F, 0.0229164176F, - 0.0101641649F, -0.00515258731F, 0.00168769748F, 0.0233891383F, - -0.00851282571F, -0.00192833156F, 0.00560424663F, -0.0120151686F, - 0.00691955211F, 0.010014629F, -0.000116914249F, 0.00854881108F, - -0.0140357837F, 0.00408388907F, -0.0120354379F, -0.00984383561F, - -0.00514139328F, -0.00248310296F, 0.0107314205F, -0.00363709172F, - -0.00922634639F, 0.00938294176F, 0.00411597732F, 0.00763624441F, - 0.00231501716F, 0.0103797559F, 0.00218500243F, -0.00677398173F, - -0.0160205271F, 0.00562009355F, 0.0147418249F, 0.00101009116F, 0.0181491636F, - -0.00693945447F, 0.0242282748F, 0.00532765547F, -0.00751335965F, - -0.00507093454F, -0.00796500593F, -0.019202102F, -0.00933371577F, - 0.00584648782F, -0.00203103758F, -0.00655254535F, 0.000526015472F, - -0.017108513F, -0.00818206742F, 0.0108936811F, -0.00168689969F, - -0.000342054438F, 0.0177349113F, -0.00285774935F, 0.0081971F, 0.00431501959F, - -0.00294676214F, -0.00579587882F, 0.0077422359F, -0.0135637373F, - -0.0131975971F, -0.00553047284F, -0.000580371358F, 0.00890558586F, - -0.0181232803F, -0.019528674F, 0.00913353F, 0.0208819099F, 0.00284981774F, - 0.00367487571F, -0.00353369489F, 0.00380829698F, -3.11868062E-5F, - -0.00772478106F, -0.00121148804F, 0.00651197322F, 0.00799740851F, - 0.00989048835F, 0.00778715F, -0.0125814816F, 0.0105063049F, -0.0101585472F, - -0.00302725961F, 0.00606134953F, 0.00118114648F, 0.0119329588F, 0.011089405F, - 0.00607850729F, 0.00309526571F, -0.00685188686F, 0.011327209F, - -0.00450653536F, -0.00405874383F, -0.00539274095F, -8.3754494E-5F, - -0.00781070907F, 0.0162187517F, -0.00993565843F, -0.0109221069F, - 0.0201183464F, 0.0123060336F, 0.0038502058F, 0.00148663053F, 0.0171384681F, - -0.000625622284F, 0.00414494751F, -0.000351218187F, -0.0153058013F, - -0.00781788584F, -0.00304422597F, 0.00471413648F, -0.000238539025F, - -0.0096369544F, -0.0098245088F, -0.00179725781F, -0.0175071675F, - 0.00475338846F, -0.0154629676F, 0.00878568646F, -7.78579197E-5F, - -0.0126066403F, 0.0075028073F, -0.0147933643F, -0.00102938537F, - -0.00294048456F, 0.00370175554F, -0.0136896893F, 0.00263854954F, - -0.00825191103F, 0.0148988385F, 0.00797565561F, 0.0075495136F, - 0.00545140542F, -0.00568275386F, -0.00821636F, 0.00491539482F, 0.0121472394F, - 0.00234598434F, -0.00385132525F, -0.00873402599F, -0.0137186116F, - -0.0042176689F, -0.0228361711F, -0.00155828078F, 0.00280174636F, - -0.00723343436F, 0.0028401739F, -0.00793373678F, -0.00319909793F, - -0.00609985646F, 0.00579642784F, 0.0168161262F, -0.00546936225F, - 0.00288006361F, 0.00154086656F, -0.00679807272F, -0.0124916984F, - -0.00535398535F, -0.0046228217F, 0.00268494477F, 0.00737315649F, - 0.0112131126F, 0.00927090086F, 0.00612927787F, -0.012036154F, - -0.00475038728F, 0.00158888451F, 0.0109490128F, 0.00968517736F, - -0.00035050654F, 0.0113811726F, 0.00374973263F, 0.0174245052F, - 0.00636486523F, 0.0230540074F, -0.0150824683F, -0.000223713636F, - 0.00523289572F, 0.00436662789F, 0.00582331186F, 0.0132226609F, - -0.0114023387F, 0.00384053728F, -0.0154325496F, -0.00605116086F, - 0.0301829502F, 0.0115082348F, -0.00185200712F, -0.00828842726F, - -0.00722050481F, -0.00477433112F, 0.0146236327F, -0.0243880283F, - 0.00779938931F, -0.0132157644F, 0.00773613062F, 0.00266060326F, - 0.00655559124F, -0.0147237396F, -0.00286156591F, -0.00141409773F, - -0.0047247773F, 0.00284973F, 0.000158796785F, -0.00111162942F, - -0.00778706931F, 0.00605295645F, 0.0140561201F, -0.00801931322F, - 0.00985490065F, -0.00378970662F, -0.0025413495F, 0.00815514F, 0.00539900782F, - 0.00744754868F, -0.0137804048F, 0.0257321279F, 0.00603005383F, - 0.00546593545F, 0.00427174522F, 0.000418512878F, -0.0172742177F, - -0.0107847089F, 0.0103799459F, -0.0052833478F, 0.00811520871F, - -0.00482427748F, -0.009882004F, -0.00241914508F, -0.00322786206F, - -0.0117566586F, 0.00576012908F, 0.00504081929F, 0.00865292083F, - -0.000923772634F, -0.0035288725F, -0.0064346604F, -0.0104649151F, - -0.00664873561F, 0.0150471227F, 0.00692381756F, 0.00535333343F, - 0.0140934438F, 0.0268551633F, 0.00294318725F, 0.00730766496F, 0.00247543189F, - -0.00415885495F, 0.0102328444F, -0.00675973156F, -0.00492606591F, - 0.0121907266F, 0.00138745271F, -0.0073876041F, -0.00149262534F, - -0.00414876826F, -0.00117798767F, 0.0221877843F, -0.0104949847F, - 0.0047911359F, -0.00836875662F, 0.00242519588F, 0.0139254089F, - 0.00494407769F, -0.00629032589F, -0.00388216437F, -0.000621538318F, - 0.00660278229F, -0.0086132139F, -0.0039795707F, -0.0108811855F, - -0.000706015271F, -0.00286838971F, 0.00563927414F, -0.00623945612F, - -0.0118178995F, -0.00583374035F, 0.00301285391F, 0.00026669231F, - -0.00248049F, -0.00628173491F, -0.00450748159F, -0.0179023761F, - -0.00569260074F, -0.00432895171F, 0.00169266318F, -0.00694761286F, - -0.0102320416F, 0.0179979969F, -0.00304080686F, -0.000911565148F, - -0.00755828293F, -0.00416444382F, -0.00169588078F, -0.00242914585F, - 0.00749799237F, -0.0156564247F, -0.00565516809F, 0.0102857072F, - 0.00739148259F, -0.0120583931F, 0.00639759284F, -0.00866185129F, - -0.0109267309F, -0.0139830261F, -0.00201014732F, 0.0194261353F, - 0.00113116822F, 0.00936897751F, -0.00517284498F, 0.00231899857F, - 0.00173771067F, -0.00526896492F, 0.0203235466F, -0.00903521199F, - -0.00445882231F, -0.0199591387F, 0.00893322F, 0.00371566857F, - -0.00287074153F, -0.0152072562F, 0.00242119096F, -0.00902202725F, - -0.019160958F, -0.00619070837F, 0.00136462122F, 0.00943011325F, - 0.00918430462F, 0.0145257562F, -0.00672657927F, -0.0023816498F, - -0.00649463432F, 0.00598790636F, 0.00643343851F, 0.00403455831F, - -0.00080870674F, 0.000500822091F, -0.0183180217F, 0.000278432184F, - 0.0114894193F, 0.00393312657F, 0.00626534596F, 0.00346087199F, 0.00764776F, - -0.00253980327F, 0.0121207926F, 0.0152539602F, -0.0111105964F, - -0.00286106346F, 0.0140640419F, -0.0199010838F, 0.00128644309F, - -0.00786273181F, 0.00600140961F, -0.0205555763F, -0.00341810985F, - 0.00361425243F, 0.00777353672F, -0.00775141455F, 0.000956314F, - -0.0127979284F, 0.012776833F, 0.000826353964F, 0.00521679223F, - -0.00591666903F, -0.0218184516F, 0.00153496035F, 0.00181845645F, - -0.00653077429F, -0.00451387092F, -0.00804540236F, 0.00131360174F, - 0.00622732425F, 0.0167679489F, 0.00170027651F, 0.0190020949F, 0.0101232231F, - 0.00238428F, 0.00237877527F, 0.00743137114F, 0.00146170147F, 0.0168895833F, - 0.0028515798F, 0.00972718F, 0.0221331753F, -0.00873037148F, 0.0124103762F, - -0.0145495487F, -0.00176503928F, 0.0126678571F, -0.0145745976F, - 0.00383344782F, 0.00653578F, 0.0154492799F, 0.00276825856F, -0.000997586874F, - 0.0108995764F, 0.0203080885F, 0.00860745925F, -0.0100343516F, - -0.000701300858F, 0.00635778811F, -0.0100525301F, 0.00520936539F, - 0.00253044884F, -0.00978735741F, 0.00678782631F, -0.00712966546F, - -0.00973638054F, 1.86897723E-5F, 0.0079839956F, 0.0187991168F, -0.004992045F, - 0.0069763707F, 0.0173147842F, 0.0136419479F, -0.000771210121F, - -0.0139191234F, 0.0153882857F, 0.0168660618F, 0.0125643089F, 0.00717453845F, - -0.00483593764F, -0.0108232405F, -0.021039078F, 0.00394017668F, - 0.0135308048F, -0.0102838101F, 0.00458278926F, -0.0102022532F, 0.0101332832F, - -0.00611236F, 0.00506209489F, -0.0117074493F, 0.00310105481F, - -0.00585747976F, 0.0138740959F, -0.030660985F, 0.00361339422F, - 0.000851517078F, -0.00339153409F, -0.00771935889F, -0.00247773458F, - 0.00517439656F, -0.0159918312F, -0.00542912818F, -0.00660623889F, - 0.00451578153F, 0.00797613896F, 0.0022116343F, -0.00438325061F, - -0.00270072115F, 0.0234323777F, -0.0114949355F, -0.00738757104F, - 0.00508660078F, -0.0206393413F, 0.00376798795F, 0.00304519874F, - 0.0140676862F, -0.000129562992F, -0.00566003472F, -0.00144800358F, - -0.00322630419F, 0.0105997603F, 0.00388369593F, -0.000251108024F, - -0.0107814595F, 0.00302141043F, 0.0092195347F, -0.0105419504F, - -0.00840861816F, 0.00481152F, -0.00607469445F, -0.00584123703F, - -0.00449617067F, 0.0046926639F, -0.00131321466F, -0.0249874368F, - -0.0131934751F, 0.000749698142F, -0.00686704041F, 0.0147432778F, - 0.0119443322F, -0.00885587F, -0.013340543F, 0.000759352406F, -2.69354896E-5F, - -0.00440696767F, -0.00318312622F, -0.0185263734F, 0.00375912851F, - 0.00670788763F, -0.0251185745F, 0.00364510063F, -0.0025651555F, 0.011266673F, - -0.00850900635F, -0.00724976F, -0.0103601823F, 0.00419259863F, 0.0159228873F, - -0.0133723831F, 0.0119425356F, 0.00370723289F, 0.0134407915F, 0.00111149205F, - 0.00947802793F, 0.00163311837F, -0.00964134559F, -0.00102641701F, - 0.000614267483F, 0.00782280788F, -0.00802238472F, 0.0222224928F, - -0.0109939976F, -0.0085825352F, 0.00696683489F, 0.00730071496F, - 0.00902631879F, 0.0199549217F, -0.0228392221F, 0.0111434404F, - -0.00581589574F, -0.00276107853F, 0.0237442292F, 0.00247926312F, - -0.00682529481F, -0.00718976F, -0.0316121429F, 0.00331887323F, - -0.00238487567F, -0.00865012F, 0.00806322135F, 0.000204046912F, -0.0060927F, - -0.00394930225F, 0.00053656369F, 0.00288434979F, 0.0187103357F, - 0.0154022789F, 7.16819632E-5F, 0.00539275398F, -0.00266919145F, - 0.00695053721F, -0.00694450783F, 0.0109412437F, -0.00326431426F, - 0.0114749167F, -0.000319091079F, -0.0114847077F, -0.0188764818F, - 0.00400711829F, 0.0110599641F, -0.0207023751F, 0.0182348751F, 0.0122678624F, - -0.00298097613F, 0.0158923231F, -0.0145307593F, -0.0167927183F, - -0.00523655722F, 0.00604445674F, 0.0040673865F, -0.00709913F, 0.00923689175F, - -0.0128292749F, 0.0152922468F, -0.00181564409F, -0.00260811672F, - 0.00395049667F, 0.00684415875F, 0.0112871788F, -0.00721268402F, - -0.0035061189F, 0.00547382608F, 0.0102005154F, -0.00496364711F, - -0.00192520407F, 0.00550613366F, -0.00201306911F, 0.00266158045F, - 0.00555801624F, -0.0063238549F, -0.00249905908F, 0.00348508917F, - -0.011632042F, 0.00618480146F, 0.00305968826F, 0.0106026987F, 0.00355941313F, - 0.0137059065F, 0.00212493609F, 0.0041413107F, 0.00653864816F, 0.00145151222F, - 0.00243590353F, 0.00983545743F, 0.00640956638F, -0.00533975661F, - -0.00379221118F, -0.000132692236F, -0.00117873377F, -0.0100174854F, - -0.00907726772F, 0.0140218185F, 0.00227373489F, 0.0022388876F, - 0.00180529349F, -0.014224018F, 0.00165801414F, 0.00181757647F, 0.0052479594F, - 0.00596650317F, -0.0191473756F, 0.00678483164F, 0.000589808857F, - -0.0104528526F, -0.0161519311F, -0.0175847411F, -0.0127698863F, - 0.00449541304F, 0.00282670325F, 0.00970586576F, -0.0115521746F, 0.010461282F, - -0.00486918911F, 0.00827986095F, -0.00415304F, -0.015993014F, -0.0139304148F, - -0.0129564079F, -0.00027404452F, -0.00543331215F, -0.00310313865F, - 0.00687347259F, 0.00942933094F, -0.00039688166F, -0.0169839971F, - -0.00498432852F, 0.00453242334F, -0.00665019453F, -0.000646214F, - 0.00385925034F, 0.0114894323F, -0.00308699F, 0.00424670102F, -0.00178426132F, - -0.00587247033F, 0.00485537294F, -0.00753640337F, 0.0132445376F, - 0.0093662953F, 0.00377664901F, -0.00752105052F, 0.00459955167F, - -0.013162164F, 0.00209355494F, -0.000293422811F, 0.00865500607F, - 0.00163767196F, 0.00855708495F, 0.000133521113F, 0.00280669192F, - -0.0180157721F, -0.00262307981F, 0.0164209045F, 0.00127417198F, - -0.00264777709F, -0.0170012172F, 0.0118077416F, -0.00703990692F, - 0.00153439655F, -0.00660424866F, 0.00922852755F, -0.012576269F, - -0.0067780083F, -0.00317920279F, 0.0147818746F, 0.000111077366F, - 0.00771275442F, 0.000932911644F, -0.00406428473F, -0.00394204F, - -0.000549929449F, 0.00531405117F, -0.0100019174F, -0.00910795853F, - 0.00536757847F, 0.00408693962F, -0.0165809914F, 0.00429431F, 0.00251080794F, - -0.00327563076F, 0.0131212529F, 0.00998222269F, -0.0127293924F, - 0.00338138943F, -0.0053871274F, -0.00467684306F, -0.00901273359F, - -0.00560387829F, -0.0188801587F, 0.000585781527F, 0.00427385792F, - 0.0106652463F, 0.0100555532F, 0.00208299747F, 0.00205589947F, -0.0014467095F, - -0.0104657244F, -0.00794769265F, -0.0120004F, -0.00116257544F, - -0.00131972495F, 0.000588683F, -0.00569921406F, -0.0133600645F, - 0.0132714864F, -0.00144543464F, -0.00457139267F, 0.00145351049F, - -0.00492069731F, -0.0117270602F, 0.0147676831F, -0.00579861924F, - 0.00232021417F, -0.00174799236F, 0.00318340841F, -0.00083559379F, - -0.00906976871F, -0.00985423755F, -0.00283995178F, 0.00814777147F, - -0.00673043821F, -0.00886278879F, 0.00908601377F, 0.010963452F, - 0.00603221124F, -0.00451478176F, 0.00323459972F, -0.0160974804F, - 0.00599229801F, 0.00535980193F, 0.000912241463F, 0.00702845631F, - 0.00607902883F, 0.00217450899F, -0.0153651508F, 0.00921045337F, - -0.00524700945F, 0.0135003962F, -0.014354825F, 0.0125274826F, 0.0116131743F, - -0.0158052854F, 0.00155229075F, -0.00144544744F, -0.0157904942F, - 0.0121419439F, 0.00877784193F, 0.00145061209F, -0.00367297418F, - 0.000338821294F, 0.00501757115F, 0.0119824689F, 0.00470937137F, - 0.0183094274F, 0.00191869191F, 0.00946547464F, -0.00819592F, 0.00770876F, - 0.00391929643F, -0.00146505621F, 0.00355794746F, -0.00178792048F, - -0.000971139118F, -0.00123281544F, 0.00143884541F, -0.0152805569F, - 0.00212810049F, 0.0033785745F, 0.00797017384F, 0.00193021144F, - -0.00151969376F, -0.00573196448F, 0.00168459886F, 0.00340447226F, - -0.0142585104F, -0.00327389478F, 0.000594555109F, 0.00391577277F, - -0.00689042127F, 0.00151074061F }, - - { -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, - -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.6F, -0.5F, -0.45F, - -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, - 0.1F, 0.15F, 0.2F, 0.25F, 0.3F, 0.35F, 0.4F, 0.45F, 0.5F, -0.5F, -0.45F, - -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, - 0.1F, 0.15F, 0.2F, 0.25F, 0.3F, 0.35F, 0.4F, 0.45F, 0.5F, -0.5F, -0.45F, - -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, - 0.1F, 0.15F, 0.2F, 0.25F, 0.3F, 0.35F, 0.4F, 0.45F, 0.5F, -0.5F, -0.5F, - -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, - -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.45F, -0.45F, - -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, - -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, -0.45F, - -0.45F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, - -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, -0.35F, - -0.35F, -0.35F, -0.35F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, - -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, - -0.25F, -0.25F, -0.25F, -0.25F, -0.25F, -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, - -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, - -0.2F, -0.2F, -0.2F, -0.2F, -0.2F, -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, - -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, - -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, -0.15F, -0.1F, -0.1F, -0.1F, - -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, - -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.1F, -0.05F, -0.05F, -0.05F, - -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, - -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, -0.05F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, - 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, 0.1F, - 0.1F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, - 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, 0.15F, - 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, - 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.2F, 0.25F, 0.25F, 0.25F, 0.25F, - 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, - 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, 0.25F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, - 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, 0.3F, - 0.3F, 0.3F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, - 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, 0.35F, - 0.35F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.45F, 0.45F, - 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, - 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.45F, 0.5F, 0.5F, 0.5F, - 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, - 0.5F, 0.5F, 0.5F, 0.5F, 0.5F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, -0.4F, - -0.4F, -0.4F, -0.4F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, - -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, -0.5F, - -0.5F, -0.5F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, - -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, - 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, - -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, - -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, - -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, - -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, - -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, - -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, - -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, - -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, - -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, - -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, - -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, - -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, - -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, - -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, - -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, - -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, - 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, - -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, - 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, - -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, - 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, - -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, - -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, - -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, - -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, - -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, - -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, - -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, - -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, - -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, - -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, -0.85F, - -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, -0.35F, - -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, -0.9F, - -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, -0.4F, - -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, 0.1F, - -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, -0.45F, - -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, 0.05F, - 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, -0.5F, - -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, 0.0F, - 0.05F, 0.1F, -0.9F, -0.85F, -0.8F, -0.75F, -0.7F, -0.65F, -0.6F, -0.55F, - -0.5F, -0.45F, -0.4F, -0.35F, -0.3F, -0.25F, -0.2F, -0.15F, -0.1F, -0.05F, - 0.0F, 0.05F, 0.1F }, - - { -0.000775528606F, 0.0158610158F, 0.00417824F, 0.00445442181F, -0.0252044573F, - -0.0027608776F, 0.00217027846F, -0.00128508545F, -0.0135706551F, - -0.00501568709F, -0.000394993374F, 0.0189161189F, -0.00500314636F, - 0.00596865965F, 0.00392828742F, 0.000503454881F, -0.00829918496F, - 0.0113634849F, -0.00305921515F, -0.000768949918F, 0.00321426406F, - 0.00395918498F, -0.0145398509F, -0.00180118391F, 0.00378196314F, 0.00942464F, - -0.00800419785F, 0.0138827311F, -0.0174361821F, -0.00663802633F, - 0.00204259553F, -0.00412528263F, -0.00240772404F, 0.0142825814F, - -0.010503496F, -0.00457228813F, 0.0119782519F, -0.00736766F, 0.00256815017F, - -0.0153462915F, 0.0130769294F, 0.00473603932F, 0.00386605F, -0.00223562913F, - 0.00253604143F, 0.00498596858F, 0.00258023362F, 0.0030856668F, 0.0126687558F, - 0.00409641257F, 0.0303223804F, -0.0110754687F, 0.00466208905F, - -0.0056147268F, 0.00156289281F, -0.00115156977F, -0.0122088846F, - 0.0100127291F, 0.000401449361F, -0.00240273727F, -0.00091189181F, - -0.00691286195F, 0.000268574076F, 0.0127989352F, 0.00534032F, 0.0113928728F, - 0.000500500435F, -0.000513502338F, -0.0081229154F, 0.00325485459F, - -0.00213916576F, 0.0128440205F, -0.0178786945F, 0.0113723315F, - 0.00208262517F, 0.00999497063F, 0.00779929152F, 0.00307828165F, - 0.00440786174F, -0.00313872495F, -0.0140126739F, -0.0286480412F, - -0.00347192306F, -0.00599342771F, -0.00679770438F, 0.00103818683F, - 0.00333339F, -0.00192469824F, 0.00545050064F, -0.00477015739F, - -0.0124289505F, 0.008232967F, 0.0244589243F, 0.00425961753F, 0.00488227513F, - 0.00540169142F, 0.0055934228F, 0.012248205F, -0.00971427653F, - -0.00593483215F, -0.00445038F, 0.00516782142F, -0.000672635855F, - -0.00932547543F, -0.00749334693F, 0.000715850852F, 0.00913964305F, - -0.000764140335F, 0.000222814124F, -0.010344889F, 0.0055178511F, - 0.0162657034F, 0.00592851825F, -0.00291305315F, -0.00042130411F, - 0.00792462751F, -0.000677362492F, 0.00224118982F, -0.00750228437F, - 0.00624033F, 0.0205782354F, 0.0082850866F, 0.00382165983F, 0.0108609153F, - -0.00755002396F, -0.00922051F, 0.00758228265F, -0.0150188301F, - -0.00263301027F, -0.00912298914F, 0.00329113496F, -0.00307021337F, - -0.00441705808F, 0.0013073947F, -0.00433972897F, 0.00778854219F, - -0.015493297F, 0.0120583726F, -0.00179423776F, 0.0106279803F, -0.0125134541F, - -0.00759708416F, -0.00962749F, -0.00419166824F, -0.00853284914F, - 0.00245185359F, -0.0170947723F, -0.00470481813F, 0.00280465069F, - -0.00826580077F, 0.000377456076F, -0.00317718391F, -0.00117208669F, - 0.0166212283F, 0.00128669839F, 0.00502851233F, 0.0034697F, 0.00220269104F, - -0.0129260011F, 0.00510806032F, 0.00614696043F, 0.0174443442F, - -0.00415595295F, 0.0188004803F, 3.52056668E-5F, -0.004440533F, - -0.0068828119F, 0.00963843F, 0.00595729F, -0.0104331421F, 0.0109550469F, - -0.0194068F, -0.0120635135F, -0.00336210965F, -0.00767521467F, - -0.0114409551F, -0.00187057967F, 0.00214045914F, -0.00814763922F, - 0.000963307F, -0.00613265252F, -0.00919403F, 0.000592719764F, -0.0164303202F, - 0.00986134354F, 0.00686417241F, 0.00686078053F, 0.00560525106F, - 0.000138332893F, -0.00741964718F, 0.0181084853F, -0.032097321F, 0.00761962F, - 0.0156609137F, 0.000592887867F, -0.00560023868F, 0.0231077485F, -0.00798319F, - -0.0073486194F, 0.00415462395F, -0.00254017464F, -0.0125175798F, - -0.0220021959F, -0.0135632716F, -0.00821506232F, -0.0062038959F, - -0.000904236862F, -0.00347292097F, 0.00186094828F, -0.0118268691F, - 0.00247854833F, 0.00660304166F, 0.0130017521F, 0.00723884813F, - -0.0011589576F, -0.0186407175F, -0.00798983872F, 0.00521528441F, - -0.00338950753F, 0.00745373638F, -0.000810468F, -0.00976062752F, - 0.00966737047F, 0.00214463961F, 0.0152875949F, 0.00369537133F, - -0.0177255254F, -0.020976048F, -0.0101330504F, 0.00174665533F, - -0.00111228088F, 0.00860178657F, -0.00860973354F, 0.0122439377F, - 0.0118216826F, 0.000356779754F, 0.0168383047F, 0.00562445726F, - 0.00156577129F, 0.0043716887F, -0.00478393491F, 0.00528927566F, - -0.000841669738F, -0.00198747544F, 0.00679919031F, 0.00189706672F, - -0.012333381F, -0.0140324859F, 0.000709113083F, -0.0129903452F, - -0.00599393621F, 0.0128324246F, 0.0126282163F, -0.00594759127F, - -0.00516326865F, -0.0138673782F, -0.00285434653F, -0.0105991391F, - 0.0103458371F, -0.00540418085F, 0.0190067329F, 0.00957122911F, - 0.000279911939F, 0.00256110076F, 0.00792530365F, -0.00532410573F, - -0.0198996849F, -0.00694270898F, 0.00587642752F, 0.0102553228F, - 0.00752519397F, -0.0242575072F, -0.00837144256F, -0.00266990322F, - -0.00710360799F, 0.00494261365F, 0.00940529164F, 0.00414964557F, - -0.00268780487F, 0.00807067472F, 0.000398448348F, -0.000770787417F, - -0.00543388259F, -5.88692237E-5F, 0.00408524927F, 0.00130324124F, - -0.010180953F, -0.00363854272F, -0.0108015416F, 0.0216824152F, - -0.00278109312F, 8.74157413E-5F, -0.014561709F, 0.00238574855F, - -0.00245847087F, 5.23772469E-5F, -0.00812040642F, 0.00987101067F, - -0.00455640582F, 0.0130314203F, 0.00382653344F, -0.00475396868F, - 0.0115423054F, 0.00312178908F, 0.0019507187F, 0.0115815168F, 0.0122164357F, - -0.0121518755F, -0.0138996849F, -0.0120874364F, -0.00345355668F, - 0.00175030914F, 0.0170906838F, 0.00832458586F, -0.0170460064F, - 0.00181900861F, 0.0186043773F, -0.0042634327F, -0.0121130235F, - 0.00701162172F, 0.00708244694F, 0.0167945903F, -0.00810318906F, - -0.0127456272F, 0.0116027985F, -0.00119489862F, 0.000778521295F, - 0.00856387895F, -0.00513250427F, 0.0116151804F, 0.0026667479F, -0.019791102F, - 0.00233982853F, -0.00466814637F, 0.0126775531F, 0.00936457235F, - -0.00429868419F, 0.00170888286F, -0.0115091931F, -0.00212975568F, - -0.00233052531F, -0.00435925042F, -0.00111223687F, 0.000800762093F, - 0.00512052048F, 0.0131914597F, 0.00619080383F, 0.000870544522F, - 0.0043987534F, -0.0123505425F, 0.0090364581F, 0.000152546127F, - -0.00509607187F, 0.00754941581F, -0.0223647244F, 0.00482345745F, - -0.00824325252F, 0.0139042148F, 0.00119834777F, -0.00449490221F, - -0.00272784522F, -0.0120277256F, -0.000664103951F, 0.0109632444F, - -0.0121835759F, -0.003339421F, -0.0214728713F, -0.0117291799F, - -0.00934504345F, 0.00239314744F, 0.00459889602F, 0.0114034042F, - 0.00951395556F, 0.00376525661F, 0.00847074296F, -0.00596337719F, - -0.00699015427F, -0.0188241098F, 0.00545353116F, -0.0170001127F, - 0.00694682961F, 0.00930311F, -0.0225135721F, -0.00931650493F, 0.0152305504F, - -0.00127561367F, -0.000301009713F, -0.0108124912F, 0.0167055279F, - -0.0083676409F, -0.00535947364F, -0.0088440571F, -0.00843056478F, - 0.0312357973F, -0.011227211F, -0.00769829843F, -0.0131377941F, 0.0162109975F, - 0.0223197173F, 0.0111055542F, 0.00992772449F, -0.00990916323F, - -0.00080309133F, 0.00550042745F, -0.00370006636F, -0.0117985578F, - -0.0197161902F, 0.0110250311F, -0.0141420988F, 0.00365919364F, 0.0174252447F, - 0.00957325287F, -0.00276913447F, 0.00243342016F, 0.0167761669F, - -0.0107981935F, 0.0137235299F, -0.00185638038F, 0.00569803268F, - -0.000114561553F, 0.00850940123F, 0.0120362332F, -0.00434703566F, - -0.0140602654F, -0.000625512679F, -0.00165749632F, 0.00563783199F, - 0.00772498688F, 0.00230876519F, 0.000547244621F, -0.0109435609F, - 0.0234567039F, -0.00153064018F, -0.0148459682F, 0.00158101623F, - -0.012751393F, 0.00160835695F, -0.0126629248F, 0.0115634995F, 0.0197329931F, - 0.00296264887F, 0.00117355911F, -0.00644469354F, 0.0108496062F, - 0.000802505529F, -0.0099800881F, -9.48156958E-5F, -0.0159697123F, - -0.00633647898F, -0.00262475642F, -0.00113143143F, 0.00633290363F, - 0.00377723249F, -0.000993509078F, -0.00191629434F, -0.000956308504F, - -0.00280167302F, -0.000562985777F, 0.00768055441F, 0.00307552284F, - 0.00189373444F, 0.0116755404F, 0.0123886336F, 0.00671709795F, 0.0102030272F, - -0.00248127175F, 0.00858528726F, -0.00508681871F, -0.0163106658F, - 0.0237682946F, 0.00948790181F, 0.00187679334F, 0.0103114368F, 0.00588027807F, - -0.00917936396F, 0.0164315309F, 0.0210398249F, -0.00821758F, -0.0106403911F, - -0.015351152F, -0.000859676744F, 0.00129043637F, 0.00611986499F, - -0.00505692465F, 0.0216708928F, 0.00652701966F, -0.00921129622F, - 0.00280825584F, -0.0204265211F, -0.00880798325F, 0.00437320443F, - -0.00989791378F, -0.00871039182F, -0.0168512482F, 0.00523489F, - -0.00415994134F, 0.0182461701F, -0.0025259261F, -0.00168672763F, - 0.00267932913F, -0.00636704965F, 0.0117856571F, -0.0087577384F, 0.010606288F, - 0.010407228F, 0.00575941755F, 0.00695397891F, -0.00716215698F, - -0.000894990109F, 0.0050123292F, 0.0100905988F, -0.000350871822F, - 0.00984661374F, -0.0168875735F, -0.0151251033F, -0.0178020969F, - 0.00695164083F, 0.00621898752F, 0.00412923889F, -0.0189656261F, - 0.00222226139F, -0.00174576417F, -0.00850276F, 0.018713953F, -0.004781642F, - -0.0038268785F, -0.0168842953F, -0.0151102031F, 0.00147858507F, - 0.00443857582F, 0.00485204672F, -0.0250147954F, 0.0156844631F, - -0.00560600078F, 0.00588763645F, 0.00384009769F, 0.00553922728F, - 3.31690026E-5F, -0.0104848677F, -0.00803322904F, 0.0135929072F, - 0.000330169103F, 0.00184021122F, -0.00559773203F, 0.00138094008F, - -0.00420616334F, 0.00697650155F, -0.00317817787F, -0.00953131076F, - 0.0105379717F, 0.012843621F, -0.00571452826F, -0.00533787766F, - -0.00224266527F, -0.00297941617F, -0.00837262F, 0.00485406863F, - 0.0152300047F, 0.00833494868F, 0.000632289215F, 0.0195150841F, - 0.00107115775F, 0.00752895605F, 0.00863258447F, -0.00217398163F, - -0.00412153965F, -0.00211764313F, 0.0166778285F, -8.99806255E-5F, - -0.0107685262F, 0.0147488564F, -0.00039117533F, -0.00272219419F, - -0.00543562369F, -0.00110985944F, 0.0252258833F, 0.00555917714F, - -0.000623684435F, -0.0269727353F, -0.00930121262F, -0.00922976434F, - 0.0045151352F, -0.0192172043F, -0.0113992319F, -0.0113260765F, - -0.0156817324F, 0.00356612983F, -0.00211041328F, 0.00346035673F, - -0.0137144914F, 0.00412348844F, -0.00650316849F, -0.0150959035F, - -0.013423725F, -0.00453513069F, 0.0137146357F, -0.00788874F, -0.00507363956F, - 0.0107758055F, 0.00445105275F, 0.0111313444F, -0.0133231329F, -0.0043000537F, - -0.00229884195F, 0.0020027766F, -0.0018799277F, -0.0172025152F, - 0.00991803594F, 0.0090571763F, 0.00120707427F, 0.00596336275F, - -0.000771914551F, -0.00430014078F, 0.0161566585F, -0.00403440697F, - -0.00414772518F, 0.00531949382F, 0.00889405329F, 0.00337820663F, - 0.00692002242F, 0.00165435206F, 0.000542250345F, 0.000530569174F, - -0.0114463847F, -0.000959195429F, 0.0111928843F, 0.00158535119F, - -0.00125476299F, -0.0012435508F, -0.00455943542F, 0.00634017726F, - -5.4934495E-5F, -0.0156342778F, 0.00492569944F, 0.0141934119F, 0.0029827035F, - 0.00138941756F, 0.00393509958F, -0.00858817808F, -0.0102232564F, - 0.0138936043F, 0.000508076744F, 0.00803592242F, -0.0032858944F, - 0.00478073303F, -0.00739071658F, -0.0106847957F, 0.004178565F, 0.0170126837F, - 0.00787302107F, 0.00825369544F, 0.0121342754F, 0.00320299738F, - -0.00674389675F, -0.018786855F, -0.0135560138F, 0.00543280132F, - -0.00535759144F, 0.00058334769F, -0.0119195869F, 0.00543368654F, - 0.0166135523F, 0.00522514572F, -0.0054128971F, 0.0148944426F, -0.014904161F, - 0.00883759372F, -0.000135832772F, -0.017463075F, -0.00610778574F, - -0.0048750923F, 0.00352298701F, -0.0038454372F, -0.00390885584F, - 0.00797547773F, 0.0235591661F, 0.00760352192F, -0.0139812548F, - 0.00191931752F, -0.000644834887F, -0.0111069223F, -0.0117321508F, - 0.00322116143F, 0.00197867258F, -0.000452238513F, -0.00179770438F, - 0.00224966346F, -0.00839283876F, 0.0083800042F, -0.0177995544F, - -0.0112180179F, 0.00127261912F, -0.0102737825F, 0.0263704862F, - -0.00611322047F, -0.00790370442F, -0.0102106929F, 0.00764170522F, - -0.00689167716F, 0.000584224181F, 0.00542923948F, 0.0172641017F, - -0.00410407083F, -0.00392562104F, -0.0045923586F, 0.0136438766F, - -0.00980903953F, -0.0118488511F, 0.00504892366F, 0.00155910058F, - -0.00536002778F, -0.0126353074F, -0.00876031909F, 0.00714492798F, - 0.0100817308F, 0.0026804409F, 0.00934415311F, -0.00232692645F, - -0.000209254737F, -0.00182095903F, 0.0225556046F, -0.0194786154F, - -0.00354146259F, 0.00944626052F, -0.00817024335F, 0.00487400778F, - 0.00364905177F, -0.0212282762F, 0.00418459857F, 0.00558278523F, - -0.00701722829F, 0.00448272936F, -0.014049028F, 0.0170698334F, 0.0108463988F, - 0.002064734F, -0.00848087F, 0.00437762728F, -0.00316259824F, -0.00431506382F, - 0.00732367672F, 0.00763581134F, 0.000737635F, -0.00673030037F, - -0.00233319681F, -0.00113277824F, 0.00490186876F, -0.0101004951F, - 0.00952710863F, -0.0225475803F, -0.00473784702F, 0.0202956218F, - 0.00932700094F, -0.0138787096F, 0.00115895423F, -0.000139352458F, - -0.00422885455F, -0.00496593211F, 0.00408186764F, -0.00902679842F, - -0.00385980634F, 0.0105133578F, -0.010583017F, -0.0064626555F, - 0.00379732251F, 0.0019852065F, -0.0115050785F, -0.0125657329F, - 0.00305301882F, 0.00634077378F, -0.00479836809F, 0.0118848188F, - 0.00726987608F, 0.00237685954F, 0.0175220259F, -0.000423028978F, - 0.00682768924F, 0.0150867F, -0.00245288666F, 0.000243506394F, -0.0166690908F, - 0.00990786683F, -0.0042738039F, 0.0120262653F, -0.0104804225F, - -0.0124035226F, -0.0275355689F, 0.0102204429F, -0.00672037527F, - -0.00460383063F, -0.00662470842F, -0.0019339642F, -0.00122382538F, - -0.00300435582F, 0.00881277211F, 0.00836775452F, 0.001999259F, 0.0104205357F, - -0.0124581745F, -0.0136921816F, 0.0120583661F, 0.00699706748F, 0.0169401225F, - 0.00926026143F, -0.00227150321F, 0.027302105F, -0.0153029654F, - 0.00768210366F, 0.00516963797F, -0.00692212628F, -0.0127553409F, - -0.00318992347F, 0.00948326197F, -0.010970722F, -0.00826492626F, - -0.00389183126F, 0.00019452651F, -0.0140529582F, -0.014616522F, - -0.00230328599F, -0.0103513263F, -0.0242778622F, 0.00459263055F, - 0.00351461512F, -0.00732279243F, -0.00310411979F, -0.00212722109F, - -0.0082295062F, -0.00115836039F, 0.00602506753F, -0.0162301268F, - 0.00169991388F, -0.00800175F, 0.0153927915F, 0.00852301F, -0.0136589613F, - 0.0112619493F, -0.00604583509F, 0.015117323F, 0.00783880893F, 0.00376103655F, - -0.00708656572F, 0.0173549969F, 0.0089722611F, 0.00529837189F, 0.0122500472F, - -0.00239396165F, -0.00932079F, 0.0125222337F, 0.00188110466F, -0.0123222042F, - -0.000721048506F, -0.00966035F, -0.00664151367F, 0.00311125838F, - 0.00356845465F, 0.00195136876F, 0.00626451569F, 0.00561466254F, - -0.00660987152F, -0.00188257825F, 0.00815486629F, 0.0141962767F, - 0.000600708358F, -0.0061621503F, 0.0140799675F, 0.00568912877F, - -0.0035351431F, -0.0177772827F, 0.0125039127F, -0.00391286612F, - 0.0265181642F, 0.00330616767F, 5.87198519E-5F, -0.0105339689F, - 0.00786792953F, 0.00708658248F, 0.00766118523F, -0.0129554467F, - -0.0215025749F, 0.0101185F, -0.00181230519F, 0.00090701005F, 0.0130872214F, - 0.0185500216F, 0.00387762045F, 0.00576252257F, 0.00702280598F, - -0.0102955066F, -0.0114690512F, -0.00668389909F, 0.00186372F, -0.0132792713F, - -0.00176409504F, 0.00175968918F, 0.00750275841F, -0.00136101933F, - 0.0143772587F, 0.00469138287F, 0.0157444216F, -0.0028324991F, -0.016499877F, - -0.0226769391F, 0.000874732446F, 0.00154567976F, 0.00750235561F, - -0.00709134527F, -0.00419937773F, -0.00710605085F, -0.000219292939F, - 0.0128668658F, 0.00648168568F, 0.00387809519F, -0.0128303654F, 0.0126421163F, - 0.0106675681F, -0.00529567851F, -0.00514507201F, 0.00507838046F, - 0.0142530557F, -0.0021381746F, 0.00486253854F, -0.0106646782F, - -0.0106796715F, 0.00708219083F, -0.0111747989F, -0.00393915502F, - 0.0082894247F, 0.00915743131F, 0.0128074363F, 0.0172422025F, 0.0046474468F, - -0.00111204421F, -0.0029621881F, 0.00899396744F, -0.0215365961F, - 0.0129077928F, 0.0196581483F, -0.00204126304F, 0.00514254859F, - 0.00527328439F, 0.00206746627F, 0.00475637708F, 0.00483957538F, 0.014136469F, - 0.0105363457F, -0.00351258391F, 0.000188994425F, 0.00418553036F, - 0.00313488417F, 0.00827185903F, 0.0153794242F, 0.00989774242F, 0.0023188137F, - 0.0121700894F, -0.00394598581F, 0.00103700079F, -0.0134863257F, - 0.0194031447F, -0.0152784446F, 0.00461069029F, 0.00878936518F, - -0.00217195484F, -0.0124366814F, -0.0208170377F, 0.00187329028F, - -0.00540263718F, -0.00182845851F, 0.012774501F, -0.00708702486F, - -0.00595187256F, 0.0096734073F, -0.00166098226F, 0.00543353707F, - -0.00670272717F, -0.00282372627F, 0.00163086061F, 0.0145432316F, - -0.0169226434F, 0.00975646637F, -0.00105453958F, -0.00370989111F, - 0.0105158472F, 0.0205012821F, -0.00912642945F, -0.00761016179F, - -0.00438919477F, -0.00323584909F, 0.00860068295F, 0.00845016912F, - 0.0137891704F, 0.00688906247F, 0.00787764788F, -0.00214588479F, - 0.0218787603F, -0.00642350456F, -0.00544623286F, -0.0239385497F, - 0.00446868362F, 0.0142876934F, 0.00387828448F, -0.00181973295F, - -0.0217251126F, -0.00242053811F, -0.00235878187F, 0.00877515785F, - 0.00463345647F, -0.00951344613F, -0.000576776802F, -0.0041727717F, - 0.00986187905F, 0.00130074692F, -0.00156594359F, -0.0160563532F, - 0.00331769977F, -0.00225063832F, 0.00587817887F, -0.00967525F, - 0.00041260678F, -0.0148165915F, 0.0108841341F, -0.00889826659F, - 0.00481086178F, 0.00216621091F, 0.0135323936F, -0.00691479398F, - 0.00131278695F, -0.0146530401F, 0.00862300117F, 0.00929547381F, - -0.00274477527F, 0.00925984047F, -0.00405609887F, 0.00738123525F, - -0.0146516152F, 0.0215271544F, 0.00752709527F, 0.0262727663F, - -0.00570879411F, 0.00322515611F, 0.0150051611F, 0.00841968693F, - 0.0214257687F, 0.000875941594F, -0.0112364283F, 0.0151791554F, 0.0012350881F, - -0.00757343788F, 0.0206511281F, 0.00890238397F, 0.0162643045F, - -0.0153294336F, 0.00637699058F, -0.0185173601F, -0.00147230981F, - 0.000422275509F, 0.011439356F, -0.000706713588F, -0.0122678662F, - -0.00978877675F, -0.0149455834F, 0.0234498698F, 0.00367318466F, - 0.00582897896F, -0.00190672511F, 0.0119227944F, -0.00106993073F, - 0.0169418417F, 0.00865829457F, 0.00774514116F, 0.00053916336F, - -0.00682777073F, -0.00506050745F, -0.00867392309F, -0.00966896396F, - 0.0173875391F, -0.0013759248F, 0.0101712085F, -0.00350673171F, 0.00447758F, - 0.0152710211F, -0.00995817F, 0.000820802234F, -9.07622889E-5F, - -0.00337183313F, 0.021502329F, 0.0221973807F, 0.000874237914F, 0.0160213113F, - -0.0195219703F, 0.000344841101F, 0.000265245588F, -0.00657162769F, - 0.0116787078F, -0.00356171303F, -0.0026954331F, 0.00241318881F, - -0.0148066366F, -0.00731915189F, -0.00375870545F, 0.0189710688F, - -0.0061715371F, -0.00601034844F, 0.00230588461F, 0.0225145239F, - -0.00626428612F, 0.0173313059F, -0.0144765759F, 0.00535897072F, - -0.000911044423F, 0.00889771804F, -0.00734359398F, 0.0117038488F, - 0.00144408213F, -0.00493607158F, 0.00157728558F, -0.00406820374F, - -0.00129213289F, 0.0119679933F, -0.00459112041F, 0.0121955257F, - 0.0130164707F, 0.00177857943F, -0.00757314404F, 0.00498794857F, - -0.00842225272F, 0.0111213448F, 0.000325105299F, -0.00146416645F, - -0.00202933466F, 0.00626010355F, 0.00345560582F, -0.00697369268F, - 0.0118794115F, 0.0170074478F, -0.00204833085F, 0.00142825732F, - -0.0127931694F, 0.00308480952F, -0.00507093268F, -0.00930427667F, - 0.00439886469F, -0.010808425F, 0.005265655F, -0.000281448534F, 0.00753787F, - -0.0229626764F, -0.00547367241F, -0.0199728198F, 0.00500181923F, - 0.0179459341F, -0.0184425861F, 0.019176783F, 0.00135438202F, 0.00464304024F, - 0.00064236566F, -0.00218744669F, -0.00404076651F, 0.00876206905F, - 0.00116835977F, -0.0074135703F, 0.00343641057F, 0.00765306875F, - 0.0063964962F, 0.0046888194F, 0.0139482869F, -0.000885962276F, - 0.00233074371F, 0.000240079666F, 0.00586876366F, 0.00297781569F, - 0.00148350536F, 0.0118695786F, -0.00713214697F, -0.00516754715F, - 0.0130746448F, -0.00705271633F, 0.0239783358F, -0.00230762595F, - 0.0087408619F, -0.0147202555F, 0.0130211404F, 0.00298214681F, 0.00542443F, - 0.0024769872F, -0.010710462F, -0.0119140185F, 0.0204094443F, 0.00507724192F, - 0.0063238726F, 0.00109564327F, 0.0144169247F, 0.00565544656F, 0.0180374552F, - -0.00710317539F, -0.00546416687F, 0.000935927732F, 0.00683290465F, - -0.00281592016F, 0.0100848442F, 0.0111807352F, -0.0256743F, 0.00157442794F, - 0.0199836828F, 0.00921304245F, 0.00015893497F, 0.0152443498F, - 0.000747716927F, -0.00763794594F, 0.000176238507F, -0.00328368833F, - -0.00558896735F, -0.0231350102F, 0.00697534671F, 0.00384631637F, - -0.00169114047F, 0.0036446536F, -0.00407474069F, -0.00224542711F, - 0.00235861028F, -0.0069422056F, 0.00226881565F, -0.00175556471F, - -0.00505492603F, 0.00220462028F, -0.00192296703F, -0.0035853961F, - 0.00206437637F, -0.00597772934F, -0.00777177466F, -0.0049949633F, - 0.0108225858F, 0.0129908677F, -0.00961234141F, -0.0133788455F, - -0.00124241342F, -0.00944866613F, 0.010350178F, -0.016637098F, - -0.0044473568F, 0.00182900263F, 0.0147243468F, -0.0174043067F, 0.0116276694F, - 0.00581878051F, 0.000183504904F, 0.00224284781F, 0.000856567873F, - 0.0107121402F, -0.00602018228F, -0.00202941406F, 0.0223372895F, - 0.00688974699F, -0.00277207047F, 0.00654081535F, 0.0164657068F, - 0.0117136864F, -0.00799791887F, -0.0125368964F, 0.0160613898F, - 0.000448971085F, 0.0163604803F, 0.0137057686F, 0.00698015327F, - -0.0156992618F, 0.00618388783F, -0.0113795493F, 0.0153492866F, - -0.00590965059F, 0.00535797747F, -0.00768240634F, 0.00628112257F, - -0.0205016099F, 0.00223738072F, -0.00457339035F, -0.0208691917F, - 0.0226231441F, 0.00666576577F, -1.00308989E-6F, 0.0164588559F, 0.0137247592F, - 0.00750207156F, -0.0154170869F, -0.000857947103F, 0.0120896334F, - 0.00384086929F, 0.0168010052F, 0.0233593F, -0.00133658957F, 0.00563294301F, - 0.00174153235F, -0.00480540562F, 0.0130929388F, 0.011534106F, 0.00262605189F, - -0.0137576321F, 0.00305267749F, -0.00321265543F, 0.00128236506F, - 0.00232111779F, -0.00207065535F, 0.000532972044F, 0.0062635839F, - 0.0118069053F, -0.00536757335F, -0.0129006868F, 0.0027662057F, - 0.00950991455F, 0.00739729684F, 0.0121117309F, -0.00480746944F, - -0.00639968179F, -0.001345178F, -0.0158152152F, -0.00998322945F, - 0.00209698803F, 0.0125828655F, -0.00601130677F, 0.00683968747F, - 0.0166311376F, 0.0152126458F, 0.00859399512F, -0.0129223755F, - -0.00301290886F, 0.0140304565F, 0.00543807F, 0.011046188F, -0.00358218397F, - -0.00144206453F, 0.00968406163F, -0.00639852136F, -0.000320784166F, - 0.00461742934F, -0.00122891844F, 0.00119902263F, -0.00502912654F, - -0.00284258369F, -0.00884731859F, 0.00575427059F, 0.0197856147F, - -0.00168935349F, 0.00697037252F, -0.0107089654F, -0.0026775708F, - -0.00911968388F, -0.00446293456F, 0.0108505664F, 0.0086394893F, - -0.0126994317F, -0.00155536109F, -0.00545614F, -0.00731411809F, - -0.000396806F, 0.01404987F, -0.00389184826F, -0.000865663751F, - 0.000317800062F, 0.0180534422F, 0.0110834464F, 0.00749593368F, - 0.000820417539F, 0.013739625F, 0.0226392262F, -0.00471897889F, 0.0192328952F, - 0.0256245919F, 0.0116846208F, 0.000268897653F, -0.0034349591F, - -0.0056886659F, -0.00642415F, -0.00208584243F, -0.00355208502F, - 0.00877610873F, -0.00647047209F, -0.00485182414F, 0.0180126F, 0.00544101885F, - -0.00587151898F, -0.00723785255F, 0.00122117542F, 0.00969149F, - -0.00165493076F, -0.00268644746F, -0.000239340457F, -0.0052269958F, - -0.0105386218F, -0.0100568598F, -0.00756009063F, 0.00980897341F, - 0.00671534706F, 0.00321874348F, -0.00898812059F, 0.00479564909F, - -0.0212505981F, -0.00533731561F, 0.00353827933F, 0.0106284432F, - 0.0129781161F, 0.00621509412F, -0.0138403224F, -0.00368756056F, - -0.0113152629F, -0.0167601127F, 0.00734598516F, -0.00142786105F, - -0.000237633314F, -0.00815983303F, -0.0110006891F, 0.000267833093F, - -0.0129344463F, -0.000552968937F, 0.0125643807F, -0.0157173239F, - -0.00522637879F, -0.00458896207F, 0.00875800196F, -0.0100845108F, - 0.0120955333F, -0.0216257758F, -0.0172171015F, -0.0141174346F, - 0.00554721151F, 0.000139856755F, -0.00536454469F, 0.00332786865F, - -0.00795496535F, -0.0117887864F, -0.00277909054F, 0.00701352069F, - 0.0126043782F, 0.00700738374F, -0.00820393581F, 0.00203278F, 0.00014005987F, - 0.00779878115F, -0.00276490906F, -0.00892424304F, 0.00794886F, 0.0135226864F, - -0.00172770477F, 0.00131190405F, -0.0263915733F, 0.0014153918F, - 0.00116773963F, 0.00559770176F, -0.0141860731F, -0.0174877774F, - 0.00419466384F, 0.0194705296F, 0.00114011229F, 0.00367679051F, - 0.00214730017F, 0.00231423019F, 0.0040546665F, -0.00593537744F, - -0.0153126791F, -0.00549441436F, -0.0035925305F, 0.00766265F, 0.00723741576F, - -0.0180796478F, 0.00221153023F, 0.000436933391F, 0.0100874687F, - -0.00400444586F, -0.00420347694F, 0.000467110629F, 0.0197182056F, - 0.0121507887F, 0.00143929897F, -0.0122719267F, -0.00219250191F, - -0.00662870426F, -0.00712499162F, 0.00806172844F, -0.0108175706F, - -0.00700499257F, -0.00442173285F, -0.000635781325F, 0.0135884732F, - 0.00646243524F, 0.00565448683F, -0.0146895191F, 0.0056438F, -0.0107559673F, - -0.00895153824F, -0.00939690135F, 0.00295222946F, -0.00477735698F, - 0.00979712512F, 0.00294793653F, 0.0156152481F, 0.00609727437F, - -0.00161779521F, -0.0155658908F, -0.000514720159F, 0.00824002735F, - -0.0147474334F, 0.0130677894F, -0.0159833804F, 0.0182400197F, 0.00304676709F, - -0.00547132455F, 0.0100286072F, -0.000149505635F, -0.0110690007F, - 0.00832342636F, -0.0066658915F, -0.0189902987F, -0.0181928538F, - 0.00158324244F, 0.00546708098F, -0.00179060327F, 0.000564992079F, - -0.00225007348F, 0.0014776371F, -0.000272876379F, -0.00185946166F, - 0.0106523838F, 0.00226577069F, -0.0192445889F, 0.00586030865F, - 0.000889645773F, 0.00484591583F, 0.0033567464F, -0.0118313134F, - 0.00426173722F, -0.0122202942F, -0.000357529672F, -0.0138813686F, - 0.00543089F, 0.00796753634F, -0.00250412337F, 0.00719918404F, - -0.00265865307F, 0.00209603878F, 0.0197576173F, -0.00961513F, 0.00620503118F, - 0.00401895493F, -0.00113026728F, -0.0100073721F, 0.00642932812F, - 0.00786176603F, 0.00466219615F, -0.00362189882F, 0.00679586269F, - -0.00667532114F, 0.00285012834F, 0.0101997908F, 0.00768211624F, - 0.0154248644F, -0.00186559733F, 0.0136893876F, -0.00338393589F, - 0.00047067285F, -0.0241449159F, -0.00621862151F, 0.000500132213F, - 0.0087774666F, -0.0185059067F, 0.0131750721F, 0.0041934019F, 0.000897026097F, - -0.0075790477F, 0.00522451755F, 0.0102104638F, 0.00952984672F, - 0.00218638382F, -0.00232133549F, -0.0097139189F, -0.00517715607F, - -0.0070425244F, -0.0102603519F, 0.0134201879F, -0.000135152324F, - 0.0105697419F, -0.00548682734F, -0.00463421224F, 0.00196428085F, - -0.00125038228F, -0.000963798F, 0.0141267236F, -0.00776024908F, - 0.00323362928F, -0.0265762061F, -0.00718485657F, -0.00465679914F, - 0.010600701F, -0.0132718328F, -0.0010741076F, 0.00501013501F, 0.0213488564F, - 0.0183989F, -0.004878846F, 0.0077244658F, 0.0094911363F, 0.0138052572F, - -0.010687558F, -0.000175488589F, 0.0053929016F, -0.00685124472F, - 0.010017097F, -0.00143342151F, 0.0140992468F, -0.00690617459F, - -0.00272436743F, -0.00286154868F, -0.0144093223F, -0.00594181195F, - 0.000951610913F, -0.00525948917F, 0.00140388077F, -0.00151512958F, - 0.00679057185F, -0.0032649145F, 0.0124787539F, 0.00755634112F, 0.00870586F, - -0.00874228403F, 0.0176365059F, 7.72561689E-5F, 0.0105169639F, - 0.00566625642F, 0.0133054378F, 0.00142F, -0.00302842725F, -0.0117613552F, - -0.00886577927F, 0.00796729792F, -0.00449779304F, 0.0135490391F, - 0.0251696389F, -0.0112350415F, -0.00921654329F, 0.000642390456F, - -0.00150485116F, 0.00658055069F, 0.00829790253F, 0.00624471484F, - -0.0112948902F, -0.00691019464F, 0.0103419749F, -0.006201474F, 0.0114321718F, - -0.000633595F, -0.00852219481F, -0.00140452478F, -0.0173866227F, - 0.00034380937F, -0.0137650967F, -0.00019928698F, -0.00508752745F, - -0.00253999233F, 0.0159112308F, 0.00600469764F, 0.00133258861F, - 0.0190213639F, 0.0189842302F, -0.00300839567F, -0.0017230052F, - -0.00721502304F, -0.00297798379F, 0.008497837F, 0.0200907234F, - -0.000896832964F, -0.00157276448F, -0.00431927806F, -0.00682808366F, - 0.00737016695F, -0.00855235383F, 0.0158315953F, 0.0095887566F, - 0.00175514014F, 0.00201586401F, -0.00617988687F, -0.00780952442F, - -0.0227075331F, -0.00793551095F, 0.00288489205F, -0.00239047129F, - -0.00308226165F, 0.00233057793F, -0.0136082005F, 0.0132889058F, - 0.0145422742F, -0.0044742208F, 0.00633772463F, 0.0040901606F, 0.00784097891F, - 0.00443400536F, 0.00141808274F, 0.00483276229F, 0.00483454857F, - 0.0099588437F, -0.00781986583F, 0.0101205911F, -0.00496620778F, - 0.00782432314F, 0.0243983567F, -0.00115413545F, 0.0141633786F, - -0.00220273412F, -0.00475763669F, 0.00885377359F, 0.00339664356F, - 0.000145779093F, -0.0128244152F, 0.00217738654F, 0.0145478724F, - 0.00797725841F, -0.0137341255F, 0.0129559347F, 0.0103252614F, 0.00395335257F, - -0.00480270712F, 0.00171433482F, -0.00480029872F, -0.0192683935F, - 0.000664213439F, 0.00274802651F, -0.0130656715F, -0.000348084228F, - -0.00383883761F, 0.00758592784F, 0.00853148289F, -0.00242689601F, - -0.0116305873F, -0.00503032887F, -0.0167230032F, 0.00604980858F, - 0.00868253876F, -0.0175658502F, 0.00662974827F, 0.00889776275F, - 0.0086098155F, 0.010306987F, -0.00204127631F, -0.00876646768F, - 0.00156671088F, 0.00434709247F, 0.000632596144F, 0.00667848531F, - -0.00344623369F, 0.00948697794F, -0.00434167404F, 0.0141145568F, - 0.012288399F, 0.00863228086F, -0.00597370323F, 0.0189655703F, 0.00325072603F, - -0.00223898934F, -0.00791371055F, 0.00113320956F, 0.00324181584F, - -0.00150972127F, 0.0337718502F, 0.00351938745F, -0.0183746926F, - 0.0129964044F, 0.00471147895F, 0.0014539262F, 0.015264255F, -0.00333484332F, - 0.00687716389F, 0.0125387814F, 0.0157226883F, -0.00468483195F, - 0.00362441246F, 0.00212353282F, 0.000625209941F, 0.00255601364F, - -0.0123865204F, 0.00344361039F, -0.0180617291F, -0.00933889672F, - -0.00411702832F, 0.010718233F, -0.0105085541F, 0.00541985594F, 0.0017466771F, - 0.00395021169F, 0.010020081F, -0.00879753F, 0.014589645F, 0.00617622351F, - -0.00584853766F, -0.00124998041F, -0.0154773127F, 0.00530868256F, - 0.00949571934F, 0.00832209736F, 0.00992349535F, -0.00924173F, -0.0031828275F, - -0.00227365317F, 0.0156142255F, -0.0141566694F, -0.00727446843F, - -0.011160871F, -0.00556849875F, 0.00699830381F, 0.0171627942F, 0.0041848137F, - 0.0097577814F, 0.0112146204F, 0.0073441132F, 0.00207859185F, 0.0104126772F, - 0.00365971774F, -0.00367357302F, -0.008589454F, 0.000665677071F, - 0.0103673125F, 0.00201239297F, 0.013907712F, 0.0123577332F, -0.00438297866F, - -0.0126456339F, -0.0126655363F, -0.0116939954F, 0.00967906136F, -0.00107497F, - -0.0136572449F, 0.0119120134F, 0.00742568029F, -0.010726124F, 0.011939263F, - 0.00210086536F, 0.00195093942F, -0.00243404531F, -0.0115157058F, - -0.0166950151F, 0.00780348666F, -0.00333812158F, 0.00146695448F, - -0.00120518263F, -0.00654262956F, 0.0295465086F, 0.00281020324F, - 0.0122888181F, 0.009994152F, -0.000452074339F, 0.0066154236F, 0.0106507149F, - 0.00102218729F, -0.00433391426F, 0.0269801561F, 0.00421748217F, 0.012652535F, - -0.00454079732F, -0.00226164027F, -0.00941517577F, -0.00127661356F, - 0.00435833307F, 0.000416211027F, -0.00689539174F, -0.00285142474F, - 0.00845257472F, -0.00948996935F, -0.00821019895F, 0.00623022858F, - -0.00226990785F, -0.00161620311F, -0.015928559F, 0.00423257425F, - 0.00154192629F, 0.0013453901F, 0.00891457591F, -0.00681490125F, - 0.0134295467F, 0.00612843689F, -0.00350547652F, 0.00851635356F, - -0.000977404F, 0.0015265086F, -0.0122977663F, -0.00457504625F, - 0.00178995158F, 0.00607001968F, 0.00506193656F, 0.000917614845F, - 0.00795888435F, -0.00029257714F, 0.00597347F, -0.00658135489F, - 0.000213544306F, -0.00178523664F, 0.00435392279F, -0.0113189528F, - 0.00854449347F, 0.00581649737F, -0.00153058337F, 0.00172312069F, - -0.00966837909F, 0.000250486133F, 0.00336196157F, 0.0156993568F, - -0.00138298434F, -0.021806F, 0.00723384F, -0.00384783954F, 0.0112330019F, - -0.0133611364F, 0.0113068568F, -0.00732754357F, -0.000116967531F, - 0.00633947412F, 0.000232395716F, -0.0050765723F, 0.00458636507F, - -0.0111213438F, 0.00846799091F, -0.000827312935F, 0.00255745882F, - -0.0110976668F, 0.0103579191F, -0.00401385035F, 0.013302614F, - -0.00521571701F, 0.0131697059F, -0.00964360218F, -0.00608558115F, - -0.0198019277F, 0.016791353F, -0.00280380086F, -0.00532236276F, - 0.00343763689F, 0.00256545469F, -0.000708374253F, -0.00797464792F, - -0.0022024361F, -0.00141579029F, -0.00110536523F, -0.0120040327F, - -0.0150788361F, -0.0192051232F, 0.00934282877F, -0.00117540243F, - 0.0202903431F, 0.0159976482F, 0.00218333583F, -0.0177168883F, -0.0001240407F, - 0.00567079848F, -0.000427828927F, -0.0129982457F, -0.0215838738F, - 0.00386219868F, 0.00755864708F, 0.0129300095F, -0.00714114355F, - -0.00499054929F, 0.00714133307F, 0.00222565397F, -0.01496989F, - 0.00180207833F, -0.00246120617F, 0.00525902212F, -0.00710250251F, - -0.00109356747F, 0.00567638036F, 0.0056202F, -0.00751148211F, -0.0131029217F, - 0.00170734152F, -0.0109648788F, -0.00207965658F, 0.00669308472F, - -0.00254235486F, 0.0063036643F, -5.37243504E-5F, -0.0022289108F, - 0.00353248371F, -0.00538313854F, -0.0228785761F, 0.00389051624F, - -0.0153404512F, 0.00914467592F, 0.00678737974F, -0.00890069F, - -0.00513592456F, -0.018146323F, 0.00834225211F, 0.00811461918F, - -0.00710000051F, 0.00553693203F, -0.00953912176F, 0.00264964788F, - -0.00807018F, 0.0104424637F, 0.011345732F, -0.0131628728F, -0.00446972391F, - -0.00848243572F, -0.00755977212F, -0.00604765909F, -0.0119552193F, - -0.00682115648F, 0.00852127653F, -0.00290098763F, 0.000809838937F, - 0.00769298803F, -0.0132649876F, -0.00795695838F, 0.016569024F, -0.013001767F, - -0.0166473668F, -0.0108860089F, -0.0180743951F, 0.0181849338F, 0.0089702541F, - -0.0111570051F, -0.0115624387F, 0.000761079253F, -0.00290306611F, - 0.000440905918F, -0.00436116F, 0.00575589202F, -0.00102549361F, - -0.00304564578F, 0.0119179236F, 0.000940301223F, 0.0123161823F, - 0.00130857085F, -0.00124519493F, 0.00181753677F, -0.00866962131F, - 0.000525049516F, 0.00684928661F, 0.00355984224F, -0.0101310844F, - 0.0151074175F, 0.00280115F, -0.00196747808F, 0.0040151272F, -0.00337722758F, - -0.00973632652F, -0.0135939727F, 0.00588573236F, -0.00416166475F, - 0.00423893891F, -0.00651386892F, -0.0112858275F, -0.00378903816F, - 0.00239182194F, -0.000198620211F, 0.00741859572F, -0.0078807F, - -0.00360291428F, -0.00666509196F, 0.00406212965F, -0.00347621343F, - 0.0207479578F, -0.00163333421F, 0.0168229472F, -0.00146916148F, - 0.00953023601F, 0.00535395229F, 0.0072015482F, -0.00191329361F, - -0.0095042875F, -0.00109248608F, -0.0126695326F, 0.00624358468F, - -0.0010088156F, -0.0046839267F, 0.00674802577F, 0.00256895064F, - -0.00342768617F, -0.00443097763F, -0.018667927F, 0.00198743911F, - 0.0114505701F, -0.00202693022F, -0.00552020594F, -0.00978672411F, - 0.0158741642F, -0.00472647976F, -0.0040650228F, 0.0253379159F, - 0.00202770019F, -0.0178117733F, -0.00668567419F, 0.00114653865F, - -0.00819705334F, 0.00763352774F, -0.000159726435F, -0.0112460535F, - -0.0152113875F, -0.0148413219F, 0.00788255129F, 0.0111742085F, - -0.000555060862F, -0.0038688F, -0.00505510345F, -0.00669557229F, - 0.00354181067F, 0.00515723089F, 0.0142773446F, 0.000399482116F, - -0.0135546885F, 0.00711636199F, 0.0069786692F, 0.00225902116F, 0.0166707486F, - 0.0056339344F, -0.00831161533F, -0.00359760481F, -0.0113220131F, - 0.00347422413F, 0.0164175462F, 0.00734686619F, -0.00198344747F, -0.00819997F, - -0.00544555811F, 0.00889588613F, 0.00136246113F, 0.000733726716F, - 0.0094101131F, 0.00548253907F, 0.0113815498F, 0.0157960206F, -0.00638591917F, - -0.00690890942F, -0.0146086309F, -0.0345816649F, 0.00189230812F, - -0.0053503057F, 0.0128284311F, -0.00934793241F, -0.0023884126F, - 3.28277201E-5F, 0.0159626156F, 0.0113777295F, -0.0275663603F, -0.013937545F, - 0.00204462186F, -0.00376782916F, 0.00446758559F, -0.00642441772F, - -0.00501086F, 0.00333432318F, 0.00373204309F, -0.0103956424F, 0.0236769803F, - -0.0241456721F, 0.00356958F, -0.0115835937F, 0.00276068714F, 0.0111674601F, - -0.0192783959F, -0.0208532587F, -0.00323717971F, 0.000380477984F, - 0.00187875F, -0.0141012724F, -0.0191332456F, 0.0115728667F, -0.0208941028F, - 0.00287586497F, 0.00773563562F, -0.0059558F, -0.00327347824F, - -0.00939998403F, 0.0206416026F, 0.0113679674F, -0.00201552571F, - -0.0114365071F, 0.00833266F, -0.00721985195F, 0.00376210478F, - -0.00217640097F, -0.00635823049F, -0.00122120022F, -0.00259408052F, - 0.0106398677F, -0.0169788972F, -0.00906841829F, -0.0121080047F, - 0.00361741078F, -0.00329536526F, -0.000159341173F, -0.00533939619F, - -0.013145132F, -0.0158593766F, 0.0050734845F, 0.0158524029F, 0.00854714122F, - 0.00663356436F, 0.00464871386F, -0.0094432272F, -0.000921259809F, - -0.0142620914F, -0.00271608797F, 0.000662594219F, -0.0171645321F, - -0.0218553804F, -0.00385276787F, -0.0048713726F, 0.000921210682F, - 0.00871089846F, 0.00410066033F, 0.00223968434F, -0.00347325765F, - 0.012757862F, 0.000957291864F, -0.0103272442F, -0.00396108674F, - -0.0036119849F, -0.0108738085F, 0.00785575621F, -0.00810256694F, - 0.020623263F, -0.00753856264F, 0.0119894296F, 0.000243510614F, - -0.0135001391F, 0.00571625493F, 0.00975440722F, -0.0125929108F, - -0.0153206466F, -0.00788856298F, 0.00760611752F, 0.00431825127F, - -0.0108927488F, -0.000696842442F, -0.00594778825F, -0.00726814568F, - -0.00184120692F, 0.00228761928F, 0.0145703405F, 0.00341424835F, - 0.00331176957F, -0.00311701838F, -0.0205553453F, 0.00859583728F, - -0.0119651277F, 0.00627199467F, 0.00292584137F, 0.00487632863F, - 0.00294795469F, -0.0241675656F, 0.0042871912F, 0.00425952673F, - 0.00326486188F, -0.0170134287F, 0.00659963069F, 0.0117446063F, - 0.00538912462F, 0.0110721979F, -0.000816377054F, 0.0129536772F, - 0.00884946808F, 0.021326039F, 0.00956957787F, -0.00403446F, 0.00528971571F, - 0.0179875344F, -0.0147258928F, -0.00749744102F, 0.012265401F, 0.00937871449F, - 0.008902F, -0.0181328934F, 0.00789270364F, -0.00259628682F, 0.012850591F, - 0.00089939707F, 0.00325236912F, 0.00230855029F, 0.011223915F, 0.0234452877F, - -0.0033954496F, 0.00839938596F, -0.00692567602F, 0.0234753918F, - 0.00526813092F, -0.00802161824F, -0.00689945696F, -0.00542818429F, - 0.00290423352F, -0.016557714F, 0.00631535659F, 0.00124191446F, - -0.0117795411F, 0.00532737933F, 0.0119301071F, 0.00412578089F, 0.0101500954F, - 0.0058891424F, 0.00327870483F, 0.0041856193F, 0.0112876864F, - -0.000107958134F, -0.00787384156F, 0.0184502471F, 0.0105305109F, - 0.00352828111F, 0.00600020587F, 0.0140895043F, -0.0012121934F, 0.012700188F, - 0.00720944162F, 0.00754961651F, 0.00321319024F, -0.00977098849F, - 0.0108835837F, -0.00195847056F, 0.0165228117F, 0.00973930117F, 0.0024696046F, - -0.0176288746F, -0.0144112352F, 0.00779589778F, -0.00808792841F, - -0.0157310367F, -0.00917431246F, 0.0134970509F, -0.000825096387F, - -0.000351441646F, -0.00543968054F, -0.00152801408F, 0.0169669911F, - 0.00493076F, 0.0250571929F, 0.00157654285F, -0.00232889084F, -0.00608096644F, - 0.00376029196F, 0.00918992143F, -0.0113605168F, 0.00431295065F, - -0.0024823444F, -0.0101839174F, 0.00424937159F, -0.000682205369F, - 0.00502501801F, -0.00222869846F, -0.00931847282F, 0.00552563276F, - -0.0100121573F, 0.00543143F, 0.0102463914F, -0.00349823106F, 0.00203573029F, - -0.00095770054F, 0.00438988348F, -0.00851642713F, 0.00400156062F, - -0.000571166864F, 0.00842263456F, 0.0158877298F, 0.00859775394F, - 0.00219142879F, -0.00775005668F, -0.00195672456F, 0.0112181865F, - 0.00320242206F, 0.00692659616F, 0.0148832211F, -0.00968052261F, - 0.00560722593F, 0.00764919585F, 0.011464159F, 0.00151121616F, 0.0234574694F, - 0.00469951145F, -0.0135564823F, 0.00311249215F, 0.0269924365F, - 0.00278710434F, -0.00256309938F, 0.00234484719F, 0.00750575308F, - -0.0112101464F, 0.0249249581F, 0.00829282403F, 0.000510474318F, - -0.0131260483F, -0.00186798989F, -0.00163668429F, 0.00736411521F, - -0.00641128421F, 0.00791824423F, 0.0206031892F, 0.00470543606F, - -0.000570702774F, 0.00781893916F, -0.0111147733F, -0.0158019513F, - -0.00215215213F, -0.00847981125F, 0.000466214522F, 0.0117657613F, - 0.008959217F, -0.00656455057F, 0.00435224501F, 0.000886913447F, - 0.00112783117F, -0.0210322049F, 0.0180339105F, 0.0132928826F, 0.00303349504F, - 0.00449428F, -0.00226442213F, -0.0103637343F, -0.00767240906F, - 0.00149470684F, -0.0216211826F, -0.00788005F, -0.00973185338F, - 0.00294343987F, -0.00859830528F, 0.0104972245F, -0.012592678F, - 0.00164030981F, 0.00186579546F, 0.000381098973F, 0.00236350112F, - 0.000588214083F, -0.00816726219F, -0.0013302553F, 0.0157932732F, - 0.00538432458F, -0.0186835397F, -0.00246785139F, -0.00705587491F, - 0.0141236866F, -0.0104817739F, -0.0203260574F, -0.000234947162F, - 0.0168130118F, -0.0176165793F, -0.00452081766F, 0.00317406445F, - 0.00541251106F, -0.00701926881F, 0.0227746908F, 0.00349711324F, - 0.0112817083F, -0.00971068349F, -0.003312357F, 0.00367178652F, 0.0086304443F, - -0.000821202178F, 0.00321304519F, -0.00436965376F, -0.00831450336F, - 0.024647614F, -0.00513101183F, -0.000937797F, -0.00360837672F, - -0.00495177833F, -0.00430343952F, -7.31522814E-5F, 0.000229167286F, - -0.0085123172F, 0.00762281613F, 0.00806368515F, -0.00711276382F, - -0.00719856843F, -0.00705515407F, 0.0177219361F, 0.00152538985F, - -0.0212311242F, 0.0120868627F, 0.00237748679F, -0.000447245169F, - -0.0031524743F, 0.00629359065F, 0.00599464215F, -0.00408114493F, - 0.00136955397F, -0.00637608534F, 0.0209051631F, -0.00951557141F, - -0.0167366341F, 0.0156591088F, 0.0118102953F, -0.00601092633F, - -0.00279439613F, 0.0103328945F, -0.0104154404F, 0.0114097232F, - 0.00553817907F, 0.00175454246F, -0.00255793077F, -0.012192375F, - 0.0189580452F, -0.00114167633F, 0.000445214F, -0.0060091326F, 0.00534940697F, - -0.00355942431F, 0.00468874956F, -0.000954106217F, -0.0117860595F, - -0.00306569855F, -0.0148360655F, -0.0132013531F, 0.00712587684F, - -0.000848451629F, 0.0139780166F, -0.00231514149F, 0.000140901655F, - -0.00326449727F, -0.00673917588F, -0.00337466877F, -0.00527123129F, - 0.000580308959F, -0.00968451705F, -0.00113394414F, -0.00862896256F, - -0.00328434538F, -0.0133786686F, 0.012872396F, 0.0149467271F, 0.0235220529F, - 0.00608011615F, 0.000550293713F, 0.00318068219F, 0.00176557992F, - -0.00352241751F, -0.00600494258F, -0.00221693772F, -0.00487688556F, - -0.00333444658F, -0.00730980607F, -0.011229638F, -0.00141383591F, - -0.0207188F, 7.67004749E-5F, 0.015902156F, -0.00747381058F, 0.0129776774F, - -0.00501021277F, 0.0155439461F, 0.00150566967F, -0.00896579772F, - -0.00411397079F, -0.00907518622F, 0.00787045155F, -0.00545624131F, - 0.00471493881F, -0.0105481017F, 0.00587800704F, 0.0165717658F, - 0.000436744245F, 0.00749999145F, -0.00827094447F, -0.000598283368F, - 0.00690896343F, -0.0110465316F, 0.00293759373F, 0.00174475554F, - -0.00800258853F, -0.00122361712F, 0.00792444125F, -0.00181599613F, - 0.00241750036F, -0.011409089F, 0.00186865951F, 0.00826357398F, 0.0180597324F, - 0.00249830261F, -0.00708337268F, 0.00589827076F, 0.00262166373F, - -0.00572770415F, 0.00838268455F, -0.00654780585F, 0.0132102035F, - -0.00337559287F, 0.00673822453F, 0.00506955525F, -0.000492708874F, - 0.00695776287F, 0.011001003F, 0.0213624369F, -0.000805990712F, - -0.00982902572F, 0.0195651352F, 0.00275750272F, -0.0159245059F, - -0.00529155182F, 0.00172064151F, -0.0109545421F, 0.0105829146F, - 0.00378195429F, -0.00136133353F, 0.00030265993F, 0.00468951743F, - 0.00443057436F, 0.00860723294F, 0.000251860241F, 0.0115625337F, - -0.00728502916F, 0.000535595464F, 0.0158405527F, 0.00324309897F, - -0.0107265878F, 0.000785720651F, -0.0126243765F, -0.019800812F, - -0.00207910594F, 0.00456749089F, -0.00500505837F, -0.000230387435F, - 0.00418270146F, -0.00180079357F, -0.00434310827F, 0.00801959354F, - 0.0227937195F, 0.0075202859F, -0.0208794978F, 0.00932995F, 0.00925591309F, - 0.00535601517F, 0.000351842F, -0.00859489478F, 0.0105685741F, - -0.00810977444F, 0.00621818565F, -0.0136741335F, 0.00323371449F, - -0.00452999584F, 0.0167075749F, 0.00325924484F, -0.000642653089F, - 0.00270519708F, 0.0083504878F, 0.00768941827F, -0.000327922171F, - -0.00746629946F, -0.000803879113F, -0.00359966652F, -0.0112468917F, - -0.00942329876F, -0.0276556946F, 0.000995377311F, 0.00363496225F, - -0.0102762775F, -0.00902675092F, -0.0013054444F, 0.0225761104F, - 0.0110333646F, -0.00754219107F, 0.00295709516F, 0.000878915F, 0.0124150347F, - 0.00940719713F, 0.0104784649F, -0.0026099626F, -0.0117678642F, - -0.00463008229F, 0.00339127425F, -0.00538824731F, -0.0097101368F, - 0.00133384042F, -0.00934070256F, -0.0068139038F, -0.0192953218F, - 0.00541933952F, 0.0163918342F, 0.00623727078F, -0.0092805773F, - 0.00388243911F, -0.00740240235F, -0.0009742619F, 0.00425612042F, - -0.000437888579F, -0.0164271407F, 0.00269205985F, -0.00145720236F, - -0.00169473584F, -0.00630907575F, 0.00910707F, -0.0191722754F, - -0.0138195679F, -0.00299300021F, -0.0100649474F, 0.00472485693F, - -0.00330508384F, 0.00790030323F, 0.00104777562F, 1.32843925E-5F, - -4.00988356E-5F, 0.00487796171F, -0.007592496F, 0.00124558981F, - -0.00240434986F, -0.0041391016F, 0.0120956441F, -0.0161552653F, - -0.00247929548F, -0.000814464F, -0.00880133826F, 0.005742223F, - 0.00178333011F, 0.00834449194F, 0.00132851512F, -0.00577122904F, - 0.00227052323F, -0.0206960924F, -0.00313799898F, -0.00833046529F, - 0.00493967161F, 0.00605F, -0.00772083458F, 0.00231521227F, 0.00682022655F, - -0.00169077446F, -0.00218755705F, -0.016577784F, -0.0134209478F, - 0.000459807285F, -0.00535181398F, 0.0153769841F, 0.0172459967F, - 0.0172588825F, -0.00249337591F, 0.000684936473F, 0.00466918945F, - 0.0112184668F, -0.00358094624F, 6.32388765E-5F, 0.0028561966F, -0.01492971F, - -0.000703185506F, 0.0152782807F, -0.0131124184F, -0.00316197379F, - 0.0183474422F, 0.00994272716F, 0.00341621344F, -0.0140711516F, 0.0191914421F, - -0.00698425155F, 0.00263903F, 0.00935927872F, 0.00781268906F, - -0.00382323819F, 0.000578327454F, -0.00797486F, -0.00909249F, - -0.00336699164F, -0.00801629107F, 0.0152703645F, 0.00193242193F, - -0.00452980911F, 0.0200263932F, -0.00867690146F, 0.0104449559F, - -0.0144314636F, -0.00791813061F, -0.00101815921F, 0.00166580302F, - -0.00110084075F, -0.00260720099F, 0.00864494F, -0.00804343F, 0.0145580852F, - 0.00935474783F, 0.00170384627F, -0.0119638871F, -0.00841040537F, - 0.00543195801F, 0.00949025061F, 0.00572598306F, 0.00772338174F, - -0.0109497681F, -0.000347900699F, -0.00659514684F, -0.00576638058F, - 0.01217895F, -0.0131112486F, 0.00574052753F, -0.00765149668F, 0.00319143827F, - -0.0039912723F, -0.00240235473F, 0.00170866132F, -0.0172139909F, - -0.00125428871F, 0.00416885549F, 0.000697232375F, 0.00400676625F, - -0.00281882612F, 0.00443606125F, 0.00389895751F, -0.00926086772F, - -0.000585956266F, 0.00238942518F, 0.00473672897F, 0.000510710117F, - -0.0116668381F, 0.00156569784F, -0.00113111839F, 0.0114547424F, - -0.0113373427F, 0.00395344384F, -5.79889711E-5F, 0.00421310728F, - -0.00221056351F, -0.00217064773F, 0.000712197565F, -0.00733961491F, - -0.00442297943F, 0.00769776851F, 0.00480000442F, 0.016184669F, - -0.00615909137F, -0.00905502215F, 0.00611790083F, 0.00509346137F, - 0.0140409339F, -0.00505795237F, -0.00319208787F, 0.00267167F, - -0.00305627915F, 0.00106871955F, -0.00809543952F, -0.00722177513F, - -0.00103400229F, 0.00299700536F, -0.0162522085F, 0.000826457923F, - -0.00141950953F, 0.00265947F, -0.00769399619F, 0.0132272914F, - -0.00435677683F, 0.000602055457F, 0.00851278F, 0.0174653772F, 0.00322624715F, - 0.00106038421F, -0.00522523699F, 0.0068608257F, -0.0220932811F, - -0.00145591761F, 0.0055127833F, -0.017835293F, -0.00750192348F, - 0.0220710132F, -0.0122621255F, -0.00280095264F, 0.0137734283F, - -0.0179055389F, -0.00454299245F, 0.007068682F, 0.00312372902F, 0.0124754282F, - -0.0065002013F, -0.00720707281F, -0.000434928F, -0.0126578826F, - -0.00586242275F, -0.00934413355F, 0.00489079719F, -0.0113392184F, - 0.00344240526F, -0.00941303186F, 0.025164891F, -0.00685248617F, - 0.00749438908F, -0.000337615667F, -0.000174806744F, 0.00159723847F, - -0.00681069726F, -0.0145190507F, -0.00729278335F, 0.000216253058F, - -0.00817529764F, 0.00122515787F, -0.0112618534F, 0.00877713F, -0.0145111969F, - 0.00805515423F, 0.00106298248F, -0.00155019586F, 0.00601404486F, - 0.00149419706F, -0.016242696F, 0.0099171577F, 0.00709059695F, - -0.00666729454F, 0.0219138302F, -0.00737058604F, 0.0120260026F, - -0.0272554215F, 0.00344826258F, -0.00862184726F, -0.0202012435F, - -0.00336053711F, -0.00918149576F, -0.00122370385F, -0.000780607341F, - 0.0096521806F, 0.00391412666F, 0.00290178484F, -0.0121339699F, 0.0110743064F, - 0.00341900578F, -2.51169386E-5F, -0.00559168169F, -0.010116348F, - 0.00937380921F, -0.00499831885F, 0.00506184204F, 0.00736343302F, - 0.00740448106F, -0.0258334894F, 0.00396317337F, 0.00631228229F, - 0.00645230478F, 0.00348266866F, 0.0072679827F, -0.00800364651F, - -0.00704589952F, -0.00519069191F, -0.0109746959F, 0.0068168561F, - -0.0184185188F, -0.00150536199F, 0.00549902907F, -0.00546318153F, - -0.00979212672F, 0.0129257403F, 0.0061045778F, 0.000455320143F, - -0.0037048189F, 0.00221692678F, -0.00260300655F, -0.00843663421F, - 0.0129743023F, -0.00702925259F, 0.00841616653F, -0.0120337531F, - -0.00337584456F, 0.00533900736F, 0.00357882632F, 0.00133794814F, - -0.000379928038F, 0.0102790305F, -0.0163584705F, 0.00315661356F, - -0.00500225089F, 0.00131772202F, 0.00150297082F, -0.00592252892F, - 0.00274625747F, -0.00539534492F, -0.0147641515F, 0.000323624F, - -0.0132858129F, -0.00472527882F, 0.0089425547F, -0.00740281632F, - -0.00793625601F, 0.00210979511F, 0.00745533453F, -0.0123051945F, 0.00505871F, - -0.00142609596F, 0.0234257821F, 0.00133674976F, 0.00655447692F, - -0.00488069141F, -0.0102501903F, 0.0125888102F, 0.00417669117F, - 0.000795236148F, -0.00128483528F, -0.0165880974F, 0.0141699128F, - 0.0109585756F, 0.00235946127F, 0.00131434901F, -0.00928859413F, - 0.0101534231F, 0.000379331177F, -0.0201958157F, 0.0179410502F, - 0.000248005876F, 0.0106342863F, 0.00603497401F, -0.00104680134F, - -0.0102054356F, 0.0133286435F, 0.0140612181F, 0.00353894546F, 0.00533706369F, - 0.00204984169F, 0.0183343887F, -0.00393155403F, 0.00740967924F, - 0.000360658305F, 0.0178077966F, 0.0048062373F, 0.000918432896F, - -0.00957059581F, -0.00813300163F, -0.00359590119F, -0.0111478604F, - -0.00206961436F, -0.00782204606F, -0.00172901945F, -0.00971809123F, - -0.00815636106F, -0.00757137733F, 0.0139017487F, 0.00902526919F, - -0.0179661289F, -0.0186817031F, -0.0270372853F, -0.0151291015F, - -0.00106272253F, -0.015657844F, -0.00368853495F, -0.01068189F, - -0.00911449082F, 0.00618454488F, 0.0107240789F, 0.000382971892F, 0.00776442F, - -0.00749712856F, 0.00656484161F, -5.01695031E-5F, 0.0162928049F, - 0.0122941863F, -0.00755792F, -9.23252155E-5F, 0.00198868895F, 0.0078675095F, - 0.00817924179F, -0.0126059465F, 0.0110772159F, 0.0128042046F, 0.0104579981F, - 0.00526919821F, 0.00702731032F, 0.0052453191F, -0.00879310444F, - -0.0027772747F, 0.00322983786F, 0.000292662182F, -0.00770472456F, - -0.00683153933F, 0.00278732716F, -0.0185887925F, -0.0115404027F, - -0.00390534569F, 0.0158042628F, 0.0138420239F, -0.00346110226F, - 0.00186582073F, 1.26757341E-5F, -0.00194723497F, 0.0135049541F, - -0.00377650163F, 0.00945722405F, -0.0149485637F, 0.0015688712F, - 0.00756797614F, -0.0104094604F, -0.00384076056F, -0.0148327434F, - 0.000776550442F, 0.00264512771F, 0.00396567071F, 0.00769359572F, - 0.0156379677F, 0.0142395925F, -0.00618796749F, -0.00262249587F, - 0.00156063074F, 0.0138611747F, 0.0106269028F, -0.00405132491F, - -0.000260075176F, -0.0127196219F, -0.00504614552F, 0.00746740447F, - 0.0163406748F, -0.00321303681F, -0.00182324869F, -0.0126924394F, - -0.00588365272F, 0.00719365291F, 0.00619257614F, -0.0137848156F, - -0.00197007577F, 0.00851861853F, 0.0173650887F, 0.0126174232F, - 0.00491420599F, -0.0072187027F, 0.0161168221F, -0.00121519901F, - 0.000877501443F, 0.00593040138F, -0.0152314855F, 0.00388408056F, - -0.00818154216F, -0.00033669293F, 0.0124125024F, -0.0049887835F, - -0.00375421485F, 0.00509831076F, -0.00917650852F, 0.00947858393F, - -0.0121653378F, -0.00295958598F, 0.00475792866F, 0.00536429789F, - -0.000578746549F, 0.00157158601F, -0.000200350536F, -0.00484400801F, - -0.0278161149F, 0.00428341562F, 0.00841003563F, 0.0112126851F, - -0.0020085182F, -0.00531669613F, -0.0006153906F, 0.00832290668F, - 0.0100145675F, 0.00739498343F, -0.00526768249F, -0.00877931342F, - -0.0126388548F, 0.00648421841F, -0.00107033271F, -0.0146083562F, - -0.00476977415F, 0.0044064736F, 0.0130841574F, -0.00968610868F, - -0.00258099264F, 0.0193816219F, -0.00746535836F, -0.0118952328F, - -0.00734489F, -0.00870415755F, -0.0100437449F, -0.00137981807F, - -0.0187129769F, -0.00489539281F, -0.0170715153F, -0.00359448441F, - -0.0114216078F, -0.00630964478F, 0.0148286028F, -0.000234374194F, - 0.00759558659F, -0.00485277642F, -0.00122737349F, -0.00784629211F, - 0.00161857298F, 0.0176153313F, -0.0130358245F, 0.00820186175F, - -0.00280831382F, 0.00801050756F, 0.00626864517F, -0.0170332082F, - -0.000967032684F, -0.0114961797F, 0.0146498019F, 0.00215974217F, - 0.00711018266F, -0.0131671196F, -0.0125597622F, 0.00325849443F, - 0.00662994292F, -0.0072298497F, -0.0093349237F, 0.00568113709F, - -0.00575621845F, 0.0142275523F, 0.00258731795F, 0.00287235808F, - 0.00796219613F, 0.0110687762F, -0.00282573211F, 0.00711017754F, - 0.00260145334F, 0.0162427966F, 0.00687436899F, -0.0102854809F, - -0.00896724779F, 0.0145551935F, 0.0198801011F, 0.00421231287F, - 0.00775512401F, -0.00180908258F, 0.0111830812F, -0.00103957811F, - -0.00731309084F, -0.00261361036F, 0.00155843596F, -0.000295197882F, - 0.00398061238F, 0.00401790487F, -0.00403033709F, -0.0140780564F, - -0.0108285714F, 0.0140246022F, 0.00485663814F, 0.0111924857F, 0.01002225F, - 0.0089087477F, 0.00994671788F, 0.000133209251F, 0.00686858036F, - -0.0123636071F, 0.0101134675F, 0.00147007708F, 0.00228136266F, - 0.00331973867F, -0.00155979674F, -0.00219108979F, 0.000586787879F, - 0.0148233688F, 0.00765825436F, -0.00308315502F, 0.00520531693F, - -0.0148021253F, -0.0112009477F, -0.00445881486F, 0.00896018185F, - 0.0236788075F, 0.00582511583F, -0.00626898836F, -0.0123803355F, - -0.00241239765F, -0.0150836119F, 0.0156154064F, -0.000790289545F, - 0.0146535933F, -0.0065589333F, -0.00653815549F, -0.00352175278F, - 0.00626879372F, -0.00209311629F, 0.0112554049F, -0.00997690484F, - 0.0138853118F, 0.00351020298F, 0.0115372008F, -0.0106353685F, - -0.00602802215F, -0.00981298275F, -0.00318934652F, -0.00297128526F, - -0.00829334371F, 0.0151499119F, -0.00178909034F, 0.00452550128F, - -0.0140974447F, -0.00642851507F, -0.0127173383F, 0.00576396659F, - -0.0248933621F, 0.00821348839F, -0.00881609879F, -0.000487868907F, - -0.00959369075F, 0.00168697757F, -0.00560979452F, 0.00320567121F, - -0.00173455977F, 0.00589864049F, -0.016483631F, 0.00760917133F, - -0.0238648765F, 0.000519721536F, -0.00297320238F, 0.0100528989F, - 0.0171277095F, 0.000950327609F, -0.0188709293F, -0.000115388306F, - -0.00981038716F, -0.0019583907F, 0.00650865305F, 0.0112598082F, 0.00428399F, - 0.00470515108F, -0.00786684F, 0.0012070206F, 0.0105828121F, -0.01358411F, - 0.0114333332F, -0.0144637711F, -0.00965354219F, -0.000240747773F, - 0.0113937249F, -0.00727630407F, 0.00789632183F, 0.00289991684F, - 0.0091226818F, -0.0085381316F, -0.00934181F, -0.011109814F, 0.00979287829F, - -0.0129223783F, 0.00419550482F, 0.00910151098F, -0.0210401751F, - -0.0115109393F, 0.00662690541F, -9.00943633E-5F, -0.00981624331F, - -0.000660712714F, -0.00860012323F, -0.00506827189F, -0.00364276976F, - 0.00236440799F, 0.01368492F, -0.0114857424F, -0.000215197841F, - 0.00439795572F, 0.0127021456F, 0.0115212006F, 0.0144735305F, -0.0110438829F, - 0.000370551599F, -0.00797395315F, -0.00799272F, 0.00257117138F, - 0.00393804908F, 0.0109666856F, -0.00397552457F, 0.00842499F, 0.00345959747F, - 0.000958170102F, -0.00446577696F, 0.00739502208F, 0.00424534781F, - 0.00737995794F, -0.0053424607F, -0.00686546555F, 0.0207012109F, - -0.018376125F, -0.00591528649F, -0.00115359528F, 0.00217669271F, - -0.00452074595F, 0.0142838387F, 0.0141709624F, 0.00124202261F, -0.00902543F, - 0.01080117F, -0.00183475378F, 0.0028792934F, 0.00491414918F, -0.0155476434F, - 0.00596301351F, -0.00106641394F, -0.0151417749F, 0.0043162806F, - -0.0149661526F, -0.00271624024F, -0.0084652612F, -0.0184441619F, - 0.00841563474F, 0.0109435897F, 0.00881613F, -0.0122703034F, 0.0138289584F, - -0.00125201547F, 0.00721271243F, -0.012783546F, 0.00884651F, -0.0156820603F, - 0.0213665087F, -0.0143809812F, -0.00283584604F, -0.0100842947F, - -0.0076267221F, 0.00292751961F, 0.00845472235F, -0.011906391F, - 0.00441668835F, -0.0137627777F, 0.01150994F, 0.0151434541F, -0.00760983443F, - 0.00530322408F, -0.00967440382F, 0.00881057139F, -0.0172384251F, - -0.00552873826F, -0.0177435596F, 0.00509970868F, -0.00230977894F, - 0.00538070453F, -0.00708635F, -0.0101650413F, -0.0145107154F, 0.00565327425F, - -0.0102178194F, 0.00914279651F, 0.00502513628F, -0.0046489737F, - 0.000499433896F, 0.0108716087F, -0.0123945652F, 0.00337063731F, - -0.00401050691F, -0.00343576702F, 0.00202037627F, -0.0058074696F, - -0.00217343471F, 0.0152455568F, 0.00120481534F, 0.00193545513F, - -0.015384296F, 0.00562128611F, 0.00635154033F, -0.0122176744F, - 0.00725381961F, 0.0119697731F, -0.0171589069F, 0.000565256167F, - -0.00557887508F, -0.00445459271F, 4.08468732E-5F, -0.00206544809F, - 0.00923901796F, -0.0136843221F, -0.00179400505F, -0.0108909244F, - -0.00557505898F, 0.0068799695F, -0.00923530664F, -0.00317911268F, - -0.00167127559F, 0.00393221434F, -0.0173229929F, 8.89397052E-6F, - 0.00270238239F, 0.00213610334F, -0.00170272775F, 0.00248982478F, - -0.0133793419F, -0.00613938132F, -0.0177571736F, 0.00537957205F, - -0.0151441917F, -0.00719144614F, 0.00708239805F, -0.00177846884F, - -0.00879599899F, -0.00640630908F, -1.74218112E-5F, -0.00253963331F, - -0.00264709489F, 0.000570761098F, -0.00247779675F, 0.00477184402F, - 0.0061366288F, -0.0109046958F, -0.00451748865F, 0.00685820216F, - -0.00426488789F, -0.00158781954F, 0.0107820751F, -0.00311885821F, - 0.00763985794F, 0.0228385832F, 0.000390620786F, 0.00893307198F, - -0.0105534783F, 0.00440662494F, -0.00735922F, -0.00388633483F, 0.0021187684F, - 0.0049468372F, 0.00129814236F, -0.00293698208F, 0.0108479913F, - 0.000296680577F, 0.00566702383F, 0.00845735054F, 0.00572151458F, - 0.0119028892F, -0.00747928349F, -0.00875300914F, 0.00101652974F, - -0.00369774573F, 0.0104435142F, -0.0027734295F, 0.0111276209F, 0.003784484F, - 0.0149617884F, -0.00656322576F, 0.0111586265F, 0.00394681608F, - -0.00551748974F, 0.00319541665F, 0.0160973035F, -0.00137012929F, - -0.000711353612F, -0.0107273115F, 0.016007727F, 0.00304938154F, - -0.000643462292F, -0.00693831407F, 0.00582046434F, 0.00257268292F, - -0.00404233485F, -0.00343694468F, -0.0140340496F, -0.0119008459F, - -0.011834803F, -0.00609197281F, 0.0125142802F, 0.00158458308F, 0.0133396145F, - -0.00168148521F, -0.00795261469F, 0.0039472119F, 0.000319652812F, - 0.00530645438F, 0.00313685625F, 0.019064961F, -0.00283840927F, 0.0209605861F, - 0.00725847576F, -0.00424655573F, 0.0167863034F, 0.0068381317F, - -0.0144856097F, -0.00251116278F, 0.0129266325F, 0.00545919267F, - -0.00444729347F, -0.000705103739F, 0.0068617505F, 0.00266358606F, - -0.00147362717F, -0.005467996F, -0.00767879141F, -0.0095533533F, - 0.0060417424F, -0.00341043808F, 0.00412094F, -0.0188036188F, 0.00867048278F, - 0.004926831F, -0.00123679754F, 0.00241819257F, -0.00402715569F, - 0.00325158355F, 0.0258749779F, 0.0139992349F, 0.00595115777F, 0.0135589028F, - 0.00409818301F, 0.00481836F, -0.0136269424F, -0.00404394884F, -0.0146099925F, - 7.63472272E-5F, -0.0102016907F, 0.0110770669F, 0.00443852227F, - 0.00270244828F, 0.00769699411F, 0.00573386438F, 0.00783029F, 0.00427783793F, - 0.00614445331F, 0.00430000713F, -0.0180232394F, -0.00290390523F, - 0.0112610823F, 0.00472623203F, -0.00723127462F, -0.00730211567F, - -0.00773883238F, -0.0122857979F, -0.0100844475F, 0.000226361954F, - 0.0058428389F, -0.0118507622F, -0.0089352373F, 0.00727288285F, - -0.0114319446F, -0.00280601135F, 0.0143813305F, -0.0108958138F, - 0.0242511798F, -0.0112558343F, -0.00606567971F, 0.00581710273F, - -0.0289047565F, -0.00696015544F, 0.00634516589F, 0.0016163527F, - -0.0235593356F, -0.00453781709F, 0.00721457088F, 0.0107543226F, - 0.00330229639F, -0.0104085328F, 0.00420502F, 0.00358405546F, -0.00178212894F, - -0.0250079278F, -0.0202197656F, -0.00552706048F, -0.00874187797F, - -0.000793524843F, 0.0172079299F, 0.00590000721F, -0.0109450193F, - 0.010130588F, -0.0027440479F, 0.011748001F, 0.0141481403F, 8.31717E-5F, - -0.0128249442F, -0.00664622709F, -0.0148775345F, -0.0144789573F, - -0.00691625709F, 0.000617431244F, 0.000311965647F, -0.0170238614F, - -0.00997861754F, 0.0118064778F, -0.00111643609F, -0.00779027957F, - 0.000404604914F, 0.0130826794F, -0.000625497662F, 0.00476801163F, - 0.0230516642F, 0.0189609379F, 0.00354832574F, -0.0133778462F, - -0.00776484236F, -0.00957972836F, 0.00277980487F, -6.4128435E-5F, - 0.002262583F, -0.000401242374F, 0.00673967646F, 0.00382063119F, - 0.00347914849F, 0.0210113507F, 0.0084063448F, 0.00343213859F, -0.0144439498F, - -0.00552307256F, 0.00165810471F, -0.00943679921F, -0.00627870066F, - -0.0156720839F, -0.0066676843F, -0.00915672723F, -0.0147821689F, - 0.00994324498F, -0.00130496547F, 0.0128848599F, -0.027148718F, - -0.0140524954F, -0.00141383067F, 0.0107465489F, -0.0170030985F, - -0.000696027593F, 0.00330703869F, -0.00332552288F, 0.012515381F, - -0.0147104962F, -0.00272524427F, -0.00298815F, -0.00575231295F, - 0.0065881107F, -0.00825467147F, 0.00947024F, 0.000856069673F, 0.00754797272F, - -0.0208085068F, -0.00343496632F, 0.00641714595F, -0.00368572772F, - -0.00966874883F, 0.0101541F, -0.00125447731F, -0.0114158327F, - -0.00223932392F, -0.00758287404F, -0.00266086101F, 0.00500562415F, - -0.00642589247F, -0.002098653F, 0.00251724781F, 0.0146628972F, - 0.00277766935F, 0.00735763833F, 0.00411865115F, 0.00159495568F, - -0.000333279604F, -0.0064210603F, 0.00444837566F, -0.00936931744F, - -0.0115569681F, -0.0186732206F, 0.00978970341F, -0.00784367509F, - -0.00301114959F, -0.00716600753F, -0.0116642723F, -0.0195998047F, - 0.00383553375F, -0.00618351344F, -0.0145866498F, 0.0055649248F, - 0.0210183263F, 0.0048348913F, 0.0119323162F, 0.00710467761F, 0.00576439872F, - -0.00425792253F, 0.00326650147F, -0.00622947514F, -0.00720166881F, - 0.0212248527F, -0.000863344874F, -0.000147156985F, 0.000938169193F, - -0.00772839691F, 0.0192698296F, -0.0171531197F, 0.000444079225F, - -0.00552467396F, -0.000303118257F, -0.0109972619F, -0.00646703225F, - 0.0103220139F, -0.00331045757F, 0.0134596685F, 0.00112724537F, - -0.0061751469F, -0.000624677865F, 0.00338219851F, -0.0065465644F, - 0.00397964194F, -0.0132974647F, -0.00667139934F, -0.0100809615F, - 0.00302967848F, 0.00311725843F, 0.00384086394F, -0.00295133214F, - -0.0116581293F, 0.0123201776F, 0.00431361841F, 0.00626981305F, - -0.00360330078F, -0.00930039212F, 0.00212017726F, 0.0254909955F, - 0.000688416301F, -0.00625611143F, 0.00215885625F, 0.000922376814F, - -0.00181470485F, 0.0046043722F, -0.00710701663F, 0.00865104608F, - -0.00424193777F, 0.00452390034F, 0.00237771193F, -0.00744110206F, - 0.00746626081F, 0.000812180864F, 0.0139838215F, 0.00728577F, -0.00894588139F, - -0.0201327447F, 0.00590801565F, -0.00199154089F, -0.00675393268F, - -0.00424414966F, 0.0199368224F, 0.0173341F, 0.0124947317F, 0.00229894719F, - 0.00588548928F, -0.0174862482F, -0.0138817756F, 0.00323236012F, - -0.00919680391F, -0.0115381656F, 0.00231301785F, 0.0200676415F, - 0.0398131721F, 0.0100492705F, -0.00381785887F, 0.0178740937F, - -0.00311454502F, -0.00288308226F, -0.00695025F, -0.0158947222F, - -0.00396543508F, 0.00265397481F, 0.00259088282F, 0.00344561087F, - -0.0069217626F, -0.00487780804F, 0.00372435944F, -0.0053862324F, - -0.0002223872F, -0.00109808985F, -0.00110611506F, 0.0128047597F, - -0.00937183294F, 0.000460817129F, 0.0101788761F, -0.0103799151F, - 0.0105502494F, -0.00256205071F, 0.00842869189F, -0.0112495376F, - -0.00168325147F, 0.00499624852F, -0.00392395491F, -0.0189949851F, - 0.00306796F, -0.0127776954F, -0.0036306784F, -0.00288373837F, - -0.00594062544F, -0.00113591028F, 0.0162601508F, 0.00213747681F, - 0.0149022797F, -0.00949599F, -0.00267238356F, -0.00755942473F, - -0.00859179161F, 6.25697E-5F, -0.000897755F, 0.00155378424F, 0.0086724367F, - 0.0127863716F, -0.0007207655F, 0.00792640354F, 0.0159013662F, - -0.00357050961F, 0.00311969547F, -0.011456F, -0.0174675342F, -0.00791138876F, - -0.00243461248F, -0.0151079101F, -0.0109435255F, -0.0150624346F, - 0.00298578828F, -0.0136014866F, 0.00853625499F, -0.00847852137F, - -0.0103578605F, 0.00148785079F, -0.0178192221F, 0.0060652676F, - 0.00202323496F, -0.00927080307F, 0.00931449141F, 0.0140107898F, - 0.00684456667F, -0.00300911558F, -0.0102352928F, -0.001212843F, - 0.0193210337F, -0.0210381877F, -0.00727889966F, 0.010417291F, - -0.00739289075F, 0.00481084082F, -0.00271785958F, -3.41946E-5F, - -0.00175202347F, 0.0150079606F, 0.012285294F, 0.0059592491F, -0.00648911949F, - 0.000201433126F, 0.00199737679F, 0.0108492794F, 0.0189528223F, - 0.00730749778F, -0.0147883948F, 0.0106022917F, 0.0181880854F, 0.00666144863F, - 0.00329380343F, -0.0014010244F, 0.00295720086F, -0.00685452344F, - 0.00317584141F, 0.0117064454F, 0.00343127805F, -0.0128151625F, - -0.00349437608F, 0.00108456647F, 0.001827723F, 0.00779454F, 0.0059078862F, - 0.0126221543F, 0.000631256378F, -0.00947180297F, -0.0139354654F, - -0.00338075543F, 0.000485979894F, 0.010288476F, 0.00411240198F, - 0.00449409802F, -0.0130091505F, -0.00654653041F, 0.00285094883F, - 0.00737219444F, -0.00138596527F, 0.0161016732F, -0.00515891705F, - -0.00851319358F, 0.00664079422F, -0.00589481555F, -0.0386400297F, - 0.0100795794F, 0.00534736365F, 0.00260472251F, 0.00898699742F, 0.0093568638F, - 0.013799916F, -0.00740874093F, 0.0045341F, -0.00675114105F, 0.00657767337F, - 0.0164098572F, 0.00316132745F, -0.00396749144F, -0.00161342463F, - 0.00426982716F, 0.0190809146F, 0.00214616675F, -0.00394851388F, - -0.013896497F, 0.0156123564F, 0.000313134224F, -0.00503478758F, - -0.00849055219F, -0.0116858082F, 0.00249717757F, -0.00202386244F, - 0.0022460348F, 0.00726987747F, 0.00125142175F, -0.00220265705F, 0.029123975F, - 0.0042872089F, 0.010389098F, -0.00907157455F, -0.00133391074F, - 6.28113849E-5F, 0.0146242641F, 0.00569002051F, 0.00294576935F, - -0.0026631949F, 0.00244489918F, -0.000954805466F, 0.00585607719F, - -0.0183015559F, 0.00510288076F, -0.0119202575F, -0.00839067902F, - -0.00948764104F, -0.00679801311F, -0.00719991047F, 0.0180897713F, - -0.00343266F, -0.0123253791F, -0.00970880315F, -0.024887871F, 0.00453338819F, - -0.00595312705F, -0.0212022327F, 0.0133914445F, -0.000433417F, - 0.000131790686F, -0.00159146031F, 0.00315806689F, 0.0184909385F, - 0.00912747066F, 0.00898297224F, -0.00505569484F, -0.000210986123F, - 0.0125765083F, -0.00129763829F, -0.0145256352F, -0.00118665805F, - 0.00998269487F, 0.00666487683F, 0.00706155645F, -0.00237897411F, - 0.000320722465F, -0.0206540097F, -0.00677666394F, 0.00100634084F, - 0.0119773233F, 0.00458280602F, 0.0103441663F, -0.000918811653F, -0.00866043F, - -0.0213377811F, 0.00485145766F, 0.00623416621F, 0.0162249431F, 0.012655532F, - -0.00452406844F, 0.0129817501F, 0.0141043412F, -0.00119678304F, - -0.000179546449F, 0.00729167135F, 0.00143132487F, -0.00349392486F, - 0.0066721607F, -0.0109009584F, 0.00725286128F, -0.00284638605F, - 0.00626099203F, -0.00145376939F, -0.0236707777F, 0.0161602423F, - -0.0102026602F, 0.0275611877F, 0.0136529263F, -0.00763630075F, - -0.00243159034F, 0.0128081599F, 0.0027319584F, -0.00829619635F, - -0.00155078666F, -0.00176476478F, -0.0252316296F, 0.0103814518F, - -0.000327154325F, -0.000514928135F, -0.0153265521F, -0.00443893764F, - 0.00357061788F, -0.0122057432F, 0.000155608752F, -0.00103448646F, - 0.00342918513F, 0.0169813335F, 0.022013383F, 0.00383725553F, -0.0132056437F, - -0.00345502421F, -0.0159919858F, 0.00689730886F, -0.00162009825F, - -0.00899628177F, 0.00811216794F, 0.00528156804F, -0.0207702164F, - 0.00107143808F, 0.0092329178F, 0.0177899376F, -0.000919004844F, - 0.00631898455F, 0.00324929343F, 0.000658129633F, -0.00583690684F, - -0.00965523627F, -0.00541307917F, 0.00351239834F, -0.00104205066F, - -0.0148262372F, -0.0119359521F, -0.0121043371F, -0.00535184052F, - 0.00520970766F, 0.00644215755F, -0.0166614987F, -0.0126827816F, - 0.00878140703F, 0.00601182925F, 0.0139131192F, 0.00691778958F, - -0.00485829031F, -0.0132843181F, -0.000580043F, -0.00859094F, - -0.00720019452F, 0.00047627039F, 0.0209544301F, -0.0066258586F, - 0.0163884219F, 0.0075655668F, 0.00541606F, 0.0105563654F, -0.0143709825F, - 0.00952169579F, 0.00449008355F, -0.00462776842F, -0.0105675068F, - -0.00435738172F, -0.0167530943F, 0.0122983055F, 0.00111804076F, 0.008428392F, - 0.00448251748F, -0.00171999692F, 0.00291212881F, -0.000975878851F, - -0.00446726941F, -0.0101571362F, -0.00843124278F, -0.0132608321F, - 0.00115320599F, 0.000421357079F, -0.0123628126F, -0.00205146545F, - 0.00670872768F, 0.00151358498F, 0.0140823768F, 0.00996740721F, - 0.00547550479F, -0.00478941202F, 0.0164572578F, 0.00537443347F, - -0.00862129685F, -0.00495389663F, -0.000706117833F, -0.00608004117F, - 0.00411369186F, -0.00438970374F, 0.00107689353F, -0.0062920507F, - 0.0016032533F, -0.00859917514F, -0.00124932197F, 0.00379020022F, - 0.00895807054F, 0.00204400695F, 0.00692274282F, -0.00412504142F, - -0.00176898309F, -0.0016562586F, -0.00203879597F, -0.000213673295F, - -0.0103284921F, 0.0108536314F, -0.00454908051F, -0.00125355541F, - -0.00841994F, -0.000946197601F, 0.0071791159F, 0.005095114F, 0.00442345394F, - -0.00385954534F, -0.00445462763F, -0.00746814162F, -0.0014791626F, - -0.00776292058F, -0.0078784693F, 0.00857901201F, 0.00324173179F, - 0.00105641026F, -0.0140476301F, 0.0124119138F, -0.00965087768F, - -0.00332210446F, -0.00506664859F, -0.00410275813F, -0.0105527686F, - -0.00887417421F, -0.00594371744F, 0.0165206213F, 0.0024556811F, - 0.00355718099F, -0.00566621264F, 0.014857091F, 0.00856767129F, - -0.00299923029F, -0.0104484968F, -0.00916871708F, -8.74717662E-5F, - 0.010583384F, -0.011617789F, -0.00320633617F, 0.0156394094F, -0.00238647824F, - 0.0189678222F, -0.00221646135F, -0.00263774628F, -0.00726656569F, 0.0225858F, - 0.00874697417F, 0.0168002F, -0.00163126457F, 0.00260592182F, 0.00731481705F, - -0.000251933932F, -0.00606682524F, -0.00927732605F, -0.00135819521F, - -0.00885428581F, 0.00813113246F, 0.0113243321F, -0.00779255899F, - 0.0095051378F, -0.0067538661F, -0.00217041094F, 0.0192507617F, - -0.0132842232F, 0.00932462793F, -0.0123759927F, 0.0103756189F, - -0.00688274205F, -0.00343468459F, -0.010281573F, -0.015214405F, -0.00463212F, - -0.00613623066F, 0.00582990516F, -0.00433168793F, -0.0224690717F, - 0.00274514F, -0.00568291917F, 0.001129237F, 0.0134343449F, -0.00754337665F, - -0.00054011005F, -0.0119897677F, -0.004296795F, -0.00246622693F, - 0.0100420136F, -0.0122247301F, -0.00186321966F, 0.00359748513F, - 0.00562924612F, -0.0115499301F, -0.0239050649F, 0.0135902921F, 0.0133311935F, - 0.0101232408F, -0.00504079647F, -0.00730600208F, 0.0174527578F, - -0.0189656243F, 0.0226030871F, -0.00577720487F, 0.00334076211F, - -0.00743942056F, -0.00245187432F, -0.0013521224F, 0.00758937141F, - 0.000433194306F, -0.0076457276F, 0.00379782496F, -0.00943132583F, - 0.0111577827F, 0.00477000047F, 0.000379190344F, -0.0151754394F, - -0.0102951638F, 0.00933550857F, 0.00440660026F, 0.00203794474F, - -0.0151696103F, -0.00518384855F, -0.00493216328F, -0.00856079813F, - -0.0153263481F, -0.00511198211F, 0.0155940354F, -0.0016513993F, - -0.00419103866F, 0.00378843397F, 0.00980801F, -0.00510544702F, - 0.00208024168F, 0.0167601109F, -0.000739627401F, 0.00657469034F, - -0.0174933877F, 0.0114032794F, 0.00503836479F, 0.0171043463F, 0.00592258479F, - -0.00477635907F, -0.0118691111F, 0.0102849863F, 0.000765745295F, - 0.00645862939F, -0.0135780126F, 0.00063895731F, 0.00739372754F, - -0.00273219543F, 0.00136940274F, 0.00780892279F, 0.00572235882F, - -0.0129219359F, -0.00845874F, -0.000622151943F, 0.00601806771F, - -0.00754414685F, -0.00381154986F, 0.00310656917F, 0.00550520141F, - -0.00622094888F, -0.00576602388F, -0.000385303982F, 0.00217651436F, - -0.00656028558F, 0.00233024778F, -0.00179426058F, -0.00998726673F, - -0.0128067508F, -0.00740615372F, -0.012634417F, 0.00308599137F, - -0.000893872522F, 0.0147262979F, 0.00438108062F, -0.00631083967F, - -0.0175817497F, -0.00807986F, -0.0054628686F, -0.00481158542F, -0.0113866F, - -0.00363587542F, -0.00459889881F, 0.00840569F, 0.0062590912F, -0.0036076156F, - 0.0148293423F, 0.0106594218F, 0.00605136435F, 0.00600624736F, 0.00298196869F, - 0.0117475744F, 0.0095265191F, -0.00191149209F, 0.00956998F, -0.00316913589F, - -0.00348773F, -0.0103591513F, 0.00863962248F, 0.000178387098F, - 0.00396018289F, 0.0050213F, -0.0127086835F, 0.0172445271F, 0.0108944317F, - -0.000400560035F, -0.00518839387F, 0.000521998561F, 0.00816505868F, - -0.0116987238F, 0.000489520258F, 0.0054985811F, -0.00789076835F, - -0.0092226956F, -0.0112368939F, -0.00157776312F, 0.00821011327F, - -0.0148722837F, -0.00654233806F, 0.000592518889F, 0.00285973796F, - -0.00978723913F, -0.0079009F, 0.00193404523F, 0.0116088595F, -0.0106465481F, - -0.00851240661F, 0.00939851627F, -0.00582399825F, 0.010099249F, - 0.00858604815F, 0.0134634348F, 0.0108726257F, -0.0318861939F, 0.00286997342F, - 0.00727554F, 0.000235309388F, -0.0104722334F, -0.00988898426F, - -0.00291519496F, -0.0145746376F, 0.00417310325F, 0.0128809474F, 0.002982951F, - 0.00989516918F, 0.0043499954F, -0.0156953968F, -0.00111537415F, - 0.0131744063F, 0.000880546751F, 0.000165105899F, 0.00387706095F, - -0.016053455F, 0.00360118458F, 0.00653398084F, 0.00520344032F, - 0.00244120881F, -0.0140904812F, -0.00177190208F, -0.0158624575F, - 0.00396105926F, -0.00371591095F, -0.0147309238F, -1.77252732E-5F, - -0.0123434346F, -0.0275695845F, 0.00801608F, -0.00925468747F, 0.00812303834F, - 0.0113205398F, 0.00239246944F, -0.00677597756F, 0.0145373037F, - 0.00985291135F, 0.000719402393F, -0.00826887321F, -0.00148632342F, - 0.00308497297F, -0.0151984105F, 0.000258256332F, 0.00962759648F, - -0.0146694798F, 0.0071215122F, 0.00537027884F, -0.0014540588F, - -0.00605308171F, -0.00473413756F, -0.00845829677F, -0.00296189962F, - -0.00679663103F, 0.0065138978F, -8.11523132E-5F, 0.0121903475F, - 0.000770064595F, -0.00396384113F, -0.0209771208F, 0.00613260223F, - 0.00457689073F, 0.00486880122F, -0.00874591712F, -0.0261328872F, - -0.0077150315F, 0.00666615181F, -0.0183474962F, 0.000131209803F, - 0.00448339572F, 0.0237851162F, -0.00179858739F, -0.0143643264F, - 0.0115580773F, -0.00648718467F, 0.00234252866F, -0.00816702191F, - 0.00190333731F, -0.00863870699F, -0.00244725496F, 0.00476296712F, - -0.00185338873F, -0.0075492938F, -0.0206390414F, 0.0103892265F, - -0.00974428281F, 0.00890636258F, -0.00480455626F, 0.00803156849F, - -0.00607842533F, -0.00870519411F, -0.00228092261F, -0.000893469143F, - 0.0118584298F, 0.00931067578F, 0.00679523963F, 0.00285081053F, - -0.0119356737F, -0.00174661819F, -0.0115298945F, -0.00710945111F, - -0.0113948397F, 0.0126071749F, 0.0106498534F, -0.0185475014F, 0.017391501F, - -0.0187278856F, -0.00132273883F, 0.00848060939F, -0.00915504713F, - -0.0167248435F, 0.0021209449F, -0.0155519303F, 0.00774585F, -0.0116878971F, - -0.00122985861F, 0.00553673506F, 0.00781782251F, -0.00875730067F, - 0.00907933712F, 0.00682168314F, -0.00877612F, -0.00439300947F, - 0.00655775238F, -0.000281456538F, 0.00990473107F, -0.00269773346F, - -0.0211311895F, -0.0117759872F, -0.0035878031F, -0.0152777787F, - -0.00045122442F, -9.89196924E-5F, -0.00456796028F, 0.035823632F, - 0.0029221226F, 0.00426157936F, -0.00329194427F, 0.00382543798F, - 0.000269505399F, -0.0140986713F, 0.00270226039F, -0.000247438729F, - 0.00437850598F, 0.000283965957F, 0.000280471F, -0.0195255354F, - -0.00893124379F, 0.00349830627F, 0.00128687336F, 0.00797606725F, - -0.000309053983F, -0.00845087226F, -0.00577077782F, -0.00150190864F, - -0.0108448295F, 0.0108714467F, -0.00993192848F, -0.011978115F, - -0.00229636184F, 0.0026309893F, 0.00921121426F, 0.00463928469F, - 0.00492155459F, 0.0187793113F, 0.00495001F, -0.00693179388F, 0.0133618433F, - -0.00613880856F, -0.0122116916F, 0.00483078323F, -0.00117962086F, - 0.00542997709F, -0.0197159499F, 0.0121902525F, 0.0149506601F, -0.0170787275F, - 0.00412125792F, 0.00552583F, -2.25051026E-5F, -0.00681675132F, - -0.0163842589F, 0.00346798892F, 0.00276770373F, 0.0117360167F, - -0.000792670064F, -0.0133754965F, -0.0122792795F, -0.00181400322F, - -0.00176054134F, -0.00719764689F, 0.00807674881F, 0.000373167451F, - 0.00289478502F, -0.00569791347F, 0.00298444694F, -0.014125892F, - 0.00942393765F, -0.00171256578F, -0.0176433325F, 0.000476711459F, - -0.00992899202F, -0.010986587F, 0.010659365F, -0.00724101299F, - 0.00361734838F, 0.000199400281F, 0.0106759407F, -0.00147256046F, - -0.00353094283F, 0.0116276583F, 0.000542828522F, -0.00375197269F, - -0.00625422643F, 0.0100894598F, -0.00554177864F, 0.00121037301F, - 0.00336441235F, -0.0022641886F, -0.00148681714F, 0.00236040656F, - 0.00651107356F, -0.0123257991F, 0.014613281F, -0.00776192965F, - -0.0236238968F, 0.00737037463F, -0.0020529239F, -0.000433991489F, - 0.0126579441F, -0.0153727513F, 0.00681453524F, 0.0133510614F, 0.00274574407F, - -0.000220937058F, -0.00546572171F, 0.00597404595F, 0.00233775354F, - 0.00260451413F, -0.00571714574F, -0.00162148499F, -0.00373442564F, - -0.0137325041F, 0.00460686721F, 0.00540542929F, 0.0111942375F, - -0.000965614687F, 0.00144650321F, -0.000707295956F, 0.00343185314F, - -0.0103740571F, -0.0100025171F, -0.00528118573F, -0.013660132F, - 0.0105389599F, 0.00970826671F, 0.00425746525F, -0.0146775413F, - -0.0158134792F, 0.00328931492F, -0.0117845265F, -0.00791628473F, - 0.00500902953F, 0.0192115456F, 0.00122931891F, -0.000226882839F, - 0.00308402674F, 0.00251346314F, -0.0059848316F, -0.0041315523F, - 0.00558285601F, 0.00773280626F, -0.00575030735F, 0.00765294721F, - 0.00503527932F, -0.018399816F, -0.00419597747F, -0.0160543304F, - 0.0116822384F, -0.0122993346F, -0.000689119624F, 0.0107004419F, - 0.00479096361F, -0.0221974272F, 0.00436080666F, 0.00326577714F, - 0.00615916215F, 0.00286146812F, -0.0147010088F, -0.020462092F, - -0.00354314083F, -0.00972461514F, 0.00630104F, 0.00916106254F, - -0.0105760107F, -0.000964776846F, -0.000769342645F, 0.00566520216F, - -0.00980218314F, 0.00141688983F, 0.00400790339F, 0.00963809714F, - -0.0024599249F, 0.00344252936F, -0.00886940397F, -0.0173236746F, - 0.00888087135F, 0.00132966391F, 0.0102813505F, 0.00293284445F, 0.0111178812F, - 0.00424978184F, 0.00745452056F, 0.01868123F, 0.00535684F, -0.00432291767F, - -0.00113152049F, -0.0268274806F, 0.000275746395F, -0.00642285217F, - -0.00222345372F, -0.0136017324F, 0.00392292347F, 0.00119893893F, - 0.00253595295F, 0.0123409238F, 0.00557235768F, 0.017163353F, -0.0110294875F, - -0.00442419806F, -0.00479563372F, -0.0118015148F, 0.0192246437F, - 0.0055762548F, -0.000871621829F, -0.00959519111F, -0.0141648222F, - -0.000795197964F, -0.0134768626F, 0.0169250425F, -0.000527886325F, - -0.00597600732F, 0.00915468484F, 0.0156961642F, -0.0144269224F, - 0.00489969365F, 0.00514568482F, -0.00597189F, -0.000175851121F, 0.0112808F, - -0.000502305571F, 0.00332582649F, -0.00235835579F, 0.0074385521F, - 0.00378233125F, 0.00127155706F, -0.0127506712F, 0.00595539063F, - 0.00945120584F, 0.00384482858F, -0.0111950431F, 0.0165562462F, - -0.0022654538F, -0.000461506716F, 0.00872318F, -0.00352656632F, - 0.00743333157F, 0.00103050948F, 0.00429009786F, -0.0110582821F, - 0.00555954454F, -0.0133499037F, 0.00733405584F, -0.0139508359F, - 0.00428581424F, 0.00156300585F, -0.0158533249F, -0.0138166975F, - -0.00183964637F, 0.0054236F, -0.00839228742F, -0.00356057473F, 0.0211043842F, - -0.0121993423F, -0.0183417276F, -0.0113247046F, -0.0088538155F, - -0.00911673345F, -0.00101098476F, 0.00191774219F, 0.00806461368F, - 0.02359212F, 0.0030577532F, -0.00596518535F, -0.0091201188F, -0.00154181535F, - 0.00125945371F, 0.00782215875F, -0.00416655047F, -0.00825439487F, - -0.00562505145F, 0.010774428F, 0.00612530671F, -0.000898258644F, - 0.00502851745F, 0.00532106031F, 0.000968951208F, 0.00156396662F, - -0.00872807205F, 0.0025544581F, 0.00696132565F, -0.00161032437F, - -0.0119870352F, 0.00427858578F, -0.0131498268F, -0.00377506902F, - 0.013027994F, 0.00810603518F, 0.00330480095F, 0.00120960968F, -0.0209700316F, - -0.00402216287F, 0.0246740244F, -0.00313148461F, 0.00949067157F, - 0.00373882847F, 0.00135849253F, 0.00627356535F, -0.00586958369F, - -0.0119681088F, -0.0133932689F, 0.00758894766F, -0.014631981F, - -0.00880277157F, 0.000976520823F, 0.00436453614F, -0.00328029715F, - 0.00296694F, 0.0114175249F, -0.00770576F, -0.0172791835F, -0.0165297147F, - -0.0131370341F, -0.00484564062F, -0.0162848681F, -0.00279709185F, - -0.00860068761F, 0.0127994884F, 9.1770431E-5F, -0.00539371837F, - -0.00993200671F, 0.00719271274F, 0.00923297275F, -0.0117540434F, - -0.00303592114F, 0.00546273543F, -0.00974032842F, -0.0127640078F, - -0.0176022667F, -0.00717155915F, 0.0026734292F, -0.00615541451F, - -0.00713295164F, -0.002996061F, -0.00406052545F, 0.00157234492F, - -0.00025620652F, -0.00989572704F, -0.0117473863F, -0.00296698697F, - -0.00795526244F, 0.00236464757F, -0.000948408735F, 0.0085838493F, - 0.00700027775F, 0.00167581718F, 0.00729870796F, -0.00837297551F, - 0.00963825267F, -0.0012692333F, 0.00196093949F, 0.00520976773F, - -0.00621535955F, -0.00987546705F, 0.000604815315F, -0.00647998136F, - -0.00121320272F, -0.0163379386F, -0.00143643201F, 0.00932065491F, - -0.00702346954F, 0.00132625632F, -2.28097197E-5F, 0.00645564636F, - 0.00728468178F, 0.00145657361F, -0.00721938536F, -0.00493407715F, - 0.00766523229F, 0.0044791624F, 0.0225774385F, -0.00130554941F, - 0.00212078588F, 0.00165288744F, 0.0125021664F, 0.00058041903F, 0.00934326F, - -0.00274564675F, -0.0128192026F, -0.00565362815F, -0.0103067243F, - -0.00654174294F, -0.0017818755F, -0.0159936491F, -0.0053675659F, - 0.000828165212F, -0.00836890843F, -0.00366056035F, 0.000364184612F, - 0.0163358878F, 0.00780608F, 0.00290531083F, 0.0186282601F, -0.0108684087F, - 0.00854967441F, 0.000729422201F, -0.00253496738F, -0.00860936474F, - -0.0144936899F, -0.00609366596F, 0.00345556322F, -0.00961837173F, - -0.0149157774F, 0.0169258974F, -0.00380954915F, 0.00209552934F, - 0.0161389727F, -0.0169949587F, -0.00630518794F, -0.003675238F, - -0.00756722363F, 0.00308210799F, -0.0187483579F, -0.00469103083F, - 0.01761044F, -0.000745660043F, 0.0044601066F, 0.00187854259F, -0.010245298F, - -0.000963423692F, -0.00675217109F, -0.0105470112F, 0.00962090213F, - -0.00351877208F, -0.00491228839F, 0.0106726745F, 0.0026162276F, - -0.00571527472F, -0.00907283649F, 0.0118473F, 0.00568325073F, - -0.00234832359F, 0.00321141188F, -0.0146692125F, -0.0150975827F, - -0.004752276F, 0.00325867021F, -0.00566213485F, -0.00925227627F, - 0.00308239646F, 0.00039747954F, -0.00381086254F, -0.00630952278F, - 0.0123808533F, 0.0145130847F, -0.0166160408F, -0.0307984482F, 0.0099128969F, - -0.00331180799F, -0.0226705279F, 0.011198137F, 0.008077031F, 0.0109178806F, - -0.0112824673F, 0.000542952097F, 0.0118751358F, 0.0047633471F, - 0.000305070484F, -0.00213137758F, -0.00218226411F, 0.00188497116F, - -0.00459562754F, 0.00571962306F, 0.0162552707F, 0.00947716553F, - -0.0126681086F, -0.00325930351F, 0.00429345341F, 0.000498150592F, - -0.0173184369F, 0.0155299883F, 0.00252803112F, 0.00394632528F, 0.0142551186F, - 0.0107613876F, 0.00956398528F, 0.00677040452F, 0.027708428F, -0.00942719076F, - -0.00530413119F, 0.00978034269F, -0.000722200144F, 0.00280668074F, - 0.00695476F, -0.0195480492F, 0.000636126439F, -0.0124440277F, - -0.00251402403F, 0.0015077811F, 0.0158195924F, -0.0221259277F, 0.0101487963F, - 0.00823280215F, -0.00889691F, -0.0134998308F, -0.00226165564F, - -0.000807667966F, 0.0125752492F, -0.0116378833F, -0.0059873811F, - 0.00514584128F, -0.0108393729F, 0.000948606932F, -0.00925081316F, - -0.00169524038F, -0.0158171393F, -0.00704084663F, 0.00493174791F, - -0.00366491196F, 0.0116134F, -0.0194813795F, 0.00182426616F, -0.00916841906F, - 0.011800278F, 0.00868491177F, 9.46484652E-5F, 0.00569023844F, - -0.00049411162F, 0.017724026F, 0.00566122076F, 0.0189306103F, 0.0018770257F, - 0.00335994526F, -0.00352249877F, 0.0161426868F, 0.0019803918F, - 0.000376859418F, -0.0168304145F, 0.0129937725F, 0.00438481942F, - -0.00298506767F, -0.00441258121F, -0.0103398468F, 0.00460666325F, - 0.00163314282F, 0.0168403517F, 0.0190167166F, -0.0108759906F, - -0.00402047206F, 0.0201576762F, -0.0141919209F, -0.0122508975F, - 0.00483820122F, -0.00549712172F, 0.00251208013F, -0.00347461132F, - 0.024095716F, 0.0109643657F, -0.00929380488F, 0.0106637832F, -0.00615208829F, - 0.00861097407F, -0.00285712699F, 0.0152267786F, -0.00339780236F, - 0.0183395222F, -0.00507153291F, -0.0013024275F, -0.000931856572F, - -0.0185432397F, -0.00868709106F, -0.00559654972F, -0.000783609226F, - 0.00795438327F, 0.0130040096F, 0.00521223946F, 0.00205375813F, - -0.0122633902F, -0.00230725715F, -0.00389086292F, -0.00508947903F, - -0.00655724481F, 0.0142272329F, -0.00862761866F, 0.00107988436F, - 0.0080444077F, -0.0180263165F, -0.0202360712F, 0.00658202171F, 0.0134827318F, - 0.0128338709F, -0.000501710456F, 0.0124201654F, 0.0106206192F, -0.014894045F, - 0.00907102693F, -0.00151290908F, -0.0086005656F, 0.00276373466F, - 0.0175740868F, -0.00242260098F, 0.0140497563F, 0.00633325242F, 0.0115455464F, - 0.0082961861F, 0.00826899242F, 0.0137538109F, 0.00072192651F, - -0.00769099034F, 0.0169647504F, -0.0054492671F, 0.0119671179F, - 0.00804950763F, 0.0116546759F, 0.0077061234F, 0.00806900766F, - -0.00165778829F, -0.0024254662F, -0.0069827768F, -0.0126559548F, - 0.00746998144F, -0.0111842249F, 0.00728113763F, 0.0012731239F, 0.0102259675F, - -0.00128018134F, 0.00707783597F, 0.0147452578F, 0.00633993791F, - -0.00765944924F, -0.00144687656F, -0.00890549F, 0.0021499428F, - 0.00370426662F, -0.0124130938F, 0.00905444752F, -0.00527060777F, - 0.000803530682F, 0.0122639928F, -0.00272743823F, 0.00508381799F, - -0.00380260102F, -0.000455121F, -0.00689896289F, 0.00382901728F, - -0.0123507511F, -0.00815127604F, 0.00677131116F, -0.00367671344F, - 0.00454696268F, 0.0106853684F, 0.00569757633F, -0.0134941153F, - 0.000497795176F, -0.00656220596F, -0.00535238534F, -0.00224090856F, - 0.0137261264F, 0.00297215628F, 0.0101651419F, -0.00200268603F, - -0.00189268496F, -0.00527955173F, 0.00497880299F, 0.00465062354F, - 0.0127352029F, 0.00957647F, -0.0188808963F, 0.0242482293F, 0.00466838526F, - 0.0137736574F, 0.00770598883F, -0.00390386372F, 0.00226698467F, - -0.00545364898F, 0.0242160279F, -0.0103057148F, 0.0150034763F, - -0.0127743063F, -0.00775394635F, 0.00303225196F, -0.00697438559F, - -0.0130490698F, -0.0221559815F, -0.0130574349F, -0.018086303F, - -0.0125770969F, 0.00647493359F, -0.00229191245F, -0.00901279878F, - 0.00215946906F, -0.000167108345F, 0.00904846657F, -0.00414098287F, - 0.0169053804F, 0.00172858883F, -0.0115317954F, 0.014270979F, 0.0127913319F, - -0.00596867502F, 0.00350901461F, -0.00834733248F, 0.00354931294F, - 0.00899378862F, -0.0197970886F, -0.00252307556F, -0.0171688646F, - 0.00339004304F, -0.00178327563F, -0.00114307902F, -0.00753294863F, - -0.00661932491F, -0.0218791906F, -0.00535419537F, 0.00550571131F, - -0.00725100748F, -0.00289562624F, 0.00456626F, 0.0121806134F, 0.015521327F, - 0.0196472108F, 0.0107973441F, -0.000543830625F, 0.00631985767F, - -0.0157710183F, 0.0114839533F, -0.00271997694F, -0.000327866757F, 0.0150298F, - 0.0155221801F, -7.57527378E-5F, -0.00211449433F, -0.0132906111F, - -0.0110836513F, 0.00868489686F, 0.00297843781F, 0.00227477308F, - 0.00731937913F, 0.0122248279F, -0.00882152468F, 0.00266712205F, - -0.00518288091F, -0.0170770381F, -0.00435101334F, -0.000414798065F, - -0.0134915877F, -0.0146737806F, 0.00182242331F, 0.000547766918F, - -0.00328752515F, 0.00989503507F, -0.00749730179F, -0.000311772514F, - 0.00682522263F, -0.000616145961F, 0.0105802296F, -0.00365458638F, - 0.0087308744F, -0.0174378492F, 0.00635722559F, 0.00495600048F, - 0.00908010639F, -0.00124674581F, -0.00425741775F, -0.00774564501F, - 0.00220343191F, 0.00563630648F, 0.0111994455F, -0.00115115789F, - 0.0146113737F, -0.00577454315F, -0.00213563815F, 0.0106628155F, - -0.00646717753F, 0.00677883066F, -0.00280963909F, -0.00292848633F, - -0.00170867553F, -0.0202577617F, -0.00805512536F, 0.0132134175F, - -0.0104741575F, 0.00485748379F, 0.00172231102F, -0.00999043789F, - 0.00210224744F, -0.00623283628F, -0.0059251138F, -0.0178994369F, - -0.00262920838F, -0.0017532903F, -0.00558077591F, -0.0135855479F, - 0.022080332F, -0.00073518086F, 0.00953563116F, -0.00412329379F, - 0.0100359749F, -0.013021809F, 0.00473299948F, -0.0058274297F, - -0.00569229946F, 0.0023167287F, 0.00712633738F, 0.00743163284F, - -0.00493628625F, 0.00314114336F, -0.00268424791F, 0.0036226036F, - 0.00871602818F, 0.00612580776F, -0.000570739678F, -0.0138350399F, - -0.0065042912F, -0.0177898984F, 0.00843959F, -0.00614147F, 0.0228227582F, - -0.000224537158F, -0.0177609641F, -0.00833050627F, -0.0137844291F, - -0.00110983639F, 0.00410453649F, 0.0100540118F, 0.00354176F, -0.00409958744F, - 0.0103641F, -0.0105443196F, -0.00478794472F, -0.013588707F, 0.0132101281F, - 0.000846490555F, -0.0052238605F, -0.00368746114F, 0.0106565719F, - 0.00433155242F, 0.0139706116F, -0.000286155788F, 0.0138713047F, - 0.0135073196F, -0.00536307134F, 0.000857351231F, -0.00805303361F, - 0.00739021087F, -0.015472318F, 0.0135449786F, -0.0123888822F, - -0.00482313149F, 0.00240243203F, -0.00382501842F, 0.0177453F, 0.00400759652F, - 0.0132665578F, -0.00414247299F, 0.0118900556F, -0.00721503887F, - -0.00195170729F, -0.00594298355F, 0.00153791881F, -0.00060144579F, - 0.00160648825F, -0.0010912756F, 0.00559146469F, 0.0106783537F, - -0.00147438259F, -0.00617585424F, 0.0126697272F, 0.00214067311F, - 0.00401678635F, 0.00717836898F, -0.00915001798F, -0.00292347651F, - 0.00900462177F, 0.0157663878F, -0.00273055234F, -0.00882121F, 0.00246428349F, - 0.00805669278F, -0.00288036256F, 0.00834194478F, -0.00350138335F, - -0.00540689286F, 0.000793219195F, 0.0074542868F, -0.00859474856F, - 0.0018764385F, -0.00449104048F, 0.00429684808F, -0.0107583767F, - 0.0026365614F, -0.00329437526F, 0.0102074249F, -0.0134453662F, - -0.000344918895F, 0.00817552209F, -0.00604703883F, -0.00699709449F, - 0.0025744359F, 0.00858567F, 0.00752337649F, 0.0124903414F, 0.00962971617F, - -0.0109679019F, 0.00510739675F, -0.00130990241F, 0.0023550333F, - -0.00982890371F, -0.00478925183F, -0.00734298909F, 0.00798226055F, - -0.0102136815F, 0.00749517139F, -0.00188309571F, 0.00312065822F, - 0.0091626551F, 0.00629343092F, 0.00510498881F, -0.011292588F, -0.0142923957F, - -0.00472248113F, -0.00112161343F, 0.010960279F, -0.00341801299F, - 0.00100630953F, -0.0113371238F, 0.00494692475F, -0.000468292244F, - -0.0138298925F, -0.00637220219F, 0.00323083042F, 0.00103394955F, - -0.00420061545F, 0.0119015165F, 0.00211841101F, 0.0108872885F, - 0.00150287303F, -0.0047110091F, 0.00354538555F, -0.00555453682F, - 0.00804346148F, -0.0124098305F, 0.000552808575F, 0.0002893481F, - -0.00124293368F, -0.00678527448F, 0.0078351358F, -0.00424471078F, - -0.0143150249F, -0.00374249881F, 0.00991661754F, -0.00326091377F, - -0.00291385385F, 0.00576390047F, 0.00212386739F, 0.0140623609F, - 0.0156282187F, 0.00310377F, -0.00138707156F, 0.00525097223F, 0.00568680186F, - 0.00212982F, -0.000645541528F, -0.0110112922F, 0.00249432353F, - 8.90076444E-6F, -0.000500367722F, -0.00423851283F, 0.0157301854F, - 0.00831181929F, 0.0127366893F, 0.0122394264F, -0.00576575194F, - -0.00493627368F, 0.00658247806F, -0.0133550987F, 0.0127549097F, - 0.00234180083F, -0.0189262F, 0.00841000769F, 0.0056524924F, 0.00948890392F, - -0.00937605742F, 0.00732179312F, 0.0256645903F, -0.00668053841F, - 0.00121572788F, 0.00065693754F, -0.0154595906F, 0.0073372866F, 0.00702532F, - 0.00926220603F, 0.0018716671F, -0.00603514444F, 0.00265315431F, 0.015402426F, - 0.0143303489F, 0.0105175301F, 0.00733677438F, 0.0146616204F, -0.00297060166F, - -0.0096584782F, 4.46987906E-5F, -0.00651120488F, 0.0013052125F, - 0.00476098154F, -0.0034083717F, 0.00699415337F, 0.023431113F, 0.0140170716F, - 0.00103336177F, 0.00595180364F, 0.0167850461F, 0.00450962316F, - -0.00224946835F, 0.00705294777F, 0.0116031785F, -0.001864305F, - -0.00812711846F, 0.00101681356F, 0.0116647752F, -0.00847555418F, - 0.00363899511F, -0.0083585782F, 0.0251935124F, -0.00973810349F, - -0.013066343F, 0.00651553972F, -0.0164834615F, -0.000320121821F, - 0.023078585F, -0.00648271199F, 0.024152156F, 0.0122167151F, 0.0152351437F, - -0.0166928973F, -0.00391626824F, 0.00290069776F, 0.00797262F, - -0.00862759445F, 0.00449936697F, 0.00829397421F, 0.0185913071F, - -0.0105097108F, -0.00945249293F, 0.00260338746F, -0.0152460299F, - 0.0014032434F, -0.00337719265F, -0.00240397174F, 0.00665555196F, - 0.00102961867F, 0.00412782282F, 0.010691179F, 0.00448733149F, 0.0134351496F, - 0.00152452209F, -0.00509639457F, -0.0119682085F, 0.0155097041F, - -0.00276093371F, 0.00326019805F, -0.00143218809F, -0.0133114466F, - -0.0013418569F, -0.00210568681F, -0.0073323804F, 0.00491109258F, - -0.0140239755F, 0.00438561896F, -0.00842521898F, -0.000960660516F, - 0.00474076811F, -0.0144008556F, 0.00122503657F, 0.0140282F, -0.0103659565F, - 0.00206242688F, 0.00688293576F, -0.0210363176F, -0.0135622518F, - -0.0154066607F, 0.0109723667F, 0.0181900114F, 0.010553278F, -0.0046828445F, - -0.00351013476F, -0.00890373625F, -0.00548343733F, 0.0172277652F, - 0.00209144852F, 0.0102508543F, 0.00743915327F, -0.00560240494F, - 0.0127892634F, -0.017939901F, 0.00429050485F, 0.0012981008F, 0.0110970698F, - 0.00198473898F, -0.00306193344F, 0.000980689F, 0.0135587025F, 0.010447802F, - 0.00870494F, 0.00226671F, 0.00362522085F, 0.0114848427F, -0.00382770901F, - -0.0140236691F, 0.00608713692F, 0.0103435516F, 0.000781612936F, - 0.0155892605F, 0.00098017545F, 0.00823711F, 0.00274497177F, 0.00991727412F, - 0.00640264619F, 0.0077612591F, -0.0034453175F, 0.0170682296F, 0.0050303014F, - -0.00434777653F, 0.00916321669F, -0.00938618928F, -0.013207674F, - -0.00818409491F, -0.00740695512F, 0.00947619602F, 0.00485895295F, - -0.0145485848F, -0.00225128606F, -0.0154742394F, 0.0106781349F, - -0.00908687152F, 0.0189753473F, -0.00862801913F, -0.00538377464F, - -0.00681232335F, -0.00517773908F, 0.0112615591F, 0.0130368387F, - 0.0113159278F, 0.0141851595F, -0.0135920215F, -0.0202871449F, -0.0140574202F, - 0.00070425868F, -0.001044909F, 0.00196633325F, 0.00398322288F, 0.0131795732F, - -0.00680857897F, 0.00250010798F, -0.00558832F, -0.0139142917F, 0.0221315194F, - 0.0078424057F, 0.00378044765F, -0.0212772656F, 0.0107541634F, 0.0198439192F, - -0.0101553826F, -0.0100441659F, 0.00525537506F, -0.0120193474F, - 0.00434548873F, 0.00301903673F, -0.00715834461F, 0.0016280103F, - 0.0166299753F, -0.00454183202F, 0.0134155117F, -0.0128837014F, - 0.00169309683F, -0.00804957F, -0.00293147285F, 0.00228262576F, - -0.00163162872F, 0.00448686117F, -0.02113593F, 0.0135856671F, - -0.00427228631F, -0.000311113807F, 0.00603758683F, -0.00327731599F, - -0.00356515381F, 0.0174513664F, 0.00548853865F, 0.0027270217F, - -0.0065829386F, -0.000215913111F, 0.00569883455F, -0.0187790748F, - -0.00120739057F, -0.00692157727F, 0.0010242766F, 0.00102818303F, - -0.00881895144F, -0.00762371253F, 0.0215816852F, 0.00271287723F, - -0.00924972072F, 0.00452813599F, 0.00150299084F, 0.00791400112F, - -0.0140429968F, -0.0215186626F, 0.00652530603F, 0.0082568517F, 0.018573733F, - 0.00290088076F, -0.0169545431F, 0.00194608246F, -0.005561003F, - -0.00618183939F, 0.0112953223F, -0.00815733336F, 0.0143330935F, - 0.0237336569F, 0.00457405811F, 0.0101723196F, 0.00762657588F, - -0.00236278027F, 0.0107713845F, -0.0040300074F, 0.0153160114F, 0.0144640384F, - 0.00366816926F, 0.00112418039F, -0.0112536829F, 0.00992161501F, - -0.00838798191F, 0.00473535573F, 0.0114337923F, 0.011495281F, 0.0130261825F, - 0.00348928105F, -0.0139159374F, -0.0157722216F, -0.000491327373F, - -0.0114630833F, 0.0012924769F, -0.00013215837F, 0.000821692112F, - 0.0143995695F, 0.00401173439F, -0.00142488081F, -0.00423183711F, - 0.0147672957F, -0.00299012172F, -0.0123679293F, 0.00065947749F, - 0.0242598075F, -0.0246372409F, -0.0186389312F, 0.00142172305F, - -0.00513418298F, -0.0119062802F, 0.0103827193F, -0.0130540421F, - 0.0119483322F, -0.0150401769F, 0.0059335432F, 0.00121057592F, - -0.00889085792F, 0.0075770556F, -0.00242636935F, 0.00376533321F, - 0.00801660679F, -0.010577119F, -0.0096848635F, 0.0166195761F, - -0.00369136105F, -0.00149116269F, 0.00426191045F, 0.00727794925F, - 0.00664245663F, 7.43318524E-5F, 6.25206885E-5F, 0.00698072463F, - 0.000544730865F, 0.0123677216F, 0.000952800503F, -0.0085620461F, - -0.00744376937F, 0.000118231619F, -4.45684E-6F, -0.00723574869F, - -0.011453527F, -0.00314070331F, -0.0121369502F, 0.0133150863F, - -0.0136243701F, 0.0168167185F, 0.00837091077F, -0.00674183294F, - 0.0112697398F, -0.0174357202F, 0.00454215519F, 0.00141920312F, - 0.00761016412F, -0.00168939249F, 0.0109856436F, 0.00512948027F, - 0.00675344653F, -0.00314506912F, -0.0010081348F, -0.00949807744F, - 0.00205727876F, -0.00534947403F, 0.012560281F, 0.0138306469F, 0.00874793064F, - 0.00615250831F, -0.0096176425F, -0.00431277556F, 0.0051211142F, - 0.0215921048F, -0.0076534343F, 0.0141017446F, -0.00698405504F, - 0.00219667586F, -0.0215213038F, 0.00257594511F, -0.00781562272F, - 0.00626004348F, 0.00391477672F, 0.00435310416F, 0.00182273041F, - 0.0071555851F, 0.00960727502F, -0.014571785F, -0.00189386157F, - -0.0103131859F, 0.00655451603F, 0.0169663578F, -0.0145042762F, - -0.00564607233F, 0.010263891F, 0.0202012938F, -0.00928006601F, - -0.00283661089F, -0.00896531437F, 0.00341532985F, -0.00170290202F, - -0.00205552881F, 0.00811835751F, -0.012045077F, 0.00745326234F, - -0.00145048695F, 0.00669162255F, 0.00946308F, 0.00190229982F, 0.00817505084F, - 0.0259409398F, -0.00115710124F, -0.00102946942F, -0.00565306516F, - -0.00646333024F, 0.00709874835F, -0.00259382185F, -0.00712221349F, - 0.00672187796F, 0.00493201427F, -0.00197944767F, 0.0068978942F, - -0.0068805851F, 0.00403091405F, 0.00139620237F, -0.0100373225F, - -0.0139132664F, 0.000861847831F, -0.0066883415F, -0.0112748481F, - 0.00758210849F, -0.010058729F, 0.0082664052F, 0.00454777479F, 0.00137589662F, - -0.00630138628F, 0.00813927874F, 0.0113315638F, 0.00258612563F, - -0.00838047266F, -0.00407344056F, -0.0111219464F, -0.0112061687F, - 0.0131022176F, 0.00509015843F, 0.0118957357F, -0.0022065104F, - -0.00259327656F, -0.0251954757F, -0.00141491636F, 0.00535871601F, - -0.00166251161F, 0.012742429F, 0.00458662584F, 0.0153925065F, 0.0169550274F, - 0.0162251238F, -0.0146402288F, 0.00161295093F, -0.00488233892F, - 0.0144287338F, 0.00228869938F, 0.00189602422F, -0.00747110834F, - -0.00423294911F, -0.00214821496F, 0.00970515795F, -0.000134748261F, - -0.0149366381F, 0.00355647714F, -0.0066648405F, 0.00550025282F, - 0.00576948794F, -0.0146771381F, -0.00499904901F, -0.000566946634F, - -0.00417601876F, 0.0124754356F, 0.00493492512F, -0.00376040628F, - 0.00578305079F, -0.0107741496F, 0.0035867244F, -0.000269045966F, - -0.0282024778F, -0.010268108F, -0.00405573286F, -0.00533753261F, - -0.00205198023F, -0.00305468985F, -0.00795328896F, -0.00860693678F, - -0.00286961021F, -0.0104869492F, -0.00306791742F, -0.00177164073F, - 0.0104060648F, -0.00954465941F, 0.0082863709F, -0.00427064253F, - -0.000336311961F, -0.00168773311F, -0.000273663318F, 0.014894573F, - 0.00371276983F, 0.00446647778F, 0.0217624065F, 0.015967736F, 0.0125999181F, - -0.00349820429F, -0.00290899212F, 0.00715101557F, 0.0124239409F, - 0.00952837337F, 0.00879783F, 0.0140139349F, 0.00920003653F, -0.00461740838F, - 0.000565929164F, 0.0161094666F, 0.0087933382F, -0.00828182232F, - 0.00103936344F, -0.00837738253F, -0.0188005529F, 0.00063427526F, - 0.0061412612F, 0.00672190683F, 0.000943803869F, -0.0322967656F, - -0.00842563249F, 0.0196152031F, 0.000894780213F, 0.0081507992F, - 0.00285600778F, 0.003875328F, -0.0153941615F, 0.0168170463F, -0.0129464949F, - -0.00436611054F, -0.00654216763F, -0.00218775892F, -0.00339658698F, - -0.0018857026F, -0.00175336155F, 0.00554834073F, -0.00761703402F, - 0.00489853509F, 0.00643779803F, -0.00794860348F, 0.0140623525F, - 0.0061426661F, -0.0101280939F, 0.00137188926F, 0.000764790864F, - -0.00495007867F, -0.00425491715F, -0.000802608498F, -0.0155429058F, - -0.00343720615F, 0.00316359615F, 0.021160353F, -0.00465429F, 0.00544042047F, - -0.00939958449F, -0.000690528424F, -0.00559907639F, 0.00769819319F, - -0.000935349613F, -0.00898070633F, 0.0101960581F, -0.00636490621F, - -0.000897122372F, 0.00370281399F, -0.0173922703F, 0.0148851564F, - 0.00126393861F, -0.0105776014F, 0.00647749752F, 0.0114795743F, 0.0155115854F, - -0.00469241617F, -0.0135879563F, 0.00417826371F, 0.0150743071F, - 0.00247026724F, -0.016874969F, -0.00730596296F, 0.00281813508F, - -0.00740363635F, 0.00991449598F, 0.00262122671F, 0.00501306076F, - -0.00778175145F, -0.00264300499F, -0.00199751928F, 0.00289271679F, - -0.0172654651F, 0.0162134729F, -0.00355903897F, -0.00583436759F, - 0.00606025802F, 0.00758572714F, 0.00125680189F, 0.00810318254F, - -0.00993831828F, -0.00231127185F, 0.000849216F, 0.00127669203F, - -0.00245334534F, -0.000264030619F, -0.00815307535F, 0.00040400945F, - -0.00207149331F, 0.00193976541F, 0.019309612F, -0.00317125977F, - -0.00818380062F, 0.000696740171F, -0.0215986986F, -0.000515180931F, - -0.0116677051F, 0.0100619029F, -0.000629970629F, -0.00160131801F, - 0.00466058264F, -0.00250403956F, -0.0129079008F, -0.00381341763F, - 0.0106498627F, -0.0172968172F, 0.00810460094F, 0.00833788142F, 0.0119936047F, - -0.0075081666F, 0.00723525323F, -0.00666159485F, 0.00945573F, 0.0110466043F, - 0.0093868291F, -0.00953175128F, -0.00703329546F, 0.00151919853F, - -0.0153254839F, 0.00282163848F, -0.011761779F, 0.0159386043F, -0.0181695744F, - -0.00342336087F, 0.00521530816F, -0.0124962246F, 0.0083758235F, - 0.0107267797F, -0.00681732455F, -0.0121429628F, -0.0150137106F, - -0.0113212513F, 0.00322663342F, 0.00684937183F, 0.00786424149F, - -0.00743343355F, -0.00931664463F, -0.0180697832F, 0.0192898791F, - 0.0130066779F, -0.00760278571F, 0.0166720804F, -0.0116926664F, - -0.00197843672F, -0.0196189936F, 0.00815954525F, 0.00335139595F, - -0.00780864526F, 0.00770741189F, 0.00500504812F, -0.00881878566F, - -0.000669459696F, 0.00642362284F, 0.00749142328F, -0.00345241767F, - 0.00254835654F, 0.0121332109F, 0.00272573903F, -0.00811164826F, - 0.0240550805F, -0.0118947122F, -0.00272820657F, -0.00664143311F, - -0.00823284F, -0.00219166256F, 0.0130363498F, 0.00307414657F, - -0.00794540532F, 0.00218795124F, -0.00401974283F, 0.00767659303F, - -0.00176402519F, 0.00695625134F, -0.0110476622F, 9.77569871E-5F, - 0.00336828781F, -0.00499755796F, -0.0075638541F, -0.00348921656F, - 0.00764386076F, -0.00171034585F, -0.00282498426F, -0.0026407002F, - -0.0108610829F, -0.00436555455F, -0.0192406662F, -0.00943510514F, - -0.00790305343F, 0.0086972F, 0.00268684654F, -3.99068085E-5F, 0.00724225538F, - 0.0135566499F, -0.0138929505F, 0.0113371909F, 0.00840076618F, 0.0013523167F, - 0.00244903495F, -0.0015369379F, -0.00428255461F, 0.00077684276F, - 0.0172432773F, -0.0177313201F, -0.0120961908F, -0.0114805074F, - -0.00434745895F, 0.00489061791F, 0.0103455177F, -0.00951826945F, - -0.0212421063F, -0.0077080694F, 0.0104874289F, 0.00619601784F, 0.0177061595F, - 0.00309537281F, -0.010877843F, 0.00453315303F, -0.0160040408F, - -0.00537816528F, 0.0110699302F, -0.00733648287F, 0.0109123811F, - 0.00478408718F, -0.00215240126F, 0.0091382172F, 0.0173986051F, - 0.00509301759F, 0.0236233287F, 0.00400711317F, -0.0212191399F, - -0.00443342235F, -0.0102185216F, 0.00194586953F, -0.00372188538F, - 0.00745858578F, 0.000797795365F, 0.00388405425F, 0.0279972423F, - -0.00586068677F, 0.00650063111F, -0.00424714154F, -0.0133687351F, - -0.00361574045F, -0.020521095F, 0.00986484438F, -0.00954030734F, - -0.00161870709F, 0.00221042824F, 0.0044154753F, 0.00584909646F, - -0.00348730688F, 0.0132055068F, 0.00350805651F, 0.0120423436F, - -0.00702875759F, 0.00316203409F, -0.00948429573F, 0.00382527243F, - -0.0225699563F, 0.0124524562F, 0.00780353229F, 0.000408890744F, - 0.0105011938F, 0.0111419773F, -0.00194916083F, 0.0104365861F, -0.0153030641F, - -0.00314281229F, 0.00764635578F, 0.001629862F, -0.0027574175F, - -0.0119749755F, 0.00153363671F, -0.00687769707F, -0.00507461838F, - -0.00359384459F, 0.00760962628F, 0.000426984363F, -0.0041548321F, - 0.00739133637F, -0.00754428329F, -0.00705089606F, 0.00287584215F, - -0.00819128286F, 0.0124293175F, 0.00342125399F, 0.00717269583F, - 0.0027019023F, -0.00587611692F, -0.0023482868F, 0.000869880954F, - 0.00983668212F, -0.01745007F, -0.0186315756F, 0.00939310156F, 0.0182153471F, - -0.0070136073F, -0.00588647416F, 0.0057888804F, -0.00860843435F, - 0.0099032633F, 0.0129669094F, -0.00228077453F, -0.00457726F, 0.00616990915F, - 0.00073969306F, 0.00338314194F, 0.000795107859F, -0.00824471191F, - 0.0015316176F, -0.00210959604F, 0.00264591887F, -0.00128975278F, - 0.00347595476F, -0.00535767758F, -0.00262930687F, 0.0131213218F, - -0.00121330447F, -0.00639643846F, -0.0046491148F, 0.0106915943F, - 0.00210838625F, 0.012646798F, -0.0027980383F, 0.0175776947F, 0.00290640141F, - -0.0243208315F, 0.00680288346F, -0.00428841682F, 0.019449072F, -0.017695656F, - -0.0181335341F, -0.0152494106F, 0.00703640049F, 0.00794689916F, - -0.0102944113F, -0.00149966055F, -0.0109736947F, 0.0124916993F, - -0.0169209801F, -0.0015310013F, -0.0115273399F, 0.0127758086F, - -0.00581402564F, 0.0157397594F, 0.0064766854F, 0.012260464F, 0.00453062961F, - -0.000523420109F, -0.015112455F, -0.00831019785F, -0.00905723497F, - 0.0071344832F, 0.010630439F, -0.00106728042F, -0.00315294508F, - -0.00579331676F, -0.00435585575F, -0.00213382766F, -0.000634915777F, - 0.00254717446F, 0.00223120535F, -0.00900588557F, 0.00284920353F, - 0.000838447129F, -0.0153937237F, -0.014710119F, -0.0029661567F, - 0.00262798928F, 0.014881596F, -0.00208268967F, -0.00701202359F, - 0.00492263958F, -0.0108876014F, -0.00104088895F, 0.00406494224F, - 0.0227159392F, 0.00750735821F, -0.016106382F, 0.0138953216F, -0.00760454079F, - -0.0123960525F, 0.00397477346F, 0.0100002103F, 0.0230161697F, - -0.00513809128F, -0.00431050221F, 0.00289890682F, -0.0167528782F, - -0.0173197351F, -0.0148998993F, 0.0124714505F, 0.00144671253F, - -0.000243946226F, -0.00806624722F, -0.0109389676F, -0.00149213232F, - 0.000225553478F, -0.0155933332F, 0.00672396552F, 0.0067718043F, - 0.00169195153F, 0.00767689524F, -0.007246817F, 0.0187772159F, 0.0106552923F, - -0.0100347735F, 0.0130765587F, 0.00889470708F, -0.00451606512F, - 0.0046229139F, -0.00384156872F, -0.00798917F, -0.00412072428F, - 0.00847204402F, 0.00557321496F, 0.00207434F, -0.0199341029F, 0.0040754904F, - 0.00926195737F, 0.00983222295F, -0.0105523216F, -0.0276005343F, - 0.00051029626F, 0.000973112299F, -0.00118559739F, -0.00391213829F, - 0.00172131509F, -0.000838432636F, 0.0225284975F, 0.00419977074F, - 0.00731295208F, 0.00867856853F, -0.0108711738F, 0.00343185221F, - 0.00984932482F, -0.00299103837F, -0.0182600953F, 0.0104311658F, - -0.0210454445F, 0.020290425F, 0.0240973458F, -0.0108083552F, 0.00377414608F, - 0.00710207131F, 0.00128387113F, -0.00929804333F, -0.0193148237F, - 0.0081669949F, 0.0109581035F, -0.000582537323F, 0.00559998443F, - -0.00538157579F, 0.012624939F, -0.010068845F, -0.0128877573F, - -0.00998974219F, -0.0036774741F, -0.0223477073F, -0.00428085402F, - -0.00262199296F, 0.00177733798F, -0.00529510761F, 0.00402447954F, - 0.00435374537F, -0.00277853501F, 0.00648944639F, -0.00480780285F, - -0.00212585251F, -0.00468863314F, 0.00195581373F, -0.0120093711F, - 0.00249675545F, 0.0126698995F, 0.00917253923F, 0.0111301737F, -0.0157264825F, - 0.0234607384F, -0.0120186443F, 0.00691974815F, 0.0253704693F, 0.00539149856F, - -0.0129827634F, -0.00430428283F, -0.00920753926F, 0.000134443486F, - -0.00573184295F, -0.00429227622F, 0.00513370242F, -0.00676815119F, - -0.00149322313F, -0.0013106335F, -0.00464333827F, -0.00952031743F, - -0.00900169369F, 0.00711385952F, 0.00711621204F, 0.00970472489F, - -0.00875929184F, -0.00971851777F, 0.0148287639F, -0.00661261659F, - -0.00174580317F, -0.00849061273F, 0.00432079332F, 0.00207258342F, - 0.00714684045F, 0.00786479842F, 0.00940140709F, 0.00012715593F, - -0.0206121746F, 0.0117304521F, -5.20374961E-5F, 0.000898540893F, - -0.0110690799F, 0.0080438694F, -0.00733759673F, -0.0013188197F, - 0.00451494427F, 0.013265783F, -0.00462513929F, 0.00413536793F, 0.0043407972F, - 0.00454509119F, 0.00619885512F, 0.0173150282F, -0.00508420262F, - 0.00896777865F, 0.0111719314F, -0.0196046F, 0.0119502358F, -0.01104734F, - 0.0187932644F, 0.0103291245F, 0.0179261621F, 0.00649487972F, - -0.000662829261F, -5.58169522E-5F, 0.00294543733F, -0.0038002003F, - 0.0136787202F, 0.00858630426F, -0.00986117776F, 0.000914307951F, - -0.00562069425F, -0.00542783458F, -0.00832463522F, 0.0094938362F, - 0.000174860048F, -0.00502236234F, -0.00233045267F, -0.0211456399F, - -0.011534824F, 0.00122142513F, -0.000972655893F, 0.00230046F, 0.00348316459F, - -0.00442896131F, -0.00622741785F, -0.000504770782F, -0.00462613488F, - 0.00772832194F, 0.00928600878F, 0.00790663064F, 0.00763315195F, - -0.0119007435F, 0.010295881F, 0.00907122064F, 0.000729961961F, - -0.0167822465F, -0.00294402801F, 0.00675084582F, -0.00159158779F, - 0.0155956596F, 0.00472722948F, 0.00218151091F, -0.0168250352F, - -0.0223371033F, 0.00548259448F, 0.012333639F, 0.0059332177F, -0.011982183F, - 0.00681166584F, 0.00695382245F, -0.00189582724F, 0.0138475774F, - -0.000644768705F, -0.00365231629F, -0.00252265111F, -0.00606310135F, - 0.00435586041F, -0.0128572397F, -0.00417368906F, -0.013674F, 0.0138783269F, - -0.00122014328F, -0.00333729014F, 0.0155971255F, 0.0189288147F, - 0.00477094529F, -0.00617283257F, 0.00392593164F, 0.0111048855F, - -0.0151810544F, 0.00123427773F, -0.00441326341F, -0.0235639382F, - -0.00359512889F, -0.010125652F, -4.06044201E-5F, -0.0101530962F, - -0.00544119766F, 0.00396726F, -0.0110936258F, 0.00588573F, -0.00514480518F, - 0.000689876557F, 0.00731833745F, -0.013789787F, 0.0164547823F, - -0.0049447692F, 0.000680350699F, -0.0171906129F, -0.0098317964F, - 0.00720529817F, 0.00481399754F, -0.00421498436F, -0.00390330353F, - -0.0142336432F, -0.0163873173F, -0.00036166457F, 0.00805059727F, - -0.00112152845F, 0.00147644628F, 0.0099464329F, -0.00214372575F, - 0.00617629895F, -0.00350399641F, 0.020782426F, 0.00352695119F, - 0.00962574501F, -0.00998376589F, 0.0162082464F, 0.00722268503F, - -0.0104742059F, -0.00298135797F, 0.00314949686F, -0.00557449926F, - -0.006466561F, -0.0139955636F, -0.00013208884F, 0.00789691F, 0.00278590899F, - 0.00677760551F, 0.012100772F, 0.0029283124F, -0.00553891249F, -0.0120423036F, - -0.0114433533F, 0.0122917062F, 0.00213094475F, 0.00298937224F, - -0.0140372887F, -0.00468687108F, 0.00794463512F, -0.00083497F, - 0.00276694517F, 0.000936646888F, 0.00395250181F, -0.00791291427F, - -0.000477748021F, 0.00343589F, -0.00678006094F, -0.000379367615F, - -0.00898162089F, 0.00428480469F, 0.0200700294F, -0.00093253731F, - 0.00431583775F, -0.00307561806F, -0.00126841583F, 0.0090364432F, - 0.00791335385F, 0.00637306599F, -0.0138642918F, 0.00701967767F, - -0.00280950498F, -0.0087188622F, -0.00259590987F, 0.0127828857F, - 0.00497643324F, -0.0040752925F, -0.0161138866F, 0.00617207913F, - -0.0156990644F, 0.00953873806F, 0.0100559415F, 0.00794275291F, - -0.00812445208F, -0.0129121495F, -0.00227121823F, -0.00711507583F, - 0.00116182037F, 0.0116757015F, 0.00183367729F, 0.00162223028F, - -0.00552367186F, -0.0203594044F, -0.0101523707F, 0.00848255586F, 0.02384899F, - -0.0105114728F, -0.0070567592F, 0.00323917624F, -0.00225985772F, - 0.0190023426F, -0.00563691091F, 0.00723541435F, 0.0025790506F, - -0.0120779769F, 0.00297061F, 0.00635577831F, 0.00649086339F, -0.0115545494F, - 0.00474227592F, 0.0120600481F, 0.00497258175F, -0.00346746552F, - 0.000221091526F, 0.00536556542F, -0.00410040468F, -0.0132604623F, - -0.000791531813F, -0.00432282919F, 0.000842723F, -0.00457171677F, - 0.0117061045F, -0.000535614F, -0.00503683416F, -0.0188546386F, 0.0149514116F, - -0.00961786509F, 0.00297480938F, -0.00926363841F, -0.00925259292F, - 0.000208346857F, -0.0018243494F, -0.00251447456F, -0.00126234407F, - 0.0066560111F, -0.00645601749F, -0.0204988476F, -0.00655021425F, - 0.00596470712F, -0.0103598814F, 0.0183906164F, -0.00737406779F, - 0.0103224479F, 0.00829242077F, 0.00656144833F, 0.00416936027F, - -0.0190096386F, -0.00754072331F, 0.00196578586F, -0.00363978534F, - 0.0203558058F, 0.000812186045F, -0.00993847288F, -0.0015694513F, - 0.000760114403F, 0.00897039566F, 0.000523280527F, -0.0179890152F, - 0.000405217346F, 0.000231169164F, 0.0115692588F, -0.0082192421F, - -0.00957744662F, -0.00481620478F, -0.000612500706F, -0.00845285878F, - -0.00867082085F, 0.0127781397F, -0.00630759168F, -0.012754309F, - 0.00972007401F, 0.00267585926F, -0.00451608468F, -0.00373515952F, - 0.011175843F, -0.00979697425F, -0.00328833889F, -0.0134944404F, - -0.00826672278F, 0.0128550911F, 0.00148825196F, 0.0130696585F, - 0.000347145047F, 0.00243606395F, -0.0132997306F, -0.0140949404F, - 0.000565198716F, 0.00256586447F, 0.00149594026F, 0.0171864685F, - 0.00867621601F, -0.00386276841F, -0.0183014125F, 0.0152394753F, - -0.00451810239F, 0.00315666921F, 0.0120598478F, -0.00342485565F, - -0.0123270219F, 0.0124353347F, 0.0188141037F, -0.00741204154F, 0.003539016F, - 0.00392770674F, 0.000581973F, -0.00350751751F, 0.0128597748F, 0.0157653F, - -0.0335102417F, 0.00305139506F, -0.00540133938F, -0.016855415F, - -0.0237436518F, -0.0162416939F, -0.00490247877F, 0.000695343944F, - -0.0010352683F, 0.00164571078F, -0.00173093751F, -0.0106054945F, - -0.00754503673F, -0.0090239346F, -0.00480262609F, -0.0087678805F, - -0.0119363852F, -0.00725627F, 0.000602538523F, 0.00234506116F, - -0.000616236532F, 0.00461744191F, -0.00777169457F, -0.0080569787F, - 0.0164970905F, -0.0103611425F, -0.00730937906F, 0.0017940118F, - -0.00801070686F, 0.0100957649F, 0.00150335522F, 0.00819803495F, - 0.00555609539F, 0.0135721406F, 0.000386538159F, 0.0215500686F, - 0.00457720226F, 0.00553927524F, -0.00172326225F, 0.00569211133F, - 0.000413161295F, -0.0219986476F, -0.00391366938F, 0.0115583437F, - -0.00925865769F, -0.00374535727F, 0.0211153571F, 0.01469032F, - -0.00473700045F, -0.00500097219F, -0.0034367051F, -0.00752872787F, - -0.00797212403F, -0.00448509026F, -0.00484950654F, 0.005634306F, -0.0046097F, - 0.00217105774F, 0.00165470876F, -0.00440964662F, -0.0123960758F, - -0.00153830857F, -0.00641982537F, -0.0123424716F, -0.007616763F, - 0.00467157084F, -0.00153397862F, -0.00133418432F, 0.00378151448F, - -0.00366233219F, -0.00555623602F, -0.000632180949F, -0.00737196812F, - -0.00468883803F, 0.0083529586F, 0.00540932966F, 0.0118075274F, - -0.00726576149F, -0.00230831909F, -0.000304011162F, -0.00181799685F, - 0.00915016F, 0.00484169647F, -0.00307458895F, -0.00244850502F, 0.0150283184F, - 0.00746246614F, -0.00073135295F, -0.00728217838F, -0.00452322373F, - -0.00195476064F, 0.00259513524F, -0.0050963047F, 0.00726579549F, - 0.000297987513F, 0.00351123908F, 0.0129080322F, -0.0163361039F, - 0.00182777725F, 0.00398038607F, -0.009322308F, 0.00257103238F, - 0.00171558047F, -0.00529206637F, -0.000950421789F, 0.00149407319F, - -0.000698405551F, -0.00807160325F, 0.00203372748F, -0.00581592275F, - -0.00224545388F, 0.0125716515F, 0.00523502F, 0.00605784636F, 0.0093606757F, - 0.00647559669F, 0.000499774353F, 0.00505174976F, 0.0095899F, 0.0192356F, - -0.0165691841F, -0.00497508422F, -0.0175684821F, 0.00840991549F, - 0.0104693957F, 0.00889684539F, -0.0017906609F, -0.00881965924F, - 0.0036834178F, -0.00261252141F, -0.000458494702F, -0.00544042466F, - -0.0127246091F, -0.00289757643F, 0.00287306635F, -0.0133255543F, - -0.0167529341F, 0.00487803156F, 0.0115279732F, 0.00368228648F, - 0.00732686743F, 0.00277662114F, -0.0136962477F, 0.00243109791F, - -0.00444001658F, 0.000935167F, -0.00189481548F, 0.0126653882F, - 0.00685895747F, 0.01161459F, -0.023035828F, 0.00991305523F, -0.00261340593F, - 0.00296763494F, -0.00309812021F, -0.0108481655F, -0.00637197541F, - -0.012140613F, 4.8622227E-5F, -0.00884785131F, 0.0029860686F, -0.0149114775F, - 0.0172778033F, -0.00258010672F, -0.00392129458F, -0.00429174583F, - -0.00931199733F, 0.00602102745F, -0.0207512043F, -0.00779839931F, - -0.0017006601F, 0.00606743433F, -0.0027887309F, 0.00627659401F, - -0.00190100411F, 0.0125040058F, -0.00518286135F, 0.00219674385F, - -0.000379778678F, 0.00907825865F, -0.00566202728F, 0.00690135825F, - 0.00360430568F, 0.0150043163F, 0.00143291068F, -0.00877803192F, - 0.00173861033F, 0.00404943293F, 0.00702243904F, -0.000802155526F, - 0.00179558888F, -0.0117141679F, 0.00775619969F, 0.00621217676F, - -0.00396458432F, 0.0114595629F, -0.0065730135F, -0.00890753418F, - 0.0191805884F, -0.00848544296F, 0.00303744781F, 0.0139948381F, - -0.00644645374F, 0.00873754453F, -0.00458050752F, -0.00235956791F, - -0.00398161681F, 0.0102699865F, -0.00386679522F, 0.000922268664F, - -0.00419282867F, 0.00566975679F, -0.00783174112F, -0.0105134882F, - -0.0116735622F, -0.000890072959F, -0.00577426562F, 0.00920523051F, - 0.0010040456F, -0.00220193923F, -0.00350050582F, -0.00933663F, - -0.00107506721F, -0.00106311089F, -0.00385356718F, 0.00736565795F, - 0.000712030684F, 0.00323308189F, 0.0122865494F, 0.00409780024F, - 0.00575295463F, 0.0086960746F, 0.013276617F, 0.00213231402F, -0.0108945863F, - 0.0118438769F, 0.00860696845F, -0.0172001533F, 0.00110998459F, - -0.000500355498F, -0.00303781615F, 0.0025913904F, 0.00737693487F, - -0.010273166F, 0.00549404137F, 0.00421761675F, 0.00751112588F, 0.0131955463F, - 0.00157915603F, -0.00784053747F, 0.00739056664F, -0.00394338463F, - 0.00585287F, -0.00408558408F, 0.0269865058F, 0.0054733078F, -0.015155714F, - -0.0102390898F, 0.0147147421F, 0.00192872854F, 0.00761825731F, - -0.00316637824F, 0.0042610094F, -0.0101006422F, -0.00305733108F, 0.00439353F, - -0.00442063389F, 0.000926945824F, 0.00866651721F, 0.0126618054F, - -0.00234895083F, 0.00379330618F, 0.00673607411F, -0.0208111275F, - 0.0105519658F, -0.0121965082F, -0.00101246033F, -0.0093228817F, - -0.00921902899F, 0.00134528801F, 0.0119537516F, -0.00231776689F, - -0.00261772843F, 0.00347825815F, 0.00801002327F, 0.00983831752F, - -0.00276346924F, 0.011201052F, 0.00800748449F, -0.0151860993F, 0.0228489116F, - 0.00918148737F, -0.00196247641F, 0.00215242151F, -0.00340485689F, - -0.00504127052F, 0.00465635909F, -0.00272964337F, -0.00904042367F, - 0.0040134117F, -0.00035349105F, -0.00642775046F, -0.0145869637F, 0.00432115F, - 0.0116889337F, -0.00707670674F, 0.016529778F, 0.0158410929F, -0.00745107839F, - -0.00977396686F, -0.00723144086F, 0.00362433214F, 0.00939558074F, - -0.011865993F, 0.00799155422F, -0.00634419406F, -0.00477631623F, - 0.0132598281F, -0.00086604018F, 0.00990527403F, 0.0287182424F, - -0.0209107418F, 0.000161878925F, 0.00234186021F, -0.0118979439F, - 0.00675317971F, -0.00868720096F, -0.00180710154F, 0.00686260546F, - 0.00930568855F, -0.012383515F, 0.00598594826F, -0.000510155049F, - 0.0135641303F, 0.0103260512F, -0.00719299447F, 0.00856077299F, - -0.000900336599F, 0.0257358011F, -0.0247964207F, 0.0055705253F, - 0.00732634636F, 0.00145727885F, -0.00635424349F, -0.00536648557F, - 0.0101824412F, 0.00191459199F, -0.00520156929F, 0.00995812751F, - 0.000773243431F, 0.00767863821F, 0.00390174054F, -0.01129237F, - -0.00153319025F, 0.00743278256F, 0.00802649744F, -0.00834590476F, - -0.00755131198F, 0.00595405791F, -0.00992513075F, 0.000973269867F, - -0.00648611505F, -0.00745276595F, 0.00312422F, -0.000287875708F, - 0.00238491339F, -0.0045724595F, -0.00372200785F, -0.00447069667F, - 0.0046624057F, 0.0025015797F, 0.00148263469F, 0.0158227012F, -0.00668143574F, - 0.0191334561F, 0.00495301699F, -0.00377634983F, 0.0161895473F, - -0.00125626544F, 0.0121356817F, -0.00581748225F, 0.00166776567F, 0.00349789F, - 0.00745144067F, -0.00230446318F, -0.00181441021F, -0.00436705863F, - -0.00722735375F, 0.00539252721F, -0.0115150688F, 0.00877788197F, - -0.00336071523F, -0.0115306238F, 0.00179974083F, -0.0144495638F, - 0.00683019077F, -0.00479940325F, 0.00203003152F, 0.00625120895F, - 0.00132486783F, -0.00490034232F, 0.00102429F, -0.000754430133F, - 0.00108699745F, -0.00519843865F, -0.00753736403F, -0.00614544144F, - 0.0266418364F, 0.00954670459F, -0.00318239746F, 0.00396223972F, 0.00995916F, - 0.00582910609F, 0.00896714814F, -0.00276323454F, 0.0109282313F, - 0.00465915399F, -0.0010900062F, -0.0166530367F, 0.00697983103F, - -0.00309390621F, -0.0060229674F, -0.00732882F, -0.0106695592F, 0.0202531442F, - 0.0138813714F, 0.00703211641F, 0.0161025636F, 0.000890751544F, - 0.00136764278F, 0.00179824105F, -0.0010310302F, 0.0139862364F, - -0.000930947717F, -0.00623694155F, 0.0140102971F, -0.0116638318F, - -0.0149286129F, -6.75590927E-5F, 0.0169024374F, 0.00635639392F, - 0.00017778833F, 0.00320252753F, 0.0138797015F, 0.00716053974F, - -0.0191785432F, 0.013470564F, -0.00134599081F, -0.00242397678F, - -0.00563905481F, -0.00114869571F, -0.000466108933F, -0.00963975582F, - -0.00268617389F, 0.00962248072F, -0.0185742397F, -0.00727061927F, - -0.00888308231F, -0.0218014307F, -0.0196998324F, -0.00840142276F, - -0.00864954479F, 0.0106707113F, -0.0129712F, -0.00108270487F, 0.0123242959F, - -0.0089670876F, 0.00470101321F, -0.0137033584F, 0.0156808812F, 0.0121687055F, - 0.00426464342F, 0.00834007375F, -0.00678326236F, -0.00339378882F, - 0.0125841163F, 4.4010565E-5F, 0.0163675398F, -0.000363977626F, - -0.0125234062F, 0.00471957354F, 0.00738908F, 0.0095264893F, 0.000773205713F, - 0.010091952F, 0.00781196868F, -0.00728389155F, -0.0034143941F, - 0.00174501655F, 0.00238694879F, -0.00122188765F, 0.011246033F, - 0.00397579093F, 0.00815130305F, 0.0070502758F, 0.000659322483F, - -0.00885432307F, 0.0104608126F, -0.0113717178F, -0.00847050268F, - 0.00138803537F, -0.00406540325F, 0.00481297588F, 0.0103224684F, - -0.00720983651F, -0.00294603407F, -0.00260901125F, 0.000727275677F, - -0.014549531F, -0.000722901896F, 0.00372921419F, 0.0048472113F, - -0.0130921537F, -0.000788825331F, -0.0134943575F, 0.0154132713F, - 1.69306459E-5F, -0.00549443625F, 0.00933672208F, 0.00745478645F, - -0.0114843696F, 0.0091111036F, -0.00905439F, 0.0110139214F, -0.00541363517F, - 0.00250945054F, -0.00141642522F, 0.000952231116F, -0.00345804542F, - 0.00404731696F, -0.0045870794F, -0.00374977756F, 0.0010503137F, - 0.00524790259F, 0.025379071F, -0.0134256734F, 0.0152109712F, -0.0184182562F, - 0.00648567639F, 0.00176582555F, 0.00766225345F, -0.00261772727F, - -0.0112489387F, -0.00303667667F, 0.0152626475F, 0.0109371608F, - 0.00570491748F, 0.00390741648F, -0.000787553377F, -0.00369617459F, - 0.000385374966F, -0.0110673271F, 0.0077152662F, 0.0176503211F, - -0.00484138168F, 0.0141075198F, 0.00861831941F, -0.00950800814F, - -0.000295786653F, -0.00513949851F, 0.00924950093F, -6.41234219E-5F, - 0.00862218812F, 0.00419433182F, -0.0196747482F, -0.00725484313F, - -0.00867340527F, -0.00559152523F, 0.00398839172F, -0.00984991249F, - -0.00143705506F, 0.00213939138F, 0.00103385351F, -0.00425129849F, - -0.0114604412F, 0.0157137215F, -0.0106643289F, -0.00537628261F, - 0.0112631293F, -0.0149087841F, -0.00549006602F, -0.0108095016F, - -0.022195369F, -0.00554446643F, 0.00915054418F, 0.00313497568F, - 0.00646973681F, 0.00773836253F, -0.00830875244F, 0.00733810104F, - 0.00138435035F, -0.019675523F, 0.0148881087F, -0.00032582003F, 0.0101368865F, - -0.0250605289F, 0.0171469692F, -0.00143504899F, -0.0085831685F, 0.00609264F, - 0.00550192874F, 0.0158152692F, 0.00449048961F, -0.0116144745F, - 0.00723899808F, -0.00387539552F, -0.00714965723F, -0.0160800591F, - -0.0049961363F, 0.0169060417F, -0.0123247541F, 0.0142130852F, - 0.000193251501F, 0.0155124729F, -0.00302926963F, -0.0068000122F, - -0.00758807221F, 0.00316975568F, 0.0151801948F, -0.00924178772F, - -0.00779551733F, 0.0062678745F, 0.0110391779F, -0.00266630948F, - 0.00492010545F, 0.00674678665F, 0.00103268668F, 0.0140390312F, - 0.00920079555F, 0.0100231962F, 0.016499741F, -0.0136012184F, -0.012440443F, - 0.00510911876F, 0.00740455557F, 0.000589887612F, -0.00418034056F, - 0.0052766283F, -0.00730124675F, -0.0139075881F, -0.0109732924F, - -0.0187108032F, 0.00907535106F, -0.00338908145F, 0.00954924803F, - 0.00564338779F, 0.0125999209F, 0.00151040265F, 0.00956432056F, 0.0153654218F, - -0.00412118714F, 0.00787730701F, 0.01689847F, 0.0047974172F, -0.00818173401F, - 0.00253499695F, 0.00153163227F, -0.00521354657F, -0.0091850115F, - -0.00314962119F, -0.00069532136F, 0.000171634078F, -0.0189433731F, - 0.00469047111F, -0.0142784743F, -0.00954244286F, 0.00464477064F, - 0.00689683063F, 0.0117203696F, 0.00834811851F, 0.0261615962F, 0.00574172428F, - 0.013535372F, 0.00941153243F, -0.010151457F, -0.00346088805F, -0.012786557F, - 0.000186219884F, 0.017741343F, 0.00228204601F, 0.000546317F, -0.0127208475F, - 0.00842826441F, 0.00946015492F, -0.0121495919F, 0.00133382285F, - 0.00487780664F, -0.00619011745F, -0.000152831024F, -0.0140167456F, - -0.00151648652F, 0.0187110677F, -0.0144462977F, -0.00747290533F, 0.0095089F, - -0.00265131122F, 0.00845226366F, -0.000863924331F, -0.015363724F, - -0.00242610346F, -0.00418345584F, -0.00827665906F, 0.0136916758F, - -0.00599853508F, -0.00922616199F, 0.0108493343F, 0.00248358282F, - -0.0156134563F, 0.00672592036F, 0.0148731731F, 0.0162019618F, 0.00403767684F, - 0.00911583286F, 0.0108738132F, -0.000694450224F, 0.01593142F, -0.00970616F, - 0.0034039123F, -0.00221232581F, 0.000124788188F, -0.0147504322F, - -0.0104823969F, -0.0172180925F, -0.00864018872F, -0.00720432587F, - -0.00794232171F, -0.0160148498F, -0.000775434892F, -0.008555796F, - -0.00789085589F, -0.0184006207F, -0.00919980183F, 0.0131912595F, - 0.0063252449F, 0.00430688635F, -0.0137962447F, 0.010648842F, -0.0175536796F, - -0.00491572637F, -0.00231939927F, -0.00771714421F, 0.0102560651F, - 0.0145992031F, -0.00956946798F, 0.0147346305F, -0.0211266689F, -0.00793983F, - -0.00298659F, 0.0111371819F, -0.0116433185F, 0.00428261841F, -0.00231485022F, - 0.00904576574F, -0.00398364523F, -0.00609606458F, -0.0240987018F, - 0.0150321983F, 0.00926030334F, 0.0142641263F, 0.000367589673F, - -0.00323228957F, 0.00816735625F, 0.026349714F, 0.00231416104F, - 0.000100375983F, -0.00557501754F, -0.00771866646F, 0.00948317256F, - -0.00380726834F, 0.0129171778F, -0.0065274043F, -0.00819791388F, - -0.00252825394F, 0.0164541F, 0.0063590128F, 0.00704054581F, -0.00982295256F, - -0.00399822835F, -0.0136758992F, 0.00180646393F, -0.00673274742F, - 0.0177923702F, 0.00268647028F, 0.0190770309F, 0.0132162189F, 0.000104907995F, - -0.0014703226F, -0.00672221696F, -0.0106909061F, -0.00649530627F, - -0.00378820556F, -0.00783200283F, 0.00226965826F, 1.11132831E-5F, - 0.00795143284F, 0.0146155404F, -0.010949133F, -0.0110204313F, 0.0132446438F, - 0.0142296916F, -0.00685016625F, 0.00184792653F, 0.00125829701F, - -0.00645567896F, 0.0139270751F, -0.00537951151F, -0.0102701159F, - 0.00251833186F, 0.0061853528F, 0.00614049379F, 0.00704455748F, - 0.00815843884F, 0.00937681552F, -0.000479533017F, -0.00932419393F, - -0.0202657226F, -0.0050217458F, 0.0111330114F, -0.000411655259F, - -0.0134520493F, 0.0165889524F, 0.0194863454F, -0.00662277173F, - -0.0220409706F, 0.00256829686F, 0.0162151307F, -0.0128008714F, - -0.00642022677F, -0.0111294733F, -0.00494919252F, 0.00574131F, 0.0151616111F, - -0.0187698286F, 0.00286576943F, -0.00728776166F, 0.0161965918F, - 0.000250050856F, 0.00252675614F, -0.000289755058F, -0.0104485191F, - 0.00499527296F, 0.0073263878F, 0.00326521392F, -0.0154812736F, 0.0084876474F, - 0.0016623179F, 0.00657253619F, -0.000628777896F, -0.00732065784F, - -0.0242244769F, -0.0233551878F, -0.000838227F, -0.0163409151F, - -0.00146473642F, -0.00327051897F, -0.00633288361F, -0.0102114994F, - 0.00118483044F, 0.00738409255F, 0.0071711163F, -0.0111545697F, 0.0164673682F, - -0.000258845626F, 0.00925333425F, -0.00967683829F, 0.000511894701F, - -0.0107122008F, 0.0106968032F, 0.00439794268F, -0.000714440539F, - -0.00267269253F, -0.0150485F, -0.00292879832F, -0.00968064927F, - 0.00963227544F, -0.0243440066F, -0.00654831855F, 0.00404619519F, - -0.00866792258F, -0.0112676723F, 0.00494225184F, -0.007238633F, - 0.00076499529F, 0.0022301611F, 0.00146753504F, 0.00806543417F, - 0.00888237823F, -0.0110088764F, 0.00762538146F, -0.00809276197F, - -0.00256925565F, -0.00683299825F, 0.00362816732F, -0.00103048317F, - -0.00096813048F, -0.00789350551F, 0.00610338384F, 0.0015184246F, - 0.00983025227F, -0.00576945068F, 0.0126395468F, -0.00896608F, 0.0109300409F, - 0.0125047835F, 0.00769165764F, -0.00867586862F, -0.00537257921F, - 0.0047991504F, 0.00442791963F, -0.00992926583F, 0.00281486264F, - -0.00195395132F, 0.0123516675F, 0.00133196847F, -0.00854555797F, - 0.0102876527F, -0.00310836197F, 0.000787768629F, 0.00649639498F, - 0.0112901274F, 0.0215730295F, -0.00719993655F, 0.0200077705F, 1.44664909E-5F, - -0.00526740262F, 7.75871376E-5F, -0.00146834436F, 0.00187974225F, - -0.00440236274F, -0.000268171862F, 0.0209786575F, -0.00826928671F, - 0.00340355607F, -0.00427841954F, -0.0186470225F, 0.000557234569F, - 0.00342354388F, 0.00865400676F, 0.00467632F, -0.00770649593F, - -0.00327051105F, 0.00797586422F, -0.00584913045F, 0.000633101619F, - -0.00774386851F, 0.0103004985F, 0.00473448168F, 0.00634875149F, - -0.00135502708F, 0.00241605192F, 0.0103286151F, -0.00856031105F, - 0.0010332145F, 0.0151936961F, 0.00815227535F, 0.00575771648F, -0.0103442511F, - -0.00392280566F, -0.00376727642F, 0.00806682743F, -0.0140343942F, - 0.00132325722F, -0.00407608598F, 0.00190718623F, -0.00210620975F, - 0.000421772682F, 0.00329762907F, 0.00836628769F, 0.01071567F, - -0.00550712366F, -0.0165726785F, 0.0107189342F, 0.00498552062F, - 0.0108948145F, 0.0011474113F, -0.000192802458F, -0.00267960038F, - 0.00842992496F, -0.00223755F, 0.000298793195F, -0.0120958546F, - -0.00897018053F, 0.0180180911F, 0.00374140358F, -0.0109159369F, - 0.00246259593F, 0.00126304559F, -0.0120124323F, -0.0100036552F, - 0.000880712527F, 0.00419322588F, 0.000469217106F, 0.00703336F, - -0.00877644122F, -0.00369031681F, 0.00219592103F, -0.00371386716F, - 0.00500494149F, 0.00917717069F, -0.0143741509F, 0.0109455055F, - -0.00451771216F, 0.00495191F, -0.00897487532F, -0.00377442455F, - -0.00198553945F, 0.0196541715F, 0.00144815643F, -0.0040536439F, - 0.000909410068F, -0.0110106086F, 0.0111854421F, 0.00386020169F, - -0.00615293253F, -0.0181933958F, 0.000675696472F, -0.00746056F, - 0.00187565689F, -0.0169741139F, -0.00873385184F, -0.00207961909F, - -0.0115066711F, 0.00675175851F, -0.0113737797F, 0.00661008572F, - -0.00725402823F, 0.00140222616F, 0.00842152722F, 0.00238665449F, - 0.0108235413F, 0.000947547087F, -0.00269542215F, -0.0196115579F, - -0.00562414946F, 0.0057081962F, -0.00282908301F, -0.00142030208F, - 0.0017571596F, -0.00594620453F, 0.00802086666F, 0.00264915684F, - -0.0102071818F, -0.0097665051F, -0.0194889959F, -0.00105719245F, - 0.0093193939F, 0.0216147304F, 0.00412557041F, -0.00881512184F, - 0.00524933962F, 0.0101820212F, -0.0171561148F, -0.00676430343F, 0.013915698F, - -0.0121293729F, -0.0108995717F, -0.00189409964F, 0.00233556284F, - -0.014828397F, 0.00371806766F, -0.00398731092F, 0.00616267649F, - -0.00491158897F, 0.0201043524F, -0.00413698098F, -0.00155020715F, - -0.00216306397F, -0.00306302495F, -0.000878996507F, -0.0104690958F, - -0.010484538F, 0.00500058848F, -0.00754213147F, -0.00790341478F, - 0.000488002639F, -0.00134511327F, 0.0065650451F, -0.0163102373F, - 0.0291663762F, 0.00355642568F, -0.00443908526F, 0.00690863747F, - 0.00425650319F, -0.00629171543F, 0.00296416436F, -0.00446924288F, - -0.00779294223F, -0.00564345811F, -0.00936761592F, -0.00316986418F, - 0.0114795137F, 0.0212487318F, -0.00459244614F, -0.00611493597F, - -0.00299048238F, -0.00879552402F, -0.000683120743F, 4.42994242E-5F, - -0.00277484115F, -0.00243704743F, -0.0276346859F, -0.00138718355F, - 0.00263035367F, -0.000347637775F, 0.0138608962F, 0.00974132866F, - -0.00555440597F, -0.0130942091F, 0.00220297719F, 0.0102257011F, - -0.0150756193F, -0.00549596502F, 0.00272910879F, -0.00978357065F, - -0.000885166111F, -0.00893496443F, -0.00614585914F, 0.01383611F, - 0.0140352687F, 0.013021823F, -0.00717956852F, -0.00921954401F, - -0.000862207613F, -0.0071055661F, -0.00370849529F, -0.0109610902F, - -0.000354896416F, 0.00744962413F, 0.0142367156F, -0.00217333226F, - -0.00778367324F, -0.00637529232F, 0.00606281636F, 0.0164012443F, - -0.0123717701F, -0.0184953175F, -0.00422341283F, 0.00956390798F, - -0.0052719661F, -0.0106038693F, 0.00276168878F, -0.00128369709F, - 0.0208364166F, -0.00371635705F, 0.000342648884F, -0.00516689F, - -0.00970442593F, 0.00736278528F, 0.00502403593F, -0.00240309862F, - 0.00422649411F, 0.00110212667F, -0.000660774414F, -0.00326261879F, - 0.0198769458F, -0.00421808707F, -0.00141271413F, -0.00229880097F, - -0.00988071691F, 0.0151349911F, 0.00650340132F, -0.0173374787F, - -0.025947636F, -0.00944580138F, 0.00510166865F, -0.0083172489F, - -0.00762071181F, 0.0178215448F, -0.0214674808F, -0.00585752493F, - -0.0113632204F, 0.00704593537F, -0.0185061488F, 0.00723973103F, - 0.00771129318F, -0.0122071924F, 0.0123608261F, -0.00180279464F, - 0.0120124798F, -0.00328594306F, -0.000303039647F, -0.00907476153F, - -0.00924393907F, 0.000829685421F, -0.00175565341F, 0.0081527466F, - 0.000845164293F, 0.0102894651F, 0.000309331226F, 0.00246781134F, - 0.0199937709F, -0.00265041133F, 0.0081087565F, 0.00410093414F, 0.0072259726F, - -0.0102121076F, -0.00874435529F, -0.0130140707F, -0.00274571078F, - -0.00862057321F, 0.00605067518F, -0.00173784164F, 0.0126540437F, - -0.0109936735F, -0.00306858984F, 0.0156154037F, -0.0013175467F, - -0.00316483341F, -0.0148909166F, 0.00110554474F, -0.00335965073F, - -0.00195296039F, 0.00574055221F, 0.0138968527F, 0.000822159403F, - -0.00370394392F, -0.00939228293F, -0.00536249F, -0.00797492079F, - -0.00730202859F, -0.000918334408F, -0.008054832F, -0.00529015763F, - 0.00310319755F, 0.000202772499F, -0.00824058522F, -0.0117345359F, - -0.0129575562F, -0.00467099482F, -0.00280715502F, 0.00591486925F, - 0.000882489141F, -0.00159429444F, -0.00333232F, -0.0075911968F, - 0.0050156815F, -0.0121178851F, -0.0111320429F, 0.00274637877F, - -0.00322251255F, -0.00826024171F, 0.00800328422F, 0.00460305857F, - 0.00494425325F, 0.0086515788F, -0.00589281879F, -0.00219481625F, - -0.00171843811F, -0.0122838141F, 0.00861609168F, 0.00601917226F, - 0.0170927215F, 0.00499066198F, -0.00588930724F, 0.0186145734F, - 0.00881810952F, -0.000712414505F, -0.00479026511F, 0.00281878142F, - -0.0137001658F, 0.0103228427F, -0.000453698827F, 0.0141103473F, - 0.00556084374F, 0.0193496868F, -0.00448418967F, 0.0094957361F, 0.010190567F, - 0.0147738941F, 0.0055905953F, -0.00153229292F, 0.00274739135F, - -0.0210596565F, -0.0118173528F, -0.00350318756F, -0.00898691732F, - 0.00737401936F, -0.026626233F, 0.000547945325F, -0.00870959647F, - -0.015727656F, 0.00308614643F, 0.00950376503F, 0.00016361638F, - 0.00130369118F, 0.00969777536F, 0.0253738966F, 0.00666651083F, - -0.00487322733F, 6.25606554E-5F, -0.00103832968F, 0.00444586203F, - 0.0105497735F, -0.001349175F, 0.00908568781F, 0.00885658F, -0.000740927237F, - 0.0017538066F, 0.000945597771F, -0.00657340651F, 0.0147565538F, - -0.00022933555F, 0.00802218448F, -0.000236948F, 0.00541895395F, - -0.00747698732F, -0.0045650322F, 0.00823714212F, 0.00554317376F, - 0.0104158968F, -0.0195762925F, -0.00828878861F, 0.00194417255F, - -0.00144152204F, 0.00981697813F, 0.00652546063F, -0.00317800185F, - -0.0055362843F, -0.00122521364F, -0.00291897194F, 0.0023290941F, - -0.00773467589F, 0.00864449143F, 0.0034081256F, 0.0259980112F, - 0.00248649111F, 0.0128244562F, -0.00559047656F, -0.00610575F, 0.00959469564F, - 0.00808746275F, 0.00333644F, 0.00600421103F, -0.0111746378F, 0.00181158225F, - 0.00784590561F, 0.00379611854F, 0.00516039412F, 0.000666846114F, - 0.00314428192F, -0.000596495287F, -0.0144055896F, 0.0116266804F, - -0.000968895794F, -0.0187625736F, -0.0257304739F, 0.0164165366F, - -0.00234404928F, 0.00268194F, 0.0014148911F, 0.00891733821F, -0.0155446287F, - -0.00137970538F, 0.00314951688F, 0.011945175F, 0.00013132536F, 0.0113702202F, - -0.0132774822F, -0.00319203408F, 0.00787237752F, -0.00183598F, 9.62464E-5F, - 0.00260905758F, -0.0162177831F, -0.0148898717F, 0.00679725083F, - 0.0110082133F, -0.00638361322F, -0.00644071074F, -0.000124032347F, - 0.012775897F, 0.0213889368F, -0.00542554818F, 0.00185973267F, -0.0115957968F, - 0.00963895F, 0.0103328321F, 0.00896969717F, -0.0103857378F, 0.0130208163F, - -0.0141413882F, -0.00423780177F, 0.0126205049F, 0.00136808772F, - -0.00686085969F, 0.00538875069F, -0.0026297234F, -0.00090302719F, - -0.00431997F, -0.00232626591F, -0.00181451475F, -0.00185247732F, - 0.00312467874F, 0.00570730539F, 0.0114502683F, 0.00689941412F, -0.006915431F, - 0.0175415576F, -0.0125815617F, 0.00629145745F, -0.0018330079F, - 0.000989122433F, 0.0134602599F, 0.00509045552F, 0.0046981629F, - -0.00419660751F, 0.00827669F, -0.0123245688F, 0.00717724487F, 0.00232004351F, - 0.00931284763F, 0.014147792F, -0.0137448702F, -0.00308857951F, - -0.00257504196F, -0.00337533304F, 0.00558010116F, 0.000285054761F, - 0.00179933687F, 0.000886571535F, -0.0122738117F, 0.00406015664F, - 0.0124280797F, -0.000780631846F, -0.00125163759F, -0.00572019443F, - 0.00274909078F, 0.00544271292F, -0.00385470688F, 0.00181286235F, - 0.0026250591F, -0.00287648081F, 0.0178825371F, -0.00285267504F, - -0.0042963177F, 0.00215275888F, -0.0040201745F, -0.00528381113F, - -0.00786700379F, -0.00722184358F, -0.000951416732F, 0.00948065892F, - -0.00688075973F, -0.000461444171F, 0.00366191845F, 0.00216729939F, - -0.0176628232F, 0.0132697085F, 0.00200285413F, -0.0147345951F, - 0.00481708767F, 0.010481528F, 0.0153514612F, 0.0104675358F, -0.00899844244F, - -0.00737187499F, -0.00682808692F, 0.00446211966F, 0.00351484283F, - -0.0153890848F, -0.00237345695F, -0.0135817584F, 0.0178557299F, - 0.000707140774F, -0.0227922909F, -0.00958074F, -0.0110297669F, - -0.00309182657F, -0.00767499767F, 0.000709012325F, 0.00282599172F, - 0.00911921635F, -0.00546861952F, 0.00617890107F, 0.0023352264F, - 0.00971508492F, -0.0119216079F, 0.00279460382F, -0.00108111149F, - -0.00920086447F, -0.00818313845F, 0.00969367288F, 9.26801804E-5F, - 0.0176962242F, -0.0017574766F, -0.000762437587F, 0.000973406946F, - -0.000208040932F, -0.00871778838F, -0.000742364966F, -0.00854824297F, - -0.0165857244F, 0.0107210185F, -0.0105255656F, 0.0173802581F, - -0.00907636527F, -0.00322279753F, -0.00301992032F, -0.0100047681F, - -0.0170657635F, -0.00238086283F, 0.0102203526F, 0.00652520778F, - -0.00271767285F, 0.00345415017F, 0.0117991474F, 0.00586165814F, - 0.00815755315F, -0.00606692F, -0.0104043465F, 0.0189578738F, -0.0152878249F, - -0.00548368972F, 0.00110940193F, -0.0223742276F, 0.00133806618F, - -0.0023073107F, -0.0186535083F, 0.00574345095F, 0.0145490561F, - 0.00849210285F, 0.00864831451F, -0.0129218483F, 0.0112829078F, 0.0212950166F, - 0.00748723466F, -0.00214712461F, 0.00147762126F, 0.000485192373F, - -0.00311289332F, 0.0138626127F, 0.00700000254F, 0.00941094F, 0.0146451332F, - 0.0190655738F, -0.00996857F, 0.00229432783F, -0.000738514354F, - -0.00111131161F, -0.00748989079F, 0.00483865244F, 0.00760111306F, - 0.00271002622F, -0.0299176779F, -0.0136619648F, 0.0153270662F, - 0.00602577161F, 0.0104745813F, -0.000438906631F, 0.00415964704F, - 0.00620481558F, -0.00329658575F, -0.00208908715F, 0.00569902547F, - -0.00458420953F, 0.00903640315F, 0.0119771883F, 0.00633751927F, - -0.00174943905F, -0.0246025976F, -0.00788364466F, -0.0171732418F, - 0.0168040507F, -0.00503140874F, 0.00557893654F, 0.0146602606F, - -0.000182563788F, 0.0168395229F, -0.0025639853F, -0.00485709123F, - 0.00392287597F, 0.00709727081F, 0.0108715193F, 0.0090285372F, - -0.00164630602F, 0.019644266F, 0.00599854253F, -0.00457433751F, - 0.00546489423F, -0.0189266354F, 3.23828026E-5F, -0.0135166533F, - -0.0201179534F, 0.000601206033F, -0.00265398622F, 0.00832020864F, - -0.013594484F, 0.000787298661F, -0.00827258453F, -8.28595057E-6F, - -0.00730855484F, 0.00181640522F, 0.00838052575F, -0.0163665079F, - 0.00134335225F, 0.00456356397F, 0.000376463664F, -0.000122420883F, - -0.00396978855F, -0.0011204601F, -0.00242861966F, 0.00327854441F, - -0.0041560852F, 0.00481167808F, -0.0049661072F, -0.00335204578F, - 0.00758261932F, -0.0157082975F, 0.0097468812F, 0.00574364886F, - 0.00601291889F, -0.00759192929F, -0.0101351822F, 0.00782917533F, - 0.00623067655F, -0.000500028837F, -0.0019671889F, -0.0173108559F, - 0.00409719953F, 0.00369160157F, 0.00211576186F, -0.00532946596F, - -0.0140041057F, -0.017138632F, -0.00865870807F, 0.00417398941F, - 0.00570364716F, -0.013927999F, 0.00591305597F, -0.0145599386F, - -0.000735846581F, -0.0121747972F, -0.00118113006F, 0.0149114914F, - -0.00423558056F, -0.014817006F, -0.0117388079F, 0.00172476145F, - -0.00530913705F, -0.0097111566F, -0.00539153907F, 0.00363015127F, - 0.0035180659F, -0.00639025541F, 0.00750986626F, 0.00261150487F, - -0.00222345954F, -0.00299174408F, -0.00282168319F, -0.00223333598F, - -0.00945870765F, 0.00544624357F, -0.0117717162F, -0.00703277532F, - 0.00420314865F, 0.0084381951F, 0.00612177327F, -0.00486539491F, - -0.00319672097F, -0.00836050231F, 0.00125785265F, -0.00867092423F, - -0.00505829044F, 0.00069481181F, -0.0101177581F, 0.0139738126F, - 0.0106932009F, -0.00218655984F, 0.00819638278F, -0.00143624085F, - -0.0121550709F, 0.000315635F, 0.00954372808F, -0.0208884813F, - -0.00630632322F, 0.00537314871F, 5.99015402E-5F, 0.00775492331F, - 0.013036333F, 0.0250846483F, 0.00483228592F, -0.00304800156F, 0.00694103912F, - -0.00228720112F, 0.00641213078F, 0.00403258624F, 0.00443688035F, - 0.00258947792F, -0.000886560418F, -0.00570455892F, -0.00507641863F, - 0.00425847201F, -0.00527339F, -0.01265427F, 0.00445142156F, -0.00721654948F, - -0.0121190445F, 0.000831353769F, 0.000264201488F, 0.0142695F, - -0.00075790839F, -0.0191649422F, -0.0133093214F, -0.00378742907F, - -0.00467621954F, 0.0152415689F, 6.81882084E-5F, -0.00677948957F, - 0.0115944762F, 0.00236934167F, 0.0054543484F, -0.00208474486F, - -0.0191537663F, -0.00285007316F, 0.0238347966F, -0.00175446493F, - 0.0148524195F, 0.00973861944F, 0.00448926445F, 0.00138653605F, - -0.00237870426F, -0.0077943F, 0.0186602362F, 0.0247606151F, -0.0123387175F, - 0.0125351343F, -0.000857450301F, -0.00514293043F, 0.00110944791F, - -0.0194523633F, -0.0040172F, 0.0062945839F, -0.00130941544F, 0.00757151609F, - 0.00464231148F, 0.00354444608F, -0.0127960918F, -0.00179744395F, - 0.000969079498F, 0.00978695787F, 0.0115421275F, 0.0201906282F, 0.0111398948F, - -0.00232557859F, -0.00979581755F, -0.00280577783F, 0.00674174959F, - 0.0101188095F, -0.0032785756F, -0.00296972273F, -0.00969393551F, - -0.000126699422F, -0.00536942855F, 0.00386737473F, 0.00476623F, - -0.00583082763F, 0.00880891F, 0.00290005165F, 0.0107977232F, -0.0124934325F, - 0.0099905245F, -0.00044191163F, -0.00155461952F, 0.00573635427F, - -0.00620093243F, -0.002571146F, 0.0128150946F, 0.00949539F, -0.000881190819F, - -0.00370692671F, 0.00599327497F, 0.0161335487F, -0.00477284053F, - -0.00273021194F, 0.00206051511F, 0.00539852539F, -0.00768757798F, - 0.0249520894F, 0.000872495119F, 0.0234574936F, 0.00498836068F, 0.0111183235F, - -0.00915137F, -0.00206627883F, -0.0068674325F, 0.00124304392F, - -0.00672989665F, 0.00772048905F, 0.00255514332F, 0.0107212523F, - 0.000349514332F, 0.000328514579F, -0.0178443156F, -0.00387376826F, - 0.00501474459F, -0.00569816492F, 0.00637538638F, 0.0117626833F, - 0.0126762697F, 0.0125691565F, 0.00345723494F, -0.00110013422F, 0.00812477F, - -0.00887493696F, 0.00588537753F, -0.0101031382F, 0.020891292F, - -0.000834536273F, 0.00535292085F, 0.0170854367F, -0.00746316276F, - 0.00107603858F, 0.00895686354F, -0.000192240943F, 0.00505824713F, - 0.0158944409F, 0.00162364112F, 0.00299876393F, 0.0240177773F, 0.0085729314F, - -0.00499986345F, -0.00477982778F, 0.00574082183F, -0.00509221805F, - 0.00781399664F, 0.0103959506F, -0.00692945626F, -0.0136884348F, - -0.000245933537F, -0.00177031872F, -0.00378778111F, 0.0118772369F, - -0.00900626369F, -0.00343299145F, 0.00627314113F, -0.00301242969F, - -0.00190136116F, -0.0094953943F, 0.00230284035F, 0.0238737408F, 0.00534541F, - 0.014516931F, -0.0236402135F, -0.00716292579F, 0.00528319133F, - -0.00505911047F, -0.0101169227F, 0.0182101242F, -0.00349434977F, - 0.0105433492F, 0.00859689713F, -0.01412623F, -0.0091819074F, 0.0182761122F, - -0.00611154875F, -0.00795581F, 0.0145472772F, 0.0029152655F, 0.00233156164F, - -0.0032261603F, 0.000188164398F, 0.00426236447F, 0.006025373F, 0.0201481115F, - 0.00194307417F, -0.0188711956F, 0.00711264648F, 0.0030318948F, - 0.000496763503F, 0.00183636881F, 0.00870632753F, -0.00250764773F, - -0.000133727532F, -0.000558007858F, -0.00291524502F, 0.0131399212F, - -0.00405573845F, -0.010367671F, -0.0107057737F, -0.0220322683F, - 0.0165386815F, 0.00416591624F, -0.00776798883F, 0.0086755706F, - -0.0027520468F, -0.0172403976F, 0.0022257315F, -0.0044959425F, - -0.00748266652F, -0.0110942703F, -0.00469662761F, 0.00135060179F, - 0.00217676698F, 0.00164505607F, 0.0107902456F, 0.00349317165F, - 0.00204790314F, -0.0160315596F, 0.00588911073F, -0.00335402624F, - 0.00130582624F, -0.000393594499F, 0.00822292082F, 0.0077251941F, - 0.0112748807F, 0.00156193902F, -0.0116281677F, -0.00025202948F, - -0.011095101F, -0.00272881123F, 0.00130322797F, 0.00898035709F, - -0.00640053116F, 0.0125693623F, -0.0220525973F, 0.00767218834F, - -0.00115672022F, 0.00798279047F, 0.00429628F, -0.0234524254F, - -0.000909173046F, -0.00653350214F, -0.00508772256F, 0.0105581405F, - -0.00255238754F, 0.00991186F, 0.00951099675F, 0.00673785526F, 0.0153583093F, - 0.00454256637F, 0.0039812983F, 0.00814352F, -0.00280772359F, 0.000722116325F, - 0.0163752362F, 0.00917112548F, -0.00382209173F, 0.014703529F, - -0.00343405688F, -0.00287365587F, 0.0153495874F, 0.00643632561F, - -0.00501339976F, 0.0123657761F, -0.00301105529F, -0.0106740994F, - 0.0139671499F, 0.00405357033F, -0.00235359045F, 0.00658307597F, - -0.0022249571F, -0.00314950035F, 0.00458938163F, -0.00688083749F, - -0.00168430211F, 0.00668145576F, -0.0150026409F, 0.000412549532F, - -0.0140601406F, 0.00545180775F, -0.0105311405F, 0.00254515605F, - 0.00875270087F, 2.46860182E-5F, -0.0227035377F, 0.013957045F, 5.93858822E-5F, - 0.029954873F, 0.00191743555F, -0.00483951857F, -0.0071859085F, - 0.00481734378F, -0.00705409143F, 0.00519038318F, -0.00159551715F, - 0.00111655938F, 0.00749974558F, -0.00538153155F, 0.00699997135F, - 0.00995014515F, -0.0111942375F, 0.0164615028F, 0.00725022703F, - 0.00439307373F, -0.00451935502F, -0.0117432186F, -0.00233487319F, - -0.0182996485F, 0.00982076302F, -0.00646693213F, 0.00262685213F, - 0.00601414917F, 0.000328154798F, -0.0100888135F, 0.0159496721F, - -0.000402746344F, 0.0120296441F, 0.0119121149F, -0.0202540793F, - 0.000355194701F, 0.00614924962F, -0.0156586897F, -0.00851334352F, - -0.0109312F, -0.0118743824F, -0.0140261119F, 0.0014137415F, -0.0082571283F, - -0.00605367403F, -0.00847076718F, -0.00291774957F, 0.0180733167F, - -0.00291763572F, 0.00278657209F, 0.000318826758F, -0.0017595205F, - 0.0111322897F, -0.00282126223F, -0.0145768197F, 0.0135086151F, 0.0113175418F, - -0.0114253657F, 0.00394143257F, -0.0083574215F, -0.0113135679F, - -0.00576557359F, -0.0148326205F, -0.00297141261F, -0.00445651729F, - 0.0110699274F, 0.00748215057F, 0.00776995579F, 0.00302330102F, - -0.00442967238F, -0.0035406719F, -0.000950714108F, -0.0247041639F, - -0.00829766225F, 0.00951567385F, 0.00921533F, -0.0136204306F, 0.00483941427F, - 0.0095929848F, 0.0143032279F, -0.00438100705F, -0.00519710919F, - 0.00181396585F, 0.000191207961F, -0.0161914416F, 0.0197598971F, - 0.00557627575F, 0.0100999521F, 0.011709841F, -0.015914578F, -0.00172884273F, - 0.000217006804F, -0.0169040356F, -0.00264598755F, 0.000535265717F, - 0.00163197098F, -0.00269586197F, 0.00435434934F, -0.00784448162F, - -0.00311655924F, 0.00177578186F, -0.00951546431F, 0.00455281F, - 0.00704585761F, 0.00325510208F, -0.000354022806F, -0.0143073173F, - -0.0130735524F, -0.00394754857F, -0.000546819123F, -0.00736599602F, - 0.0135710817F, 0.0072742058F, -0.0073199519F, -0.000467966165F, - -0.0144489156F, 0.00714831F, -0.00382742682F, -0.0178323612F, 0.00349526363F, - -0.020302264F, -0.00933982F, -0.00257902639F, -0.00490266318F, -0.004052551F, - -0.0089873774F, -0.00232409663F, 0.00861538947F, 0.00840394944F, - -0.00730315596F, 0.00201075105F, -0.0165422335F, -0.00354189868F, - -0.0126057416F, -0.000734359433F, -0.00945108756F, -0.00724986102F, - -0.000696378702F, -0.00226588827F, 0.000495612796F, -0.0159864612F, - -0.0198871884F, 0.0122415442F, 0.00812679809F, -0.000584786292F, - 0.00568823097F, 0.0174903888F, 0.0182498936F, 0.00192903879F, - -0.00564026507F, -0.00991161F, -0.0124464016F, 0.0088792881F, - -0.00903559569F, -0.00801728759F, -0.000316850288F, 0.00232548639F, - -0.000870162F, 0.0113879805F, -0.00441271299F, -0.0273276642F, 0.00111045F, - 0.0051514958F, 0.0230480302F, -0.00813322F, -0.000342938845F, -0.0195779577F, - -0.000455842412F, 0.00870888121F, -0.0158810429F, -0.00281437929F, - 0.00645227684F, 0.00666392222F, 0.00103014056F, -0.00782251731F, - -0.00353473774F, -0.00262984354F, 0.00258771679F, 0.0060898033F, - -0.0199773218F, 0.00662090816F, 0.00393207371F, -0.00126715808F, 0.00706881F, - -0.00735564576F, 0.00678392546F, -0.00431123236F, 0.0135400742F, - 0.0142180705F, 0.018430803F, -0.00282684411F, 0.0148662124F, -0.0156808179F, - 0.0203581303F, -0.00140456122F, 0.000850017474F, -0.00214570318F, - 0.00895895064F, -0.0174654163F, -0.0061900476F, 0.00689185224F, - 0.0139272595F, 0.00673484057F, -0.00834483095F, 0.00974164903F, -0.00960731F, - 0.00412046164F, 0.0144512644F, 0.0126732206F, -0.0144489883F, 0.00730181718F, - 0.00392624F, -0.00285777589F, -0.0032826059F, -0.00934373401F, 0.0104132481F, - 0.00540850637F, -0.0100857047F, -0.00446248706F, 0.00945632439F, - -0.00723555218F, 0.00152997929F, 0.00393918622F, -0.00531605305F, - 0.015926091F, 0.0108868377F, -0.0134282066F, -0.00499499589F, 0.00206588092F, - 0.00665280782F, 0.00366261974F, 0.00956460647F, -0.00599323353F, - -0.00575803639F, 0.0134074334F, 0.00144955155F, -0.00826189108F, - 0.0189377107F, -0.0094494354F, 0.000373902731F, 0.0144663583F, - -0.00993637275F, 0.00741275772F, 0.00166644831F, 0.00322840852F, - -0.00757204602F, -0.0057293009F, -0.00558601134F, 0.00360845029F, - -0.0206099562F, -0.00285107782F, -0.000166839716F, 0.00876308233F, - 0.00487532467F, -0.0057932511F, 0.00100213476F, 0.00670656795F, - -0.0267695319F, -0.00137028226F, -0.0129921241F, -0.00177668931F, - 0.0140186129F, -0.00148047681F, -0.0176090207F, 0.00697658863F, - -0.00149790558F, 0.00314117433F, 0.00699388655F, 0.00321330642F, - 0.00675828522F, -0.0030056152F, -0.000306082715F, -0.00285750558F, - 0.00616644043F, -0.0219022203F, -0.0137953572F, 0.00585533911F, - -0.00568843167F, 0.00509838248F, 0.00505675655F, -0.00247083884F, - 0.00953020062F, 0.00617931597F, 0.0117881456F, -0.0159603935F, 0.002722929F, - -0.000783651252F, 0.0103443814F, -0.00714886142F, -0.0148588642F, - 0.00223937258F, 0.00659882091F, 0.000872123695F, 0.0138677619F, - 0.00947162416F, 0.00399446674F, 0.00068379F, 0.00578467501F, -0.0170379337F, - -0.0158436466F, 0.00264999783F, 0.0183853377F, 0.0185379554F, 0.00176585501F, - -0.00851549581F, 0.00550378952F, 0.00192786637F, -0.00211821962F, - -0.00485797273F, 0.0149412919F, -0.00225935201F, -0.00208621868F, - 0.00462122867F, 0.000974338793F, -0.00852425396F, -0.00677496241F, - 0.00722497562F, -0.0168005619F, 0.0126635665F, 0.00251916726F, - 0.00707513839F, 0.0120356223F, -0.00581919309F, 0.00212407811F, - -0.00870569423F, 0.0120495232F, 0.00582285225F, -0.00308083813F, - -0.00404251087F, 0.00426369114F, -0.00335414684F, 0.00974965654F, - -0.00188186555F, -0.0109141776F, -0.00801011F, 0.0100252172F, -0.0105669117F, - 0.00687093846F, 0.01147654F, 0.00938393F, -0.00142893218F, -0.01094246F, - 0.00712100836F, -0.000324999157F, -0.00159894687F, -0.00190223788F, - 0.00462071551F, 0.0044099451F, -0.00435285596F, 0.00367413322F, - 0.00601538783F, 0.00119185157F, 0.0180555861F, 0.00415353477F, - -0.0151990345F, -0.017152464F, 0.0031401536F, 0.0141218677F, -0.0111827459F, - -0.0013633715F, -0.00584876351F, -0.0124590266F, -0.00808312744F, - 0.00363989151F, -0.00702300156F, 0.014923431F, 0.00810455345F, 0.0059395195F, - 0.000293051446F, 0.00988017395F, -0.0227876455F, 0.00368329138F, - 0.00347566162F, 0.00157831574F, -0.0217425339F, 0.0116524911F, - 0.000471749343F, 0.00226347125F, -0.00663459534F, -0.010315503F, - 0.00767380558F, -0.00657272199F, -0.00147548364F, 0.00612785807F, - 0.00714136427F, 0.0105225649F, 0.0105256224F, -0.00105343701F, - 0.00568516599F, -0.0140433721F, -0.0148437861F, 0.00795101281F, - 0.0120451553F, -0.00165011454F, 0.01728433F, 0.0129510025F, 0.00142312865F, - 0.0065450971F, 0.0166820716F, -0.0209777132F, 0.00604825839F, - -0.00941487774F, -0.0178228803F, -0.0104186535F, -0.00478552142F, - 0.00220937328F, -0.013646828F, 0.00298465742F, 0.00206070696F, - -0.00486467127F, -0.0124782352F, 0.00351265422F, 0.00237119896F, - -0.017237816F, -0.000909590453F, 0.0216099229F, -0.00956050679F, - -0.00532526569F, 0.00527212908F, 0.00162470446F, -0.00196062657F, - 0.00756397704F, -0.00304546184F, 0.0147196352F, 0.018865006F, -0.0108926389F, - -0.00487420755F, -0.00810251292F, 0.00297543895F, -0.00913379807F, - -0.00931405F, 0.00960174948F, 0.00443358626F, -0.00060507236F, 0.0211983565F, - -0.00676770695F, 0.000250471901F, 0.000826104F, -0.00145125808F, - -0.00397082232F, -0.00999505166F, 0.0102553703F, 0.00533455517F, - -0.0034673207F, 0.0121861333F, -0.00752987666F, -0.00186795252F, - -0.00808879267F, 0.000553612306F, 0.016647419F, 0.00268238946F, - 0.00739545329F, 0.0111637181F, 0.0209503639F, 0.00327807153F, 0.0131524019F, - 0.0111634675F, -0.0131458454F, -0.0100913951F, 0.00121617026F, - 0.00789548922F, 0.0115976837F, -0.0110566663F, -0.0183990244F, - 0.000718320371F, -0.0010123403F, 0.00554150762F, 0.00719766039F, - -0.0123438789F, -0.0106595466F, -0.00299196132F, 0.00577011518F, - -0.00912424177F, -0.00162081816F, -0.00489023747F, -0.00550848665F, - -0.0138484F, -0.0157907158F, -0.0133880964F, 0.00914924778F, -0.0078079463F, - 0.0116146617F, 0.00564816687F, 0.00508184638F, 0.00233032345F, - -0.00750744343F, 0.016692508F, 0.00341909705F, -0.0149681419F, - -0.0076842308F, -0.00571702467F, -0.0150201917F, -0.000723557489F, - -0.000673801289F, 0.0134581318F, -0.0113603566F, 0.00761187309F, - 0.00797148887F, 0.00604493916F, 0.0264750272F, 0.0146698114F, -0.0028404363F, - 0.00677356916F, -0.00263454881F, -0.0035949531F, -0.00132701942F, - 0.00485030422F, -0.00505385362F, -0.0118081272F, 0.0176401827F, - -0.000561149558F, -0.00498456F, 0.0113536492F, -0.0109369224F, 0.0137840621F, - 0.00109918171F, 0.00953179225F, -0.0150136715F, 0.00111467985F, - -0.00843772478F, -0.0142815709F, 0.00108262722F, 0.00305131986F, - -0.0063716271F, 0.00410790043F, -0.0018159796F, -0.00183186086F, - -0.00483553251F, -0.00949063897F, 0.0138979098F, -0.00366245233F, - 0.00984177832F, -0.0049434891F, -0.00870988145F, -0.00348684122F, - -0.016090136F, 0.015924491F, 0.00343150063F, 0.0190388374F, 0.00804606732F, - 0.00812002923F, -0.014737173F, 0.0191885643F, 0.0156264529F, -0.0112019908F, - 0.00771200238F, -0.0110262567F, 0.0187117513F, 0.0165523868F, 0.00816953555F, - 0.00826552231F, -0.00560867F, -0.00755617907F, 0.00828181114F, - -0.0244501848F, -0.00579265226F, 0.00144276978F, -0.00549461925F, - -0.00681924168F, -0.00234204787F, -0.00803243648F, -0.0193279497F, - -0.00150835374F, -0.00625204435F, 0.0118714701F, 0.0175164882F, - 0.0143542038F, -0.00642226357F, 0.0142021729F, 0.00589619856F, - -0.00389448018F, 0.00627028F, 0.00359024433F, 0.00413055113F, 0.0109091364F, - 0.00764613878F, 0.00530910259F, -0.000462375378F, -0.0146340532F, - -0.00433880929F, -0.0115911886F, -0.0285972022F, -0.00384054706F, - -0.00222527981F, 0.000139488286F, 0.000293354708F, -0.00817166734F, - -0.010680099F, 0.00961812F, 0.00152216607F, 0.00929211825F, 0.00271850522F, - 0.00170520891F, 0.00465358933F, 0.00443667313F, 0.0214415211F, 0.0129868053F, - 0.0124088014F, -0.002987138F, 0.00548075F, 0.000634801341F, 0.00755171338F, - 0.00250762841F, -0.00156638864F, 0.00459545292F, 0.0048921369F, 0.0142763F, - 0.0257075299F, 0.005870366F, -0.00779658323F, 0.0054877596F, -0.0066460087F, - -0.0101513229F, 0.0097750565F, 0.00328108273F, -0.0124798128F, - -0.0110133495F, -5.69525473E-5F, -0.000355111639F, -0.01032509F, - -0.000214448621F, 0.00260128593F, -0.012261929F, 0.00712716253F, - -0.000435321766F, 0.00373241631F, -0.0167846028F, -0.0189060215F, - 0.000820441754F, 0.00915001519F, -0.00377010112F, -0.0117482124F, - 0.0110918805F, -0.000752078253F, 0.02004187F, -0.0030011253F, 0.00408819597F, - -0.00292908098F, 0.00162347173F, 0.00712935114F, -0.0147901559F, - 0.00998313259F, 0.0210744012F, 0.0117656933F, 0.00431359792F, - -0.00683216611F, -0.00991702173F, -0.0034459196F, -0.00413202448F, - 0.00118768727F, -0.0119643789F, -0.0013649608F, -0.00600395212F, - 0.00942269806F, 0.00118989823F, 0.00426967675F, -0.00246602809F, - 0.00809787121F, 0.00818305835F, -0.0171254184F, -0.00838149339F, - -0.000683990249F, -0.000396377698F, -0.00688883383F, -0.00265003345F, - -0.00218155421F, -0.0183885917F, 0.000509998063F, 0.0029591606F, - -0.0100609399F, 0.00287850457F, -0.00136283715F, 0.00171203283F, - 0.00453133462F, -0.0027870019F, 0.000175089212F, -0.013276861F, - -0.00934714824F, -0.0017962117F, -0.0034996022F, 0.00227952912F, 0.00285994F, - 0.0084199179F, -0.00543256383F, -0.00315497094F, -0.0119594261F, - 0.0218817443F, 0.0015553257F, 0.00140731863F, -0.00191514369F, 0.0109194415F, - -0.00672568614F, -0.00245831045F, -0.00268985F, -0.0108398963F, - 0.00808439404F, 0.0178857967F, -0.00450330414F, -0.0156812128F, - 0.0155271851F, 0.0105670588F, -0.00183023186F, -0.00331107946F, - 0.000709634798F, 0.00631419756F, -0.0062539936F, -0.00602772785F, - -0.00604228303F, -0.00277583F, -0.00590237044F, -0.0130488267F, 0.00577062F, - -0.00162543019F, 0.00243516779F, 0.00483243819F, 0.00588846067F, - 0.00185485487F, -0.0024750887F, -0.00578491809F, -0.0079903407F, - 0.000376764016F, 0.00301015913F, -0.00300785317F, 0.00912715122F, - 0.00423747161F, -0.0206608195F, 0.00851838943F, -0.00400784519F, - -0.0133619858F, 0.00565249F, -0.0128691597F, 0.0164568499F, 0.00466164108F, - -0.00363693F, -0.0132235596F, 0.00835979823F, -0.00785336F, 0.000107259009F, - -0.00548669975F, -0.00771688437F, 0.00389713561F, -0.00638589356F, - 0.0168863311F, 0.00523840357F, -0.000360626465F, -0.00745816063F, - 0.00213986565F, -0.00617473619F, -0.012453516F, 8.96071E-6F, -0.00687359879F, - -0.000627792208F, -0.0212994236F, -0.0026919432F, 0.00871864334F, - -0.00320751662F, 0.0152571397F, -0.00300596F, 0.00682365568F, 0.00943123F, - -0.000248964672F, -0.00544509059F, -0.00998161826F, -0.00208556233F, - 0.011834465F, 0.000562260509F, 0.000441430137F, 0.00574615179F, - -0.00205582334F, -0.00601086F, -0.0144016333F, 0.00784727372F, - -0.00570771098F, 0.00273574726F, -0.00419597747F, 0.0119537795F, - 0.000693318958F, 0.0162419174F, -0.0178361107F, 0.0183847845F, - -0.00583932735F, -0.00817897916F, 0.00349651417F, 0.00626499625F, - 0.0163337234F, 0.00976979546F, 0.00739877252F, 0.0145698385F, 0.00554047478F, - -0.00158373639F, -0.00166426692F, 0.00107735745F, 0.00422165683F, - 0.000122608617F, 0.00183486089F, 0.0130239502F, 0.00726984581F, 0.00893584F, - 0.00290881F, -0.00496719172F, -0.0179128293F, -0.00747946789F, - -0.00671630586F, 0.00272884173F, -0.00294138677F, -0.00447096489F, - 0.00418669404F, 0.0043142233F, -0.00967123825F, -0.00158976333F, - 0.0094296867F, -0.0207032152F, -0.00886673573F, -0.00201267865F, - 0.000668779947F, 0.00128918362F, 0.0100781778F, 0.00868725684F, - 0.0135771511F, -0.0116882296F, -0.00658930046F, 0.00547775626F, -0.00908715F, - 0.000207556775F, 0.0134586142F, -0.0288243F, -0.00847232435F, - -0.00424667727F, 0.00576135796F, 0.00570561225F, -9.6613323E-5F, - 0.0186780952F, 0.00121156301F, -0.00348961912F, -0.00543815922F, - 0.00252176099F, 0.000335034478F, -0.0113305915F, 0.00255949F, - 0.000567923707F, 0.00174902263F, 0.00479356106F, 0.00224578357F, - -0.00352186803F, -0.00195634528F, -0.000200749928F, 0.00370808668F, - 0.0122389635F, -0.0242479015F, 0.00665577687F, 0.00398038095F, - -0.00989647489F, -0.00322844391F, 0.0150623908F, 0.00654860772F, - -0.00286549586F, -0.0169387683F, 0.00299782073F, 0.00944076292F, - 0.0110261226F, 0.00732939038F, 0.00202663033F, 0.0120462701F, - -0.00903186295F, 0.00243307487F, 0.0184263177F, 0.00458332431F, - 0.0114043504F, -0.0109566553F, 0.00284072338F, -0.0132146245F, - 0.00320818438F, -0.00742746936F, 0.00704090064F, -0.00209513609F, - 0.00852668658F, 0.006065208F, 0.00375310914F, -0.0105908383F, 0.0119368006F, - 0.00178268214F, 0.0121315541F, -0.00984221417F, 0.00386106665F, - 0.00107427314F, -0.00607563881F, -0.00186648185F, 0.00430871127F, - 0.00515588792F, 0.00798905827F, 0.0150814019F, -0.00823997427F, - 0.0119399475F, 0.00266045518F, -0.00502014F, -0.00402357103F, 0.00703776488F, - -0.00803945865F, -0.00493808696F, -0.00716333278F, 0.000672107271F, - -0.0221882313F, -0.00215214631F, -0.003543742F, 0.00886650383F, - -0.0109668961F, -0.000432240049F, -0.00434203492F, 0.0105957156F, - -0.0116431657F, 0.0130716302F, -0.00682832766F, -0.00502596935F, - -0.00402607676F, 0.0103822192F, -0.0103589781F, 0.0120129483F, - 0.00309394812F, -0.0120712901F, 0.00283408118F, 0.0118146259F, - 0.00787453167F, -0.0114116184F, 0.010404801F, -0.00561621413F, - 0.000295880716F, -0.0164175462F, -0.022146F, 0.00133132958F, -0.00217440887F, - -0.00383613235F, 0.0131943431F, -0.00470123114F, 0.000305313704F, - 0.0149102081F, -0.000582587905F, -0.00424072193F, -0.00552923838F, - -0.0087319063F, -0.0114492048F, -0.00254639331F, 0.00617300114F, - -0.000520293775F, 0.00883355364F, -0.00695249438F, 0.0104364334F, - 0.0145627204F, -0.0027762535F, -0.00480544241F, 0.00585306343F, - -0.0118698785F, -0.00246774452F, -0.0035018085F, -0.0178535115F, - 0.0124506662F, 0.00861052051F, -0.00431510666F, -0.0074847159F, - 0.00390412239F, -0.00260622706F, 0.0103723835F, 0.00423017889F, 0.007576562F, - -0.00476636F, 0.0172025599F, 0.00997792F, 0.0213395376F, -0.00469154865F, - 0.000955116528F, -0.00268672383F, 0.00499958731F, 0.00428673485F, - -0.00579312677F, -0.00935300719F, -0.00732066529F, -0.00906316377F, - -0.00198203046F, 0.0163386744F, -0.00249503972F, -0.00370350736F, - 0.0126718879F, 0.0132186655F, 0.0155587234F, 0.00613913313F, 0.0108829895F, - 0.00202094461F, 0.00589029258F, 0.0136985052F, 0.00342336833F, 0.0213351119F, - -0.005088971F, 0.0117815891F, -0.00908639748F, 0.000479008071F, - 0.00433640275F, -0.0134000229F, -0.00280785374F, -0.00394292269F, - -0.0056253518F, 0.00505866F, -0.0061477269F, -0.00859252457F, -0.0163018107F, - 0.01263F, 0.00563532906F, -0.000934026262F, -0.00882351492F, 0.0192328915F, - 0.00151274388F, 0.00491767377F, -0.0127212293F, -0.00051698403F, - -0.0197290778F, -0.0181144364F, 0.000490531675F, 0.0112551823F, - 0.00794190913F, -0.00631425576F, 0.000147157945F, 0.00733732432F, - 0.00221606856F, 0.00487291766F, -0.0041464013F, 0.00269076973F, - -0.000304181449F, -0.0107013779F, -0.0130927414F, 0.00649516378F, - -0.0043375534F, -0.00274075032F, -0.00392734446F, -0.000103154904F, - -0.0132839959F, -0.000650725153F, 0.00764898723F, -0.00412752945F, - 0.014269324F, -0.0102640232F, 0.00816092826F, -0.00158992945F, - 0.000693209586F, -0.00175618788F, -0.0174643472F, -0.00603611208F, - -0.00384115824F, 0.00795326382F, -0.0136169354F, -0.00516670104F, - -0.00960431714F, -0.0165629573F, 0.0237619132F, -0.00448657153F, - -0.0102194166F, -0.00180946675F, -0.00245330227F, 0.00663480721F, - 0.00757895829F, -0.0015651068F, 0.0011853684F, 0.00909950305F, - -0.00953702535F, -0.00170410296F, 0.00308588985F, 0.00281156064F, - 0.00296732737F, 0.0223672092F, -0.00977164414F, -0.00054918346F, - -0.00880026724F, -0.000299996493F, -0.00401333161F, -0.00426731072F, - 0.0101444814F, 0.0055863387F, -0.00879596826F, 0.00692407414F, 0.0271423813F, - 0.00251132203F, -0.0070546926F, 0.00710619753F, -0.00449452968F, - -0.00827250071F, -0.00833701249F, -0.00856872834F, 0.0139977373F, - -0.00205904408F, -0.00445542764F, -0.00613538176F, -0.00812453497F, - -0.0138980942F, -0.0172165129F, -0.0130226044F, 0.00731007149F, - -0.000224844029F, -0.00710245734F, -0.00402360456F, 0.00293011148F, - 0.0104243839F, 0.00104166556F, 0.00344792521F, -0.00338518084F, - -0.0154310865F, 0.00411865255F, 0.00594221754F, 0.00199301424F, - 0.0108551867F, -0.0015694421F, -0.0157499239F, -0.0247512795F, - -0.00188993802F, 0.0054951678F, 0.00517140049F, -0.00102575601F, - -0.00124066463F, 0.00612896075F, -0.00143609452F, 0.00567843718F, - 0.0137197375F, -0.0119350543F, -0.0128331305F, -0.0162263587F, - -0.00538309338F, -0.0130340885F, 0.00802691188F, 0.0154841263F, - 0.0013339743F, 0.0086148791F, -0.00161628763F, -0.00574022F, -0.0021875354F, - -0.00170372066F, 0.00316964299F, -0.00935527403F, 0.0110969283F, - 0.0252710711F, -0.00103594596F, 0.00689209485F, 0.0163406841F, - 0.00315749249F, -0.0133162569F, -0.00827657152F, -0.011146687F, - -0.000666904205F, 0.00143360428F, -0.00755304284F, 0.0114801126F, - -0.000233452985F, -0.00158158795F, -0.00756784435F, 0.00558102643F, - -0.00359566673F, -0.00480747409F, -0.0234577134F, -0.0141897779F, - 0.0034688469F, 0.00297717517F, 0.0116468659F, -0.00304427464F, - -0.00315670925F, 0.0152615858F, 0.00716251414F, 0.00910099223F, - 0.0094187269F, -0.0109736966F, 0.00696405256F, 0.00326500926F, - 0.00654099602F, -0.0150653878F, -0.00700424518F, 0.0191873815F, - -0.0119749717F, -0.00483569223F, -0.0154347923F, -0.0123280203F, - 0.00357050681F, 0.014672392F, -0.00819225144F, -0.0146170268F, - -0.0173787698F, -0.0112183308F, -0.00824927259F, -0.00393957365F, - 0.00418272661F, 0.000729453284F, -0.00884515885F, 0.0226715654F, - 0.0168899689F, 0.00174055307F, -0.000344239175F, -0.00949957781F, - 0.00612775236F, 0.00305751292F, -0.000263379072F, 0.00338509469F, - 0.0170609634F, 0.00359327416F, -0.00102103234F, 0.00495885918F, - 0.0077937562F, 0.00498822518F, -0.00010832413F, 0.00889483653F, - -0.0054543796F, 0.0117893275F, -0.000377203629F, -0.00348768593F, - -0.00584341865F, 0.00367176463F, -0.00985309947F, 0.000593452F, - -0.00320745585F, -0.00130961731F, 0.0130207147F, 0.0227746498F, - -0.00702242041F, -0.00103169947F, 0.000628459733F, 0.000775328663F, - -0.00588128669F, -0.0118728168F, -0.0145294312F, 0.00182566873F, - 0.00973493047F, 0.0228263941F, 0.0142299151F, -0.00795252435F, 0.0157520585F, - 0.00199345336F, -0.00874077342F, -0.000798406138F, 0.0212091804F, - 0.00112871837F, 0.0098296F, -0.0129087782F, 0.0196918231F, -0.0010251425F, - 0.00455655158F, 0.000256459578F, 0.00206584739F, 0.0143634593F, - 0.00189920724F, -0.00470028957F, 0.0198134519F, 0.00574611919F, - -0.0054272064F, -0.00143305457F, 0.00245883269F, -0.0137803964F, - 0.00974758063F, 0.0156880151F, 0.00607971242F, 0.00949179F, 0.00309105031F, - -0.00320677692F, -0.00932937209F, -0.0162786245F, 0.011421321F, - -0.0121712405F, 0.00475241197F, -0.0273975F, -0.0112359114F, 0.0182074867F, - -0.00886619929F, -0.0109649273F, -0.0106337322F, -0.00399938971F, - -0.0015659699F, 0.0134922722F, -0.00141021854F, 0.0146116856F, - -0.0015172842F, -0.00611104956F, -0.0142802354F, -0.0032053215F, - -0.012386309F, 0.0163137913F, 0.00659521669F, -0.00238836464F, - -0.0263429154F, 0.00358035229F, -0.00384794152F, -0.0100439824F, - -0.008684217F, 0.000934609F, -0.00635444513F, -0.0106935548F, 0.0112242308F, - 0.0105731376F, 0.00497035729F, 0.00129188178F, -0.00839284714F, - -0.00963468384F, -0.0042086F, -0.0185599681F, -0.00669568777F, 0.0161939636F, - -0.00864211377F, 0.00735868094F, -0.00764558837F, 0.00510463165F, - -0.0056976066F, -0.00349583849F, 0.00301415962F, -0.0113307657F, - -0.00105308858F, 0.0063897809F, -0.00239511929F, -0.00105284166F, - -0.0198710561F, -0.00118659018F, 0.0110763125F, 0.00768786622F, - 0.00206487114F, 0.00831255224F, -0.00826683082F, 0.0129958568F, - 0.000319393963F, -0.00515935384F, 0.00728704454F, -0.0013257534F, - 0.0101292534F, -0.00589013333F, -0.0160741359F, -0.017753914F, 0.0044478355F, - -0.0141055128F, -0.0104152262F, 0.00217866548F, -0.00404795026F, -0.0101099F, - -0.00323909987F, 0.0091754254F, -0.0170388725F, 0.0109549565F, 0.016494995F, - -0.00848276075F, -0.00588681921F, -0.00520207081F, 0.0114541594F, - 0.00940322224F, -0.00180396449F, 0.00799726602F, 0.0110878516F, - 0.00709998328F, 0.00939324498F, -0.0178074483F, 0.00315892766F, - -0.00226821029F, -0.00680163177F, 0.000408464897F, 0.00939817913F, - 0.00709950831F, -0.00373870367F, -0.0128362579F, 0.0021632507F, - 0.00607359083F, 0.0105294073F, 0.0116651487F, -0.00318220165F, - 0.00124811416F, 0.0109218303F, -0.00123262336F, 0.0072215437F, 0.0084319571F, - 0.00451467512F, -0.000611010706F, -0.0121888369F, 0.00748501718F, - -0.00615778333F, 0.00674939062F, -0.000335894292F, 0.00826248F, - -0.0163029209F, 0.00738032488F, -0.0126989735F, -0.00375013729F, - 0.00293091941F, -0.0165897645F, -0.00106116291F, 0.0192840137F, - -0.0140878223F, 0.00711837597F, 0.00332484208F, 0.00799936708F, - -0.00056109525F, -0.00443070242F, 0.00634751935F, -0.0117581645F, - 0.00299292244F, 0.00469817407F, -0.011773373F, -0.00547284167F, - -0.0162808467F, 0.0157060511F, -0.00965396687F, -0.0112309586F, - 0.00485555921F, 0.00134483119F, 0.00472392701F, 0.00916079618F, - 0.0198445655F, -0.0121751474F, -0.00967979711F, -0.00383697287F, - 0.00860353652F, 0.00790269859F, -0.00890131667F, 0.0107167764F, - 0.00974758063F, -0.0109342188F, -0.00118643208F, -0.00657964358F, - 0.000580325606F, 0.0034484514F, 0.000295512349F, -0.00043170806F, - -0.0114322044F, 0.0108414013F, 0.00119759107F, 0.000708720705F, - 0.0045833718F, -0.013887357F, -0.0134089058F, 0.0112364283F, 0.0114205172F, - -0.00740283635F, -0.0140840113F, 0.0150860026F, -0.00727427F, 0.0049714325F, - 0.00784298126F, -0.00293683656F, -0.0168140624F, -0.0226659141F, - 0.000589289644F, -0.0014717289F, 0.00400028797F, 0.00611113198F, - -0.00357515761F, 0.0253734235F, 0.00185361935F, 0.0110978829F, - 0.00770517159F, 0.0147723407F, 0.0104862954F, -0.0134391533F, - -0.000706781924F, -0.00455470709F, -0.0170813389F, -0.00898537226F, - 0.00257540634F, 0.000819541514F, 0.00791456F, -0.00971397944F, 0.0141916508F, - 0.000131699009F, -0.00512098894F, -0.00395612139F, -0.00694739074F, - -0.0142690036F, -0.00526017183F, -0.00839918759F, -0.00447455933F, - 0.0158339906F, -0.000148201492F, 0.0128195798F, -0.00182695745F, - 0.0114326505F, -0.00222180085F, -0.00971090887F, 0.00743510434F, - 0.00892101508F, -0.0038333917F, 0.00729898969F, -0.00331577961F, - 0.00469245203F, 0.00991563406F, 0.00243606698F, 0.00343145034F, - -0.00675501907F, -0.0023568077F, 0.00591349741F, 0.0167110637F, - 0.00340824085F, 0.00563934632F, 0.00224987883F, 0.000476537476F, - 0.00416564243F, -0.0218340755F, 0.00683616893F, -0.0019438907F, - -0.0116598792F, -0.006641543F, -0.0238001272F, 0.00949161407F, - -0.00852631312F, -0.0149376877F, 0.00557196187F, 0.011927384F, - -0.00933647156F, -0.0171397962F, 0.00724189123F, 0.0058634F, - -0.000927072426F, 0.0031126698F, 0.00328340847F, 0.00916586164F, - 0.00409257365F, 0.00947989617F, 0.00624549435F, 0.000151734959F, - 0.00886339135F, 0.00295229116F, -0.00196279702F, 0.0122882091F, - 0.00565193687F, 0.00262085162F, -0.00584109453F, -0.00323730242F, - -5.67840361E-5F, 0.00460744649F, 0.00880976394F, -0.0106657604F, - -0.0208050963F, -0.00258741947F, 0.00202633371F, -0.00415815227F, - -0.0131891211F, -0.00393328257F, -0.00598841067F, -0.0189334266F, - -0.0200821292F, 0.0071320571F, 0.0143096708F, -0.0130020501F, - -0.00804887805F, 0.0095954258F, 0.0112356329F, 0.0112811122F, - -0.00684068119F, 0.0045075668F, -0.00911579933F, -0.0100519974F, - 0.0117926747F, 8.17647451E-5F, 0.0132290721F, -0.0115729943F, 0.00621884177F, - -0.00912398752F, -0.000956898322F, -0.00247527519F, -0.008516673F, - -0.0114424014F, -0.00811994076F, 0.00410924293F, -0.00509156613F, - -0.000857649487F, 0.00183787779F, 0.00296900189F, 0.000204435404F, - 0.00397195062F, 0.00053654646F, 0.0065893759F, -0.0213207901F, - 0.00687327329F, -0.00465921126F, -0.000102667829F, 0.00731865084F, - -0.0108667025F, 0.000449607673F, 0.0164585076F, 0.00940428395F, - -0.00240497431F, -0.00773366401F, -0.00481441664F, -0.0107633164F, - -0.00039758737F, 0.00295716408F, -0.00906085502F, 0.0157791059F, - 0.000742999604F, -0.0173189119F, -0.00184255873F, -0.00508533465F, - -0.00677169953F, -0.0165903866F, 0.00342773623F, 0.00358721125F, -0.0074925F, - -0.0161771495F, -0.0028199F, 0.0125551727F, 0.0111357141F, 0.00586682186F, - 0.00475013861F, -0.0265388899F, 0.00825549569F, -0.00102406123F, - 0.00690905564F, -0.0147006419F, -0.00685739191F, -0.00913723744F, - -0.00324008195F, -0.00389330601F, -0.0125696901F, 0.015060137F, - -0.00345897349F, 0.0111589245F, -0.0144176092F, 0.00323728449F, - 0.00782592688F, 0.00246002595F, 0.00146000867F, 0.00224161451F, - 0.0116201025F, 0.0209739227F, 0.00615601335F, -0.00895176642F, - 0.00132683129F, 0.00500237383F, -0.00330726849F, -0.0108662555F, - -0.0163995773F, 0.0019915381F, -0.00185050745F, 0.00951694697F, - 0.0131726926F, -0.0113462871F, -0.0148902163F, -0.000722244557F, - -0.00735364249F, 0.00486994535F, -0.0164170116F, -0.00587264122F, - 0.00485702744F, 0.00790051091F, 0.0045279027F, 0.00508059328F, - 0.00267540105F, 0.0102412943F, -0.0139154F, -0.00647140713F, -0.00599286612F, - 0.00384955015F, 0.00575879868F, 0.0131306611F, 0.0046050828F, 0.0016218751F, - 0.0149111133F, -0.00397912459F, -0.0161369517F, 0.0338192396F, - -0.00279884832F, 0.00288151065F, 0.00081650744F, 0.00459327409F, - -0.0121348882F, 0.00655959919F, -0.00937164109F, -0.0114549836F, - -0.0111199273F, 0.0113064144F, 0.0150870737F, 0.00415251963F, -0.00578128F, - -0.00610038778F, -0.0118054943F, 0.013394068F, -0.00733677438F, - 0.00894347578F, -0.00833160896F, 0.00815415103F, 0.00547547173F, - 0.0130732749F, 0.0199463572F, 0.0109349303F, 0.00422317861F, -0.0136924908F, - 0.00631021F, 0.000795869739F, 0.00967699476F, 0.00158043555F, - 0.000571822806F, 0.000377333752F, 0.00871590804F, -0.00371024874F, - 0.00480215112F, 0.00322217F, -0.00194725313F, 0.000589990756F, - -0.00899143331F, -0.0126030026F, -0.00970660523F, 0.00146078027F, - -0.0143810613F, 0.00181671325F, -0.00986186322F, -0.00515589351F, - -0.000330293959F, -0.0130371852F, 0.00662167976F, -0.0180502124F, - -0.00974199F, -0.015746301F, 0.00693900324F, -0.0216276515F, 0.00934376474F, - -0.0103773205F, -0.00283500669F, 0.00580010377F, 6.8269539E-5F, - 0.00964762084F, 0.0101836994F, -0.014893733F, -0.00159973337F, - 0.00105471571F, 0.0017640302F, -0.00367243448F, 0.009904298F, 0.00749432063F, - 0.0133543462F, -0.00653837F, 0.00192587706F, -0.000376526063F, - -0.0127902906F, 0.0101553379F, 0.00270554377F, 0.00539030181F, - -0.00730044767F, -0.000584190653F, -0.00075970561F, -0.0140913259F, - -0.00383463129F, -0.00882741064F, -0.0151891122F, 0.00937035307F, - 0.00745620439F, -0.00129229669F, 0.0039165495F, 0.00313520897F, - 0.00536802923F, 0.000454566354F, -0.010584278F, -0.00422953209F, - 0.00312723033F, 0.01212712F, 0.00846833736F, 0.00820522662F, -0.00791785587F, - 0.0036945811F, -0.0151561145F, 6.27376648E-5F, -0.00621419773F, - -0.00286003225F, -0.00360621861F, 0.000954431889F, -0.0182320289F, - -0.00929459F, 0.00303583778F, -0.010790308F, -0.00862734392F, - -0.00190728449F, -0.0041715405F, -0.010152611F, -0.0212348849F, - -0.00244072382F, -0.00811713841F, 0.0111093009F, 0.00607442949F, 0.00913061F, - 0.0179012604F, -0.00712739863F, -0.00298371632F, 0.00646459591F, - 0.00939903408F, 0.00396471703F, 0.00794168748F, 0.00242961803F, - -0.0103146117F, 0.0129815424F, -0.00877392758F, -0.00626505772F, - -0.0190117452F, 0.00113627617F, -0.00389950583F, 0.00251785363F, - -0.0204338133F, -0.00147599611F, 0.00363414525F, -0.0152216358F, - -0.00476104207F, 0.0138409548F, -0.00328856311F, -0.0202475693F, - -0.00507425703F, 0.00123609148F, -0.00910505932F, 0.0126875835F, - 0.0205790475F, 0.0192897804F, -0.00611378485F, -0.00318332855F, - 0.0133932633F, -0.00019934494F, 0.0179950222F, -0.00953461137F, - 0.00847968925F, -0.00456777F, 0.0146814566F, -0.00524626719F, 0.00839460827F, - 0.011560386F, -0.00732943974F, 0.0120492028F, 0.0137911271F, -0.0169500913F, - 0.00617368054F, 0.0071210009F, 0.00923986547F, -0.0109405564F, - -0.00569094392F, 0.00544165308F, 0.00999117456F, 0.00474948436F, - -0.00909638684F, -0.00424722489F, 0.0108665377F, 0.00665981602F, - -0.00754653616F, -0.00504151592F, -0.00413309783F, -0.00347395195F, - 0.0112043172F, 0.0114162285F, -0.0026672245F, 0.00508830277F, - -0.00425397744F, -0.0078449687F, -0.00322460453F, -0.00888257194F, - -0.00776570057F, -0.0081029227F, 0.0225768331F, -0.0137260836F, - 0.0135445753F, 8.90241135E-5F, -0.00334359542F, 0.00793170836F, - 0.0010033031F, -0.01139405F, 0.0222195666F, -0.00157946325F, -0.002582388F, - -0.00134373514F, -0.00137744087F, -0.0119714625F, -0.0033908973F, - 0.00655434607F, 0.00592576F, -0.0216135811F, -0.000729826046F, - -0.00288433256F, 0.000705651939F, -0.00623942586F, -0.0124198925F, - -0.0024241826F, 0.00685114879F, 0.00781257916F, 0.00793736F, -0.00338293F, - -0.00270160404F, 0.0173449516F, -0.00523719704F, -0.0180157572F, - -0.002817865F, 0.00737095112F, 0.0104655614F, -0.000949052279F, - 0.0112863304F, 0.00581431715F, 0.0186337717F, -0.0126926713F, - -0.00582897477F, -0.00222006789F, 0.0164413191F, -0.0190411061F, - -0.00711268559F, 0.0139792832F, -0.00114936137F, -0.00420319F, - 0.00104171841F, 0.0154999439F, 0.00811808743F, -0.0154756838F, - -0.0119319139F, 0.0118666505F, 0.00663628802F, 0.005898959F, 0.00488778716F, - -0.00696620112F, 0.000658978301F, 0.00193876901F, -0.0199670363F, - -0.00532731088F, 0.000325764238F, 0.00638503721F, -0.00300396257F, - -0.00148228661F, 0.00555834174F, -0.0175512526F, -0.00581595721F, - 0.0084460834F, -0.00717175519F, 0.010792111F, -0.00910228863F, - 0.000317778322F, 0.0124412347F, 0.00890588854F, -0.0126067298F, - 0.00273661315F, 0.0148161668F, 0.00247413316F, -0.0100617949F, - -0.00470758323F, -0.0111587979F, 0.00245051063F, -0.0152296294F, - -0.00263812323F, 0.0167462286F, -0.00599395763F, 0.018476475F, - -0.0106868064F, 0.00283664162F, -0.0157795399F, 0.00891504157F, - -0.00458517391F, 0.00654001674F, 0.0090099331F, -0.00485885143F, - 0.0110736154F, 0.0153497709F, -0.00982276723F, -0.00700341212F, - 0.00241651828F, -0.0167093258F, 0.0134653384F, 0.0128934486F, - -0.00403972669F, 0.00978456344F, 0.00464172941F, -0.00201700465F, - 0.0106525384F, -0.014439106F, 0.012476135F, 0.0033974757F, 0.0100359721F, - -0.00461720582F, -0.00156401785F, 0.00610643253F, 0.00722426549F, - -0.00225060154F, -0.0131766675F, -0.0191903859F, -0.0097056916F, - -0.0208268259F, -0.0112352399F, 0.0138033833F, -0.0146121252F, - -0.00287182466F, 0.00857614074F, 0.00297925784F, -0.0141050117F, - 0.0135353673F, 0.00953521859F, 0.0192077216F, 0.00263368036F, 0.0105084917F, - -0.00552891381F, 0.0108628096F, 0.00567227928F, -0.033271119F, -0.011226424F, - -0.00971039571F, 0.00518287532F, -0.00447305199F, -0.00120696088F, - 0.0091474466F, 0.00398288F, -0.00614094408F, -0.00388910226F, -0.0108468859F, - 0.00908082072F, -0.00985303055F, 0.00893125497F, -0.00707507227F, - -0.00170108501F, 0.00363343675F, -0.0152639309F, -0.00865420233F, - -6.76201962E-5F, 0.00350162759F, -0.000361728336F, -0.00223735371F, - 0.0116430093F, 0.00251855794F, 0.0146835931F, 0.000296761F, -0.0132311489F, - 0.00627738703F, -0.00203055842F, -0.00171559257F, -0.0115053235F, - -0.00396296196F, 0.00951441191F, -0.00996545609F, -0.00404568855F, - 0.00545434142F, 0.000845140719F, -0.00129067188F, -0.0215546973F, - -0.000793545623F, 0.00133145868F, 0.00190566923F, 0.00113477011F, - -0.0109780356F, 0.0136496332F, -0.015152026F, -0.006254985F, 0.00573936384F, - 0.00434380118F, -0.0192600749F, -0.00259031355F, -0.00721284654F, - 0.0114314109F, 0.00135656202F, -0.00150411F, -0.00743269F, 0.00699598668F, - 0.00012361251F, -0.0130842794F, 0.00747748F, -0.0102310712F, -0.00183461956F, - -0.00377608277F, -0.0254941788F, 0.00260109105F, 0.00412282348F, - 0.00740306452F, -0.00168654F, 0.000236181921F, 0.00813339092F, - 0.00471249176F, 0.0157290641F, -0.0103403069F, -0.00562063511F, - 0.00892554782F, -0.00540846353F, 0.0129668741F, 0.00422446616F, - 0.0112657081F, 0.0100844409F, 0.0110272579F, -0.00283222692F, - -0.00142470445F, -0.0013838571F, -0.0078881951F, 0.00165730622F, - -0.0019432226F, -0.0144751342F, 0.00891684648F, -0.0186453443F, - 0.00986950845F, 0.00164673512F, -0.0178466663F, 0.0100514628F, - 0.00793790724F, 0.00119626021F, 0.00251882407F, 0.00282843504F, - -0.00138611218F, -0.000773047446F, 0.000982715865F, -0.00487883715F, - -0.00697182259F, -0.00606441172F, 0.0142215081F, -0.013506324F, - -0.00238924916F, 0.000702141086F, -0.00503786653F, 9.69935354E-5F, - 0.00350648421F, -0.00834049564F, -0.0018035199F, -0.0179428142F, - 0.00355641544F, -0.0035690777F, 0.00726108393F, -0.00191798632F, - 0.0154075334F, -0.00392700685F, 0.00630543055F, 0.0205682907F, - -0.00933223218F, 0.00180595799F, -0.0164511763F, -0.0018075509F, - 0.009911431F, 0.0099700233F, -0.00398922432F, 0.0101699308F, -0.00532519212F, - -0.00850048754F, -0.000198323876F, 0.00373755582F, 0.00396889681F, - 0.0113913007F, 0.0024867109F, 0.00821589492F, 0.014274844F, 0.015742911F, - -0.00153007323F, 0.00370588386F, -0.0142944334F, -0.00130848086F, - 0.00765679264F, -0.0057614008F, 0.00716373604F, -0.00950268F, -0.0103373341F, - -0.0065715909F, -0.009983222F, -0.00700126821F, 0.012427113F, 0.00434958423F, - -0.0092289662F, -0.00549263926F, -0.00357603398F, 0.00689362222F, - -0.0139574455F, -0.0143853268F, 0.00663427589F, 0.00373587804F, - -0.00326235197F, -0.00342820678F, 0.0126443245F, -0.00858793408F, - -0.00191069476F, -0.0101382118F, 0.00421652663F, 0.0166470911F, - -0.0174129661F, 0.000360001315F, 0.00989639F, -0.0126515962F, 0.00908038579F, - -0.000504746102F, 0.0181332156F, 0.00432175025F, 0.01417909F, 0.0127862152F, - -0.00617247866F, -0.0048855287F, 0.00988520589F, -0.00420631096F, - 0.0051131472F, 4.91286082E-5F, 0.0122498265F, 0.00549114915F, - 0.000826490286F, 0.0124544203F, 0.00184435886F, -0.00235862238F, - 0.000823406444F, -0.00566631F, -0.00929725915F, -0.00192491838F, - -0.00646779034F, 0.0054310346F, -0.0118890498F, 0.00447457517F, - -0.00354619557F, 0.00531734107F, 0.00777564105F, 0.00205649831F, - -0.00581671344F, -0.00884611F, -0.00689366274F, -0.000196212073F, - -0.0073137288F, 0.0060855276F, -0.00705983816F, -0.0268557873F, - -0.00213247933F, -0.00743870903F, -0.0052339565F, 0.0081064906F, - 0.00134939165F, -0.0155838672F, 0.0128738433F, -0.00461251242F, - 0.0144384531F, -0.00332161016F, 0.00533750607F, -0.008685546F, - -0.0161228813F, 0.0150858806F, -0.00333269499F, 0.00561361713F, - 0.0168033186F, -0.0120945163F, 0.0130096F, 0.00488500902F, -0.00999388658F, - -0.00875897892F, -0.00162657432F, 0.00451555615F, 0.00426041242F, - 0.00957663264F, -0.00395563198F, 0.00306608505F, 0.00282775983F, - -0.00596858934F, 0.01196172F, -0.0100061009F, -0.0129884435F, - -0.00634744577F, -0.00474302424F, 0.0118057849F, 0.00854314771F, - 0.0201460421F, -0.00291947508F, -0.0107206255F, -0.016021844F, - -0.00199463824F, -0.00669793319F, 7.52961641E-5F, 0.000403491082F, - 0.0131933931F, 0.0134530598F, 0.00675225072F, -0.00922331866F, - -0.0149265705F, 0.010042672F, 0.0115387645F, 0.000775117F, 0.000565483177F, - 0.00733144348F, -0.0027944881F, 0.00454382319F, -0.00258267438F, - -0.00869199075F, 0.00543969357F, -0.000995856128F, -0.000824638584F, - 0.00620852225F, -0.0128303422F, 0.000700930366F, 0.00449095899F, - 0.0134903165F, -0.00677257031F, 0.000814949803F, -0.00954622868F, - 0.020135833F, -0.0117389318F, 0.00642124517F, 0.0198714081F, -0.00512183271F, - -0.00540655339F, 0.00634357706F, -0.00374165596F, -0.00364950974F, - 0.0172991697F, 0.0188301373F, 0.00429553166F, -0.0011062118F, - -0.00309665664F, 0.00155921723F, 0.00985906F, -0.00542653445F, - 0.00205530599F, 0.000465342979F, -0.0126794772F, -0.000580352033F, - 0.00110347546F, -0.00509355729F, 0.0177341849F, 0.0155430278F, - 0.00609762967F, -0.00599576579F, -0.0143108498F, 0.00857799221F, - -0.00148417661F, -0.00602199929F, 0.000894779223F, -0.00714370795F, - 0.00215777406F, 0.00749019627F, -0.00194598944F, -0.000381123798F, - -0.00799370743F, 0.000289463263F, 0.00802429765F, 0.00678232964F, - -0.0154816248F, -0.00861876644F, 0.00680924254F, 0.00138258841F, - -0.00708211865F, -0.00654735649F, -0.000231331433F, 0.000888113864F, - 0.003409717F, 0.00892509054F, -0.002904183F, 0.00481455447F, 0.00622221688F, - 0.00457579782F, -0.00258017518F, 0.0152330035F, -5.36268417E-5F, - 0.00722273579F, -0.00237820228F, -0.00905564893F, 0.00864997786F, - 0.00638301345F, -0.00803981721F, -0.00309899286F, -0.00924331F, - -0.00891389232F, 0.000607355323F, -0.00730715599F, -0.0211430844F, - 0.00510421256F, 0.0102985119F, 0.00108750351F, 0.00384809077F, - 0.00877888314F, -0.00776275573F, -0.0110995397F, 0.0158661567F, - 0.0254115704F, 0.00674606254F, -0.00515933149F, 0.0154518513F, - -0.00295760157F, -0.00650635175F, 0.0026148383F, 0.00887743F, -0.0142455492F, - 0.0020208084F, 0.00972937047F, -0.00590151036F, 0.005072081F, 0.0148957372F, - -0.00385108637F, 0.00133610656F, 0.0124067087F, -0.0140627418F, - -0.0165782962F, 0.00935504772F, -0.0127412993F, -0.00756297866F, - -0.00725017954F, 0.0187051482F, -0.00569247454F, -0.00203902042F, - -0.00276087737F, -0.0100659905F, 0.00801427476F, -0.0108845076F, - 0.000645166729F, 0.0163665395F, 0.00329759275F, 0.00289757876F, - 0.00437728036F, 0.00629142346F, 0.00507390359F, -0.00764480652F, - -0.010025599F, -0.00269736606F, 0.00218277564F, 0.00562984077F, - 0.00623707334F, -0.0064295372F, -0.00823255908F, 0.00729229F, - -0.00524986163F, 0.00862046238F, -0.00998331048F, -0.0103434315F, - -0.00682512065F, 0.0138489315F, 0.00661110645F, 0.0159894936F, - 0.00114289706F, 0.00386219053F, 0.0256672781F, -0.00315723335F, - 0.0151672801F, 0.0230624303F, -0.00200150418F, -0.0225721207F, - 0.00149986905F, -0.0110580074F, -0.00474620238F, 0.00484093698F, - -0.00263882382F, -0.0191327967F, 0.00699362904F, 0.00948849134F, - 0.00675970828F, -0.00227155606F, 0.00756864762F, 0.00791097F, - -0.000762148527F, -0.00377204781F, 0.000639164238F, -0.00414423971F, - 0.00674623437F, 0.00260010245F, 0.00542755146F, -0.0103736855F, - -0.00536068808F, 0.00563509064F, -0.00269379443F, 0.00374070136F, - -0.00882680342F, 0.00354744238F, -0.00901869498F, 0.000946403656F, - 0.000649476191F, -0.00765948F, 0.0169615746F, 0.00713179912F, 0.0075075631F, - 0.00816343073F, 0.00720695825F, 0.0020997352F, -0.0198560636F, - -0.00620160811F, -0.00208520237F, 0.000146094011F, -0.00558254309F, - -0.00583352335F, -0.0143796662F, 0.0028111143F, -0.0156678222F, - -0.000313019438F, 0.0131244138F, 0.0032871333F, 0.00996939652F, - 0.00956347305F, -0.0178993512F, 0.00878467411F, -0.00131786114F, - 0.00392156374F, -0.0168418046F, -0.00378916063F, -0.00457176752F, - -0.0204859693F, -0.0218044575F, -0.0140192676F, -0.0181087311F, - -0.00238605239F, -0.0145440847F, -0.0205020271F, 0.0028394768F, - 0.0145883691F, 0.0137949167F, -0.0102535347F, 0.00818758F, 0.00187577528F, - 0.0138689214F, -0.00960265566F, -0.00543315709F, -0.0136832017F, - 0.00825013779F, -0.00460509118F, 0.00492594857F, 0.00392750138F, - 0.000401926722F, 0.00767199416F, -0.00163217983F, -0.00146287528F, - 0.0253759231F, 0.0019082739F, 0.00130747817F, 0.00260432716F, 0.00570719456F, - -0.0100012217F, 0.0103468569F, 0.00183131592F, 0.00193279772F, - -0.00176939485F, -0.0155207114F, 0.00705315173F, -0.0103688557F, - 0.00903296098F, -0.00449014502F, 0.00927102938F, 0.018134281F, - -0.00493468577F, -0.00331743F, -0.00152821466F, -0.00253953552F, - -0.00416629622F, -0.0186721031F, -0.00600344222F, -0.00999125186F, - 0.0119473655F, -0.00897907279F, 0.0147230485F, -0.00397888711F, - -0.0234641414F, -0.000984688872F, -0.0044021965F, -0.00515243411F, - 0.0104433233F, -0.0145964772F, -0.013800079F, -0.00714029279F, - -0.00192053174F, -0.0120857805F, -0.0153516736F, -0.00366958138F, - 0.0105937757F, -0.00584186893F, 0.00774454605F, 0.00126402418F, - -0.0033347276F, -0.0161841922F, 0.00317594525F, -0.00858912058F, - -0.00198432035F, 0.00369482F, 0.0254014861F, 0.016698597F, -0.00303919031F, - -0.00661713444F, -0.000345804729F, -0.00502072275F, 0.00160582596F, - 0.00401780615F, -0.00494345929F, -0.00211347826F, 0.0143066803F, - 0.0254885238F, 0.0104467375F, 0.000635122415F, -0.00360755203F, - 0.000627673347F, 0.00180392386F, -0.0178387798F, -0.00391388452F, - -0.00793881342F, 0.00970621686F, -0.00139020407F, 0.00246500084F, - -0.00429168344F, -0.00412431639F, 0.000978641328F, 0.00253055175F, - -0.0159456562F, -0.00601330865F, 0.0130851045F, 0.00508471765F, - -0.012299086F, -0.0245719738F, -0.0026485268F, 0.00194025983F, - 0.00832810439F, -0.00204771F, -0.00310538639F, 0.0070186751F, - -0.00606941292F, -0.00287002767F, 0.024815008F, 0.00341338967F, - -0.00120117154F, 0.00543927727F, -0.00201872154F, 0.00527492724F, - 0.00318684732F, -3.18043785E-5F, -0.000123892067F, 0.0104316268F, - -0.00163732644F, 0.0123688206F, -0.00130061153F, -0.00340121705F, - 0.001667295F, -0.00251093786F, 0.00667114789F, -0.0104970988F, 0.0091307722F, - -0.00192718825F, -0.000710070133F, -0.000803806062F, 0.00933422614F, - -0.0135707203F, 0.0111876391F, -0.0101729268F, -0.00691588596F, - -0.00139999145F, 0.0042817397F, 0.00655299751F, 0.0161758F, 0.0186724421F, - 0.00660281116F, 0.00747674238F, -0.00267312303F, -0.00104773615F, - -0.00308906124F, -0.0141268214F, -0.020285625F, -0.00031313629F, - 0.00316062849F, -0.00986199081F, 0.0105108144F, 0.00944234431F, - 0.00754843885F, 0.000847589108F, -0.00781584F, -0.0213365797F, - 0.00212896499F, 0.00633255439F, -0.0102404058F, -0.00443816558F, - -0.000293877354F, 0.00843161903F, -0.00502035534F, 0.0186402332F, - 0.00403329358F, -0.00189543609F, 0.00270255492F, 0.0141438115F, - 0.0147948219F, 0.0153063377F, -0.0063176821F, -0.00457745744F, - 5.68682954E-5F, 0.00941525307F, 0.00550416484F, -0.0101328371F, - 7.16487702E-5F, 0.00870079361F, -0.00684957625F, 0.000999811688F, - 0.0111516472F, -0.0138566149F, -0.0098284306F, 0.00764265144F, - -0.00609194394F, 0.00426895823F, -0.00656871172F, -0.00607952382F, - 0.00550140068F, -0.00177791726F, -0.00476185884F, 0.0113058947F, - 0.00617003487F, 0.00969737582F, -0.0158781856F, -0.0165918767F, - 0.0123525821F, 0.000756647321F, 0.00609227316F, 0.0138653126F, - -0.0107753817F, -0.00436149724F, 0.0058758636F, -0.00554784155F, - 0.00732722925F, -0.00773652783F, 0.0155263804F, 0.000883155852F, - -0.00132990908F, 0.00489410432F, 0.0010725226F, 0.000955269F, - -0.00175141857F, 0.00594115816F, -0.00798492786F, 0.020011289F, - -0.00136406382F, 0.00145630899F, -0.00538961496F, -0.00171606848F, - -0.0228167884F, -0.00977043808F, -0.00187470671F, 0.012074261F, - -0.0029122096F, 0.011576782F, 0.00565431546F, 0.000660419522F, 0.0150811346F, - -0.00979294721F, 0.00617172616F, 0.0104134725F, -0.00987467356F, - -0.00723765837F, 0.00571917463F, 0.00517107919F, 0.00478765741F, - 0.016270319F, -0.00459967926F, -0.00414656F, -0.00797348935F, -0.000607479F, - -0.00181028584F, 0.00427359436F, -0.0167947281F, 0.00426570373F, - -0.00367882708F, -0.00597434212F, -0.00736994F, 0.0117227221F, 0.00111751F, - 0.00107343472F, 0.0165523943F, 0.000346653891F, -0.00445862534F, - 0.00961721502F, -0.000103735307F, -0.00358841242F, 0.00798347313F, - -0.00912835F, -0.00187539659F, 0.00170978F, 0.0160842072F, 0.00969422795F, - 0.00234738109F, -0.0134181231F, -0.0279598329F, 0.00777069619F, - -0.0207393486F, -0.0104116481F, -0.00158310693F, 0.00584737118F, - 0.00270392257F, 0.00260994514F, 0.00596291F, -8.95208E-5F, -0.00914051291F, - -0.00238894112F, -0.00666612945F, -0.00497452635F, -0.00752374157F, - 0.0118305758F, 0.0064993049F, 0.0207115244F, 0.00398291415F, 0.00359667721F, - -0.0108448602F, -0.000749867642F, 0.01419592F, -0.00192779885F, - 0.0188158862F, -0.0114131132F, 0.00702804467F, 0.00460037496F, - 0.00890181307F, 0.0213947818F, -0.00332691101F, -0.00377380173F, - 0.00559643703F, 0.00267558894F, 0.00441816403F, 0.00398362568F, - -0.014540147F, 0.015072261F, 0.00108696031F, -0.0110949017F, -0.0246182606F, - -0.00438364549F, 0.00420136563F, -0.0139292581F, -0.00129075535F, - -0.000817045802F, 0.00772941159F, 0.0142721888F, 0.00334891467F, - 0.00059158029F, 0.00413123704F, 0.00620916067F, 0.0160066746F, 0.006141291F, - 0.00329975784F, -0.00245538657F, -0.0148275327F, -0.0065816883F, - 0.00294654095F, 0.00836873427F, -0.0149130542F, 0.00491171F, 0.000532338745F, - -0.00940586347F, -0.000400216173F, 0.00303742057F, -0.00177564076F, - -0.0126520395F, -0.015258925F, 0.00701963808F, 0.0021788436F, - -0.00583978649F, 0.00873520505F, 0.00624904269F, -0.0025604798F, - -0.0115780532F, 0.00205143844F, -0.00147132028F, 0.0127058F, - -0.000108233879F, 0.00203506718F, -0.00692844437F, -0.00789161678F, - 0.0161573514F, -0.00204585935F, 0.00109368074F, -0.00497907167F, - -0.00358441216F, 0.00795499515F, -0.00266949483F, 0.00116530526F, - 0.00606399728F, -0.00857618544F, -0.0135192527F, 0.0134631777F, - 0.00295270677F, -0.000509500736F, 0.0182730295F, 0.00391919538F, - -0.00492909597F, 0.00999675132F, -0.0256509352F, -0.00549943792F, - -0.0341723263F, -0.00737404916F, 0.0197913647F, 6.95670169E-5F, - 0.00395012554F, -0.0038151436F, 4.56097077E-5F, 0.00446882518F, - -0.00637867628F, -0.00295781088F, 0.0177737083F, -0.00791976F, - -0.0178079195F, -0.00446970109F, 0.0040035625F, 0.010065631F, 0.0113515249F, - -0.00342581118F, 0.010936385F, 0.00780216232F, 0.00734199304F, 0.0115672844F, - -0.00750575F, 0.00559615251F, 0.00775133679F, -0.00640123244F, - 0.00684683491F, 0.00765427109F, -0.0143191768F, 0.00386386621F, 0.010294605F, - -0.00477295322F, -0.0141409691F, 0.00213076943F, 0.000959111436F, - -0.0023086383F, -0.011094464F, -0.00263162632F, 0.0025854446F, - 0.00892124325F, -0.00086210086F, -0.0038130905F, 0.00592326839F, - -0.0129489992F, 0.0113958409F, 0.000148116436F, 0.00169986731F, - 0.0139915636F, -0.0106213205F, -0.00642440841F, -0.00118597376F, - -0.00155132671F, 0.00248704688F, 0.00919230189F, -0.0062808427F, - 0.0107248398F, -0.0057919072F, -0.011165618F, -0.0174134094F, 0.0123253912F, - 0.0159916449F, -0.00108298741F, -0.00449184934F, 0.00155515422F, - -0.0103882188F, 0.00147537189F, 0.00671634311F, -0.0176607836F, - 0.000520290749F, -0.00648910319F, 0.00401158584F, 0.00457535544F, - -0.00286605489F, 0.00273030484F, -0.00429387344F, 0.000808779092F, - -0.0114902928F, 0.00794317108F, 0.0082121538F, -0.0111992564F, - 0.00252664788F, -0.00565089937F, 0.0119962394F, -0.0122380964F, - 0.0111757275F, 0.00345366658F, 0.0123003153F, -0.00894245692F, - 0.000982855563F, -0.00169249612F, 0.000754483335F, 0.000660036F, - -0.00203100522F, -0.00522769662F, -0.0161452983F, -0.013901365F, - 0.000622130057F, -0.0143775977F, 0.0102122519F, 0.00447945902F, - 0.00240942324F, 0.00250885938F, -0.00849091541F, 0.00453834655F, - -0.0132710803F, 0.00367494207F, 0.00418651151F, 0.00460906373F, - 0.00964735262F, -0.0184924752F, 0.0091607077F, 0.000938134268F, - 0.0155083491F, -0.00615695352F, -0.0010771954F, 0.00401597191F, - 0.0021789805F, 0.0018781774F, -0.00097503385F, -0.00447532721F, 0.0150329F, - -0.00292137195F, 0.0120722763F, -0.00611107284F, -0.00756055629F, - -0.00459836842F, -0.0119006457F, 0.00688747503F, -0.00471612439F, - 0.00659536151F, -0.018590996F, -0.0101012951F, 0.00377810909F, - -0.00439132843F, -0.0104748F, 0.0044442662F, -0.00281858724F, 0.00390353031F, - 0.00986471213F, -0.0152894519F, 0.00491141668F, -0.000246796262F, - -0.0165093821F, 0.0102973217F, 0.0119823087F, -0.0114528518F, 0.00104409538F, - -0.00687032146F, 0.0102043273F, 0.00295380619F, 0.0186670572F, - 0.00529346103F, -0.00247490266F, -0.0122920061F, 0.00145245786F, - -0.00483108405F, 0.00379379396F, 0.00534779718F, -0.00652681943F, - -0.0085472865F, -0.00218478963F, -0.0130641814F, 0.00579546578F, 0.02087784F, - -0.00456127198F, 0.00995901227F, 0.00559455063F, 0.000499813352F, - -0.00678678835F, -0.00139483274F, -0.00545871118F, 0.000317989819F, - -0.00440134807F, 0.00438532047F, 0.00903363153F, 0.000802528288F, - -0.00657092361F, 0.00696174195F, -0.0107777873F, 0.00691225799F, - 0.00160544016F, 0.00911286F, 0.00020651496F, 0.0278061442F, -0.00772581762F, - -0.00369587424F, 0.0173218232F, 0.0112713715F, 0.0104674175F, 0.00416891649F, - -0.00389385642F, 0.0128223849F, -0.00477155298F, 0.0131699191F, - 0.000363931089F, 0.00660485076F, 0.00501710922F, 0.00873635F, 0.00240829866F, - -0.0063277646F, 0.0122228405F, -0.0120914755F, -0.00419241656F, -0.00967704F, - 0.00527803274F, -0.00100965274F, 0.00629266817F, -0.0141111491F, - -0.00756964693F, -0.0180539694F, -0.00903360266F, -0.00332884374F, - -0.00511254463F, -0.0115309088F, 0.0180224627F, -0.00658281753F, - 0.00469128555F, 0.0015429199F, 0.0123246443F, -0.0133213094F, - -0.00890617911F, 0.00761257391F, -0.00636412F, -0.0170552786F, - -0.000315226614F, 0.00583206F, -0.0118423896F, 0.0117249861F, - -5.44310387E-5F, -0.00111627846F, 0.0162319317F, 0.0108071472F, - 0.00991448201F, 0.0114286887F, 0.000558962405F, -0.00485242624F, - -0.0173561331F, -0.0129278414F, -0.00798370782F, 0.00598064624F, - -0.00212052F, -0.00514888624F, 0.00572280446F, -0.0107071409F, - 0.00296789594F, 0.0117446678F, -0.0117502306F, -0.00767440395F, - 0.00929301698F, -0.00551816402F, -0.00783040747F, -0.00169631431F, - -0.0118372673F, -0.00844010245F, -0.00489069F, 0.00630274F, -0.00684327073F, - -0.00428666128F, 0.00519749848F, -0.0134286946F, -0.00573210232F, - 0.0142203644F, 0.00466394145F, 0.0083418563F, 0.00332156941F, -0.017360663F, - 0.0130568715F, 0.00940584298F, -0.0018976659F, 0.0055763307F, 0.0174762681F, - 0.0113174161F, 0.00902158F, 0.00100185105F, -0.00514295045F, 0.000233098559F, - -0.0141602904F, -0.00307345856F, 0.00282246619F, 0.0133979209F, -0.00328689F, - 0.000232639562F, 0.00902524F, -0.0116003277F, 0.0146626281F, 0.0081310207F, - 0.0117429178F, -0.00669280067F, -0.0136374952F, 0.00727308495F, - 0.00310805975F, -0.0104999933F, 0.014847694F, -0.00403567031F, - -0.00922598504F, -0.00123357493F, 0.00101911975F, 0.00250840187F, - 0.00571685052F, -0.00554398F, -0.0069865007F, -0.0130405501F, -0.0116823427F, - 0.00388718F, -7.62728159E-5F, -0.00237315614F, 0.00935648847F, - -0.0174343549F, -0.00317796785F, 0.0112733757F, 0.0108066304F, - -0.0126515096F, -0.010834083F, -0.00152983947F, 0.0165652987F, - -0.0125133358F, 0.00916692335F, -0.0126095358F, -0.0122858733F, - 0.0130930273F, -0.0108137112F, 0.0149592618F, -0.00873578F, -0.0109571172F, - 0.00680720946F, -0.0122162905F, 0.00836920366F, 0.0189389344F, - 0.00571445329F, 0.000403601531F, 0.000783774303F, -0.000352173578F, - -0.00707886182F, 0.00177699514F, -0.0158652253F, 0.00572845805F, - -0.000525523443F, -0.00751492567F, -0.0143329334F, 0.00500885F, - 0.00984617136F, -0.00198910804F, -0.0196395852F, -0.0019204102F, - 0.0132601215F, 0.00527075259F, -0.00738138426F, 0.00130446872F, - 0.0030111263F, 0.0183926877F, -0.00913893897F, -7.31815E-5F, -0.011403068F, - -0.00125762052F, 0.0107548889F, -0.000945302483F, 0.0122311199F, - 0.00255447067F, 0.00494859F, 0.000475172506F, -0.0132444026F, 0.00949559826F, - 0.0142862024F, 0.000315220066F, -0.00920971297F, 0.00470872642F, - 0.00539943343F, 4.51675369E-5F, 0.0052702371F, 0.00291574514F, - -0.00693598855F, -0.00782562047F, 0.00645301072F, -0.0146531258F, - 0.00213443558F, -0.00690236175F, 0.000253131584F, 0.00675558904F, - -0.0108878734F, 0.0234610867F, -0.0185009595F, 0.00589166582F, 0.0116948364F, - 0.00479350798F, 0.00243461411F, -0.0123669263F, 0.00491468329F, - -0.0256169718F, -0.00877799932F, -0.00599773973F, -0.0144015085F, - 0.00656949123F, -0.0150905419F, 0.0112076038F, 0.013004587F, 0.00817379169F, - 0.0226342045F, 0.000261052919F, -0.0198266022F, 0.00281470898F, - -0.00762368785F, -0.0192213655F, -0.00495188544F, 0.0188514907F, - 0.0103181759F, 0.0068453392F, -0.00502070924F, 0.0238830596F, 0.0178635903F, - 0.00165783579F, -0.0127247889F, -0.0141014215F, -0.0132942125F, - -0.0133721242F, 0.0213597175F, 0.00758615369F, 0.0019426553F, -0.0129320016F, - 0.0101582976F, 0.00894244295F, 0.00282341917F, -0.0043577156F, -0.00880728F, - -0.00435435F, 0.00590763846F, -0.00875315256F, -0.00996347F, 0.00824798457F, - -0.0142544573F, 0.013830998F, 0.00853586197F, 0.0114285769F, 0.0111149652F, - 0.00243579154F, -0.011837719F, 0.00654102257F, -0.00679764803F, - -0.0293632373F, -0.00573277753F, -0.0112413336F, -0.00429847045F, - -0.00325004128F, 0.00767567754F, 0.00524342945F, -0.0113656102F, - -0.00405222643F, 0.00822348F, -0.00019794851F, 0.00704139657F, - -0.0126277618F, -0.0107950196F, 0.000637637509F, -0.00599131966F, - 0.0191095918F, -0.00977444462F, -0.0109030381F, 0.0028402016F, - -0.00113727758F, -0.00224586017F, -0.0184538439F, -0.019992236F, - -0.00765316887F, -0.00399350654F, 0.00800313149F, 0.0142205982F, - -0.00474886643F, -0.000859986758F, -0.0118320668F, -0.00606947718F, - -0.0185996182F, -0.00778925838F, 0.00718946848F, -0.0100755291F, - 0.0164395291F, -0.00969918072F, -0.00534508331F, -0.000660648104F, - 0.00477556372F, -0.00500214845F, -0.0120482212F, -0.00271390425F, - 0.0186586492F, 0.0258328672F, 0.0146466643F, 0.000771487597F, -0.0030523811F, - -0.00657990109F, 0.00580186164F, 0.00621089386F, -0.00575384824F, - -0.0141922915F, 0.013429963F, 0.00133999F, -0.00456036534F, -0.00901100878F, - 0.00436699623F, 0.000244363706F, 0.00823528599F, -0.00140632666F, - -0.0190052F, 0.00316752447F, 0.0064594727F, 0.000716577459F, -0.00932266191F, - 0.0168270394F, 0.00527530722F, -0.00678707939F, 0.00696581556F, - -0.00595766352F, 0.00859448779F, 0.0079957759F, -0.0140721966F, 0.00812073F, - 0.00142497092F, -0.00425457442F, 0.00126016617F, 0.00822795462F, - 0.00766486209F, -0.00210711686F, -0.00424264697F, 0.013603461F, - -0.0117784943F, 0.0131798824F, 0.0138405887F, -0.00897514448F, -0.00019633F, - -0.0156683195F, 0.00500394218F, 0.00322319753F, -0.00865332317F, - 0.00486142375F, 0.00464977929F, 0.000879878353F, 5.52665406E-5F, - 0.010657561F, 0.00763829285F, 0.000508240191F, -0.01461206F, -0.0130167929F, - 0.00759852398F, -0.002793469F, -0.0129074706F, 0.0105782375F, 0.00761299673F, - -0.0106704142F, 0.00631218776F, 0.0119917262F, 0.0040937569F, 0.00910525676F, - 0.0155975176F, 0.00490976544F, 0.00580132054F, -0.00415161531F, - -0.0112436954F, -0.00460193586F, -0.00988458283F, -0.00137592049F, - 0.00672226F, -0.0120002637F, -0.000544991868F, -0.0170877595F, - -0.00189419335F, -0.00565015571F, 0.000179104507F, -0.00134765764F, - -0.00314844446F, -0.00385350036F, -0.000132933652F, 0.00512388721F, - -0.0132945832F, 0.00432968326F, 0.00965019595F, 0.00664926367F, - 0.000343518623F, -0.0201076511F, 0.00752319908F, 0.0042283223F, - -0.00129166828F, -0.0200583953F, 0.00953318458F, 0.00330536207F, - 0.00130766584F, 0.00190895644F, 0.00273446622F, -0.0042462796F, - 0.00420422573F, -0.0151061043F, -0.00923043F, -0.00290146051F, - -0.0151747447F, 0.000891653239F, -0.00339530781F, -0.0213533081F, - -0.00968703F, -0.00432495819F, -0.0083657885F, 0.00320425699F, - 0.00225831661F, -0.00115593465F, -0.00819172524F, 0.000260513654F, - -0.00221930793F, 0.00558548188F, 0.003847552F, -0.00688409386F, - -0.0015551449F, -0.00870279595F, 0.00142554753F, 0.0103983637F, - -0.0133907488F, -0.00714761112F, 0.0109405145F, 0.00925918575F, - -0.00664618751F, 0.00572268525F, -0.00113471132F, -0.00818061084F, - 0.00918477401F, -0.00229149451F, -0.00615567667F, -0.00209924532F, - -0.0030564589F, 0.00311525469F, 0.00414390163F, 0.000863470836F, 0.00740644F, - -0.0138352839F, 0.00381878624F, 0.00263877469F, -0.00346331811F, - 0.00794366281F, 0.0199730173F, 0.016805321F, -0.00940269791F, 0.00611123582F, - 0.0107873762F, 0.00439097267F, -0.00743832812F, -0.000379795762F, - -0.00500246603F, 0.0123433769F, 0.0057455604F, -0.013896537F, 0.00803247932F, - 0.00498803426F, -0.0029429174F, -0.0114570493F, 0.0128170298F, - -0.00647909101F, -0.00631154608F, -0.0113024991F, 0.00264160219F, - 0.0063731703F, 0.00453042891F, 0.0197554678F, -0.00288535189F, - 0.00117230369F, 0.00942064F, -0.00867826F, 0.0066358163F, -0.0117944153F, - -0.0122280987F, -0.00435648486F, -0.00889294408F, 0.00633804034F, - -0.0226952042F, -0.00307927141F, -0.000341620558F, 0.00502269762F, - -0.00740516325F, 0.010287893F, -0.00520543335F, 0.0178238321F, - -0.00216582417F, 0.0113336788F, -0.00796850119F, -0.020087216F, 0.000691027F, - 0.0113873295F, -0.00910983607F, 0.00178009132F, -0.0143731814F, - -0.00450948812F, 0.00852904562F, 0.0100003174F, 0.00562996976F, - -0.0179635175F, 0.00453528F, 0.00790848583F, 0.0032694696F, -0.000951697119F, - 0.00772507349F, -0.00543411F, 0.0154550485F, -0.00668624276F, - -0.00392552046F, -0.00633794395F, -0.00546100596F, 9.0795249E-5F, - 0.0122302053F, 0.0153323561F, 0.0146355843F, 0.0150325811F, 0.00903636F, - 0.0068326178F, 0.00260658399F, 0.00884311087F, -0.0127270455F, - 0.000138792209F, 0.00842916F, 0.0109771816F, 0.00839006156F, 0.000312357181F, - 0.0193455108F, 0.00617605867F, -0.00375698879F, 0.00798854F, - -0.000152886641F, -0.0179128F, -0.00143971387F, -0.0106503237F, - 0.0019768416F, 0.0125346165F, -0.00345577952F, -0.0115960287F, -0.010078799F, - -0.0230669603F, -0.00683505414F, -0.000821203867F, 0.00258209F, - -0.00756561197F, -0.00842525624F, 0.00667294115F, -0.00576494588F, - -0.00402099267F, 0.0012520937F, 0.00735330535F, -0.00180259498F, - -0.00358319655F, -0.000275132857F, 0.00795870088F, -0.00337168481F, - -0.00580526702F, -0.0102355415F, 0.00880421884F, -0.0229309089F, - 0.00644393079F, -0.0142175201F, 0.00470659882F, 0.00896550715F, - -0.00465477398F, -0.00435984228F, -0.00241460814F, -0.000678380486F, - 0.00712403236F, -0.00986332074F, 0.00277957134F, 0.011729517F, - 0.00172983494F, -0.0175636075F, 0.00118877622F, 0.0024738037F, 0.0117174992F, - -0.0179753136F, -0.00217644661F, -0.00864558388F, -0.00546369841F, - -0.00294519356F, 0.0149876643F, -0.00147263461F, 0.00345274736F, - 0.0142848119F, 0.00921899173F, -0.00317952409F, -0.00169067131F, - 0.0192646496F, -0.00865906943F, 0.0140936254F, 0.00201288378F, - -0.0097577218F, -0.0131282788F, -0.00701409904F, -0.0153605752F, - -0.00513053266F, 0.00964442827F, 0.0203753524F, 0.0018503922F, - -0.00819600467F, 0.00834161788F, 0.00650813431F, 0.00439399434F, - 0.00521914475F, -0.0194336418F, 0.00697208429F, 0.013350362F, 0.00720590819F, - 0.00548471417F, 0.0195066258F, 0.00143367576F, -0.0169659406F, - -0.0015323034F, -0.00990177598F, -0.0036346782F, 0.00824583787F, - -0.0126744146F, 0.000271922851F, 0.00176404952F, 0.00601978041F, - 0.00248938613F, 0.0100244582F, 0.00505215116F, -0.0140789282F, 0.0169734806F, - -0.00750333816F, 5.8528316E-5F, -0.000879982952F, 0.00260281703F, - 0.00853117649F, -0.00650220038F, 0.0140731037F, 0.0118474625F, - 0.00226693763F, -0.00264566182F, -0.00700089848F, 0.0139185674F, - 0.00345578021F, -0.00393449469F, -0.00515920157F, 0.00957557652F, - 0.0080285091F, -0.00539242662F, 0.0115917735F, -0.00810783543F, - -0.0114843072F, -0.0205459055F, 0.0052960855F, 0.01251237F, -0.00663645472F, - -0.00727131031F, 5.90041873E-6F, -0.0114133023F, -0.0120354937F, - -0.00424769428F, -0.0054216804F, -0.00118147838F, -0.0127429115F, - 0.0100501897F, -0.00116857653F, 0.0072308532F, 0.00122684217F, - -0.00455801748F, -7.03425758E-5F, -0.00224448112F, 0.0160280541F, - -0.00286313682F, -0.002619704F, -0.00379232131F, -0.00185662985F, - -0.0073167379F, 0.000525663432F, 0.00373012549F, -0.00186299579F, - -0.00279626483F, 0.00456456048F, -0.00609328039F, -0.00706410734F, - 0.0229207277F, -0.00766067347F, -0.0147368023F, 0.00542316819F, - -0.0146864941F, 0.0044798227F, 0.0144949341F, -0.00506732147F, - -0.00997519307F, -0.00972504076F, -0.00223674881F, 0.00566991232F, - 0.00680791F, 0.000190546722F, -0.00271838461F, -0.00224291417F, - -0.00467946706F, 0.00138943829F, 0.00856956188F, 0.00707469648F, - -0.00622462062F, 0.00315701519F, -0.0054877433F, 0.0109238559F, - 0.00454847421F, 0.00683596777F, 0.0181782972F, 0.00227874494F, -0.012378837F, - -0.00902538747F, -0.00595100643F, -0.0059053055F, 0.0154301887F, - -0.0277699418F, 0.0111726746F, -0.00841038488F, -3.70251291E-5F, - 0.00508688251F, 0.00765611464F, -0.00649414863F, 0.0054252F, -0.00403909059F, - -0.0038244389F, 0.00730640162F, 0.0109781493F, -0.00568374665F, - 0.0144046294F, -0.00437566964F, 0.00469397381F, 0.00936943106F, - -0.00235395553F, -0.00484212302F, -0.00573835289F, 0.00524511933F, - -0.0049881679F, -0.021833593F, -0.00964389F, -0.0111281099F, -0.0159970764F, - 0.00133071933F, 0.0031960085F, -0.0227758102F, 0.00733719F, -0.00896098837F, - 0.00432360126F, 0.00854634866F, -0.00119142572F, 0.0131064886F, - 0.0144104902F, -0.0111276619F, 0.00939484406F, 0.000980431F, -0.00540845329F, - -0.00518529024F, -0.00861836F, -0.0101273488F, 0.00736500043F, - 0.00475086877F, 0.0115062483F, -0.00137570268F, 0.00938786659F, - -0.0026440057F, -0.000417604519F, -0.0171720833F, 0.0103706876F, - 0.0245346967F, -0.019944733F, -0.0127940234F, 0.00396372518F, 0.00815402064F, - -0.0104734916F, -0.00344811426F, 0.0101242112F, 0.00827763602F, - -0.010865367F, 0.0073923585F, -0.00569823897F, 0.0129281553F, 0.00467986241F, - -0.000986226F, -0.0202762317F, -0.00334137445F, -0.0106069045F, - -0.00931628421F, 0.00610441621F, -0.000593170931F, -0.000925382483F, - 0.00677140383F, -0.0262150113F, 0.014998192F, -0.00249204226F, - 0.00259426259F, -0.00639638025F, -0.000748636143F, -0.00630461471F, - 0.0130407689F, 0.00571418228F, 0.00185379561F, 0.00178762898F, - 0.00151403423F, 0.00619669119F, -0.00199739565F, -8.19150227E-5F, - -0.000840152847F, -0.00592862256F, 0.0241696779F, -0.0155866956F, - 0.00484715356F, -0.00751856435F, -0.00395105686F, -0.00210977043F, - 0.00718555925F, -0.0070112F, -0.0100616217F, -0.0116462214F, 0.0127951447F, - 0.0071296664F, -0.00763723068F, 0.0113368267F, 0.00125207857F, 0.0142147904F, - 0.00382088358F, -0.00098626758F, 0.00853030384F, -0.00865852833F, - -0.0162747335F, -0.00822222512F, -0.00237892F, -0.000447101367F, - -0.00576554891F, 0.0078307474F, -0.00103021052F, -0.0115544554F, 0.00589625F, - 0.00601706607F, -0.0064454563F, -0.00109535374F, 0.0108905453F, - -0.00509106321F, 0.0108934585F, 0.00643010717F, 0.00230172556F, - 0.0149421543F, 0.0134906294F, -0.0100497818F, -0.000105429237F, - 0.00133876083F, -0.0108453715F, -0.0104801329F, -0.017533889F, - -0.000523897936F, -0.0112682469F, 0.00707194302F, -0.0308012236F, - 0.0111493496F, 0.000958345248F, 0.00764886383F, 0.0046710819F, - -0.00236558099F, -0.0022328468F, -0.0141008403F, -0.00380105339F, - 0.0041347337F, 0.0035027815F, 0.00415723259F, -0.00365700596F, - -0.0105744489F, 0.00873942487F, 0.00613536406F, -0.0118618486F, - -0.0112359449F, 0.00622776058F, 0.00413032854F, -0.00395582849F, - 0.0134384399F, -0.00406344375F, 0.0029939434F, 0.0106040211F, 0.00425320864F, - -0.0193572864F, 0.00781959109F, 0.00285741827F, 0.00104212272F, 0.0041654F, - -0.00624023937F, 0.000522132439F, -0.00450366084F, -0.00471450202F, - -0.00739120087F, 0.00261034304F, 0.00478184596F, 0.00896024797F, - -0.00843677297F, -0.00437969761F, -0.0113962311F, -0.00841011666F, - 0.00690411124F, -0.0134068187F, -0.0113843605F, -0.00488603674F, - -0.00748026324F, -0.00161902222F, -0.00502001168F, 0.00695070298F, - 0.000711603207F, -0.00237589399F, 0.0218174849F, 0.000544715323F, - -0.0067891581F, -0.0090024583F, -0.012430856F, 0.0091513833F, -0.0108062383F, - 0.00654915161F, 0.000995422364F, 0.00430637319F, -0.00962369796F, - -0.00497203F, -0.00465018954F, 8.34046E-5F, -0.00394802075F, -0.00942197628F, - 0.00991070829F, 0.00326970709F, -0.00757129863F, -0.00571115036F, - 0.0101710968F, -0.00202074042F, 0.00482372381F, 0.00191672111F, - 0.0244615339F, -0.00225616712F, 0.0110625327F, 0.00510475831F, - 0.00302684773F, 0.00127657724F, -0.00421586679F, 0.00498807384F, - -0.00060326152F, -0.0201527234F, -0.00277743861F, -0.0133961672F, - -0.0163325835F, 0.0014488796F, 0.0117681166F, 0.00109385222F, -0.0129860258F, - -0.00518493913F, -0.00533061475F, 0.00439965259F, 0.00620754901F, - 0.0193938278F, 0.0057096919F, -0.011113951F, -0.00590036297F, - -0.00910621881F, 0.0050519472F, -0.00552755501F, 0.00138267048F, - -0.00809210353F, 0.00706435461F, 0.000420446333F, -0.000187317637F, - 0.00217954861F, 0.00958426576F, 0.0128386486F, -0.0104635246F, - -0.00278241909F, 0.00891971681F, 0.0097179329F, -0.00830631796F, - 0.00555114634F, 0.00917500816F, 0.00182796F, -0.00465008F, -0.00168538594F, - 0.0112129562F, 0.00926194526F, 0.00695696659F, -0.0143467886F, - 0.00112598704F, 0.00379118952F, -0.00918849465F, 0.0114725977F, - -0.00612991024F, 0.00629571872F, -0.00504389F, 0.00275416207F, - -1.01622863E-5F, 0.00468589691F, 0.00288942852F, -0.0130158765F, - 0.00045274588F, 0.00313971774F, -0.0158060323F, -0.00154801016F, - 0.00505807297F, -0.00891959481F, 0.00100346468F, 0.0102062542F, - 0.0109500354F, -0.0153916525F, 0.00393081177F, -0.00904911105F, 0.004355927F, - -0.00315117161F, 0.00101697806F, 0.00364987762F, 0.0070364573F, - 0.0109942229F, 0.00256183534F, -0.00890872441F, 0.026898101F, -0.00794091F, - -0.0027610017F, -0.0156668816F, -0.022733625F, 0.00599058066F, - -0.0113010891F, 0.00753528625F, 0.00199720054F, 0.00146959617F, - -0.00327487523F, -0.00307613448F, 0.00840888824F, 0.00752514461F, - -0.0209917743F, 0.00356017891F, -0.00439602695F, -0.00789375883F, - 0.0013233521F, 0.000407747313F, -0.00778023619F, 0.0132180611F, - -0.00752178347F, 0.0056408F, 0.00528897764F, 0.0171354115F, -0.0138215506F, - -0.0118990196F, -0.0129706133F, 0.00241658976F, -0.0113809705F, - -0.00453598285F, -0.00327791413F, 0.00428846758F, -0.0131118651F, - -0.0106986966F, 0.00337329623F, 0.00208305498F, -0.0051506157F, - -0.00773026608F, 0.0105247218F, -0.0102035468F, 0.00536739733F, - 0.00102754764F, -0.00125167042F, -0.00558773382F, -0.00122676545F, - -0.00874006655F, -0.00491681416F, -0.0208570808F, 0.0231107883F, - -0.000534782652F, -0.00766847935F, -0.00301623414F, 0.00584553974F, - -0.0193565432F, 0.0155389942F, -0.00303174369F, -0.00455396157F, - -0.00859267358F, -0.0154038267F, -0.00049338484F, 0.00110493961F, - -0.0137798032F, -0.00507227611F, 0.0174688939F, 0.00588086201F, - 0.00527542457F, 0.00859923F, 0.000663463201F, -0.013596884F, 0.0228927676F, - -0.0153335687F, 0.0201165602F, 0.0158066116F, 0.0027628825F, 0.00868425F, - 0.00866291299F, 0.00328376703F, 0.000794657099F, 0.0102741364F, - -0.00281053735F, 0.00805259217F, -0.0123213539F, 0.00546317035F, - 0.0126680899F, -9.93818685E-5F, -9.0786074E-5F, -0.00130904326F, - -0.0113362418F, 0.000545177551F, 0.00914671924F, 0.0115274582F, - -0.00394883892F, -0.0151537191F, -0.000500293332F, 0.00660880283F, - -0.0128238676F, 0.00324735F, 0.00506764418F, 0.00522604957F, 0.00718525797F, - 0.00903546158F, -0.0125471288F, -0.00622292748F, -0.00785371F, 0.0138069903F, - -0.00296712271F, -0.00811213348F, 0.00382817676F, -0.00394165283F, - 0.0137355151F, -0.00634921156F, 0.00836610328F, -0.00541887339F, - 0.00149053894F, -0.00176316313F, 0.00635948079F, -0.00754006626F, - 0.015509258F, -0.000653359573F, 0.0101053715F, -0.0107427975F, - -0.00206130324F, 0.000226569857F, -0.00618416863F, 0.0118985306F, - -0.0170592237F, 0.000763112737F, -0.0179400425F, 0.0076081939F, - -0.0156481061F, -0.00381788495F, 0.016620012F, -0.0143868327F, - -0.00568960467F, -0.00251211342F, 0.00921988301F, -0.00347050722F, - 0.00922045391F, 0.00546421483F, -0.00522661442F, 0.00281128683F, - 0.0263431724F, 0.00977600645F, 0.00737937959F, 0.0148225538F, 0.0110867806F, - -0.0190654397F, -0.00689088693F, 0.0109098041F, -0.000367201719F, - 0.00409415644F, 0.00824047904F, -0.0207188558F, -0.00708967494F, - -0.000535019848F, -0.00450515933F, -0.0196007695F, -0.0143713234F, - 0.0103022251F, 0.0136306556F, -0.00581731275F, -0.00483395F, 0.00168419594F, - 0.0169161204F, -0.00255756825F, -0.00283666095F, 0.00243376032F, - -0.017139338F, -0.0112771941F, -0.0161346458F, -0.00523078488F, - 0.00360356644F, 0.0174959693F, 0.00224504154F, 0.00873746909F, - 0.00889241695F, 0.00901557F, 0.00100530847F, 0.0135415541F, -0.0178444106F, - 0.0131469751F, -0.00212086947F, 0.00117940083F, -0.015785601F, 0.0108792409F, - 0.00342254434F, 0.00568367587F, -0.0162495598F, -0.000191993327F, - -0.00597449299F, -0.00899198186F, -0.0188244544F, 0.00905613881F, - -0.0046553174F, -0.0123265767F, -0.00371007924F, -0.00385739F, - 0.00378319272F, 0.000243871647F, 0.00916235056F, 0.0033308249F, - -0.00191452587F, 0.0151203535F, 0.0126674185F, 0.00983457826F, 0.0100433221F, - 0.00256377435F, -0.00427057454F, -0.000877141603F, 0.0125469044F, - -0.00101980299F, 0.00320106046F, 0.00516606309F, 0.0182748716F, - 0.00638318527F, 0.00807339232F, -0.00369233522F, -0.00455893576F, - 0.00304623833F, -0.0101838605F, -0.00277790241F, -0.0098066479F, - -0.00198356644F, 0.00649772538F, 0.00747784134F, 0.0015624084F, - -0.00561816199F, 0.000288510375F, 0.0176582541F, 0.00408203667F, - 0.00427465234F, 0.0105518568F, 0.000732222572F, 0.00863587297F, - -0.0241366886F, -0.00993128214F, -0.0121619301F, -0.00270300242F, - -0.00143592362F, -0.00104950415F, 0.0105044171F, 0.00531939743F, - -0.0106477933F, 0.00287071336F, 0.00190136407F, -0.00591482408F, - -0.00100233452F, -0.0105298497F, 0.00735443691F, 0.000290677446F, - 0.0136549342F, -0.00237637432F, -0.00914751645F, -0.00243983581F, - 0.00137131196F, 0.0196387041F, 0.00606618822F, -0.0078037F, 0.0267630592F, - -0.00249964721F, -0.00585629F, -0.00168059533F, 0.0050266278F, - 0.00491973059F, -0.00739296339F, 0.00633987878F, 0.00668966072F, - 0.00368683855F, -0.00789577F, 0.00200967956F, -0.000575430575F, - -0.00785992481F, 0.0111758886F, 0.0144126005F, -0.0047146366F, 0.0154545372F, - 0.0174075048F, -0.0047254716F, 0.00415156363F, -0.0117893536F, - -0.00194012851F, 0.0106441258F, -0.00685399678F, -0.00660266913F, - 0.00808929F, 0.0204423219F, -0.0223374832F, 0.0122550568F, 0.00454051048F, - -0.0189454332F, 0.00977488235F, 0.00178415293F, -0.00773087936F, - -0.00591044454F, -0.00328885438F, 0.00345060462F, -0.00162007532F, - -0.00279754074F, -0.00274686888F, -0.010318079F, 0.00494077F, 0.0145826321F, - 0.0038870906F, -0.0149144595F, 0.0102422154F, -0.00159879948F, - -0.00780036813F, 0.0116701368F, -0.00453815842F, 0.0155317597F, - 0.00423787907F, 0.00954616722F, -0.00255126413F, 0.00884274F, -0.0030587262F, - -0.00541483518F, -0.00311415805F, -0.0110745756F, 0.00199767575F, - 0.000985215185F, -0.00224061566F, -0.0245555323F, 0.00567048369F, - 0.00390582392F, -0.0156529434F, -0.00563247222F, -0.00718672667F, - -0.0137291327F, 0.0036518781F, 0.010550878F, 0.00865463912F, 0.0244415905F, - -0.015747752F, 0.0156087186F, -0.00560316583F, -0.0113179656F, - -0.0186424702F, -0.0192514881F, 0.031904269F, 0.00685140211F, - -0.00502352882F, 0.00291098212F, -0.00172848615F, 0.00508536818F, - -0.010983536F, 0.00687943352F, 0.012140641F, -0.00440084236F, 0.0115913181F, - 0.0157713387F, -0.0069704121F, 0.0131553235F, -0.00891519338F, - 0.00713753607F, -0.00124303205F, 0.00414125668F, -0.0121366838F, 6.47754E-5F, - -0.0152350375F, -0.0128780799F, -0.00860913377F, 0.0141343288F, - 0.0162510667F, -0.0112367496F, -0.00843258388F, -0.00137489149F, - -0.0108732805F, -0.00963954069F, -0.0184987523F, -0.0139130522F, - 0.0118724741F, -0.0031251F, -0.00513899326F, -0.00304275798F, 0.00242224941F, - -0.013133537F, 0.00929457F, 0.00893738121F, -0.0130382143F, 0.0156693365F, - -0.00892558F, 0.00549826259F, 0.0245014727F, -0.00725194067F, - -0.00850516558F, -0.00412769848F, 0.0045782323F, 0.0201077778F, - 0.00761621725F, 0.00624311296F, -0.00475634169F, 0.0113373008F, 0.0146487F, - -0.0108975843F, -0.00842342898F, 0.00330455741F, 0.0020888194F, - -0.0051623988F, -0.0106416568F, -0.00659849681F, 0.0101864468F, - -0.00130236417F, 0.00481775543F, -0.00429985346F, -0.0256171208F, - -0.00438527809F, 0.0137761338F, -2.55079685E-5F, 0.00551319262F, - -0.02315538F, -0.0141493343F, 0.000886672467F, 0.00837278366F, 0.0104018496F, - 0.00650851801F, 0.00528615341F, 0.000487202749F, 0.0061846762F, - 0.00610535871F, -0.00816727616F, -0.00271999068F, 0.00852077734F, - 1.39881859E-5F, -0.0170184784F, -0.00643846579F, 0.0107060252F, - 0.0184879638F, -0.00708234543F, -0.0106598902F, -0.000920429127F, - 0.0100721512F, -0.0075561963F, 0.00547824847F, -0.00831920467F, - 0.00579725485F, -0.0132572092F, 0.000172904911F, 0.00206125341F, - -0.00441872515F, -0.00841366779F, 0.00854082592F, 0.00559852878F, - 0.00959698949F, 0.013986134F, -0.00306370109F, -0.01624199F, 0.014411686F, - 0.00723466929F, -0.00304431468F, 0.0131800594F, -0.0042809411F, - -0.00156347419F, -0.0103136646F, -0.0135325622F, -0.0144176884F, - -0.0214983225F, -0.00705043087F, 0.00524966279F, 0.0115769831F, - 0.0163347777F, -0.0135930693F, 0.00793047342F, 0.000736470392F, 0.011892423F, - -0.00890330505F, 0.0176299922F, 0.00262938626F, 0.0165270455F, 0.000734411F, - -0.00223605544F, 0.0135315564F, -0.0100409016F, 0.0248960853F, - -0.000253885868F, 0.00165282015F, 0.0166688915F, 0.00289974641F, - 0.00243694102F, 0.00939746946F, 0.0107865855F, -0.00221494935F, - -0.00572237931F, -0.00143183488F, 0.00173347257F, -0.0132021988F, - -0.0142939957F, -0.00609024335F, -0.00267727906F, 0.00768400216F, - -0.00235659163F, 0.0149628995F, 0.0124631096F, 0.0155443884F, -0.0110195829F, - 0.00613789028F, -0.0120941121F, 0.00613737386F, 0.00782772806F, - -0.00366286188F, 0.0120395869F, -0.00438616471F, -0.00739992037F, - -0.0210835673F, -0.00210266048F, 0.00365286297F, 0.00739601813F, - -0.0123350183F, 0.0128010372F, -0.000964952633F, 0.00483656069F, - 0.00119759468F, -0.009463205F, -0.00140641036F, -0.00199010293F, - 0.000635386F, -0.00307350187F, 0.00693047745F, -0.00625698827F, - -0.0112081263F, 0.0105857207F, 0.00217928947F, -0.00831092149F, - 0.00445617829F, 0.0111763775F, -0.00419262098F, 0.0163966771F, - -0.0218341108F, 0.00712954672F, 0.015913384F, -0.00266181026F, - -0.0112210298F, -0.00354917743F, -0.0063618212F, -0.00167442276F, - -0.0128609957F, -0.0117462557F, 0.00726604275F, 0.00489541609F, - -0.00442024693F, 0.000479829556F, -0.00360836973F, 0.00641096756F, - 0.00288948067F, -0.00070604973F, 0.00804823358F, -0.00304622529F, - -0.029913811F, -0.00512065738F, -0.00141499832F, -0.0163755864F, - 0.00324702961F, -0.00745177548F, -0.00947893F, -0.00173545117F, - 0.00572787039F, -0.00292015471F, 0.00282890606F, 0.0121243224F, - 0.0125576714F, -0.00752631761F, -0.013384657F, 0.0116735213F, - -0.00401305687F, 0.00524595845F, -0.00767999282F, -0.005569336F, - 0.00253013591F, 0.00308695599F, -0.001986332F, -0.00279469229F, - 0.00430998765F, 0.00485978415F, 0.00339124515F, -0.00277919346F, - 0.013778287F, -0.00232979096F, -0.00513091F, 0.00760389911F, -0.00159675023F, - 0.00527774123F, -0.00996260438F, -0.000600137399F, 0.011576728F, - -0.0100468341F, 0.000945368141F, -0.000496586668F, 0.00493888045F, - -0.0118183037F, 0.0157178361F, -0.011154267F, 0.00242814212F, - -0.00182404777F, 0.0216486156F, -0.014429437F, 0.012955212F, -0.00354311429F, - -8.48120108E-5F, 0.0117130904F, -0.00549089443F, 0.0112493476F, - 0.000718080904F, 0.00747349486F, -0.0145763243F, -0.0139890639F, - -0.0145976702F, 0.00419671228F, -0.0165543742F, -0.0128378551F, - 0.000158814408F, 0.0103233559F, -0.00382128777F, 0.0194810778F, - 0.00694957329F, -0.00940460805F, 0.000458769617F, 0.00375795434F, - 0.0164145455F, 0.00891986117F, -0.00263735931F, -0.0189212766F, - -0.000311216572F, 0.0024716428F, 0.011916169F, 0.0132263172F, 0.00102446624F, - -0.00339643797F, 0.000996770803F, 0.0181808099F, 0.0272904802F, - 0.00917069707F, -0.00436673127F, -0.00718517927F, -0.00152689591F, - 0.000942549319F, 0.00207653828F, 0.0203836374F, -0.0134958448F, - -0.0103837969F, -0.00624627294F, -0.0150164757F, -0.00982788205F, - -0.00102241116F, -0.00222680485F, -0.0149884084F, -0.00887347665F, - 0.00410212623F, 0.000985081308F, -0.0153710842F, -0.000564168207F, - 0.0159297157F, -0.00828640163F, 0.0018458002F, 0.00547810458F, - 0.00215631956F, -0.00143605901F, -0.00741308462F, 0.0134064369F, - 0.00339478557F, 0.00661561964F, 0.00321124727F, 0.00321040908F, - 0.00998371933F, 0.00626687054F, 0.0193835367F, -0.00390691683F, - 0.0134480875F, 0.00896315929F, 0.00491198758F, -0.00618550135F, - 0.00716167223F, -0.000719059084F, -0.000810757745F, -0.00865437184F, - 0.00168795756F, -0.0169075709F, 0.0141345989F, 0.00725446967F, -0.014138489F, - -0.0183275435F, -0.0245746132F, -0.012491148F, 0.000162805693F, - 0.0244492199F, 0.0042701778F, -0.00106961408F, -0.00503802579F, - -0.0063828337F, -0.00789166F, 0.00799271837F, 0.000452775916F, 0.0125048282F, - 0.0057764966F, -0.0182392076F, -0.0168963186F, 0.0115025F, 0.00831022579F, - -0.00657114573F, -0.00122830551F, 0.0160251912F, 0.0268975887F, - -0.00794791244F, -0.00726663927F, 0.00766135147F, -0.0120225502F, - 0.00111216167F, -0.00364097161F, -0.0135068651F, 0.0235557538F, - 0.00328386645F, 0.00799258891F, -0.0128087727F, -0.0111655286F, - 0.00130486023F, -0.0110305138F, 0.0140056843F, 0.00939721242F, 0.00626312F, - 0.00192217471F, 0.00399088208F, -0.00145072246F, -0.0243077818F, - 0.0175974425F, 0.0105668306F, 0.00165654102F, -5.12149127E-5F, - -0.00536748068F, 0.00564548373F, 0.0247344319F, -0.00336431665F, - 0.00606006617F, 0.000727235572F, 0.0152787566F, 0.00705342F, 0.011448835F, - -0.000828173F, -0.0034770302F, 0.00108966383F, -0.00995164551F, - -0.00140714878F, -0.0133358343F, 0.0107201487F, -0.0196635F, -0.0224503092F, - 0.0202591848F, -0.00244647684F, -0.0116789853F, -0.00765930535F, - -0.00535470387F, 0.00627005287F, 0.00206209347F, -0.016943777F, - -0.00246106717F, 0.00253029238F, -0.0038730267F, -0.00373285264F, - 0.0143862888F, 0.000175104913F, 0.00156168174F, 0.00560918776F, - 0.00112195383F, 0.00163236191F, -0.0162966195F, -0.0112252366F, - 0.00472875871F, 0.00474161282F, 0.0022565925F, -0.00566982059F, - 0.00668928912F, -0.0112928646F, 0.0106445309F, 0.00381332915F, - 0.00712342747F, -0.00128040195F, -0.0144608868F, 0.00365296169F, - -0.0237109195F, 0.0147979651F, -0.00606178818F, 0.000781808572F, - 0.0162261203F, -0.000168074315F, -0.0119495196F, 0.00577734224F, - -0.0182437543F, 0.00109896343F, 0.00669862377F, 0.00566718215F, - -0.0210822318F, -0.000300914922F, -0.0144046471F, -0.00238380628F, - -0.000674585579F, 0.00782610197F, 0.00573310256F, 0.00693181343F, - 0.0111994985F, 0.00557611929F, -0.00750830164F, -0.0116106439F, - -0.00638177618F, 0.0108556822F, 0.00458804751F, 0.00234544161F, - 0.00235285354F, -0.0139460051F, -0.004713987F, -0.0155852074F, - 0.000730242289F, 0.00957028847F, -0.00845791F, -0.027177738F, 0.00304885535F, - -0.00636867154F, -0.00407325709F, 0.00529258233F, 0.00124180852F, - 0.0164792407F, 0.0022282342F, 0.0182495099F, -0.00589879462F, -0.0132789919F, - 0.00222953781F, -0.0104290396F, 0.017307844F, -0.0114800166F, -0.0130506214F, - -0.00288539124F, 0.00468917377F, -0.00261396F, 0.00560930511F, 0.0223693643F, - 0.00515640574F, -0.00619109813F, -0.00165692263F, 0.0170640498F, - 0.00483271759F, -0.00248178258F, 0.00567833F, -0.00148400222F, -0.00469502F, - -0.00201891246F, 0.00146424631F, 0.0131074516F, -0.0181382168F, - 0.00671468768F, -0.0111512141F, 0.00355406851F, 0.0126544451F, 0.0110842614F, - -0.0120936325F, -0.00463367905F, -0.00568227051F, 0.000925423461F, - -0.023455929F, -0.0249814838F, -0.000382133905F, 0.00890882779F, - -0.00188697933F, 0.00358647713F, -0.00240170211F, 0.00497094449F, - 0.024489155F, -0.00561755477F, -0.0057997969F, -0.0204708967F, - 0.00407756958F, 0.00328573608F, -0.00851147529F, -0.000966229592F, - 0.00382625987F, -0.00379187264F, 0.01068174F, 0.0142909642F, 0.000546754163F, - -0.0126190847F, -0.0048964587F, 9.6771626E-5F, -0.0160645898F, - 0.00440248335F, -0.00508587435F, -0.0101459315F, -0.00346454675F, - -0.00515009044F, 0.00313541922F, -0.00491812499F, -0.0254862104F, - -0.000130416665F, -0.00742585352F, 0.00540121691F, 0.000790711551F, - 0.00774406362F, -0.00452963263F, 0.00351620209F, 0.00515390234F, - -0.00856723823F, 0.000208611527F, -0.00637638103F, -0.0071596182F, - -0.0121105826F, -0.00421641162F, 0.00376442983F, 0.0131356614F, - 0.0143385911F, -0.000103749444F, -0.00232398883F, 0.00940189604F, - -0.000524281815F, 0.0116262157F, 0.0117512923F, -0.00800351426F, - -0.00489332573F, 0.00030594F, 0.00348889269F, -0.00755016506F, 0.0133255124F, - -0.00236009341F, -0.0181010645F, 0.0077850339F, -0.00114738941F, - -0.00271744281F, 0.00960006658F, -0.0131883249F, 0.00476636086F, - 0.00765957171F, 0.00776039623F, -0.00600422127F, 0.00847484823F, - -0.00686491467F, 0.00914134F, 0.00848363061F, -0.0017821315F, 0.0197819415F, - 0.0032298353F, -0.00902855303F, -0.00346198329F, -0.0140755465F, - 0.00130307744F, 0.000538271386F, -0.00025950413F, -0.00807229895F, - -0.00474021677F, -0.0148948198F, -0.00345678604F, -0.00400705589F, - 0.00899164658F, 0.00572913606F, -0.00666013081F, 0.00532867899F, - -0.00948618911F, -0.0063973316F, -0.00670046592F, -0.00414830027F, - 0.0176749155F, -0.0039649196F, 0.0115076369F, 0.00326442881F, 0.00840015896F, - -0.00332999602F, -0.00374889F, 0.00783554744F, -0.00147340028F, - 0.00737815769F, 0.0110556073F, 0.00719599891F, -0.0139994845F, - -0.00941081624F, 0.0117101613F, 0.0230141766F, -0.0078638792F, - -0.00307111535F, 0.0162986107F, -0.00107983407F, -0.0112733673F, - -0.00737476768F, -0.00508630183F, -0.0022554337F, -0.00465615932F, - 0.00302487472F, -0.00465131877F, -0.00661134347F, -0.00746720517F, - -0.00703522F, 0.0120881042F, -0.00522080949F, -0.00235638907F, - 0.00959734526F, 0.00495207F, 0.00811203476F, 0.0103955884F, 0.00142710551F, - 0.00657374831F, -0.00173332554F, 0.00144430343F, 0.00130031293F, - -0.00415687123F, 0.00315270573F, -0.0158577971F, 0.0124662882F, - -0.0153658083F, -0.00976542197F, 0.0165318828F, -3.73418879E-5F, 0.0041646F, - -0.0184707195F, -0.00156514475F, -0.0116233844F, 0.0186955128F, - 0.0084689064F, 0.00428218301F, 0.0157846045F, 0.00283904583F, -0.0158542413F, - -0.00330533669F, -6.8751935E-5F, 0.00961345062F, 0.0123912115F, - 0.00351480744F, -0.00696572056F, -0.0262447782F, -0.000419564167F, - 0.011899746F, 0.0092371041F, -0.00567082688F, -0.00404176163F, - 0.00174422294F, -0.0077951448F, 0.0100718224F, 0.00339756045F, 0.0238725804F, - -0.00296160788F, -0.0116364341F, 0.00334473979F, 0.00905232131F, - -0.00894725882F, -0.00256314385F, -0.00290328846F, -0.00937480293F, - 0.0257579982F, -0.0101886801F, 0.00106052216F, -0.0070984792F, - -0.000986338127F, -0.0154390074F, -0.00511698378F, -0.0100938957F, - 0.0115366559F, -0.0120310346F, -0.00891603436F, -0.000121417914F, - 0.00320983329F, 0.000357503304F, 0.00918162148F, 0.00682543265F, - -0.00450703921F, 0.0136043234F, 0.00285326038F, -0.00302635436F, - -0.00552446675F, -0.0127807315F, 0.00906990748F, 0.0224710461F, - -0.00103537797F, 0.00467686F, -0.0188580453F, 0.000711890752F, 0.0106741553F, - 0.00665636361F, 0.00517506292F, -0.00406890921F, 0.0147088561F, 0.00978213F, - 0.0116334576F, -0.00128668023F, -0.00211436185F, -0.00902887434F, - 0.000400065299F, 0.0069056889F, 0.010190662F, -0.00620112801F, - 0.00828477833F, -0.0149335917F, 0.00515705859F, -0.0109523879F, - -0.000120974037F, 0.0147192189F, 0.00121587189F, 0.00358747714F, - -0.00661286758F, -0.0209118407F, -0.0118661681F, -0.00244632619F, - -0.00343282567F, 0.00889538694F, 0.00117802864F, 0.0036266977F, - -0.0105614914F, 0.00663053617F, -0.0180781633F, 0.00308645773F, - 0.00704290532F, -0.00391134573F, 0.00591093395F, -0.000135048875F, - -0.000384990213F, 0.0067344741F, -0.00127520494F, -0.00148364832F, - 0.00293301395F, -0.0225417838F, 0.00280112447F, 0.019903684F, - 0.000633251271F, 0.00295734312F, -0.00385594112F, 0.00307785161F, - -0.0011438675F, -0.0148461647F, 0.000414487091F, -0.0049559148F, - 0.00232073269F, -0.0100384755F, -0.00200171419F, 0.0195925627F, - -0.0114228018F, 0.00441154325F, -0.00333504286F, -0.00591241755F, - 0.0281198584F, -0.0125925681F, 0.00271475525F, 0.00683532283F, - -0.00337021193F, 0.00435306132F, 0.0198839251F, 0.00970817F, 0.0266637F, - -0.00394822098F, -0.00570657151F, 0.00475080824F, -0.00437722029F, - -0.00610152818F, -0.00493043195F, 0.00639769714F, -0.0118972966F, - 0.00533180637F, -0.000611464609F, 0.00916669611F, -0.0113771195F, - -0.0117571363F, 0.0141718229F, -0.0148364333F, -0.0028423036F, - 0.00306749367F, -0.000657911296F, -0.0289428644F, 0.00122393307F, - -0.00537276454F, -0.00315562543F, 0.00314858952F, 0.000251538237F, - -0.0106649352F, -0.00887276791F, 0.000317861559F, 0.00495781703F, - 0.00608334318F, -0.00639010733F, -0.00515747909F, -0.00921945926F, - -0.00258299313F, -0.0121282628F, 0.000178589253F, -0.00153745711F, - 0.00467732409F, -0.00451437291F, -0.00279148319F, -0.0165072493F, - -0.00338452F, 0.00334835192F, 0.00578541728F, 0.00188719947F, - -0.00288650743F, 0.00157800689F, 0.0195060633F, 0.00231026672F, - -0.000970829919F, -0.00746480329F, 0.00432273466F, -0.00381981558F, - -0.00158407423F, 0.00653421739F, -0.00423073862F, -0.00273199147F, - -0.0153691787F, -0.0102694416F, 0.00302445376F, 0.00147506385F, - -0.0100805601F, 0.00581560936F, 0.00487211626F, -0.0024997897F, - 0.0152755585F, 0.00081342092F, 0.00137960224F, 0.00192347215F, - -0.000542990572F, -0.00915569905F, 0.000160812677F, 0.0132896528F, - -0.0119501101F, 0.00685660494F, 0.0023393468F, 0.0124383876F, - -0.00928483065F, 0.00874045584F, -0.00986462459F, 0.0151614314F, - 0.00810628664F, -0.000882161898F, -0.00970279053F, -0.00245811231F, - 0.00452467194F, 0.0116749471F, 0.00595305441F, -0.00922063179F, -0.00957685F, - -0.000500864873F, -0.0104243727F, 0.00268263347F, 0.00341631169F, - 0.00686640665F, 0.00618662871F, 0.00852533337F, 0.00842424855F, - 0.0107943295F, -0.00268490915F, -0.01715222F, 0.0058637117F, 0.00364510762F, - -0.00998150837F, 0.00828623772F, 0.00347994617F, 0.0145379845F, - -0.00152372185F, 0.00521185668F, -0.0152770504F, 0.000429254724F, - 0.0214112569F, -0.0120312721F, 0.0158692021F, 0.00399168441F, -0.0164630376F, - 0.0178527571F, 0.00441814912F, 0.000160606127F, -0.000643533655F, - -0.0255021565F, 0.00355216302F, 0.00953462534F, -0.00387545256F, - -0.0026140546F, 0.000579566753F, -0.0177632831F, 0.000636209326F, - -0.00953692943F, 0.0090165576F, 0.0130604981F, -0.0206305385F, - -0.0151761463F, -0.00216743769F, -0.0215207133F, -0.0188085921F, - -0.00351908058F, 0.0044541033F, 0.00577220693F, 0.0110780168F, -0.011500041F, - 0.0015523117F, 0.00705672288F, -0.00980365369F, 0.00510211615F, - -0.00168181118F, -0.000766239653F, 0.00335645606F, 0.00868163444F, - -0.0102868835F, -0.0276397616F, -0.00241384609F, -0.0130017688F, - -0.0175508931F, -0.00616303831F, 0.00454253936F, 0.00332522206F, - -0.000321248604F, -0.00469973264F, 0.00514778355F, 0.00125496369F, - 0.00521366391F, -0.00293690781F, -0.00435216911F, -0.00666972529F, - -0.00224598334F, -0.00258689723F, 0.0119988974F, -0.0040527028F, - -0.00115605432F, -0.00963533111F, -0.00338707073F, -0.0127818473F, - -0.014043794F, 0.00561034214F, 0.00990044884F, 0.0191922057F, - -0.00531198084F, 0.00984944217F, -0.00543128932F, -0.00715812529F, - -0.0115772123F, -0.00376842543F, -0.0111049507F, 0.00457039569F, - 0.00328528532F, 0.0068154335F, -0.00904809404F, 0.00162727025F, - -0.00457422622F, -0.0168818235F, 0.00417808583F, 0.0100084534F, - 0.00716800895F, 0.0250197556F, 0.00907529239F, 0.00965044741F, - -0.000994559377F, -0.014664636F, 0.014063783F, -0.00370007218F, - 0.0114281895F, -0.026036026F, 0.00677923905F, 0.0201412942F, -0.00332107604F, - 0.0266361013F, -0.0063877129F, 0.012433718F, 0.0129313944F, -0.00415557297F, - -0.00593754975F, 0.00560405431F, 0.0132597489F, 0.0158806778F, - -0.00457812753F, 0.00492398068F, 0.0152561013F, 0.0068721287F, -0.00973568F, - 0.0138275819F, -0.00148083293F, 0.00989361107F, -0.0134592447F, - -0.00162794162F, 0.00119336648F, -0.00339503307F, -0.0112428013F, - -0.00404749671F, -0.0105815539F, -0.0108703952F, 0.00890064519F, - 0.00596305355F, -0.00864655059F, 0.0151500478F, 0.00273076235F, - 0.00492095901F, 0.000262882328F, -0.00694643753F, 0.00902296F, - -0.00212911423F, 0.010786281F, -0.0149058308F, -0.0101262769F, - 0.00881300215F, -0.00137202651F, -0.00358270272F, -0.012042868F, - -0.00670181634F, -0.0210006721F, -0.00767023116F, -0.0109892627F, - -0.0151363639F, 0.000756046968F, 0.00736205839F, -0.00374184665F, - -0.00720200734F, -0.00723438477F, -0.0116556091F, -0.0132733611F, - 0.00753068924F, 0.00678172242F, -0.00657194201F, -0.00894556567F, - -0.00576448534F, -0.00124403217F, -0.00256687449F, -0.00520972908F, - 0.0217990912F, 3.62120772E-5F, -0.0175594F, 0.00933854841F, -0.000965158222F, - 0.00279465597F, 0.00584664196F, 0.0043321019F, 0.00551028131F, - 0.00432723761F, -0.00177709921F, -0.00939497072F, -0.0131743485F, - -0.0138200065F, 0.000238877925F, -0.00549035706F, 0.0038237772F, - -0.00944164395F, 0.0119897202F, -0.00847102609F, -0.0112218028F, - -0.00923987F, -0.00514239445F, -0.00318787F, 0.0131260874F, -0.00624222774F, - 0.00276578404F, -0.0140370028F, 0.012934559F, -0.00236761919F, 0.0135334563F, - 0.00413826248F, 0.0109257679F, 0.0142601226F, 0.000740920717F, - -0.00124206406F, -0.0130072283F, 0.0126241343F, -0.0149170067F, - -0.000776837405F, -0.00639400445F, -0.00242053461F, -0.00592319807F, - -0.00940367393F, 0.000970605761F, -0.0162111707F, -0.0133657688F, - -0.00596989598F, 0.0021475628F, -0.00210327492F, -0.00531823142F, - 0.00616622F, -0.00760036753F, -0.00724263536F, -0.0105980271F, - 0.00488536106F, 0.00270710932F, -0.00707237795F, 0.00612854725F, - 0.00731844315F, -0.00210564653F, -0.000123002203F, 0.00404454162F, - 0.00635669F, 0.00243751472F, -0.00809019897F, -0.00143746287F, 0.0073343846F, - -0.00358222448F, -0.00245969696F, 0.00985156093F, -0.00899213925F, - 0.00358674652F, 0.0212500226F, 0.0047055413F, 0.00490283594F, -0.0194091499F, - -0.000271138735F, 0.00559514109F, -0.00268302066F, 0.00250689941F, - 0.00796387345F, -0.00605606753F, -0.0131708914F, -0.00795809925F, - -0.0155088194F, -0.0106928591F, -0.00205953419F, -0.0023324152F, - -0.00652526971F, -0.0030051982F, -0.0140807815F, -0.00103399379F, - 0.0100623174F, -0.0105763432F, 0.00694915419F, -0.0108317388F, - -0.0168715343F, 0.0133713204F, 0.0103398887F, 0.00377661735F, -0.0012212093F, - -0.00847311504F, -0.00307072932F, 0.00523210689F, 0.0102591673F, - 0.00656942511F, 0.00963631272F, -0.00314170728F, -0.00348060322F, - -0.00754384464F, -0.00443016319F, -0.0061697904F, 0.00428114645F, - -0.00365959294F, 0.0150299957F, -0.00274453033F, -0.00291744061F, - -0.000549751625F, 0.00390491658F, 0.0013114115F, -0.0074511664F, - -0.00372634595F, 0.000435205497F, -0.00252435589F, -0.00398856075F, - 0.000607451F, -0.0066830474F, 0.0135620898F, 0.0132345269F, 0.000643341569F, - 0.00258404412F, 0.000585436181F, 0.0118952915F, 0.00114630081F, - 0.00818498805F, 0.0105637591F, -0.0013725484F, -0.00496739894F, - 0.0292337611F, 0.0028949962F, -0.0126497205F, 0.00221832516F, 0.00117109134F, - -0.0121711241F, -0.000453550892F, -0.00106342963F, -0.00522570219F, - 0.00332676503F, 0.0033556663F, -0.00722528715F, 0.0167448353F, - 0.00973438378F, -0.00208234764F, -0.0111034755F, 0.00547133852F, - -0.00279626F, -0.00435322849F, 0.0071036513F, 0.0114174224F, -0.00929781143F, - 0.00457719853F, -0.00115279318F, -0.0121626863F, 0.0183874127F, - 0.000138397372F, -0.00377541431F, 0.0131076863F, 0.0148300929F, - 0.0169141218F, 0.0053697126F, 0.00892329868F, 0.00825213268F, - -0.00181680964F, 0.00509658782F, -0.00292695733F, -0.00333857909F, - -0.0161348023F, 0.00569736771F, 0.0041722944F, -0.0080173593F, - -0.00640964508F, 0.00299262977F, 0.00741409F, -0.00648815557F, - -0.0111413701F, 0.00267153443F, -0.0090073524F, -0.00254706829F, - 0.0302838609F, -0.014565208F, 0.0246322062F, -0.00344822626F, 0.00377284829F, - 0.0171465408F, -0.0108118327F, 0.00542239612F, 0.00592371495F, - -0.00780776469F, 0.0135275181F, -0.0273525417F, 0.00428187614F, - 0.0145771587F, 0.0115769496F, -0.00881167F, -0.000128636093F, 0.00255926768F, - 0.00198750803F, 0.012461869F, -0.0132653462F, 0.0102525298F, -0.00160977978F, - -0.00124664127F, 0.00909190811F, -0.000699874188F, -0.0014256346F, - -0.0151585294F, 0.00794390216F, -0.0103323301F, -0.00543303182F, - -0.0113323182F, 0.00585925765F, -0.00538617885F, 0.00344071561F, - 1.15264202E-5F, -0.0026214486F, 0.011730616F, 0.0236094613F, -0.00488567073F, - 0.00868581235F, 0.0051389006F, -0.00673434418F, 0.00127408095F, - -0.0246750247F, -0.0143179325F, 0.00812323578F, -0.00472069671F, - -0.00209804089F, 0.00771872699F, 0.000852991361F, 0.0143483253F, - -0.00199372252F, -0.00056807982F, 0.0141554819F, -0.00359754544F, - -0.0163876824F, 0.0188888498F, 0.00522824703F, 0.00375411287F, 0.023494618F, - -0.00146871572F, 0.00158166443F, 0.0180072263F, 0.0170009676F, - 0.00475507043F, 0.020725755F, 0.00190972898F, -0.00479325792F, - -0.0137700513F, -0.00450306F, -0.00655573932F, 0.0192623138F, 0.0078706434F, - -0.0130325472F, -0.0245005861F, -0.0016375382F, -0.0116022294F, - 0.00897732843F, 0.000866130926F, -0.00211162819F, -0.00104253786F, - -0.00884358492F, 0.015132254F, 0.0139531652F, -0.00383990374F, 0.0153359678F, - -0.0147774415F, -0.00883347914F, 0.00378628261F, 0.00124336639F, - 0.00186451222F, -0.010987333F, -0.00200375845F, -0.00843966F, - -0.00755054969F, 0.00821558F, -0.00925004669F, 0.000957490876F, - -0.0173386503F, -0.0114988079F, -0.00276220031F, 0.010972227F, - -0.00898249913F, 0.00333732925F, 0.00873157103F, -0.00188466068F, - 0.00956221856F, -0.00797326863F, -0.00684203161F, 0.00284206378F, - 0.00651782611F, -0.0062472932F, -0.00545020681F, -0.00602304516F, - 0.0109281857F, 0.015545981F, 0.0135147721F, -0.0195183679F, 0.00215047877F, - 0.00282932632F, -0.00596768735F, -0.00368406787F, 0.00144042796F, - -0.00395096047F, -0.00561394356F, 0.00559890317F, 0.00720232166F, - -0.00530546F, -0.00703403214F, -0.00441795588F, 0.00349664199F, - 0.000249489764F, -0.0129340161F, 0.0108659156F, -0.0147741232F, - 0.00316821435F, -0.00413206592F, -0.00181384396F, 0.0128875067F, - 0.0169727374F, -0.00521148462F, 0.00390147627F, 0.0200241655F, - 0.00126878044F, 0.00263735768F, 0.00342390384F, 0.0120205199F, 0.0083725F, - -0.00829004869F, 0.00817138888F, 0.01115919F, -0.00240964792F, - -0.00123755843F, 0.00250427844F, -1.32179721E-5F, -0.00596146286F, - -0.00554080587F, -0.00232470152F, -0.00827783067F, -0.012509956F, - -0.00783489645F, -0.0105539486F, -0.0167066064F, 0.00685257185F, - 0.00202201447F, 0.0173761901F, 0.0207264721F, 0.00521888305F, -0.0122711156F, - -0.0065811649F, -0.0203924011F, 0.00523962127F, 0.00279645761F, - 0.00976982806F, -0.00847206451F, -0.0270119142F, 0.00392953213F, - -0.000777788053F, -0.00264613959F, 0.00708949054F, -0.00925118197F, - -0.0180025399F, 0.00506157102F, 0.00644878345F, 0.00541949272F, - 0.00174760795F, 0.00399672193F, 0.0045924657F, 0.000805216841F, - 0.00372201414F, -0.0130133517F, -0.024686452F, 0.00434396369F, - 0.00636431435F, -0.00577942049F, 0.00707484735F, -0.00565484073F, - 0.00072334049F, -0.00872018F, 0.00156077964F, -0.0207194407F, - -0.00744618056F, 0.0140974326F, 0.0154357525F, 0.0200561024F, 0.0130575178F, - -0.00943125132F, 0.00676853023F, -0.00646584341F, -0.0117562246F, - -0.00365861435F, -0.000396497373F, -0.0309284199F, -0.0179375149F, - 0.0103914151F, 0.00853522308F, -0.0125603862F, -0.00652765622F, - -0.00118321751F, -0.00765880523F, -0.00325004384F, 0.00441292906F, - -0.0007127141F, -0.0104051558F, -0.00549963F, -0.00349606061F, - 0.00215450185F, 0.00625193771F, -0.00333907828F, -0.0184799936F, - -0.00314392522F, 0.00013134646F, 0.000454771362F, 0.00348018087F, - -0.000848587777F, -0.0192012899F, -0.00344667793F, 0.00308926799F, - 0.0259276312F, -0.0060036378F, -0.00975735672F, 0.00803445838F, - -0.00100403803F, -0.0151399132F, -0.0102481134F, -0.0133183813F, - 0.00495227939F, 0.012529524F, 0.00557643548F, 0.0121261925F, 0.0111627309F, - 0.00105732982F, 0.00167400111F, 0.000776775472F, 0.0131408377F, 0.00900513F, - 0.00834704377F, 0.00513377693F, 0.00548549555F, -0.00325880689F, - 0.015681399F, -0.000658950594F, 0.0263234898F, -0.00279737776F, - -0.0159979966F, -0.00963913836F, 0.00272002653F, -0.00385679328F, - -0.0175618194F, -0.0155388396F, 0.0175122488F, -0.0105418619F, - -0.00506429328F, -0.0056455778F, 0.00392265059F, -0.0181822926F, - -0.00476252194F, -0.0101638222F, 0.0163924638F, 0.0191979036F, - -0.00646259636F, -0.00846057292F, -0.00313847652F, 0.00968176499F, - 0.0143320858F, 0.00670807622F, -0.0130410595F, 0.0086931549F, 0.0074854116F, - 0.0143631538F, -0.0120247612F, 0.00444146F, 0.00551637867F, 0.00574463839F, - 0.00199023355F, 0.00241435249F, 0.0108336629F, -0.00224618427F, - 0.000868916686F, -0.0111124106F, 0.000864228117F, 0.0060117892F, - 0.00516509544F, 0.00569040747F, -0.0139075974F, 0.00965254288F, - -0.00454696873F, -0.0119895032F, -0.00343567808F, -0.00739969034F, - -0.0152831795F, 0.0102597279F, -0.0133373579F, -0.0127210645F, - -0.0228775013F, -0.000203162184F, -0.00429813238F, -0.00740903383F, - 0.00126657041F, 0.00385490339F, -6.35978649E-5F, -0.00638496038F, - 0.00378459971F, 0.000467832549F, -0.0126903318F, 0.0251441896F, - -0.00353117334F, 0.0019252582F, 0.0129885646F, -0.0078029437F, - -0.000500582741F, -0.0123474766F, -0.00597515563F, -0.000220501388F, - -0.00753967883F, 0.00160683098F, -0.00261453539F, -0.00673610065F, - 0.0051869303F, 0.000398606295F, 0.00479053F, 0.00820665434F, -0.00808507856F, - 0.0132459886F, 0.00334569742F, 0.00408452051F, -0.00626951223F, - -0.0110234013F, 0.014861146F, -0.00483012851F, 0.0042057219F, 0.000827412F, - -0.00779158156F, 0.00239074952F, -0.00740348035F, 0.00140935939F, - 0.0102090416F, -0.00281606172F, 0.00279787974F, -0.00206277706F, - -0.011232771F, 0.0175661966F, -0.000552578131F, 0.0131400079F, 0.0054048025F, - -0.00490823621F, 0.000753740605F, 0.0125785777F, -0.0043550767F, - 0.0154950935F, 0.000818716595F, -0.00902174693F, -0.00617010891F, - 0.0147971865F, 0.00622682972F, 0.0021330046F, -0.0135526927F, - -0.00294064637F, 0.00741695706F, -0.000633340038F, 0.00961809605F, - 0.00892933644F, -0.0049124728F, -0.00375305978F, 0.00292337895F, - -0.00701441942F, 0.00855951197F, 0.0255233217F, -0.0136597445F, - 0.00491006346F, -0.0121544721F, -0.00250410382F, -0.00338696316F, - -0.000895139354F, -0.00682420563F, 0.00524745649F, -0.00912547857F, - -0.00976949278F, 0.00585254934F, -0.0022225331F, -0.00312314392F, - 0.00860783923F, -0.00260606525F, -0.0189025942F, 0.0156915281F, - 0.00344704534F, 0.0105870059F, 0.00106904679F, -0.00953097548F, - -0.0068142768F, 0.00708674733F, -0.0078364322F, 0.00351734646F, - 0.0050500459F, -0.0130422581F, 0.00494988728F, -0.00426219311F, - 0.0187269673F, -0.0130339637F, -0.0115748048F, -0.00441917637F, - -0.00954733882F, 0.00510254595F, 0.0223720744F, 0.00991587F, 0.00429450395F, - 0.00125940377F, -0.00917442515F, -0.0168043058F, 0.00170414615F, - -0.00621789275F, -0.000903666F, -0.00578809902F, -0.00753313443F, - -0.0143572567F, 0.00630624034F, 0.00876345299F, 0.00182350038F, - 0.00696630683F, 0.00884870626F, -0.00632689614F, 0.00459239725F, - 0.00059727591F, 0.004198357F, -0.0100921327F, 0.0246099774F, -0.00310592516F, - 0.000874927558F, -0.00632616272F, -0.0124954265F, 0.00194623496F, - -0.00678163F, -0.00976270624F, -0.00485943444F, 0.00729477359F, - -0.000478909322F, 0.00467696274F, 0.0167844687F, -0.00640864717F, - -0.00977773871F, 0.00523094786F, -0.00210035173F, 0.00672305562F, - 0.00529208081F, -0.00822926499F, 0.00355328619F, -0.00220704358F, - 0.0112248063F, 0.0160587747F, -0.00101164624F, -0.00667086476F, - -0.00716070225F, 0.0170294717F, -0.0065722093F, -0.0025555077F, - -0.00331825553F, 0.00246491353F, 0.00486832531F, -0.0174293239F, - -0.0049585253F, 0.00415348588F, -0.0151754431F, -0.00240871892F, - -0.00137903914F, 0.0123652378F, 0.0132345706F, 0.0130587611F, - -0.00391099928F, -0.0112808496F, 0.00454010954F, 0.00151465705F, - 0.00401805528F, 0.0128746685F, -0.00598115195F, -0.0111165065F, - -0.0054345387F, 0.000814362778F, 0.0139313629F, 0.0223006736F, - -0.0108336983F, 0.00432024617F, 0.008711976F, 0.0115658771F, -0.00780673232F, - 0.0170041062F, 0.0110848136F, -0.00747149298F, 0.00663087796F, - -0.00742164906F, -0.00387999788F, -0.0205713045F, -0.00476295827F, - 0.0132060517F, 0.00220255787F, 0.000279699423F, 0.00315754837F, - 0.0080641862F, 0.0159948394F, 0.0128244339F, -0.00897551142F, - -0.00918306783F, -0.000327784801F, -0.0191398393F, 0.00255047809F, - -0.0149209425F, 0.010560194F, 0.00570114283F, 0.0103770886F, 0.000401615805F, - 0.00161296444F, -0.0017098313F, 0.00730406074F, 0.00742356898F, - 0.00684580812F, -0.0167411026F, 0.0174437612F, -0.00833258498F, - 0.00706744334F, 0.0023898615F, -0.00135453336F, 0.00225898018F, - -0.010749259F, 0.00440374389F, -0.0113764368F, -0.00192032196F, - -0.000329731905F, 0.0133803654F, 0.00208866177F, -0.000681538309F, - 0.00186255388F, 0.0144063365F, 0.00269361446F, 0.00191461435F, - -0.00206412561F, -0.0117629049F, 0.000915627694F, -0.0139027173F, - -0.000691956782F, 0.00541399885F, -0.0102683753F, -0.0136092296F, - -0.0081817992F, 0.0140839517F, -0.00983024575F, -0.00858981721F, - -0.00264270301F, 0.00647244276F, 0.00299705099F, -0.0192193408F, - -0.0119044902F, 0.00466776453F, -0.00976069272F, -0.013378391F, - 0.000237935747F, 0.00365324575F, 0.0240299869F, 0.0105656376F, - 0.00182842626F, -0.00122539164F, 0.00260125892F, -0.00583702046F, - 0.0096727293F, -0.00923287123F, -0.0153385559F, -0.0130645223F, - -0.0111761214F, 0.0094107911F, 0.00782449171F, -0.0170999803F, - -0.00264702179F, 0.007226469F, 0.012290311F, -0.00619265251F, - -0.000151520595F, 0.014885162F, 0.0132851684F, 0.00295684161F, - -0.0118653253F, 0.00773780374F, 0.00731968833F, -0.00892395526F, - -0.00198912295F, -0.00346570462F, 0.00797058F, 0.00350699341F, - 0.00501760561F, -0.00692113349F, 0.00883502793F, -0.00270069344F, - 0.00877270196F, 0.00127004157F, 0.0169860888F, 0.00212831935F, - -0.0043946486F, -0.0252267346F, 0.00618170947F, 6.65625485E-5F, - -0.00393963931F, -0.00450464804F, 0.0189528931F, 0.00821481831F, - -0.00794697274F, -0.00566722266F, -0.00172430451F, 0.00437020883F, - -0.0113345571F, -0.00765795773F, 0.00125470513F, 0.0031503262F, - 0.00850255787F, -0.00788497645F, -0.0233027786F, 0.00114352664F, - 0.00317183533F, 0.000108029024F, -0.0118797366F, -0.0186545942F, - -0.00422618259F, -0.0122098615F, 0.00608142046F, -0.00828290079F, - 0.00427486096F, 0.0113516133F, -0.00389438635F, 0.0122109493F, - -0.00535176834F, -0.010669942F, -0.0087466212F, 0.0199279152F, - -0.00660625193F, 0.02056982F, 0.0100630699F, 0.00235421839F, 0.00161397806F, - -0.00446687F, -0.00678407121F, -0.0111320894F, -0.00126328389F, - -0.00395859452F, 0.0114576714F, 0.00634489767F, 0.000234813197F, - 0.00200924557F, -0.000919949729F, -0.00617952086F, 0.00545941433F, - -0.0132646393F, 0.000985672232F, -0.000640629558F, -0.0135951638F, - -0.00437705824F, -0.00678962469F, 0.00283876527F, 0.000530011428F, - -0.00384626025F, 0.000413451926F, -0.0171678122F, -0.00333550479F, - -0.00185330596F, 0.0228377208F, 0.00360474177F, -0.00215031253F, - -0.0114586409F, 0.00761288311F, 0.0203305501F, 0.00280779088F, - 0.00117983203F, 0.00467365747F, -0.0172348972F, -0.00367632275F, - -0.00446194084F, 0.00569512136F, 0.018397538F, -0.00861111842F, - -0.00698335469F, 0.00564190838F, 0.0152112665F, -0.00381842931F, - -0.00632069679F, 0.00196647015F, 0.00816258136F, -0.0111777438F, - -0.000444903853F, 0.00925780088F, -0.000756406749F, -0.00582944276F, - -0.0044315597F, -0.0124084977F, 0.0102326507F, 0.0106802247F, 0.0168840066F, - -0.00225042878F, 0.0134710707F, 0.000725145568F, -0.00389722851F, - 0.0139203379F, -0.00520799961F, 0.006907376F, -0.0112847481F, 0.0145034352F, - -0.00161716586F, -0.000190877268F, 0.00869966671F, -0.0073347115F, - -0.0127729597F, -0.00988047291F, -0.0179439541F, -0.0156923626F, - -0.00570699F, -0.00205133948F, -0.00984611176F, -0.00314956065F, - -0.00584741076F, 0.0099666426F, 0.00024184311F, 0.0038249169F, - 0.00335403276F, 0.00876248535F, 0.00582515F, -0.0248452537F, 0.00258166832F, - -0.00275312504F, -0.0113429362F, 0.000410165812F, -0.0050790119F, - -0.0073633641F, 0.011321F, -0.000641899474F, -0.00451901089F, 0.00426527066F, - -0.0180928446F, 0.000264618546F, 0.0114723723F, 0.00200885F, 0.00750312302F, - 0.0105666537F, 8.91281306E-5F, -0.00119715033F, 0.00736121228F, - 0.0165683068F, -0.00479468051F, 0.0178763904F, 0.00929612108F, - -7.06718347E-5F, -0.0144612938F, -0.00888216216F, -0.00442086114F, - -0.0120150335F, -0.02970309F, 0.00326791732F, 0.0108683482F, 0.0124098193F, - 0.00260777958F, -0.000559478533F, -0.00104822277F, -0.00825214107F, - -0.00745384861F, 0.00509305391F, 0.00152559637F, 0.00713458844F, - 0.0101822438F, 0.0104202861F, 0.000264638395F, 0.00240001944F, - -0.000131463647F, 0.00743971718F, 0.0026439114F, -0.00288660289F, - -0.00839793123F, -0.0237031933F, -0.00747447228F, -0.0104713524F, - -0.00179469783F, 0.011161102F, -0.00933684595F, 0.011167272F, 0.019878868F, - -0.00248230761F, -0.00645465637F, -0.0151393348F, -0.00592554267F, - -0.000785864366F, 0.0173675623F, 0.00905802194F, -0.0157387126F, - -0.00440372713F, 0.0172263384F, -0.00635364093F, 0.017226601F, - -0.000755745627F, 0.0148186022F, 0.00403472688F, -0.0103331339F, - -0.00120561069F, 0.0024673515F, 0.00324347173F, 0.0206856709F, 0.0102970358F, - -0.0120988768F, 0.00933498144F, -0.00494692475F, -0.00599525729F, - -0.0080457041F, 0.0103549184F, -0.00735259196F, -0.00211830065F, - 0.015615114F, 0.00116444612F, 0.0188732035F, 6.5534965E-5F, -0.00447682291F, - 0.00777245825F, -0.00227897498F, 0.0144227175F, 0.0101817455F, - 0.00536656426F, -0.00251481053F, 0.0130262505F, -0.000205644625F, - 0.0125985965F, -0.0041627218F, 0.0117372572F, 0.00728649F, 0.00479185116F, - 0.0177496988F, -0.0185191669F, 0.00617728382F, 0.0112163788F, - 0.000118837183F, -0.00459465198F, -0.00113548851F, -0.00525794923F, - -0.00931742229F, 0.0233422257F, -0.00739961257F, -0.0145181417F, - 0.00267887954F, -0.0114384955F, 0.00821793824F, -0.0126883583F, - -0.00199367129F, -0.0307447184F, -0.0161443427F, 0.00489574345F, - -0.00700524403F, 0.00476687588F, 0.00370823406F, 0.0040004896F, - -0.0092725046F, -0.00196468947F, 0.00288523547F, -0.00181744876F, - -0.00706841238F, -0.0137368655F, 0.00869702F, -0.00762708532F, - 0.00728835119F, 0.0058227675F, -0.00646886695F, -0.00491628F, 0.00167728448F, - 0.0120434826F, -0.00667448482F, -0.0133697744F, 0.00552694965F, 0.00014214F, - 0.00201393827F, 0.0152478646F, 0.00140031206F, -0.00819466822F, -0.00448939F, - 0.00688748434F, -0.00668638106F, -0.0087251123F, 0.00499434164F, - -0.00245015882F, 0.00643375237F, -0.0041985414F, -0.000508695724F, - 0.0129360361F, -0.00547821913F, -0.00715135923F, 0.0171364024F, - 0.00395527249F, -0.00266487687F, -0.013791766F, -0.00170265476F, - -0.00159016426F, 0.000558107684F, 0.00447505666F, -0.0105550429F, - -0.0146916173F, -0.00465025054F, 0.00331633026F, -0.00272018788F, - 0.00120985077F, 0.0196537469F, -0.00639762776F, -0.0117971357F, - -0.00831110682F, -0.00287983939F, 0.00888599362F, 0.00888121501F, - -0.00214770902F, -0.0015903567F, -0.0140373772F, -0.0115360748F, - -0.00355891185F, -0.000967002474F, -0.0271768F, 0.00166400836F, - 0.00547377113F, -0.0107839117F, -0.00602268474F, -0.00771618634F, - 0.00104717305F, 0.00537300203F, 0.00448653381F, -0.000871583063F, - 0.0104073482F, 0.00613988796F, 0.00968355406F, -0.0205564871F, 0.0129877049F, - -0.0212001689F, 0.0149137238F, -0.000267775642F, -0.00679695606F, - -0.00401208969F, -0.00557866786F, 0.000753024593F, 0.00371093093F, - 0.0106948055F, -0.012385604F, -0.00883637927F, -0.0092618F, 0.00102472678F, - 0.0117161991F, -0.0107668964F, 0.00111048401F, 0.00698614726F, - -0.00747745903F, -0.00974855199F, 0.00105293223F, -0.00851027109F, - -0.00210020668F, -0.0167115182F, 0.00409870641F, 0.0113368807F, - -0.00637702178F, -0.0118063968F, -0.00453124242F, -0.023098357F, - -0.00734938495F, -0.0122824805F, 0.00314564863F, 0.0085613206F, - -0.00449111918F, -0.00403556321F, 0.00413223309F, 0.00598741043F, - -0.00505741639F, -0.00218362804F, -0.00221424946F, -0.00144822348F, - 0.00151030114F, 0.00099561282F, -0.00455523795F, 0.0103530819F, - -0.0103701521F, -0.0099713644F, -0.00887661334F, 0.00113518618F, - 0.000188572F, -0.00479886588F, -0.0109409234F, 0.00151542888F, - 0.00320231263F, -0.00557715353F, 0.00729164202F, -0.0180271845F, - 0.0117339045F, -0.00590544F, -0.0118022021F, -0.00381578179F, - -0.00262363418F, 0.009681236F, -0.00624780357F, 0.0176570248F, - 0.00255851191F, -0.000386298285F, 0.0168492235F, 0.000479943788F, - 0.00898517948F, 0.0064400835F, 0.00592188118F, 0.00317512895F, - 0.00439858297F, -0.00635828078F, -0.00553606916F, -0.00483616861F, - 0.00803821906F, -0.000225580705F, 0.00969447661F, -0.00871170871F, - 0.0160908606F, -0.00543036172F, -0.00755331665F, -0.00924166106F, - 0.00061852776F, -0.00816112384F, -0.00376119558F, 0.00154698396F, - 0.00095243071F, 0.00905576814F, 0.00690868264F, 0.0104487408F, - -0.00438191695F, 0.00588569511F, -0.0109383278F, -0.0114517128F, - 0.00184289075F, -0.00340833166F, 0.00549009303F, -0.00220521144F, - 0.0238704234F, 0.00520370575F, 0.0139587112F, -0.00982437655F, - -0.0169909373F, 0.00968409609F, 0.00645261165F, 0.00634458428F, - 0.00113542529F, 0.00408515567F, 0.00922563951F, -0.00328741129F, - 0.00484349066F, 0.0256892797F, -0.0105077252F, 0.00042554806F, - -0.0114420326F, 0.00327131851F, 0.0106610535F, 0.000808252F, -0.00778048206F, - 0.0151525727F, 0.00309870765F, -0.00535282493F, -0.00346615864F, - 0.00312485755F, -0.00667654444F, -0.0103771444F, -0.00466044107F, - 0.00825342536F, -0.00147676421F, 0.0199466124F, -0.00702966517F, - 0.00324960169F, -0.0129445381F, -0.0209355801F, 0.00344457966F, - 0.00269738026F, -0.00116278382F, 0.0112655424F, 0.0127234068F, - -0.00632303068F, 0.0038873672F, -0.00405592052F, 0.00633710204F, - -0.0124655692F, 0.000383875071F, 0.00845773146F, -0.00145682693F, - 0.00661716843F, 0.00147552125F, -0.0220921207F, 0.00815432332F, - -0.0156896897F, 0.00206545438F, -0.0091680428F, -0.00106219633F, - -0.00610902626F, 0.00899232272F, -0.00405131234F, -0.0141717736F, - 0.0014943087F, -0.00370161748F, 0.00703777233F, 0.000830581528F, - 0.00774534931F, 0.0100914435F, 0.00641798321F, 0.00445859088F, 0.00979101F, - 0.000339482882F, -0.000924968335F, 0.0170039441F, 0.0204439F, - -0.00731406873F, -0.00704370253F, -0.00589858601F, 0.0129194017F, - 0.013488383F, 0.0181119125F, -0.00488270307F, 0.00470631942F, 0.00921742339F, - -0.00858068187F, -0.0101005891F, -0.0071898438F, 0.012851174F, - 0.00348398788F, 0.000902203436F, -0.00738477288F, -0.0299117863F, - -0.00685042422F, 0.00140633073F, 0.00207824167F, -0.00567543693F, - -0.00901785493F, -0.000945969252F, -0.0084985476F, -0.0172628015F, - 0.00319710607F, 0.00204429799F, 0.00266935932F, -0.0141282529F, - -0.000811515609F, 0.00763591798F, -0.00509392377F, 0.00161808659F, - -0.00925786F, 0.00904746726F, 0.0163099486F, 0.000774713524F, - -0.000587413728F, -0.0244131815F, -0.00414245948F, 0.0111712422F, - 0.00907121319F, 0.00823608134F, 0.00604400458F, -0.00964090507F, - -0.000831336132F, 0.00906256214F, 0.0173445418F, -0.0102158356F, - -0.00723882439F, 0.0053472491F, 0.0098416023F, 0.00880581327F, 0.0226965882F, - -0.00537293544F, -0.000105184277F, 0.00756623643F, 0.00620608032F, - -0.00173900102F, -0.0065766992F, 0.00628574518F, -0.0102091515F, - 0.00730939209F, -0.00821923F, -0.00541654695F, 0.00625501387F, - -0.00463870401F, -0.0010646747F, 0.0213055834F, 0.0226023793F, - 0.00383218285F, -0.0190721322F, 0.0116076441F, -0.0025970526F, - -0.0062522809F, -0.00678870594F, 0.0153960241F, -0.00697992183F, - -0.00191220362F, -0.00256249029F, -0.00549533451F, -0.0036405928F, - 0.00966559537F, -0.00549763115F, -0.0189805459F, 0.0194546F, -0.00635860115F, - -0.000628961832F, -0.00031550345F, -0.0272362288F, -0.00245278049F, - 0.0142344749F, 0.0189207401F, 0.00223618816F, 0.00806058571F, 0.0053524822F, - 0.00341849308F, 0.00173123728F, 0.00698203314F, -0.0144305956F, - 0.0165257566F, -0.000927457702F, -0.010905969F, -0.00215128856F, - -0.000546430587F, -0.00944237597F, 0.00835207663F, 0.01282568F, - 0.00546440948F, -0.0113836452F, -0.0156937744F, -0.0043619941F, - 0.0154318493F, 0.00436796108F, 0.00804889947F, 0.00957706664F, - -0.0151248872F, 0.00144616805F, 0.0018018228F, 0.0131272897F, 0.00228582183F, - -0.0042152782F, 0.00122440991F, 0.00339650176F, 0.00220783683F, - 0.00407240447F, -0.00359015679F, -0.0033602193F, 0.0113385851F, - -0.00666043069F, -0.00670315837F, 0.0141015695F, -0.0036479237F, - 0.00560943736F, 0.00161627075F, 0.000771637191F, 0.0151136685F, - -0.00376383564F, -0.00329973432F, 0.00918245688F, 0.0161849242F, - -0.0147193773F, -0.0195624325F, -0.00514147105F, -0.00794143602F, - -0.00851106551F, 0.0035895172F, 0.0137788476F, -0.000360926293F, - 0.0105903801F, 0.00183626928F, -0.00175341289F, 0.00264432584F, - 0.000729927444F, -0.00349916518F, -0.0281643067F, 0.0109810987F, - 0.0190056376F, 0.0117359357F, 0.00642753206F, 0.0128491092F, 0.0185238346F, - 0.0139983268F, -0.00690197F, -0.00511888601F, 0.00286785257F, - -0.00518778106F, -0.0168993138F, 0.00714110304F, 0.00420259638F, - -0.00457388116F, 0.00987349F, -0.0135405557F, 0.00855333F, 0.00948518794F, - -0.00875867717F, 0.00303792371F, 0.00246606627F, -0.0166455451F, - -0.0118058361F, 0.00611076038F, 0.00546668144F, 0.0108799059F, -0.001949645F, - -0.0114846611F, -0.0151500991F, -0.00509217475F, 0.00593788922F, - -0.00443434389F, -0.00138750568F, -0.00944714714F, 0.00496736914F, - -0.00190998625F, 0.000338154205F, 0.0156532601F, -0.0168142412F, - -0.00930423569F, 0.00590411946F, 0.00352796167F, -0.0155539252F, - -0.00201343768F, -0.000749431725F, 0.0080447F, 0.00680388976F, - 0.00474610785F, -0.00765478844F, -0.00150204333F, -0.00538357534F, - -0.0121151134F, 0.0124713844F, -0.00345275551F, 0.00565229077F, - 0.00908270665F, -0.000629266724F, -0.00189481338F, 0.0150093501F, - -0.0110407909F, 0.00963395834F, 0.00256026164F, -0.00652531534F, - -0.00894814823F, -0.00460921F, 0.0118202176F, 0.0061968877F, 0.0183988474F, - 0.00640721712F, 0.00687596155F, 0.00974912755F, 0.0215604417F, 0.01096249F, - 0.00802342407F, 0.00862018391F, 4.69021825E-6F, 0.00411623949F, - -0.00205092109F, -0.017610576F, 0.00542669743F, 0.00401596213F, - 0.00660774484F, -0.00444436911F, 0.00971276499F, -0.00189655914F, - -0.0130029237F, 0.0126778344F, -0.00522416225F, -0.00294456142F, - 0.00500088558F, -0.00353451399F, -0.00167737168F, 0.0047438F, - -0.00119614229F, -0.00245818147F, -0.0024128519F, -0.00111322617F, - -0.000916776F, -0.0107973861F, -0.00687575433F, 0.0133646652F, - 0.00724473363F, -0.00154348055F, 0.0102443667F, -0.00640535215F, - 0.00511354301F, 0.000477090594F, -0.00376258884F, 0.00225213519F, - -0.00637925556F, -0.00531308213F, -0.00313584087F, 0.000216770844F, - -0.000556734623F, -0.000102717582F, 0.0062688631F, -0.0132075567F, - 0.00148109195F, 0.00701826671F, -0.0179278795F, 0.000491478771F, - -0.0172722265F, -0.00284204911F, -0.020085955F, -0.00752543658F, - -0.00363213569F, -0.00208879705F, 0.00661787484F, -0.0158569925F, - 0.00202866062F, -0.0112603568F, 0.00166842574F, -0.000773100124F, - -0.00203767093F, -0.00803790148F, -0.000443101249F, 0.00621805666F, - 0.00531390775F, -0.00781523809F, -0.00349532254F, 0.0153602585F, 0.00545873F, - -0.0101329759F, 0.00207679858F, 0.00790157914F, 0.011386659F, - -0.00600382453F, 0.0199611615F, 0.0227934867F, -0.00521205924F, - -0.00729134539F, 0.00109314162F, 0.00189327577F, -0.0206986312F, - 0.000286159979F, 0.00146910117F, -0.00261259149F, -0.00297245849F, - 0.001308662F, 0.00875845831F, -0.000261806912F, 0.00922726374F, - 0.00223467289F, -0.00103806704F, -0.000176951333F, -0.0212078914F, - 0.00704553863F, 0.00229240232F, 0.00331505109F, -0.00715109287F, - -0.0166254938F, 0.00365792029F, 0.00742044859F, 0.00656036707F, - 0.0159435812F, -0.00352978986F, 0.0018889535F, -0.00255382946F, - 0.0174320806F, 0.00822849944F, 0.00692217937F, -0.00875406712F, - -0.00476310495F, -0.00712113269F, 0.0044564479F, 0.00215551F, 0.002675178F, - 0.00275967643F, 0.0224584378F, 0.00395895075F, 0.0159136299F, -0.0034051775F, - 0.00284806499F, -0.00971898064F, -0.00426509418F, 0.0132259037F, - 0.00435437355F, -0.00963632297F, -0.00539589114F, 0.00227034045F, - -0.000152063993F, -0.0113737965F, -0.00650298595F, -0.0040875352F, - -0.00172225689F, -0.0102861915F, -0.0122175403F, 0.00191221538F, 0.00494132F, - -0.00386824808F, -0.0190337822F, 0.00220953603F, 0.00449993601F, - 0.000922474253F, 0.00991458F, 0.0137016876F, -0.0173753556F, 0.00772137195F, - -0.00602264283F, -0.00337324664F, 0.00113030954F, 0.00200966769F, - 0.00440406334F, -0.000830486417F, -0.00153058779F, 0.00220889412F, - -0.0166391395F, 0.00206293189F, 0.00610174797F, -0.0085238032F, 0.00522584F, - 0.0079100132F, -0.01816478F, 0.00466381619F, -0.00219782442F, 0.00390901137F, - 0.00924052857F, -0.000670990383F, 0.0118872831F, -0.0117861489F, - -0.00068168313F, 0.0101065757F, -0.000673577F, -0.0062642605F, 0.022117544F, - -0.0120545039F, -0.00363461557F, 0.0022602065F, 0.0120234182F, 0.0166099779F, - 0.00104306743F, 0.00604840601F, 0.0176109225F, 0.00903518405F, - -0.00831245631F, -0.00456506247F, -0.0152500747F, -0.000833935395F, - 0.00259649474F, -0.00362305762F, 0.005092829F, 0.0196991377F, - -0.00939953607F, -0.000763645337F, -0.00599040929F, 0.0020466526F, - -0.0106113674F, 0.00328149949F, -0.0182222351F, 0.00755978934F, - 0.00962073263F, -0.0129405931F, -0.00650104322F, -0.000234956548F, - -0.00728067057F, 0.00632697484F, -0.0257518236F, 0.0112690758F, - 0.00813431945F, -0.00615655957F, 0.00634579314F, -0.000150181251F, - -0.0027668823F, -0.00514763175F, -0.000803246221F, -0.00538238976F, - -0.0144384727F, -0.000270162302F, -0.0131998F, 0.00341306813F, - -0.00401720125F, 0.00307671865F, 0.00240706792F, -0.0038629882F, - -0.00716686668F, -0.000372406532F, 0.00515601505F, -0.00881721918F, - -0.0121867582F, 0.000390623725F, 0.00447578682F, -0.00760400109F, - -0.00680760574F, 0.00899766F, 0.0125495298F, 0.00467800209F, 0.000837047235F, - 0.00692507671F, -0.00863104872F, -0.00348432432F, 0.00585705414F, - -0.00932316F, 0.016750982F, -0.00811309F, 0.00275863823F, -0.00528918533F, - -0.0113808969F, 0.0100419251F, 0.000162202879F, 0.0141568892F, - 0.00373359583F, -0.0152941654F, -0.00688190432F, -0.00564839831F, - 0.0031621228F, 0.0241944604F, -0.00144114962F, -0.0091853356F, - -0.0045276829F, -0.0101923821F, 0.000773190695F, 0.0168748833F, - -0.0153546743F, 0.00886729546F, 0.0100950524F, 0.00639102515F, - 0.00464774482F, -0.000242374692F, -0.00790909771F, -0.0194232985F, - 0.00744404085F, -0.00454045227F, 0.00943470933F, 0.00635156222F, - 0.000818080327F, 0.00193168689F, 0.00712374505F, 0.00311651477F, - 0.0161081143F, -0.00397987198F, 0.0115677463F, 0.0105718048F, 0.0108680036F, - 0.00915920362F, -0.00651367567F, -0.00418918533F, -0.00777303148F, - -0.0120512918F, 0.00603725156F, 0.00266542076F, -0.00530816335F, - 0.00441965275F, 0.0054116291F, 0.0205660518F, -0.00124120421F, - -0.00380577985F, 0.0076958267F, -9.87691237E-5F, 0.0109392079F, - 0.00684306165F, -0.0129181882F, 0.00390815455F, 0.018547751F, 0.00531918835F, - -0.00426470628F, -0.000172819447F, -0.00421660068F, 0.000844377675F, - 0.0136726731F, -0.00803554058F, 0.00152114453F, -0.00892815273F, - -0.0182154477F, -0.0017997406F, 0.00674922904F, 0.00450674631F, - -0.00400591735F, -0.00165810622F, 0.0176691934F, 0.00456193043F, - -0.00681889337F, -0.0145317875F, 0.00178672967F, -0.0042464016F, - -0.00351861096F, -0.0138006397F, 0.000523712544F, 0.0150764929F, - -0.0165934954F, -0.0113442689F, 0.00698201498F, 0.0106016658F, 0.0380823649F, - -0.0111904675F, 0.00145811471F, -0.00104435091F, 0.00665602414F, - -0.0144492285F, -0.0054734163F, 0.000592058757F, -0.0115366848F, - 0.0128350612F, -0.00360603305F, 0.00749933766F, -0.00570436195F, - 0.00742274756F, 0.00890977401F, 0.00735996058F, 0.00193312671F, - 0.00681603048F, 0.00162709318F, -0.00674607465F, -0.00285110739F, - -0.00162167614F, 0.000578804931F, -0.00407698145F, -0.0013305326F, - 0.0096465582F, 0.00173685886F, -0.00396762928F, -0.0126274982F, - 0.0108098844F, -0.00148123945F, 0.0141340196F, -0.00388603983F, - -0.0187104568F, 0.015227423F, -0.00244979258F, -0.00534303114F, - 0.0182826351F, -0.0102010351F, 0.00787786674F, -0.000925188593F, - 0.00705548562F, -0.00442146184F, 0.000153063302F, 0.0104224253F, - 0.00744932052F, 0.00533608533F, 0.00255798758F, -0.00260075391F, - 0.00892937649F, 0.00508356513F, -0.00219964143F, 0.00430174F, 0.00453064311F, - 0.00243249419F, 0.000473593F, -0.00447404897F, -0.0175264273F, - -0.0163678359F, 0.00953483116F, -0.00482659787F, -0.0072058F, 0.0152515052F, - -0.00252439873F, 0.0102916388F, 0.0287473612F, -0.00126276F, -0.0154040596F, - 0.00714721764F, -0.01467227F, -0.00237481808F, -0.00517015718F, - -0.0188401099F, -0.00718942704F, 0.013635355F, 0.00327199721F, - 0.00230073952F, -0.00296803424F, 0.00463456102F, 0.00361909764F, 0.0055318F, - 0.0130779948F, 0.00614039088F, -0.000859700667F, 0.0127236797F, - 0.000430098793F, -0.0116800796F, -0.00751311751F, -0.0110279154F, - 0.0108655244F, -0.0150757516F, 0.00623927452F, -0.0233706925F, - -0.00973123405F, 0.0212428011F, 0.00635117F, 0.0195986591F, 0.00474998169F, - 0.00018257089F, -0.00880441349F, 0.0031130244F, 0.00533302361F, - 0.00112526875F, 0.0124820489F, -0.0038609F, -0.00592042226F, 0.00893818401F, - 0.000123657548F, -0.00863299798F, -0.00236807298F, 0.0169215631F, - -0.00591130322F, -0.00894729141F, 0.0128512336F, 0.0156583413F, - 0.00602530828F, -0.00784831122F, 0.01267308F, 0.0040616761F, 0.000490369392F, - -0.00437433086F, 0.0131419152F, -0.022271635F, -0.010370953F, - -0.00480518024F, 0.00976492278F, -0.00381548121F, 0.00744105689F, - -0.00671577873F, 0.00190216524F, -0.00183436391F, -0.009199908F, - 0.00578697026F, 0.00756662898F, -0.0064369468F, 0.00482996879F, - -0.00425477186F, 0.00283677783F, 0.00632855529F, 0.00381395523F, -0.0078308F, - -0.00576376077F, 0.0137537867F, 0.0193966813F, -0.00276389834F, - -0.00365181803F, -0.0156215224F, 0.00129346841F, 0.0101842685F, - 0.00917642843F, 0.00509199733F, -0.0115098627F, -0.0090640625F, - -0.00760056963F, 0.00861070491F, 0.00339999911F, 0.000826970791F, - -0.00371331675F, -0.00204184186F, -0.00988834538F, 0.0168218538F, - 0.00789366197F, -0.0157591794F, 0.002497026F, -0.0135355284F, - -0.00427347561F, 0.0105854487F, -0.00558283646F, -0.0021453721F, - 0.0123480661F, 0.0129679423F, 0.00622411072F, -0.00752784498F, - 0.000614777033F, -0.00427444791F, 0.00845658127F, -0.00520393439F, - -0.0166249052F, -0.00552653195F, -0.00728781894F, 0.00254924223F, - 0.00788557343F, 0.0220875889F, -0.00628494192F, -0.0042446563F, - 0.00422044611F, 0.00616091955F, 0.0141520947F, -0.00150715758F, - -0.000721882214F, 0.00907895342F, -0.0135004837F, -0.00563409412F, - 0.00278873718F, 0.01552907F, 0.00930534489F, -0.0025948009F, -0.00220774021F, - -0.00982799195F, 0.0101353396F, -0.0112943165F, -0.00373231363F, - 0.00222415337F, 0.00914204307F, -0.00851059519F, -0.000192751279F, - -0.00270217308F, -0.00823665503F, -0.0166388191F, -0.000381018966F, - -0.0141971903F, 0.000367911765F, -0.00475205714F, -0.0143587217F, - -0.00519472919F, 0.0138893267F, 0.00771000143F, -0.00595741393F, - -0.000468586863F, -0.00493002171F, 0.0190373491F, -0.00911698397F, - 0.0131475432F, 0.00335992943F, 0.0105176279F, -0.00225736946F, - -0.0077736848F, -0.00583704608F, -0.00825794227F, 0.00638166582F, - 0.00350649841F, 0.0149623789F, -0.00526276F, 0.00440548779F, -0.00296133803F, - -0.0100011779F, 0.00254661147F, 0.00198297109F, 0.00313449162F, - 7.10724578E-7F, 0.00781413F, 0.00363557157F, 0.00616503553F, 0.00738072954F, - 0.00344914058F, 0.00306026125F, -0.00200305949F, 0.00653097918F, - -0.00622696057F, -0.00774021819F, 0.00377023476F, -0.00542524131F, - -0.000917271769F, 0.0115713449F, 0.00984586775F, 0.00368894683F, - 0.00558820041F, 0.00452712597F, 0.0119144302F, -0.00327286497F, - 0.00344546675F, 0.00280438177F, 0.00630164472F, -0.0069244937F, - -0.00412118621F, -0.00146335107F, -0.00492515741F, -0.000743862416F, - -0.0106762825F, 0.0071085738F, -0.0055946745F, 0.00165638363F, - 0.00145633391F, -0.0124540934F, 0.00410182262F, -0.00494324695F, - 0.0152295679F, 0.0110919336F, -0.0184471812F, -0.0056841732F, -0.0125137046F, - 0.00113975664F, 0.00102501176F, 0.00455101207F, -0.00930116884F, - -0.00398722664F, -0.00351100857F, -0.0011707159F, 0.00278460793F, - 0.00459645037F, 0.0148892524F, 0.000299606676F, -0.00154303107F, - 0.00920015946F, -0.00788015686F, -0.0098093152F, 0.00055305264F, - -0.00638234895F, 0.0127193853F, 0.0109067913F, -0.0107364142F, - -0.00121459935F, -0.00326305162F, 0.0158556476F, -0.014623154F, - 0.00698458962F, 0.00781947F, -0.00389276864F, 0.00474753F, 0.00358287152F, - 0.000959867204F, -0.0070757214F, -0.0243993495F, -0.00252788491F, - 0.00123909931F, -0.00711464649F, 0.00572738F, -0.00645026658F, - 0.00511694653F, 0.00675618742F, 0.00579930749F, -0.00133738515F, - 0.0125075066F, 0.00349616469F, -0.0131070456F, -0.0125211906F, - -0.00827510841F, -0.00842738897F, -0.00838633347F, 0.00794826F, - -0.0124113411F, 0.0201779474F, 0.00365531119F, 0.00601974735F, - 0.00686672516F, -0.00507492712F, -0.00297313556F, -0.00338335591F, - -0.00149325503F, 0.0178124979F, 0.012923006F, 0.0150890611F, -0.00122259383F, - 0.00772184134F, 0.0041506635F, 0.00459712092F, 0.016236769F, -0.00505138794F, - -0.000503551564F, 0.0161407664F, 0.00383024826F, -0.00233178516F, - -0.00174233248F, -0.00586589938F, -0.0161290262F, 0.0164367594F, - 0.00915148668F, -0.017274566F, 0.00276933797F, 0.00694238674F, - -0.00134743471F, -0.0295900796F, -0.00356107228F, 0.00191527442F, - -0.00703322468F, -0.00032124776F, 0.0115560209F, 0.0153286271F, - 0.00117121404F, -0.00861574057F, -0.000892852724F, 0.0072045126F, - -0.00568152731F, -0.00259258109F, 0.00209248508F, -0.00450316072F, - -0.0126599558F, 0.00504312478F, 0.00408189604F, -0.00865975F, - -0.00733348355F, -0.00730248448F, 0.0190533791F, -0.00162430445F, - 0.00222089468F, -0.0252564717F, 0.00899014343F, 0.00965255685F, - -0.014537598F, -0.0130632035F, 0.000267429452F, -0.0139156161F, - -0.00210291077F, -0.0184256453F, -0.00382140116F, 0.0206650235F, - 0.0098992642F, 0.00938921701F, -0.0243566204F, 0.00181981525F, - 0.00153963082F, 0.00731268106F, 0.00664801756F, -0.0149999103F, 0.010636149F, - 0.00238278671F, -0.00350213167F, -0.00542385643F, 0.00360119459F, - 0.021581715F, -0.00397652527F, 0.0108608408F, -0.0160477404F, 0.00494505279F, - 0.0112971421F, 0.0155564323F, -0.000689042441F, 0.0097103361F, - -0.00202262285F, 0.0113019114F, 0.0156197296F, -0.0140819959F, - -0.00480893394F, 0.00883260835F, -0.00549708307F, 0.00494813779F, - -0.00209320197F, -0.00069375115F, -0.00074427831F, 0.0089965919F, - 0.00212205364F, -0.0057097571F, 0.00358466688F, -0.0106915599F, - 0.000589539355F, 0.00341695803F, -0.00250398926F, -0.00136014842F, - -0.0141280266F, 0.0113955149F, 0.00370234F, 0.00114271382F, -0.00432205945F, - -0.0113560343F, 0.00132990105F, -0.00890210364F, -0.00855097547F, - -0.00574143883F, -0.00797552F, -0.00376223098F, 0.002569417F, - -0.00212954241F, -0.00284808641F, -0.00843525678F, 0.00563361403F, - 0.0174868386F, 0.00133948389F, -0.00933815539F, -0.00592341507F, - -0.0034337528F, 0.0152200758F, -0.00725790905F, -0.00356958783F, - -0.00988454F, 0.00458197063F, -0.0221075527F, 0.00852727238F, - -0.00811955519F, -0.0085292263F, 0.00438179588F, 0.0121734887F, - -0.0192575585F, -0.0186118707F, 0.00551533466F, -0.0067505734F, - -0.0163462665F, 0.00217174203F, -0.000757090689F, -0.008551999F, - -0.00469906768F, -0.000731744221F, 0.00218655914F, -0.0204809122F, - -0.0034783571F, -0.00415574107F, -0.0076114051F, -0.000690110552F, - -0.00297882175F, 0.0141702276F, -0.020799201F, -0.00677532936F, - -0.011172222F, -0.00736315828F, -4.07718107E-6F, -0.0098684663F, - -0.00476838509F, 0.00754395034F, -0.00812550914F, -0.00268528F, - -0.00497291237F, 0.0100935502F, -0.0128879389F, 0.00697854813F, - 0.0079856962F, 0.00432761386F, -0.000768104452F, -0.00272465288F, - 0.0143198101F, -0.0031974474F, 0.0103670852F, 0.00473338645F, - -0.00821115449F, 0.00590992719F, -0.00456202636F, -0.0034822938F, - 0.00922718365F, -0.00350060221F, -0.00958114583F, -0.0193206575F, - 0.00578415347F, 0.00417332072F, -0.00401047664F, -0.00745610753F, - 0.000832239864F, 0.00376584986F, 0.000185713943F, -0.00260165054F, - 0.00105452049F, -0.00350887864F, 0.00344576244F, -0.0171285216F, - -0.00847304612F, 0.0151413698F, 0.000153045476F, 0.00587626267F, - -0.00222554873F, 0.0116869062F, 0.0106069772F, 0.0115340995F, 0.0150553603F, - 0.0143449521F, 0.00466231909F, 0.00447282754F, 0.0146646723F, 0.0032033748F, - 0.00294943759F, -0.00812490843F, -0.0024768624F, 0.0033417528F, - 0.0102207921F, -0.0135005806F, 0.00515064225F, 0.000671677233F, - -0.0127314096F, -0.00014021447F, 0.00906498637F, 0.00903650094F, - 0.00443271501F, 0.0137632694F, -0.00869805552F, 0.00982232671F, - 0.0107251769F, 0.00190447574F, 0.0201984197F, -0.00099243864F, 0.0106775323F, - 0.0177724566F, -0.0151111577F, 0.012989196F, 0.00176723942F, -0.00179450971F, - -0.000378049852F, 0.0113623198F, 0.0240841415F, 0.00850792322F, - 0.00315217697F, -0.00449594669F, 0.00540524442F, 0.0104075577F, - 0.00941171683F, 0.00876687374F, -0.00344886188F, 0.00794266444F, - -0.000640587299F, 0.00636173086F, 0.0126631875F, 0.0155499782F, - 0.00333319954F, 0.00828198344F, 0.0102829505F, 0.017218478F, -0.00119593483F, - 0.0162794441F, -0.00219944981F, 0.00804833136F, 0.022598736F, 0.010319462F, - -0.00638431124F, -0.0123345507F, 0.00647421647F, 0.00222425186F, - -0.00929414F, -0.00505260518F, 0.00385796721F, 0.0122930901F, 0.0114478674F, - 0.00364197814F, -4.53598E-5F, -0.0109759048F, 0.00413160585F, 4.70147861E-5F, - 0.00470287865F, 0.00848768465F, -0.0152654555F, -0.00882692728F, - 0.00827058591F, -0.0107699623F, -0.0209423099F, -0.0104413042F, - -0.00205429178F, -0.00985969324F, 0.00490727881F, -0.00130182959F, - 0.00974928774F, 0.00572577165F, 0.0174735412F, 0.0024315787F, - -6.76259151E-6F, 0.0229094531F, 0.00975618884F, 0.00259129261F, - -0.000205309523F, 0.00170216791F, 0.00328907161F, 0.00257719029F, - -0.00966949F, -0.00364290504F, -0.00781005062F, -0.00451354729F, - 0.0045745112F, 0.00115126488F, -0.00214999728F, -0.00270025525F, - 0.000977655756F, -0.00549013587F, -0.00517657F, -0.000231101556F, - -0.0050278916F, 0.00413192343F, -0.007229398F, -0.0147690242F, 0.0173535701F, - 0.00119545509F, -0.00958619453F, 0.00921747647F, -0.00183684495F, - 5.79099651E-5F, 0.00100923458F, 0.0072344006F, 0.00600264408F, - 0.000388356304F, 0.000745725F, 0.00284433644F, 0.00676639145F, - -0.0145052643F, -0.0048570619F, -0.0263423398F, 0.0166447237F, - -0.000472157059F, -0.00345489732F, -0.0191119574F, 0.00529277697F, - -0.00899513345F, 0.000910801755F, 6.87624051E-5F, 0.0141681954F, - -0.00919959694F, -0.00469477149F, -0.0102343224F, -0.00820281357F, - 0.00094087969F, 0.00228280458F, 0.0039565512F, 3.28656506E-6F, - -0.00738035515F, -0.0112298084F, -0.01009358F, 0.00490468694F, - -0.00290405424F, -0.00707578892F, -0.0193929952F, 0.00220859423F, - 0.0029609527F, -0.0111362496F, -0.0148108816F, 0.0223596171F, 0.00360263907F, - -0.0115034515F, 0.0045310813F, 0.00716659799F, 0.0179185905F, - -0.00640563248F, -0.0236559827F, -0.0104185482F, 0.00406121602F, - 0.00336204702F, -0.00696934527F, -0.0081429733F, -0.004244118F, - 0.0193413664F, 0.00119007798F, 0.00155620032F, -0.010750995F, - -0.00776083488F, -0.00578200119F, -2.53327889E-5F, 0.00666797115F, - 0.00997138F, 0.0114594912F, 0.0104909092F, -0.00145600073F, 0.00172863156F, - -0.0131232282F, 0.0011868024F, -0.0115178041F, 0.00350375683F, - -0.000975266914F, 0.000156567388F, -0.00850843918F, -0.00289805955F, - 0.0126146218F, -0.00198539556F, -0.00483843684F, 0.000484403165F, - -0.00921443477F, 0.00708487676F, -0.00670979917F, 0.00475128973F, - 0.00911560468F, 0.0101000592F, 0.00866737869F, 0.00255930168F, -0.00880863F, - -0.00882011559F, -0.00724722352F, 0.00974316522F, 0.00269596721F, - 0.00583807426F, 0.00738955429F, -0.00289069116F, 0.0100683011F, - 0.00129939255F, 0.0100129033F, -0.00760310283F, -0.00834685285F, - -0.00892007351F, 0.0125119649F, -0.0238938574F, 0.0132474964F, - 0.00274004205F, 0.00275519816F, -0.0133710746F, 0.00171735859F, - 0.0091105327F, -0.0118303215F, -0.010132174F, 0.0010564588F, -0.018576555F, - -0.0144913F, -0.0169885159F, 0.0161085054F, 0.00494281435F, -0.00989538804F, - 0.0160712376F, -0.00598697504F, -0.00422981847F, -0.00968439411F, - -0.00902091432F, -0.00679841964F, 0.00702496525F, -0.0149875591F, - -0.00707387179F, -0.00988010131F, 0.0244812258F, 0.00109923107F, - 0.00387872872F, 0.00552564859F, -0.0221977159F, -0.0216332637F, - 0.0105015263F, -0.00507390546F, 0.0080092987F, -0.010847846F, - -0.00730357785F, 0.0116975214F, 0.0114658093F, 0.00433793571F, - -0.00144576083F, -0.0092568F, -0.0237093456F, 0.00703183329F, - -0.000226572418F, -0.0187232904F, -0.00118122948F, 0.00141497015F, - 0.00211550109F, 0.0200269092F, -0.00100925041F, 0.00296608778F, - 0.0210243836F, 0.00305866101F, 0.000712041045F, -0.000990333618F, - -0.00798231922F, 0.00856686383F, -0.00387315452F, -0.00134037377F, - -0.00309651322F, 0.00982712395F, 0.0111401668F, -0.0182626769F, - 0.00608791877F, -0.000150083346F, 0.010771363F, -0.018807305F, - 0.00432903972F, 0.0119898301F, 0.0118342536F, 0.00972606F, -0.0170742162F, - -0.00612798F, -0.0130779846F, -0.0132323848F, 0.0119533082F, -0.0103726182F, - -0.00575993769F, -0.00900462735F, -0.00278383517F, 0.00441666506F, - -0.000412819878F, 0.00316842436F, 0.00234223483F, -0.010655826F, - 0.00827805325F, -0.00628208322F, -0.00468977727F, 0.00280979089F, - 0.0106746191F, -0.00698630419F, -0.00246779481F, -0.0100303497F, - 0.00375294592F, 0.00044043138F, 0.00690990221F, 0.00332311727F, - -0.0114383288F, -0.00981378555F, 0.0112551711F, -0.0118791824F, - -0.0176339094F, 0.0179901F, -0.00452514878F, -0.00135919428F, 0.0104271639F, - -0.00116422179F, 0.00104700704F, 0.00236965832F, 0.00680269208F, - -0.0107131489F, 0.0103945471F, 0.00219085044F, -0.0125661148F, - -0.000614805729F, 0.000243669492F, 0.0158439726F, 0.00340416934F, - 0.000473340595F, -0.0191009324F, -0.0116910422F, 0.00726371584F, 0.00713026F, - -0.00240506232F, -0.00627797376F, 0.0169135742F, 0.00800524838F, - -0.00366039085F, 0.000551505305F, -0.00877148192F, -0.00406525051F, - -0.00709938537F, -0.00596455485F, 0.0130932098F, 0.0124030495F, - -0.00888364576F, -0.0071498258F, -0.0104919F, -0.00421637902F, - -0.0141712595F, 0.0088409828F, 0.013470782F, 0.000590355136F, - -0.00282432325F, -0.0285129827F, 0.00443926267F, 0.00458590919F, - -0.00919465441F, -0.00893311948F, 0.00219934876F, 0.012717545F, - 0.0111185713F, 0.013628507F, -0.00661085267F, 0.006322606F, 0.00159354182F, - -0.0062179314F, -0.0120480331F, 0.00457032863F, 0.000669632747F, - 0.00909491163F, 0.00143697276F, 0.00308674295F, -0.00275416928F, - -0.000592940371F, -0.00162136345F, -0.0120545039F, 0.000713093323F, - 0.00078498543F, -0.00565358391F, -0.015013109F, 0.00317689497F, - -0.00523467036F, -0.0197712965F, 0.00100673106F, 0.0217413325F, - -0.0113426009F, -0.00605777F, -0.0128899533F, -0.00581363542F, - 0.00371526578F, 0.00958361384F, 0.000708855514F, 0.00494980766F, - -0.00759367598F, 0.000949155306F, 0.00964782666F, -0.00885164551F, - -0.0126489382F, 0.00370337442F, 0.00339702098F, 0.00376610504F, - -0.00322707254F, 0.0248903427F, 0.01415125F, 0.00384173263F, -0.00686951168F, - 0.0203828F, -0.00379208918F, 0.00625804299F, 0.00468565384F, 0.0140124122F, - 0.0185797755F, -0.000373167277F, -0.00321884803F, -0.00117356575F, - -0.00576912705F, 0.0019253951F, -0.0162134245F, -0.0100236805F, - -0.015616579F, -0.00278398767F, -0.00742154289F, -0.0120551633F, - -0.012318287F, 0.00792581867F, -0.0125969118F, -0.00447013695F, - -0.000541266345F, 0.00500970939F, -0.000888014503F, 0.00374931702F, - 0.0129395062F, 0.00237210095F, -0.00846603047F, 0.00903900806F, - 0.00395609625F, -0.0110454205F, -0.00315169338F, -0.00775616569F, - 0.00864718575F, -0.00611550547F, -0.0156411156F, 0.00641376F, - -0.00220714509F, 0.00390639342F, -0.00425496325F, -0.00347797526F, - 0.0210346207F, 0.00277939579F, -0.00161631289F, 0.00096217118F, - -0.00471976586F, -0.00409393525F, -0.00335469609F, 0.0057853614F, - -0.0134332515F, -0.00430894317F, 0.000810733705F, 0.00162755F, - 0.00924395397F, 0.00558887888F, 0.0164745189F, -0.00773441233F, - -0.0095214257F, 0.00287359161F, -0.00980995875F, 0.00266654347F, - 0.00608815439F, -0.0284584798F, 0.00975471456F, 0.0152063994F, - -0.0164994262F, 0.00193875213F, -0.00600715913F, 0.00339698209F, - 0.00411951588F, 0.00537343603F, 0.00226849783F, 0.0197823234F, - 0.00192568835F, -0.0142036397F, -0.00277479249F, 0.0148183741F, - 0.0099480832F, 0.00896519516F, -0.00120642711F, 0.0083580669F, - -0.0151238143F, 0.015973106F, 0.00108992064F, -0.00977827515F, - -0.0110905292F, 0.00656929193F, 0.00500303088F, -0.00126396038F, - -0.00495359814F, -0.00592883769F, 0.0058039655F, -0.00272197812F, - -0.0185238756F, 0.0072036949F, 0.01686134F, -0.0115542896F, 0.0121890176F, - -0.0129077574F, -0.00635544769F, -0.0187226683F, 0.00337665738F, - 0.0138563225F, -0.00666017458F, 0.00535093062F, -0.00550202839F, - 0.00515652262F, -0.00290338695F, 0.00184381113F, -0.0164411F, -0.0069009522F, - 0.0150454938F, -0.00132908078F, -0.00409563212F, 0.000503369549F, - 0.00525033381F, -0.0112838289F, 0.00135322951F, 0.00615500053F, - -0.00623223139F, -0.0226985868F, 0.000806967728F, 0.00705600902F, - -0.00821539294F, -0.0188466758F, 0.00888236798F, -0.00158344652F, - -0.0252194833F, 0.00726947282F, -0.00115101901F, -0.000782220915F, - -0.00736710243F, 0.00474857632F, -0.0266373213F, 6.22997104E-5F, - -0.000204185824F, 0.00304115145F, 0.00421474548F, 0.0152607402F, - 0.00572050363F, 0.00971425418F, -0.0047967732F, 0.00146275281F, 0.010828237F, - 0.00883401F, -0.00689703133F, 0.00394656043F, 0.0123789674F, 0.0142678637F, - 0.00869886857F, -0.00751946401F, -0.0199944638F, -0.00153775862F, - -0.0165354051F, -0.00256583118F, -0.00688959192F, -0.00541473599F, - -0.00551051972F, -0.0132680666F, 0.00711107627F, 0.00133117754F, - 0.000137593961F, 0.0183581505F, -0.0100761782F, -0.00598558458F, - -0.00121415721F, -0.00883046538F, 0.0170295555F, 0.00345048052F, - 0.0104036471F, -2.15162036E-5F, 0.000756335678F, 0.000497040339F, - -0.00668485276F, -0.00918406248F, 0.0143209882F, 0.011586519F, - -0.00341760111F, 0.00414870027F, -0.0129709356F, -0.00867476128F, - -0.0151013248F, 0.0128633054F, -0.00344603672F, -0.0100651672F, - 0.0133585436F, -0.022767555F, -0.00941388123F, 0.00483491225F, 0.011685024F, - -0.00293389359F, 0.00444128131F, -0.0016461733F, -0.0109177139F, - -0.00209778082F, -0.00921317283F, 0.00515797967F, -0.0130659686F, - 0.0107083861F, 0.00479790755F, 0.00250851759F, -0.00296662073F, - 0.00701297168F, 0.00770718837F, 0.00272372458F, -0.0111753773F, - 0.0223629214F, -0.00596520212F, 0.0164968297F, 0.0088732373F, 0.00062321627F, - 0.00288449624F, 0.0118452823F, 0.00160962134F, 0.0042758421F, 0.00505951F, - -0.018594848F, 0.00568388076F, -0.0127252368F, -0.0109069049F, - 0.00790868048F, -0.00467206305F, -0.00204225699F, -0.00659005903F, - -0.00462440122F, -0.00948135648F, 0.00774958963F, -0.00432801945F, - -0.0137765249F, -0.012400629F, 0.00865960214F, -0.0067513762F, - -0.0104267541F, 0.00070980587F, 0.00573872356F, -0.00695253443F, - 0.00111334492F, 0.0109680602F, -0.00617390685F, 0.00768557796F, - 0.00562321395F, -0.00417117961F, 0.0128852157F, -0.00740228081F, - -0.0100384792F, -0.00268373103F, 0.00582832471F, -0.0246173628F, - 0.00637024175F, -0.000138191259F, -0.00408700528F, -0.00371627347F, - 0.00730590895F, 0.00469300896F, 0.00267600571F, -0.0106903948F, - -0.00835817121F, 0.0022389614F, -0.00239363685F, -0.00020373665F, - -0.00575938681F, 0.00934053585F, 0.00366359553F, 0.00212751771F, - 0.0152539574F, -0.00788956787F, 0.0129614258F, 0.0125877885F, -0.0303778406F, - 0.00390660157F, -0.00810889155F, 0.00885807257F, 0.00286012026F, - -0.00224996568F, -0.0182457455F, 0.0150701283F, 0.0117853619F, - -0.00271225418F, -0.00687562441F, 0.00682390342F, 0.00658790627F, - 0.00467104418F, 0.00540388655F, -0.0155842556F, 0.00484979898F, - -0.012201691F, 0.00463117845F, -0.00826024264F, 0.0096634645F, 0.015825009F, - 0.0106740752F, 0.00833896548F, 0.00428232271F, -0.00284220744F, - -0.0130049344F, -0.000879488187F, 0.01420373F, 0.00657256F, 0.0199527964F, - -0.000619281316F, 0.00791666843F, 0.00167312368F, -0.00491368957F, - -0.015146059F, 0.00076972408F, -0.00259434246F, 0.0121934982F, - -0.00922685768F, 0.00315292645F, 6.99037046E-5F, -0.000238594337F, - -0.0181366932F, 0.010490926F, -0.0120847793F, -0.00862451922F, 0.0203227606F, - -0.0093373945F, 0.00713167759F, 0.00314375316F, 0.00526189711F, - -0.0205599591F, -0.0096992841F, 0.0111075919F, -0.00030203248F, - -0.0063926978F, 0.00434361445F, -0.000846999406F, -0.00217128894F, - -0.0089943679F, -0.00547525194F, 0.00341695524F, 0.0265209228F, - -0.00869153813F, -0.0165432263F, 0.000988207408F, -0.00260438F, - -0.00815337524F, 0.00404046802F, -0.0131278196F, -0.00432158075F, - -0.00256340532F, 0.0112661952F, 0.000469386694F, -0.00502782548F, - -0.00293766521F, -0.00056534441F, -0.00786136929F, 0.00656650774F, - 0.000326095527F, -0.00554095488F, 0.00570903206F, 0.0186731424F, - 0.00485250959F, 0.00956541F, -0.000992438407F, 0.00381547748F, 0.0120365955F, - -0.00260291388F, -0.00599611364F, -0.00501967873F, 0.000654279487F, - 0.0119679803F, -0.00239707879F, 0.0096509438F, 0.0208771639F, 0.013648387F, - 0.0127848191F, 0.00359839085F, -0.00606127828F, -0.00353704719F, - -0.00670083F, -0.00237543834F, 0.00725069921F, -0.000769107195F, - 0.0119537516F, 0.00255557406F, -0.0160262771F, 0.00542672258F, - -0.00115422765F, 0.0164701957F, -0.00435965881F, -0.0029932796F, - 0.0100939358F, -0.000561836583F, -0.0152385347F, -0.00407890463F, - -0.00271289749F, 0.0155462408F, 0.00455467496F, -0.00830988493F, - 0.000826257F, 0.000376311917F, -0.00433185091F, 0.00645306194F, - -0.00847928692F, 0.00167669088F, 6.6790155E-5F, 0.00261923112F, - -0.00872057769F, -0.0132820094F, -0.0145372804F, -0.0197239444F, - -0.0144507838F, 0.000227721699F, 0.00843855F, 0.00450637517F, 0.0119820032F, - -0.0013599114F, -0.000429195788F, 0.0148180118F, 0.00687111076F, - -0.00497181853F, 0.00562190404F, -0.0141195236F, -0.0104955109F, - -0.0110041443F, -0.0032442857F, 0.000287242729F, 0.00157927896F, - -0.0228242911F, -0.00023423032F, 0.00020870562F, 0.00397741329F, 0.0078036F, - 0.00419006497F, -0.01875747F, 0.00478796661F, -0.00522647798F, - -0.00245595584F, 0.0018882486F, -0.00411479548F, -0.0140142674F, - -0.000882882683F, -0.0154710663F, 0.0133425649F, -0.00906302314F, - -0.00286620599F, -0.00152348075F, -0.0122228898F, -0.000227402765F, - 0.00104769971F, 0.0144549739F, -0.00460834848F, 0.0185420793F, - 0.00626278762F, -0.000417117146F, -0.00672120182F, 0.0154845761F, - -0.00124933419F, -0.00813244097F, -0.00664699636F, -0.0144937681F, - -0.00106714817F, 0.00142329466F, 0.0106326435F, 0.00151414739F, - -0.00640144479F, -0.00498665078F, 0.0134324254F, -0.00513687497F, - 0.00246355543F, -0.00283945445F, 0.0118583133F, 0.00355551182F, 0.00490159F, - 0.0126711428F, -0.00890185125F, -0.00198462326F, 0.00256930012F, - 0.000269632146F, 0.00186494063F, -0.00841286F, 0.000474466302F, - -0.0157784354F, 0.0152148288F, 0.000329047587F, 0.00592041342F, - -0.0115487091F, -0.00284580467F, -0.00434625056F, -0.00420663645F, - -0.00664837286F, -0.00897795F, 0.00811461639F, -0.0110358056F, - -0.00341118523F, -0.00989178102F, -0.0113526043F, -0.00136486674F, - -0.00214572111F, -0.0102133621F, 0.00989274401F, 0.0140696429F, - 0.00196786318F, 0.00486315228F, -0.00232708012F, -0.00494801346F, - 0.00629333034F, -0.0137920193F, 0.0134207485F, -0.0118818404F, -0.028205689F, - -0.0099053923F, 0.00127548014F, 0.0031143676F, -0.00475044362F, - 0.00380023685F, 0.00567343505F, -0.0136701269F, 0.0110856071F, - -0.000589404895F, -0.00512264296F, 0.00814895052F, -0.00296704331F, - -0.0110664666F, -0.00521077774F, 0.0119289616F, 0.0194726139F, - -0.0137789212F, 0.00845719781F, 0.0025444557F, 0.0168355256F, 0.00422309572F, - -0.0079200333F, -0.0209834967F, -0.00830139313F, 0.00965357386F, - 0.00900634378F, 0.0117417937F, -0.0187313445F, 0.00445096F, 0.00953069795F, - -0.00813550781F, 0.0102127921F, -0.00215767464F, 0.00427149097F, - -0.00797330681F, -0.0162121765F, 0.00448434614F, -0.00773144187F, - 0.0166053679F, 0.00646209111F, -0.00981708057F, 0.00180121476F, - 0.0117890285F, -0.00612753909F, -0.00563140213F, 0.00594781293F, - -0.00624790881F, -0.0156051358F, 0.0102640381F, -0.00626869779F, - -0.0165575519F, 0.000176459667F, 0.00889091939F, 0.00374864158F, - -0.00838304684F, -0.0186857581F, 0.00519896531F, 0.0219650213F, - -0.00403179461F, -0.00290294364F, 0.00362853613F, 0.00532794371F, - 0.00304389466F, -0.0122576868F, 0.00150427688F, -0.00156770565F, - -0.0062763337F, -0.00342247565F, 0.00503572868F, 0.007039438F, - 0.000967747706F, -0.00797257F, -0.0194764044F, -0.000830592704F, - 0.000510233862F, -0.00991629343F, 0.000313318131F, -0.000801584916F, - -0.00870773196F, 0.0048732874F, -0.00292941276F, -0.00428006658F, - -0.0092771193F, -0.010333729F, -0.0110287992F, -0.000980973826F, - -0.000135515715F, -0.0117730293F, 0.0165785179F, -0.00759951025F, - -0.00935418624F, -0.0171881635F, -0.00678058621F, -0.00244946894F, - -0.00683667557F, 0.0193101484F, 0.0015629906F, -0.00401485153F, - 0.00415929779F, -0.00984797068F, 0.00304308394F, 0.00687288772F, - -0.00241538882F, 0.000783930882F, 0.00813681539F, -0.0022138136F, - -0.0103255101F, -0.0108958893F, 0.000370157242F, 0.00207336619F, - 0.00493320264F, 0.00782645121F, 0.0151094683F, -0.00116168417F, - -0.00208626897F, -0.00810494833F, -0.00739367353F, 0.000582462351F, - 0.000859842403F, -0.00460760621F, 0.00211775F, 0.00648798468F, - -0.0079434691F, 0.00123800559F, -0.00513679953F, 0.0054291673F, - 0.00342324399F, 0.00375767378F, -0.000170235784F, -0.0149394562F, - -0.00942210387F, 0.0021573815F, -0.004516616F, -0.00221600197F, 0.011913457F, - -0.000318845472F, 0.0072962204F, 0.00901944656F, -0.0138305621F, - 0.00489945896F, -0.000800735F, 0.00792106241F, -0.00175888976F, - -0.00325119123F, -0.00705284718F, 0.00414731726F, 0.00342165213F, - 0.00530455308F, -0.00168148521F, -0.00980549213F, 0.00759268738F, - 0.00415574759F, -0.00642161164F, -7.04443755E-6F, 0.00943392608F, - -0.0124553395F, 0.000854189F, -0.00553705916F, 0.00462173205F, 0.0196080394F, - -0.000305002846F, -0.0133541422F, -0.00554809719F, 0.00813353F, - 0.00425445894F, 0.0145906527F, -0.0073769046F, -0.00929106772F, - 0.00691299327F, 0.00927176885F, -0.0265439413F, 0.00576701574F, - 0.0138181411F, -0.00510170776F, -0.00965195522F, -0.00695138F, -0.01355776F, - 0.0141768781F, 0.00307898596F, 0.00277667074F, -0.00334256492F, - 0.0189415738F, -0.0101480596F, 0.0122649949F, 0.00138187618F, 0.00901335478F, - -0.00706762681F, -0.00587467197F, 0.000249734527F, 0.00698856544F, - 0.0153881088F, -0.00427223183F, 0.00972042605F, -0.00788472872F, - 0.00980707631F, -0.00666271197F, -0.00774618F, 0.0163247399F, 0.00559740048F, - 0.00799503829F, -0.014182698F, -0.0190078355F, -0.000667302F, - -0.00236338726F, 0.00999651F, 0.00238481956F, -0.00711259805F, 0.0176166911F, - -0.0109915342F, 0.00696717435F, 0.00501984451F, -0.00460342F, - -0.000491750718F, 0.00430854037F, -0.00866310764F, 0.00415230682F, - 0.0150311179F, -0.00157862133F, 0.00355559587F, -0.00757667888F, - -0.0153356455F, -0.0210016891F, -0.0019849136F, -0.00396286929F, - -0.00495453505F, 0.0124992328F, 0.0093419319F, 0.00223435625F, - -0.0151616819F, -0.0180037208F, -0.0118486499F, -0.0284190234F, - -0.0175002981F, -0.00538842753F, 0.00621451717F, 0.00715296669F, - 0.00733157434F, 0.00533588929F, 0.000121857338F, 0.00282611838F, - 0.00709160138F, 0.00933833F, -0.000100900943F, -0.0358139277F, - 0.00685368432F, 0.00637838524F, -0.011083724F, 0.00889103487F, 0.0024230415F, - 0.00895293429F, 0.00634688186F, -0.00558077311F, 0.00751828589F, - 0.00285437424F, -0.000166834347F, -0.00744087202F, 0.0079718614F, - 0.00403741235F, 0.0185000338F, 0.0111627355F, -0.0159918386F, 0.00957937352F, - 0.00146734645F, 0.0103945639F, -0.00575775467F, 0.000273415644F, - 0.00743719889F, -0.011770715F, 0.0163556766F, -0.00260299677F, 0.0030166395F, - 0.0085563343F, 0.013922031F, -0.0174575355F, -0.00467775809F, -0.00707794F, - 0.0083027808F, 0.000936958881F, -0.00818277709F, 0.00350320037F, - 0.00490059657F, -0.0120440628F, -0.00597489579F, -0.00534280855F, - 0.00723923463F, 0.00855887495F, 0.00510397693F, 0.0113451201F, - -0.00622062106F, 0.000609278504F, 0.00765685132F, 0.000456022914F, - 0.0043397462F, 0.00564277451F, 0.00209007156F, -0.0076947026F, - -0.0111092338F, 0.00549790775F, -0.0110798283F, -0.00951938517F, - 0.00402496569F, -0.0111059742F, -0.0049807881F, -0.00768978521F, - -0.0104090013F, -0.0156216715F, -0.00191046612F, 0.00975766312F, - 0.00119118125F, 0.00364699657F, -0.00705289328F, 0.00535642123F, - -0.00295124226F, -0.00987030566F, -0.00727016618F, 0.0149832591F, - -0.00327656488F, -0.00977027696F, -0.0124807032F, 0.000798798515F, - 0.000784095144F, -0.00407799846F, -0.0111393463F, -0.00658319425F, - 0.0178248F, 0.00180598604F, 0.0017249228F, -0.00728101144F, -0.00493693305F, - 0.00194433681F, -0.00201068376F, 0.00385941169F, -0.0262482148F, - 0.00700952113F, -0.000971136906F, 0.00193026441F, -0.0162314866F, - 0.014334389F, 0.0160508174F, 0.00160823844F, -0.00320825959F, 0.00341965491F, - 0.00251301F, 0.00262391171F, -0.0170952547F, 0.00334728672F, 0.00677421829F, - -0.00907499529F, -0.00262888358F, -0.00862294901F, 0.00676228711F, - 0.0018629015F, -0.0119725913F, 0.00666816952F, 0.00751598319F, 0.0036370426F, - -0.00987713225F, 0.0012329797F, -0.00701925159F, -0.00559492735F, - -0.0274387859F, 0.00357300695F, -0.0135452701F, -0.0187806953F, 0.00932232F, - -0.00370745058F, -0.0107690347F, -0.00977181364F, -0.0166037269F, - 0.0148343127F, 0.025337087F, 0.008822768F, -0.00771301938F, 0.00318129733F, - 0.00344897574F, -0.0157913584F, -0.00692117214F, 0.0035104868F, - 0.00545145897F, 0.0235253014F, 0.00251303473F, 0.00983978529F, - -0.000509231293F, 0.000203163028F, -0.00422023609F, 0.00852819253F, - 0.00718109822F, 0.00237296079F, -0.00673691928F, -0.00414115936F, - -0.0221188F, 0.00616548909F, 0.00234770658F, 0.00245951721F, 0.0235058721F, - -0.00368222897F, -0.0145387566F, 0.0091266837F, 0.00934207253F, -0.00392882F, - 0.0145757524F, 0.00567703F, -0.00133206125F, 0.0103700487F, -0.0135208284F, - 0.000261168025F, -0.00392879359F, 0.000331078278F, 0.00595156848F, - -0.0300741717F, 0.013904674F, 0.00670656422F, -0.000555802893F, - 0.00635082927F, -0.0015985606F, -0.00681483746F, 0.0119356057F, - 0.0122893713F, 0.0157280862F, 0.001194585F, -0.0182180759F, 0.0185544956F, - -0.00659944303F, 0.00686821109F, 0.00151975779F, -0.00406792294F, - -0.0145510752F, 0.00792859495F, -0.00941572F, -0.00525475154F, - -0.00120945915F, 0.00476736F, 0.00948020443F, 0.00155187724F, - -0.00243601738F, -0.012227268F, -0.0180305317F, -0.00132327224F, - 0.00823720265F, 0.000556260347F, 0.00168406474F, -0.0201725308F, - -0.0101537053F, -0.00247249706F, 0.00888479501F, -0.0102100698F, - 0.00175223825F, 0.0070524062F, 0.00911324099F, 0.0153045356F, 0.0111680115F, - -0.00437776372F, -0.0130439317F, -0.0127994288F, -0.00339784F, - -0.00319584785F, -0.00345988967F, -0.0175144803F, 0.000267757016F, - 0.0117714927F, -0.00715581747F, -0.0114565305F, -0.0138053354F, - -0.0111561008F, -0.0163828954F, 0.0114207556F, 0.011362127F, 0.00774478121F, - 0.0166301299F, -0.0153078586F, 0.00728487223F, 0.0170850251F, - -0.00944722816F, 0.00878449436F, -0.00159838633F, 0.000821564754F, - 0.00172351964F, -0.00502871396F, -0.0213760026F, 0.00217101164F, - 0.00154421991F, -0.00587319722F, -0.0107048F, 0.0074308319F, -0.0231081173F, - 0.00782105885F, -0.00464464305F, -0.00404122053F, 0.00913720485F, - -0.02469665F, 0.0109546678F, -0.00867581926F, -0.014022301F, 0.00646450557F, - 0.00462819776F, 0.00362373469F, -0.00225882721F, -0.00444007944F, - 0.0160221867F, 0.00885025784F, -0.0022702571F, -0.016030563F, 0.00462710764F, - 0.006824804F, 5.87071081E-5F, -0.00375500531F, -0.00561090745F, - 0.00748269586F, 0.00254655466F, 0.000250024372F, 0.016417047F, 0.0028165793F, - -0.00332195358F, 0.00520965643F, 0.00110765116F, 0.0136277182F, - 0.00112208258F, -0.00156734081F, 0.00105793809F, 0.00569416303F, - -0.00950032286F, 0.0136572709F, 0.00205458165F, -0.00312061678F, - 0.000350557675F, 0.0231224634F, 0.000283919886F, 0.0137042776F, - -0.00213993411F, 0.0013813636F, 0.0042754719F, 2.05707438E-5F, - 0.00867792405F, -0.00538323727F, -0.00256516365F, -0.00944383536F, - 0.00553828664F, 0.0137840398F, -0.00534535712F, 0.00750926649F, - -0.00741352327F, -0.00850750878F, -0.00205379794F, -0.00724479137F, - -0.000842826383F, -0.00737177813F, 0.00653271331F, -8.94285113E-5F, - 0.00680783438F, -0.00944652129F, -0.0071924529F, 0.0126636289F, - -0.00123463909F, 0.00583887147F, -0.000718492665F, 0.00151078752F, - -0.00289039477F, 0.0164420549F, 0.000963497849F, 0.00467461674F, - 0.0046830764F, 0.00640099263F, 0.00170040166F, 0.000329292525F, - 0.00584814744F, 0.00590833975F, 0.00793662574F, -0.0194158927F, - -0.000790475751F, 0.00523048081F, -0.0126074785F, 0.0117479153F, - 0.000852561381F, 0.000831644458F, 0.00296490174F, -0.00529616326F, - 0.00693012681F, 0.00221211812F, 0.0167064834F, 0.00139398954F, - -0.000777591544F, -0.00201540138F, -0.00571056921F, 0.00342334085F, - 0.00839199591F, -0.0181688704F, 0.00266318256F, 0.00569178676F, - -0.00653453032F, -0.00139170978F, 0.00855774805F, -0.00342132663F, - 0.011024667F, -0.00507523352F, -0.000713735295F, 0.00517946389F, - 0.0258550663F, -0.000348817935F, 0.00633369479F, 0.00813773833F, - 0.00493736099F, 0.005423069F, 0.0156409033F, 0.0126136392F, 0.00825202F, - 0.0104797995F, -0.000780404604F, 0.00860382F, -0.00198114756F, - -0.0104321092F, -0.016212903F, 0.00993865076F, 0.000138918665F, - 0.0133889848F, 0.00328176748F, 0.00317496806F, -0.0152499145F, - 0.00744328368F, 0.00893348083F, -0.00356457266F, -0.00829485618F, - -0.0116654672F, 0.000263745897F, -0.000618490099F, 0.0085660629F, - -0.00247507193F, -0.019412173F, -0.00713809859F, 0.00718760304F, - -0.00638807239F, 0.000304179848F, -0.00779381627F, -0.00628878595F, - 0.000173491891F, 0.00456671882F, 0.0127713708F, -0.00271966611F, - 7.66565518E-6F, 0.00466582784F, 0.00442993781F, 0.0014840035F, 0.0122861685F, - 0.00273332442F, -0.00663980143F, -0.0031517311F, -0.000939372054F, - 0.000850239128F, 0.00265384209F, 0.000694982242F, 0.00545278378F, - 0.000499237503F, 0.0153871886F, -0.00374346483F, -4.80052E-5F, 0.0209963955F, - 0.0139282979F, 0.00708575407F, -0.00310315681F, -0.0120743467F, - -0.00385682471F, 0.0139474422F, 1.9801033E-5F, -0.00456121098F, - -0.00401818939F, 0.0177758671F, -0.00741371186F, -0.00318241352F, - -0.0146444514F, 0.00252528978F, -0.0117296474F, -0.00954174902F, - 0.0103519112F, 0.00519100856F, 0.00215753121F, -0.0158862155F, -0.014684651F, - -0.00640575727F, 0.00142304716F, -0.0178147517F, -0.0197070893F, - 0.0128453979F, -0.0126976343F, -0.00178477389F, 0.00571896648F, - -0.00881442335F, 0.00482811F, -0.0105476836F, -0.0163535606F, - -0.00433062064F, 0.0101411846F, 0.00738594029F, 0.00115841872F, - 0.00932633597F, 0.00764493179F, 0.00246370886F, -0.00393247046F, - 0.00282219495F, 0.00597372F, -0.0101857157F, 0.00650213379F, 0.0152629698F, - -0.0104336487F, 0.0211867411F, -0.012266336F, -0.0184680708F, -0.000551359F, - 0.0204781853F, 0.00882553868F, 0.0079900315F, -0.011971131F, -0.00419253716F, - 0.0198804978F, -0.00421457F, -0.0168075822F, 0.0220264327F, 0.00270734145F, - -0.00625502924F, 0.0216431525F, 0.0117619531F, -0.0126118036F, - 0.00117990957F, 0.00263520214F, -0.00986209884F, 0.00170308189F, - 0.000477113703F, 0.00305499882F, 0.000221282447F, 0.0106566446F, - -0.00992608164F, -0.0139482357F, -0.00331723946F, -0.0018508191F, - 0.00723810215F, 0.0089775715F, 0.028396951F, -0.0238676816F, -0.0156244263F, - -0.00693661673F, 0.0209589899F, 0.00938611291F, -0.00353875407F, - -0.00563729275F, -0.00856943615F, -0.0125780487F, -0.00510116899F, - 0.0039317375F, -0.0093521364F, -0.0116172731F, 0.00890120212F, - 0.00329665467F, -0.00683505787F, 0.00279002031F, 0.00873320829F, - -0.00800253265F, 0.012500694F, 0.00653027324F, -0.011384815F, 0.00429242151F, - -0.00623579463F, -0.0154649066F, 0.00975417905F, -0.00476918649F, - -0.00700238766F, 0.016458137F, 0.00994061679F, 0.0103597576F, - -0.00229505077F, 0.0093574971F, -0.000749732368F, -0.0118326647F, - -0.0106235715F, -0.00256507844F, -0.000976098585F, -0.000744778372F, - 0.0169641618F, 0.00430266047F, 0.0067515946F, -0.0016640079F, -0.0181206986F, - -0.0152015677F, 0.0129295196F, 0.00534511311F, -0.00199261634F, - 0.00954557769F, -0.000942670216F, -0.00366801652F, -0.00711313775F, - 0.00898908F, -0.017858889F, 0.000793333864F, -0.00291179982F, - -0.000225791358F, -0.0205927957F, -0.00669441512F, 0.0050666309F, - -0.00394503213F, 0.00502837496F, -0.00326260249F, -0.0160884522F, - -0.000549341959F, 0.0212966539F, 0.00270954659F, -0.0212551951F, - -0.0041244626F, 0.00582472375F, -0.00270377402F, -0.00306857796F, - 0.00302298204F, 0.00452032452F, 0.0112568922F, 0.00401226245F, - -0.00139352574F, -0.0151094785F, 0.00654827F, -0.00770294573F, - -0.00573534658F, 0.00367895374F, 0.00669532828F, -0.00338299922F, - -0.000886886439F, 0.00774846645F, 0.00925758574F, 0.019536471F, - -0.0251888372F, 0.00574324513F, -0.00363567518F, 0.0013679472F, - -0.00446415599F, -0.00183661957F, 0.0054011438F, 0.028387459F, - -0.0121140731F, 0.00738286646F, 0.00253482815F, -0.00998357683F, - 0.0083253663F, 0.00816220697F, 0.00888558105F, -0.00999387261F, - 0.0125372335F, -0.00494992F, 0.00576808071F, 0.00171687896F, -0.012183642F, - 0.00564187206F, 0.00587276276F, -0.0172079802F, -0.0118523473F, - -0.0068158065F, 0.0100890705F, -0.0220386963F, -0.00920160953F, 0.00883241F, - 0.0106659494F, 0.00447815098F, -0.0082470607F, -0.01205137F, -0.00463955F, - -0.00269401213F, 0.00424878206F, 0.00484208576F, 0.00958952401F, - -0.00222120434F, 0.010530985F, -0.0140954563F, 0.0125678349F, 0.0125674019F, - -0.0124923494F, 0.00801402144F, 0.00842484F, -0.00861732196F, -0.0183507614F, - -0.00511267502F, -0.0102960551F, -0.00830063131F, -0.00622779923F, - -0.00176107802F, -0.000897228718F, 0.000533784856F, -0.00191542227F, - -0.00279151532F, -0.00898606144F, 0.00721542537F, -0.00018217109F, - 0.0124807646F, 0.00527680805F, 0.00664464151F, 0.0104426555F, -0.00972559F, - 0.00806654152F, 0.000531627156F, 0.00352226477F, -0.00246244809F, - -0.000115177805F, 0.00981709F, 0.00315101678F, -0.0157281514F, - 0.00960235856F, -0.0131567027F, -0.0103555471F, -0.0212677531F, - 0.0114463177F, -0.00260004797F, -0.00937034469F, -0.0129898237F, - 0.00114785112F, -0.00664731441F, -0.000523108232F, -0.010769167F, - -0.0114997383F, 0.00314257783F, 0.00420687441F, -0.00246099127F, 0.00530639F, - 0.0190813635F, -0.00588708697F, 0.00372519065F, 0.0129144909F, 0.0100359833F, - 0.00171017111F, 0.00727729872F, 0.0054612332F, 0.00864802F, 0.00189591199F, - 0.0131636206F, -0.00193860987F, -0.00612038421F, -0.0046375F, 0.00432397518F, - 0.00485851848F, 0.00624728855F, 0.024337098F, -0.000170390384F, - -0.0219697282F, -0.00558759645F, 0.00902018137F, 0.00989395101F, - -0.0218783319F, -0.00466771F, 0.000431723078F, 0.0123300934F, -0.0236531049F, - 0.0170658808F, -0.0213403646F, -0.00514862547F, 0.00376793882F, - 0.00616077241F, 0.0212780703F, -0.00909718685F, 0.0022153277F, - -0.0175146069F, 0.0177889485F, 0.0133066271F, -0.00661007129F, - -0.00993655901F, 0.00889291707F, 0.00208415603F, 0.00977786724F, - 0.0202530939F, -0.00395622756F, -0.00379999029F, 0.0127233835F, - 0.0176383536F, 0.000602143118F, 0.0056331763F, 0.00964173675F, - -0.0141443545F, -0.00514026266F, -0.0105153611F, -0.00464559114F, - 0.00741961831F, -0.00520445732F, 0.00447967183F, -0.00847843848F, - -0.0111801242F, 0.0081906179F, 0.00270632165F, -0.00992285646F, - -0.00090403622F, -0.0155209722F, -0.00800409354F, -0.0125789298F, - -0.00826451369F, 0.00517594768F, -0.00761704F, -0.000329811737F, - 0.00519820722F, -0.00226430944F, 0.00448080711F, 0.00124079199F, - 0.0124370325F, 0.0221423823F, 0.00310856663F, 0.0174019188F, 0.0132021131F, - 0.00179647561F, -0.0024363366F, -0.0118559832F, -0.0131054949F, - -0.0076505281F, -0.0077744F, -0.00158789277F, -0.0149668772F, -0.0171192884F, - 0.0119530372F, 0.0151404263F, 0.011811248F, -0.0172659438F, 0.00997143798F, - 0.00323038572F, -0.0051106F, -0.0053227949F, 0.000551735342F, - -0.00178253686F, -0.00134078437F, -0.00551593117F, -0.00456393789F, - 0.017461814F, -0.00464793295F, 0.0108204549F, 0.0012846993F, 0.00169500127F, - -0.00477618398F, 0.0194868427F, 0.00842715427F, -0.00133900647F, - 0.0234186798F, -8.43616E-5F, -0.0163555667F, 0.0129508432F, 0.00302676018F, - 0.00425032852F, -0.00194407941F, -0.00813953672F, 0.0123803699F, - -0.001178985F, -0.0121629182F, -0.00496592419F, 0.004041682F, - -0.00505532045F, -0.00404504687F, 0.00313420966F, 0.00596040767F, - 0.00697907433F, 0.00236609625F, -0.0133344978F, -0.017430393F, - -0.0113804704F, -0.00544092432F, 0.00814364199F, -0.00169321278F, - 0.00224624085F, 0.00476330332F, 0.0028871398F, -0.00336273923F, - 0.00234968867F, 0.0144994007F, 0.0105253207F, -0.00205270573F, - 0.00779897906F, -0.00136097F, 0.0193002354F, 0.000628198031F, 0.0128845815F, - 0.00252927025F, -0.0147092957F, 0.00640649069F, 0.00765822176F, - -0.00681435782F, 0.00667431531F, -0.00286006648F, -0.0138621414F, - -0.00337856938F, 0.00495959353F, 0.00112730276F, 0.0159398038F, - 0.00119761121F, 0.0100380871F, 0.0256532971F, -0.016072683F, -0.00470176665F, - 0.00329117663F, 0.0133223478F, -0.00688769296F, -0.00497456174F, - 0.0151001262F, -0.0068829176F, 0.00347739505F, -0.0138637852F, 0.00960421F, - 0.0143483765F, 0.00347835221F, 0.0145532694F, -0.00482335594F, - 0.00315160071F, -0.00826201681F, -0.00586920558F, 0.0118281757F, - -0.0117057804F, 0.0119192414F, -0.0211512223F, -0.00333117461F, - 0.0103723882F, -6.82578548E-6F, -0.00509916665F, 0.0118882414F, - -0.0101001495F, 0.0075815795F, 0.00123452337F, 0.013901243F, 0.00937694497F, - -0.00646174559F, -0.0162207764F, -0.00513112918F, -0.00972137321F, - 0.00724958116F, 0.00697567221F, -0.000333074713F, -0.00270499382F, - -0.00523198675F, 0.00683519151F, -0.00199641823F, 0.0147553757F, - 0.00670989556F, 0.0175325479F, -0.0136607727F, 0.00905828737F, - 0.000309137686F, -0.00980134122F, -0.00812304F, 0.00797795784F, - -0.0110055953F, -0.000811614736F, 0.00137852936F, -0.00492680725F, - 0.00991544127F, -0.0060969945F, -0.000717136078F, 0.00430404209F, - -0.0153642194F, -0.0142124435F, 0.0158055276F, -0.00282366388F, - -0.00181617774F, 0.00869487412F, 0.00609011855F, -0.0056325756F, - 0.00642017368F, 0.00679256907F, -0.0152141294F, -0.0106545128F, - -0.0024905703F, -0.00380663038F, 0.0218164604F, 0.000716995F, 0.00519707846F, - 0.00360334292F, 0.0047442005F, 0.000635193777F, 0.00609278074F, - -0.00702339411F, -0.00550296903F, -0.0151377795F, 0.00965325534F, - -0.00246702577F, 0.00784119125F, -0.00790148322F, 0.00225391868F, - 0.00932633225F, 0.000219433015F, 0.00360450498F, 0.00386066223F, - 0.0161210876F, 0.00504076388F, 0.00799176F, 0.0110399146F, 0.0178024471F, - 0.00789275393F, -0.00332343671F, 0.00527333096F, 0.0049271551F, - 0.00745427748F, -0.0148150679F, -0.00894225482F, 0.0216979086F, - 0.00759861525F, 0.019901406F, -0.00459780963F, -0.00396820297F, - -0.0055776909F, 0.00673999498F, -0.0100007914F, -0.00291569601F, - -0.00505538238F, -0.0055664368F, 0.0145658832F, -0.00859369338F, - -0.0102463216F, -0.0158675481F, -0.0052186735F, 0.0103369644F, - -0.00676176138F, -0.00507817324F, 0.00982619263F, -0.00526232878F, - -0.00202443521F, 0.00339776138F, -0.00261039427F, 0.0103375353F, - 0.00727892946F, 0.00228268374F, -0.0056980215F, -0.00541978329F, - -0.000556720886F, -0.0117435753F, 0.00427403115F, 0.00319721922F, - 0.0157470293F, 0.0121144513F, -0.00376814208F, 0.00303928903F, - -0.000646157772F, 0.0160434786F, 0.00903514866F, 0.00107476232F, - -0.00631324854F, -0.00855531357F, -0.0327219665F, 0.0109231053F, - -0.00106012006F, -0.00658409623F, 0.0159704927F, 0.0139825754F, - 0.00823991094F, -0.000549329096F, -0.00710910233F, 0.00855512545F, - -0.00357096293F, -0.012462317F, 0.00427326234F, 0.0153359855F, - -0.0118728708F, 0.00378257502F, -0.0132125868F, 0.0140526965F, - -0.0113199335F, -0.00754948379F, -0.00628027599F, -0.00387277477F, - -0.00314355874F, 0.0101753529F, -0.00794930756F, 0.00357149448F, - 0.0189800765F, -0.00101783348F, -0.0103488741F, -0.00986962765F, - -0.00661281077F, 0.00871104188F, 0.00742651895F, -0.0118615944F, - 0.0013478779F, 0.00178657647F, 0.00790613331F, -0.00592794269F, - -0.0180630255F, -0.0125853904F, -0.00591016281F, 0.0114366543F, - 0.0102196401F, -0.00360275782F, -0.0129048415F, -0.00329605024F, - -0.00289839553F, -0.0135782026F, 0.0216322523F, -0.00439044554F, - -0.02193827F, -0.0176708791F, -0.00156463368F, 0.00187574152F, 0.0126804747F, - 0.00548469927F, -0.00656459108F, 0.00974997226F, 0.0155245541F, - 0.00483299373F, -0.00328834169F, 0.00933360495F, 0.0200152081F, - -0.00278699468F, -0.00572136417F, 0.00235736859F, -0.011803179F, - -0.00332207908F, 0.00181795598F, 0.00517131342F, 0.00551706F, 0.00128861971F, - -0.00604208745F, -0.0095023578F, -0.00484650442F, 0.00131907943F, - 0.0139420172F, -0.0069480706F, -0.00312406057F, -0.00592882326F, - -0.00643839454F, 0.00497517688F, -0.00558671F, -0.0150249F, -0.0116847055F, - 0.00464711897F, 0.0177941117F, -0.006873F, 0.00677212421F, -0.01041664F, - 0.00568185747F, 0.000230957434F, -0.00708522368F, 0.00631574122F, - 0.00706711551F, 0.0102209F, 0.0139513416F, -0.00923646148F, 0.0185666643F, - -0.00532444241F, -0.0156294834F, 0.0117686167F, 0.00118173368F, - 0.00321640749F, -0.0106854131F, 0.0171589907F, -0.00300548482F, - -0.00533052534F, 0.00178356015F, 0.00789918751F, 0.00639902521F, - -0.00398548739F, 0.00835759F, -0.000807555451F, 0.0016712239F, - -0.00156128046F, -0.0159316622F, -0.00362953171F, 0.0204994716F, - 0.00125445134F, -0.00936214067F, 0.000352119096F, -0.00554456795F, - -0.00930598378F, 0.000987091917F, -0.000588350871F, 0.0232215766F, - -0.0123417573F, -0.00834373944F, -0.00963667408F, 0.00923886616F, - -0.00721059972F, 0.0115432637F, 0.00610475475F, 0.0152064916F, 0.0117924027F, - 0.00224840641F, -0.0041384995F, 0.000799181056F, -0.00110459421F, - -0.000705628365F, -0.00161309668F, -0.00898494571F, -0.00197808677F, - 0.000331213494F, 0.00713437889F, -0.0180250984F, -0.0104329735F, - 0.0126394667F, 0.00692645F, 0.00843435433F, -0.00689978944F, 0.00704675587F, - 0.00479064789F, 0.00716532534F, 0.0157003049F, -0.00233491929F, - 0.0205707159F, 0.00220612576F, -0.017016435F, -0.0123031484F, - -0.00285592978F, 0.0084100971F, -0.0082540093F, -0.00360688381F, - -0.00380675402F, 8.8986948E-5F, -0.00431293948F, 0.000521810085F, - 0.0166974217F, -0.0235935431F, 0.0117483633F, -0.0073310039F, -0.0041356436F, - -0.00661214814F, -0.0136510814F, -0.0124738421F, 0.00346067152F, - -0.00451442366F, -0.00317849265F, -0.00249855313F, -0.0131431948F, - -0.00558578037F, 0.0220581014F, -0.00429618498F, -0.00705451192F, - 0.00713040028F, -0.00140697486F, 0.0052487934F, 0.0125367912F, - -0.00217617489F, -0.00255325902F, -0.000257669482F, -0.00772302365F, - -0.0083809467F, 0.000762661686F, -0.00961647555F, -0.0181839019F, - -0.00266301073F, 0.00284790015F, -0.0117804725F, -0.0111885928F, - 0.00460527884F, 0.0178258214F, 0.0187704973F, 0.0118460273F, 0.000905450142F, - -0.00221879059F, 0.00793146715F, -0.00561518129F, -0.00381945F, - -0.0101088751F, -0.00554247387F, 0.00759990606F, -0.00743100233F, - 0.00624393439F, -0.0233189128F, -0.0139915682F, 0.00267862761F, - 0.0135279624F, -0.00479365373F, -0.00476892618F, 0.0130003029F, - 0.00774875749F, -0.00193719927F, 0.00157954323F, -0.00699435687F, - -0.00457875F, 0.00690750638F, -0.00119003479F, 0.0154273435F, - -0.00214499235F, -0.0121220108F, -0.00566502847F, 0.00674186833F, - 0.00514043355F, -0.0120019084F, 0.000302801724F, -0.0198786799F, - -0.000960866921F, 0.0111624282F, -0.0195286088F, 0.00884611532F, - -0.00301713822F, 0.016268976F, 0.00709478464F, -0.00041359922F, - -0.00353435287F, 0.00809080247F, -0.00246537453F, 0.00538191432F, - 0.00239141355F, 0.00796730351F, 0.00540126301F, 0.0276941508F, 0.0181301646F, - -0.00456670113F, -0.0103292465F, -0.00447065337F, 0.00284333201F, - -0.00527795823F, 0.00429566158F, 0.0165522825F, -0.00701334793F, - -0.000944353698F, 0.0163850095F, 0.013548851F, 0.00700958353F, - -0.00710879918F, -0.00119131384F, -0.0202737022F, -0.00105403317F, - 0.00789205916F, -0.00178683875F, -0.0134207401F, -0.00179360504F, - -0.0062283F, -0.0195008F, -0.00151825382F, 0.00207092683F, 0.00802948605F, - 0.00104644732F, -0.0108452346F, -0.0113788415F, -0.0031024504F, - 0.000742456526F, 0.00644704094F, 0.00015038214F, 0.00590201421F, - 0.000564514252F, 0.00146289123F, -0.0198878814F, 0.000144113481F, - -0.00843331404F, -0.00222902792F, 0.00475437054F, -0.00760454498F, - 0.00448744604F, 0.00129258982F, -0.00666751154F, 0.00152059388F, - -0.0057013738F, -0.00921003707F, -0.00506199617F, 0.00702074543F, - 0.0148036731F, 0.0193549097F, -0.0010896665F, -0.00910105556F, - 0.00111483503F, -0.00204730383F, 0.0119754355F, 0.0016471179F, - 0.000570059579F, -0.00696980907F, -0.0118325762F, -0.00262779975F, - -0.000876441714F, 0.0046806084F, 0.0142307F, -0.0137101933F, 0.00522060599F, - 0.0104690464F, -0.00416472275F, -0.0110450601F, -0.0142780319F, - 0.00642209593F, 0.0205712654F, -0.0060520703F, -0.00915730372F, - 0.00121830078F, 0.00151653751F, -0.00132566295F, -0.00895553F, - 0.00814058166F, -0.0128454138F, 0.00574545F, 0.0138635337F, 0.0135685727F, - 0.00708654383F, 0.000811440754F, -0.0173374768F, 0.0178466775F, - 0.0111779813F, -0.00559146749F, 0.00247087842F, 0.00747926F, 0.000245094154F, - 0.00792092457F, -0.0056338557F, 0.00792458188F, -0.00312298397F, - 0.00321377837F, -0.0070566358F, 0.000439092459F, 0.000171240361F, - 0.00259141321F, 0.000270016026F, -0.00779766263F, 0.00975966F, - -0.000952404167F, -0.00225640694F, 0.0121604409F, 0.00184402918F, - -0.0170422811F, -0.00230580871F, 0.0158606116F, 0.00478251092F, 0.00338805F, - -0.00983634F, -0.00840895F, -0.00853072666F, 0.00787253771F, 0.00606980687F, - -0.0123015037F, 0.0111683337F, 0.0109092342F, -0.00554330135F, 0.0152980844F, - 0.00961534F, 0.00706241513F, -0.0183837507F, 0.00860675052F, -0.00855132658F, - 0.00521474937F, -0.0130810961F, -0.00832560286F, 0.008867383F, - -0.000681260368F, -0.0138180433F, -0.00903564598F, -0.00569513859F, - 0.00155699276F, 0.0089006694F, 0.0121894917F, 0.00268727774F, 0.00179486698F, - -0.00833939947F, 0.0154375639F, 0.00717447093F, -0.000257498672F, - 0.0214638226F, -0.00731432671F, 0.00410900405F, 0.0143988319F, - -0.00170559017F, 0.0104619451F, 0.00728248805F, 0.00504401373F, - -0.00115220691F, 0.0124828313F, 0.00223487732F, 0.000522529299F, - -0.0216958653F, -0.0129071604F, 0.00262192101F, -0.00532693788F, - 0.000246124255F, 0.00428792043F, -0.0162330586F, -0.0113607654F, - 0.00497807702F, 0.00416595815F, 0.0103979269F, 0.0094217211F, 0.0074279746F, - 0.00353689189F, -0.0107484628F, 0.00879076868F, -0.00223486777F, - 0.00647337036F, 0.00622044411F, 0.0119417915F, -0.00210395642F, - -0.00648231944F, -0.0126785375F, -0.0165537521F, -0.0132175069F, - 0.0149359675F, 0.00355675747F, 0.00701211626F, -0.00027010584F, - 0.00601805281F, 0.00134031218F, 0.0166365299F, -0.00442248071F, - 0.0152020613F, -0.000551466F, 0.00793015771F, 0.012412739F, -0.000356051751F, - 1.28230877E-5F, 0.0184572097F, -0.00270363572F, -0.0022692224F, - 0.0045780777F, -0.00394436531F, 0.00918713864F, 0.0102147888F, - 0.00320389378F, 0.00320465653F, -0.00999101251F, 0.00520234089F, - -0.0122123314F, -0.016241638F, 0.00738644786F, 0.00506748026F, 0.0155828306F, - 0.00210003974F, 0.0105641671F, -0.00671939319F, 0.011407732F, - -0.00161052297F, 0.000839918444F, 0.000794147723F, 0.0102534331F, - -0.0134741617F, -0.0107291499F, -0.00369528751F, -0.00380859128F, - -0.00755509548F, -0.00427390635F, -0.0156349894F, 0.000207078891F, - -0.00597965578F, 0.00161284092F, -0.01294694F, -0.000546276569F, - -0.0149046648F, -0.00455952203F, -0.00160157634F, -9.20736566E-5F, - 0.0259776246F, 0.00251063425F, -0.00940871146F, 0.0191250127F, - 0.00197130325F, 0.002153215F, -0.000795486907F, -0.0162833016F, - -0.0018571493F, 0.00502973748F, -0.0177845117F, 0.00163585693F, - 0.00218119728F, -0.0118965628F, -0.0038056348F, 0.00637774449F, - -0.0178551245F, -0.00657640118F, -0.00418258738F, -0.00191127835F, - -0.0077317F, -0.00976677F, 0.00563391764F, -0.00324832881F, -0.00612369273F, - -0.0114622898F, 0.00532758329F, -0.0203429647F, -0.00371302105F, - -0.0165246949F, -0.00242791884F, 0.000870447257F, 0.00353320967F, - 0.00218323292F, 0.0151157593F, -0.0167102739F, 0.00502325688F, - -0.00290989201F, 0.0109606124F, 0.00958169065F, 0.00813343562F, - -0.0103647467F, -0.00270215352F, -0.0222186223F, -0.00958228204F, - -0.0138990646F, 0.000419355405F, -0.00781940855F, 0.00358536863F, - 0.00197665789F, 0.00935720373F, 0.01330241F, 0.00261092163F, -0.0174261276F, - 0.0111564295F, 0.00131689094F, 0.00213936507F, -0.000953851617F, - -0.0155156776F, -0.00116631901F, -0.00106682826F, -0.00480203377F, - 0.0102480333F, 0.00563334348F, -0.01280744F, -0.00999433454F, 0.0115380781F, - 0.00761810876F, -0.0108860545F, 0.00428486F, -0.00742303627F, 0.00939744711F, - -0.00783397444F, 0.00777107477F, 0.00913247466F, 0.00641664676F, - 0.0126746548F, -0.00473650638F, 0.0225952305F, 0.0167043433F, 0.00469744625F, - 0.00304844696F, 0.00752998516F, 0.00800657924F, 0.0009932213F, - 0.00952892378F, -0.0105001964F, -0.0133529045F, -0.00451673102F, - -0.0161892846F, 0.0106001124F, 0.00177896477F, -0.000318787468F, 0.00520131F, - 0.00936970674F, 0.0164151639F, 0.00132267922F, 0.0223850496F, 0.0173494071F, - -0.0153939845F, 0.00862485077F, 0.014956479F, 0.0118091209F, -0.000992839F, - 0.00940625928F, 0.00135613943F, -0.00504085049F, 0.0113614593F, - 0.00145962369F, 0.0100913327F, -0.00596662145F, 0.0103226658F, - -0.0193592068F, 0.0047079525F, 0.0120451022F, -0.0146702742F, 0.00960462634F, - -0.0108943032F, -0.0116987899F, 0.00875102263F, -0.0165209938F, - -0.000585600559F, -0.00117586006F, 6.62028515E-6F, 0.0126372632F, - 0.00247086422F, 0.0114471503F, -0.00930912606F, 0.00886232F, -0.0095624486F, - 0.0141742174F, 0.00244876905F, -0.000359095138F, -0.00131152396F, - 0.0110852607F, -0.00909347646F, -0.000245004019F, 0.0233801883F, - -0.00197842368F, 0.0176831521F, -0.00675295945F, 0.00425049802F, - 0.00457716547F, 0.00529601937F, 0.0083704479F, -0.00693936693F, - -0.00835315697F, 0.0099331392F, 0.00593654F, 0.000939893F, -0.00687829778F, - -0.00503309118F, 0.0041158027F, -0.00095640996F, 0.0115273697F, - 0.00877156202F, 0.00928682182F, -0.00611023279F, 0.00592046184F, - 0.014704803F, -0.00970492F, -0.0104007926F, 0.000439974858F, -0.00327213F, - 0.00362800877F, -0.0124798957F, 0.018391259F, -0.00183387205F, - -0.00399745489F, -0.0016625073F, 0.00110518758F, 0.00710061193F, - -0.00763463F, -0.0055767186F, 0.000418006646F, -0.00508190878F, - 0.00590314809F, 0.0156609416F, 0.0104715303F, -0.00153667363F, 0.0157827307F, - -0.012060293F, -0.000416834984F, -0.00263627246F, 0.0155488485F, - -0.000383707811F, -0.00895290542F, -0.00563060166F, -0.00163327705F, - 0.00309019233F, -0.00736580975F, 0.00534234336F, -0.013661935F, - 0.0043198471F, -0.000220991307F, -0.00173272146F, -0.00514963409F, - 0.00363060739F, 0.00227728626F, -0.00395891909F, -0.00135227526F, - -0.0090025086F, 0.00411246065F, 0.00400529802F, 0.0068303193F, - 0.00211393088F, -0.017007824F, 0.00939228479F, -0.00526777F, 0.00257556466F, - -0.003467659F, 0.00636631949F, 0.00325079F, -0.0102344416F, -0.00351038389F, - -0.0017110859F, -0.00467684353F, 0.019524673F, -0.0187667981F, - 0.00715859374F, 0.0108969482F, 0.00578348665F, -0.0138023607F, -0.01725775F, - 0.00913604628F, 0.00901700184F, 0.00532656815F, -0.00425471086F, - -0.001205971F, 0.00662016263F, -0.00701595237F, 0.00131495297F, - -0.00165241747F, -0.00409045536F, 0.00105568534F, -0.0131668933F, - -0.0067316005F, -0.0098949736F, 0.0105472943F, -0.00546931243F, - -0.0191989075F, -0.00847938F, 0.0101154223F, -0.0110332062F, 0.00645868666F, - -0.00907314F, 0.0249251928F, -0.00133863743F, 0.000292565615F, - 0.00416547898F, -0.0064713764F, -0.000561038032F, -0.00618282566F, - 0.00149473059F, -0.00501317345F, -0.00907083228F, 0.010613245F, - 0.0100168074F, 0.0157692637F, 0.0103127854F, 0.00508588133F, 0.00672290288F, - 0.009761394F, 0.0169132445F, 0.00589771569F, 0.0159522891F, -0.00792189222F, - 0.00077211007F, 0.00331474398F, -0.0109556802F, 0.0109898839F, - -0.00916141178F, 0.0106557012F, 0.0107263122F, 0.00180419849F, - -0.0102292178F, 0.0176916923F, -0.00555416197F, 0.00324413483F, - -0.00316105853F, -0.00157884392F, -0.0124633322F, 0.00796612166F, - -0.021656733F, 0.00454495801F, 0.000519794063F, 0.00428929832F, -0.00592114F, - -0.00618167361F, 0.00714928936F, 0.00552424602F, -0.00171817211F, - -0.00438032486F, 0.0056970208F, 0.00277074846F, -0.0148858801F, - -0.00115579355F, -0.0127476F, 0.0143801644F, -0.00533524435F, - -0.00421264116F, 0.00435461383F, 0.00298680877F, -0.0132539757F, - -0.0175476652F, 0.0147055583F, 0.00802056864F, 0.00664191274F, 0.0071429708F, - 0.00290325331F, -0.00901522674F, 0.00534439087F, 0.00517198769F, - 0.0200491156F, -0.00451799948F, -0.00467825495F, -0.0072085266F, - 0.0104897721F, 0.00762937F, 0.00534575246F, -0.0129610226F, -0.00137989037F, - 0.0164885763F, -0.0179346148F, 0.0169565305F, 0.012133114F, 0.00673077395F, - -0.0205621477F, 0.000272638717F, 0.000699643861F, -0.00269824569F, - -0.00183177739F, 0.0133535145F, -0.00745274452F, -0.00932509638F, - 0.00584147172F, 0.00354642F, 0.00237608608F, 0.0044304654F, -0.00348890875F, - 0.00469840551F, 0.0105843516F, -0.00947772432F, 3.293384E-5F, - -0.00159379665F, -0.00332564628F, -0.0133581543F, 0.00506688049F, - -0.0199019F, 0.00714496849F, 0.00326597411F, 0.00253367447F, 0.0113574192F, - 0.0139316088F, 0.00634399895F, -0.00595962815F, -0.0208964F, -0.0109519837F, - -0.0035624248F, 0.000604834233F, 0.00716791349F, -0.00248821964F, - 0.0103263306F, -0.00134247961F, -0.00800495595F, -0.00205084449F, - -0.000405972532F, 0.0215932503F, 0.00455555459F, 0.0095964307F, - 7.88690959E-5F, 0.00205289037F, -0.00242667459F, 0.00773296785F, - -0.00205631158F, -0.00318702636F, -0.00870844442F, -0.00548889535F, - 0.0110974638F, 0.00287802937F, 0.00114155898F, -0.00791667681F, - 0.00454297476F, -0.00961532537F, 0.000787675555F, 0.0271046087F, - -0.0067826556F, 0.0095966775F, -0.0114376917F, 0.0207160506F, 0.00223357417F, - 0.00403364189F, 0.0016944313F, -0.00410932256F, 6.41781589E-5F, - -0.0128535079F, -0.0081271641F, 0.00881223939F, 0.00514235F, 0.00546136638F, - -0.0115041686F, 0.000587890856F, -0.00252655265F, -0.00677799108F, - -0.00421746681F, -0.00120240299F, 0.00466020172F, 0.0136498874F, - -0.0148531571F, 0.00792559236F, -0.00386031531F, 0.000544266659F, - -0.00306784594F, -0.01126571F, -0.00624221703F, -0.0152055575F, - 0.00616547465F, 0.00573962927F, -0.0055270358F, -0.0179976579F, - -0.0128434747F, 0.000101576203F, -0.0179562736F, -0.0110328058F, - -0.00547266286F, 0.00680706231F, 0.00704117771F, -0.00114063581F, - -0.0101183979F, -0.00282737147F, 0.0017103249F, -0.00794872176F, - -0.0153896427F, 0.0103428457F, 0.0125258863F, -0.00345277623F, - -0.0025986142F, -0.00633500749F, 0.00139849272F, 0.00503634615F, - 0.00738122826F, -0.00333550945F, -0.000559396F, -8.27088297E-5F, - -0.00668757781F, 0.0147529813F, -0.00738532888F, -0.00754261855F, - 0.0277220849F, 0.0100704338F, -0.00312853581F, -0.00407922361F, - -0.0224878769F, -0.000577080937F, -0.0147183277F, 0.0176357944F, - 0.0135350078F, -0.0186101757F, -0.00643469952F, -0.0237953123F, - -0.0165850092F, 0.00383511442F, 0.0166464206F, 0.00322752586F, - 0.00293617905F, 0.00436305953F, 0.0110636177F, -0.00220241304F, - -0.010214828F, -0.0190305095F, -0.00927247386F, 0.0154238585F, - 0.00150182308F, 0.00634696707F, 0.00657689525F, -0.00393621856F, - -0.000111954374F, -0.000488457619F, -0.00522349263F, -0.00312317302F, - 0.0146981208F, -0.00828836951F, 0.00510861492F, 0.00171319838F, - 0.0140559627F, -0.00997382402F, 0.0105340621F, 0.00151007017F, - -0.00119928038F, -0.0121895876F, 0.00721319905F, 0.00601587258F, - -0.00384263019F, -0.0107644424F, -0.0147059476F, 0.0223425832F, - -0.00845612772F, 0.0048852074F, 0.0119946813F, -0.00713722641F, - 0.00612813514F, 0.00198638812F, 0.00754899532F, 0.0149907349F, 0.0143701816F, - -0.00242784852F, -0.00594991F, 0.0171299409F, -0.00532505428F, - -0.0133817699F, 0.0111858267F, -0.00500049302F, 0.0127654681F, 0.0137593672F, - -0.0129013211F, 0.0100012757F, -0.00692725275F, -0.0104148565F, - 0.0152298724F, 0.018269334F, 0.00634854333F, 0.00990108307F, 0.0110561466F, - 0.00263927411F, -0.00964427087F, 0.0091263447F, -0.000107403306F, - -0.0148991104F, 0.0196914617F, 0.000152663459F, -0.0111054908F, - 0.00389482803F, 0.00126685668F, 0.0230541565F, -0.00545123126F, - 0.000318174978F, -0.00903017074F, 0.0115304478F, -0.00126411207F, - -0.000111522F, -0.0114809712F, -0.00601267442F, -0.00373916375F, - 0.00666603073F, 0.0162021611F, 0.0062248935F, 0.00371773518F, - -0.00248496816F, -0.00692411559F, -0.00481797615F, -0.00355783175F, - 0.00657984475F, 0.00169692934F, -0.00737863733F, 0.00520841498F, - -0.00827079825F, -0.00589712895F, 0.000176578105F, 0.00555268675F, - -0.00421882235F, 0.000154214737F, 0.0033577357F, 0.019514177F, - 0.00478405692F, -0.00607063F, -0.00166309404F, -0.0172700565F, 0.0209801532F, - 0.00597110577F, -0.0185994059F, -0.00960617233F, 0.00489440374F, - -0.000669706205F, -0.0213902164F, 0.0129982252F, -0.00322728581F, - -0.00627568364F, 0.0116002103F, 0.00195558928F, 0.00502403313F, - 0.00459394464F, -0.00460354099F, -0.0022047332F, -0.0119894929F, - -0.00127053261F, 0.00560206641F, 0.00818800088F, -0.0106732296F, - -0.00642293179F, -0.00260094972F, 0.00345112081F, -0.00668300502F, - -0.012307724F, -0.0100369817F, 0.00578598352F, 0.0131055443F, 0.00827192515F, - -0.00467776274F, -0.00586864538F, 0.000206240074F, -0.00772151165F, - -0.0180099346F, -0.000255881372F, -0.00566280307F, 0.0136706755F, - 0.0108669205F, 0.00748682488F, -0.00538999029F, -0.0142679457F, - 0.00552426931F, 0.0155160632F, 0.0140628209F, -0.0204155669F, 0.0101400642F, - 0.0196049418F, 0.00209200545F, 0.00328504876F, -0.00574146F, 0.00278106635F, - 0.00636697654F, 0.00590087101F, -0.00396725116F, -0.00538008707F, - -0.00108055526F, 0.00332821556F, 0.00230673375F, 0.00246931356F, - -0.00340052322F, 0.00562687358F, 0.00089362351F, -0.00500081107F, - 0.0121375183F, -0.0174503103F, -0.00637791073F, 0.00192113244F, - 0.0179205108F, -0.0112426942F, 0.00143539638F, 0.00368901645F, - -0.00976417121F, 0.0105852168F, 0.00364308595F, -0.00147153495F, - 0.0032787465F, -0.00467840396F, -0.000792022329F, -0.0103981839F, - -0.00271953782F, -0.000450620893F, -0.0189614538F, -0.00641978579F, - -0.00159202795F, 0.0119040404F, 0.00970571488F, -0.00534053845F, - -0.00195372384F, 0.00167388341F, -0.00486918772F, -0.00922048278F, - -0.00884053204F, 0.00864140317F, -0.000666178879F, -0.0185857844F, - 0.00587869529F, -0.00752007589F, -0.00600095419F, 0.00324375834F, - -0.0144643281F, 0.00608585961F, -0.00398737F, -0.00296900095F, - -0.00861212332F, 0.00330196624F, 0.0146218697F, 0.00131997291F, - 0.00370398792F, 0.000278731575F, 0.0223973282F, 0.017090017F, -0.0136047043F, - -0.0150319887F, -0.00681830663F, 0.00569811882F, 0.000249120611F, - -0.0157949217F, 0.00977106951F, 0.00334680174F, -0.0101088779F, - -0.00126974075F, 0.00761246774F, -0.00144262088F, 0.00295728422F, - 0.00802555121F, -0.000572477875F, -0.000828258926F, -0.00855401F, - 0.0110560451F, -0.0103426566F, 0.0178043861F, 0.0171949156F, 0.00761325844F, - -0.000545489835F, 0.0141614079F, 0.0222613495F, 0.00350951985F, - -0.0101502892F, -0.00983991195F, 0.000852057768F, -0.00426252605F, - 0.00161034265F, -0.00139143108F, 0.00404531974F, -0.00151539047F, - 0.00862430781F, 3.79242E-5F, 0.0082772607F, -0.0161206089F, 0.00601492403F, - -2.55759151E-5F, 0.00674141804F, 0.0053985035F, -0.00116123306F, - -0.00341859809F, 0.00460640946F, -0.00944501907F, -0.00178175548F, - -0.00786156207F, -0.00121702068F, 0.016158117F, 0.00416405685F, - 0.0145027125F, -0.0149281612F, -0.00207729521F, 0.00759039354F, - 0.0020618944F, -0.00794178341F, 0.00682816841F, -0.00406200904F, - -0.00801595952F, -0.00629257131F, -0.0176434871F, -0.0203756858F, - -0.00240462599F, 0.0150054581F, -0.00209568371F, -0.00616069604F, - 0.0192356575F, 0.0115319053F, -0.0119752232F, -0.00144138234F, - -0.00271038967F, -0.00823882781F, -0.00251297723F, 0.000272525533F, - -0.0108314725F, 0.00912213F, 0.00661709718F, -0.00245678518F, 0.00664766179F, - 0.00663118204F, -0.0268861949F, 0.0148170376F, 0.00853340048F, - -0.00795115158F, 0.00213056873F, 0.00513113849F, -0.00362954359F, - 0.0131524522F, 0.0161064416F, 0.0101269344F, -0.00436065253F, - 0.000333253207F, 0.00351407845F, 0.0133387232F, -0.0160141736F, 0.01360525F, - -0.0123085314F, 0.00108358555F, 0.0162993055F, 0.00619331608F, 0.0129505442F, - 0.00563231902F, -0.00177064154F, -0.00920683891F, -0.0118310377F, - -0.00520734F, 0.00627475139F, 0.00309343194F, 0.00706583215F, - -0.00349310646F, -0.01247737F, 0.0103621595F, -0.00535849575F, -0.012977697F, - -0.0198904946F, 0.00663259905F, 0.0106671629F, 0.00515502365F, - -0.00787993893F, -0.00982021913F, -0.011716F, -0.0119392294F, 0.00509048672F, - -0.000258594664F, -0.00212854799F, 0.00392366806F, 0.0101218559F, - 0.00014672667F, -0.00196832838F, 0.010335899F, 0.0133541981F, 0.00353524F, - 0.0165255778F, 0.00786653161F, -0.00922706F, 0.000676216208F, - -0.00130654371F, -0.00900413282F, -0.0142373722F, 0.00114482862F, - -0.00770109845F, -0.0101831974F, -0.0019667442F, 0.0100468323F, - -0.00700661447F, -0.0129350442F, 0.0101595121F, -0.00455577765F, - 0.00145996094F, -0.0127204079F, 0.0211080778F, -0.00411433354F, - 0.00405945256F, 0.00877046864F, 0.0181542765F, -0.00215825881F, - -0.011240676F, 0.0157761667F, 0.0171094015F, 0.00796202105F, 0.00504739303F, - 0.00969375856F, -0.00288441032F, -0.00375012285F, -0.00739060715F, - 0.00358294859F, 0.0109391175F, 0.00667406758F, 0.00128343888F, - -0.00828838069F, -0.00825272687F, 0.00744747557F, -0.0125429537F, - -0.0101027759F, -0.00599201955F, 0.0016732855F, 0.00414363947F, - 0.0149153415F, -0.0127100684F, 0.0058994554F, 0.0081838686F, 0.0119031342F, - 0.014206334F, 0.00558066461F, -0.00424418086F, 0.00397427566F, - 0.00198839116F, 0.00859253854F, 0.00398919731F, 0.00856936257F, - -0.00374450092F, 0.00775628584F, -0.00616647769F, 0.00476862071F, - -0.00717230327F, -0.00333192875F, -0.00479205744F, -0.00325406156F, - -0.0106357038F, -0.0100244833F, 0.0105330031F, -0.0184054933F, - -0.0175393727F, 0.00190614408F, -0.0199659634F, 0.00618009688F, - -0.00970280357F, -0.00120001845F, -0.000785926648F, 0.00653855829F, - 0.0278591625F, 0.00549238035F, 0.00862724334F, 0.00759193068F, - -0.00191054831F, 0.0040459428F, -0.0263668187F, -0.0059477F, 0.00509046623F, - -0.00917032F, 0.0100617642F, 0.0118211256F, -0.0073226844F, 0.0149497092F, - 0.000177303329F, 0.0129623786F, 0.0152798453F, -0.00533906976F, - 0.0206455197F, -0.00617633201F, -0.000839377695F, 0.0038484782F, - 0.0134568335F, 0.00251795F, -0.0021214732F, 0.00339451851F, 0.000389691151F, - 0.00839306787F, 0.00540152518F, 0.0175627396F, 0.00269140489F, 0.0052226726F, - -0.0038840503F, -0.00966398418F, 0.0132756494F, 0.00851210207F, - -0.00358518981F, -0.0126257241F, 0.0143075455F, -0.0036006826F, - 0.0113329953F, 0.00416655652F, -0.0041931F, -0.0261524338F, 0.00437413529F, - -0.018807698F, 0.00465144543F, 0.0161941629F, 0.0221442301F, 0.00557167409F, - -0.014908608F, -0.00481454702F, -0.00917794649F, 0.00354488054F, - 0.0240122247F, -0.00117705099F, 0.00342230056F, -0.016699614F, - -0.00130331179F, 0.00689931493F, 0.00119580445F, -0.00349462847F, - 0.00236488599F, 0.0180283431F, 0.0109379832F, 0.00396720134F, 0.00287363236F, - -0.00927207153F, -0.00635334244F, 0.0096470546F, 0.0151082519F, - -0.00309132785F, -0.0101538515F, 0.00844842941F, -0.0173657387F, - 0.0087813288F, 0.00364766247F, 0.00933969393F, 0.00749195227F, - 0.00992471538F, -0.010112009F, 0.00974461809F, 0.00567629095F, 0.00691125F, - 0.00535085704F, -0.0113800252F, -0.00909609906F, 0.00186330953F, 0.00661637F, - 0.00797673874F, 0.000115088871F, -0.00299720466F, 0.00559321046F, - 0.0157131571F, 0.0172193125F, 0.00483620167F, 0.000627775735F, - -0.0017489657F, 0.0115806786F, -0.00774235185F, -0.00146341114F, - -0.00246987F, 0.00237367884F, -0.00164961244F, 0.0043172529F, 0.00502220402F, - -0.00640573958F, -0.018557474F, 0.0114631886F, -0.00664665829F, - -0.00313468068F, 0.00964059867F, -0.0179679245F, 0.00672331592F, - 0.00839125179F, -0.00875904F, 0.00536632026F, -0.00813567732F, - 0.00762989046F, -0.00897175726F, 0.0046724109F, -0.00246327464F, - 0.0089348983F, -0.000176250396F, -0.00818571F, -0.00580006745F, - -0.0167511217F, 0.00238244492F, 0.00123456144F, 0.00234990171F, - 0.0200639758F, 0.00738032209F, 0.00429832609F, 0.00545122102F, - 0.00904528331F, 0.0115998192F, 0.00146752677F, 0.000103364364F, - -0.0112729464F, 0.00917043071F, -0.0219329763F, -0.0122520169F, - 0.0303916782F, -0.00709007494F, 0.000770473038F, 0.00174837676F, - -0.0108782342F, 0.00168134412F, 0.0193907917F, 0.00544936815F, - -0.00195374247F, 0.0126713011F, -0.00303781708F, 0.0158788897F, - -0.00185613916F, -0.00121553929F, -0.00156426127F, 0.00450107548F, - -0.0087818671F, -0.00427185185F, 0.00663292734F, -0.00226758444F, - 0.0111778677F, 0.000286102382F, -0.00574881537F, 0.0135676833F, - -0.0132283783F, -0.00668918062F, -0.0108781783F, 0.00662369048F, - -0.020436272F, 0.00367182703F, -0.00261057355F, 0.00449443096F, - 0.00043925707F, -0.0179587249F, 0.010629924F, 0.00717439177F, - -0.00523036951F, 0.00105821411F, 0.00995215774F, 0.0193639621F, - -0.0082708681F, -0.00154940679F, 0.0059805382F, 0.00321618817F, - -0.0127175851F, -0.00833309F, -0.00710965879F, -0.00523235276F, - -0.00412192661F, -0.00984313339F, -0.0122933974F, 0.00251951837F, - -0.009980659F, 0.00757690566F, 0.00301101268F, 0.00362282153F, - 0.00565349124F, -0.00886983145F, -0.0222398378F, -0.00791588519F, - 0.0079494305F, -0.00576335425F, -0.0102836462F, -0.000270639837F, - -0.0118625155F, -0.00316908373F, 0.00689691585F, -0.00278249499F, - 0.0083003426F, 0.0178124756F, 0.00631975662F, 0.00161926751F, 0.00114894682F, - -0.0068954262F, -0.0221526194F, -0.00396562088F, 0.00737281283F, - 0.000881149434F, 7.24827623E-5F, 0.00410929F, 0.00314611779F, 0.0127100823F, - 0.00480366312F, 0.000674361771F, 0.00640757941F, 0.00663840398F, - -0.00714790961F, -0.015105512F, -0.0040649618F, 0.0160877947F, - 0.00140215643F, 0.0225702077F, 0.00476742163F, 0.0063068429F, 0.00570714287F, - 0.00659942441F, -0.00594469393F, 0.0134940315F, 0.0032352889F, - -0.000278988882F, 0.00656564254F, -0.00177610049F, 0.00635497319F, - 0.00827981F, 0.0164363701F, 0.00403388031F, 0.0205672663F, -0.00471116602F, - 0.00528389F, 0.00487574888F, 0.0103010545F, -0.00259020296F, -0.00375608099F, - -0.0192362033F, 0.0149364611F, -0.0127122439F, 0.00306995772F, - 0.00796195213F, 0.00217591482F, 0.00392421056F, -0.00362733938F, - 0.0027182966F, 0.0133185154F, 0.001515107F, -0.00456173392F, 0.0101227555F, - -0.00282389065F, -0.00499267178F, 0.00357953878F, 0.00995678734F, - -0.00629345374F, 0.0032884595F, 0.0131226787F, 0.0106332377F, 0.00595240854F, - 0.0138158863F, -0.00459869253F, -0.00960604288F, 0.00798290689F, - 0.00936840568F, 0.00941405166F, 0.00174253585F, -0.000713216781F, - -0.00601957832F, -0.0114264786F, -0.0108501669F, 0.0107196318F, - 0.00166709675F, -0.0147476867F, -0.00179146905F, -0.00166342664F, - 0.0119167417F, 0.00692182919F, -0.0175295379F, -0.0110477637F, - 0.00150495558F, 0.0154641094F, -0.0104678432F, 0.000339280698F, - 0.0116806589F, -0.0122296764F, 0.0027097F, 0.0148799727F, 0.0127832796F, - -0.0112161785F, 0.00555475429F, -0.0059912568F, 0.000562351779F, - -0.00547393179F, 0.0107999258F, -0.0158283375F, 0.02048398F, -0.0102250054F, - 0.0188156459F, -0.0132794678F, 0.000347052788F, 0.013256155F, - -0.00445488095F, -0.00996771082F, 0.0101404684F, -0.00528432149F, - 0.00813815091F, -0.0010952244F, -0.0105335275F, 0.0116297584F, - -0.00520046661F, 0.023119159F, -0.00131046923F, -0.00982614F, - -0.00388459885F, 0.00809156056F, -0.00275681F, -0.00468852744F, - 0.0112376893F, -0.0140232239F, 0.00324628688F, 0.000175393157F, - 0.00358571601F, 0.0144496439F, 0.00103065209F, -0.0063816458F, - -0.00196974888F, 0.000737284601F, -0.00601243367F, -0.00362191675F, - -0.000857011124F, -0.00755258137F, 0.00914005097F, -0.00642423658F, - -0.0133899851F, -0.00739693269F, 0.00119124446F, -0.00948631112F, - -0.0129931895F, 0.00244311406F, -0.0106779449F, 0.00487804227F, - -0.0127985096F, -0.00852875505F, 0.00121402729F, 0.0254185814F, - 0.00220475951F, -0.00789521541F, -0.000835495477F, -0.00553109124F, - 0.00350801414F, 0.00883407146F, -0.000528292148F, -0.00821187F, - -0.0116041405F, 0.006351599F, 0.00473568F, -0.00301350979F, 0.0211990923F, - -0.00902290549F, -0.0210691076F, -0.00154333084F, -0.0179154947F, - -0.0112969931F, -0.0120436372F, -0.00177403749F, 0.00470194547F, - 0.0105299754F, -0.00967292301F, -0.000378177268F, -0.0177542549F, - -0.0047188797F, -0.00380424014F, 0.00751578622F, -0.00418762164F, - -0.00770346913F, -0.00537898298F, 0.00427801069F, 0.000717354182F, - -0.00446317205F, -0.00136967713F, 0.00913499482F, -0.000332730269F, - 0.0130901048F, 0.00373820937F, -0.00814307667F, 0.00661400147F, - 0.00916285254F, -0.0106948605F, 0.00354286982F, 0.0114238774F, 0.0161108039F, - -0.0078154F, 0.0200413708F, -0.00668485416F, 0.00226334296F, -0.0111487256F, - 0.00640576519F, -0.00975909457F, -0.00161144882F, 0.00361303496F, - 0.00814694911F, 0.00603603199F, -0.00683733774F, -0.0105167888F, - -0.000867107941F, -0.00798216462F, -0.0152665749F, 0.00187898555F, - 0.0170311537F, 0.0054370393F, -0.0140063781F, 0.00184525724F, -0.0144398455F, - -0.00351709F, -0.00310493633F, -0.00117536716F, -0.00392750185F, - -0.0278340615F, -0.00393151492F, -0.00930689F, 0.0205890648F, 0.00846173F, - -0.00723294774F, -0.00013959677F, 0.00479464419F, 0.0109100509F, - -0.0078421412F, -0.00775537593F, 0.00184088433F, 0.0108792298F, - -0.00890195463F, -0.000197791567F, -0.00349034392F, -4.86970675E-5F, - -0.0106288586F, 0.0147341872F, -0.0183124971F, -0.00368194981F, - -0.00955980644F, 0.00307190954F, -0.00419563F, -0.0138286669F, -0.00983058F, - -0.0077997488F, 0.0131860739F, -0.00959985517F, -0.00717902416F, - 0.00807715952F, 0.0105892867F, 2.95766949E-5F, -0.00253826077F, - -0.00203600479F, -0.00325922482F, 0.00165064016F, 0.00670730462F, - 0.00809789449F, -0.0118510583F, -0.0100517487F, -0.00765666785F, - -0.0176734626F, -0.00490408717F, 0.00713540334F, -0.000517339155F, - 0.0105730686F, 0.000673148839F, 0.0151175577F, -0.00804192666F, - 0.0121359695F, 0.0196430702F, 0.00442407932F, 0.000384042942F, - -0.0187739469F, 0.00980705395F, 0.00752614066F, -0.00603240449F, - 0.00896884222F, -0.0159592107F, -0.009992579F, -0.00668317359F, - -0.00244207517F, 0.000277410785F, -0.0186815355F, 0.00211232388F, - 0.00614588847F, -0.00696233613F, -0.00945395604F, 0.0138486559F, - -0.028954396F, -0.00918987952F, 0.00231549144F, 0.00176081888F, - 0.0174915176F, 0.0094127208F, 0.000711908273F, -0.00196020887F, - 0.000954522926F, -0.00227037584F, 0.0120030921F, -0.013142799F, - 0.00815729238F, -0.0162184052F, 0.000578719F, -0.00328457681F, - 0.00110027019F, -0.00317836553F, 0.00296473713F, 0.0212024767F, - 0.0100547466F, 0.0118710129F, 0.00156303647F, 0.00807174295F, -0.0101660639F, - 0.00804308709F, 0.00626306282F, -0.0102165816F, 0.00244054268F, - -0.00330120046F, 0.0126724532F, 0.0115087163F, -0.00356955547F, - 0.00168274657F, -0.0036486024F, -0.00453781523F, -0.00325860642F, - -0.0100466479F, -0.00922567397F, -0.00922850333F, -0.00542000961F, - 0.0163443293F, 0.0165279452F, -0.0120439595F, 0.00681217201F, - -0.000709610409F, 0.00450716214F, -0.0084731495F, 0.0192507096F, - -0.005803084F, -0.0157697555F, -0.0179949459F, 0.0148146972F, - -0.00314315362F, -0.0117746834F, 0.0113371452F, 0.0174013712F, - -0.00947504397F, -0.0195554215F, -0.00505796308F, -0.00648553157F, - 0.0253604483F, -0.00163884426F, 0.00133991125F, -0.0129861161F, - 0.00211077486F, -0.00731688878F, 0.0253886394F, -0.00675656274F, - 0.00970167387F, -0.0126099745F, -0.0176399667F, -0.00721136341F, - 0.00827053934F, -0.00316947023F, -0.0145729333F, -0.00297736283F, - 0.00697136298F, 0.00335259782F, 0.00185212726F, -0.00686588418F, - 0.00808142219F, -0.00796362944F, -0.000973475457F, -0.00228706887F, - -0.0179252885F, 0.00291790254F, 0.00592247071F, 0.00944099F, 0.0092589315F, - -0.00689411396F, -0.000486420904F, -0.0107542705F, -0.00922434498F, - 0.00947873853F, -0.0164787695F, 0.00428760797F, -0.00276540103F, - -0.00118275313F, -0.00229618605F, -0.0119468598F, 0.00265272358F, - -0.00856098905F, -0.00164880278F, -0.0155889057F, 0.00203568931F, - -0.000173727516F, 0.0165841058F, 0.00192993239F, 0.000376554963F, - -0.00313879596F, 0.00831472781F, 0.00360751175F, -0.00740411691F, - 0.00702558085F, 0.0136573566F, -0.00343235605F, -0.00838903338F, - -0.00526487129F, -0.0220838375F, -0.00666168891F, -0.0104411151F, - 0.00423907442F, -0.00428333692F, -0.0143113323F, -0.00125782471F, - -0.00626101717F, -0.00310610631F, -0.0137418341F, 0.0147238271F, - 0.0147350701F, 0.00378832384F, 0.0036835291F, -0.00756817264F, - -0.000726224214F, -0.00536726555F, -0.00274199899F, -0.00211573555F, - 0.0188765824F, 0.0114328573F, 0.00127236394F, 0.00884711463F, -0.0176843423F, - 0.0205857102F, 0.00225937879F, -0.00019756757F, 0.00220829528F, - 0.0165042095F, 0.0148073854F, -0.00407198211F, 0.0135538373F, 0.018761361F, - -0.00485590287F, 0.00222470984F, 0.00341510726F, -0.0111033069F, - -2.84829239E-5F, -0.00367562752F, -0.00194664975F, 0.0055630412F, - 0.00215285039F, -0.00583550707F, -0.01146062F, 0.00180057238F, -0.011608229F, - 0.00524463505F, -0.00945359562F, 0.0106316041F, 0.0173916128F, - 0.000415513176F, 0.00359120918F, -0.00969579257F, -0.0055252919F, - -0.00837013684F, -0.0008170933F, -0.000143738405F, -0.0102037676F, - 0.00183445239F, 0.0154727176F, -0.00727291405F, 0.00221114699F, - 0.00164220238F, -0.00186145864F, 0.00551191252F, 0.00166220043F, - 0.00747850165F, -0.024021592F, -0.0202741548F, 0.000117905991F, - 0.00214647874F, 0.00629229145F, -0.00663312478F, -0.0166233424F, - 0.000772364903F, 0.0156638306F, -0.0122926058F, -0.0181741137F, - 0.00160840922F, 0.00105195574F, -0.0022144129F, 0.00663437322F, - -0.00456514256F, -0.00752594F, 0.00890123565F, -0.00293882564F, - -0.00206399546F, -0.0111623583F, -0.00408828817F, 0.00956395F, - 0.00218541571F, -0.00875196513F, 0.0102825547F, 0.00466904882F, - 0.00461231358F, -0.00791879091F, 0.00483348314F, -0.00518837804F, - 0.00133403088F, 0.0215017665F, 0.00374831841F, 0.0179085247F, -0.0176111199F, - 0.00192480825F, -0.0115302745F, -0.0111825923F, 0.000947656867F, - 0.00625650352F, -0.00911722891F, -7.3800169E-5F, 0.0192685835F, - -0.00615221076F, 0.00266132341F, 6.18696E-5F, -0.00464321719F, 0.0164652746F, - 0.0106726419F, -0.0251515247F, -0.0105656469F, -0.0132362442F, - 0.00369388447F, 0.0058033471F, 0.0109619806F, 0.0031328015F, 0.0162254479F, - -0.00188632065F, -0.00135514874F, 0.0100871669F, 0.00104036764F, - -0.00512231886F, 0.000421396515F, -0.00314487633F, -0.0127660455F, - -0.00818396173F, 0.00616041385F, -0.0103607401F, -0.0120322807F, - -0.00632730359F, -0.0164867882F, -0.0030524889F, 0.00019820864F, - 0.00599935604F, 0.00223549851F, -0.00147669145F, -0.00702698808F, - -0.0142305084F, 0.0107856309F, 0.000870933291F, -0.0143346656F, - -0.00265181973F, 0.0115901511F, 0.0097942492F, -0.0177223608F, - 0.00727898115F, -0.0153750088F, -0.0091431858F, -0.016597474F, 0.0115004489F, - 0.0038027782F, -0.000920740655F, 0.00654774159F, -0.00226643379F, - -0.00699414872F, -0.000266748713F, -0.0179578271F, 0.00147879764F, - 0.00651198439F, 0.0154588958F, 0.00227732467F, 0.0184499882F, 0.00385047169F, - 0.0050796615F, -0.0196603555F, 0.018355865F, 0.0138930129F, 0.0288943164F, - -0.0115202842F, 0.000143073674F, -0.0100401007F, -0.00214623474F, - 0.0119011188F, 0.0114131477F, -0.00704085734F, -0.0153752342F, - -0.00426372094F, -0.0157169532F, -0.0115357237F, 0.0108932359F, - -0.00587352691F, 0.00341552286F, 0.0130161028F, -0.0119743673F, - -0.00740450853F, 0.00814183056F, -0.00283310772F, -0.0183838047F, - 0.00417139F, -0.00401700474F, -0.00118541101F, 0.00177602645F, 0.0124033773F, - 0.0173287205F, 0.000589107163F, -0.000593120058F, 0.00498746103F, - 0.00861738343F, -0.0110229077F, 0.010411608F, -0.0144194122F, 0.0104750562F, - 0.012518947F, 0.0129036549F, 0.00845029112F, -0.00834973F, -0.00260763499F, - -0.00131064863F, -0.00935855F, -0.0166930705F, -0.00313482899F, - -0.000748136663F, 0.0171443541F, 0.0190055352F, 0.00326428376F, - 0.000370232126F, -0.01571187F, 0.0217334535F, 0.0184177049F, -0.0005522953F, - -0.0226418488F, -0.0143853957F, -0.00625057705F, 0.00622589653F, - 0.00413257862F, 0.0125363059F, -0.00335203158F, -0.00197762088F, - 0.0111555634F, 0.00530435052F, -0.00455703028F, 0.0039730873F, - 0.00138199062F, 0.00141925376F, 0.00713908346F, 0.00736204535F, - 0.0144241443F, 0.0152732106F, -0.0132795051F, 0.00704878708F, 0.00874630176F, - -0.00522220554F, 0.000704026606F, 0.0111519266F, 0.0103909932F, - 0.00423740223F, -0.00487555237F, -0.00375379017F, 0.00124695781F, - 0.0102880932F, 0.000470719417F, 0.0184160639F, 0.00797404349F, - -0.00446850853F, 0.00537252612F, 0.00878707599F, -0.00881046057F, - -0.0091519272F, -0.00144400424F, -0.00353209954F, -0.000305412424F, - 0.00233414955F, -0.00179356732F, 0.00608550804F, -0.00253223768F, - -0.00644677738F, 0.0191087741F, -0.00531440508F, -0.00728849322F, - 0.00307407556F, -0.00229351013F, -0.00798074901F, -0.010345526F, - -0.00224194746F, 0.00362439733F, 0.0020059715F, 0.0152075179F, - -0.00712067774F, 0.00750992214F, 0.00850701146F, -0.00322241313F, - 0.00461266935F, 0.0144001981F, 0.00651477184F, 0.000632267853F, 0.006551011F, - -0.00467541255F, 0.0179632753F, -0.0173824672F, -0.000589666655F, - -0.00614353875F, 0.0028671606F, 0.000893194927F, -0.00331021263F, - 0.00724921143F, 0.0182566829F, -0.0256751906F, 0.00107335881F, -0.016180791F, - -0.00904515665F, -0.0049856808F, -0.010398305F, -0.00600025803F, - 0.00846826378F, -0.00698997453F, -0.005957318F, 0.00409551756F, - -0.00684664212F, 4.40740441E-5F, 0.00831392314F, 0.00448554475F, - -0.0143446308F, 0.00320833619F, 0.00014332596F, -0.0144400215F, - 0.0124360593F, 0.00193752162F, 0.00709736813F, -0.00480231084F, - 0.00263725454F, -0.0107290689F, 0.00692678429F, -0.000563777867F, - 0.000515769294F, -0.00101531704F, -0.00552780274F, 0.0103065642F, - -0.00451936293F, 0.0033601597F, 0.0143154692F, -0.0033913122F, 0.0187958814F, - -0.00849514455F, 0.00434127636F, 0.00550709059F, -0.0121247834F, - 0.0148232384F, -0.00843677949F, -0.00152467168F, -0.0117022116F, - 0.00638694223F, -0.00030661386F, 0.00763169164F, -0.00441744225F, - -0.00882133096F, -0.0158421211F, -0.00403007772F, 0.00460254913F, - -1.80821808E-5F, -0.00501017366F, 0.0176609326F, -0.00463913381F, - 0.00290423585F, 0.0176888499F, 0.00175045629F, -0.0086518582F, - 0.00153568888F, -0.00652997056F, 0.00308434456F, 0.00422200793F, - 0.0181537587F, 0.0110775828F, -0.00118029222F, 0.00755803334F, - 0.00874268543F, 0.000112732007F, 0.00536844134F, -0.00664209621F, - -0.0049409843F, -0.00689609256F, -0.00586120971F, 0.000405234547F, - -0.00432451302F, 0.00595584186F, 0.00125556451F, -0.00215668324F, - -0.0216503125F, 0.00110427965F, -0.00416230038F, 0.0135641219F, 0.018383313F, - 0.0012341059F, -0.0218425523F, -0.00832761172F, 0.0147307273F, - 0.00139089278F, -0.00356312F, -0.00922123343F, 0.0171827134F, 0.00214325171F, - -0.0012263289F, -0.000693674141F, 0.0119721843F, -0.0012305408F, - 0.00656315451F, 0.00514876749F, 0.0159636233F, 0.00827358F, -0.00525483768F, - 0.00956671406F, -0.00723735197F, 0.00410131F, -0.000604378176F, - -0.0107236831F, -0.0012977178F, -0.00733904261F, 0.00293637207F, - -0.0117068533F, -0.00970789511F, -0.00931727793F, -0.00497954758F, - -0.00783484802F, 0.0135866096F, -0.00995109603F, -0.00469664857F, - 0.0104480553F, -0.00819447637F, 0.00831835531F, 0.00294192F, -0.00526003819F, - 0.0162654743F, -0.00565971155F, 0.00811579265F, -0.00172589091F, - -0.016424004F, 0.0153012965F, 0.0100420965F, -0.00684647262F, 0.0108070262F, - 0.00650224648F, -0.001842572F, -0.00878990814F, -0.00178980012F, - 0.0193678588F, -0.0166199859F, 0.00176146F, -0.00450978754F, -0.00367577188F, - -0.00285667693F, 0.00940108672F, 0.000531289F, 0.00583176408F, - -0.00122618768F, 0.0154507598F, 0.0112957414F, 0.00438138889F, 0.0058307508F, - -0.0102024591F, -0.000739556621F, 0.00704010809F, 0.00122426299F, - -0.00241443096F, -0.00155404862F, -0.00402568094F, 0.00379089359F, - 0.00978347752F, -0.0109731685F, -0.00679203728F, -0.00378011377F, - 0.00574491499F, 0.000579330139F, 0.00295626465F, -0.00996932F, - -0.00595819391F, -0.00662926491F, -0.00115781673F, 0.00627593324F, - 0.00238689827F, 0.0147438888F, -0.00832337514F, -0.018991882F, 0.0122561967F, - -0.00591240544F, -0.00301448326F, 0.0200716257F, 0.00207527261F, - 0.00351657113F, -0.0190213528F, -0.00243810075F, 0.00216557598F, - 0.000910281728F, -0.0110436417F, 0.00416190736F, -0.0117417499F, - 0.00144855271F, -0.000490512815F, -0.0121412883F, -0.00962586794F, - 0.000800713F, -0.00965912361F, 0.00779422F, 0.010913291F, 0.00711266929F, - 0.00660631806F, 0.00625301898F, 6.5179479E-6F, 0.0014029434F, 0.00520937191F, - 0.00816753693F, -0.000519340334F, -0.0109168785F, 0.00311616017F, - -0.0010759189F, -0.00654598139F, -0.00482462207F, 0.0235445928F, - 0.00930873398F, 0.00418383442F, -0.00781563111F, -0.0108156092F, - -0.0167074129F, 0.000551555073F, -0.00065116724F, 0.0141450549F, - 0.00700910669F, -0.0199539009F, -0.00810813159F, -0.00236599171F, - 0.0142195178F, -0.0116581479F, -0.0195188038F, 0.00460377196F, - 0.00370692438F, 0.00918080099F, 0.00378885F, -0.00274780835F, -0.0104512898F, - 0.00592193846F, 0.00995762087F, -0.00201392779F, -0.00930528F, 0.0147393532F, - -0.00834947079F, 0.0100717787F, 0.00956952479F, -0.00519656949F, - -0.00387424813F, 0.00242929603F, -0.00327331945F, -0.0082035521F, - 0.00742010446F, -0.0102151837F, -0.00627428154F, 0.00122425135F, - 0.0110072177F, 0.00488092238F, 0.0117595801F, -0.0107202F, -0.00933603942F, - 0.0140397949F, 0.00745769311F, -0.000503226765F, -0.0107654016F, - 0.0110674715F, -0.00521032233F, -0.00891375262F, 0.00676560821F, - -0.00649886066F, -0.00212281803F, 0.00670928787F, 0.0117066586F, - -0.00587195856F, -0.0144003779F, 0.0161570348F, -0.00208855351F, - -0.00606798381F, -0.00803401414F, 0.00793907233F, 0.00577098F, - -0.00486791972F, -0.00286983699F, 0.0110275634F, 0.00690356316F, - 0.00146828254F, -0.0119059542F, -0.0107290708F, 0.00958107226F, 0.000775513F, - 0.0157109201F, -0.0051881033F, 0.00425488595F, -0.0132729048F, - 0.00816479884F, 0.0165186021F, 0.0033928426F, 0.000309963623F, - -0.0117211342F, -0.00921886601F, -0.00758297089F, 0.0117463395F, - 0.0186107028F, -3.66019376E-5F, 0.00438005105F, 0.000734780508F, - -0.000739740091F, -0.0189099126F, -0.00522060785F, 0.00890563149F, - -0.000360099541F, 0.00507219089F, -0.000997574418F, -0.00682778237F, - -0.00732657639F, 0.0132213123F, 0.00721160835F, -0.00858184136F, - -0.0201637726F, 0.0103310896F, 0.00270369602F, 0.000105810825F, - -0.000412702066F, -0.0132918078F, 0.000848422933F, 0.00232001278F, - 0.0121835293F, 0.00659917248F, 0.0112223327F, 0.00422755815F, 0.00919109769F, - 0.0127869379F, 0.00304812077F, -0.0130583756F, -0.0189559143F, -0.016448006F, - 0.0332264714F, 0.00712379254F, 0.00465778541F, 0.00499145174F, - 0.00176826143F, -0.00904006138F, -0.0163163189F, -0.0118175261F, - 0.0110565331F, 0.0103218574F, -0.00713669183F, 0.00414870167F, - -0.00598325254F, -0.00256967032F, 0.0138728702F, -0.00409447309F, - -0.0219191462F, 0.0153214363F, 0.0147951143F, 0.00755433738F, -0.0107886987F, - 0.00721160183F, -0.00181287189F, 0.0171567F, -0.000500515918F, - -0.00249090372F, 0.0130091906F, -0.0186449829F, 0.00522198854F, - 0.00769100944F, -0.00561438315F, 0.00872958917F, 0.00130252505F, - 0.0118999649F, 0.0185270682F, -0.00175640872F, -0.0055235615F, - -0.000651780574F, -0.000771762454F, 0.00867938F, 0.00431692321F, 0.0148492F, - -0.00708781229F, 0.00827928353F, 0.00575883873F, -0.00284069497F, - 0.00820704643F, -0.00936524663F, -0.00248722034F, 0.00166220963F, - -0.0241133552F, -0.00234519318F, -0.0101747736F, -0.000244493451F, - 0.00664888648F, -0.0270693563F, 0.000865924812F, -0.00302766729F, - 0.00342877326F, -0.000265927927F, 0.0185648724F, -0.00813671388F, - 0.00939987227F, -0.0088765F, -0.0120260147F, -0.00920467637F, 0.00369374873F, - -0.00250499789F, 0.0132084303F, -0.00483745476F, 0.0042799497F, - -0.00157306821F, -0.00226008613F, -0.00222412031F, -0.00109418831F, - -0.000606588961F, 0.0287665203F, -0.00289564207F, 0.00923024211F, - 0.0026933481F, -0.0202500224F, -0.015709199F, -0.00334028387F, - 0.000183020951F, 0.00130916107F, -0.00808526855F, -0.0076793842F, - 0.00752401957F, 0.00548028899F, -0.00308339437F, -0.0109557966F, - 0.00747493375F, 0.0096657807F, 0.00146599149F, -0.00488004228F, - 0.00693546282F, 0.0126333339F, 0.0137846172F, -0.0111616924F, 0.00262165559F, - -0.020480847F, 0.00509782974F, 0.0131596476F, -0.00820560288F, -0.013383192F, - 0.00302704354F, 0.00852540508F, 0.00243154052F, -0.00426667184F, - 0.00571762538F, 0.00298842741F, 0.00863290206F, -0.00675659813F, - 0.00718831038F, -0.0101994583F, 0.0126106283F, -0.0114159947F, - -0.00218130741F, -0.00883557834F, 0.00224367506F, -0.00633892277F, - -0.014823732F, -0.00101637747F, -0.00286703696F, 0.0148585429F, - 0.00103231333F, 0.00022965773F, -0.00937283598F, 0.00229139836F, - 0.0308491308F, -0.0180227477F, 0.00182968727F, -0.00582955079F, - 0.00751657085F, -0.00222157757F, -0.0136667527F, 0.00269598654F, - -0.00530075189F, -0.0203375462F, 0.00846168771F, -0.00121340877F, - 0.0104061114F, 0.000194807682F, 0.00917111803F, 0.00358525082F, - -0.00285852561F, -0.00875807181F, -0.0076132305F, 0.00396045903F, - 0.000819647743F, -0.0143184597F, -0.00762896892F, 0.00140923751F, - -0.00976036116F, -0.00309048058F, -0.00781420525F, 0.00759018818F, - 0.0064892075F, 0.00968437F, -0.00996215176F, -0.00821469165F, - -0.00180083176F, 0.00445382437F, 0.0182528533F, 0.00220874883F, - 0.00451479061F, 0.011303951F, -0.00427284557F, 0.0249921493F, 0.00530516822F, - 0.00152318925F, 0.00295624835F, 0.00432440965F, -0.00109767925F, - -0.00529264193F, -0.00414617406F, -0.00288411323F, -0.00457671238F, - 0.00439119153F, 0.00278169662F, -0.00153607514F, 0.00631248718F, - -0.00319328532F, -0.000649969559F, -0.0113536669F, -0.00424959278F, - -0.0196538195F, 0.0138799772F, -0.0208921451F, -0.0022247273F, - -0.00274866656F, -0.0023553127F, 0.00271138293F, 0.00714577362F, - -0.00203588395F, 0.0132113863F, -0.00521917082F, 0.00121493323F, - 0.00495387288F, 0.00125555729F, -0.00662552379F, 0.00734069664F, - 0.0161913484F, -0.0043027075F, -0.00563100493F, -0.00224045105F, - -0.00806452F, 0.0106271766F, 0.00204587751F, 0.00115502405F, 0.00755873183F, - 0.00985326245F, 0.0123286825F, 0.0139593948F, 0.0200097449F, 0.00230497238F, - 0.00577284815F, 0.00538904546F, 0.00756786112F, -0.00201564282F, - 0.00575805083F, 0.0125205787F, -0.0177389719F, 0.0016788817F, - -0.00656030048F, 0.00649343943F, -0.0118825855F, 7.53031418E-5F, - -0.00744010648F, -0.00519888103F, -0.00849631894F, 0.00895124208F, - 0.00961073581F, -0.00459308038F, -0.0156258736F, 0.00780275138F, - -0.000972725044F, 0.00489174156F, -0.00637259707F, -0.00780960778F, - -0.00791170914F, -0.00684119435F, 0.00375706237F, 0.00996860396F, - 0.0147681478F, 0.00127666118F, 0.00737558305F, -0.00475142617F, - 0.0126199583F, -0.00230174023F, 0.00623515202F, -5.43299939E-5F, - 0.00253817905F, 0.00710704084F, -0.0235857237F, 0.0145028662F, 0.0004976255F, - -0.00467883516F, 0.0122035025F, -0.00987368915F, -0.0146992719F, - 0.015837241F, 0.0138640096F, 0.00711081084F, -0.00357848709F, - -0.00409955485F, 0.00964207947F, -0.0124714449F, -3.16440346E-5F, - -0.00366075244F, -0.00963200442F, 0.00625099661F, -0.0089607F, - -0.0106688552F, 0.0232962649F, -0.00497766631F, 0.00628114818F, - 0.000507526915F, -0.0158391111F, -0.00241462071F, -0.011039828F, - -0.00234593893F, -0.00157863542F, -0.0121442173F, -0.0243678335F, - 0.00898853689F, 0.00540696317F, 0.00785624608F, 0.000609947077F, - -0.00818063598F, 0.00559970737F, 0.00152955367F, 0.00544590689F, - -0.00402111607F, 0.00696100807F, 0.00545821525F, 0.0044078636F, - 0.0034453806F, -0.0217902958F, 0.0135636181F, 0.00835439563F, - -0.00180981657F, 0.000264862116F, 0.00159290421F, 0.0119718974F, - -0.0028386151F, -0.0152254859F, -0.0199967679F, 0.000542538357F, - 0.00993639883F, -0.00978017319F, 3.98125121E-5F, 0.00577167934F, - 0.00461397879F, 0.00729612261F, 0.0040468378F, 0.00350788166F, - -0.00145403133F, 0.00530941412F, -0.0072151362F, -0.00510750478F, - 0.000488955935F, 0.0114097474F, -0.00610145042F, -0.00315586338F, - -0.000736010901F, 0.00324956304F, -0.00452348124F, 0.0110411551F, - -0.00231255568F, 0.00363996346F, 0.00770349F, 0.00346507132F, 0.00296456437F, - -0.00395143265F, 0.0147043131F, 0.0098792715F, -0.0180290509F, - -0.00712593133F, -0.00394274294F, -0.00900735334F, -0.00609743688F, - -0.00711611F, -0.000555051141F, 0.00629805F, 0.0111005064F, 0.00875064917F, - -0.0106328F, 0.00186354457F, -0.00636650622F, 0.00869390555F, 0.00542957103F, - 0.0143559827F, -0.0123293744F, -0.0174613465F, -0.00778315961F, - 0.000182962322F, 0.0122034634F, -0.00764410524F, 0.000246728159F, - -0.00662574498F, -0.01579744F, 0.00590585591F, -0.00605601F, 0.00574942678F, - 0.000487739278F, -0.025673788F, 0.00446085399F, 0.0106064156F, - -0.0121526597F, 0.00286288559F, -0.00399585115F, 0.0199698899F, - 0.00642783614F, 0.00460700411F, -0.00380806345F, -0.00982981734F, - -0.00587072037F, 0.012613263F, -0.00493741781F, -0.00815004855F, - 0.00902367383F, -0.00308850501F, -0.0135130649F, 0.00801101048F, - 0.00309003424F, -0.0070536579F, -0.00485091098F, -0.0104245581F, - 0.00109547575F, -0.0136241186F, 0.00713401288F, -0.00730961468F, - 0.00296824076F, -0.00879711192F, -0.00315969344F, 0.00743297394F, - -0.0109122247F, 0.0114514828F, -0.0155973257F, 0.0212268326F, 0.00476595247F, - -0.0054337536F, -0.0179056469F, 0.00384893292F, -0.00169523142F, - 0.000884958543F, -0.00956773944F, 0.010667407F, 0.00546749542F, - -0.0287401583F, -0.0106210252F, 0.0140199782F, 0.0136193689F, -0.0136705646F, - -0.0184355602F, 0.00339824543F, 0.00156087289F, 0.00774790486F, - -0.00979829952F, -0.0156728458F, -0.0170805808F, 0.00858500786F, - -0.0144976638F, 0.00658507971F, 0.0166470166F, -0.00309864385F, -0.0122054F, - 0.0125197908F, 0.00266986527F, 0.00325052836F, 0.00665897178F, - 0.00405164436F, -0.000889393617F, 0.00892580207F, -0.0116535621F, - -0.0109778354F, 0.00243071048F, 0.0122112604F, 0.000436364673F, - -0.0141785648F, -0.0013975671F, -0.0108540738F, -0.00366014591F, - 0.00574723026F, -0.00162507698F, -0.00149689324F, -0.00710084056F, - 0.00647112029F, 0.0175621863F, 0.00892597809F, 0.0077951909F, 0.0242246725F, - -0.00921784062F, -0.00579724275F, -0.00971385371F, -0.00282595F, - -0.00484548323F, 0.0134720216F, 0.011878646F, -0.00989298336F, - 0.00479368633F, 0.00867333F, 0.0212031361F, -0.0132559491F, 0.0139468974F, - -0.0132208616F, 0.0156024415F, 0.00687103067F, -0.0118483873F, - -0.00392614631F, -0.0002338372F, 0.00849385F, 0.0278541F, 0.0118685747F, - 0.001654875F, -0.00222460623F, 0.00681595318F, 0.00383831956F, - 0.00328186573F, -0.000378546742F, 0.00992051233F, 0.0120655978F, - 0.000732186F, -0.0247849F, -0.0106656346F, 7.33802735E-5F, 0.00277474662F, - 0.0176579673F, 0.00659266394F, -0.00151380408F, -0.0124666383F, - -0.00913956482F, 0.0118654985F, -0.0221241321F, -0.00387598039F, - -0.00864918F, 0.0150066037F, -0.00139436114F, -0.00449954206F, - -0.0168578923F, 0.00426801387F, 0.00975274108F, 0.0107141677F, - -0.00288932328F, -0.00945763942F, -0.00725834211F, -0.00418592663F, - 0.00274424907F, 0.0172980931F, 0.00938364F, -0.00966185331F, 0.0180536658F, - -0.00874273386F, -0.0101476284F, 0.0231786855F, 0.0176922809F, - 0.00996190216F, -0.003578217F, -0.0061707953F, -0.0057803113F, - -0.00369025837F, -0.0078840442F, 0.0107310284F, -0.00282790256F, - 0.00484208856F, -0.0120248375F, -0.000141898432F, -0.00390071305F, - 0.00375897018F, 0.00417381153F, -0.00638849521F, 0.00732101407F, - -0.00136353285F, -0.000703252212F, -0.00274251471F, -0.014648173F, - 0.0004941019F, -0.00272959634F, -0.002577062F, -0.0016060262F, - -0.0123451548F, -0.00755990809F, -0.00563252671F, -0.00410121307F, - -0.0116621424F, -0.000160699477F, -0.0223720539F, 0.00585501082F, - -0.00646275328F, -0.00348371454F, 0.00827079918F, -0.00137147401F, - -1.03515995E-5F, -0.021060504F, 0.00855001621F, 0.00524137774F, - 0.0141180409F, 0.0164631512F, -0.00817852188F, -0.00251711486F, - 0.000229716054F, -0.00259842933F, 0.00270845788F, -0.00218560337F, - 0.00858097151F, 0.0104553048F, 0.00363026F, -0.0025153649F, 0.0089570377F, - -0.00408044038F, -0.00805836357F, -0.00826170947F, -0.00196203426F, - 0.0133246351F, 0.00519353291F, 0.00720321899F, -0.0042518517F, - 0.00449821074F, -0.00717044296F, -0.00514587F, 0.00669198763F, - 0.000461898191F, -0.0104281763F, -0.0101558045F, 0.0115395058F, - -0.00452936813F, -0.00103587017F, -0.00315006892F, -0.0143370703F, - -0.000873853685F, 0.0105601037F, 0.0149948616F, 0.000888352282F, - 0.00848323852F, 0.00386921479F, 0.000792335137F, 0.0105195604F, - 0.00399222085F, -0.0152558377F, 0.0123050241F, -0.00442318758F, - 0.00174531026F, 0.017297687F, 0.0137113454F, 0.00389193115F, 0.00549549097F, - -0.00398042F, 0.0105788717F, 0.00868664496F, -0.0184896924F, -0.00194930751F, - -0.00183150615F, 0.00339986477F, 0.0155017814F, 0.00831070263F, - 0.00112762861F, -0.00902558118F, 0.00539287413F, 0.0107722096F, - 0.00320066838F, -0.0014696354F, 0.00976186711F, -0.0106329555F, - -0.00244412618F, 0.0131280543F, -0.00622028904F, 0.00935245398F, 0.00512918F, - -0.00120218156F, 0.00821855385F, -0.0106219901F, -0.0061282008F, - 0.00442063622F, 0.0142867658F, 0.000210460916F, -0.00944257714F, - 0.0117712049F, -0.0134586776F, 0.00558032654F, 0.0114779538F, - -0.00267901272F, -0.00523893349F, 0.00723521085F, -0.0120575987F, - -0.00116085389F, -0.00469363667F, 0.0152421473F, -0.0134079969F, - 0.00380709535F, 0.0144727407F, -0.00433893269F, 0.00970728F, -0.00632594945F, - -0.00175160554F, -0.00593485497F, 0.0111561054F, -0.0140439346F, - -0.00716218492F, -0.0177518986F, -0.0130586093F, 0.0117663741F, - -0.00509867631F, 0.0146241868F, -0.013919902F, -0.00111821806F, - -0.00810381863F, -0.000164803423F, 0.0207000189F, -0.0224575829F, - 0.00175400241F, -0.00307738921F, -0.0024915284F, 0.00452665146F, - -0.0138422055F, 0.0156319458F, 0.00876971241F, 0.010258209F, 0.0140635082F, - -0.00289031374F, 0.00399128627F, 0.00139544881F, 0.00772985676F, - -0.00729542738F, -0.00343727507F, -0.00597895402F, 0.0128612481F, - -0.000938160636F, -0.0027170442F, -0.00217388081F, -0.00524704065F, - -0.000397402939F, 0.00385056389F, 0.0136972032F, -0.0145006245F, - 0.0178716891F, -0.00276024314F, 0.00434085121F, -0.0167857595F, - -0.0166810416F, 0.00531713897F, -0.000532173843F, 0.00162972382F, - -0.0163573232F, -0.000427574763F, 0.00042887227F, 0.00957007147F, - -0.00918851F, 0.0220431983F, -0.0135682551F, 0.00540400529F, -0.00534443185F, - -0.00685000559F, -0.00558058638F, 0.00832713116F, -0.00209112139F, - -0.00134364318F, 0.00846021716F, 0.00911284517F, -0.00379828503F, - 0.00767536415F, 0.00246994919F, -0.00269353832F, -0.0112883784F, - 0.0148318782F, -0.00809874572F, -0.0043756F, -0.00243599131F, -0.0225545801F, - -0.000765747507F, -0.00582318613F, -0.0191770904F, 0.000597921899F, - -0.0244338028F, 0.0122160604F, -0.0241281055F, -0.00916020945F, - 0.00741781667F, 0.00622532656F, -0.00539050531F, 0.0126429945F, - 0.000291603908F, 0.0068666474F, 0.00500013772F, 0.00894226693F, - -0.00566371251F, -0.000369159563F, 0.0191462189F, -0.00303577026F, - 0.00160665484F, -0.00257748435F, -0.0080710724F, 0.000334364449F, - -0.0136994487F, 0.0240179356F, 0.0084466422F, -0.00693107909F, - -0.0165850874F, -0.0166160464F, -0.00883974414F, 0.0055152881F, - -0.00405833963F, 0.0010410354F, 0.00916933175F, 0.0110356128F, - -0.00520178583F, 0.00718087144F, -0.01297554F, -0.013284307F, - -0.00316246692F, -0.000559739885F, -0.011171639F, -0.00254502427F, - -0.00660137F, -0.00215839688F, -0.00835232809F, 0.00492059672F, - -0.00796860829F, 0.00114004419F, -0.00898981094F, 0.0152105875F, - 0.0158806667F, -0.00489053316F, -0.0150139369F, 0.00187543523F, - -0.00735143945F, 0.0194401275F, 0.000500242284F, -0.00378556852F, - 0.00773303164F, 0.00533947721F, -0.0137842782F, 0.0113354856F, - 0.00508969231F, -0.0256632287F, 0.000689912064F, -0.00699215289F, - 0.0114082191F, 0.0246441867F, -0.000351198571F, -0.00356690842F, - 0.00592814153F, -0.00615402125F, 0.0116653498F, -0.00853917189F, - 0.0176354889F, 0.00387403066F, 0.00649035862F, 0.00400006119F, - 0.00547122862F, -0.00417871028F, 0.00459891278F, 0.015054821F, - -0.0016505688F, -0.0159545541F, 0.00566156302F, 0.00130479189F, - 0.0109417634F, -0.000333964679F, -0.00210879836F, 0.00205555372F, - -0.0063353749F, 0.00569211785F, -0.00431893812F, -0.00295903068F, - -0.0143765453F, -0.0134844175F, -0.00103031471F, 0.00291006546F, - 0.00330903148F, -0.00891345832F, 0.00696386071F, -0.00137663179F, - -0.00227498589F, 0.00035058F, 0.00545525504F, -0.0120773083F, 0.00205254089F, - -0.0162954088F, -0.00776646612F, 0.00211308338F, -0.00589624792F, - 0.00170598F, 0.00780293671F, -0.00709762285F, 0.0117894663F, 0.00366754364F, - 0.00364433322F, 0.0168617163F, -0.0041565923F, 0.0141566517F, - -0.00760629866F, 0.0138573777F, -0.0209774058F, -0.0145820873F, - 1.48847475E-5F, -0.00550917583F, 0.00379915559F, -0.00932137854F, - 0.0111175468F, 0.00492202118F, -0.00447389763F, -0.00859452784F, - 0.000231882266F, -0.00406473363F, -0.00505229458F, -0.00266848644F, - -0.00213415781F, -0.00480025401F, 0.00293965056F, 0.0176382735F, - 0.00967665669F, -0.00386189832F, -0.000410187873F, -0.00225309166F, - -0.00460092584F, -0.0195427835F, 0.000928587397F, -0.00664211344F, - -0.0141127827F, 0.000278994092F, -0.0145570608F, -0.00694386335F, - 0.0153728649F, -0.00123870478F, -0.00341483648F, 0.0163759366F, - -0.00667150691F, 0.023450898F, 0.00105812331F, -0.0308572073F, - -7.66361554E-5F, -0.00124627247F, 0.00549253309F, 0.00301415799F, - -0.0141618345F, 0.0131043941F, 0.0115879085F, 0.00420197099F, - -0.00332164485F, -0.00670623733F, 0.00394138601F, -0.01304706F, 0.00225964F, - 0.00188040582F, 0.00674908562F, 0.00611548778F, 0.00642908458F, - 0.00322480779F, 0.011186216F, 0.00121825049F, 0.00348261814F, 0.00268803234F, - -0.00961619336F, 0.00880910456F, 0.0106653394F, -0.00923575554F, - 0.0137945311F, 0.0206228327F, 0.00574715948F, 0.0190611891F, 0.00179936248F, - -0.0118364701F, 0.0310732648F, 0.00928799156F, 0.0175468288F, -0.0212018117F, - -0.000844413764F, 0.00579963252F, -0.000701653422F, 0.00663292827F, - -0.0198227558F, 0.00704621105F, 0.0144643635F, -0.00317248353F, - -0.00207220227F, -0.00445293868F, -0.00400130404F, 0.00268110726F, - -0.00935316F, -0.00366323628F, -0.00358875119F, 0.00635639392F, - -0.00279506156F, 0.00218369556F, 0.00428427244F, -0.0145584391F, - 0.0140258251F, 0.00487222895F, 0.0105367526F, -0.0152274193F, 0.00284587289F, - -0.00382768805F, 0.0053662F, 0.0069386973F, -0.000942881336F, 0.00751101319F, - -0.000852858822F, -0.00376383355F, 0.00163995451F, -0.00484472699F, - -0.0097888913F, -0.00362236984F, -0.01276075F, -0.00191985804F, - -0.011024964F, -0.00214828062F, -0.00690413639F, -0.021336006F, - 0.00474658748F, 0.017153319F, -0.000948888599F, -0.0107863285F, - -0.00312460866F, -0.00313380733F, 0.00318876118F, 0.00751032401F, - -0.0106395138F, -0.00820808951F, 0.00325860712F, -0.00482295407F, - 0.0041511422F, -0.00744166272F, 0.0047125658F, -0.010839452F, 0.00375991897F, - -0.0155164655F, -0.00222205068F, 0.00815602764F, 0.0127121694F, - 0.00473797508F, 0.0209862851F, -0.00858022459F, -0.00210241275F, - 0.00158628181F, 0.0101727853F, 0.000469945691F, 0.00711273728F, - 0.000698667078F, 0.00373147358F, -0.00853668153F, 0.00407767668F, - 0.00359637104F, 0.0127293216F, -0.00423046341F, -0.0167346634F, - -0.000488500053F, 0.00189689442F, 0.00951963663F, -0.00633621821F, - 0.0114405444F, -0.00991512369F, -0.00795094948F, 0.00853706338F, - 0.0389655791F, 0.0136689469F, -0.0143427746F, 0.00900318101F, 0.00291242381F, - -0.0126844309F, 0.00914721843F, -0.00082930486F, 0.00547398068F, - 0.00300873932F, 0.00128555123F, -0.0134154987F, 0.00840416271F, - 0.00558972359F, 0.00348113291F, 0.00241601444F, -0.00908185169F, - 0.00283456221F, 0.00336760445F, -0.00764290197F, 0.00614005793F, - -1.02705662E-5F, -0.00355123449F, 0.00652058376F, -0.00324159791F, - 0.0132839577F, 0.00473278761F, -0.00387263647F, -0.0102793258F, - 0.00695341825F, -0.0004337274F, 0.00651515787F, -0.00621462F, 0.00120072672F, - 0.00406356342F, 0.00279859384F, -0.00453919359F, -0.00922512542F, - 0.000104137449F, 0.0118534174F, 0.0152732758F, -0.00155029155F, - -0.00163800281F, -0.00170813757F, 0.00775604462F, -0.00356551749F, - 0.00635510171F, -0.00572542753F, 0.00391778583F, 0.0174773838F, - -0.0137216104F, 0.00413908297F, 0.00642030826F, 0.00376389013F, - 0.0130150719F, 0.00292648887F, 8.22893853E-5F, 0.00522930874F, 0.0175907407F, - 0.0155017609F, 0.0132795228F, -0.0137473568F, -0.00246458827F, - 0.00426666252F, 0.0135798762F, 0.0129782856F, -0.00211487594F, - 0.00103538274F, 0.00155494502F, 0.0125579545F, -0.0130874664F, - -0.00481005665F, 0.00782785751F, -0.00142215728F, -0.00487948908F, - -0.0149992267F, -0.0048241117F, -0.00776319951F, 0.000727100763F, - 0.00419158302F, -0.011842032F, 0.0151683278F, -0.00211313507F, 0.0157500356F, - 0.0124047222F, 0.00359872985F, -0.0112861348F, 0.00193670753F, - -0.0159679502F, -0.00664180843F, -0.00118768692F, 0.00158053159F, - -0.00200979807F, -0.00313148415F, -0.00272384915F, 0.00619605556F, - 0.00432715612F, 0.00520866457F, 0.00549219176F, -0.00092743471F, - -0.0133730257F, -0.0158458166F, 0.0123167336F, -0.00384192285F, - 0.00162151654F, 0.0018690693F, 0.0157811847F, 0.00805209577F, - -0.00533378543F, -0.00759677216F, 4.86744193E-6F, -0.0112658795F, - 0.0180649329F, -0.00596277788F, -0.0120436931F, 0.00372717925F, - -0.0104350252F, -0.0127286343F, -0.0141510582F, 0.0181730874F, - -0.00513415318F, -0.0052484856F, -0.0120695494F, 0.0011590136F, - 0.00177967071F, -0.000349330541F, 0.0151930675F, 0.00548389927F, - -0.00350029883F, -0.00134002254F, -0.00230079703F, 0.00407284591F, - -0.00971948F, 0.000638417143F, -0.00834736787F, 0.00394933112F, - 0.000572108489F, 0.0138560375F, -0.00698165689F, -0.0169178769F, - 0.000582008914F, -0.0111852614F, 0.00548435189F, 0.00211556745F, - 0.00218369346F, -0.00587718282F, 4.22877893E-5F, -0.00767227029F, - -0.0126069291F, -0.00944516901F, 0.0122259157F, 0.00710486807F, - -0.00387269887F, 0.00883456226F, -0.0104104159F, 0.0144379344F, - -0.00423896778F, 0.0125318216F, -0.0123960264F, 0.00603521476F, - -0.0151303764F, -0.00154559279F, 0.00824225415F, 0.00315661705F, - -0.0225264989F, 0.0200040676F, 0.0121912118F, -0.00821946189F, - -0.0145681556F, 0.0116983373F, 0.0103067104F, -0.00208681542F, - -0.00821966399F, -0.0176398885F, 0.00636751531F, 0.00213181F, - -0.00570953684F, -0.00717215938F, -0.00162842579F, -0.00120306469F, - 0.00525310822F, -0.00335260131F, 0.000473820779F, 0.00792435464F, - -0.0046773185F, 0.00880181137F, 0.027912762F, -0.00266062166F, - 0.000963331317F, 0.00483930763F, -0.00323339179F, 0.00816712435F, - 0.0076167942F, -0.00954332296F, 0.00540084671F, -0.000164901532F, - -0.00644898182F, -0.00324857212F, 0.0114701241F, -0.0155257806F, - -0.0104926508F, -0.00525269751F, -0.0180767532F, -0.00311337574F, - 0.00473585399F, -0.0106012141F, 0.00511568785F, 0.008058751F, - -0.00490552466F, 0.00308967289F, -0.010568128F, -0.00571173197F, - -0.00525944447F, 0.0111257145F, 0.00109138538F, -0.0117097124F, - -0.00255836477F, 0.0210762229F, -0.0171362832F, -0.000453286746F, - -0.0300285891F, -0.00756303687F, 0.00798011478F, -0.00424032239F, - -0.00805222429F, 0.000518485F, 0.00177871343F, -0.0196196306F, - -0.0162208118F, 0.00769469049F, 0.0125760511F, -0.00547939073F, 0.01306553F, - -0.000327142654F, 0.00626846636F, 0.0155597525F, -0.00253522513F, - -0.000551588659F, -0.0139322449F, -0.00535829691F, -0.00218953495F, - -0.0113830762F, 0.00311185652F, -0.00148114201F, -0.00973450579F, - 0.000102103011F, -0.00595592F, 0.0160402823F, -0.000288881914F, - 0.00483463146F, 0.0248848982F, 0.0182354022F, 0.021290319F, -0.00276907277F, - -0.0182399563F, -0.00223604869F, 0.00217250083F, -0.0194646623F, - -0.0118330559F, 0.0055391104F, 0.00302418228F, -0.00637681363F, - -0.0173037983F, -0.00235834206F, 0.0029846949F, -0.000517386361F, - -0.00227501849F, 0.0101176454F, 0.00595567096F, -0.015897518F, - 0.00196765945F, 0.00232513295F, 0.00615416234F, -0.00875890907F, - 0.00244586985F, -0.0181289855F, 0.00796413422F, 0.00197989121F, - 0.00360605237F, -0.0081564784F, -0.00392552139F, 0.0026991514F, - -0.00516834203F, 0.00620251428F, -0.00141003134F, -0.00195995439F, - 0.00770840095F, 0.00293053244F, 0.000391124107F, -0.000857492094F, - 0.0132574411F, 0.00402933266F, 0.00858011749F, -0.00270041218F, - -0.00595925516F, -0.000524305447F, 0.00554990349F, 0.00295628444F, - 0.00445701182F, 0.0191970915F, -0.0105596762F, 0.00311318249F, - -0.00689468859F, 0.00385255553F, 0.00601177057F, -0.00518806837F, - 0.00812005438F, 0.00016958674F, -0.00167224393F, -0.00325466436F, - -0.000465687859F, -0.00986265112F, 0.000697755895F, 0.00356887118F, - -0.00498062093F, -0.00749617629F, -0.0035267577F, 0.00993317831F, - -0.0196086131F, -0.0121858185F, -0.0083729066F, -0.0129992934F, - 0.00733442139F, 0.0035447462F, -0.00675104558F, 0.00231797458F, - 0.00345232477F, 0.0233824011F, 0.000267550495F, -0.00652810745F, - -0.00614051474F, 0.0102088479F, -0.00151642074F, -0.00819833856F, - -0.0145962397F, 0.0062250779F, 0.0105104875F, -0.00157906068F, - 0.00030398462F, -0.0065382719F, -0.0131539991F, -0.0126891723F, - 0.0100570684F, -0.0111004412F, 0.0196880214F, 0.00929221418F, 0.0110843312F, - 0.0105338264F, 0.00918913074F, 0.0123775704F, 0.00345557509F, 0.0108897584F, - 0.000672933F, -0.00466373051F, 0.0108063007F, -0.00432533724F, - -0.0221109651F, 0.00989219919F, 0.00682534277F, 0.00141459168F, - -0.00339364866F, 0.00162611715F, -0.00465529645F, 0.00638920302F, - 0.0107953036F, -0.00116860354F, 0.00318714371F, 0.00982409157F, - 0.0115468316F, -0.0107911332F, -0.00716166431F, 0.0181316938F, - -0.000472741958F, -0.0121994214F, -0.00720004598F, -0.00663598441F, - -0.0168201346F, 0.0172553528F, 0.00680585532F, -0.0129610794F, 0.0113326088F, - -0.00646110252F, -0.00202541566F, 0.0114030326F, -0.0105801299F, - -0.00750865275F, -0.00379176275F, -0.00124429341F, 0.0015208117F, - -0.00524167204F, -0.0144970044F, -0.00638909498F, -0.00543501973F, - 0.00112299027F, 0.0110808834F, 0.0014360284F, 0.00474335533F, - -0.000792114122F, -0.00167539751F, -0.0171967857F, 0.00422095694F, - 0.0143676698F, -0.00510931667F, -0.0044035418F, -0.0129202092F, - 0.00587162049F, 0.002478929F, -0.000358949F, -0.00275196391F, - 0.000258419488F, -0.000393454015F, 0.0010097475F, 0.00348906545F, - 0.00968003366F, -0.0182480309F, 0.0114443647F, -0.0130054113F, 0.0112021714F, - -0.00934096333F, 0.00946411304F, 0.0151738282F, -0.00482316501F, - -0.00458735F, -0.00743225636F, 0.000178612157F, -0.00474760961F, - 0.0056065903F, -0.0153095769F, 0.00741863856F, 0.00637752702F, - 0.00912102684F, -0.00481130043F, -0.0168448519F, 0.00808165874F, - -0.000847853953F, -0.00742506562F, 0.0125405956F, 0.0169640314F, - -0.00680880435F, -0.00553294457F, 0.00500324741F, 0.00124444172F, - 0.00576959876F, 0.00633180561F, -0.00296138041F, -0.0129391635F, - 0.00428689271F, -0.00788488518F, 0.00741848815F, -0.000637895486F, - -0.0172821861F, -0.0116468333F, -0.00242096628F, 0.0118141826F, - -0.00287382561F, 0.0020174107F, 0.00487620663F, -0.000681803387F, - 0.0130635127F, -0.012870078F, -0.00726421876F, 0.0087057678F, -0.0123758456F, - 0.0136480704F, 0.027230585F, -0.0173521824F, -0.00122077088F, -0.0112130037F, - -0.0159480609F, -0.00601457106F, -0.00881456118F, -0.00589895574F, - -0.00386302243F, -0.0118545284F, 0.00825474F, 0.00250551151F, -0.0110400114F, - -0.00777809648F, -0.00443985639F, -0.000726388418F, 0.0135308066F, - -0.00991805F, -0.00204355386F, 0.00223125867F, -0.00598592497F, - -0.00474022934F, -0.00719607295F, 0.0167287719F, 0.00420095213F, - 0.00176797097F, 0.00480528921F, 0.0139525468F, 0.00921054371F, - -0.00557197025F, 3.51287854E-5F, -0.00740048336F, 0.00476179458F, - -0.0103026042F, -0.00484391116F, 0.00194817421F, 0.00462509226F, - -0.00888783485F, 0.00881826505F, -0.00278703985F, -0.00569971045F, - -0.00471758796F, 0.0113721332F, -0.00421329774F, 0.0122622075F, - -0.0113231186F, -0.00272054155F, 0.00372964283F, -0.00364027382F, - -0.00576561503F, 0.0111467075F, -0.000906975532F, -0.0057825176F, - 0.0148844253F, -0.000924055348F, -0.00684783189F, -0.000612437143F, - -0.00497220503F, 0.0101547614F, -0.00618925504F, 0.0077985758F, - -0.0131884469F, 0.00245299912F, -0.0123941302F, -0.00196684361F, - -0.000985791441F, 0.0114227664F, 0.0122019993F, 0.00161531661F, - 0.00407503406F, 0.00058560638F, 0.0113939717F, 0.0146092512F, - -0.00869352277F, 0.00472954288F, 0.0127940318F, 0.0010912416F, - 0.000118238902F, 0.00609651254F, -0.00350742764F, 0.00351965451F, - -0.00240270793F, 0.0056355847F, 0.00186355517F, -0.0102692097F, - -0.00189078937F, 0.0194416605F, 0.00525746122F, 0.00601420738F, - -0.00570397452F, 0.0137006221F, -0.0204343442F, 0.00804810692F, - 0.0077587394F, 0.00232706475F, -0.00877318718F, -0.0112949312F, - -0.0110205188F, -0.0164240599F, 0.0066929888F, 0.00929391384F, - -0.00144495594F, 0.000906372385F, 0.015428504F, 0.0107370894F, - -0.00227719382F, 0.00444448926F, 0.00199372973F, 0.00763949193F, 0.0171525F, - 0.0028508273F, -0.0162821766F, 0.0188025683F, -0.00696644513F, - -0.0114733325F, 0.00211905618F, -0.00544591062F, 0.0115737543F, - -0.00232862518F, -0.0151530355F, -0.0080424F, -0.0100504411F, - -0.00512022339F, 0.0144393593F, 0.00242891605F, -0.0117361415F, - 0.00132520613F, -0.00277081737F, 0.00195413665F, 0.00662226137F, - 0.00318345847F, -0.00408452563F, -0.000917147845F, -0.0080370754F, - 0.0127355782F, 0.0141925663F, -0.0127308099F, -0.00578432716F, - 0.00140436704F, -0.00797125231F, 0.0063271434F, -0.0137796625F, 0.011901116F, - -0.00670567201F, -0.027971277F, 0.00639176881F, 0.0134460861F, 0.017376259F, - -0.00179556687F, -0.00155774853F, 0.0127034411F, -0.00191080302F, - 0.00334346201F, 0.0110853389F, -0.00899981894F, 0.0112184435F, - 0.00778828748F, -0.00573136192F, 0.00402200501F, -0.0236843452F, - -0.00366105349F, 0.0106694894F, 0.00081103394F, -0.0174830686F, - 0.00743006775F, -0.0133572239F, 0.00681689056F, -0.023978848F, - -0.00829703361F, -0.000850366F, -0.00435511069F, -0.0127100945F, - 0.0122939562F, 0.013957398F, -0.00912338216F, 0.0224411376F, -0.0192671567F, - -0.0109175192F, 0.00719409296F, 0.0214593243F, -0.0068917023F, - -0.00776773598F, -0.00901300181F, 0.00210323301F, 0.00927669462F, - -0.0108967163F, 0.00913666934F, 0.00537221F, 0.00870134F, 0.000838093692F, - -0.00120286655F, 0.0072248634F, -5.58739412E-5F, -0.0131152095F, - -0.00339820888F, 0.00047828845F, -0.00659169303F, 0.00440562889F, - -0.00461368216F, -0.00900442F, -0.000896815618F, 0.00739430077F, - 0.0132848583F, 0.00475834915F, -0.00249851053F, -0.00621563429F, - 0.00693583675F, 0.00601564F, 0.0217327829F, 0.0124208247F, 0.00326938392F, - 0.0178889222F, -0.000958965567F, 0.00410784595F, -0.00397057133F, - 0.000985224731F, 0.00985835399F, 0.00887062121F, -0.00390774664F, - 0.0190524533F, -0.00507649593F, 0.00208414532F, 0.00155122357F, - 0.0160597824F, 0.00955417287F, -0.000938524143F, 0.00703915162F, 0.0148482F, - -0.00102291699F, 0.00620633271F, -0.00283904653F, 0.0154795777F, - -0.000568709162F, -0.00923233F, -0.0178666F, -0.0128508024F, 0.000381147547F, - 0.00265584653F, -0.000649420661F, 0.00492453249F, -0.00519138761F, - -0.0119354818F, -0.00629825238F, 0.0234874282F, -0.00934597664F, - -0.0125761339F, -0.00293385936F, -0.0160471201F, -0.00152157375F, - 0.0092283329F, -0.0321088098F, -0.0103351856F, 0.00572567759F, - 0.00067411043F, -0.00543055497F, 0.00397020159F, 0.0163959302F, - -0.00918523595F, 0.0299922917F, -0.00647427188F, -0.00119782949F, - 0.0167094693F, 0.00418461207F, -0.00364616839F, 0.00467007281F, - 0.0136781484F, -0.0120410286F, 0.00494005671F, 0.000712852343F, - 0.00297969696F, -0.00660395203F, -0.00867998227F, -0.000406906154F, - -0.00377785298F, 0.00587249547F, 0.0134317614F, -0.00519561F, -0.0144162783F, - 0.00103374117F, -0.00115654024F, -0.00522332F, 0.00400968222F, - -0.00173004274F, -0.0140090343F, 0.0129897511F, 0.00733522931F, - 0.0240065828F, -0.00389260333F, -0.0196500234F, 0.00252896291F, - -0.00230592536F, -0.0064198F, -0.000709387299F, -0.0104627293F, - 0.00080444233F, 0.0264797639F, 0.000487564306F, 0.000966385473F, - -0.00330868829F, 0.00349930883F, -0.00840669125F, -0.00167803443F, - 0.0105274F, -0.0045505506F, -0.00210936065F, -0.00842359755F, 0.00437616091F, - 0.0041056755F, 0.0169870444F, -0.00671876594F, 0.00600612722F, - 0.00345642981F, -0.011714614F, -0.00648410711F, -0.0229014959F, - -0.000375038246F, 0.000767656078F, -0.0114918752F, -0.0166391898F, - -0.0023655314F, 0.00298227649F, -0.00724273F, 0.00174489769F, 0.0115050171F, - -0.00161632511F, 0.00498945126F, 0.018087361F, 0.00687254453F, - 0.00110254844F, -0.00421057F, 0.00503846398F, -0.0115035661F, - -0.00697974069F, 0.00940381922F, 0.00735726F, 0.0215279236F, -0.0163564049F, - -0.0015701605F, -0.00258231303F, -0.00686122803F, -0.000746992533F, - -0.000737346651F, 0.00562499696F, -0.01030776F, 0.0111858845F, - 0.00577367377F, 0.00364651531F, -0.00883105677F, -0.00159118616F, - 0.00289239874F, 0.000825458439F, -0.00622548116F, 0.00403831108F, - -0.00557353674F, -0.0122830449F, 0.00891698617F, 0.00627406687F, - -0.0050811409F, 0.000310142204F, 0.00525527541F, 0.00224256958F, - -0.0022456476F, 0.00394314F, 0.00333257392F, 0.00214854022F, 0.00776587892F, - 0.0183096621F, -0.00472138729F, -0.00100395095F, 0.0126899844F, - 0.00109972269F, 0.00627136976F, 0.0172677673F, 0.00316484086F, 3.4185472E-5F, - 0.00239996286F, 0.00671694102F, 0.0118838847F, -0.00221662666F, 0.021305209F, - 0.00176432892F, 0.00774609763F, 0.00167837518F, -0.0105387401F, - -0.00807107612F, -0.010099045F, 0.00450271834F, -0.00124482624F, - -0.0053782789F, 0.00424794527F, 0.00596117F, 0.0047229575F, 0.00624157721F, - -0.00373309152F, -0.000228582896F, 0.00266688503F, -0.0101314262F, - -0.00321222027F, -0.00312104053F, -0.00772901904F, -0.000467861973F, - -0.00211049453F, 0.00310985371F, 0.0161591191F, -0.0015809919F, - -0.018393863F, -0.000977474265F, 0.0189044569F, 0.00626669638F, - 0.0127224615F, 0.00781647302F, -0.0195517689F, 0.00463426439F, - -0.0128321126F, -0.000775112596F, -0.00616823556F, 0.00521081733F, - -0.0155439293F, 0.00759788975F, 0.010025369F, -0.00352051831F, 0.0121056503F, - -0.00534663443F, 0.0232679136F, -0.00728442613F, -0.000787599827F, - -0.0184475202F, -0.00123469857F, -0.00662699854F, -0.0072124172F, - 0.0235716682F, 0.0191194061F, 0.0194645431F, -0.00705852872F, - -0.00122754485F, 0.00826054F, 0.00483837537F, 0.00415479625F, -0.0062932712F, - 0.00450671371F, 0.00404396048F, 0.00741764251F, 0.00771437213F, - 0.0118811429F, -0.0152232638F, -0.0196997765F, 0.00426145829F, - 0.000527241151F, 0.00480035646F, -0.00243189698F, -0.00530685438F, - -0.00408657314F, -0.00103080587F, 0.00528568681F, -0.00864373F, - -0.0144309094F, -0.00112786947F, -0.00668609608F, 0.00492062047F, - 0.00733068306F, 0.00907008816F, -0.000999824726F, -0.00820580125F, - -0.00606370671F, -0.000466852507F, -0.0198025648F, -0.00222590356F, - -0.00987561233F, 0.0183458775F, -0.0136000309F, -0.0016739053F, - -0.0110126091F, 0.0295984466F, 0.00552467303F, 0.0106449658F, 0.00594838243F, - -0.00101089792F, -0.000730203756F, 0.00664793234F, 0.0111062909F, - -0.000352096773F, 0.0136404959F, 0.00997260306F, -0.0190216F, 0.00429187203F, - -0.00112657715F, 0.000873674056F, 0.00770434737F, -0.00815323275F, - 0.00765129225F, -0.00445314916F, 0.0138514349F, 0.00995736755F, - 0.00339047145F, -0.0117087411F, -0.00958286319F, -0.00540573243F, - 0.00472416077F, 0.00434790784F, 0.00664987927F, 0.00172171835F, 0.016799869F, - -0.00893250853F, -0.00961275119F, 0.0202328283F, -0.0195548125F, - 0.0220665801F, 0.0106397783F, -0.00348398183F, -0.0111146923F, 0.0114308847F, - 0.0150609277F, -0.00741319545F, -0.000800974958F, -0.0122112334F, - 0.00942810718F, 0.00477551948F, 0.00677454F, 0.018066911F, 0.00656801742F, - 0.012461354F, -0.00388866151F, 0.00404586084F, 0.000526814198F, - -0.0137681942F, -0.0128991511F, 0.00284979166F, 0.0148060592F, - -0.000561292109F, 0.0116611812F, 0.00725750206F, 0.00252102455F, - 0.0347308852F, 0.000797589426F, -0.0126111517F, 0.0105394833F, - -0.0068939575F, 0.00652176468F, -0.0198872816F, -0.0128104575F, - 0.00244958675F, 0.00908636209F, 0.00404222263F, -0.0163726658F, - 0.000153085159F, -0.00785621721F, 3.24453649E-5F, 0.00944436248F, - -0.00814072695F, -0.00772530818F, 0.00169963413F, -0.00366216339F, - -0.0110482248F, 0.00659554545F, 0.00512561854F, 0.00188729167F, - -0.0184942894F, -0.00760422274F, -0.00870078895F, -0.0141477706F, - 0.0094405571F, -0.0111968489F, -0.00569674931F, -0.0274336617F, - 0.00359626557F, -0.00762975076F, -0.0238593128F, -0.0155619653F, - -0.00694200583F, 0.015126F, -0.0121420203F, 0.00902766362F, -0.00419972092F, - -0.0053269458F, -0.00848853495F, -0.00709136855F, 0.0152937165F, - -0.0199161619F, 0.00790891424F, -0.0162240565F, 0.00163142662F, - 0.00232947804F, -0.014147005F, -0.0113189649F, -0.00365767628F, - -0.00533692399F, 0.0130521469F, -0.00372932875F, -0.00609354209F, - -0.0157030635F, 0.00116368616F, 0.000247039396F, -0.0191894397F, - -0.0130893551F, 0.00272947317F, 0.00719628483F, -0.0134772975F, - -0.0104853958F, 0.00573915523F, -0.00512757199F, -0.00117783819F, - 0.0142679187F, -0.0140438117F, -0.00550578069F, -0.0159847084F, - -0.0127053224F, -0.00503036845F, 0.00889095198F, 0.00641175965F, - 0.00897216517F, 0.00606762571F, 0.0018469583F, -0.0163508467F, - -0.0158208981F, 0.00646428624F, 0.0113955969F, -0.0169004872F, - -0.00281076482F, -0.00218712143F, -0.00152580813F, 0.0206912346F, - 0.00888423715F, -0.00148090604F, 0.00874758884F, 5.00656206E-5F, - -0.0015849066F, 0.001525047F, -0.00271701533F, 0.0137364808F, - -0.00445610471F, -0.000938964891F, -0.00758670643F, -0.00281512924F, - -0.000826258271F, -0.0191136077F, -0.0123010175F, -0.00108688732F, - 0.0124992635F, -0.00120967801F, -0.00166667893F, 0.0190333705F, - 0.00367891765F, -0.00235006749F, -0.0112031708F, -0.00152155862F, - 0.0104866317F, -0.0015404328F, 0.000362148F, 0.0102394745F, 0.0021377434F, - -0.000722758123F, 0.000148420062F, -0.0115120076F, -0.0043197982F, - -0.0038317095F, -0.00740325404F, -0.0110796178F, -0.016776707F, - -0.00561721111F, -0.000981126679F, -0.00185435021F, -0.00264508603F, - -0.00954499468F, -0.00844511576F, 0.00342909968F, 0.000571215118F, - 0.0299962722F, 0.000880714564F, -0.0114470841F, 0.00551410066F, - 0.00128032337F, 0.000486658304F, -0.0080950791F, -0.0078251455F, - 0.00685460027F, 0.0119370539F, 0.000276347419F, -0.0132715516F, - -0.0162301026F, 0.00457924558F, -0.0155654838F, 0.00315802987F, - -0.0107207894F, -0.0124523193F, -0.00195198506F, 0.00994493812F, - 0.0158264302F, 0.0023427445F, 0.0136256777F, 0.00550941331F, -0.00842086226F, - 0.00306022447F, -0.0169161614F, 0.0202139448F, 0.0102940025F, 0.00167363207F, - -0.0095410645F, 0.00169038086F, 0.0108428448F, 0.00464364188F, - 0.00971081667F, -0.0072955247F, -0.0149215823F, -0.00675702794F, - -0.00618763175F, 0.0143843591F, -0.00692260871F, -0.0141042564F, - -0.000616996549F, 0.00633663F, -0.00564325461F, -0.000262603367F, - -0.00386002404F, 0.000328853261F, -0.00427639624F, -0.0188590568F, - -0.00826942734F, -0.0101825306F, 0.00264641689F, 0.00283741648F, - -0.00380569533F, -0.00500197383F, 0.00386845949F, 0.00870108F, - 0.00162095274F, 0.0156493969F, 0.0114086885F, -0.00204765541F, - -0.00177889259F, 0.00244029029F, -0.0110165542F, 0.00105176668F, - -0.00581967365F, -0.0130687682F, 0.0187999643F, 0.00336509338F, - 0.00451117288F, -0.00293088844F, 0.00156368862F, 0.00119079277F, - 0.00359072862F, -0.00637645088F, 0.0161153916F, 0.000243381175F, - 0.0128367357F, -0.00147639425F, -0.00489684148F, 0.00404807925F, - -0.0107390136F, 0.0212601386F, -0.0175844822F, 0.0103590963F, - -0.000867696363F, -0.00923068263F, 0.0137072541F, 0.0155295562F, - -0.0218542982F, 0.010405005F, 0.00512126F, -0.00528724818F, 0.00514848158F, - -0.0114603844F, 0.00837129F, -0.00406864192F, 0.00741526531F, - -0.00801707804F, 0.00939893071F, -0.0105367387F, -0.01280984F, 0.0105339028F, - -0.00301217241F, -0.00769924326F, 0.000814606203F, 0.0033475461F, - -0.00562763028F, -0.00218046084F, -0.00855718367F, 0.00357403164F, - 0.000457131391F, -0.00414596498F, 0.00142223667F, 0.00780039793F, - 0.00453326898F, 0.00670201611F, -0.00991858263F, -0.00851037633F, - -0.00715765031F, 0.00145957619F, -0.00204606401F, -0.00211326871F, - -0.00651623821F, 0.00198431779F, -0.00798125472F, 0.0145721808F, - 0.000213904932F, 0.000280094333F, 0.00474151736F, -0.00341432658F, - -0.00616643066F, -0.0100533543F, -0.012033849F, -0.00338367512F, - 0.00788208935F, 0.0161279831F, 0.00514814164F, 0.000654125819F, - 0.00306451926F, 0.00577320624F, 0.00909504F, 0.00105627102F, -0.00762836542F, - 0.00839541759F, -0.00491000293F, -0.00982531533F, -0.00301363133F, - 0.01345558F, -0.00154109881F, 0.0106564322F, -0.000487181329F, - -0.00190940115F, -0.00404584641F, 0.00073880417F, 0.0154490406F, - 0.0111681037F, 0.0159158222F, 0.016943939F, -0.00445501925F, -0.00329497806F, - -0.0245225281F, -0.0154692763F, 0.00610113377F, -0.0208528787F, - -0.00184441404F, -0.00983049F, 0.0115776993F, -0.00362926093F, - 0.00166683516F, 0.0156896655F, -0.00667215139F, 0.00169482175F, - -0.00433400599F, 0.00689066714F, 0.0181795768F, 0.00646173535F, - -0.0084116431F, 0.00326372869F, 0.0106964232F, -0.0146331498F, - -0.00336356019F, -0.00371223595F, 0.00767652784F, -0.00684467936F, - 0.000900069368F, 0.00563505292F, 0.00255078217F, 0.00456275046F, - -0.00174422609F, 0.00527981715F, -0.00135376025F, -0.0108908359F, - -0.00800403673F, 0.0120817469F, -0.00247566798F, 0.0275599398F, - -0.0172516219F, 0.0120519148F, 0.00934338942F, -0.0143028777F, - 0.00299778162F, -0.00379907433F, -0.0118876044F, -0.00136589073F, - 0.0103337364F, 0.00805398822F, 0.00553791365F, 0.00142323738F, 0.0114586418F, - 0.00629555341F, 0.00522193639F, 0.00820602197F, -0.013981265F, - -0.00156058092F, 0.00369607052F, -0.0123916958F, -0.00758759445F, - 0.0186769627F, -0.00847674813F, -0.0170401279F, 0.00592763862F, - 0.0158968773F, 0.00798518304F, 0.00270524435F, 0.00412320951F, - 0.00639506616F, 0.000204669836F, 0.0234852713F, 0.00539989956F, - -0.00977550261F, 0.00706972601F, 0.0165323056F, 0.000395815616F, - -0.00809073728F, -0.0157075115F, 0.0105142817F, -0.00319431373F, - 0.00962027907F, 0.00567102246F, -0.00262400042F, -0.00130105065F, - -0.00891318079F, -0.0119105F, -0.00864309818F, 0.0119716469F, 0.0211973824F, - -0.00266842102F, 0.00264760479F, -0.00162005541F, -0.00810900889F, - -0.00784295332F, -0.00669563329F, -0.00716089644F, 0.000197821937F, - -0.0128555605F, 0.0199589897F, 0.00248866482F, 0.00861221366F, - 0.00242112949F, 0.00528655667F, -0.0046930355F, -0.00180241093F, - -0.0115468549F, 0.0119597418F, 0.00858325232F, 0.00291460287F, - 0.00226576882F, 0.00363020413F, -0.0170389097F, -0.00417167461F, - -0.00728790322F, -0.00568496296F, -0.000169244624F, 0.00571700651F, - 0.00352315651F, 0.00463755429F, -0.012521876F, 0.0158500504F, - -0.00390990917F, 0.00534117827F, -0.00528676622F, -0.0169772711F, - 0.00444662431F, -0.00561655406F, 0.00745000411F, -0.0174425766F, - -0.0115601644F, -0.00818633F, 0.00209752703F, 0.00177506986F, 0.0113325259F, - -0.0154518066F, 0.00770803029F, 0.000979238306F, 0.00568217738F, - 0.00165821007F, -0.00969567243F, -0.00242801104F, 0.000974986062F, - -0.00522098131F, -0.00302263605F, -0.0146055529F, 6.22971857E-5F, - -0.00234897085F, -0.00857792422F, -0.00607563F, 0.00287200836F, - -0.0038615095F, -0.0290150046F, 0.00156566291F, 0.00308837369F, - -0.00942750089F, 0.00381097198F, -0.00368526881F, -0.00145764009F, - -0.00298130861F, 0.00350048183F, 0.0181561764F, -0.0147011513F, - -0.00216953945F, -0.00151091919F, 0.00208822498F, 0.0154276937F, - -0.00131853856F, 0.0147283934F, -0.00965049863F, 0.00295675988F, - 0.00205161655F, -0.0110969013F, 0.0112981629F, 0.00364731438F, - 0.00364795723F, -0.0162594877F, 0.00969663076F, -0.0149020283F, - 0.0166815743F, 0.00236158632F, 0.00229725125F, 0.00143904157F, 0.0160843097F, - -0.00108542328F, -0.00527172443F, -0.0147214122F, 0.00517673837F, - 0.0129575552F, 0.00744887069F, 0.0125343818F, 0.00298236427F, -0.0175345F, - -0.00506949844F, -0.00446399674F, 0.00711495848F, 0.00856310315F, - -0.00158456771F, 0.00166504132F, 0.00160685147F, -0.0125251869F, - -0.0113014597F, 0.000828108226F, 0.00399183854F, -0.00563141704F, - -0.00455741957F, -0.00464019692F, -0.00335411262F, 0.00341963256F, - -0.00898089632F, -0.00995604694F, -0.00194644509F, 0.00764289265F, - -0.00670857215F, -0.020843666F, 0.00737753464F, -0.00258927F, -0.0125040971F, - 0.00169688987F, 0.00617543841F, -0.0139824869F, -0.000871956872F, - -0.0140121654F, -0.00562477484F, -0.00273612957F, 0.00185453484F, - -0.00692833913F, -0.00603667F, 0.0213119052F, 0.00735721923F, - 0.000626220251F, 0.00395735353F, 0.0128424363F, 0.00634696474F, - -0.0114901736F, -0.0108437454F, 0.0112182545F, -0.00134242233F, - -0.0119327353F, -0.00944378693F, 0.0138590597F, 0.000148897176F, - 0.00184046524F, -0.00376231736F, -0.00705556758F, -0.000164219193F, - -0.00337804365F, 0.00257695932F, -0.000423935882F, -0.00376100233F, - 0.0126109794F, 0.000932092487F, -0.0017247569F, -0.00222594733F, - -0.00777864642F, 0.00668931194F, 0.0131150903F, -0.00120401557F, - -0.0042537204F, 0.00269288756F, 0.00477185426F, 0.00755493855F, - 0.0165708624F, 0.00351214968F, 0.0234521683F, -8.67685812E-5F, - 0.00138171902F, -0.00387373962F, -0.00185775594F, -0.000937720353F, - 0.0104235215F, -0.0149863502F, 0.00727555295F, 0.0101554794F, -0.0088731749F, - 0.0137548866F, 0.00907369144F, -0.0122919278F, -0.0066081644F, - 0.00105969631F, -0.00887264591F, 0.0164167564F, -0.000690307817F, - -0.0138167422F, -0.00335340877F, 0.0082127722F, 0.0117705436F, - -0.0160178486F, 0.00625276193F, -0.0010484393F, 0.00726343179F, - 0.00836838689F, 0.00907834433F, -0.00502549391F, 0.00257201283F, 0.00891365F, - -0.00146868161F, 0.00629096F, -0.00392174162F, 0.00719097117F, 0.009902725F, - 0.0107279979F, 0.00723044947F, -0.00580705609F, -0.0111737419F, - 0.0092112245F, -0.00472431583F, -0.00370533438F, -0.00310726045F, - -0.00286862534F, 0.00718937488F, 0.0105679026F, -0.0101080742F, - -0.0101571456F, -0.000245005242F, -0.0125164837F, -0.00299851666F, - 0.00165000511F, -0.011925403F, -0.0211634599F, -0.0153606366F, - -0.00409255642F, 0.00435641827F, 0.0161757022F, 0.00744006783F, - 0.00235312642F, 0.00356899039F, -0.0193092F, -0.00719225407F, 0.00430717971F, - 0.00978497788F, 0.0190421771F, -0.0118090771F, 0.000525740965F, - -0.0138150854F, -0.00628917199F, 0.00405166112F, 0.0175227243F, - 0.00822902471F, -0.00742634293F, -0.00817989465F, 0.000982407F, - 0.00496147946F, 0.006578892F, -0.00495023653F, 0.00107270735F, - -0.00902178604F, -0.00395199703F, 0.00500763627F, 0.000464630983F, - -0.0120586241F, -0.0102080535F, -0.0086295139F, -0.0154303806F, - 0.00504532317F, 0.0100476975F, -0.00778702181F, 0.00249261083F, - 0.00560201379F, -0.0150044812F, -0.00869465712F, -0.00515050581F, - 0.00878261775F, 0.00780909276F, -0.00117383036F, -0.00637927838F, - -0.00628808932F, -0.00134915044F, 0.0202238224F, 0.00936258491F, - 0.00326815504F, 0.00830444228F, -0.00287058484F, 0.00900654F, - -0.00698366063F, -0.0170806441F, 0.00549038593F, 0.0007578308F, - -8.86614362E-5F, 0.0222581495F, -0.00823632162F, 0.00517135439F, - -0.0135765979F, -0.00111162255F, 0.00773243932F, -0.00692686206F, - 0.0115817515F, -0.0050835805F, -0.01128833F, -0.0104703493F, 0.00928228907F, - -0.0037535571F, -0.00918586738F, 0.00367906271F, -0.025551809F, - -0.00741172535F, 0.0175082106F, 0.0144077698F, 0.00737865409F, 0.0114323543F, - 0.0120465774F, 0.0108777331F, 0.00239887065F, -0.0146271484F, -0.0139279012F, - -0.00231899926F, 0.000428206637F, -0.0122084077F, -0.0130357351F, - -0.00922519341F, -0.0211728737F, 0.0057594697F, -0.00849248655F, - -0.00825364F, 0.0138306702F, -0.0092461193F, -0.00618457654F, 0.00203275937F, - 0.00129724259F, 0.0142972432F, -0.00627887389F, 0.0106908502F, - 0.00150740717F, 0.0268179886F, 0.0021005664F, -0.00323638506F, - -0.00242524245F, 0.0087014325F, 0.00871997233F, -0.0201605912F, - -0.00549721718F, 0.00685730111F, 0.0065245959F, 0.0120419962F, - -0.00865459442F, -0.00656898506F, 0.00215086085F, -0.000853727513F, - -0.00147179875F, -0.00215669232F, -0.00574660581F, -0.0143456403F, - 0.00988671556F, -0.00203084806F, 0.0100523559F, 0.013537325F, 0.0121903298F, - 0.000485275581F, -0.0045126453F, -0.00360378274F, -0.00236409879F, - -0.0270924494F, 0.0102002F, -0.00342130614F, 0.00593330106F, 0.000519514142F, - -0.00155263231F, -0.0115718562F, -0.000589259434F, 0.00505875191F, - 0.0103922067F, 0.0228236839F, -0.0022913632F, 0.000801487F, -0.00840396341F, - 0.00318610435F, 0.00412326F, -0.0169371087F, 0.00405132305F, 0.00903669465F, - -0.020929087F, -0.00214962824F, -0.000843297807F, -0.00552778086F, - -0.00451792311F, 0.000553863472F, -0.000211928127F, 0.0193234701F, - -0.00176678121F, 0.00458842563F, -0.00673730532F, -0.00778085738F, - 0.0099210171F, 0.00332923257F, 0.000566124567F, -0.00849743281F, - 0.00276305433F, 0.00798887573F, 0.014304461F, -0.013634461F, 0.000827554904F, - 0.00225562789F, 0.0184850693F, -0.00898373872F, -0.00558836432F, - 0.0171898566F, -0.0167788193F, 0.00708075054F, -0.00977245718F, - 0.000552553334F, 0.00969580282F, -0.00249594287F, -0.00667379843F, - -0.00531111192F, -0.00866663177F, 0.0129696764F, 0.00414196448F, - -0.0015139879F, -0.00198210147F, -0.00596455904F, 0.00562342769F, - -0.0107929008F, 0.000234923617F, 0.0097867F, 0.00566662336F, -0.0109260511F, - 0.00758077484F, -0.0022036233F, -0.0055524148F, -0.00355164963F, - -0.00357728032F, -0.0152100325F, 0.00799110625F, 0.000820459914F, - 0.00697110966F, 0.00465995539F, 0.0111694578F, -0.00960643776F, - -0.00421506865F, 0.000195846602F, -0.0125209447F, -0.00212897174F, - 0.0133443549F, -0.000502635143F, 0.00107791671F, 0.012344894F, - -0.00222739088F, -0.000153893183F, -0.00853151735F, 0.00163156295F, - 0.0107463701F, 0.00191534893F, -0.00689231418F, 0.00504770037F, - -0.0203198921F, -0.01924932F, 0.00768891163F, -0.0061529181F, 0.00545836F, - -0.00833698828F, 0.00642491132F, 0.0016296471F, -0.00911817327F, - 0.00445832731F, -0.00954602752F, 0.00799153652F, -0.0132564465F, - -0.00347544625F, -0.00395738473F, 0.0118254228F, 0.007648509F, - 0.00275339326F, -0.00123066292F, -0.000543523696F, 0.00574299693F, - 0.00608086726F, -0.00883718301F, -0.00244943937F, 0.00650767749F, - 0.0108346315F, 0.00571971666F, 0.0126326745F, -0.00790071581F, - 0.00445494428F, 0.0139689995F, 0.0152372895F, -0.0116894273F, - -0.00249910424F, 0.00503075123F, -0.00624321261F, -0.0140111F, 0.0120613864F, - -0.0100125596F, -0.0245043207F, 0.00799352862F, -0.0020078118F, - 0.00249916129F, 0.0003772747F, 0.0050654849F, -0.0113528101F, 0.0115071F, - -0.00632530451F, 0.00382866687F, 0.00275656418F, 0.0148418806F, - -0.0131554762F, -0.0014325243F, -0.0081453342F, 0.00119858258F, - 0.00405747676F, 0.00168316578F, -0.0035161F, -0.0065095243F, -0.00164851302F, - -0.0146346595F, 0.00430675969F, 0.0153495232F, -0.00222442788F, - -0.00389709766F, 0.0143782748F, -0.00159546593F, -0.000531517144F, - -0.00213438226F, -0.00701747229F, -0.0210485514F, -0.00669307122F, - -0.00574380578F, 0.00132527784F, -0.00705663F, 0.00107105891F, - 0.00843474641F, 0.000134197282F, 0.00272108684F, -0.00668269582F, - -0.00926518F, 0.0173125304F, -0.0112728933F, 0.00232423609F, -0.0159032587F, - 0.000526562275F, -0.0163435861F, 0.00538721727F, -0.00900297891F, - -0.0050319992F, 0.00758181699F, 0.00144001737F, -0.0137152812F, - 0.00649520755F, 0.00762231136F, 0.0198876951F, -0.000949431502F, - 0.0064684581F, -0.00407050923F, -0.00734419189F, 0.00493897218F, - -0.0138306562F, -0.0166180525F, -0.00578834023F, -0.0162897315F, - 0.0083350176F, 0.00865103398F, -0.0138128484F, 0.0103521803F, 0.0172064174F, - 0.0153336804F, -0.0109485369F, -0.00506557198F, 0.00671636593F, - -0.00547830341F, -0.00204671104F, 0.0110048503F, -0.00932352F, - -0.00721197855F, 0.00397744356F, -0.00407378701F, -0.00282591628F, - 0.00525773643F, -0.00361076137F, -0.00217089825F, 0.00306037976F, - 0.00873599946F, 0.0152699575F, -0.00304663111F, -0.00359734264F, - -0.00649040332F, 0.00726204878F, 0.00902847853F, -0.00465374673F, - 0.0204362143F, 9.43532268E-6F, -0.01603516F, 0.0109773939F, -0.00268812128F, - -0.0182784479F, 0.00577301113F, -0.00522463536F, 0.0120268082F, - -0.0023538461F, -0.0178694148F, -0.00591633655F, -0.0100348582F, - -0.00184177386F, 0.00105460617F, -0.00138890988F, -0.00110208418F, - -0.00518674543F, 0.00866981503F, 0.00136655068F, -0.0202589631F, -0.004533F, - -0.00888945255F, -0.00215792865F, -0.00305487402F, 0.0012451692F, - 0.00666356133F, 0.0154783931F, -0.00940837618F, 0.00553742051F, - -0.000463158096F, 0.0038727154F, -0.0128917275F, 0.00081215828F, - -0.00419058744F, -0.00661835959F, -0.000659391342F, 0.00232926174F, - 0.00416299514F, -0.00453345198F, -0.00401810696F, 0.00328164198F, - 0.00487652747F, 0.0223461017F, 0.00783959F, -0.0172790885F, 0.0245913882F, - -0.00483187335F, 0.00239446177F, 0.00268788822F, 0.00626701163F, - 0.00974984374F, -0.00175619021F, -0.00953766331F, -0.00402379315F, - -0.00448814221F, -0.00589429215F, 0.0187111795F, -0.001651239F, - -0.017495947F, -0.00496306224F, -0.00687383488F, 0.0286024492F, - 0.00135789497F, 0.00251656794F, -0.00720831752F, -0.00628689257F, - 0.0192759894F, -0.00670138188F, -0.00058223278F, 0.00577098737F, - -0.00317045092F, 0.0144361202F, -0.00123023603F, 0.0089629814F, - -0.00375808706F, -0.0110117514F, -0.00681635598F, 0.00965373591F, - 0.00561395334F, 0.00864520855F, 0.00586898671F, -0.011842466F, - -0.000486498291F, 0.00984739233F, -0.00986497942F, 0.00763099175F, - 0.00120347214F, 0.000329423026F, -0.00817059539F, 0.0107628517F, - -0.00214346428F, -0.0138744293F, -0.0125604412F, 0.00117444317F, - -0.0110733621F, 0.0100993337F, 0.001212726F, 0.0013126228F, -0.00545282569F, - -0.00163367693F, 0.0037047863F, -0.00599719631F, 0.00809923559F, - 0.0044453321F, 0.013684148F, -0.0210422222F, 0.00727933086F, 0.0064237616F, - 0.00422038138F, 0.00105329289F, 0.0144563047F, -0.00364260073F, - 0.0134017328F, 0.0103859333F, 0.00888307F, -0.00525973225F, -0.00245780032F, - 0.00846548192F, 0.00711643137F, -0.00859713834F, 0.00674127229F, - 0.0036530646F, -0.00645706942F, 0.0133718746F, 2.53398375E-5F, 0.0233226437F, - 0.00973230321F, -0.00900416076F, -0.00652957195F, -0.00499003753F, - 0.00275691948F, 0.00847547688F, 0.0010064349F, -0.0146971755F, - -0.00259097619F, 0.00473415293F, -0.0162547398F, 0.0114353383F, - -0.0133005241F, 0.0119815366F, -0.0122867385F, -0.0128483083F, - 0.00594677776F, 0.00935017783F, 0.00948191434F, -0.0057687643F, - -0.012775341F, 0.0161453374F, 0.0152644897F, 0.00495290104F, 0.00798343401F, - 0.0143511761F, -0.0130846975F, 0.0062275324F, -0.00224301033F, - 0.00942761637F, -0.0174853466F, 0.0212738607F, 0.0245549362F, 0.00236521871F, - 0.00173112471F, 0.015378532F, -0.00443612365F, -0.00333957514F, - 0.00612343894F, 0.000569023134F, 0.0036470911F, -0.0110438317F, - -0.00357399695F, -0.000740019779F, -0.0138197783F, -0.014302928F, - 0.00269290875F, 0.0113050006F, 0.0072944616F, 0.000275661179F, - 0.00764503703F, 0.00481620757F, 0.00148290757F, 0.0060279048F, - 0.00989936292F, 0.0030421773F, -0.00347606186F, -0.0148931742F, - 0.00450661778F, -0.0162339043F, -0.0019070697F, -0.00467423908F, - 0.00256756088F, -0.00317132915F, -0.00805173721F, 0.00927964505F, - -5.03583578E-5F, 0.00463245809F, -0.000614541175F, -0.00275537628F, - -0.00575320935F, -0.015553277F, 0.00206089346F, 0.00120291172F, - 0.0180349611F, -0.00267987908F, -0.00161654863F, -0.00439054053F, - -0.00829987787F, -0.0227130968F, -0.00360163441F, 0.00680074655F, - 0.000204046461F, 0.00452262163F, -0.00352274743F, -0.00494383648F, - -0.000728744373F, -0.0165203325F, 0.0190030988F, -0.00314571988F, - 0.0204606429F, -0.000781921553F, -0.00816234853F, -0.000295141683F, - -0.0227027182F, -0.00268901535F, -0.00338573265F, 0.00541921239F, - 0.00322784227F, 0.000837143336F, 0.018529065F, -0.00589294173F, - -0.00801784731F, 0.0248774905F, -0.00291735167F, 0.00720556732F, - 0.0127366437F, 0.00356453191F, 0.0184643548F, 0.00752228778F, - -0.00503509864F, 0.00639328081F, 0.0131329084F, -0.0160254799F, - 0.0111123165F, 0.0116857151F, -0.00276785041F, -0.00673119957F, - 0.00195586449F, -0.00470260438F, 0.00522475783F, -0.00280677946F, - 0.0176265743F, -0.00169921666F, -0.00787615497F, -0.00735268323F, - 0.0154773714F, -0.016131565F, -0.0100790979F, 3.45240624E-5F, 0.00224429742F, - -0.001532975F, -0.0179669019F, -0.0132598635F, -0.0112426095F, - 0.00147208897F, 0.0107087204F, -0.0134601686F, -0.00364440121F, - 0.00596253807F, -0.00234107906F, -0.00397477392F, -0.00468585966F, - -0.0047074547F, -0.0068056155F, 0.0160893686F, 0.0131680304F, -0.0139388666F, - 0.00468549924F, -0.015376823F, -0.00248671742F, -0.00220656255F, - -0.000911446579F, -0.012691888F, 0.00695909886F, 0.0136387134F, 0.012336134F, - 0.0187486149F, 0.0162229743F, 0.0129247485F, 0.00547057483F, - -0.000476778339F, -0.0136767467F, -0.00765791861F, 0.00201939931F, - 0.00236361264F, 0.00745534059F, 0.000858042273F, 0.00608214829F, - -0.00877481233F, 0.00766959228F, -0.00726447254F, -0.00290750572F, - -0.0144314514F, 0.000337540259F, -0.0022882116F, -0.0067626927F, - 0.00728013506F, -0.00216591894F, -0.00461708382F, -0.00863781478F, - 0.0132906614F, -0.0155267278F, -0.00629178109F, 0.00339902611F, - 0.00078655628F, 0.0105877966F, 0.00430168631F, 0.0141180102F, -0.0126020489F, - -0.00219185348F, 0.00960758328F, 0.00991095789F, 0.0112392F, -0.00690248189F, - -0.0052293716F, -0.00593356183F, -0.00913714431F, -0.00733672688F, - -0.0115525126F, -0.0045682448F, -0.0126018459F, -0.000848003081F, - 0.0037299667F, 0.00567989517F, -0.00197629F, -0.00406256644F, -0.0191774108F, - -0.00289598014F, 0.014339217F, -0.00433700392F, 0.00228209514F, 0.0157788F, - -0.00302946451F, 0.0125437444F, 0.00724508F, -0.0104453154F, 0.0220315121F, - 0.00531398086F, 0.00202838518F, 0.00855645817F, 0.00880132895F, - 0.0164209399F, 0.00128465786F, 0.00307954196F, 0.00572046405F, - -0.00651966361F, -0.00948934164F, -0.0195728261F, -0.0162724F, - 0.000334931799F, 0.00994744804F, -0.00621941919F, 0.00115703943F, - -0.00931774545F, 0.0025924847F, 0.00445670309F, -0.00468583079F, - 0.00534995925F, 0.00170885946F, 0.00209384831F, 0.0101257265F, - 0.00243063294F, 0.00477559585F, 0.00354528474F, 0.00562799815F, - 0.00515063945F, -0.0102518294F, -0.00339095714F, 0.00160270894F, - -0.00725198677F, -0.00861105509F, -0.00692912238F, 0.00776548358F, - 0.00474471645F, 0.016591223F, 0.00381742045F, -0.0206072666F, 0.00293568871F, - -0.0139548779F, 0.0151990112F, 0.00818897504F, -0.0003074345F, - 0.00984416064F, -0.000156902955F, 0.00632106606F, 0.00311358436F, - 0.00227345037F, -0.0117253959F, 0.00566620333F, -0.0122307679F, - -0.0179482158F, -0.00242818589F, 0.00721588731F, 0.00201679789F, - -0.00726529025F, -0.0021594977F, -0.00946421921F, 0.00116509025F, - -0.0114837065F, 0.00444892608F, 0.0111771114F, -0.0130992085F, 0.0149816144F, - -0.00111996394F, 0.0093345074F, -0.00702103833F, 0.00556302443F, - 0.0270989705F, 0.00735719781F, 0.00215493F, 0.0191875063F, 0.0215472486F, - 0.000472495682F, -0.0047134F, -0.00217068777F, 0.000543763512F, - -0.000549969729F, -0.0100356936F, -0.00877434F, 0.00587891834F, - -0.00512107415F, -0.0102510611F, -0.00452177878F, -0.00341149722F, - -0.0057807439F, 0.00361141795F, -0.0114969285F, -0.0124980742F, - -0.0204481445F, -0.0205241796F, 0.00123764807F, -0.00477212109F, - 0.0107201692F, 0.00445911F, 0.00642947853F, 0.00870700367F, 0.000957069336F, - -0.000419202494F, 0.00482928474F, -0.0215000901F, -0.0136174802F, - 0.00467681233F, -0.0124783823F, 0.00180578989F, 0.011397806F, - -0.00258757896F, 0.00248216093F, -0.00513077294F, 0.0119767282F, - 0.00820360146F, -0.0170358978F, 0.00412349589F, 0.0148178479F, - 0.000195475368F, -0.0126936231F, -0.000829422206F, -0.000621774583F, - -0.00609931163F, 0.00268466794F, 0.00275849598F, 0.0113553042F, - -0.0116355428F, 0.00673854677F, 0.0124756461F, 0.00234689843F, - 0.00846985262F, -0.00143453968F, -0.00124973583F, -0.0128696691F, - -0.0123039261F, 0.0217275042F, -0.0153984344F, 0.0107036754F, 0.0078459885F, - 0.00683715334F, -0.00327185541F, -0.00448775943F, 0.00585560361F, - 0.00728378026F, 0.00648140535F, -0.00282257237F, -0.0199038796F, - 0.00364536163F, 0.000345370616F, -0.0114420597F, -0.0121678533F, - -0.00485463254F, -0.000489724742F, 0.00672893086F, -0.0311120916F, - 0.00533726F, -0.00808159169F, -0.00459225196F, -0.0146868667F, - -0.00430308655F, -0.00513378298F, 0.00819254946F, 0.0228723381F, - 0.0015461545F, -0.00869022869F, 0.00968175568F, 0.00277766143F, - -0.00552067813F, 0.00306020956F, 0.00253321463F, -0.00112855493F, - -0.00896012876F, 0.00301446021F, 0.00376024749F, -0.0096704457F, - -0.00317380414F, 0.00745554268F, -0.00114063383F, -0.00617840094F, - 0.00193786772F, -0.0112684472F, -0.000985419378F, -0.0091386158F, - -6.35544711E-5F, -0.00204002415F, -0.00515243225F, 0.00349255442F, - -0.00253057433F, 0.0167095959F, -0.00322006689F, -0.00926653389F, - 0.00840875413F, -0.00863277633F, 0.0137014063F, 0.00181928743F, - -0.00719566084F, -0.0101815211F, -0.015389001F, 0.024928052F, 0.0196175855F, - 0.0184666347F, 0.00381797645F, 0.000525078503F, 0.00436203182F, - -0.000502872514F, 0.00295656826F, 0.00820025615F, -0.0157079473F, - -0.00783189666F, -0.00924814306F, -0.00755814184F, 1.04265382E-5F, - 0.00695359381F, -0.0163894407F, 0.00199272227F, -0.00922503136F, - -0.0267716404F, 0.00072683685F, -0.00578032527F, -0.00544320839F, - 0.000639837F, 0.00170894829F, -0.012633604F, -0.00385141443F, 2.74701115E-8F, - 0.00401939545F, -0.00345873693F, 0.0130083924F, 0.00285056978F, - 0.000734560599F, 0.00108591584F, 0.0144564211F, -0.000962461694F, - -0.00523963245F, -0.00276757265F, -0.00552187301F, 0.0122859925F, - -0.00508702872F, -0.0194558688F, -0.0129457656F, 0.00652013253F, - 0.0114478124F, -0.0121159097F, -0.012213611F, -0.00402054749F, - -0.00371217472F, -0.020196151F, -0.0107771521F, 0.0110934656F, - -0.0111424113F, -0.00193707715F, 0.00356509443F, 0.0144925611F, - -0.00313227018F, 0.00443859678F, 0.00152476027F, 0.00270510046F, - -0.0219306629F, 0.00163572747F, -0.00453612488F, 0.000927187211F, - -0.00476236781F, -0.00573699642F, -0.0146072274F, -0.0129177682F, - -0.00296836812F, 0.0221211016F, -0.00128462166F, 0.00106285326F, - -0.00311044464F, 9.19667582E-5F, -0.00645048078F, 0.00257955631F, - 0.0146850105F, 0.00897530653F, -0.00799394865F, -0.00471053738F, - -0.000463121978F, 0.00277841627F, 0.014110906F, 0.015789073F, -0.0159313586F, - 0.0184171535F, 0.00481474958F, 0.00334362593F, -0.00561886793F, - -0.00513323676F, 0.00583662232F, 0.00729006436F, 0.00449712761F, - -0.00346179213F, -0.00459624408F, 0.0261943806F, -0.0169819221F, - 0.00207189983F, -0.00262190821F, 0.0248174854F, 0.00359058147F, - 0.0128676575F, -0.00813460816F, -0.000736867427F, 0.0181609336F, - -0.0201880913F, 0.00693730311F, -0.00474435883F, 0.000173978668F, - -0.00911336206F, 0.00857820734F, -0.000324745342F, 0.00791560952F, - 0.0088581657F, -0.0111405123F, 0.000563374662F, 0.00517080398F, 0.00286583F, - -0.0109906448F, -0.0024696365F, 0.0176024493F, -0.00753502036F, - 0.0288844425F, -0.00965403114F, -0.0030298538F, -0.00188103353F, - -0.0152550209F, -0.0148396622F, -0.013433612F, 0.00706410222F, 0.0154353855F, - -0.00974339806F, -0.00884695631F, 0.00873464439F, -0.000888794719F, - -0.00726696337F, -0.00189409428F, 0.00093334011F, 0.00340457726F, - 0.00141358026F, 0.000573891506F, -0.020530995F, 0.00603622152F, - -0.0110547515F, 0.0099049937F, -0.00753253186F, 0.00857216772F, - 0.000179969502F, 0.00602995511F, -0.0117959902F, 0.019365536F, - -0.0225989185F, -0.0189716499F, 0.00622573867F, 0.0036937003F, - 0.000413351983F, -0.0122880097F, 0.00369836041F, 0.00592788775F, - -0.001729312F, -0.0208641924F, -0.011183179F, -0.0191770792F, -0.0101417825F, - 0.00625640526F, 0.011356541F, 0.0166957211F, 0.00180422282F, 0.00605555531F, - -0.00962612F, -0.00154689385F, 0.00593330152F, 0.000305758964F, - 0.00371812168F, -0.00501142582F, -0.0104241017F, 0.00177066028F, - 0.00220309757F, 0.001043158F, 0.00669297157F, 0.00964746531F, - 0.000510338345F, 0.00614982657F, -0.00589789404F, -0.00839041453F, - -0.0036213065F, -0.00310004666F, -0.0034170479F, -0.000562247704F, - -0.0013995663F, -0.00220418698F, 0.00270596636F, 0.00842069276F, - 0.00119122583F, -0.0238515623F, 0.00313952891F, -0.0180557687F, - -0.00846972689F, 0.0151250735F, -0.014548569F, -0.0203215182F, - -0.00367643405F, -0.00596373715F, 0.00258727185F, -0.0100939982F, - 0.0139768813F, 0.00268789F, -0.0048314943F, 0.00276625575F, -0.00919816736F, - 0.00897379126F, 0.00417543855F, 0.00762092136F, -0.0041708746F, - -0.00654175505F, -0.0107778339F, 0.00118069747F, -0.00981981494F, - 0.0194518976F, -0.00139968318F, 0.00561446277F, -0.0183967743F, - 0.00105897349F, 0.00338996225F, 0.0171200652F, 0.0142607326F, -0.0159558933F, - -0.00180020137F, 0.00170036056F, -0.0024551861F, -0.0105302706F, - -0.00602304842F, 0.0122461235F, -0.00793869514F, 0.000275087805F, - -0.00743603706F, 0.0106544886F, -0.00806435477F, 0.0083842352F, - -0.00560676213F, 0.00583605515F, 0.00241843122F, 0.0140002863F, - -0.0020743371F, -0.00950464327F, 0.00441760197F, 0.0028692598F, - 0.00397763634F, 0.0019336472F, 0.00358611858F, -0.00460460922F, - -0.000877454411F, -0.0162172392F, -0.00477445312F, 0.016204929F, - 0.0108950678F, -0.00486403424F, 0.00519264024F, -0.00189967011F, - 0.00415629428F, 0.0155112157F, -0.00255987351F, 0.00361997401F, - 0.00194972532F, 0.000501820934F, -0.00808509F, -0.00170466478F, - -0.00988277234F, 0.00312649761F, 0.0042629973F, 0.00526970206F, - 0.00655861106F, -0.00301760458F, 0.00887495652F, -0.00969346799F, - -0.00773373852F, 0.00141605234F, 0.0213081203F, 0.0210713167F, - 0.00231176289F, -0.00967837218F, -0.00218789489F, -0.0067241122F, - 0.000478003145F, 0.00380726159F, 0.0151864188F, -0.00216044043F, - 0.0183584578F, -0.00037081007F, -0.0115857311F, 0.011153711F, 0.0136075569F, - 0.0053069F, 0.00802378543F, 0.00603878F, -0.00256645284F, 0.00352937891F, - -0.000378842233F, 0.0193169862F, -0.00168840855F, -0.0153127238F, - 0.000237301938F, 0.00208693417F, -0.00399728399F, 0.00754817715F, - -0.000191918734F, 0.020258598F, 0.00234889612F, -0.00735420175F, - 0.00154501689F, 0.00885242783F, -0.00641486607F, -0.00843803398F, - -0.0133768544F, -0.0162578356F, -0.0113042612F, 0.00295446161F, - 0.00813437905F, 0.013063035F, 0.00576218916F, 0.00677784905F, - -0.00327188987F, -0.0055678403F, -0.00726326788F, 0.00503999926F, - 0.00990965F, -0.009959355F, -0.014264253F, -0.00271508982F, -0.00191810413F, - -0.000442454271F, -0.00332222646F, -0.0143902143F, 0.00181389879F, - -0.00629331451F, -0.00442873547F, 0.0083171688F, -0.0118779903F, - -0.0134590501F, -0.00197236263F, -0.016768055F, 0.0041281241F, - 0.00123090658F, -0.00208916399F, -0.00120440207F, 0.00234466395F, - -0.0043369513F, 0.0122478325F, 0.0114308083F, -0.00215528579F, - -0.000605912704F, -0.00235269219F, 0.0108022336F, 0.00441674562F, - -0.0087703F, -0.00781117799F, -0.0155846765F, 0.00690395245F, - -0.00201461092F, -0.0018399671F, -0.0115133058F, -0.0147479977F, - -0.00544385333F, 0.00740756234F, 0.00577061111F, -0.0121477693F, - -0.0154686123F, 0.00123180717F, 0.0237778276F, -0.00730781211F, - 0.0230088159F, -0.00189145328F, -0.01242455F, -0.00499338191F, 0.0179095268F, - -0.00523266708F, 0.00451254519F, 0.0031733755F, -0.00368810818F, - 0.00934428535F, -0.00691735884F, -0.00742415199F, 0.000311789918F, - -0.00494539458F, 0.0143374F, 0.00701397518F, 0.0117311459F, -0.0216639023F, - 0.0133555727F, -0.000665287662F, -0.00457396777F, 0.00974386372F, - 0.00838695094F, -0.00449346425F, -0.00871112756F, -0.0131441085F, - 0.00995740388F, -0.0203671101F, -0.00704057142F, -0.00469599431F, - 0.00312806573F, 0.020225212F, 0.00281191268F, -0.00360671035F, - -0.00634146761F, -0.0116947899F, -0.0153024653F, 0.0051896614F, - 0.0164149366F, 0.00756024057F, -0.00199359376F, 0.0220761038F, 0.00927914F, - 0.00183723052F, -0.0137241287F, -0.0114774154F, 0.0161304679F, 0.0116754407F, - -0.0153916841F, 0.00179018453F, 0.00919118F, 0.00188266428F, -0.00810746569F, - 0.00761594065F, 0.0108035281F, 0.00980454497F, 0.0110270604F, 0.00713053206F, - -0.00642038463F, -0.00373738375F, 0.00401238631F, 0.0143632712F, - -0.00754028698F, -0.0175482668F, 0.0277026463F, 0.00592418807F, - -0.0106544048F, -0.021051826F, -0.008204774F, -0.00549733499F, 0.0106152371F, - 0.00779364258F, 0.0159851201F, -0.00197616173F, 0.00786509365F, - -0.00343967462F, -0.00469064107F, -0.00854412187F, 0.00722885411F, - -0.00543117337F, -0.00828760676F, 0.00258408091F, -0.00896146521F, - -0.00490901573F, -0.0054238704F, 0.00165675627F, 0.003221513F, - 0.00191928481F, -0.00984626915F, -0.0232148673F, -0.00355673744F, - -0.00472835032F, 0.0177609026F, -0.0100373449F, -0.0121590719F, - -0.0023804775F, 0.00166960142F, -0.0011941049F, -0.0137003176F, - 0.00451591797F, -0.0112198172F, -0.00817271881F, 0.00265666516F, - -0.00407323521F, 0.00946238F, 0.0161362924F, 0.00343894144F, 0.0188374259F, - 0.00396737363F, 0.0157852676F, 0.00363545516F, -0.0115844188F, - -0.00510409288F, -0.0129153663F, -0.00797085278F, 0.0116341114F, 0.00422225F, - 0.00395586807F, -0.0037485396F, -0.0152083291F, -0.00784346648F, - -0.000326113455F, -0.00972642284F, -0.00493488321F, 0.0169422459F, - -0.00792742055F, -0.0120532652F, 0.0161825828F, -0.00470785378F, - -0.00693169469F, -0.000324137072F, -0.00639476255F, 0.00749273738F, - -0.00259343837F, -0.0105974283F, 0.021603791F, -0.00828453619F, -0.01344328F, - -0.001641626F, 0.00744924322F, -0.0173521917F, 0.0137142492F, 0.00107069081F, - -0.00955833402F, -0.00725622F, 0.0114041781F, 0.00805990398F, 0.00898273941F, - 0.0103293126F, -0.00109921931F, -0.0135983778F, 0.0127629554F, - -0.00682328828F, -0.00433161715F, -0.00784844F, 0.00430773525F, - 0.0110269031F, 0.0184486322F, -0.0124553218F, 0.00602214551F, -0.0118917795F, - -0.0127961598F, 0.00663367892F, -0.0098537067F, -0.0163940135F, - -0.00731172878F, 0.00941178F, -0.00621108199F, -0.013169406F, - -0.00224541547F, -0.00489262445F, -0.00393758528F, 0.00271534245F, - 0.00684563909F, -0.0161951855F, -0.0119221043F, 0.0034764451F, - 0.00578353042F, -0.0120477201F, -0.0149990367F, -0.00456015347F, - 0.00876551494F, -0.00365110673F, 0.00921223592F, 0.0117805069F, - -0.0105410274F, 0.00397662353F, 0.012739216F, 0.000231429134F, - 0.00745598646F, -0.00391389336F, -0.00259940163F, -0.00226668105F, - 0.00943297148F, -0.00469945231F, 0.0131741259F, 0.00193676644F, -0.00801503F, - 0.0108657116F, 0.0113905873F, 0.0020125166F, 0.000764597266F, 0.0018991757F, - -0.00490439311F, -0.00720160408F, -0.0050866385F, -0.00481194118F, - 0.00664254511F, -0.00155457738F, -0.00475739734F, -0.00422264868F, - -0.0135699436F, 0.00512874546F, 0.0188765656F, 0.00335099176F, -0.021696249F, - 0.000136637187F, 0.0129864803F, -0.0129182739F, -0.0128757283F, - -0.0082029514F, -0.0118059963F, 0.00265617575F, 0.0222066734F, - -0.00813418534F, -0.0142385727F, 0.00500989333F, -0.00237492356F, - 0.00613208394F, 0.00482416106F, 0.000551375328F, -0.00424922723F, - 0.00574958744F, 0.00281220372F, 0.00631708233F, 0.0163889471F, - -0.00431523426F, -0.0145408204F, 0.00571734225F, 0.0178628489F, - -0.0105013428F, 0.00650319364F, 0.0220314674F, 5.36303469E-6F, - 0.000223730807F, -0.0011004305F, -0.00442979252F, -0.00866530742F, - 0.00849259458F, 0.00253639347F, -0.00652199378F, 0.0091112731F, - -0.0104206866F, 0.0114193028F, -0.00308213336F, 0.00949061383F, - 0.0384846218F, 0.00532721402F, -0.00775719713F, -0.00286744745F, - 0.00183766964F, 2.35019143E-5F, -0.00706401467F, -0.00518252468F, - 0.0129466299F, -0.0128986752F, -0.00281729409F, 0.00392971514F, - 0.0185022037F, -0.00166190648F, 0.010259157F, 0.0115294456F, -0.0068409727F, - -0.00717346184F, 0.0107638044F, 0.00611998094F, 0.00571946939F, - -0.00489605078F, 0.00333885546F, -0.0042542778F, 0.0111133223F, - -0.00805519056F, -0.00497766491F, 0.00710588554F, -0.0196188428F, - -0.00194088812F, -0.0012579473F, -0.0107184919F, -0.0129705863F, 0.00754546F, - -0.00234630844F, -0.00236398331F, -0.00355830672F, 2.40528789E-5F, - 0.0112358751F, -0.00745768799F, 0.00677839201F, 0.00421519112F, - -0.00268972642F, -0.0248229988F, 0.00176580157F, 0.00902581494F, - 0.000409228378F, -0.0126438765F, 0.0123226373F, -0.000546214695F, - 0.0181674901F, 0.0184111129F, -0.00372947124F, 0.0067424709F, 0.00258241501F, - -0.0208974462F, 0.00069885928F, 0.00101624557F, -0.00928621739F, - 0.00479952525F, 0.00622157473F, 0.011959645F, 0.016269397F, 0.00312827877F, - 0.00690921815F, -0.00174698432F, 0.00848813634F, -0.00307323132F, - 0.00639905268F, -0.0109328469F, -0.0013847152F, 0.012532902F, 0.00142130663F, - 0.0048883995F, 0.00207291031F, 0.00658449717F, 0.00422000652F, 0.0081940759F, - 0.00388328824F, 0.0137584778F, -0.0113263642F, -0.00917654671F, - -0.0285458509F, 0.00728869904F, -0.00297074788F, -0.0134501178F, - 0.00817635562F, -0.00686675264F, -0.0102774613F, -0.0270469598F, - 0.00677903509F, 0.0117923263F, 0.0039794175F, -0.0106734624F, - -0.00350818294F, -0.00163637439F, 0.00403689779F, -0.00153432426F, - 0.00394114945F, 0.000550755707F, 0.0017598687F, -0.000367667904F, - 0.0152179562F, 0.0130207604F, -0.00423050718F, -0.00690854713F, - 0.00328680454F, -0.00614123279F, 0.00738830678F, -0.0129577126F, - -0.00312022609F, -0.015400256F, 0.0065606148F, 0.00420373678F, - -8.8884095E-5F, -0.00367755839F, -0.00319842855F, 0.00830476359F, - -0.011930678F, 0.0031035752F, -0.00342916977F, 0.000545585179F, - -0.0110619906F, -0.00388280558F, 0.000712096866F, -0.00620816881F, - -0.0105161089F, 0.00948640704F, 0.0135404412F, 0.00180784659F, - -0.00622947142F, 0.00455341302F, -0.00730826566F, -0.00374011649F, - 0.0103974296F, -0.0160609204F, 0.00827597827F, -0.0152042489F, - -0.00233910419F, 0.0220872425F, -0.0099221766F, -0.00253954413F, - -0.00850588176F, 0.00995102711F, 0.00430736132F, 0.00145493634F, - -0.000248135417F, -7.02569814E-5F, -0.000685418956F, 0.0102553088F, - 0.0124375094F, -0.000523116207F, 0.00653738249F, -0.00411110651F, - 0.00943036936F, -0.0053239367F, -0.0129429502F, 0.00894216541F, - 0.00703961868F, -0.00360614224F, 0.00181997672F, -0.000529076904F, - 0.00232596043F, 0.00748709263F, -0.0066863778F, -0.0124316616F, - -0.0017605722F, 0.0079508815F, 0.0140222702F, -0.0123445028F, 0.0101778265F, - -0.00467681745F, 0.01328234F, -0.0036260609F, 0.0133978F, 0.000354719639F, - -0.0166180637F, 0.00756371627F, -0.0127358213F, 0.00257333182F, - 0.00312412251F, 0.00249579758F, -0.00852085464F, 0.00146988838F, - 0.020766791F, 0.00566307036F, -0.0101135867F, -0.00163358543F, - -0.00708064483F, 0.0228204839F, -0.00251273089F, -0.00706484076F, - -0.00841088314F, 0.01215016F, -0.00263891066F, 0.000718549F, -0.00382606313F, - -0.00491020549F, 0.0153032811F, -0.00124880357F, 0.00351445121F, - -0.0038726076F, 0.0094214892F, 0.0256627F, 0.012522649F, 0.00255200546F, - -0.00671899319F, -0.0141264098F, -0.00433202973F, 0.00309687853F, - 0.0190265682F, 0.0116574923F, 0.0109437322F, 0.00558127556F, -0.0268949457F, - -0.00923763681F, -0.00444498472F, 0.00787948072F, -0.0129773216F, - 0.0151011171F, -0.00715430221F, 0.00847794209F, 0.00409403211F, 0.000707243F, - 0.0140500562F, 0.0100138411F, -0.0076948083F, -0.00969027728F, - -0.0149177313F, 0.00896977261F, -0.00199550157F, -0.0131098116F, - 0.00318462169F, 0.0083441427F, -0.00978375599F, 0.00798883382F, - 0.00507679768F, -0.00967734586F, 0.00669874297F, -0.00736026699F, - -0.0061285845F, 0.0190458074F, 0.00746470504F, 0.00951804686F, -0.00902506F, - 0.00109060493F, 0.00877520256F, -0.0110903857F, -0.00565637648F, - -0.00582806813F, -0.0245692171F, 0.00368502154F, -0.0157142635F, - 0.0077095353F, 0.00895956F, -0.00706925336F, 0.00908233784F, 0.00866308808F, - 0.00232248125F, 0.0154409502F, 0.00716866134F, -0.00578200212F, - -0.000610524905F, -0.00912793446F, -0.00838256069F, -0.0100322133F, - -0.00093985541F, -0.00757571356F, 0.000206646058F, -0.000663202954F, - 0.00254507549F, -0.00486279326F, -0.00740305288F, 0.00855721254F, - -0.00867691357F, -0.00822458509F, 0.00453697052F, -0.00102047878F, - 0.0153150242F, 0.000249211589F, -0.00953713898F, 0.0113502396F, - 0.0101319142F, 0.0053807185F, -0.00478726532F, 0.00240810728F, 0.0154786846F, - -0.0076637934F, 0.00664855121F, 0.0131055079F, -0.00999328773F, - -0.026354773F, -0.0131262457F, -0.0102311252F, -0.0118194139F, - -0.00525483582F, 0.00284643099F, 0.000977850868F, 0.0257401299F, - 0.0038892983F, 0.0160760023F, -0.0145659437F, 0.0160414055F, -0.0182862747F, - 0.00458926382F, -0.0126834512F, 0.00407680776F, -0.00340994028F, - 0.00484859152F, 0.00195739325F, -0.00296313618F, -0.0136676421F, - 0.00908881333F, -0.00493613817F, 0.0110675646F, 0.00410330528F, - -0.0217806734F, -0.00513170101F, -0.00437817769F, -0.0123425741F, - -0.00612765318F, 0.0209087618F, -0.00580439717F, -0.000351203285F, - -0.00781987235F, -0.00253363489F, 0.00547617488F, 0.00851825252F, - -0.0062192725F, 0.00121035811F, 0.00103597541F, 0.00241801934F, - 0.0152311707F, -0.0101349652F, 0.00203148834F, -0.0105134677F, - -0.000952470233F, -0.0124813365F, -0.0187459458F, 0.000976681593F, - 0.000859210209F }, - - { 2.03328F, 1.99986F, 1.95879F, 2.19102F, 2.0607F, 2.16974F, 1.9217F, 1.95389F, - 2.15324F, 1.91189F, 1.91909F, 2.16267F, 1.94661F, 1.94782F, 2.19605F, - 1.97606F, 2.28059F, 1.96556F, 1.04048F, 1.03998F, 1.04051F, 1.03912F, - 1.97968F, 1.03891F, 1.03885F, 1.04174F, 1.03771F, 1.03862F, 1.04153F, - 1.04163F, 1.03981F, 2.00871F, 1.99682F, 2.14776F, 2.0602F, 2.01868F, - 2.29589F, 2.27203F, 2.0769F, 2.2109F, 1.04298F, 1.04156F, 2.29F, 2.0226F, - 2.03793F, 1.85776F, 1.63752F, 1.99214F, 2.03555F, 2.1472F, 1.76625F, - 1.99231F, 2.02144F, 1.97929F, 2.16567F, 1.99138F, 2.03631F, 2.14821F, - 2.17994F, 2.13416F, 2.09999F, 1.96761F, 1.93364F, 1.89622F, 2.26265F, - 2.12192F, 2.13802F, 2.00095F, 1.87654F, 2.15126F, 1.70026F, 1.69842F, - 2.01175F, 1.81129F, 2.30347F, 1.88704F, 1.77487F, 1.88012F, 1.99795F, - 1.99149F, 1.95757F, 2.11817F, 1.86796F, 2.08149F, 2.08613F, 2.22466F, - 2.27291F, 1.9835F, 2.16079F, 2.26095F, 1.80577F, 2.10157F, 2.59112F, - 1.98879F, 1.9532F, 2.18284F, 2.27601F, 2.14676F, 1.12125F, 1.85891F, - 2.15084F, 1.82798F, 1.70656F, 1.45604F, 2.22941F, 2.04717F, 2.03135F, - 1.60305F, 2.31389F, 1.85433F, 2.14977F, 1.03556F, 2.30169F, 2.28607F, - 1.9436F, 1.9853F, 2.28149F, 2.15992F, 1.16763F, 1.22319F, 1.22309F, 1.84847F, - 1.79088F, 1.51318F, 1.13412F, 1.66967F, 1.49915F, 1.5553F, 1.81169F, - 1.71959F, 1.73995F, 2.386F, 2.13253F, 1.025F, 0.605321F, 1.11333F, 1.75505F, - 1.76271F, 1.27652F, 1.76078F, 1.55322F, 1.09465F, 2.68108F, 1.79193F, - 1.7271F, 1.55166F, 1.6633F, 1.58804F, 1.12114F, 1.82853F, 1.74376F, 1.99249F, - 1.87671F, 1.48944F, 2.15433F, 2.14485F, 2.10634F, 0.736078F, 1.68349F, - 1.68621F, 1.45029F, 1.9497F, 1.73265F, 1.03349F, 1.08613F, 1.15409F, - 0.995276F, 1.81939F, 1.85033F, 1.01402F, 1.81449F, 1.58416F, 1.73045F, - 1.76751F, 1.72277F, 1.18922F, 2.71371F, 1.77017F, 1.75979F, 1.74565F, - 2.1127F, 2.09279F, 1.14879F, 1.58405F, 1.42856F, 1.02074F, -0.282087F, - -2.27743F, 1.83459F, 1.74529F, 1.82239F, -0.546783F, 1.81971F, 1.03264F, - 0.990515F, 0.944428F, 1.80632F, 2.0859F, 0.596379F, 1.07622F, -1.03042F, - 1.87338F, 1.79768F, 1.83247F, 1.77653F, 1.72968F, 1.87932F, 1.79159F, - 0.884234F, 1.03713F, 0.89485F, 1.98598F, 1.29221F, 0.80695F, 2.01247F, - 1.8378F, 1.51011F, 1.22979F, 1.96218F, 1.85333F, 1.92422F, 0.953696F, - 1.55043F, 1.11599F, -0.00345637F, 0.534234F, 0.971214F, -3.5774F, 1.75878F, - 1.80845F, 1.79593F, 1.70944F, -1.92208F, 1.60338F, 0.91223F, 1.03931F, - -2.33295F, -1.93668F, -1.38284F, -2.10131F, -1.31333F, 1.83227F, 1.77452F, - 1.54174F, 0.289154F, 1.75089F, 0.834527F, 0.908824F, 2.00393F, -3.65983F, - 1.86552F, 1.89208F, 1.82914F, -1.85856F, 1.76887F, 1.82612F, 1.78781F, - 1.7554F, 1.67053F, 1.70524F, 1.44992F, 1.57477F, 1.75598F, 1.62242F, - 1.80238F, 1.28403F, 1.03972F, 0.283913F, 1.98303F, -1.5404F, -2.56334F, - 1.91287F, 1.88526F, 1.72715F, 1.81079F, 0.832978F, 0.578417F, -1.91079F, - -1.1669F, 2.02788F, 1.81825F, 1.62539F, 1.93365F, -0.694286F, 2.00955F, - 1.62087F, 0.600836F, 2.03159F, 1.70767F, 1.89059F, 1.6001F, 2.3544F, 1.7917F, - 1.93871F, 1.02927F, 1.51581F, -0.234797F, -1.39833F, 1.80804F, 1.76428F, - -3.73251F, -0.374167F, 1.73293F, 1.87947F, -0.626187F, 1.74712F, -0.21656F, - -0.488956F, 0.307896F, -1.45529F, -1.28969F, 1.83343F, 2.02926F, 1.75661F, - 1.7492F, 0.699203F, 1.43038F, 1.59511F, 1.65062F, -1.04239F, 1.13601F, - 0.83384F, 2.08347F, 1.71541F, 1.83589F, 2.03841F, 1.78551F, 1.85992F, - 1.30102F, 0.890814F, -1.15714F, 1.87122F, 1.59951F, 1.67606F, 2.01824F, - 1.94767F, 1.76428F, 2.15364F, -0.511249F, 1.71012F, 1.81268F, 1.14574F, - 1.07268F, -1.43304F, 1.80794F, 0.135559F, -1.73349F, 1.23395F, -1.2484F, - -2.20619F, 2.01883F, 1.77215F, 1.84048F, -1.36619F, 1.66434F, -0.551968F, - 1.70592F, -2.18434F, 1.79385F, 1.7912F, 1.73939F, 0.260397F, 1.23772F, - 1.16842F, -0.369194F, -1.24336F, 1.85858F, 1.88658F, 1.7297F, -1.57977F, - 1.4383F, 1.66844F, -1.826F, -0.695101F, -1.77628F, 1.83465F, 0.0593452F, - 1.51323F, 1.8069F, 0.591924F, -2.14132F, -1.05625F, -2.22402F, -1.22435F, - 2.18601F, 2.02667F, 1.03858F, 1.68731F, 1.6872F, -0.234189F, 1.18416F, - 1.15302F, 2.03162F, 1.71733F, 1.592F, 1.17447F, 0.803299F, 1.79308F, - -0.98057F, 1.18621F, 1.46985F, -0.97204F, -0.731462F, -0.529415F, -1.22863F, - 1.67788F, 0.355139F, 0.133903F, -0.713221F, -0.354969F, -0.607241F, - -0.161171F, 1.80764F, 1.68214F, 1.72678F, 1.79282F, 1.06338F, 1.77267F, - -1.09687F, -0.978349F, 1.01087F, -2.93687F, 0.219757F, 2.12022F, 1.91577F, - 1.80901F, 1.7406F, 1.19005F, 1.21548F, 1.08649F, -0.0688065F, -0.998716F, - -1.81441F, 2.04113F, -1.87495F, -1.933F, 1.77525F, 1.78184F, -1.71028F, - -0.609874F, 0.072312F, 1.86502F, 1.84423F, 1.55718F, -0.661683F, 1.56921F, - 1.76984F, 1.23731F, 1.76888F, -4.257F, 1.46563F, 1.22215F, 1.04808F, - -0.681766F, 1.04503F, 2.08136F, 1.84408F, -1.69258F, 1.74678F, 1.5034F, - 1.6429F, 1.55394F, 1.67443F, 1.10253F, 2.01887F, 1.99082F, -1.78465F, - 1.8427F, -1.48713F, 2.0182F, 0.208609F, 1.50768F, 1.39637F, 1.15648F, - -1.44025F, 0.360326F, 2.04364F, 1.55804F, 2.37843F, -1.52346F, -1.08805F, - -1.11983F, -0.275369F, -1.77744F, -0.83745F, -1.27353F, 0.443438F, - -0.653445F, -0.83728F, -0.1875F, 0.355772F, -1.29562F, -1.56123F, 1.04368F, - -1.44892F, 1.19237F, -0.662761F, -1.00927F, -1.25704F, 0.216458F, 2.00196F, - -0.808102F, 0.95122F, -0.720024F, -1.41439F, -1.78895F, 1.94494F, 1.8692F, - 1.8598F, 1.68034F, 1.33395F, -0.943335F, -0.596567F, 1.58819F, 1.36399F, - -1.87457F, 1.36663F, 0.9428F, -1.58294F, 0.476328F, 0.250976F, 1.63771F, - 1.38415F, 1.18164F, 0.397741F, 1.53013F, -1.92992F, 1.90813F, -1.66803F, - 1.43315F, -0.817934F, 1.31174F, -0.827204F, -1.10259F, -0.827904F, -1.32975F, - 1.04915F, -0.725361F, -0.72227F, -0.672143F, -1.34034F, -0.817547F, - -1.24452F, 1.07169F, -0.592521F, 1.34306F, -0.986045F, -0.638933F, 1.63916F, - 1.63071F, 1.34058F, -0.897741F, -0.654424F, -0.469821F, 2.33268F, 0.946204F, - -0.92767F, -0.980127F, -1.15702F, -0.728818F, -1.18656F, -1.17268F, - -1.04371F, -1.06457F, -0.899562F, -0.644788F, -1.04639F, -0.723207F, - -1.21461F, -1.00438F, -1.16995F, -1.88157F, -1.04526F, -1.18327F, -1.79123F, - -1.81881F, -1.8069F, -1.80636F, -1.80229F, -1.80693F, -1.83661F, -1.81252F, - -1.78457F, -1.80984F, -0.974018F, -0.9667F, -0.947951F, -1.79795F, -1.76884F, - -0.966374F, -1.02085F, -1.03058F, -0.972049F, -0.970031F, -1.8325F, - -1.80558F, -1.80469F, -1.81752F, -1.80622F, -1.8091F, -1.80465F, -0.755154F, - -1.79428F, -1.79435F, -1.79587F, -1.08969F, -0.970115F, -1.81971F, -1.02869F, - -1.03238F, -1.01693F, -1.92638F, -1.82308F, -1.81312F, -1.80792F, -1.80926F, - -0.961041F, -1.02769F, -1.02712F, -1.82233F, -1.80901F, -1.8001F, -1.81271F, - -1.85117F, -1.81253F, -1.79233F, -1.60435F, -1.08802F, -1.01932F, -1.00966F, - -1.07371F, -1.79385F, -1.80669F, -1.4614F, -1.82792F, -1.78317F, -1.81041F, - -1.80745F, -1.80364F, -1.80923F, -1.81793F, -1.08414F, -1.05082F, -1.77316F, - -1.79648F, -1.82012F, -1.81565F, -1.8151F, -1.79471F, -1.81731F, -1.81754F, - -1.82544F, -1.80578F, -1.81454F, -1.80687F, -1.80891F, -1.80528F, -1.79862F, - -1.80641F, -1.80757F, -1.80263F, -1.79046F, -1.80922F, -1.80889F, -1.80827F, - -1.81592F, -1.80812F, -1.80071F, -1.808F, -1.80988F, -1.91057F, -1.81296F, - -1.8073F, -1.69323F, -1.77205F, -6.85205F, -1.79862F, -1.80745F, -1.80129F, - -1.74919F, -1.70993F, -1.80844F, -1.80535F, -1.8089F, -1.80699F, -1.80882F, - -1.84581F, -1.80145F, -1.933F, -1.80856F, -1.81488F, -1.71928F, -1.68459F, - -1.78205F, -1.85439F, -1.79248F, -1.21232F, -4.29649F, -1.60449F, -1.72898F, - -1.72383F, -1.73419F, -1.76496F, -1.81043F, -1.81269F, -1.80732F, -1.16874F, - -1.53234F, -1.21928F, -1.28282F, -1.46389F, -1.22764F, -1.80605F, -1.56978F, - -1.539F, -1.43827F, -1.75203F, -1.59063F, -1.66752F, -1.65651F, -1.4939F, - -1.46785F, -1.52606F, -1.38004F, -1.47394F, -1.67804F, -1.04129F, -1.48066F, - -1.58999F, -1.62011F, -1.04559F, -1.8081F, -0.9252F, -0.855445F, -1.80525F, - -1.82681F, -0.912711F, -1.8182F, -0.959358F, -1.83334F, -1.69824F, -1.73265F, - -1.67315F, -1.70151F, -1.6014F, -1.56146F, -0.854249F, -0.919216F, -1.72285F, - -1.56818F, -0.953413F, -0.917771F, -0.924086F, -0.848533F, -0.850785F, - -1.19532F, -1.76928F, -1.72167F, -1.75579F, -1.43511F, -1.57712F, -0.917836F, - -0.948874F, -0.933692F, -0.831164F, -0.885201F, -1.81311F, -1.54231F, - -1.82523F, -0.967856F, -0.926988F, -0.863688F, -0.838998F, -0.85207F, - -1.53517F, -1.83763F, -1.81255F, -1.58986F, -0.958334F, -1.83477F, - -0.913987F, -0.910052F, -1.2255F, -0.895889F, -1.80897F, -1.89582F, - -0.956446F, -0.963572F, -2.28293F, -1.70547F, -1.80586F, -1.02529F, - -0.884818F, -1.72572F, -1.57052F, -1.80893F, -1.50736F, -1.0492F, -0.954315F, - -0.833813F, -1.77286F, -1.58715F, -0.133183F, -0.930586F, -0.143395F, - -0.196131F, -0.932037F, -0.0645388F, -1.78504F, -1.79313F, -0.251208F, - -1.70021F, -0.583566F, -1.53834F, -1.65971F, 0.318993F, 0.495046F, 0.556562F, - 0.607381F, -0.201254F, -0.879838F, -1.62895F, -0.997069F, -0.869552F, - -1.80285F, -0.140338F, -0.160034F, 0.222661F, -0.915828F, -0.977645F, - -0.979075F, -0.814821F, -1.01583F, -0.215857F, -0.194135F, 0.043671F, - -1.65215F, 0.358673F, 0.489251F, -0.516199F, 0.461077F, -0.389743F, - -1.63595F, -0.761371F, -0.692024F, -0.804857F, -0.935816F, -0.109254F, - 0.217151F, 0.118883F, 0.0436387F, 0.326215F, 0.0574342F, 0.110362F, - 0.585055F, -1.0604F, -0.186149F, 0.364966F, -0.521539F, -0.747555F, - -0.352029F, 0.0430976F, -0.492104F, -1.59052F, 0.665013F, 0.27715F, - 0.193947F, 0.597455F, 0.737174F, 0.104124F, 0.0236538F, 0.401965F, - 0.0332075F, 0.501417F, 0.731605F, 0.201645F, 0.25914F, -0.137911F, - -0.0129434F, -0.0696548F, 0.079581F, 0.0449648F, 0.29501F, 0.6231F, - 0.608006F, 0.221215F, 0.408739F, 0.458665F, 0.543099F, -0.421864F, - -0.259012F, -0.168725F, 0.369017F, 0.432392F, 0.745765F, 0.794159F, - 0.759263F, 0.80831F, 0.790123F, 0.164973F, 0.0964316F, 0.227379F, 0.295291F, - 1.41644F, -0.0219396F, 0.87028F, 0.935248F, 1.82601F, -0.115957F, 0.628203F, - 0.0273261F, 0.57276F, 0.0424671F, -0.093069F, 0.942684F, 0.581151F, - -0.43147F, 0.0824267F, 0.0461146F, 0.586984F, 0.225304F, -0.131028F, - 0.666653F, -0.0223425F, -0.448218F, 0.894004F, 0.625437F, -0.699444F, - -1.6794F, -0.73565F, -0.0840643F, 0.84061F, -0.469388F, -0.757155F, - -0.788962F, -0.118526F, 0.743605F, 0.499129F, 0.426856F, -0.781704F, - -0.528057F, -0.354219F, 0.813468F, -0.375556F, -0.0145113F, 0.0980125F, - -0.0605249F, -0.0128211F, 0.505328F, -0.214279F, 0.589454F, -0.193952F, - -0.189814F, 0.989826F, 0.65115F, 0.196479F, 0.694362F, 0.7399F, 0.0449526F, - 0.301046F, 1.83221F, -0.0590583F, 0.858979F, 0.623573F, -0.197025F, - 0.893334F, 1.01072F, 0.593527F, 0.558779F, -0.175324F, 0.12501F, 0.871458F, - 0.808911F, 0.777192F, 1.54576F, 0.784551F, -1.04357F, 0.753928F, 0.685638F, - 0.507717F, 1.48556F, 1.54602F, 0.0750286F, 1.06648F, 0.111307F, 0.937042F, - 0.317917F, -0.159487F, -0.0212159F, 0.716882F, 0.652867F, 0.758165F, - 0.0775353F, 0.567399F, 0.661031F, 1.16683F, 0.466309F, -0.0549522F, - -0.353229F, 0.301785F, 0.0231631F, -0.185641F, 1.41148F, 0.138773F, - -0.0322683F, 0.684281F, 0.924019F, 0.0727427F, 0.486772F, 1.02231F, - 0.986706F, 0.878364F, 0.864058F, 0.0962804F, 0.259775F, 0.538307F, 0.880052F, - 1.03088F, 0.87389F, 2.41125F, 0.172705F, 1.37136F, 1.00651F, 1.50378F, - 0.730466F, 1.37186F, 1.69983F, 0.695325F, 0.57958F, 0.790023F, 0.745665F, - 0.796135F, 0.0374946F, 0.799072F, 0.805946F, 0.748177F, 1.3984F, 0.543039F, - 0.705427F, 0.938688F, 0.222947F, 0.684118F, 0.756F, 0.79299F, 0.647892F, - 0.417615F, 0.746592F, 0.9545F, 1.46125F, 1.32084F, 0.752493F, 0.759105F, - 1.94359F, 0.69268F, 0.270248F, 1.06609F, 0.718092F, 0.323483F, 1.42546F, - 1.53798F, 1.21014F, 1.02301F, 0.701968F, 1.01746F, 1.39352F, 0.855727F, - 0.752683F, 1.91032F, 1.96608F, 1.85385F, 0.873583F, 1.37363F, 1.40162F, - 1.8851F, 1.886F, 1.7366F, 0.873857F, 0.650498F, 0.675489F, 0.993203F, - 0.917567F, 1.46643F, 1.72731F, 1.92566F, 0.873461F, 1.01578F, 0.648858F, - 1.1102F, 0.919475F, 1.80547F, 1.91544F, 1.61334F, 1.09075F, 0.968354F, - 1.05302F, 0.964601F, 1.04566F, 1.33984F, 1.29812F, 1.54632F, 2.26053F, - 1.74898F, 1.12006F, 1.8466F, 1.84828F, 0.794583F, 0.623801F, 0.871004F, - 1.57988F, 2.36584F, 1.05317F, 1.81698F, 1.35132F, 1.3419F, 2.15481F, - 1.12573F, 2.17266F, 1.0143F, 1.05254F, 1.02016F, 1.01939F, 1.05892F, - 2.15142F, 2.17099F, 1.01448F, 0.973597F, 1.01021F, 1.04395F, 1.05649F, - 1.45406F, 2.17718F, 1.0101F, 1.41484F, 1.58862F, 1.04664F, 1.05507F, - 1.10849F, 1.04607F, 1.02175F, 1.05188F, 1.06734F, 2.16819F, 0.996804F, - 0.983941F, 1.0565F, 1.06372F, 2.3073F, 2.18442F, 1.01214F, 2.15518F, - 2.28815F, 2.30858F, 2.29862F, 2.28855F, 2.17825F, 2.17764F, 4.18043F, - 1.0069F, 2.2908F, 2.29606F, 2.29307F, 1.82098F, 2.2736F, 2.27633F, 1.009F, - 2.28761F, 2.32534F, 2.28712F, 2.28976F, 2.17065F, 2.18301F, 2.16813F, - 2.16454F, 2.16564F, 1.90483F, 2.24723F, 2.24154F, 3.90761F, 3.01823F, - 2.1276F, 1.07241F, 1.04863F, 1.04358F, 2.28713F, 2.07078F, 2.16227F, - 2.09516F, 2.1804F, 2.13767F, 2.18512F, 2.24419F, 2.03543F, 2.30419F, - 2.24169F, 2.17193F, 2.24821F, 2.30148F, 2.28561F, 2.20723F, 2.1493F, - 2.15991F, 2.16113F, 2.04558F, 2.28982F, 2.1664F, 2.19514F, 2.08154F, - 2.13677F, 2.07532F, 2.27676F, 4.7748F, 2.23818F, 2.27377F, 2.2856F, 2.28645F, - 2.31605F, 1.05245F, 1.06382F, 2.29253F, 2.2916F, 2.27425F, 2.29537F, - 2.23747F, 2.12213F, 1.05672F, 2.28976F, 2.18711F, 1.92435F, 1.846F, 2.05518F, - 2.2891F, 2.24698F, 2.2895F, 2.30532F, 2.29732F, 2.15844F, 2.06419F, 1.05476F, - 2.29107F, 2.16589F, 1.8532F, 2.12697F, 1.91412F, 1.97004F, 1.95126F, - 2.27147F, 1.82864F, 1.92175F, 2.15475F, 2.06551F, 2.27066F, 2.32575F, - 2.29375F, 2.27037F, 2.28939F, 2.17242F, 2.28048F, 2.26576F, 2.269F, 2.25545F, - 1.04136F, 2.13328F, 2.18633F, 2.38511F, 2.29004F, 2.15556F, 2.16203F, - 2.20666F, 2.27223F, 2.27856F, 2.31604F, 2.27009F, 1.04124F, 2.27388F, - 2.26519F, 10.4976F, 1.04377F, 2.17117F, 2.14066F, 1.04151F, 1.04239F, - 1.0404F, 1.04355F, 2.32975F, 1.04057F, 1.04331F, 1.04079F, 1.04057F, 1.0416F, - 1.0414F, 2.15515F, 1.04275F, 1.04063F, 2.18316F, 1.04134F, 1.03715F, - 1.04157F, 1.04369F, 1.04044F, 1.04166F, 2.14845F, 1.04156F, 2.11433F, - 1.04297F, 1.04094F, 1.04214F, 1.04301F, 2.2291F, 1.0457F, 2.15531F, 2.16077F, - 2.17972F, 2.13771F, 2.45189F, 2.15556F, 2.06973F, 2.34685F, 1.04035F, - 1.04224F, 2.16954F, 2.14498F, 2.13781F, 1.13818F, 2.1603F, 2.20962F, - 1.04157F, 1.03964F, 1.06022F, 1.04585F, 1.04077F, 1.04119F, 1.04094F, - 1.04086F, 2.11227F, 2.12593F, 1.05403F, 1.03781F, 1.03901F, 1.0413F, 2.1789F, - 1.03933F, 2.14713F, 1.04088F, 1.04523F, 1.43709F, 1.04027F, 1.04073F, - 1.04347F, 1.04546F, 1.03946F, 1.04432F, 1.36356F, 1.36706F, 2.21688F, - 1.06916F, 1.05452F, 1.0394F, 1.0412F, 3.75186F, 1.03604F, 1.04151F, 1.04123F, - 1.04117F, 1.0358F, 3.84244F, 1.04007F, 1.04576F, 1.04903F, 1.04191F, - 1.03762F, 1.02348F, 1.04053F, 2.28062F, 2.15529F, 2.2661F, 2.14037F, - 2.14407F, 2.16771F, 2.14567F, 2.14826F, 2.14776F, 2.11512F, 2.12091F, - 2.2283F, 2.22841F, 2.29367F, 2.16364F, 2.27017F, 2.15938F, 2.12003F, - 2.16568F, 2.16595F, 2.16317F, 2.0875F, 2.11843F, 2.161F, 2.25459F, 2.23924F, - 2.08531F, 2.10928F, 2.10351F, 1.58439F, 2.29132F, 2.27041F, 2.16164F, - 2.03075F, 1.0422F, 2.16289F, 2.17535F, 2.18402F, 2.15733F, 2.17901F, - 2.15866F, 2.21052F, 2.15502F, 2.1426F, 1.96754F, 2.08526F, 2.09624F, - 2.07093F, 1.03955F, 1.03937F, 2.12895F, 1.04056F, 2.17377F, 2.20948F, - 2.08696F, 1.02668F, 1.03953F, 1.03831F, 1.03977F, 1.03953F, 1.04087F, - 2.17177F, 1.03859F, 1.03613F, 1.04166F, 1.04342F, 2.09776F, 2.2929F, - 1.04473F, 1.04055F, 1.03614F, 1.04267F, 1.04019F, 1.04382F, 2.29658F, - 1.04909F, 1.04781F, 2.41243F, 1.04014F, 1.03398F, 1.02374F, 1.03764F, - 1.03815F, 1.02636F, 1.03801F, 2.27787F, 2.17118F, 2.15182F, 2.14249F, - 2.15551F, 2.16368F, 1.991F, 2.01817F, 1.90512F, 2.26434F, 2.14252F, 2.15101F, - 2.13823F, 2.1399F, 2.14722F, 2.02981F, 2.36432F, 2.11386F, 2.19636F, - 2.11895F, 2.28981F, 2.20115F, 2.17591F, 2.14434F, 2.26323F, 2.14724F, - 2.15838F, 2.15755F, 1.99256F, 2.07659F, 2.11509F, 2.28005F, 2.29633F, - 2.29726F, 2.29243F, 2.28419F, 2.29402F, 2.28685F, 2.28773F, 2.14164F, - 2.16145F, 2.25249F, 1.86494F, 2.34354F, 2.14978F, 2.15097F, 1.80379F, - 2.30588F, 2.06189F, 2.06931F, 2.16388F, 2.29062F, 2.23371F, 2.19469F, - 2.28249F, 2.28004F, 2.26975F, 2.24561F, 2.29185F, 2.28626F, 2.30057F, - 2.30138F, 2.2454F, 2.02349F, 2.12828F, 1.47247F, 2.19115F, 2.29928F, - 2.26222F, 2.10163F, 0.951276F, 0.966301F, 2.29331F, 2.11256F, 1.98895F, - 2.28807F, 2.30033F, 2.29496F, 2.27947F, 2.17105F, 2.15485F, 2.15035F, - 2.1345F, 2.14448F, 2.15324F, 2.16269F, 2.27975F, 2.27859F, 1.91751F, - 2.25834F, 2.12558F, 2.15052F, 2.14422F, 2.19561F, 2.14074F, 2.15111F, - 1.96398F, 2.16944F, 2.14888F, 2.01952F, 2.04682F, 2.14091F, 2.26038F, - 2.02234F, 1.9912F, 1.96395F, 2.00889F, 2.01004F, 2.13808F, 2.24166F, - 2.24379F, 2.2881F, 2.27942F, 2.16627F, 2.1602F, 1.89815F, 2.27985F, 2.31216F, - 2.30768F, 2.29975F, 2.28864F, 2.16237F, 2.15819F, 2.13541F, 2.16315F, - 2.17314F, 2.27798F, 2.34505F, 1.91449F, 1.94567F, 2.15299F, 2.14799F, - 2.14224F, 2.16217F, 1.97587F, 2.13816F, 2.28144F, 2.27206F, 2.2875F, - 2.28595F, 2.17893F, 2.15223F, 2.15408F, 2.28473F, 2.28275F, 2.28893F, - 2.29168F, 2.30236F, 1.98331F, 2.14727F, 2.2983F, 2.18893F, 1.98126F, - 2.14999F, 2.13708F, 2.16752F, 2.14425F, 2.26733F, 2.14291F, 2.09317F, - 2.33681F, 2.27728F, 2.28366F, 2.28215F, 2.32665F, 2.10957F, 2.1202F, - 2.24447F, 2.19006F, 1.95125F, 2.1584F, 2.03445F, 2.28457F, 2.27184F, 2.1186F, - 2.10807F, 2.21199F, 2.29521F, 2.28645F, 2.28098F, 2.16139F, 2.17358F, - 2.23704F, 2.15741F, 2.38131F, 2.20123F, 1.88642F, 2.2783F, 2.13929F, - 2.47863F, 2.14413F, 2.1193F, 2.18277F, 2.11484F, 2.15764F, 2.14975F, - 2.15329F, 2.1479F, 2.15536F, 2.14961F, 2.14856F, 2.144F, 2.15596F, 2.1546F, - 2.15577F, 2.03997F, 1.87223F, 2.16189F, 2.14222F, 2.15051F, 2.1365F, - 2.14362F, 1.91491F, 1.91154F, 2.15164F, 2.15295F, 2.16034F, 2.15169F, - 2.16136F, 2.14802F, 1.9579F, 2.14774F, 1.9524F, 2.04878F, 2.1485F, 1.99291F, - 2.14469F, 1.96177F, 1.96517F, 1.9617F, 2.21407F, 2.22117F, 2.11754F, - 2.27142F, 2.29046F, 2.29982F, 2.16272F, 2.0134F, 2.05491F, 2.01123F, - 2.15743F, 2.27214F, 2.15181F, 2.27649F, 2.14988F, 2.16316F, 1.93979F, - 2.13595F, 2.13237F, 2.05645F, 2.26338F, 2.00247F, 2.07526F, 2.11733F, - 2.13632F, 2.17121F, 2.23677F, 2.23003F, 2.1143F, 2.1542F, 2.30107F, 1.87335F, - 2.37832F, 2.04937F, 2.29243F, 2.15431F, 2.14484F, 2.26994F, 1.89703F, - 2.27194F, 1.98164F, 1.96493F, 2.31215F, 2.29602F, 1.6861F, 2.27882F, - 2.29217F, 2.20711F, 2.18036F, 2.15645F, 2.15053F, 2.11291F, 2.16774F, - 2.2756F, 2.28361F, 2.31525F, 1.95534F, 2.16075F, 2.22333F, 1.92892F, - 2.17656F, 2.27281F, 2.18941F, 2.03452F, 2.27684F, 2.15483F, 2.21904F, - 2.19438F, 2.15198F, 2.2974F, 2.2883F, 2.32386F, 2.28138F, 2.41184F, 2.1139F, - 2.19852F, 2.1524F, 3.42694F, 2.05648F, 2.24011F, 2.28187F, 2.26665F, - 2.14064F, 2.17788F, 1.95189F, 2.22276F, 2.11598F, 2.25085F, 2.12637F, - 2.15288F, 2.11646F, 2.26205F, 2.19627F, 2.032F, 1.96465F, 2.15313F, 1.81852F, - 1.96522F, 2.15692F, 2.14563F, 2.14619F, 2.3019F, 2.2634F, 2.14963F, 2.21207F, - 2.23559F, 2.27032F, 2.28832F, 2.18773F, 2.26802F, 2.24383F, 2.09859F, - 2.23692F, 2.1482F, 2.1513F, 2.09088F, 2.15284F, 2.13881F, 2.17259F, 2.11286F, - 2.1281F, 1.98769F, 2.16546F, 2.20884F, 1.45346F, 2.14454F, 2.34663F, - 2.30516F, 2.32225F, 2.10801F, 2.12749F, 2.21796F, 2.14824F, 2.02573F, - 2.12102F, 2.15225F, 2.15577F, 2.14611F, 1.98543F, 2.21069F, 1.71598F, - 1.04969F, 2.17269F, 2.12533F, 1.68447F, 2.29612F, 2.17291F, 2.14735F, - 2.13078F, 2.14761F, 2.04045F, 2.01598F, 2.0146F, 1.62403F, 1.81656F, - 2.13581F, 1.84076F, 1.85779F, 1.92018F, 2.25485F, 2.17739F, 1.68773F, - 2.07398F, 1.63542F, 1.73277F, 1.03881F, 1.81497F, 1.55733F, 1.70746F, - 1.49281F, 1.68819F, 1.05242F, 2.24933F, 2.15988F, 2.19698F, 2.15813F, - 1.05489F, 2.26114F, 2.25491F, 2.2568F, 2.36802F, 2.02856F, 2.15885F, - 2.36392F, 2.0116F, 2.03598F, 2.01898F, 1.80037F, 1.78489F, 1.72824F, - 1.81135F, 1.70249F, 1.76983F, 1.67021F, 1.54081F, 1.79213F, 1.73192F, - 1.59194F, 1.01703F, 2.17456F, 1.99045F, 1.89307F, 1.83673F, 1.81242F, - 1.71318F, 1.70798F, 1.83368F, 1.97095F, 1.52038F, 1.81769F, 1.64912F, - 1.05559F, 1.01613F, 2.01605F, 1.99783F, 2.1435F, 1.85401F, 1.80697F, - 1.78357F, 1.77935F, 1.82452F, 1.80149F, 1.79135F, 1.74513F, 1.6608F, - 1.83296F, 0.834612F, 0.872188F, 0.868446F, 1.04231F, 1.01141F, 2.02185F, - 2.5081F, 2.21365F, 2.14939F, 1.49384F, 1.69983F, 1.73581F, 0.851303F, - 0.871306F, 2.07047F, 2.16575F, 2.13591F, 1.02217F, 1.04679F, 2.42849F, - 2.01345F, 2.13789F, 2.09645F, 2.12241F, 1.66686F, 2.16535F, 2.19702F, - 1.95409F, 1.91133F, 2.0094F, 1.93602F, 1.8696F, 1.75819F, 1.88067F, 1.73742F, - 1.78346F, 1.61353F, 1.97494F, 1.79328F, 0.826972F, 0.863006F, 1.04097F, - 1.04351F, 2.1512F, 1.78123F, 2.12674F, 2.04713F, 2.1491F, 1.89481F, 1.70146F, - 1.84316F, 1.80022F, 1.75208F, 1.52527F, 1.62068F, 1.47982F, 2.23603F, - 0.860883F, 2.27838F, 2.10595F, 2.16116F, 2.01096F, 2.00802F, 1.85878F, - 1.77634F, 1.86542F, 1.86174F, 2.24477F, 2.10188F, 1.70717F, 0.801832F, - 2.84006F, 2.16995F, 2.09685F, 1.54604F, 1.04853F, 1.79753F, 1.06992F, - 2.29322F, 2.32345F, 1.99854F, 2.01385F, 2.05073F, 1.92068F, 2.01219F, - 1.95676F, 1.78791F, 1.74702F, 1.80056F, 1.82755F, 1.81056F, 1.92641F, - 1.9742F, 1.71364F, 1.75303F, 1.73946F, 1.72533F, 1.01498F, 2.07598F, - 2.04591F, 2.02682F, 1.98647F, 2.03256F, 1.90551F, 1.92456F, 2.03094F, - 1.87039F, 1.81067F, 2.15116F, 1.72672F, 1.76549F, 1.67933F, 1.77829F, - 1.62132F, 0.825906F, 2.03506F, 2.06514F, 1.79444F, 1.03669F, 1.87882F, - 1.69982F, 1.97206F, 1.76858F, 1.81112F, 0.827986F, 0.742472F, 0.827751F, - 0.771462F, 2.29659F, 1.05309F, 1.86393F, 1.06099F, 1.03975F, 1.74479F, - 1.80753F, 2.16371F, 1.98263F, 1.04526F, 2.05334F, 1.55575F, 1.61397F, - 1.88347F, 1.03755F, 2.01127F, 2.03041F, 1.72142F, 1.02114F, 0.769079F, - 0.770553F, 2.27206F, 1.90772F, 1.8416F, 2.03776F, 1.49465F, 2.28681F, - 2.02659F, 2.0091F, 1.59763F, 2.06213F, 2.58408F, 3.28856F, 1.16152F, - 1.48722F, 1.15788F, 2.01949F, 1.78097F, 1.66818F, 0.982153F, 0.781324F, - 2.03479F, 2.02428F, 1.88977F, 1.71719F, 1.69928F, 1.71559F, 1.16691F, - 1.11889F, 1.15963F, 1.13763F, 1.16079F, 1.13526F, 1.07192F, 1.09476F, - 1.13664F, 1.08502F, 1.78843F, 1.6696F, 1.55256F, 1.10716F, 1.43102F, - 1.16343F, 1.17387F, 1.12449F, 1.10859F, 1.0888F, 1.78035F, 1.49018F, - 1.14534F, 1.80671F, 2.01303F, 2.18063F, 2.0115F, 2.0383F, 1.15753F, 1.27438F, - 1.15464F, 1.17245F, 1.70879F, 1.02298F, 1.07281F, 1.14037F, 1.15143F, - 1.07808F, 1.16481F, 1.77467F, 0.773113F, 1.01852F, 1.39608F, 1.12037F, - 1.64577F, 0.991981F, 2.12542F, 1.58824F, 1.1151F, 1.02424F, 0.988275F, - 1.09032F, 1.14721F, 1.07027F, 0.988756F, 1.1513F, 1.07325F, 0.995093F, - 1.10796F, 0.998739F, 1.04921F, 1.07602F, 1.02909F, 1.17389F, 1.08982F, - 1.96099F, 1.76071F, 1.86122F, 1.45725F, 1.13652F, 1.22515F, 1.08889F, - 1.18791F, 1.061F, 1.86317F, 1.82071F, 1.66649F, 1.82786F, 1.08084F, - 0.815165F, 0.848754F, 0.826491F, 0.785656F, 1.87292F, 1.10913F, 1.83217F, - 1.97644F, 1.97913F, 0.861142F, 1.96157F, 2.07221F, 1.54747F, 2.03134F, - 2.13591F, 1.99208F, 1.35342F, 2.39645F, 8.95678F, 1.39598F, 1.53673F, - 1.66085F, 0.910826F, 1.09334F, 0.926097F, 1.47619F, 1.16846F, 1.07026F, - 0.956969F, 1.0397F, 1.07494F, 1.0791F, 1.77278F, 1.41132F, 1.60702F, - 0.91508F, 0.943493F, 1.46816F, 1.05341F, 0.931257F, 1.05442F, 0.998375F, - 1.97542F, 1.88252F, 2.0614F, 1.38929F, 1.31952F, 1.07571F, 2.46496F, - 1.16496F, 1.12206F, 1.23561F, 1.03951F, 1.05294F, 1.72282F, 1.6015F, - 1.58112F, 1.56943F, 2.48399F, 2.22133F, 1.58271F, 1.44643F, 1.56632F, - 2.12214F, 2.18295F, 1.79342F, 1.8037F, 2.3622F, 1.68328F, 2.04923F, 4.5107F, - 2.11778F, 1.7655F, 0.760528F, 1.8189F, 1.12019F, 1.60131F, 1.73231F, - 1.87789F, 2.02372F, 1.81208F, 1.77875F, 1.55012F, 1.61732F, 1.64145F, - 1.71566F, 1.73269F, 1.24856F, 1.79057F, 6.55175F, 2.29203F, 1.98677F, - 1.07314F, 1.12145F, 2.10666F, 1.47889F, 1.01659F, 1.87366F, 1.16161F, - 1.33907F, 1.1869F, 1.22622F, 1.83013F, 2.39856F, 1.75391F, 0.78529F, - 2.11148F, 1.01871F, 1.63015F, 1.995F, 2.61597F, 2.02799F, 2.39778F, 2.06893F, - 1.63646F, 1.04903F, 1.03436F, 1.89491F, 1.38112F, 1.04738F, 1.02823F, - 1.04163F, 1.04188F, 1.04583F, 1.03675F, 2.29026F, 2.26126F, 1.13352F, - 2.04028F, 2.06611F, 1.76553F, 1.03864F, 2.15277F, 1.03833F, 2.15221F, - 2.1354F, 1.02705F, 1.04119F, 1.78183F, 2.27675F, 2.28029F, 1.04299F, - 1.64562F, 2.3588F, 1.03874F, 1.03515F, 1.12946F, 1.18115F, 1.10379F, - 1.18625F, 1.98665F, 2.0231F, 2.17194F, 1.67344F, 1.42834F, 1.92966F, - 2.06291F, 1.79508F, 1.65398F, 1.68953F, 1.46355F, 1.02326F, 1.03394F, - 1.33699F, 1.42498F, 1.30809F, 2.02832F, 2.28842F, 2.12075F, 0.985279F, - 1.09017F, 1.12496F, 1.27215F, 1.16858F, 1.19719F, 1.20559F, 1.19755F, - 1.9937F, 1.05649F, 1.99795F, 1.74068F, 0.682716F, 2.28605F, 1.5722F, 2.2752F, - 1.05984F, 1.01909F, 1.05627F, 1.0636F, 1.04059F, 0.738372F, 0.738459F, - 0.65618F, 1.70378F, 1.66743F, 1.96074F, 1.06105F, 1.93777F, 1.02675F, - 1.27059F, 1.0296F, 1.47909F, 2.49283F, 0.99915F, 1.1328F, 1.16206F, 1.2394F, - 1.15055F, 1.17834F, 1.16747F, 0.937002F, 1.9856F, 1.04759F, 1.87977F, - 2.0343F, 1.03755F, 1.56053F, 2.1794F, 1.63259F, 1.74249F, 1.91928F, 1.76372F, - 1.77364F, 1.76519F, 1.71152F, 2.35504F, 1.15225F, 1.59201F, 1.4852F, - 1.52691F, 0.758838F, 0.822286F, 0.738268F, 1.15453F, 0.895969F, 0.944932F, - 2.27323F, 2.25872F, 2.1376F, 1.6009F, 1.87647F, 1.8449F, 1.97657F, 1.0332F, - 1.82512F, 1.83159F, 1.66286F, 1.76694F, 2.13125F, 0.968397F, 1.03713F, - 1.77713F, 1.83405F, 1.78055F, 1.04444F, 1.81311F, 1.46305F, 1.35951F, - 0.682183F, 0.751485F, 0.759055F, 0.779975F, 0.764889F, 0.908138F, 0.718647F, - 1.06322F, 0.748576F, 0.126941F, 1.10941F, 0.921059F, 1.18315F, 0.928839F, - 2.29285F, 2.35325F, 1.94316F, 0.893543F, 1.68814F, 1.73613F, 1.18086F, - 1.45968F, 2.42148F, 2.02063F, 0.853824F, 1.15542F, 1.10025F, 2.14508F, - 1.90937F, 1.79014F, 1.97707F, 1.04758F, 1.04999F, 2.14329F, 1.03112F, - 1.05781F, 1.39285F, 1.83154F, 1.04527F, 2.03989F, 2.08243F, 1.99884F, - 1.17959F, 1.60029F, 1.77172F, 1.44052F, 1.18386F, 1.45939F, 1.51496F, - 0.66893F, 0.728603F, 1.03135F, 1.18838F, 1.24503F, 0.876724F, 0.763282F, - 0.188233F, 0.112673F, 1.38181F, 0.691272F, 0.901255F, 2.27694F, 1.43314F, - 1.66681F, 2.25503F, 1.03486F, 1.0036F, 1.67219F, 1.33897F, 1.10598F, - 0.797503F, 1.43705F, 1.21442F, 0.698558F, 0.915561F, 1.50426F, 1.19699F, - 0.743789F, 0.731188F, 0.960082F, 0.990288F, 2.33418F, 1.9943F, 2.07029F, - 1.43021F, 2.1078F, 1.06704F, 1.42936F, 2.08973F, 0.97478F, 2.15389F, - 1.92228F, 2.0873F, 1.87922F, 2.09333F, 1.28081F, 2.09033F, 2.2117F, - 0.799698F, 0.597975F, 1.44439F, 1.26372F, 36.6085F, 0.62319F, 1.16634F, - 1.10864F, 0.962898F, 1.09181F, 0.762632F, 0.853467F, 2.27486F, 1.31964F, - 2.25165F, 0.848063F, 0.140886F, 0.495844F, 2.14095F, 1.80386F, 1.20949F, - 0.980264F, 0.710944F, 0.51175F, 0.718656F, 1.62264F, 1.82044F, 1.01942F, - 4.0085F, 2.0953F, 0.872884F, 0.570491F, 0.724402F, 3.60324F, 1.59311F, - 0.70075F, 0.597931F, 1.21955F, 3.43774F, 1.23466F, 1.18545F, 1.17068F, - 1.19926F, 1.96908F, 1.04791F, 1.02796F, 2.11272F, 1.66597F, 2.04466F, - 0.770173F, 0.832901F, 1.19291F, 0.918608F, 2.34967F, 2.14529F, 2.27412F, - 1.04927F, 1.75265F, 2.24201F, 2.13194F, 1.02437F, 2.12261F, 1.07508F, - 2.07614F, 1.05238F, 1.03243F, 1.79676F, 1.83644F, 2.10807F, 0.843049F, - 0.937232F, 0.732107F, 1.02592F, 2.30232F, 2.26106F, 2.30314F, 1.94821F, - 2.13589F, 1.02015F, 2.09123F, 1.67194F, 1.10115F, 1.05405F, 1.02831F, - 1.64189F, 1.58478F, 1.10986F, 0.838348F, 0.724062F, 1.09821F, 2.3031F, - 0.994969F, 2.14315F, 1.04035F, 1.74662F, 1.02473F, 1.42102F, 0.845414F, - 0.993116F, 2.22265F, 1.27344F, 1.92697F, 2.23235F, 1.66995F, 1.5469F, - 1.09178F, 2.05741F, 1.05515F, 0.854948F, 1.05685F, 2.21583F, 1.88543F, - 1.0186F, 0.931315F, 2.3545F, 2.4877F, 2.54296F, 1.83724F, 2.08396F, 1.76057F, - 0.95189F, 1.07465F, 2.81646F, 0.87147F, 0.757406F, 1.70066F, 1.10295F, - 2.34451F, 0.907056F, 1.05193F, 1.22648F, 1.83166F, 1.792F, 1.96688F, - 1.75005F, 1.0311F, 1.0829F, 1.02906F, 1.87447F, 1.04793F, 1.04932F, 1.94666F, - 1.06077F, 1.03482F, 2.30199F, 2.14616F, 1.06548F, 1.04173F, 1.77458F, - 1.70024F, 1.04479F, 1.0445F, 1.04176F, 1.60859F, 0.765076F, 1.13884F, - 2.76885F, 0.945309F, 1.57941F, 0.991003F, 2.33063F, 1.05153F, 1.03861F, - 1.08301F, 1.78744F, 1.93306F, 0.912097F, 1.0539F, 0.916066F, 1.70848F, - 0.709599F, 2.13147F, 2.23772F, 1.11129F, 2.09206F, 1.70831F, 1.04415F, - 1.16671F, 1.05521F, 0.979699F, 1.68437F, 2.82458F, 2.31748F, 2.28375F, - 2.2419F, 1.73383F, 1.05155F, 1.05346F, 1.52683F, 1.69761F, 1.69528F, - 0.879016F, 2.15079F, 1.07829F, 1.07715F, 1.0441F, 1.07156F, 1.0435F, - 1.81827F, 1.04331F, 1.04941F, 1.76362F, 0.984666F, 1.71838F, 2.13004F, - 2.17184F, 1.91911F, 1.63615F, 2.13722F, 0.966848F, 1.06729F, 1.05114F, - 1.03611F, 1.05116F, 1.03433F, 1.04901F, 1.05795F, 1.95998F, 0.725524F, - 2.17035F, 2.48488F, 2.13212F, 1.15462F, 0.731304F, 2.28253F, 2.19688F, - 0.69068F, 1.19838F, 0.979111F, 1.93356F, 1.74452F, 1.96589F, 1.07898F, - 1.06134F, 1.04775F, 1.03892F, 1.04723F, 1.04535F, 1.98684F, 1.80158F, - 0.783753F, 1.06382F, 1.05857F, 0.955948F, 0.995305F, 2.10865F, 1.05263F, - 1.04139F, 1.04359F, 2.12415F, 1.00629F, 1.0144F, 0.994248F, 1.84067F, - 1.05079F, 0.705149F, 0.937367F, 2.03345F, 1.07523F, 0.742229F, 0.530977F, - 1.00298F, 1.05903F, 1.01714F, 1.06928F, 1.00541F, 1.92677F, 1.1139F, - 0.736231F, 1.09608F, 1.86337F, 0.960315F, 2.07509F, 0.969906F, 2.36335F, - 1.49127F, 0.956498F, 1.05426F, 1.71861F, 1.75039F, 0.9561F, 1.04463F, - 1.02854F, 1.95436F, 1.03443F, 1.05873F, 2.04989F, 1.07858F, 1.04327F, - 1.05032F, 1.05409F, 1.05368F, 1.05816F, 2.02784F, 1.17662F, 1.06634F, - 0.740587F, 0.823733F, 1.01407F, 1.96395F, 0.824624F, 0.985279F, 1.13725F, - 2.73267F, 1.91492F, 0.945961F, 0.947552F, 0.950801F, 1.91371F, 1.04134F, - 2.03737F, 1.0445F, 1.04765F, 2.02152F, 1.04347F, 2.00132F, 1.04613F, - 1.04212F, 1.04427F, 1.05305F, 1.05637F, 2.31211F, 2.04385F, 1.03839F, - 1.99644F, 1.04032F, 1.96506F, 1.04927F, 1.04253F, 1.03828F, 1.03912F, - 1.04757F, 1.0179F, 1.05518F, 2.14333F, 1.0403F, 1.02703F, 2.01868F, 2.00652F, - 1.12468F, 0.815259F, 1.07118F, 1.05197F, 0.736822F, 1.04285F, 1.97935F, - 1.69408F, 1.08261F, 2.05998F, 1.94039F, 2.05743F, 2.09246F, 1.94493F, - 2.27669F, 2.09539F, 1.04298F, 1.04569F, 1.04318F, 1.04318F, 1.04262F, - 2.17093F, 2.27733F, 0.821124F, 0.739025F, 2.30566F, 2.31143F, 1.04393F, - 1.04577F, 1.04854F, 1.04228F, 1.04491F, 1.04618F, 1.04595F, 2.16902F, - 2.18944F, 1.0349F, 2.13668F, 1.03956F, 1.11728F, 0.825069F, 2.28546F, - 1.87051F, 2.27805F, 1.99365F, 2.28917F, 2.32051F, 2.27608F, 1.0397F, - 2.28014F, 1.05432F, 1.04241F, 2.17947F, 2.28827F, 2.01934F, 1.04343F, - 1.0384F, 1.04651F, 1.03771F, 1.90771F, 2.27884F, 2.28181F, 2.27527F, - 2.27134F, 1.21865F, 1.05417F, 1.06313F, 0.814715F, 2.29927F, 2.297F, - 2.29776F, 2.22064F, 1.04834F, 1.04873F, 1.87455F, 2.26383F, 1.03976F, - 1.09594F, 1.04675F, 2.21414F, 2.15297F, 2.15341F, 5.97349F, 2.0876F, - 1.21289F, 1.22279F, 2.28927F, 16.0201F, 2.63539F, 2.31823F, 2.27756F, - 2.28869F, 2.27345F, 2.17281F, 2.26242F, 2.22345F, 1.04521F, 1.04609F, - 2.28035F, 2.2883F, 2.2783F, 2.23629F, 2.30646F, 2.29504F, 2.2937F, 2.31279F, - 2.23973F, 2.26764F, 2.29467F, 2.28468F, 2.29809F, 2.30972F, 2.18509F, - 2.29663F, 2.29007F, 2.28508F, 2.30778F, 2.31657F, 2.14735F, 2.16699F, 2.276F, - 2.26687F, 2.27173F, 2.12702F, 2.26597F, 2.16944F, 2.29648F, 2.29345F, - 2.25665F, 2.3121F, 2.17184F, 2.16126F, 2.37684F, 3.06298F, 2.28307F, - 2.18626F, 2.2657F, 1.04461F, 2.30003F, 2.34229F, 2.18791F, 2.3072F, 2.13241F, - 2.38893F, 2.28018F, 2.16559F, 2.22413F, 2.16706F, 2.26938F, 2.4239F, - 2.35635F, 2.17056F, 2.19496F, 2.28338F, 2.13208F, 2.31463F, 2.31742F, - 2.28341F, 2.14505F, 2.61124F, 1.04512F, 2.07549F, 2.2442F, 1.04093F, - 1.04218F, 1.03925F, 1.03851F, 2.24171F, 1.04494F, 2.02264F, 2.28348F, - 2.07778F, 2.07683F, 2.06617F, 2.22542F, 2.18966F, 2.14013F, 2.24125F, - 2.20225F, 2.271F, 2.38959F, 2.2809F, 2.32221F, 1.9429F, 1.04267F, 2.69478F, - 1.04623F, 2.25154F, 1.04303F, 1.04045F, 1.03594F, 2.26185F, 2.26971F, - 1.04273F, 2.20403F, 1.04677F, 1.03895F, 1.04117F, 2.33998F, 2.2851F, - 2.11999F, 2.11143F, 2.1428F, 2.13432F, 2.33214F, 2.28911F, 2.29761F, 2.3058F, - 2.26992F, 1.04677F, 2.23028F, 2.33362F, 2.36575F, 2.37454F, 1.04911F, - 1.03547F, 1.04336F, 2.28751F, 2.27483F, 1.04417F, 2.24081F, 1.04034F, - 2.18975F, 2.30961F, 1.98584F, 1.04409F, 2.28185F, 2.31483F, 2.05584F, - 2.09131F, 2.1934F, 2.42115F, 2.29341F, 2.32915F, 2.1784F, 2.23215F, 2.39535F, - 2.28555F, 1.04958F, 2.2432F, 2.42214F, 1.04493F, 1.0426F, 1.04343F, 2.56918F, - 2.20294F, 2.13947F, 2.22004F, 2.27763F, 2.31823F, 2.29851F, 2.24581F, - 2.28307F, 2.02466F, 2.5682F, 2.37651F, 2.18612F, 1.03789F, 2.29561F, - 2.23978F, 2.00537F, 2.05349F, 2.29366F, 2.77561F, 2.3107F, 2.1694F, 2.20657F, - 12.6842F, 2.37135F, 2.29734F, 2.29086F, 2.17826F, 2.30107F, 2.15799F, 2.215F, - 2.30319F, 2.15818F, 2.14915F, 2.23031F, 2.30045F, 2.07915F, 1.73557F, - 1.6715F, 6.5377F, 2.19764F, 2.14636F, 2.20826F, 2.23831F, 2.28172F, 2.30534F, - 2.16436F, 2.12881F, 2.13978F, 2.1969F, 2.12685F, 2.34135F, 2.2468F, 2.33198F, - 2.26472F, 3.33342F, 2.15182F, 2.34434F, 3.14443F, 2.27853F, 2.2781F, - 2.22102F, 1.04242F, 2.24064F, 2.28694F, 2.29617F, 2.13455F, 2.08201F, - 2.29221F, 2.27295F, 2.08349F, 2.24931F, 2.14039F, 2.17064F, 2.28463F, - 2.30863F, 2.2535F, 1.04383F, 2.55327F, 1.04669F, 2.28985F, 1.91804F, - 1.04176F, 1.91332F, 1.12753F, 2.28683F, 2.26519F, 3.35527F, 2.13644F, - 2.44644F, 2.28246F, 2.53299F, 2.30157F, 2.28999F, 2.27518F, 2.1112F, - 2.29593F, 2.29885F, 2.27866F, 2.27994F, 2.33417F, 2.18577F, 1.04405F, - 2.28142F, 2.31722F, 2.49078F, 1.03007F, 2.29704F, 1.05625F, 1.04086F, - 1.03915F, 1.04806F, 1.04663F, 1.03269F, 2.30051F, 2.16293F, 2.24953F, - 2.28786F, 2.1346F, 2.29337F, 2.34333F, 2.38782F, 2.23638F, 2.08071F, - 1.03494F, 2.14761F, 2.07144F, 2.2831F, 2.23383F, 2.23434F, 1.04165F, - 2.46572F, 1.99336F, 2.04336F, 2.27289F, 2.2621F, 2.30925F, 2.27803F, 2.2847F, - 2.3009F, 1.30562F, 2.59012F, 2.28806F, 9.38547F, 2.28581F, 2.27299F, - 2.27123F, 2.27998F, 2.26339F, 4.43615F, 2.26026F, 2.15696F, 2.2869F, 1.0424F, - 2.14843F, 2.29943F, 2.25713F, 1.03952F, 2.26927F, 2.20017F, 2.20622F, - 2.26562F, 2.30597F, 2.27419F, 2.37239F, 1.06849F, 2.42636F, 2.23739F, - 2.26165F, 1.04198F, 2.33022F, 1.04003F, 2.27999F, 1.04401F, 1.04223F, - 1.04428F, 1.04133F, 2.30358F, 2.29065F, 2.08523F, 2.05886F, 2.23366F, - 2.09969F, 2.1045F, 2.13746F, 2.3151F, 2.28828F, 2.26638F, 2.29064F, 2.28422F, - 2.05906F, 1.04534F, 2.27108F, 2.26589F, 2.27259F, 2.33601F, 1.04691F, - 2.29435F, 2.26388F, 2.28859F, 2.246F, 2.28363F, 2.2907F, 2.27872F, 2.27725F, - 2.34149F, 2.18545F, 1.0447F, 1.0484F, 2.34968F, 2.29019F, 1.04745F, 2.50273F, - 2.20267F, 5.40449F, 2.11522F, 2.26458F, 2.26567F, 2.54464F, 1.91366F, - 1.74888F, 1.82154F, 1.72592F, 1.71169F, 2.27377F, 2.24753F, 1.80373F, - 1.61995F, 2.11283F, 1.78779F, 2.2815F, 2.31054F, 1.04336F, 1.04337F, - 1.04163F, 1.50904F, 1.04359F, 1.04086F, 1.04252F, 1.04137F, 2.61935F, - 1.04189F, 1.03938F, 2.32328F, 1.94795F, 1.97355F, 1.97073F, 1.91666F, - 1.8032F, 1.67332F, 2.02511F, 2.04237F, 1.9574F, 1.99791F, 1.87509F, 1.81136F, - 1.68136F, 1.97607F, 1.65484F, 1.78661F, 1.89204F, 1.70837F, 1.04476F, - 1.96507F, 1.06312F, 1.05336F, 1.04281F, 1.04031F, 1.0425F, 1.04688F, - 2.10551F, 1.04295F, 1.05528F, 1.84378F, 1.74424F, 1.71302F, 2.0418F, - 1.80228F, 1.67631F, 1.95705F, 2.03379F, 1.73389F, 1.99921F, 1.94566F, - 1.82396F, 1.73799F, 2.03532F, 1.95429F, 1.95419F, 2.18793F, 1.07396F, - 1.0346F, 2.12864F, 1.87458F, 1.96307F, 2.01419F, 2.03547F, 1.9954F, 1.97033F, - 1.9654F, 1.87306F, 1.03744F, 1.05666F, 1.11325F, 2.10282F, 1.1405F, 1.71583F, - 0.902264F, 1.80195F, 0.939859F, 1.74874F, 1.6301F, 1.87393F, 1.59338F, - 1.74866F, 1.86079F, 1.70384F, 1.78095F, 1.02944F, 1.93516F, 1.79706F, - 1.79377F, 1.70217F, 1.6403F, 1.60857F, 1.64746F, 1.08425F, 2.38002F, - 1.18705F, 2.22736F, 1.30659F, 2.35251F, 2.40828F, 1.08969F, 1.06287F, - 2.0065F, 0.931326F, 1.77775F, 0.831657F, 1.12462F, -1.33847F, -2.00272F, - 1.17186F, 0.892533F, 0.92381F, 0.982275F, 1.92376F, -1.5004F, 1.90394F, - 1.87756F, 1.92894F, 0.752157F, 0.535457F, 2.01679F, 1.87514F, 1.87579F, - 1.90945F, 2.01165F, 1.02989F, 0.875081F, 1.87724F, 2.01974F, 0.309325F, - 1.66284F, 2.18098F, -0.176686F, 2.18062F, 0.954172F, 0.784196F, 0.405227F, - 0.755947F, 0.917274F, 1.13099F, 0.830345F, 0.826032F, 0.59101F, 0.588274F, - 1.88003F, 0.973376F, 0.828628F, 0.7457F, -0.497536F, 1.88501F, 1.95247F, - 0.916072F, 1.75212F, 0.502989F, 1.54531F, 1.18337F, 1.22112F, 0.867054F, - 1.90354F, 1.93922F, 1.08413F, 0.741916F, 1.96913F, 2.25567F, -1.03728F, - 0.230028F, 1.19648F, 1.98012F, 0.838137F, 0.583926F, 0.888979F, -2.04865F, - 1.17206F, 2.02483F, 0.939932F, 1.10394F, 1.58337F, 1.14246F, 0.942675F, - 0.855967F, 0.857454F, 1.72212F, 1.72923F, -0.749039F, 0.293748F, -1.70562F, - 0.748769F, 1.45876F, -6.38901F, -2.71951F, -2.37523F, -0.944949F, -2.57627F, - -4.49822F, -2.44772F, 0.291078F, 1.14518F, 0.572992F, -0.623377F, -1.80719F, - 1.60672F, 0.753753F, 0.595062F, -0.647536F, 1.58616F, -0.736487F, 0.0120021F, - 0.751498F, 0.553427F, 1.76914F, 0.00874523F, 1.14989F, 0.830913F, 0.579372F, - 1.58324F, 1.32775F, 1.24631F, -1.24876F, 1.26731F, 0.780379F, 1.16197F, - 1.10195F, 1.29084F, -7.66854F, -0.625948F, 1.34582F, -0.863014F, 0.925653F, - -7.83313F, 0.977236F, -3.52228F, 1.71436F, 1.59288F, 1.16055F, 0.988962F, - 0.81718F, 0.845102F, -3.50461F, 1.74468F, 0.330927F, 1.66273F, -1.737F, - -1.20656F, -1.64565F, -0.85621F, -1.14376F, -1.23593F, -0.829371F, 1.51566F, - 1.25432F, 0.101035F, 0.0812328F, 0.125914F, -1.02914F, 0.0727827F, - 0.0568395F, -1.44624F, -0.130579F, 1.81533F, 1.53333F, -1.91079F, 1.7032F, - 1.24195F, 1.62713F, 1.20996F, 1.54426F, -1.05231F, -1.44639F, 1.91665F, - 0.0315219F, 1.72917F, 1.09068F, 0.944744F, 1.04578F, -1.08375F, 1.77589F, - 0.328672F, 1.92362F, -1.65112F, 0.897589F, 1.96022F, -0.267397F, 1.80104F, - 0.143354F, 1.71778F, 0.91043F, -1.02963F, 1.80191F, 0.288134F, 1.78641F, - 1.82661F, 1.32902F, -1.36033F, 1.89895F, 1.77694F, 1.45434F, 0.573841F, - -0.986865F, -1.02094F, 1.9774F, 0.420535F, 1.84048F, -0.218946F, -0.66097F, - -0.0893525F, -0.955901F, 1.97983F, 1.79106F, 1.85892F, -1.87301F, -0.411697F, - 0.464377F, -0.540818F, -0.955729F, 0.286528F, 1.70992F, -1.70526F, 1.91386F, - 1.96737F, 1.87802F, 1.56167F, 1.90731F, -1.7125F, -1.69471F, -2.36411F, - 0.918787F, 0.95008F, 1.17421F, 1.07826F, 0.979496F, -1.80237F, -1.64848F, - -1.44937F, 1.7883F, -0.181163F, -0.232216F, -0.0966118F, 0.30574F, - -0.722339F, 1.33411F, -2.41162F, -1.71971F, 1.63196F, -0.494419F, -0.466062F, - 0.291375F, 0.098479F, -0.977452F, 0.261475F, -0.399174F, 0.23947F, - -0.478515F, -0.595581F, -1.37353F, -1.67341F, -0.244934F, -0.301653F, - -0.613647F, -0.812028F, -0.901457F, -1.62528F, -0.798585F, -0.958664F, - 0.400748F, -0.482057F, 1.31381F, -0.987803F, -0.744618F, -1.02953F, - -1.82993F, -2.05918F, -0.520848F, -0.529962F, -0.773438F, 0.0796551F, - -1.81047F, -1.12766F, 1.15403F, 1.31621F, -0.686968F, 0.583612F, 0.674824F, - -0.986545F, -2.6154F, -0.911809F, -0.947325F, -0.682956F, 0.309548F, 1.244F, - -0.823428F, -1.39386F, 0.337878F, 1.27416F, -0.720354F, -1.38104F, -2.6552F, - -2.24205F, -2.27746F, -0.752642F, -2.32065F, -0.928979F, -1.54912F, 1.3779F, - 1.57967F, 1.32677F, 1.19779F, -0.416828F, -2.21001F, -1.18923F, -1.62344F, - -0.842525F, -0.987308F, 0.415682F, 1.61397F, 1.88911F, 1.54715F, 1.99527F, - 1.6657F, 0.504379F, 0.951926F, -1.74352F, -1.304F, -0.507313F, -1.30555F, - -0.357877F, 1.16223F, -1.13652F, -0.934313F, -0.950287F, -1.20632F, -1.5055F, - -2.1203F, 1.83539F, 1.74508F, -0.618179F, 1.58985F, 1.78404F, -1.69565F, - -0.36553F, 1.18958F, 1.05443F, -0.736113F, -0.614703F, -0.675553F, -1.87068F, - -1.54806F, 1.72763F, -0.851429F, 0.340813F, 2.12705F, -1.06184F, -0.897446F, - -0.838731F, -0.375405F, 1.18898F, 0.54997F, -0.0495322F, -2.12562F, - -1.92263F, -1.88596F, 0.192874F, 0.333419F, -0.889875F, -0.642683F, - -0.420254F, 1.18607F, 1.03115F, -1.40123F, -0.448115F, -7.07862F, -1.86975F, - -3.75462F, -0.848799F, -0.556086F, -0.18588F, 0.510234F, 0.261373F, 1.64196F, - -0.791036F, 1.12593F, 1.61399F, -1.66774F, 1.20399F, -0.697238F, 1.31188F, - 0.931656F, 1.79035F, -1.51607F, 1.69188F, 1.30277F, 1.20831F, -2.57207F, - 1.9532F, 1.78454F, 1.74182F, 1.13602F, -0.992394F, -0.184874F, -0.892225F, - 1.64046F, -1.08351F, -1.08174F, 1.86563F, 1.83809F, -1.89872F, -0.250643F, - -0.570257F, 1.68736F, -1.79251F, 0.788839F, -1.05232F, 2.23448F, -0.950246F, - 0.467931F, -1.78136F, 1.72839F, -1.04002F, -0.741718F, 1.17113F, 0.66073F, - -2.3596F, -0.646846F, -1.36246F, -1.808F, 2.04765F, 1.7087F, 1.73877F, - 1.64986F, 1.19285F, -0.635235F, -1.88333F, -1.67148F, -1.8831F, -1.8393F, - -2.74221F, 1.74306F, 0.368806F, -0.904462F, 1.55182F, -0.496959F, -1.57106F, - -1.58205F, 0.257738F, -0.80432F, 1.74294F, -1.11493F, 1.71208F, -0.62606F, - 0.930333F, -1.23254F, 0.51422F, 2.13924F, -2.24455F, -1.06787F, 1.27171F, - -1.05716F, -1.0886F, -0.619575F, -0.590753F, -0.146357F, 0.138282F, - 0.332316F, 0.283393F, -1.49265F, -1.06448F, -0.567266F, -0.969425F, -1.4489F, - 1.77274F, -1.7335F, 1.65079F, -0.5879F, -0.576817F, -1.48507F, 1.27139F, - -1.5881F, -0.990171F, -1.11288F, -0.441728F, -0.541202F, 1.1264F, -1.03749F, - -1.79863F, -0.645266F, -0.570781F, 1.70819F, 1.81979F, 0.258189F, -0.612656F, - -0.842904F, 1.7975F, 1.48604F, 1.31824F, -1.95333F, -0.973899F, -1.69951F, - -1.29503F, 0.432485F, 1.20072F, 1.32176F, -0.638046F, 1.16373F, -1.70306F, - -2.61403F, -0.990852F, -0.903397F, -0.428452F, -0.639369F, -0.0594257F, - -1.35806F, -1.94497F, 1.74173F, -1.26651F, -1.66581F, 1.70679F, 1.76031F, - -1.39884F, -0.698626F, -0.55905F, 1.08511F, 0.447852F, -0.910643F, 2.05792F, - -1.3075F, -0.795721F, -1.31075F, -1.2973F, -1.43614F, 2.02556F, -0.420488F, - 1.34587F, -1.08091F, -1.9826F, -0.956889F, 1.7637F, -1.94721F, 1.73397F, - -0.57147F, 0.0262949F, -0.424144F, -0.828215F, -0.862622F, -0.805702F, - -0.517686F, -9.77058F, -3.30248F, 1.2915F, -0.920187F, -1.84506F, 1.54185F, - 1.29909F, -0.690474F, -1.04659F, -0.791813F, -1.36304F, -3.09522F, -1.1131F, - -0.875488F, -1.37082F, -1.01836F, -1.14798F, -0.999019F, 0.318273F, - -1.09217F, -0.569767F, -0.904999F, -1.4009F, -0.632115F, -0.548795F, - -0.895012F, -0.936723F, -0.69363F, -1.13136F, 0.659549F, -1.27951F, - -0.585432F, -1.27353F, -0.355179F, -0.98056F, -0.93888F, 1.33735F, -1.68791F, - -0.554329F, -1.05343F, -0.810022F, -1.36353F, -1.07531F, 1.14209F, -1.02085F, - -1.41114F, 0.334688F, -1.46162F, -1.01209F, -0.455529F, -0.547194F, - -1.10951F, -0.611988F, 1.83853F, -0.645337F, -0.477626F, -0.678016F, - 1.75157F, -2.99164F, -1.31003F, -3.55806F, -0.698725F, 1.49149F, -0.901371F, - -0.829915F, -2.00168F, -1.18891F, -0.826452F, -1.80921F, -0.681447F, - 1.56028F, 1.09417F, -1.08191F, -0.653956F, -0.569575F, -1.02124F, -1.1253F, - -0.995573F, -0.587754F, -0.107058F, -1.40161F, -1.38931F, -0.984382F, - -0.464117F, 1.16646F, -0.568425F, -0.831849F, -0.639019F, -0.995066F, - -0.966114F, -0.63995F, -0.555468F, -2.79302F, -0.566209F, -0.816974F, - -0.794897F, 1.21805F, -0.874977F, -1.5851F, 1.63483F, -0.699109F, -0.841483F, - -2.93968F, -0.779182F, -0.709406F, -0.659634F, 1.61964F, -0.931688F, - -0.778085F, -0.939735F, -1.06894F, -0.499666F, -1.53883F, -1.33082F, - -1.25049F, -1.03647F, -0.926131F, -0.598735F, -1.62981F, -0.509816F, - -1.13703F, -1.44435F, -0.881284F, -0.78103F, -0.665791F, 0.704337F, - -1.38864F, -0.86497F, -1.04697F, -0.941916F, -1.1561F, -1.8146F, -1.33374F, - -1.5624F, -0.91787F, 1.23895F, 1.2938F, -0.459559F, -0.89082F, -0.580565F, - -2.18391F, -0.44355F, -0.536283F, -1.40655F, -0.763779F, -1.76543F, - 0.834629F, -0.725023F, -0.792648F, 1.19228F, 1.20676F, 1.15308F, 4.82134F, - 1.22966F, 16.6208F, 2.29291F, 1.17435F, -1.08113F, -1.46811F, -1.43075F, - 1.17794F, -0.996337F, -1.073F, -1.30785F, -0.997608F, -1.8081F, -1.08079F, - -0.737306F, -0.449337F, -0.633542F, -0.978731F, -0.817489F, -0.753334F, - -0.616942F, -0.653404F, 1.20102F, 1.21045F, 1.01936F, 1.03042F, 1.00361F, - 2.20446F, 4.30325F, 5.79084F, 1.09592F, 1.25103F, 1.15775F, -1.06782F, - -0.200231F, -0.256391F, 1.05472F, -1.02134F, -0.567606F, -0.176861F, - -1.02442F, -1.14952F, -1.32677F, -1.32777F, -1.11047F, -0.554118F, -1.10466F, - -1.41543F, -0.639586F, 1.17113F, -1.47158F, 1.29267F, -1.22013F, -0.358943F, - -0.624368F, -0.810911F, -1.08208F, 0.286117F, -1.22588F, -1.12661F, - -0.657032F, -0.874518F, -1.38645F, -0.894738F, 1.13532F, 1.15326F, 1.12224F, - 1.03814F, 1.10735F, 0.694126F, 1.04969F, 1.13981F, -1.12463F, 5.14668F, - 0.880972F, 0.700466F, -1.01969F, -0.573588F, -1.03387F, -1.16303F, -1.14108F, - -0.812406F, -0.937084F, -1.24194F, -0.557035F, 0.434573F, -1.1117F, - -1.16156F, -1.38859F, -0.842778F, -0.825419F, -1.31698F, -0.471973F, - -0.96397F, -0.892887F, -1.26471F, -1.38662F, 1.72227F, 1.49114F, -1.01457F, - 0.656198F, 1.59657F, 1.30614F, -1.11214F, -0.516906F, -0.9208F, -0.756764F, - -0.53178F, -0.82354F, 0.764522F, 1.17165F, 1.15717F, 1.09193F, 0.996224F, - 0.994589F, 1.07607F, 0.973984F, 0.910442F, -1.08315F, -0.956592F, -1.10325F, - -1.12109F, 0.988782F, -1.06248F, -1.24674F, -0.176187F, -1.18571F, -1.00387F, - -0.988407F, -1.02487F, -1.9593F, -1.10352F, -1.18266F, -1.21508F, -1.13427F, - -0.985797F, -1.40326F, -1.30157F, -1.08732F, -1.13088F, 0.397381F, 1.54614F, - -1.10409F, -1.16163F, -1.09331F, -0.712878F, 0.758792F, -0.893562F, - -0.924458F, -1.13031F, -0.868084F, 1.14483F, 1.20051F, 1.09903F, 1.099F, - 1.21474F, 1.1662F, 1.07161F, 2.22575F, 0.89947F, 3.82646F, 1.00323F, 2.4734F, - 1.28682F, 2.95449F, 2.76974F, 0.479232F, 2.73055F, 0.229044F, -1.0949F, - -1.11725F, -1.16656F, -1.10947F, 1.10185F, 0.441334F, 0.439776F, -0.599096F, - -0.519099F, -0.899585F, -1.03817F, -1.05621F, -1.07186F, -1.16709F, - -0.825133F, -1.07841F, -1.5003F, -1.0252F, -1.48127F, -0.674731F, -0.528661F, - -1.28681F, -0.767299F, 1.11131F, -1.19011F, -0.656152F, -1.01638F, -1.02049F, - -0.591286F, 1.95755F, 1.0391F, 1.07361F, 1.14061F, 3.66655F, 3.13637F, - 0.32538F, 3.65733F, 0.228376F, 0.968483F, -1.17549F, 0.208239F, 0.0724114F, - -0.572236F, -0.876575F, -1.32585F, -0.975646F, -0.957534F, 0.55982F, - -1.18774F, -1.15269F, 1.10802F, -0.556928F, 0.733723F, -0.98178F, -1.12705F, - 0.537504F, -1.53193F, -1.08099F, -1.62031F, 0.951099F, 1.02807F, 0.854284F, - 0.810804F, 0.981305F, 1.00343F, 1.04969F, 4.79754F, 1.97905F, 1.09062F, - 4.72273F, 1.04022F, 2.31161F, 4.4511F, 0.417327F, 0.945538F, 1.08849F, - 2.18187F, 2.66692F, 1.78611F, -1.03524F, -0.499454F, -0.780906F, -0.981721F, - -0.704182F, 0.937297F, 1.24435F, 0.932787F, 5.18818F, -1.15419F, -0.968383F, - 1.2629F, 21.0535F, -0.986344F, -0.739942F, -1.08798F, -0.849911F, -0.725242F, - 0.740006F, -0.995111F, -0.513396F, -1.05589F, -0.95728F, 2.4341F, -0.680958F, - -0.792081F, -0.993481F, -0.695042F, -1.50363F, -0.485173F, -0.854703F, - -0.595208F, 1.31892F, 2.66706F, 9.20579F, 0.91872F, 3.03563F, 4.6898F, - 5.26808F, 2.93424F, 5.02533F, 0.977034F, -1.80589F, -1.82374F, -1.05189F, - -1.40862F, 0.464322F, -0.963515F, 0.952358F, 3.10726F, 14.3025F, -0.869127F, - -1.11802F, 2.37674F, -1.38682F, -0.519115F, -0.883865F, -1.07666F, - -0.708965F, 0.409384F, -0.644814F, -0.764736F, -0.978284F, -0.687174F, - -0.657919F, -0.905284F, 2.08322F, -1.82953F, -0.952953F, -0.924548F, - -0.768822F, 0.650616F, 1.84289F, -0.896976F, -1.26742F, -0.528601F, - 0.0796066F, 0.978101F, 3.8519F, 0.426979F, 2.18807F, -0.802677F, -1.10757F, - -1.80905F, -1.27566F, -0.600085F, -1.11993F, -1.09387F, -1.81021F, 1.63733F, - 0.353525F, 0.18356F, -0.889625F, -1.0783F, -1.01329F, -0.848452F, -0.622221F, - -0.570746F, -0.896798F, -0.853867F, -0.967703F, -1.32453F, -0.967929F, - 2.73509F, 0.930805F, 0.272486F, 1.43927F, 0.709505F, 2.6726F, 0.570422F, - -1.04288F, -1.45857F, -1.16213F, -0.968079F, -0.963657F, -1.33182F, - -1.12889F, -0.946434F, -0.0176518F, 1.54796F, 1.44828F, -1.27397F, -1.05223F, - -1.32762F, -0.861334F, -0.991276F, -0.929698F, -0.543333F, -0.870773F, - -0.816831F, -0.898961F, -0.961382F, -0.980328F, 0.131343F, 0.0442059F, - -1.29788F, -0.93892F, -0.998691F, -0.962428F, 0.135184F, -0.518012F, - -0.977457F, -0.933074F, 2.05982F, -0.652772F, 7.0568F, -1.08643F, 0.932659F, - -1.67778F, -0.974766F, -1.82422F, -1.03492F, -1.80378F, -0.963976F, - -0.983579F, -1.83841F, -1.00606F, -0.976453F, -1.80978F, -1.11187F, - -1.00315F, -0.988251F, -1.06328F, 0.67194F, -1.27257F, -0.741225F, - -0.860527F, -0.990023F, -0.894028F, -1.0369F, -0.739834F, -0.88988F, - -1.06926F, -0.618772F, 1.62396F, 1.46596F, -1.81832F, -1.68937F, -1.80337F, - -1.8211F, -0.746204F, -1.1691F, -1.15508F, 0.000584466F, -1.70434F, - -1.79309F, -1.79971F, -1.82198F, -1.02218F, -0.51181F, -0.966987F, - -0.968901F, -1.0066F, -1.09087F, -0.974166F, -0.870706F, -0.981793F, - -1.24809F, -0.986563F, -1.7561F, -1.28304F, -0.96437F, -0.989287F, -1.00943F, - -1.1552F, -1.78652F, -1.38839F, -4.17129F, -0.9337F, -1.5155F, -0.93914F, - -0.878857F, 0.2624F, 0.655743F, 1.76386F, 1.3521F, -0.544285F, -1.99526F, - 0.882342F, 0.315209F, -1.80853F, -1.36721F, -1.4023F, -1.80151F, -1.82027F, - -1.31202F, -0.845249F, -0.993671F, -1.14736F, -1.00735F, -0.960686F, - -1.0404F, 0.0453544F, -1.82676F, -0.808101F, -1.0795F, -1.80524F, -0.973994F, - -1.04501F, -1.52351F, 1.67647F, -1.11173F, -1.0338F, -0.11223F, -1.03957F, - -0.630954F, -0.871651F, -0.8702F, 2.21153F, 1.63413F, 0.033907F, -1.46721F, - -1.83208F, -1.03256F, -0.516797F, -0.953684F, -1.11799F, -1.44609F, - -1.80254F, -0.501987F, -0.513039F, -0.9805F, -0.977689F, -1.21031F, - -1.80683F, -1.78019F, -1.83269F, -1.80798F, -0.823048F, -0.510541F, - -0.493865F, -0.987159F, 0.0697616F, -0.822223F, -0.982346F, -0.97157F, - 0.0439464F, -0.000478915F, -1.86595F, -1.62011F, -1.09294F, -1.31097F, - -0.942877F, 0.523622F, -0.645645F, -0.944453F, -0.968412F, -0.925186F, - -1.41968F, -1.29193F, -1.35181F, -1.40332F, -1.35037F, -1.64338F, -1.68534F, - -1.67479F, -0.979171F, 0.0100724F, -0.989198F, -1.33776F, -1.27485F, - -1.2517F, -0.669729F, -1.19403F, -0.998815F, -1.75506F, -1.86364F, -1.32574F, - -1.35354F, -1.81325F, -1.17677F, 0.02272F, 0.298976F, -0.998132F, -1.95689F, - -1.01538F, -1.79038F, -0.710283F, -1.39133F, -1.86968F, 1.23078F, 1.15175F, - 1.88454F, -1.14235F, -0.414414F, -1.23355F, -1.32781F, -1.39741F, -1.63752F, - -1.65079F, -0.95921F, -1.0301F, -0.0854423F, -0.251408F, -1.10679F, - -1.28546F, -1.38652F, -1.73245F, -1.81535F, -1.8288F, -0.955216F, -1.04914F, - -0.793267F, -0.695353F, -1.07325F, -1.02384F, -1.36144F, -1.21164F, - -1.09708F, -1.08469F, -1.33516F, -0.502579F, -1.11404F, -0.862341F, - -2.03343F, -1.42771F, -1.45436F, -1.00033F, 0.29527F, -2.8792F, -1.16022F, - 1.59445F, -1.05267F, -0.460961F, 1.85814F, 1.74776F, 1.49937F, -3.86447F, - -1.14364F, 1.56431F, 0.524107F, -1.46116F, -1.75001F, -0.827323F, 0.362753F, - 0.945513F, -1.35291F, -1.36311F, -2.25006F, -1.71665F, -1.62522F, -1.87558F, - -0.699532F, -0.540925F, -0.958113F, -1.82157F, -1.14455F, -0.780757F, - -1.00277F, -1.42153F, -1.79476F, 1.80508F, -0.796603F, -1.87028F, -0.958727F, - -0.959358F, -0.579345F, -0.735955F, -0.953391F, 1.06335F, 1.99256F, 1.1719F, - 1.63765F, -0.984147F, -0.490491F, 1.37913F, 0.813951F, 1.19796F, 1.42579F, - -1.27709F, -1.34138F, -1.35045F, -1.62456F, -1.70814F, -1.84986F, -1.81282F, - -0.998621F, -1.35488F, -1.83759F, -1.84774F, -0.935399F, -0.800238F, - -1.03809F, -0.970216F, 0.437076F, 0.0449503F, -1.89274F, 1.41554F, 1.94419F, - 1.81317F, 0.0502737F, -0.294994F, 1.03439F, -1.06065F, 1.25377F, 1.37532F, - -1.77272F, 1.66493F, -1.67534F, 1.52666F, -1.68377F, -1.39972F, -1.70704F, - -1.36928F, -1.60331F, -1.58802F, -1.35835F, -1.32023F, -1.43221F, -1.68144F, - -1.43931F, -1.60406F, -5.7056F, -1.67332F, -1.97376F, -0.968488F, -1.81414F, - -1.81829F, -0.993643F, -1.34437F, -0.987047F, -1.35229F, -1.81274F, - -1.71352F, -1.04036F, -1.43351F, -0.95396F, -1.12327F, -1.02455F, -0.985139F, - -1.23926F, -0.62304F, -0.999183F, -1.15263F, -1.40155F, 2.06363F, -0.640901F, - 1.07326F, 0.312004F, -1.23542F, -0.798419F, -0.894443F, 1.90779F, - -0.0111234F, 0.118115F, 0.607811F, -0.401269F, -0.632253F, -1.57074F, - 1.05563F, -0.910505F, -0.850921F, 1.77087F, 1.43318F, 1.29418F, 0.772751F, - -3.18889F, -3.13901F, -1.81348F, 0.00275405F, -1.74824F, -0.878591F, - -0.979992F, -1.78253F, -1.08622F, 0.0366501F, -1.37548F, -0.9952F, - -0.981278F, -1.118F, -0.667195F, 0.636648F, 1.63944F, 1.62162F, 1.15843F, - 1.84622F, 2.08504F, -1.07573F, -1.81288F, -1.81727F, -1.83965F, -1.82836F, - -0.979505F, -0.98972F, 0.323216F, -1.07469F, 0.614065F, -1.81762F, - -0.953491F, 0.625383F, 0.0811721F, 1.13593F, 1.75228F, 0.765168F, -0.659811F, - 1.39358F, -1.3437F, -1.40365F, -4.94422F, -1.29838F, -1.68518F, -1.73609F, - -1.72377F, -1.82474F, -1.73959F, -0.824322F, 0.872057F, -1.04514F, -1.79477F, - -1.14129F, 0.374697F, -1.26816F, 0.316683F, -1.25123F, -1.00308F, -0.823697F, - -0.854324F, -1.12278F, -0.927897F, 1.13132F, 0.628796F, 1.7228F, -0.985963F, - 2.05647F, -1.25277F, 1.67816F, -1.16406F, -1.44531F, -0.548801F, -1.34094F, - 1.46828F, -2.26363F, -2.27859F, -1.64344F, 0.885924F, 1.02195F, -1.50705F, - -0.925699F, 0.172315F, 0.047068F, 0.068857F, -1.31859F, -0.945486F, - 0.904166F, 0.986409F, -0.959941F, -1.51552F, 1.75918F, 0.0764393F, 1.08467F, - -1.08585F, 0.740496F, 0.00564792F, -0.993472F, -1.63808F, -0.997292F, - 0.829716F, 0.806112F, -1.33502F, 1.11799F, -1.4353F, -1.56211F, -1.3231F, - -1.55428F, -1.34525F, -1.37964F, -1.62498F, -0.962635F, 0.0802776F, - -0.0827707F, 0.930889F, 0.86886F, -0.32676F, -1.37319F, -1.40186F, -1.3129F, - 0.0759761F, 0.199967F, -1.02676F, -0.824521F, -1.47347F, -1.15676F, -1.4157F, - -1.59989F, 0.0173608F, -1.63896F, 2.11805F, -0.815151F, -0.994495F, -1.1835F, - -1.04808F, -1.92575F, 1.16211F, 0.619747F, 1.63477F, 2.02633F, -0.754435F, - -0.786475F, 0.861021F, 0.0313518F, -0.863055F, -1.04781F, 2.02793F, 1.60276F, - 1.36006F, 2.0172F, 1.94883F, 0.960679F, 0.900588F, -1.48483F, -2.06731F, - -1.33488F, 0.903404F, -0.977875F, 1.3354F, -1.39182F, -1.48292F, -1.32937F, - -0.978133F, -1.68179F, 0.144368F, -0.97934F, 1.79189F, -0.976138F, - -0.891056F, -1.82806F, 0.534435F, 0.606787F, -0.966386F, -0.778613F, - -1.39007F, -0.88644F, 1.13468F, 1.31244F, 1.93769F, -0.811776F, -2.19681F, - 2.29268F, -0.594437F, 0.756096F, -1.91499F, 0.249186F, 0.0430874F, 0.518639F, - 1.34222F, 1.6283F, -1.00246F, 1.0844F, -2.42631F, -1.5874F, 1.751F, 1.73072F, - 1.34958F, -1.39142F, 1.49018F, -0.870415F, -0.738176F, -0.872498F, - -0.766701F, -1.40483F, -1.45316F, -2.94758F, 0.275883F, 0.224055F, 0.993972F, - 0.825152F, 0.9491F, 1.07526F, 1.06204F, -0.996146F, -1.00905F, -1.38452F, - -1.30603F, -2.85848F, -0.970644F, 0.223321F, 1.0066F, 0.892796F, 0.951447F, - 0.956651F, 0.103749F, 0.414739F, 1.11086F, 2.02468F, 1.42205F, 1.56608F, - 1.1358F, 1.30611F, -0.633631F, 1.21794F, 0.820201F, 1.64736F, -1.0827F, - 1.37674F, -2.6358F, -1.0009F, 0.62865F, -0.95186F, 2.19714F, -0.254494F, - 1.22095F, -0.716563F, 1.9572F, 0.641913F, 0.706794F, -0.793515F, -0.960473F, - -1.39017F, 1.88967F, -1.13014F, 0.719147F, -1.85968F, -0.907543F, 1.43583F, - 1.00212F, 1.12125F, -0.990584F, 1.28441F, -1.39332F, -1.47661F, -1.46794F, - -1.34374F, -1.01415F, -0.0130414F, 0.855146F, 0.964791F, 1.0126F, 1.66385F, - -1.44131F, -1.06967F, 0.331962F, 0.0564079F, 0.149561F, -0.953678F, - -0.183374F, 1.44914F, -0.885604F, 1.89919F, -0.251602F, -0.791081F, - -0.964596F, 1.13704F, 2.21628F, 1.14439F, 1.12363F, 1.71871F, 1.52197F, - 1.27979F, -0.965704F, 0.727659F, 0.71512F, -1.8654F, 1.8087F, -0.211564F, - -1.22058F, -3.37905F, 1.3916F, -1.99259F, -1.68399F, 0.291606F, 0.979875F, - 1.06201F, 0.98897F, -1.01206F, -0.993373F, -2.54654F, 0.106973F, 1.11805F, - 1.1561F, 0.8206F, 2.35882F, 1.51543F, 1.72935F, -0.966345F, -0.271765F, - 1.80932F, -1.75154F, 1.2844F, 0.829387F, 1.63075F, 1.29422F, -0.796504F, - 1.2486F, -1.43948F, 1.48473F, -1.79233F, 1.22917F, 0.705148F, 0.630904F, - 1.25665F, 0.86134F, 0.793051F, -0.932924F, -2.26889F, -1.80894F, -6.76881F, - -1.37275F, -0.973753F, -1.39873F, -0.828363F, 0.855963F, 1.16035F, -1.74434F, - 0.480517F, 0.384583F, -0.836016F, 0.380136F, -0.177252F, -1.4724F, 0.309777F, - -1.3026F, 0.831115F, 1.76455F, -1.42548F, 1.10994F, 1.12148F, 1.66932F, - 0.782248F, 0.754061F, -0.754574F, 1.19012F, 1.61188F, 1.71873F, 1.79525F, - -0.905959F, 1.3419F, 1.50862F, -0.785491F, 2.38694F, 0.922475F, 1.69503F, - 1.14143F, -1.43642F, -1.08657F, -1.45487F, -4.39828F, 0.997669F, 1.0016F, - -0.084257F, -3.65029F, 0.25104F, 1.04845F, 1.64468F, 0.750472F, 0.698084F, - 0.559355F, 0.507296F, 0.585046F, 0.754868F, -1.3443F, 0.873758F, 1.24969F, - 0.541314F, 0.533459F, -1.00391F, -1.0524F, 1.04197F, 1.04547F, -0.0926966F, - 0.957498F, -0.952996F, -0.972084F, -1.84962F, -1.03858F, 0.740657F, - -1.03763F, 1.13805F, -0.676185F, -0.903169F, -0.968796F, 0.825028F, - -0.836588F, 1.34628F, 0.743959F, 1.0054F, -1.76398F, 1.29772F, 0.754516F, - -1.69048F, -1.74713F, 0.899208F, -1.39842F, -2.10941F, -1.114F, 1.17662F, - -1.03524F, 1.15764F, 1.03221F, 0.990708F, 0.9612F, 0.792726F, 1.09198F, - 0.0175149F, 0.870265F, 0.174173F, -0.915261F, 0.798462F, -1.21735F, - -1.58247F, -1.09976F, 0.00216768F, 0.218793F, 0.593865F, -0.974237F, - 1.12192F, -1.79817F, -0.510656F, -0.905017F, 0.0842628F, 0.0157148F, - 0.620273F, -0.822407F, -0.680616F, -0.902375F, 1.71418F, -1.11703F, - -0.778919F, -1.36349F, -1.34232F, -1.05809F, -2.57334F, -0.848726F, - -0.645129F, -0.943535F, -1.0234F, -1.96724F, 1.07022F, 1.03213F, 0.909725F, - -1.21128F, 0.00197659F, -1.08627F, 1.19469F, -1.07832F, -5.86405F, -1.05075F, - -1.04919F, 0.044073F, -1.14842F, 0.544007F, -1.81471F, -0.762038F, - -0.739054F, -0.166032F, -1.42662F, -1.19261F, -2.13128F, -1.3946F, -1.19501F, - 1.25086F, 1.24021F, 6.70232F, 1.2654F, 1.18188F, 1.17707F, 1.13493F, - 1.13125F, 1.20005F, 0.974579F, 1.15641F, 0.790418F, 1.14946F, 0.575625F, - 0.523923F, -0.0470188F, -0.00952542F, -1.0785F, -1.09622F, -1.93225F, - -1.29345F, 0.403169F, -1.52222F, -1.86734F, -1.91203F, -1.848F, -1.6195F, - -1.52599F, -1.67022F, -0.990312F, -0.919644F, 1.19088F, 0.668847F, 1.07443F, - 1.04205F, -0.214429F, -1.23313F, 0.470741F, -1.41177F, -0.612872F, 0.942763F, - -0.936002F, -1.35452F, -1.27308F, -1.19516F, -1.11464F, -0.460239F, - -0.860057F, 0.641566F, 0.95277F, -1.01047F, -3.16831F, -1.76753F, -1.29388F, - -1.89343F, -1.24832F, -0.973845F, -1.3681F, -0.938382F, -2.30588F, 0.120562F, - -1.20717F, -0.952653F, -1.14022F, -0.815886F, -5.03849F, -1.71828F, - -3.86001F, -1.23064F, -0.98113F, -1.11855F, -1.09309F, -0.840847F, -1.88577F, - -0.993352F, -1.0898F, -0.943946F, -1.70907F, -0.946936F, -0.908533F, - -1.53935F, -0.694373F, -1.05306F, -1.64371F, -1.3196F, -1.38633F, -0.553219F, - -2.28561F, -0.822102F, -0.400423F, -0.766454F, -0.53291F, -1.45026F, - -0.5475F, -1.48666F, -0.79911F, -1.43122F, -1.4299F, -1.79723F, -1.59321F, - -0.455748F, -1.49828F, -0.939499F, -1.44539F, -2.5653F, -1.54108F, -1.07045F, - -0.977356F, -1.8062F, -1.65591F, -0.818506F, -3.41087F, -3.76895F, - -0.679731F, -0.70754F, -0.878663F, -1.72437F, -1.6446F, -0.704131F, - -2.90594F, -1.50094F, -0.983584F, -0.984728F, -1.13939F, -1.36398F, - -1.74625F, -1.50481F, -1.92676F, -1.83306F, -1.81709F, -0.99932F, -1.47334F, - -0.850575F, -0.860595F, -0.936177F, -1.00672F, -1.78433F, -1.49868F, - -1.85555F, -1.12836F, -1.04398F, -1.16804F, -1.1166F, -1.00238F, -0.827596F, - -1.39251F, -0.844883F, -1.63789F, -4.70455F, -1.35011F, -1.669F, -1.66017F, - -1.70286F, -1.68858F, -1.49823F, -1.59398F, -1.61665F, -1.38287F, -1.52986F, - -1.68423F, -1.57529F, -1.31887F, -1.45471F, -1.34309F, -1.28022F, -1.29749F, - -0.912544F, -1.06587F, -1.36681F, -1.06089F, -1.57642F, -0.940385F, - -1.57553F, -2.01584F, -1.66552F, -1.59475F, -0.571818F, -1.38182F, -1.35931F, - -1.36466F, -0.947014F, -1.73692F, -0.946554F, -1.02965F, -1.0639F, -2.55252F, - -1.03429F, -1.9103F, -0.979776F, -1.67679F, -1.53852F, -0.889732F, -1.2217F, - -1.19133F, -1.16836F, -1.48555F, -0.837527F, -1.67308F, -0.932416F, - -0.948383F, -0.847601F, -1.14332F, -1.36836F, -1.14583F, -1.32365F, - -1.87314F, -1.27051F, -0.959179F, -0.977416F, -0.999057F, -1.53502F, - -2.51226F, -1.63891F, -1.60402F, -1.71712F, -0.896245F, -1.34421F, -1.45032F, - -0.959455F, -1.59555F, -0.978F, -1.01247F, -1.59697F, -0.963908F, -1.64347F, - -1.63539F, -0.846587F, -0.913898F, -1.57884F, -1.70885F, -1.34995F, - -0.887407F, -0.939482F, -1.05186F, -1.711F, -1.90848F, -0.948586F, -1.65517F, - -1.66213F, -1.42539F, -1.26715F, -1.75923F, -0.702273F, -1.87556F, -1.42445F, - -1.27048F, -1.76301F, -1.86248F, -0.960285F, -1.3475F, -1.35501F, -1.61791F, - -1.56583F, -1.61248F, -1.62514F, -1.504F, -1.62901F, -1.33893F, -1.39857F, - -1.39251F, -1.41672F, -1.0044F, -1.37771F, -1.44389F, -1.3802F, -1.36594F, - -1.71656F, -1.08642F, -1.36109F, -1.42918F, -1.62108F, -1.72881F, -1.56046F, - -1.31991F, -1.4834F, -1.38373F, -0.924183F, -2.68118F, -1.66983F, -1.63945F, - -1.66633F, -0.968466F, -1.68781F, -1.62983F, -0.95801F, -1.00296F, -1.82443F, - -1.07642F, -0.882584F, -1.60066F, -1.06555F, -0.921019F, -1.25656F, - -0.873328F, -1.87956F, -1.70963F, -1.80986F, -1.83211F, -0.968329F, - -1.59598F, -4.22777F, -1.68235F, -0.971085F, -1.70481F, -1.11654F, -1.73564F, - -1.09168F, -0.913255F, -0.959124F, -0.992157F, -0.99309F, -1.00944F, - -1.97732F, -0.996961F, -1.72408F, -0.818275F, -1.83927F, -0.782385F, - -0.943745F, -1.31979F, -1.71226F, -1.13591F, -1.5888F, -1.61265F, -43.5962F, - -1.61849F, -1.37088F, -1.74149F, -1.31429F, -0.98968F, -0.91086F, -1.12746F, - -0.946739F, -1.29805F, -0.985771F, -1.34112F, -0.945002F, -0.997613F, - -0.970574F, -0.968305F, -1.64035F, -0.993601F, -0.967432F, -0.933149F, - -1.31863F, -1.80753F, -0.935967F, -3.60437F, -1.05035F, -1.78759F, -1.57273F, - -0.959715F, -1.3709F, -1.76084F, -1.84383F, -1.87466F, -0.958013F, -1.9718F, - -1.4859F, -1.16374F, -1.79187F, -1.23289F, -1.77831F, -1.61105F, -1.82978F, - -0.907456F, -0.974249F, -0.971734F, -1.62256F, -1.63046F, -1.62459F, - -1.62675F, -1.63265F, -1.70794F, -0.954024F, -3.92507F, -1.00636F, - -0.959179F, -2.4749F, -1.72926F, -1.39927F, -1.3359F, -1.79824F, -0.956259F, - -0.985009F, -1.04576F, -1.59839F, -0.967717F, -1.61622F, -1.55955F, - -1.67591F, -1.6709F, -1.7396F, -1.2837F, -0.951503F, -1.29827F, -1.32158F, - -0.623689F, -1.18707F, -0.156089F, -1.77756F, -0.95896F, -1.23246F, - -1.45075F, -1.64735F, -0.98175F, -0.970114F, -1.10574F, -0.954808F, -1.0405F, - -0.970526F, -1.84044F, -1.66577F, -2.96673F, -1.33159F, -1.33927F, -1.74499F, - -1.54566F, -1.85319F, -0.896106F, -1.38833F, -1.69997F, -1.32824F, -1.87583F, - -1.6293F, -1.82666F, -1.85717F, -1.82313F, -0.872327F, -1.9206F, -1.82815F, - -1.40981F, -1.63954F, -1.44449F, -1.9258F, -1.69613F, -1.74599F, -1.11992F, - -1.81225F, -1.64934F, -1.73488F, -1.36994F, -0.985299F, -1.85859F, - -0.954757F, -1.84925F, -1.70701F, -1.61993F, -1.14632F, -1.81644F, - -0.978461F, -0.984091F, -1.80145F, -1.82237F, -1.25332F, -1.54956F, - -1.16372F, -1.87769F, -1.84982F, -0.817424F, -1.82681F, -1.19116F, - -0.579161F, -1.19085F, -1.35424F, -1.85457F, -1.2411F, -1.81903F, -1.75914F, - -0.984765F, -1.04797F, -1.68028F, -0.862152F, -1.67837F, -0.940652F, - -1.79589F, -1.82406F, -1.66506F, -1.64056F, -1.19751F, -1.72044F, -1.50306F, - -1.79745F, -1.6031F, -1.34773F, -1.16276F, -1.70761F, -1.06834F, -1.57238F, - -0.953411F, -1.58653F, -1.7431F, -1.06563F, -1.5968F, -1.90782F, -1.19302F, - -1.87007F, -1.06646F, -1.13901F, -0.976646F, -1.59573F, -1.81483F, -1.70165F, - -1.79164F, -1.71967F, -1.06411F, -1.06513F, -1.72306F, -1.71032F, -1.63156F, - -0.981933F, -1.83066F, -1.71529F, -1.42641F, -1.90122F, -1.84706F, -1.7074F, - -1.76361F, -1.31908F, -2.67306F, -1.64471F, -1.73972F, -0.970794F, -1.33725F, - -1.35395F, -1.0237F, -1.81665F, -0.958644F, -1.18977F, -1.32496F, -1.76833F, - -1.80901F, -1.80324F, -1.16555F, -1.01854F, -0.979173F, -0.976018F, - -1.23187F, -1.09685F, -1.03887F, -1.71689F, -0.991698F, -1.8553F, -0.985207F, - -1.66986F, -4.22455F, -2.11428F, -3.306F, -1.80869F, -5.42281F, -2.20244F, - -1.80836F, -1.38549F, -1.39125F, -1.14492F, -0.98922F, -1.78128F, -0.977849F, - -1.16613F, -1.02562F, -1.04838F, -1.06232F, -1.73188F, -1.39473F, -1.55792F, - -1.89366F, -1.80006F, -1.17412F, -1.08574F, -1.11831F, -0.944645F, -1.68583F, - -1.11227F, -1.29176F, -1.8333F, -1.18587F, -0.968941F, -0.93508F, -0.947479F, - -1.16853F, -0.948924F, -1.1046F, -1.05643F, -1.77488F, -1.22553F, -1.78563F, - -0.97967F, -0.950262F, -1.78645F, -1.06773F, -0.974542F, -0.980539F, - -1.95017F, -1.67471F, -1.60134F, -1.59932F, -1.68668F, -1.80864F, -1.85104F, - -1.81026F, -1.60171F, -1.69561F, -0.960153F, -0.949845F, -1.16236F, - -1.19062F, -1.79599F, -1.80342F, -1.78109F, -1.73418F, -1.67082F, -1.61468F, - -1.59007F, -1.80321F, -1.8002F, -1.7917F, -1.74786F, -1.67267F, -1.8082F, - -1.80801F, -1.83097F, -1.61899F, -1.69118F, -1.6973F, -1.59204F, -1.81316F, - -1.81126F, -1.62543F, -1.80523F, -1.69945F, -1.62307F, -1.56857F, -1.72369F, - -1.76318F, -1.63251F, -1.91815F, -14.2646F, -15.1123F, -1.71018F, -1.78635F, - -1.80575F, -1.70219F, -1.64657F, -1.6216F, -1.60857F, -1.87755F, -1.61108F, - -2.14571F, -1.80904F, -1.80726F, -1.81421F, -1.82137F, -1.78814F, -1.80856F, - -1.83519F, -1.81009F, -1.69125F, -1.78908F, -1.81045F, -1.80737F, -1.51668F, - -1.81028F, -1.80545F, -1.81718F, -1.79609F, -1.48958F, -1.80288F, -1.79682F, - -1.82609F, -1.84392F, -1.81297F, -1.81044F, -1.71393F, -2.24159F, -7.6304F, - -5.03642F, -4.98535F, -5.34031F, -6.05506F, -1.80782F, -1.8317F, -1.71516F, - -1.80906F, -1.8128F, -1.81111F, -1.75265F, -1.71195F, -1.7184F, -1.80283F, - -1.81116F, -1.80928F, -1.69209F, -1.76507F, -1.92683F, -1.81106F, -1.87427F, - -1.81563F, -1.78528F, -1.80546F, -1.79788F, -1.80072F, -1.80504F, -1.80524F, - -1.82554F, -1.80999F, -1.80726F, -1.80699F, -1.81006F, -1.81678F, -1.80013F, - -1.80451F, -1.81442F, -1.80019F, -1.80919F, -1.79637F, -1.80236F, -1.83884F, - -2.33376F, -1.84891F, -1.81646F, -1.79157F, -1.80297F, -1.70105F, -1.68385F, - -1.81392F, -1.55407F, -1.80685F, -1.78094F, -1.79736F, -1.83113F, -1.80262F, - -1.81852F, -1.73778F, -3.95064F, -1.88877F, -1.58553F, -3.61406F, -4.28968F, - -1.63908F, -1.80997F, -1.89824F, -1.80216F, -1.80499F, -1.82798F, -1.47677F, - -1.78581F, -1.76708F, -1.50837F, -1.56308F, -1.8087F, -2.41901F, -1.82013F, - -1.80653F, -1.75121F, -1.80961F, -1.81127F, -1.79622F, -1.804F, -1.80948F, - -1.81179F, -1.80528F, -1.81188F, -1.80554F, -1.84332F, -1.79444F, -1.81568F, - -1.65615F, -1.8178F, -1.79063F, -1.82312F, -1.80639F, -1.73392F, -1.79897F, - -1.90371F, -1.79781F, -1.46377F, -1.82206F, -1.47827F, -3.70811F, -3.57228F, - -1.63593F, -1.60102F, -1.64812F, -1.82138F, -3.94798F, -3.73183F, -1.66118F, - -1.64251F, -1.67897F, -1.5874F, -1.81057F, -1.81566F, -1.69529F, -1.65441F, - -1.81769F, -1.81987F, -1.81242F, -1.81082F, -1.70579F, -1.81618F, -1.43476F, - -1.80634F, -1.86417F, -1.68801F, -3.06149F, -1.68278F, -1.86819F, -1.33971F, - -1.26607F, -1.221F, -2.93695F, -1.80526F, -1.80789F, -1.55806F, -1.24904F, - -1.20099F, -1.40052F, -1.66389F, -1.68034F, -1.58671F, -1.8169F, -1.80881F, - -1.29488F, -1.16412F, -1.80859F, -1.22401F, -1.39301F, -1.80851F, -1.40417F, - -1.46772F, -1.48004F, -1.27018F, -1.19523F, -1.30136F, -1.51092F, -1.54439F, - -1.25587F, -1.92067F, -1.23165F, -1.8204F, -1.72005F, -1.8147F, -1.67977F, - -1.78434F, -1.79586F, -1.2755F, -1.29559F, -1.26954F, -1.68961F, -1.80845F, - -1.81388F, -1.32572F, -1.54058F, -1.28637F, -1.2867F, -1.29837F, -1.24045F, - -1.80484F, -1.48487F, -1.48115F, -1.3283F, -1.80824F, -1.79871F, -1.55505F, - -1.43513F, -1.489F, -1.53712F, -1.45777F, -1.24427F, -1.46002F, -1.70916F, - -1.81225F, -1.80789F, -1.82195F, -1.80823F, -1.52613F, -1.45512F, -1.1427F, - -1.78943F, -1.64173F, -2.46488F, -1.79545F, -1.81413F, -1.53772F, -1.46803F, - -1.52259F, -1.22362F, -1.13314F, -1.80663F, -1.38969F, -1.81199F, -1.20225F, - -1.82874F, -1.46662F, -1.5153F, -1.80639F, -1.45945F, -1.43807F, -1.48187F, - -1.72337F, -1.71822F, -1.6738F, -1.70473F, -1.49855F, -1.53959F, -1.59302F, - -1.92865F, -1.58564F, -1.6308F, -1.64111F, -1.60549F, -1.77489F, -1.69678F, - -1.70594F, -1.7436F, -1.78412F, -1.31831F, -1.25935F, -1.53031F, -1.66372F, - -1.93606F, -1.75288F, -1.81489F, -1.41878F, -1.80909F, -1.55412F, -1.75069F, - -1.56516F, -1.53869F, -1.29254F, -1.52461F, -1.19812F, -1.1449F, -1.2087F, - -2.50577F, -1.11051F, -1.56818F, -1.80782F, -1.56491F, -1.22076F, -1.53575F, - -1.27007F, -1.44273F, -1.46365F, -1.25389F, -1.11387F, -1.4572F, -1.72494F, - -1.82903F, -1.39057F, -1.489F, -1.46126F, -1.74605F, -1.83651F, -5.63905F, - -1.36516F, -1.47128F, -1.56071F, -1.81472F, -1.51527F, -1.45003F, -1.24213F, - -0.957462F, -1.54083F, -1.7548F, -1.7997F, -1.54788F, -1.91915F, -1.33182F, - -1.27178F, -1.1884F, -0.96209F, -0.957561F, -0.946252F, -1.9641F, -1.43306F, - -0.958666F, -1.90412F, -1.77123F, -1.52398F, -1.43271F, -1.44915F, -1.47085F, - -1.26008F, -1.86999F, -1.80919F, -1.44691F, -1.59189F, -1.51389F, -1.75399F, - -1.82164F, -1.62417F, -1.39217F, -1.47199F, -1.57626F, -1.48367F, -1.47611F, - -1.79718F, -1.62713F, -1.5886F, -1.67036F, -1.66889F, -1.81189F, -1.28311F, - -1.41815F, -1.50654F, -1.59378F, -1.69511F, -1.65463F, -1.83843F, -1.82578F, - -1.71758F, -1.8377F, -1.82071F, -1.74273F, -1.4301F, -1.82597F, -0.957427F, - -0.951322F, -0.860193F, -1.81473F, -1.33162F, -1.81191F, -1.1189F, - -0.942744F, -1.90999F, -1.39426F, -0.916575F, -1.45516F, -1.65085F, - -1.81528F, -1.50126F, -1.27693F, -1.64424F, -1.84829F, -1.71806F, -1.31623F, - -1.70338F, -1.57482F, -1.5623F, -1.66241F, -1.58472F, -1.63492F, -1.5826F, - -1.70313F, -1.71011F, -1.70935F, -1.79407F, -1.60544F, -0.932546F, -1.18637F, - -0.885114F, -0.923672F, -1.32404F, -1.57575F, -1.67051F, -2.24586F, - -1.74408F, -1.46085F, -1.46285F, -1.57188F, -1.20076F, -1.18615F, -0.878696F, - -0.985129F, -0.966877F, -0.969934F, -0.954731F, -1.36965F, -1.53888F, - -1.41622F, -1.72838F, -1.67693F, -1.81271F, -1.42263F, -1.34025F, -1.2902F, - -1.53613F, -1.18022F, -0.917689F, -1.22137F, -1.1183F, -1.12386F, -0.908971F, - -0.984935F, -0.961996F, -0.983082F, -0.966101F, -4.52379F, -1.62675F, - -1.7616F, -1.41715F, -1.47749F, -1.80964F, -1.28322F, -1.44416F, -1.80484F, - -1.44997F, -1.37841F, -1.22316F, -0.872775F, -0.966399F, -0.937008F, - -0.973868F, -1.97976F, -1.83974F, -1.80716F, -1.48563F, -1.16542F, -1.65717F, - -1.23666F, -0.946733F, -1.91915F, -1.79284F, -1.83702F, -1.13099F, -1.51661F, - -1.81637F, -1.49353F, -1.39156F, -1.38664F, -1.48131F, -1.69267F, -1.44267F, - -1.96345F, -1.5937F, -1.58656F, -1.6325F, -1.65314F, -1.86254F, -1.80702F, - -1.36339F, -1.26218F, -1.46975F, -0.924515F, -0.962939F, -0.957025F, - -0.951039F, -1.41934F, -1.37184F, -1.42009F, -1.59442F, -1.62313F, -1.7007F, - -1.67408F, -1.79882F, -0.92567F, -0.927336F, -0.878474F, -0.720776F, - -0.692809F, -1.37063F, -1.40895F, -1.7019F, -1.36217F, -1.62601F, -1.66561F, - -1.56815F, -1.53102F, -1.53183F, -1.2292F, -1.12471F, -0.860681F, -1.72854F, - -1.80503F, -1.8017F, -1.46354F, -1.51005F, -1.7914F, -1.51061F, -0.942129F, - -0.961252F, -0.962496F, -1.71826F, -0.973137F, -0.96643F, -1.65338F, - -1.81271F, -0.850484F, -1.45489F, -1.38689F, -1.70235F, -1.66891F, -1.76389F, - -1.80401F, -1.29789F, -1.65558F, -1.48549F, -1.67404F, -1.68047F, -1.46213F, - -0.954783F, -1.40168F, -1.41674F, -1.83888F, -1.3376F, -1.72677F, -1.82871F, - -0.916406F, -0.955992F, -1.82033F, -0.989492F, -1.82751F, -1.7063F, - -0.897857F, -1.23286F, -1.52235F, -1.68202F, -1.54958F, -1.6504F, -1.59322F, - -1.80841F, -1.6915F, -1.67809F, -1.69898F, -1.64386F, -1.61512F, -1.60098F, - -1.83049F, -1.78289F, -1.58307F, -1.62552F, -1.67074F, -1.58578F, -1.80866F, - -0.924475F, -1.77395F, -1.63441F, -1.80771F, -1.71389F, -1.51646F, -1.39141F, - -0.775176F, -1.7802F, -1.60017F, -0.649112F, -1.23261F, -0.867982F, - -0.551454F, -0.923675F, -1.3125F, -1.69015F, -1.45349F, -1.47353F, - -0.692957F, -1.62607F, -1.79389F, -1.81813F, -0.932999F, -0.847282F, - -0.81172F, -1.77781F, -1.79797F, -0.984181F, -0.989302F, -0.997316F, - -0.885281F, -1.35152F, -1.15103F, -4.08504F, -1.39769F, -1.44199F, -1.45796F, - -1.60704F, -1.55217F, -1.6266F, -1.68477F, -1.58719F, -1.53393F, -0.936133F, - -1.35452F, -2.74984F, -1.70024F, -1.74177F, -1.60345F, -1.68573F, -1.58288F, - -1.67936F, -1.67802F, -1.57632F, -1.26129F, -1.21504F, -2.35408F, -0.865093F, - -0.994116F, -0.927883F, -1.3339F, -1.36513F, -1.34415F, -1.29444F, -2.88836F, - -1.31977F, -1.41629F, -3.54174F, -1.48707F, -1.42238F, -1.53482F, -1.58624F, - -1.54335F, -1.22861F, -1.73533F, -1.75838F, -0.660465F, -0.804851F, - -0.661752F, -1.56778F, -0.873048F, -0.831761F, -1.73966F, -1.63993F, - -1.03816F, -1.72626F, -1.6566F, -1.29367F, -2.52006F, -1.8841F, -1.70645F, - -1.29371F, -1.19909F, -20.7497F, -1.42015F, -4.62894F, -1.32358F, -1.04353F, - -1.47657F, -1.70437F, -1.80054F, -1.62282F, -1.7033F, -1.70297F, -1.30075F, - -11.9652F, -1.65544F, -1.37122F, -1.69473F, -1.26612F, -2.51666F, -1.01323F, - -1.67774F, -1.6613F, -1.65873F, -1.71339F, -1.08553F, -1.25639F, -1.06719F, - -1.40595F, -1.23419F, -0.836793F, -1.73698F, -1.00888F, -1.24131F, -1.02529F, - -1.09731F, -1.7879F, -1.3431F, -1.78757F, -1.89698F, -1.89908F, -1.19361F, - -0.804143F, -1.29218F, -1.39617F, -0.670813F, -1.29239F, -1.35658F, - -6.56995F, -1.37063F, -2.80822F, -1.54417F, -1.25733F, -1.62764F, -0.923068F, - -1.56514F, -18.6522F, -1.86503F, -0.745446F, -1.32014F, -1.72624F, -1.73438F, - -1.71472F, -1.78637F, -1.01356F, -0.984099F, -3.71487F, -0.901427F, - -0.958424F, -2.40895F, -1.6601F, -1.61521F, -1.76086F, -1.1461F, -2.4349F, - -1.41001F, -5.10963F, -1.44984F, -0.980389F, -0.990847F, -0.770771F, - -1.18646F, -1.66221F, -1.23096F, -0.916794F, -1.2638F, -1.13739F, -0.986364F, - -0.983025F, -1.29654F, -1.17191F, -0.919524F, -1.84785F, -1.80188F, - -1.51338F, -0.976414F, -1.77689F, -1.07009F, -1.74344F, -1.77056F, - -0.931589F, -0.912844F, -0.835705F, -1.55165F, -0.949956F, -1.50352F, - -1.54033F, -1.53373F, -1.4664F, -1.49832F, -1.31544F, -1.51171F, -1.56751F, - -1.62955F, -0.851705F, -1.62272F, -0.963567F, -0.941842F, -4.2803F, - -1.52606F, -1.52666F, -3.1045F, -0.935752F, -0.958374F, -0.928746F, - -1.31899F, -0.896013F, -1.18087F, -1.29961F, -0.956657F, -1.28906F, - -1.80632F, -1.69282F, -1.33039F, -1.26118F, -1.75166F, -1.03427F, -1.77422F, - -1.34375F, -1.02957F, -1.64716F, -3.92331F, -0.977507F, -1.7092F, -1.43813F, - -0.979081F, -0.957183F, -1.27655F, -1.7005F, -0.66439F, -0.928598F, - -1.57237F, -1.53304F, -0.940515F, -2.93749F, -1.03147F, -1.69884F, - -0.954112F, -0.659673F, -1.00084F, -1.05553F, -0.802759F, -1.76337F, - -1.80045F, -1.80511F, -1.82445F, -6.31746F, -5.00315F, -0.916562F, - -0.903195F, -0.956604F, -0.867779F, -1.55908F, -1.41388F, -1.59612F, - -1.58797F, -1.18509F, -0.953554F, -1.47181F, -0.999588F, -0.971701F, - -0.97672F, -1.5185F, -1.00171F, -1.4962F, -1.71018F, -1.73575F, -1.6247F, - -1.72977F, -1.56369F, -1.65482F, -1.62932F, -2.89495F, -0.903183F, - -0.980669F, -0.931081F, -0.981512F, -0.92866F, -0.958637F, -0.934677F, - -0.96094F, -0.952567F, -0.949387F, -3.87632F, -0.945327F, -1.33237F, - -0.981629F, -1.52497F, -1.42305F, -0.908086F, -0.622467F, -1.52448F, - -1.67975F, -0.958168F, -1.7325F, -1.78427F, -1.79464F, -1.80974F, -1.7429F, - -1.77542F, -1.90778F, -1.40152F, -0.920826F, -1.30026F, -0.907031F, - -0.955089F, -1.39024F, -1.46576F, -1.2335F, -1.54233F, -1.51955F, -1.56837F, - -1.61275F, -1.25899F, -1.66741F, -1.27065F, -1.06661F, -1.80072F, -0.979895F, - -0.970528F, -1.33304F, -0.88831F, -0.882265F, -0.888014F, -1.1506F, - -1.66137F, -1.35404F, -1.51537F, -1.76472F, -1.57516F, -0.954042F, -1.56012F, - -1.67704F, -1.35112F, -0.700502F, -0.951151F, -0.953647F, -1.80761F, - -1.10041F, -1.81316F, -2.37439F, -1.56458F, -0.950013F, -1.02133F, -1.34734F, - -2.03231F, -1.26416F, -0.881272F, -0.908897F, -1.34921F, -1.62311F, - -9.76239F, -1.32882F, -0.949715F, -2.93483F, -1.60683F, -1.26439F, -1.36982F, - -1.36653F, -0.910091F, -0.782545F, -0.964106F, -1.27751F, -1.23306F, - -0.849724F, -0.977395F, -0.966943F, -1.54063F, -1.20084F, -0.9682F, - -0.869944F, -0.971502F, -0.947747F, -0.976517F, -0.962297F, -1.82493F, - -0.888709F, -0.582679F, -0.861214F, -1.87717F, -1.2025F, -1.59991F, -1.499F, - -1.94063F, -1.21489F, -0.887328F, -1.43687F, -1.69632F, -1.12979F, -1.67962F, - -1.60539F, -0.951911F, -1.74421F, -0.952403F, -1.36114F, -0.743586F, - -1.02807F, -5.8213F, -1.78448F, -1.12793F, -1.87497F, -0.722674F, -0.562673F, - -1.31922F, -1.56947F, -1.50279F, 0.0318774F, -0.934959F, -1.04451F, - -1.33784F, -1.37488F, -1.52077F, -1.59593F, -1.37758F, -1.43127F, -1.32666F, - -1.61649F, -0.668247F, -1.47359F, -1.33674F, -1.30641F, -1.58629F, -1.59725F, - -1.53821F, -1.29017F, -1.43263F, -1.35389F, -1.39125F, -1.36598F, -0.887634F, - -1.40068F, -1.91579F, 0.0283269F, -1.52631F, -0.917781F, -1.94938F, - -0.922116F, -1.04436F, -0.569493F, -0.651809F, -0.846216F, 0.0445938F, - -1.1231F, -0.950349F, -0.955831F, -1.23828F, 0.357668F, -1.45472F, - -0.705584F, -0.87928F, -1.8041F, -0.761664F, -1.53056F, -0.0223744F, - 0.241374F, -1.14281F, -0.935225F, -1.54663F, -1.8632F, 0.479374F, -0.378373F, - -2.1324F, -1.30903F, -0.725384F, -1.64875F, -0.564255F, -0.961443F, - -0.973183F, -0.82713F, -0.667446F, -1.00046F, 0.223467F, -1.20175F, - -1.26269F, -1.46375F, 0.502679F, 0.0363415F, -1.42825F, 0.910615F, - 0.0390929F, -0.249069F, -1.00691F, 0.511204F, 0.949101F, -0.565899F, - 0.55397F, -0.264791F, -0.715203F, -0.777583F, -0.82969F, -1.10221F, 1.14133F, - 0.137111F, 0.0782395F, 1.05919F, 0.260935F, -1.16839F, 0.667878F, -1.31142F, - 0.105828F, -1.93852F, 0.459814F, 0.573279F, 1.16389F, -1.20534F, 0.674098F, - 0.746112F, -0.638885F, -1.52269F, 0.420082F, 0.594215F, 0.512311F, - -0.539674F, 0.805849F, -1.43852F, -1.3956F, -1.15449F, -1.01013F, 0.0497062F, - 1.13612F, -1.76551F, -1.33972F, -1.79317F, -0.501291F, 0.418116F, 0.332915F, - 0.732208F, 1.00512F, 0.232209F, -1.25225F, 1.25721F, -1.25197F, 0.637538F, - 0.45168F, -1.81723F, -1.06404F, -0.764107F, 1.11473F, -0.879757F, -1.39845F, - -1.19255F, -1.18419F, 0.414981F, 0.679505F, 1.15058F, -1.17411F, 0.67495F, - -1.23327F, -0.7162F, 0.242861F, -0.144285F, 0.533861F, 0.723232F, 0.153369F, - -1.7572F, -1.27535F, -1.06701F, -1.3071F, 0.876589F, 3.83476F, 0.503846F, - 0.961916F, -0.0220729F, -1.16107F, -0.937646F, -1.39557F, 0.622536F, - -0.626596F, -0.891035F, -0.876573F, 0.22028F, 0.71453F, 0.519039F, 0.85337F, - 2.1966F, -0.951184F, -0.278743F, 0.687124F, 0.216396F, 0.0508988F, -0.88131F, - -0.896009F, -0.871998F, -0.898436F, -0.985077F, -0.911606F, -1.22859F, - -1.48778F, 0.505722F, -1.27534F, -1.2785F, 2.14593F, -1.40641F, 1.13887F, - 1.13316F, -1.30407F, 1.29097F, 1.26948F, 1.59972F, 1.32723F, -1.17951F, - 1.31363F, 0.0370012F, -1.18627F, -0.723136F, -0.888116F, -1.02533F, - -1.04922F, -0.889569F, -1.253F, -0.985898F, 0.668635F, 0.374725F, 0.9379F, - 0.547053F, -1.56038F, 0.653041F, -0.696631F, 0.433379F, 0.729615F, 2.07189F, - 1.15799F, 1.58008F, -0.162865F, 1.28073F, -0.95523F, 1.63772F, -0.871121F, - -1.46027F, 0.49901F, 0.676949F, 2.52963F, -0.98929F, -0.977085F, -0.343912F, - -0.761934F, -1.0507F, -1.81344F, -1.32302F, 0.522582F, 0.885544F, 0.600398F, - -1.00994F, 0.528F, 0.834871F, 0.700294F, 1.11499F, -1.33188F, -0.906049F, - 0.379436F, 0.362738F, 1.80793F, -1.05011F, -1.7007F, 0.948964F, 1.79907F, - 0.943605F, 0.948649F, 0.201552F, 0.954658F, 0.542081F, -1.2339F, 0.101155F, - 0.0833504F, -0.704679F, 1.04214F, -1.63302F, -1.6036F, 1.15906F, 1.04468F, - 0.888182F, -1.26072F, 0.0653644F, -1.71221F, 0.0736862F, 0.112061F, - -1.02181F, -1.03155F, -1.72576F, -0.87544F, -1.71896F, 0.569386F, 0.0295061F, - -0.35691F, -1.14246F, -0.81316F, 0.485824F, 1.4643F, 0.894973F, 0.0588885F, - -1.13583F, -1.04632F, -0.869084F, -1.93709F, -1.16991F, 0.589331F, 0.184215F, - 0.749474F, 0.779222F, -1.65233F, -1.60872F, 1.14837F, -1.37277F, 1.49238F, - 1.90079F, 1.0813F, 0.953541F, 1.17126F, -1.63724F, 0.669637F, -1.03733F, - -1.26047F, -1.68723F, -1.67432F, 0.749035F, 0.507774F, 0.793329F, 0.970327F, - -1.13395F, -1.53391F, 0.707002F, -0.406501F, 0.0563757F, -0.0791569F, - -1.61459F, 0.851615F, -1.42427F, -0.409668F, 1.05778F, 1.41019F, -0.569461F, - 0.845378F, -1.27584F, -1.30004F, -1.31832F, -1.36365F, -1.6464F, -1.62932F, - -1.3482F, 0.388262F, 0.325369F, 0.403256F, 0.566091F, 0.507502F, 0.730259F, - 0.749242F, -0.0744772F, -1.23906F, 0.533685F, 0.498857F, 0.453644F, 1.60534F, - 0.494299F, 0.519411F, -0.506519F, 0.596569F, 0.25089F, -0.128083F, 0.271763F, - 0.301591F, -0.898358F, -0.881292F, -0.698115F, 0.0947186F, 0.13291F, - 0.419956F, 0.793547F, -1.33834F, -1.60714F, -0.936564F, -1.22606F, -1.48225F, - 0.518348F, -1.16279F, 0.972131F, 0.585018F, 1.57891F, 0.580759F, -1.0988F, - 0.538227F, 0.811744F, 1.14557F, 0.272851F, 0.315091F, 0.056604F, 0.11533F, - 0.984629F, -1.17435F, 0.94462F, -0.381007F, -0.350653F, -0.860583F, 2.21585F, - -0.50956F, -0.0829888F, -0.732482F, -0.579263F, 0.680259F, -0.00716774F, - 0.0107542F, 0.165633F, 0.477594F, -1.20145F, -1.34822F, -1.32076F, -3.1438F, - -3.42006F, -1.36321F, -1.42349F, -1.39889F, 1.10888F, -1.35566F, 0.511621F, - 0.512779F, -1.34719F, 1.07176F, 1.06686F, -0.0397067F, -1.75278F, 0.694441F, - 1.17833F, 0.613999F, 0.355275F, -1.48335F, -1.32182F, -1.93205F, 0.957244F, - -0.801752F, 0.392573F, 0.720594F, 1.10685F, 0.52491F, 0.159526F, 0.374747F, - 1.19987F, -1.24784F, -1.26756F, -1.55168F, -1.40051F, -1.35435F, -1.37362F, - -1.35969F, -1.24076F, -1.44968F, 0.38606F, 0.680818F, -0.13881F, 0.680679F, - 0.656239F, 0.667566F, -0.0876208F, 0.438996F, 0.441992F, 0.576168F, - -0.0346151F, -1.64809F, -1.77114F, -1.38697F, 1.16438F, -0.135021F, - 0.548185F, 0.857781F, -0.849651F, -1.50984F, -1.68565F, -1.56203F, -1.73982F, - -1.64644F, -1.54487F, 3.72799F, 3.78925F, 1.0404F, 1.16542F, 0.504961F, - -1.09324F, 1.16156F, 0.687105F, -1.72009F, -1.67807F, 1.09669F, 0.646624F, - -1.67526F, 1.10642F, 0.649495F, -1.3283F, -1.37303F, -1.54528F, 0.96241F, - -1.57748F, 0.412529F, 0.635894F, -1.44137F, -1.67582F, 0.49238F, 0.657363F, - -1.66545F, 0.511859F, 0.109108F, 0.155856F, 0.484087F, -0.844279F, 0.31767F, - 0.375171F, -0.945312F, -1.42821F, 5.06724F, 1.17831F, 0.587511F, 0.505538F, - -1.27553F, 0.635643F, 0.648025F, -1.40828F, -1.71881F, 0.623153F, 0.617563F, - -0.0765279F, -1.75546F, 0.286607F, 0.625858F, 0.325847F, 1.2434F, 0.637133F, - -1.46593F, -1.26409F, -1.40022F, -1.2059F, 0.156573F, 2.84168F, -1.28909F, - 0.826475F, 0.839998F, 0.587538F, 0.672903F, 0.507639F, -0.786536F, - -0.0742824F, -0.898756F, -0.975984F, 0.0291447F, -1.1097F, -1.29071F, - -1.46497F, -4.74939F, 0.340192F, 0.331902F, 0.656072F, -1.55788F, -0.89241F, - -0.897028F, 0.132952F, -1.06238F, -1.11206F, 0.393839F, 2.72265F, -1.22028F, - 1.15863F, 0.867834F, -0.723901F, -0.999187F, -0.90325F, 0.137915F, - -0.779505F, 0.712064F, -0.717238F, -0.711007F, -1.74921F, -1.83849F, - -1.25649F, 0.483312F, 0.488556F, 0.741646F, -0.735695F, 0.24155F, -0.937225F, - -0.0200734F, 0.982621F, -0.972245F, -1.03397F, -1.40995F, 0.715909F, - 0.765233F, -1.59752F, 0.213837F, -0.960609F, -1.66138F, 0.506176F, - -0.828415F, 0.23073F, -1.31341F, -0.966899F, -0.926958F, 0.0675943F, - -1.23602F, 0.971526F, 0.621339F, -0.82739F, -0.872974F, -1.20224F, -1.50208F, - 0.933698F, -0.749962F, -0.884433F, -1.0019F, -0.997963F, -0.872856F, - -0.959888F, 0.169042F, 0.0611106F, 0.662356F, -1.02877F, -8.26289F, 5.58291F, - 0.610838F, 0.654935F, 0.721275F, -0.965094F, -0.787941F, 3.47095F, 1.37788F, - 0.507234F, -1.00053F, -0.976334F, -0.856691F, -1.02944F, -1.0539F, 0.960752F, - -1.17574F, 0.66924F, -0.963114F, -0.977757F, -1.00575F, -1.05702F, 0.567053F, - -0.962509F, -0.207811F, 0.402497F, 1.11189F, 0.763405F, -0.856326F, - -1.00946F, -0.0401002F, -1.55759F, 0.553529F, -0.249274F, -2.45304F, - 1.08795F, 0.443024F, 0.13505F, 2.85579F, 5.47281F, 2.27833F, 2.33709F, - 0.0262771F, 1.33648F, 5.06844F, -1.68805F, 1.12066F, 1.10314F, -1.58094F, - -1.57216F, 1.10552F, 2.30833F, 2.93107F, 0.940436F, -1.81091F, -0.662733F, - -0.0788516F, -0.924061F, 0.0277708F, -1.04066F, 1.3649F, -2.23286F, - 0.596436F, -1.72942F, -1.71505F, -0.972317F, -0.767014F, -0.69571F, - -0.66439F, -1.26446F, -1.58857F, -1.72729F, -0.973814F, -1.77282F, 0.848703F, - 0.0970971F, -1.40038F, 0.657301F, -0.720402F, 0.635913F, 0.390031F, 2.30319F, - 4.02506F, 3.31355F, 3.05315F, 0.918684F, 0.720342F, 0.695602F, 0.32412F, - 0.625409F, -0.163452F, -1.02538F, 3.31984F, 0.931701F, 3.5418F, 3.77156F, - 0.595096F, 0.537739F, 0.0428448F, 2.40654F, 1.56395F, 1.28347F, 0.684822F, - 0.576857F, 0.967372F, -0.728395F, -0.835889F, 0.034102F, 0.650489F, - 0.208366F, 0.373924F, 0.546087F, 0.000234109F, 1.77116F, -0.885473F, 1.0498F, - -0.858361F, 0.626605F, 0.757123F, 0.988633F, 7.53107F, 1.70708F, 7.05972F, - 0.700185F, 0.578989F, -0.933784F, -0.888834F, -0.939496F, 1.05653F, 1.14039F, - 12.5441F, 0.880087F, -0.965052F, -0.110009F, 0.713642F, 1.10578F, 0.451113F, - 0.728858F, -0.833061F, 1.07408F, -0.748137F, 1.08599F, 0.145142F, 0.857283F, - 1.01332F, 1.13361F, 1.04261F, 1.16897F, 1.18451F, 1.14533F, -0.720529F, - -0.914984F, -1.6226F, 0.268812F, 0.205619F, 0.54214F, -0.780808F, -0.704107F, - -0.912989F, -0.167589F, 0.0934939F, -1.56573F, 1.01367F, 1.10398F, 0.507095F, - -0.863472F, 1.18618F, -0.952972F, -0.0152651F, 0.436049F, 0.727684F, - 0.708013F, -1.56834F, 0.947431F, 1.03489F, 1.03025F, 1.06791F, 1.03646F, - 1.11142F, 0.473907F, 0.682659F, -0.879291F, 0.566843F, 0.899411F, -1.21952F, - -0.886243F, -0.840671F, -0.846326F, 1.12666F, 0.83408F, 1.1466F, 0.461758F, - 0.539881F, 0.87886F, -0.784412F, -0.746162F, -0.794593F, -1.21281F, - -1.21724F, 0.0494307F, -1.23613F, -1.30188F, -1.36069F, 0.957464F, 0.814479F, - 0.909917F, 0.944942F, 0.161515F, 0.226736F, 1.00101F, 0.2396F, 1.00707F, - 0.378995F, 1.21686F, 4.34634F, 0.83432F, 1.20919F, 1.11825F, 1.16254F, - 0.834761F, 1.18057F, 1.17387F, 0.960347F, 1.11937F, 1.14215F, 1.16979F, - -0.114294F, 0.792484F, 0.660492F, 0.955073F, 0.149381F, 0.159749F, - -0.886151F, -1.2526F, -1.23816F, 0.858403F, 0.0612149F, 0.187659F, 1.01253F, - 1.04178F, 1.11463F, 0.710356F, 0.696323F, -1.26061F, 1.00131F, 1.15554F, - 1.28249F, 0.606382F, 0.669233F, 0.681714F, 0.902171F, 0.219137F, -0.92975F, - -0.928289F, -0.834726F, 0.677025F, 0.876218F, 0.901796F, 0.229093F, - 0.0426175F, 1.17439F, 0.520937F, 0.684427F, -0.767991F, -0.542411F, - -0.802484F, -0.970791F, 0.230857F, -0.7681F, 0.0473768F, -1.00568F, - 0.480277F, 0.569666F, 0.6791F, 0.778919F, -0.532262F, -1.02018F, 0.426005F, - 0.0381411F, 0.709526F, -0.248856F, 0.20802F, 0.0017054F, 0.454334F, - 0.543508F, -0.210502F, -1.69827F, 0.471983F, 1.22741F, -0.139049F, - -0.110177F, 0.229466F, -0.866511F, 1.29874F, -1.0178F, -0.269386F, - -0.857416F, -0.954128F, -0.744603F, -0.520563F, 0.185575F, -0.87617F, - -0.916867F, -0.857435F, -0.984386F, -0.667561F, -0.858306F, -0.0697125F, - 0.0326684F, -0.863209F, 0.0904124F, -0.0681433F, -0.756237F, 0.470805F, - -0.980718F, -1.70218F, -1.69423F, 0.817812F, 0.844439F, 0.894557F, 0.882736F, - 0.964451F, -0.0980216F, 0.719466F, 0.667627F, 0.563764F, -0.357958F, - -0.509385F, 0.0496223F, 0.871816F, 0.88741F, 0.968055F, 0.298574F, 0.640669F, - 0.652604F, 0.12806F, 0.0461107F, 0.0435644F, 0.657008F, 0.776364F, - -0.510625F, -0.159458F, 0.141386F, -0.191469F, -0.0716897F, 0.598508F, - 0.559919F, 0.9654F, 0.538316F, -0.212939F, 0.637199F, 1.11366F, 1.34037F, - 1.10783F, 3.05455F, 2.50973F, 1.18577F, 1.22859F, 0.431846F, 0.135302F, - 1.10764F, 1.16261F, 0.0539164F, 0.848796F, 0.651658F, 0.737362F, 0.608712F, - 0.424525F, -0.166066F, 0.698228F, 0.299449F, 0.316625F, 0.918384F, 1.07329F, - 0.222054F, 0.549134F, 0.0345374F, 0.518884F, 0.943993F, 0.64449F, 0.215238F, - 0.0559112F, 0.572575F, 1.36F, 1.30757F, 1.11269F, 1.29372F, 1.20602F, - 1.22473F, 1.01464F, -0.110936F, 0.651364F, 0.10404F, 0.113826F, 1.30264F, - 1.20786F, 1.17983F, 1.12822F, 1.70719F, -0.135993F, 0.0449932F, 0.538471F, - 0.934636F, 0.499576F, 0.190348F, 0.0168044F, 1.15787F, 0.671772F, 0.72292F, - 0.801527F, -0.196808F, 0.763983F, 0.66936F, -0.21843F, 0.683299F, 0.990232F, - -0.0224802F, 0.506886F, 0.0727704F, -0.251323F, 0.869483F, 0.509351F, - 1.15009F, 1.15248F, -1.57887F, -1.8649F, 1.15996F, 0.501657F, 0.771529F, - 1.23159F, 1.25149F, 1.37916F, 0.273358F, 0.575888F, 0.713202F, 0.716252F, - -0.123479F, 0.21063F, 0.541594F, 0.492007F, -0.237245F, 0.472167F, 0.523124F, - -0.0115577F, 0.58859F, 0.625288F, 0.58252F, -0.0740499F, 0.0421396F, - -0.770601F, 0.0571027F, 0.80104F, 1.17984F, -1.73238F, -1.63191F, 1.15266F, - 0.0320623F, 0.335271F, 0.393575F, 0.186001F, -0.0999496F, 1.33776F, - 0.724933F, 0.836459F, 0.671141F, 0.516155F, 0.649447F, 0.748497F, 0.770913F, - -0.138199F, 0.515619F, -0.142528F, 0.0943708F, -1.59004F, 1.17797F, - 0.0447399F, 0.94361F, 0.779787F, 0.629103F, 0.633562F, 1.10125F, 0.98613F, - 0.608709F, 0.00343914F, 0.102022F, 0.580127F, 0.405547F, 0.394762F, - 0.0256733F, 0.478746F, -0.0872052F, 0.614423F, -0.145145F, 0.533966F, - 0.604023F, 0.655733F, 0.546741F, -0.160579F, 0.630589F, -0.190545F, - 0.0822174F, 0.510317F, 0.510302F, 0.614269F, 0.9918F, -0.15658F, -0.166024F, - 0.471393F, 0.0217252F, 0.203156F, 0.654609F, 0.566932F, 0.175239F, -0.77572F, - 0.591075F, 0.675049F, -0.354496F, 0.959216F, -1.65828F, -1.66399F, 0.636756F, - -1.65195F, -1.68999F, -1.74824F, 0.554108F, 0.569702F, 0.192567F, 0.327078F, - 0.307849F, 0.542293F, 0.043571F, 0.548085F, 1.51977F, 0.529829F, 0.177389F, - -0.15729F, -0.142691F, 0.193444F, 0.120658F, 0.211685F, 0.229576F, - -0.164863F, -0.160337F, -0.0395355F, 0.094919F, -0.622075F, -0.620272F, - -1.53842F, -1.58622F, 1.13975F, -1.02511F, 0.457781F, 0.652599F, 0.563283F, - 0.924265F, 0.723467F, 1.56956F, 0.546751F, 0.131822F, 0.227634F, 0.143727F, - -0.162109F, 0.231429F, -0.0770057F, -0.619744F, -1.87749F, 0.562624F, - 0.665323F, 0.580521F, 0.0969084F, 0.977192F, 0.559946F, -0.140213F, - 0.466025F, -0.172539F, 0.492604F, 0.149778F, -0.335087F, -0.130583F, - -0.21578F, -0.1936F, -1.80247F, -1.61055F, 0.534546F, 0.55258F, -0.533752F, - 0.669771F, -0.476688F, 0.980274F, -1.6478F, 0.546519F, -0.219356F, 0.595489F, - -0.196257F, 0.623878F, -0.188273F, -0.0259986F, 0.690812F, 0.556099F, - 0.137339F, -0.076011F, -0.871321F, -0.178738F, -0.81496F, 0.288943F, - 0.909633F, 0.544415F, -0.357342F, 0.555308F, 0.695582F, 0.376425F, -1.71015F, - -1.75942F, -1.68247F, -1.82737F, -1.77545F, -1.77644F, -1.78991F, 0.562353F, - 0.289092F, 0.323357F, 0.728515F, 0.542799F, 0.751767F, 1.1381F, 0.61749F, - 0.468415F, 0.775032F, 0.673135F, 0.698726F, 0.703247F, 0.980859F, 0.156361F, - 0.095688F, 0.0873133F, 0.044263F, -1.70419F, -1.78795F, -1.77315F, -1.92656F, - -1.86575F, -1.60206F, -1.85124F, 0.0741349F, 0.524857F, 0.561036F, 0.587183F, - 0.831465F, 0.294508F, 0.816759F, 0.614833F, 0.515814F, 0.30114F, 0.840118F, - 0.202871F, 0.15369F, -0.168922F, -1.42234F, 0.201627F, 0.327825F, 0.657032F, - 0.56127F, 0.201536F, 0.185398F, 0.271316F, 0.105929F, 0.721635F, 0.577625F, - 0.617158F, 0.33499F, 0.0233861F, 0.733867F, 0.0206143F, 0.475387F, 0.850921F, - 0.554437F, 1.04755F, 0.492077F, 0.0554908F, 0.406493F, 0.676257F, -0.472342F, - 0.593511F, 0.595656F, -0.185444F, 0.953947F, 1.0558F, 0.887275F, 1.00946F, - 0.451352F, 0.662523F, 0.561005F, 0.216969F, 1.49767F, 0.586642F, 0.10285F, - -0.223485F, 0.0105772F, -1.54482F, -1.6065F, -1.78269F, -1.8323F, -1.77044F, - -1.78277F, 0.328738F, 0.740784F, -1.79964F, -1.80973F, -1.70728F, 0.786177F, - 0.677507F, -0.210311F, -1.30193F, -1.82351F, -2.02154F, -1.83564F, 0.672604F, - 0.462022F, 0.2283F, 0.472553F, 0.67423F, 0.263589F, 0.812453F, 0.547114F, - 1.36716F, 1.85775F, 0.370454F, 0.733411F, -1.62532F, 0.0904673F, 0.111077F, - -0.196469F, 0.459692F, 0.729771F, 0.687156F, 0.494509F, 0.0262665F, - 0.633052F, 0.042199F, 0.723878F, 0.563676F, 0.0360318F, 0.199776F, - -0.161432F, 0.300051F, 0.364172F, 0.793248F, 0.307451F, 0.327575F, 1.00756F, - 0.926486F, -1.043F, 0.692589F, -1.20953F, 0.427325F, 0.493916F, 0.496265F, - -0.098623F, 0.674405F, 1.02745F, -0.0740666F, 0.512328F, 0.0223029F, - 0.772214F, -0.17484F, 0.367583F, 0.454298F, 0.180154F, -1.17193F, -1.46312F, - -1.00109F, 0.304968F, 0.303264F, 0.757483F, 0.865287F, 0.115375F, 0.30786F, - 0.384962F, -0.190804F, 0.318451F, 0.0468718F, 0.176097F, -0.0401571F, - 0.630616F, 0.289119F, 0.843389F, 1.00089F, 0.479287F, 0.537636F, 0.301261F, - 0.755274F, 0.669969F, 1.24348F, 0.784271F, 0.787934F, 0.0343274F, 0.397406F, - 0.772716F, 0.751294F, 1.56443F, 0.679413F, 1.3755F, 0.512413F, 0.146146F, - 0.190955F, 0.236953F, 0.796203F, 0.544875F, 0.644927F, 0.88303F, 1.05643F, - 1.19248F, 0.323071F, 0.704887F, 0.583003F, 0.616093F, 0.999049F, 1.70131F, - 0.965214F, 1.00781F, 2.07942F, 0.503115F, 0.486934F, 0.694161F, 0.68083F, - 0.967842F, 0.512155F, 0.678438F, 0.793983F, -0.0209033F, 0.680117F, - 0.691835F, 1.24184F, 1.18183F, 1.53474F, 1.89959F, 1.73979F, 0.571626F, - 1.46436F, 1.68417F, 0.786716F, 0.620207F, 0.679793F, 0.476564F, 0.493544F, - 0.752131F, 1.702F, 1.21153F, 1.01215F, 0.994808F, 1.49841F, 1.53345F, - 1.03584F, 0.833696F, 1.04285F, 2.1729F, 2.18708F, 1.44524F, 1.97512F, - 2.13763F, 1.99445F, 2.18875F, 2.3185F, 2.28305F, 2.14642F, 2.30069F, - 1.06033F, 2.27152F, 2.27792F, 1.04146F, 1.04402F, 2.33486F, 2.14517F, - 1.04284F, 2.34037F, 2.28498F, 1.03944F, 2.28297F, 1.07652F, 2.29927F, - 1.0422F, 1.04003F, 1.04428F, 1.04258F, 1.04021F, 1.04106F, 2.22222F, - 1.04045F, 2.19117F, 2.23151F, 1.04105F, 2.27556F, 2.74496F, 3.57566F, - 1.04068F, 1.04477F, 1.0431F, 2.148F, 1.04345F, 1.07726F, 1.07178F, 1.0405F, - 1.04072F, 1.04205F, 1.04016F, 1.04087F, 1.04112F, 1.04687F, 1.04066F, - 1.03545F, 1.91652F, 2.06496F, 1.89014F, 1.83862F, 1.95717F, 1.04027F, - 1.03907F, 1.04308F, 2.13682F, 1.04F, 1.04016F, 1.03917F, 1.04049F, 1.0406F, - 1.0423F, 1.05041F, 1.03781F, 2.33442F, 2.30994F, 2.31543F, 2.30927F, - 1.04494F, 1.03959F, 1.0395F, 1.04035F, 1.04005F, 1.04537F, 1.03954F, - 1.03703F, 1.0405F, 0.973426F, 2.49911F, 1.04241F, 1.0323F, 1.043F, 2.6717F, - 1.03756F, 1.04229F, 2.50976F, 2.2939F, 1.04192F, 2.19057F, 2.68606F, - 2.30506F, 1.04394F, 1.02623F, 1.03705F, 2.33532F, 2.25192F, 2.30049F, - 2.32964F, 1.04012F, 1.042F, 2.10146F, 1.04927F, 1.0382F, 2.03061F, 1.69452F, - 1.52442F, 2.51062F, 1.03849F, 1.05371F, 1.03961F, 1.04157F, 2.10264F, - 2.18568F, 1.03889F, 1.03825F, 1.07664F, 1.04157F, 1.92954F, 2.09754F, - 1.04069F, 1.03427F, 1.05902F, 1.02196F, 1.99819F, 1.03796F, 1.03967F, - 1.04351F, 1.03888F, 1.02971F, 1.04485F, 1.04107F, 1.89428F, 1.04183F, - 1.04182F, 1.02721F, 1.03973F, 1.04263F, 2.40658F, 2.19582F, 1.04605F, - 2.14572F, 2.06726F, 1.03809F, 1.90078F, 1.04033F, 1.04013F, 1.04151F, - 1.04998F, 2.67501F, 2.49387F, 1.86089F, 0.848391F, 0.801434F, 1.39769F, - 1.78124F, 2.31707F, 1.36497F, 2.06726F, 1.96773F, 1.2826F, 1.81412F, - 1.81386F, 0.845031F, 1.84027F, 0.837767F, 1.80314F, -0.252317F, -0.207375F, - -0.23968F, -0.20245F, 1.78361F, -0.202521F, -0.256842F, -0.150725F, - -0.160596F, -0.237454F, -0.204143F, -0.161999F, -0.214825F, 2.47253F, - 2.62463F, 1.15043F, 2.55502F, 2.49815F, 0.619399F, 0.7944F, 0.807009F, - 0.769465F, -0.18014F, -0.167723F, 1.42501F, 2.48875F, 2.78125F, 2.22973F, - 2.04486F, 2.50171F, 2.76215F, 1.37711F, 2.22677F, 1.88928F, 2.5179F, 1.8973F, - 1.28017F, 2.06848F, 1.82452F, 1.30917F, 0.586163F, 1.16775F, 1.2604F, - 1.95861F, 1.99728F, 2.08817F, 0.886085F, 1.29239F, 2.73402F, 2.47967F, - 2.4498F, 1.38746F, 2.45353F, 2.40905F, 2.21043F, 2.90638F, -0.0267789F, - 2.32719F, 2.20325F, 2.26255F, 1.76601F, 1.80305F, 1.93395F, 2.43413F, - 1.87073F, 2.55914F, 2.01226F, -0.0134826F, 0.717333F, 1.95371F, 2.47104F, - 1.46721F, 2.22956F, 1.09091F, 0.897493F, 1.83189F, 1.58129F, 1.15923F, - 0.925845F, 1.25911F, 0.857883F, 4.42449F, 1.37595F, 2.15677F, 2.26204F, - 2.81133F, 0.980214F, 1.99778F, 1.79027F, 3.47152F, -0.0298761F, 1.81934F, - 1.18657F, 0.87023F, 0.0433727F, -0.00527282F, 1.73617F, 1.83499F, 0.857724F, - 1.3398F, 2.12328F, 4.23449F, 4.40535F, 3.61939F, 3.51367F, 2.7779F, 3.19788F, - 3.63992F, 2.86539F, 2.89738F, 4.15986F, 2.42536F, 4.07779F, 1.52812F, - 1.2575F, 3.69069F, 3.88781F, 3.80702F, 3.01727F, 4.31633F, 4.27127F, - 3.00108F, 2.12023F, 2.31336F, 11.8313F, 3.20017F, 3.85355F, 3.65462F, - 2.84371F, 2.77301F, 3.16037F, 2.87562F, 3.01611F, 3.81885F, 3.6041F, - 2.70718F, 1.3696F, 1.35617F, 1.59504F, 8.11332F, 2.76789F, 2.96909F, - 2.70236F, 1.15616F, 2.45317F, 0.869326F, 2.40315F, 3.69511F, 10.9122F, - 2.38698F, 2.57251F, 0.898432F, 2.4912F, 2.65139F, 2.73975F, 2.71172F, - 2.44782F, 4.01416F, 15.8338F, 3.15013F, 3.04209F, 2.83308F, 1.40884F, - 4.85424F, 2.9553F, 2.25713F, 4.07964F, 3.80276F, 5.94113F, 10.8779F, - 3.03201F, 3.34819F, 3.35525F, 5.06035F, 2.16491F, 3.16277F, 3.43672F, - 5.79016F, 2.67812F, 4.00942F, 3.1282F, 3.42254F, 9.02399F, 3.69785F, 2.278F, - 2.85536F, 2.84491F, 2.52103F, 3.41922F, 2.34673F, 5.02756F, 3.31492F, - 5.42443F, 3.21107F, 4.43344F, 0.929757F, 2.25558F, 2.72492F, 2.79583F, - 2.17763F, 3.22676F, 3.2932F, 3.5803F, 5.63533F, 2.71449F, 3.83523F, 2.72967F, - 2.56893F, 4.00935F, 11.7925F, 3.28661F, 2.57266F, 2.56982F, 2.40238F, - 6.68366F, 3.09681F, 3.23759F, 4.06089F, 9.15133F, 6.73813F, 7.5139F, - 7.66088F, 7.33042F, 2.53378F, 2.98556F, 3.03471F, 4.94944F, 2.48432F, - 0.936017F, 5.19981F, 1.98927F, 12.1098F, 3.53547F, 2.34334F, 2.77582F, - 7.36802F, 3.93035F, 2.29582F, 2.73906F, 3.10322F, 3.73795F, 2.26034F, - 2.70019F, 3.64539F, 3.59662F, 2.79301F, 2.12912F, 2.67203F, 0.873646F, - 3.45066F, 1.94283F, 7.40544F, 9.09013F, 2.06828F, 2.30541F, 2.67545F, - 1.68607F, 0.935029F, 2.8047F, 7.4093F, 7.03181F, 2.66684F, 1.97321F, - 2.59682F, 1.78381F, 5.42592F, 2.14935F, 3.63925F, 4.27358F, 2.61879F, - 2.85441F, 1.72905F, 6.03423F, 3.0829F, 3.73641F, 2.06141F, 2.64637F, - 1.97236F, 5.04237F, 5.48932F, 3.18663F, 3.04073F, 12.1933F, 2.35687F, - 2.96304F, 2.30178F, 4.95241F, 2.9209F, 3.95991F, 2.33311F, 3.13864F, 3.8142F, - 6.66328F, 3.43018F, 2.63816F, 3.15795F, 3.1564F, 4.37746F, 2.73518F, - 3.69204F, 2.7871F, 3.40745F, 2.86804F, 0.935809F, 3.45071F, 2.76782F, - 2.15624F, 2.28086F, 2.34636F, 2.74963F, 2.71829F, 5.45123F, 6.06195F, - 2.31212F, 2.42432F, 2.41919F, 1.97138F, 2.31959F, 2.07227F, 2.37367F, - 5.45359F, 2.95237F, 2.3453F, 3.29836F, 3.98792F, 7.52312F, 2.98847F, - 5.10508F, 3.19107F, 2.69869F, 3.45801F, 4.06969F, 2.55901F, 3.49782F, - 2.5142F, 5.22068F, 3.71773F, 5.0749F, 2.12093F, 4.01767F, 3.5139F, 2.36944F, - 3.49072F, 5.06058F, 3.80895F, 2.73933F, 2.36054F, 6.03476F, 2.30949F, - 2.35596F, 2.77839F, 4.7859F, 2.74872F, 2.3884F, 4.22379F, 5.34261F, 4.46463F, - 3.62245F, 4.98635F, 3.47009F, 2.29701F, 3.6647F, 4.06459F, 5.52043F, - 8.60954F, 7.56595F, 3.36443F, 2.22052F, 3.96632F, 2.5742F, 2.7284F, 5.36061F, - 3.78548F, 3.07172F, 2.269F, 3.74927F, 2.79761F, 2.03901F, 3.20176F, 3.44216F, - 5.11175F, 3.77648F, 2.77918F, 3.26946F, 2.15417F, 2.16566F, 4.83844F, - 2.12154F, 5.0972F, 5.10375F, 4.89719F, 2.37046F, 2.09923F, 5.41276F, - 2.27641F, 3.5437F, 2.78462F, 2.58781F, 2.97832F, 2.64171F, 3.60981F, - 3.29133F, 3.13128F, 5.06019F, 5.31811F, 2.74402F, 2.53716F, 2.66617F, - 2.70284F, 3.78383F, 2.3463F, 2.7296F, 3.6187F, 5.27457F, 4.89594F, 3.37037F, - 4.74451F, 4.86115F, 3.33387F, 3.15571F, 4.84492F, 4.9837F, 5.04962F, - 2.93084F, 3.71361F, 3.37181F, 1.93442F, 2.64003F, 3.54543F, 3.79447F, - 2.9285F, 6.4527F, 5.02449F, 2.14397F, 2.80626F, 2.12083F, 2.73903F, 3.36795F, - 2.76566F, 7.25471F, 2.81862F, 3.26231F, 3.29446F, 3.51193F, 2.54194F, - 3.5783F, 2.19791F, 2.5672F, 4.54423F, 3.51886F, 5.64919F, 4.36158F, 4.98827F, - 3.26871F, 4.07333F, 2.0356F, 4.0982F, 4.85874F, 4.18435F, 5.69155F, 3.12144F, - 3.24642F, 1.66734F, 1.66875F, 5.34851F, 7.41382F, 1.86475F, 5.05891F, - 5.17969F, 5.00898F, 1.82395F, 5.09234F, 5.06315F, 4.98149F, 4.19574F, - 2.92378F, 2.91284F, 2.79252F, 2.19146F, 1.64873F, 3.91134F, 5.35433F, - 2.25655F, 1.77685F, 5.3096F, 2.27665F, 4.34298F, 4.18203F, 3.53794F, - 3.29469F, 2.42751F, 3.83241F, 5.38108F, 4.74728F, 1.65824F, 3.77695F, - 5.33209F, 3.86582F, 4.84829F, 4.36668F, 3.95942F, 5.13775F, 4.80933F, - 2.79525F, 5.35594F, 3.12581F, 4.02166F, 3.67059F, 5.0124F, 3.15975F, - 3.36222F, 3.18801F, 1.79176F, 2.73031F, 1.86152F, 1.68023F, 1.79464F, - 4.95519F, 2.94404F, 2.15315F, 2.17641F, 2.22075F, 5.57263F, 1.82712F, - 5.02604F, 2.97911F, 1.82216F, 2.79091F, 1.6571F, 2.13614F, 3.10961F, - 3.11192F, 3.73613F, 1.73231F, 2.16032F, 2.24291F, 4.27709F, 2.42695F, - 1.64523F, 1.02362F, 1.24833F, 2.30878F, 1.08811F, 1.10452F, 1.06224F, - 1.04676F, 1.7585F, 2.17057F, 1.07984F, 2.17823F, 1.27733F, 1.18369F, - 1.26929F, 0.610746F, 1.65182F, 1.27695F, 0.374143F, 0.35416F, 0.584333F, - 0.598935F, 0.466719F, 0.583593F, 0.34487F, 0.625086F, 0.639051F, 0.937232F, - 1.10766F, 1.02895F, 0.875602F, 0.616895F, 0.601398F, 0.907174F, 1.17328F, - 1.15138F, 1.08535F, 1.05948F, 0.616143F, 0.591781F, 0.584778F, 1.26677F, - 1.10149F, 1.00918F, 1.24671F, 1.78261F, 1.09021F, 1.28596F, 1.25134F, - 1.30547F, 0.909839F, 0.970222F, 1.16599F, 1.15109F, 1.13627F, 0.671434F, - 1.06638F, 0.978142F, 0.977233F, 0.754556F, 0.895138F, 1.17474F, 1.17933F, - 1.26509F, 1.13603F, 1.22608F, 1.3121F, 1.33501F, 1.2208F, 0.953708F, - 1.00999F, 1.32625F, 1.16222F, 1.14383F, 1.64798F, 1.07511F, 0.688164F, - 0.951846F, 1.23887F, 0.923076F, 0.602499F, 0.492199F, 0.618945F, 0.468822F, - 0.363368F, 1.67253F, 1.66771F, 0.086983F, 0.718678F, 0.627794F, 1.10064F, - 1.12763F, 1.01511F, 0.943251F, 0.889554F, 0.535327F, 0.419839F, 0.627954F, - 0.105132F, 0.171334F, -0.115642F, 0.496729F, 0.751256F, 0.27743F, 0.581761F, - 0.611297F, 0.610186F, 0.160681F, 0.0776296F, 0.11987F, 0.00994117F, - 0.422441F, 0.135358F, -0.00956022F, -0.848978F, 0.100599F, 0.117724F, - -1.4574F, -1.67333F, -12.2331F, -0.942802F, 0.596654F, 0.134039F, -1.53167F, - -1.49391F, -0.637225F, 0.12691F, -0.851174F, 0.102517F, -0.340886F, - -1.02894F, -0.935047F, -2.48336F, 0.169681F, -1.2846F, -1.59211F, -2.16153F, - -2.23601F, 0.91728F, 0.391576F, -0.810223F, -7.88111F, -2.08659F, -2.14996F, - -2.25843F, -2.10155F, -2.2645F, -0.42949F, -0.853058F, -0.476083F, - -0.738964F, -0.814578F, -0.818669F, -0.791287F, -0.85518F, -0.763718F, - -0.315142F, -0.826854F, -0.781778F, -2.18853F, -1.52533F, -2.11214F, - -2.05744F, -1.97881F, -0.835075F, -0.820764F, -0.835097F, -2.04855F, - -2.23896F, -2.0235F, -2.08923F, -2.07697F, -2.05018F, -2.07835F, -2.58505F, - -0.392069F, -1.24265F, -1.23975F, -1.02559F, -0.310172F, -1.22579F, - -0.420095F, -1.09806F, -0.341339F, -1.86929F, -1.93669F, -2.19973F, - -2.08249F, -2.08444F, -1.35417F, -1.24087F, -1.18843F, -2.15451F, -0.813121F, - -1.25398F, -1.23668F, -1.23619F, -1.24277F, -1.25571F, -0.836712F, -1.95826F, - -1.91001F, -1.99873F, -0.968984F, -0.846442F, -1.31642F, -1.2513F, -1.18995F, - -1.17282F, -1.13825F, -1.21051F, -0.84953F, -0.626828F, -1.08995F, -1.23305F, - -1.25269F, -1.23381F, -1.18756F, -0.850025F, -0.591468F, -0.584232F, - -0.827426F, -1.0936F, -0.594471F, -1.32243F, -1.1889F, -0.777271F, -1.18978F, - -1.22172F, -0.676141F, -1.06671F, -1.11154F, -0.666656F, -1.75109F, - -2.33993F, -2.50088F, -1.14046F, -1.79021F, -1.3614F, -1.30659F, -0.993933F, - -2.59363F, -1.15727F, -1.18309F, -0.982964F, -2.0626F, -2.67595F, -1.06643F, - -2.76215F, -2.19972F, -1.06994F, -2.24753F, -1.25633F, -1.19805F, -2.37286F, - -1.24412F, -2.87228F, -2.03696F, -2.02397F, -2.7888F, -2.76035F, -2.53499F, - -2.73514F, -2.35555F, -1.46106F, -1.9683F, -2.409F, -1.33639F, -1.26923F, - -2.71973F, -2.56272F, -2.23949F, -1.46534F, -1.28221F, -1.27268F, -1.29717F, - -2.45021F, -2.70581F, -2.40685F, -2.21268F, -2.06897F, -2.67199F, -2.79401F, - -2.68809F, -2.80497F, -2.56851F, -2.07143F, -2.26863F, -2.66148F, -2.37556F, - -1.26884F, -2.78757F, -2.80051F, -2.33623F, -2.2473F, -2.81312F, -2.45914F, - -2.32703F, -2.55306F, -2.10002F, -2.1984F, -2.6894F, -2.65733F, -2.81703F, - -2.4944F, -2.38787F, -2.84075F, -2.06192F, -2.11324F, -2.71221F, -2.79861F, - -2.75366F, -2.69086F, -2.51106F, -2.82664F, -2.68741F, -2.74665F, -2.78965F, - -2.68958F, -2.75274F, -2.63459F, -2.72831F, -2.70558F, -2.611F, -2.74287F, - -2.20173F, -2.70865F, -2.1838F, -2.27145F, -2.26631F, -2.65721F, -2.38593F, - -2.26539F, -2.80038F, -2.78893F, -2.22864F, -2.83149F, -2.66932F, -2.68396F, - -2.68478F, -2.68492F, -2.68244F, -2.67009F, -2.65075F, -2.32793F, -2.2332F, - -2.78655F, -2.82738F, -2.73259F, -2.85369F, -2.74037F, -2.41354F, -2.82656F, - -2.21337F, -2.83541F, -2.4965F, -2.80568F, -2.84557F, -2.65625F, -2.62197F, - -2.8253F, -2.49329F, -2.45106F, -2.20719F, -2.24504F, -2.60081F, -2.65446F, - -2.81209F, -2.87047F, -2.65263F, -2.66535F, -2.64435F, -2.09499F, -2.83398F, - -2.64999F, -2.79639F, -2.73945F, -2.82406F, -1.56334F, -2.70861F, -2.66431F, - -2.32639F, -2.23323F, -2.99838F, -2.70507F, -2.69379F, -2.6248F, -2.59922F, - -2.22341F, -2.59119F, -2.71623F, -2.75641F, -2.47117F, -2.34467F, -2.22762F, - -2.21826F, -2.20988F, -2.78884F, -2.75757F, -2.38245F, -2.6891F, -2.67761F, - -2.21563F, -2.72778F, -2.41888F, -2.72139F, -2.80975F, -2.65435F, -2.23258F, - -2.64406F, -2.84094F, -2.69404F, -2.30497F, -2.21452F, -2.74729F, -2.82531F, - -2.67417F, -2.67109F, -2.66331F, -2.65144F, -2.51151F, -2.8252F, -2.69803F, - -2.14415F, -2.74412F, -2.65567F, -2.60013F, -2.2457F, -2.34118F, -2.68099F, - -2.80333F, -2.72406F, -2.71661F, -2.8239F, -2.23794F, -2.25974F, -2.52864F, - -2.14006F, -2.1771F, -2.73595F, -2.8223F, -2.72683F, -2.76156F, -2.49453F, - -2.27016F, -2.20854F, -2.68603F, -2.83126F, -2.71386F, -2.67465F, -2.32614F, - -2.66411F, -2.39858F, -3.4086F, -2.86831F, -2.59038F, -2.67845F, -2.53615F, - -2.72699F, -2.71222F, -2.8845F, -2.65834F, -2.78361F, -2.52988F, -2.69303F, - -2.83033F, -2.61665F, -2.83315F, -2.67552F, -2.80777F, -2.22136F, -2.33978F, - -2.38637F, -2.81705F, -2.88689F, -2.37377F, -2.20076F, -1.64396F, -1.79636F, - -2.66543F, -2.62696F, -2.32207F, -2.77805F, -1.91912F, -2.25014F, -2.82211F, - -2.6782F, -2.68457F, -2.12156F, -2.72919F, -2.81871F, -2.82876F, -2.83466F, - -2.6559F, -2.64981F, -2.81829F, -2.27319F, -2.36968F, -2.6627F, -2.2476F, - -2.24189F, -2.74542F, -2.74773F, -2.81836F, -1.43289F, -1.92995F, -2.59273F, - -3.03625F, -3.19903F, -2.77573F, -2.69936F, -2.30333F, -2.26822F, -2.26502F, - -2.67351F, -2.42622F, -2.55465F, -2.27764F, -2.30714F, -2.22523F, -2.29233F, - -2.17197F, -2.11862F, -2.69115F, -2.65135F, -2.79585F, -2.34673F, -2.23178F, - -2.26176F, -2.25274F, -2.12293F, -2.02397F, -2.73572F, -2.53017F, -2.30288F, - -2.15358F, -2.2847F, -2.40713F, -2.29309F, -1.91224F, -2.9924F, -2.63779F, - -2.74523F, -2.79897F, -2.71568F, -2.39485F, -2.54108F, -2.41928F, -2.42443F, - -2.36845F, -2.14043F, -2.76686F, -2.60861F, -2.70922F, -2.54393F, -2.42272F, - -2.59481F, -2.57188F, -2.80751F, -2.17563F, -1.35294F, -1.09408F, -1.10759F, - -1.09996F, -1.08705F, -1.04553F, -1.33735F, -1.30253F, -1.38367F, -1.36521F, - -1.09953F, -1.05967F, -1.04098F, -2.70775F, -1.3073F, -1.36831F, -2.54415F, - -2.80564F, -1.06112F, -1.04295F, -2.09783F, -1.06467F, -1.09646F, -1.05326F, - -1.05932F, -1.54466F, -1.38375F, -1.36337F, -1.05612F, -1.04723F, -0.742467F, - -1.36975F, -1.35441F, -1.42773F, -0.0510482F, -0.0518662F, -0.0118576F, - -0.00170406F, -1.5408F, -1.32125F, -1.90019F, -1.10883F, 0.0344197F, - -0.0291711F, 0.0361544F, -2.21143F, -0.877532F, -0.856537F, -1.0976F, - -0.476026F, 0.0514791F, -0.0253184F, 0.0592111F, -1.5153F, -1.34556F, - -1.32352F, -1.35757F, -1.38797F, -2.25705F, -1.02826F, -0.865294F, -1.67528F, - -1.18115F, 0.413507F, -1.09564F, -1.07303F, -1.05954F, -0.501847F, -1.97459F, - -1.4179F, -1.32443F, -1.54089F, -1.30356F, -1.50304F, -1.03229F, -2.13012F, - 0.388167F, -0.739881F, -1.3714F, -2.6919F, -1.0875F, -0.889516F, 0.298656F, - -1.52743F, -1.37709F, -1.39863F, -1.31434F, -1.07783F, -1.54862F, -1.21005F, - -2.03685F, -2.10241F, -2.15762F, -1.58936F, 0.154246F, -1.19758F, -0.145155F, - -1.24251F, -1.22503F, 0.178928F, -1.10715F, -1.07728F, -0.577634F, - -0.508401F, -0.809495F, 0.166874F, 0.308586F, 0.309349F, -1.05925F, - 0.0531429F, -2.34812F, -2.281F, -2.34634F, -2.14367F, 0.0542366F, -0.804053F, - 0.107933F, 0.0343224F, 0.169865F, 0.311862F, 0.286126F, -1.05463F, - 0.0559099F, -2.31302F, -2.35604F, -1.52256F, -2.27005F, -2.3372F, -2.03848F, - -1.0787F, -2.17869F, -2.2512F, -1.37558F, -2.13958F, -1.55685F, -1.22227F, - -0.53387F, -0.715455F, -0.501962F, -1.49842F, -0.723656F, -0.380253F, - -0.437948F, -1.07307F, -0.288977F, -1.43243F, -1.09767F, -1.1098F, - -0.707254F, -1.31191F, -1.29575F, 0.430338F, -1.00549F, -0.878467F, - 0.751222F, 0.506699F, -0.259056F, -0.809703F, -0.854617F, -3.71185F, - -0.208773F, -1.58264F, 0.319954F, -0.222053F, -0.20604F, -0.181192F, - -0.186994F, -2.56325F, -0.243632F, -0.263384F, -0.211317F, -0.259237F, - -0.26992F, -0.31041F, -1.35725F, -0.178845F, -0.168259F, -1.34445F, - -0.17996F, -0.182038F, -0.166914F, -0.245092F, -0.180151F, -0.203233F, - -1.61812F, -0.166546F, -1.46652F, -0.203055F, -0.287539F, -0.309231F, - -0.257356F, -1.44814F, -0.288588F, 1.33331F, 1.39407F, 1.18571F, 1.20576F, - 1.00699F, 1.15971F, 0.168883F, 1.60066F, -0.228673F, -0.169392F, 1.40435F, - 1.14124F, 1.233F, -0.236503F, 1.30467F, 0.673446F, -0.238831F, -0.263076F, - -0.0201321F, -0.0534548F, -0.248777F, -0.213857F, -0.212038F, -0.206269F, - 1.08781F, -1.53267F, -0.147066F, -0.168736F, -0.247853F, -0.129338F, - -1.55968F, -0.169776F, 1.29348F, -0.170139F, -0.207145F, -0.237126F, - -0.21168F, -0.263838F, -0.252354F, -0.147892F, -0.161353F, -0.14715F, - -0.207843F, -0.102221F, -1.64344F, -0.0833787F, -0.148479F, -0.233998F, - -0.237907F, 3.15942F, -0.178968F, -0.15579F, -0.252775F, -0.26998F, - -0.237741F, 2.04588F, -0.234593F, -0.229904F, -0.252421F, -0.166596F, - -0.0917029F, -0.230529F, -0.179496F, 0.869507F, 1.28017F, 0.797031F, - 1.22661F, 1.2569F, 1.42371F, 1.18697F, 1.34506F, 1.38199F, 1.08467F, - 1.09993F, 0.981392F, 0.907753F, 0.9429F, 1.28041F, 0.704922F, 1.15847F, - 0.206182F, 1.12502F, 1.3779F, 1.38436F, 1.57669F, 1.08387F, 1.12056F, - 0.910255F, 0.908003F, 1.07036F, 1.08021F, 0.815863F, 0.35928F, 0.957852F, - 0.85495F, 1.28057F, 1.95747F, -0.169864F, 0.312911F, 0.166411F, 0.282302F, - 0.308456F, 0.166175F, 1.23571F, 1.46247F, 1.40849F, 1.33041F, 1.76728F, - 1.99655F, 1.07632F, 0.808146F, -0.149234F, -0.268101F, 1.28761F, -0.252471F, - 0.792234F, 0.896416F, 1.12806F, -0.19188F, -0.149058F, -0.165813F, - -0.160624F, -0.166633F, -0.161613F, 1.20588F, -0.165173F, -0.210835F, - -0.161611F, -0.131104F, 1.08049F, 0.62123F, -0.182141F, -0.167443F, - -0.222686F, -0.18054F, -0.166489F, -0.180731F, 1.4136F, -0.172962F, - -0.0283509F, 0.990514F, -0.131828F, -0.140552F, -0.165247F, -0.130164F, - -0.175936F, -0.124648F, -0.141369F, 0.613916F, 1.17388F, 1.30763F, 1.35051F, - 1.37074F, 1.23893F, 1.9781F, 2.00014F, 1.87127F, 0.30138F, 1.17433F, - 1.34815F, 1.15917F, 1.19902F, 1.32084F, 1.40486F, 0.938193F, 1.09471F, - 0.41814F, 0.678749F, 0.299616F, 1.34698F, 1.32443F, 1.35165F, 0.699155F, - 1.35301F, 1.32898F, 1.26073F, 1.79673F, 1.90596F, 1.09163F, 0.933001F, - 0.189877F, 0.276421F, 0.943667F, 0.291247F, 0.629897F, 0.638698F, 0.920721F, - 1.25176F, 1.2222F, 0.226825F, 1.82777F, 0.350968F, 1.20506F, 1.23881F, - 1.05622F, 1.56077F, 1.33711F, 1.86329F, 1.33655F, 0.10689F, 0.757755F, - 0.424282F, 0.935586F, 0.702983F, 0.626297F, 0.901476F, 0.317991F, 0.372178F, - 0.619115F, 0.14489F, 0.703217F, 1.85644F, 1.10033F, -0.605173F, 0.430693F, - 0.79111F, 1.47542F, 1.09006F, 0.0269742F, 0.0108088F, 0.907506F, 1.09378F, - 1.10107F, 0.366246F, 0.384303F, 0.0942007F, 0.866092F, 1.38043F, 1.37115F, - 1.36238F, 1.28807F, 1.35462F, 1.33751F, 1.37856F, 0.36301F, 0.314848F, - 1.89982F, 0.472281F, 1.19588F, 1.21319F, 1.23268F, 1.39442F, 1.35346F, - 1.36768F, 2.39714F, 1.28458F, 1.32383F, 1.39334F, 1.54667F, 1.21452F, - 0.861053F, 1.92529F, 1.77103F, 1.8266F, 1.90861F, 1.90432F, 1.10662F, - 0.990569F, 0.00693489F, 0.00547487F, 0.16008F, 0.408718F, 0.694579F, - 1.72476F, -0.00249987F, -0.0367136F, 0.931703F, -0.0192565F, 0.625378F, - 1.3728F, 1.36283F, 1.1734F, 1.25542F, 1.34289F, 0.361586F, 2.57645F, - 1.89409F, 2.32125F, 1.19965F, 1.23108F, 1.24629F, 1.18629F, 2.16496F, - 1.21195F, 0.866349F, 0.005885F, 0.159487F, 0.108479F, 0.789436F, 0.707974F, - 0.404915F, 0.0419107F, 0.0425121F, 0.924304F, 0.199161F, 0.094471F, 1.94053F, - 1.254F, 0.366304F, 0.429272F, 2.20135F, 1.15372F, 1.22015F, 1.37461F, - 1.21453F, 0.776338F, 1.22489F, 2.01197F, 0.733575F, 0.0305245F, 0.16107F, - 0.138544F, 0.190617F, 1.08393F, -1.51818F, 1.06515F, -1.4185F, 2.09549F, - 1.40022F, 1.53264F, 0.0622628F, 0.139442F, 0.68937F, 0.57903F, 0.869537F, - 0.143482F, 0.274909F, 0.888438F, 1.34531F, 1.31519F, -0.462997F, 1.31251F, - -0.0953649F, 1.17552F, -0.571149F, -0.3894F, 0.0748854F, 1.64986F, 1.278F, - -0.0385255F, 0.805606F, 1.08916F, 1.22604F, 1.24162F, 1.19003F, 1.21064F, - 1.3645F, 1.18529F, 1.27729F, 1.24245F, 1.16566F, 1.19091F, 1.21175F, 2.2497F, - 1.82887F, 1.12309F, 1.23698F, 1.23582F, 1.21889F, 1.25281F, 1.85949F, - 1.85356F, 1.34943F, 1.30867F, 1.30888F, 1.40882F, 1.33969F, 1.31211F, - 2.11043F, 1.29114F, 2.3131F, 2.43935F, 1.30474F, 1.74817F, 1.15143F, - 1.75423F, 1.78446F, 1.83991F, -0.239566F, 0.711378F, 0.68751F, 0.698416F, - 0.954525F, -0.003198F, 1.20363F, 2.0558F, 2.31902F, 2.54665F, 1.15435F, - 0.4534F, 1.20805F, 0.808031F, 1.26951F, 1.19609F, 2.10075F, 1.3545F, - 1.28252F, 1.53925F, 0.136264F, 1.77233F, 1.98949F, 2.05116F, 1.10691F, - 1.12659F, 0.990841F, -0.366296F, 0.685617F, 1.11763F, 0.873708F, 1.80452F, - 0.460185F, 2.229F, 0.456804F, 1.21574F, 1.33656F, 0.289618F, 2.05281F, - 0.334711F, 1.84007F, 1.8513F, 0.211486F, -0.0682335F, 0.140365F, 0.0936837F, - -0.809714F, -0.351872F, 0.791037F, 0.709921F, 0.57026F, 0.701669F, 1.13938F, - 0.611961F, 0.0513348F, 0.0524435F, 1.89129F, 1.31703F, 2.59715F, 1.8013F, - 1.37428F, 0.37271F, 1.3376F, 1.51523F, 0.869754F, 1.11362F, -0.354259F, - 0.828715F, 1.12026F, 0.623257F, 0.056625F, 0.173246F, 0.263702F, -0.45248F, - 1.33038F, 1.44153F, 1.33168F, 3.89545F, 1.56638F, 0.905889F, 0.0871203F, - -0.379758F, 1.30876F, 1.14201F, 1.79294F, 0.897834F, 0.292003F, 0.80014F, - 1.32232F, 1.13449F, 1.25246F, 0.943354F, 0.791442F, 1.92036F, 1.85001F, - 1.36025F, 2.31658F, 1.97021F, 1.19057F, 1.3381F, 1.32535F, 0.0493891F, - 0.714684F, 1.39869F, 0.716473F, -1.17404F, -1.18448F, 1.47385F, 1.39597F, - 1.19354F, 1.41063F, 1.10969F, 1.32171F, 1.27435F, 1.16345F, 0.671546F, - 1.36423F, 1.35019F, 2.24717F, 1.31541F, 1.36693F, 1.76987F, 1.30766F, - 1.33745F, 1.31691F, 1.36315F, 1.4076F, -0.380756F, 0.348083F, 1.26793F, - 1.21F, 1.30564F, 1.21138F, 2.31226F, 1.31701F, 1.24924F, 1.36294F, 1.25303F, - 2.16865F, 0.346747F, 2.44785F, 0.871157F, 1.30919F, 1.38483F, 2.20606F, - -0.111848F, 1.26936F, 1.32551F, 1.34509F, 1.14888F, 2.53412F, 2.64321F, - 2.67988F, 2.38135F, 2.32333F, 1.39827F, 2.65577F, 3.0823F, 1.80156F, - 0.993463F, 1.31589F, 2.16392F, 1.54917F, 2.86016F, 2.49937F, 0.870969F, - 2.51583F, 2.15246F, 2.42437F, 2.43485F, 2.3852F, 0.909052F, 0.61906F, - 1.40052F, 1.43028F, 1.44771F, 0.907453F, 0.890897F, -0.414248F, 1.337F, - 1.2274F, 2.38498F, 1.12783F, 1.22674F, 2.22702F, 2.58389F, 2.73555F, 3.1609F, - 3.09851F, 3.19529F, 2.28189F, 3.3716F, 2.55702F, 3.30246F, 2.83833F, - 3.04255F, 2.47032F, 2.77709F, 0.901406F, 1.37496F, 2.46829F, 3.41927F, - 2.9971F, 3.11831F, 3.03316F, 3.34499F, 2.34929F, 1.73085F, 2.9048F, 3.09797F, - 2.36615F, 0.856786F, 0.899295F, 2.28705F, 2.48174F, 1.1828F, 2.2871F, - 2.39598F, 2.25605F, 2.64171F, 2.53356F, 2.55762F, 2.74483F, 2.81293F, - 2.81986F, 3.0253F, 0.942648F, 0.925506F, 0.91762F, 0.902501F, 0.893184F, - 2.51639F, 2.79751F, 2.4295F, 1.2912F, 2.33614F, 2.49789F, 2.5169F, 0.940791F, - 0.920803F, 0.875766F, 1.17667F, 1.539F, 0.897439F, 0.863087F, 2.57101F, - 2.59712F, 1.12117F, 2.64701F, 1.30833F, 0.37051F, 1.28894F, 1.28307F, - 1.91199F, 2.25943F, 2.53084F, 3.84723F, 3.07251F, 2.60794F, 2.98445F, - 3.02011F, 2.55359F, 2.04701F, 1.84423F, 2.60461F, 0.926093F, 0.909524F, - 0.905611F, 0.848389F, 1.19949F, 3.11489F, 1.27825F, 3.29298F, 1.34318F, - 2.64147F, 2.88322F, 3.67985F, 2.2429F, 2.79551F, 2.77974F, 3.16556F, - 2.78784F, 0.903456F, 0.903455F, 0.709286F, 1.22893F, 1.33374F, 2.1982F, - 2.50906F, 2.33352F, 3.0537F, 2.85073F, 1.74901F, 1.35055F, 2.05938F, - 2.45165F, 0.926575F, 0.34683F, 1.30473F, 1.36973F, 2.76059F, 0.863332F, - 2.71234F, 0.88209F, 0.89967F, 1.22368F, 2.22407F, 2.02126F, 2.03666F, - 3.33919F, 2.64136F, 2.08165F, 3.23096F, 3.04127F, 3.2829F, 2.81008F, - 2.38895F, 2.0469F, 1.85471F, 2.43185F, 2.62182F, 2.53522F, 2.42648F, - 0.897699F, 2.05114F, 2.09537F, 2.19043F, 1.96677F, 2.03849F, 3.31748F, - 2.26665F, 2.31849F, 3.32136F, 3.25199F, 1.4043F, 2.94326F, 3.51648F, 3.2965F, - 2.52994F, 2.72336F, 0.92204F, 2.5335F, 2.69473F, 2.52658F, -0.201164F, - 2.36162F, 3.35413F, 1.89097F, 1.64497F, 3.0744F, 0.92738F, 0.926233F, - 0.903594F, 0.94194F, 0.279853F, -0.220642F, 3.28671F, -0.258182F, -0.169F, - 2.63395F, 3.12393F, 1.32589F, 2.41675F, -0.175266F, 2.07614F, 2.62077F, - 2.06673F, 2.1124F, -0.166552F, 2.51056F, 2.28902F, 1.88331F, -0.150085F, - 0.925746F, 0.972595F, 2.28936F, 2.37561F, 2.72329F, 1.78567F, 1.95002F, - 0.644139F, 2.00903F, 2.58968F, 1.8707F, 1.88223F, 0.16691F, 1.00802F, - 2.14694F, 2.7997F, 2.36884F, 2.60505F, 3.04738F, 2.8081F, 0.873164F, - 0.929309F, 2.63535F, 2.62867F, 2.11906F, 2.46976F, 2.00506F, 2.25367F, - 2.03346F, 2.09374F, 2.06471F, 2.03561F, 2.13438F, 2.2239F, 2.36105F, - 2.29284F, 2.30278F, 2.30621F, 2.5598F, 2.81586F, 2.77281F, 2.03213F, - 2.87544F, 2.10366F, 2.50122F, 2.15742F, 2.22853F, 2.23342F, 2.35062F, - 2.79798F, 2.35736F, 2.56596F, 2.64962F, 1.1431F, 2.07714F, 2.16174F, - 2.58121F, 2.52558F, 2.02877F, 2.12387F, 3.30431F, 2.3415F, 2.2184F, 2.23935F, - 2.29431F, 2.30097F, 2.39271F, 2.35682F, 0.950334F, 0.89697F, 2.44489F, - 1.97032F, 3.33765F, 2.22273F, 4.49754F, 3.27619F, 2.07722F, 2.21384F, - 2.25987F, 2.16787F, 2.13361F, 2.04417F, 2.28919F, 2.07499F, 2.23818F, 2.196F, - 2.19439F, 2.34944F, 2.34444F, 2.3256F, 2.33816F, 2.28273F, 2.26642F, - 2.44397F, 3.04028F, 3.0813F, 2.51374F, 2.23417F, 2.12172F, 2.27249F, 2.2252F, - 2.34261F, 3.25693F, 2.69533F, 2.90941F, 2.40182F, -0.102039F, 0.934135F, - 0.987916F, 0.920867F, 0.954986F, 1.1455F, 0.835034F, 2.59082F, 1.1901F, - 1.83019F, 1.00359F, 2.58746F, 1.87727F, 2.05002F, 1.98619F, 2.42245F, - 2.13328F, 1.29007F, 4.19507F, 15.1404F, 2.97923F, 2.737F, 3.32281F, 2.26331F, - 2.10822F, 2.32808F, 3.07009F, 1.99436F, 2.18089F, 2.31711F, 2.33395F, - 2.32885F, 2.37395F, 2.63263F, 2.87945F, 3.16904F, 2.21393F, 2.30767F, - 2.89333F, 2.13642F, 2.35909F, 2.13994F, 2.17843F, 2.51354F, 2.31506F, - 1.81998F, 2.49114F, 2.93131F, 2.12286F, 4.83662F, 2.02559F, 2.00787F, - 2.27863F, 2.22732F, 2.39542F, 2.28059F, 2.82887F, 2.81567F, 3.17234F, - 4.91281F, 1.14386F, 2.81324F, 2.77188F, 2.78694F, 1.12159F, 2.34087F, - 2.56516F, 2.48771F, 1.2117F, 2.43161F, 2.27211F, 8.96465F, 3.92004F, - 3.03818F, 0.937831F, 2.67553F, 0.88634F, 2.8291F, 3.15591F, 3.27676F, - 2.51998F, 2.67306F, 3.08117F, 1.03122F, 0.975956F, 2.35514F, 2.4484F, - 2.58243F, 2.18635F, 2.08158F, 3.02857F, 0.659026F, 2.05981F, -2.14012F, - -2.16512F, 2.58656F, 2.83651F, 2.09692F, 2.36405F, 2.13179F, 2.51318F, - -2.12961F, 1.71317F, 3.24163F, 1.37661F, 2.31355F, 0.948072F, 2.15471F, - -0.240342F, 2.33689F, 2.06467F, 1.48307F, 2.24721F, 1.24702F, 1.86189F, - 2.90147F, -0.270957F, -0.239258F, 2.39096F, 2.66068F, 2.34295F, -0.210685F, - -0.175642F, -0.175753F, -0.28764F, -0.278139F, 0.0744992F, 0.0823205F, - -1.74682F, 2.50206F, 2.55788F, 2.46241F, -0.213448F, 1.12892F, -0.133235F, - -1.18022F, 2.67369F, -0.213082F, -0.16775F, 2.31444F, 0.0870316F, 1.37494F, - -0.226873F, 2.13096F, 0.976182F, -0.230067F, -0.176039F, -1.73252F, - -1.82565F, -1.82172F, -2.07239F, 2.48324F, 2.50562F, 2.73159F, 2.79117F, - -2.92993F, 2.26803F, 2.72265F, 2.25374F, 2.35502F, 2.83338F, -2.60128F, - -0.251466F, -0.143358F, -2.86011F, -2.76913F, -2.74693F, 2.66464F, 1.25169F, - 1.13996F, -1.53296F, -1.72492F, -2.23773F, -2.45034F, -2.23774F, -2.30684F, - -2.25888F, -2.26619F, 2.53525F, -0.257861F, 2.62601F, 2.9909F, -2.11545F, - 0.0993311F, -2.58589F, -1.57407F, -0.162085F, -0.289541F, -0.165377F, - -0.27314F, -0.132757F, -1.11119F, -1.14311F, -2.08544F, 2.23556F, 2.37291F, - 2.2836F, -0.229867F, 2.7721F, -0.174323F, 1.74949F, -0.274155F, -1.52277F, - 0.342922F, -1.54394F, -2.15936F, -2.09616F, -2.26102F, -2.10552F, -2.16026F, - -2.16985F, -1.12569F, 2.58843F, -0.261231F, 2.20949F, 2.53962F, -0.255143F, - 2.74811F, 1.31853F, 2.02532F, 2.47441F, 3.18468F, 2.51308F, 2.28469F, - 2.27866F, 2.44639F, 0.990421F, -0.0355083F, 2.66679F, 2.82839F, 2.71863F, - 0.915942F, 0.936792F, -1.14491F, -2.15985F, -1.00768F, -1.12081F, 0.0974861F, - 0.286766F, 1.20499F, -2.72232F, 3.21224F, 3.19603F, 2.55285F, -0.249496F, - 3.25174F, 2.54149F, 2.54764F, 2.41364F, 1.23766F, 0.840687F, -0.256124F, - 2.35617F, 2.85264F, 3.02343F, -0.0515767F, 2.57715F, 2.74675F, 2.40688F, - -1.09959F, -1.11136F, 0.950238F, 0.857731F, 0.912589F, -1.11688F, -1.12725F, - -1.10288F, -1.15733F, -1.12521F, -1.95223F, -0.981455F, -1.1799F, -1.10814F, - 0.107835F, 0.339593F, -0.756721F, -1.08829F, -1.30426F, -3.05662F, -2.62221F, - -2.7275F, -1.62785F, 2.6094F, -1.09275F, -1.16593F, -1.17869F, -1.55667F, - 2.23666F, 3.11854F, 2.45976F, -0.251612F, -0.165986F, 1.39228F, -0.213823F, - -0.161731F, 2.67735F, 3.61301F, -0.204614F, 1.23194F, 1.57856F, -2.3123F, - -1.07583F, -1.21171F, 3.00357F, 2.72955F, 1.58868F, 2.76771F, 2.74034F, - -1.08326F, 0.915638F, -1.14928F, -0.295637F, -0.257846F, -1.11533F, - -1.15176F, -1.11371F, -1.12246F, -2.36886F, -1.06358F, -1.10203F, 0.0440825F, - -1.25004F, -1.22921F, -1.59086F, -1.09323F, -1.08008F, 2.53484F, 1.52017F, - -0.141988F, -0.994885F, 2.72632F, -1.26891F, 0.665421F, -1.12347F, -1.18683F, - -1.10244F, -1.16417F, -1.14939F, -0.989332F, -2.04658F, -0.715726F, - -0.491373F, 1.92153F, -1.19971F, 0.783312F, -1.12977F, -1.23574F, -1.53494F, - -0.332482F, -1.50904F, -1.4776F, 1.31074F, 3.05964F, 2.90194F, 1.33883F, - 2.66899F, 1.35338F, -1.00827F, -0.997166F, 2.0016F, -1.05748F, -4.81483F, - -1.06305F, 0.033149F, -0.071076F, -1.23594F, -0.330126F, -1.0806F, -1.00259F, - -0.718587F, -1.23083F, -1.45682F, -1.10395F, -0.90075F, -0.938904F, 1.13851F, - -2.37412F, -1.04743F, -0.152136F, -1.0426F, -1.21903F, -1.04159F, -0.548359F, - -0.072689F, -0.985669F, -2.10523F, 1.12254F, -1.27483F, -1.31351F, -1.08683F, - 2.35171F, 0.318304F, -1.08795F, -1.80773F, -1.99729F, -5.2961F, -2.03167F, - -2.05305F, -2.13512F, -2.22104F, 2.64267F, -0.260497F, -0.136588F, 1.24096F, - 2.83598F, -1.17646F, 0.935959F, -1.09724F, -2.15204F, -1.12272F, - -0.00900915F, 1.15075F, -1.57361F, -0.172565F, 3.02267F, 1.44702F, 1.40456F, - -0.183362F, 1.295F, -0.133374F, 1.56218F, -0.138294F, -0.182027F, 2.57692F, - 2.64111F, 1.07745F, -1.13903F, -1.35478F, -1.15076F, -1.10659F, -0.0113115F, - -0.072231F, -0.00879277F, 3.38837F, 1.20507F, -0.144508F, 4.13351F, 3.01923F, - -0.211523F, -0.201023F, -0.287198F, 2.51827F, 2.67067F, -1.1072F, -1.10547F, - -1.11729F, -2.33719F, 0.11492F, -1.10125F, -0.0788316F, -0.281151F, 3.10239F, - -0.287061F, 2.32687F, -1.10854F, -1.11348F, -3.47642F, -1.16316F, 0.160204F, - 0.863012F, -0.158511F, -2.55854F, -0.107979F, 1.05392F, -0.165295F, - -1.04987F, -0.983471F, -2.25663F, -1.14296F, -1.42153F, -2.00002F, 0.104862F, - 0.0367734F, -0.406905F, -1.25416F, 1.08418F, -2.3335F, -1.15502F, -1.09108F, - -1.72463F, -0.327835F, -1.07586F, -0.568485F, -0.124209F, 1.32912F, - -1.27192F, -1.10887F, -2.18267F, 3.29858F, 2.55496F, 3.15563F, 2.51009F, - -1.1077F, -1.13867F, -0.272699F, 3.2577F, -0.219016F, -0.171294F, 3.39626F, - -0.204956F, -0.25774F, 1.30893F, 1.21629F, -0.140591F, -0.204505F, 3.07297F, - 2.22256F, -0.239174F, -0.134081F, -0.150341F, 2.55805F, 0.98373F, -0.317049F, - -0.721766F, -1.02563F, -2.59281F, -0.166239F, -1.61732F, -0.202924F, - -0.28537F, -0.219982F, 2.37456F, -2.2622F, -1.04526F, -0.167097F, -1.03323F, - -2.23513F, -1.1451F, 0.259475F, 0.342607F, -0.242101F, 1.07916F, 2.50105F, - -0.243481F, -1.00518F, -0.163428F, -1.39723F, -0.539069F, 0.290496F, - -2.18539F, -2.19924F, -1.56266F, 2.61802F, -0.233785F, -0.262717F, 2.79704F, - 2.28529F, 2.37087F, 0.948229F, 1.15597F, -0.21957F, -0.225432F, -0.142973F, - -0.158886F, -0.286411F, 3.15383F, -0.251423F, -0.267668F, 2.39918F, - -0.966792F, 2.73318F, 1.30565F, 1.33527F, 1.76099F, 2.20382F, 1.11255F, - -1.37479F, -1.11552F, -0.278911F, -0.213966F, -0.167085F, -0.288932F, - -0.206276F, -0.146746F, -2.28573F, -1.14952F, 1.25908F, -1.10878F, 1.26919F, - -0.303158F, -1.15455F, 0.670965F, -1.18436F, -1.11581F, -1.18782F, -1.1382F, - 3.02405F, 3.02598F, 1.85151F, -0.264937F, -0.152176F, -0.174766F, -0.216313F, - -0.262659F, -0.285239F, 1.78252F, 2.92753F, -1.1175F, -1.10628F, -1.0636F, - -1.1141F, -1.06737F, 1.1774F, -0.217345F, -0.171736F, -0.247043F, 1.34532F, - -0.265647F, -0.227968F, -0.311631F, -1.22815F, -1.04995F, -1.1203F, - -1.12415F, 1.0341F, -1.09801F, -1.13592F, -1.23438F, -0.271654F, -0.154837F, - -0.153809F, -0.190215F, -0.300307F, 1.75014F, -0.263653F, -1.16165F, - -1.14292F, -1.05472F, -1.3421F, -2.20378F, -1.12927F, -2.27044F, 0.0298201F, - -1.12904F, -0.254603F, 2.28358F, 2.75113F, -1.1108F, -0.163646F, -0.275725F, - 2.12569F, -0.271015F, -0.129884F, 1.8821F, -1.10306F, -1.09704F, -0.167572F, - -0.161991F, -0.163015F, -0.197531F, 1.83415F, -0.313507F, -1.13463F, - -1.12199F, -1.11069F, -0.958776F, 1.76766F, -1.11046F, -0.951631F, -1.1543F, - 0.518467F, 0.929955F, -1.11478F, -1.12507F, -1.12606F, 2.32137F, -0.257052F, - 1.99486F, -1.12086F, -1.10564F, 2.30752F, -0.17596F, 1.91886F, -0.143291F, - -0.25601F, -0.160918F, -1.11777F, -1.12615F, 0.00440096F, 2.29275F, - -0.228782F, 2.246F, -0.241274F, 2.40745F, -0.171347F, -0.239319F, -0.163844F, - -0.135188F, -1.12571F, -1.08374F, -1.11017F, 2.45054F, -0.176852F, - -0.266948F, 2.28528F, 1.88367F, -0.34282F, -1.0936F, -1.10303F, -1.09235F, - -1.11715F, -0.134237F, 1.84731F, -2.2179F, -0.344348F, 2.66286F, 2.29601F, - 2.61477F, 2.69553F, 2.10236F, -0.999097F, -2.20084F, -0.169725F, -0.173626F, - -0.203643F, -0.149116F, -0.252517F, 2.09025F, -1.00732F, -1.09839F, - -1.12006F, 0.056819F, -0.108728F, -0.207831F, -0.262886F, -0.2492F, - -0.284882F, -0.168593F, -0.168549F, -0.205067F, 2.17514F, -1.31732F, - -0.287742F, -2.19739F, -0.226086F, -0.337826F, -1.0922F, 0.299576F, - -2.27813F, -0.674251F, 1.92627F, 0.277543F, -0.724174F, -0.608334F, - -0.182483F, -0.858178F, -1.04166F, -0.265412F, -1.45193F, -0.87792F, - 1.98301F, -0.25332F, -0.251608F, -0.200786F, -0.178196F, 1.72331F, - 0.0520121F, -0.998845F, -0.366132F, 0.0569893F, -1.4299F, -1.09998F, - -1.05922F, -1.08231F, -0.742382F, -0.113451F, -0.682408F, -1.32626F, - -0.166333F, -0.200992F, -2.27722F, -0.624611F, -0.180295F, -0.312075F, - -1.09544F, 1.29856F, -1.36884F, -1.31309F, -3.50033F, -2.21781F, -1.49783F, - -1.44563F, -0.748718F, -8.61068F, 0.264825F, -1.37533F, -1.22682F, - -0.0312094F, 0.678532F, 1.18193F, -0.498872F, -0.167468F, -0.198161F, - -0.232746F, -0.809586F, 0.110628F, 0.0291782F, 0.335697F, -0.0289031F, - 0.382025F, 0.268372F, 0.625994F, 0.678261F, -0.176458F, -1.13061F, - -0.716655F, 0.0316746F, -0.029401F, 0.311541F, -0.570378F, -0.527544F, - 0.276382F, 0.732281F, -0.162759F, -1.40975F, -1.38974F, 0.298536F, 0.365939F, - -0.857284F, 1.10388F, -0.071267F, -0.747348F, -0.523799F, 0.0412912F, - -1.24513F, -0.135179F, -1.37297F, -1.32086F, 0.697038F, -0.380125F, - -0.629092F, -0.0811982F, 0.0761747F, -0.147631F, -0.75177F, -0.715188F, - -1.42213F, 0.640545F, -1.46191F, -1.06405F, -0.389483F, -1.32942F, - -0.714989F, -1.39735F, 0.938325F, -0.413122F, 0.72518F, 0.781033F, -1.33804F, - 0.978125F, -1.30451F, -0.0517474F, -0.0476736F, 0.272621F, 1.14854F, - 0.57288F, -0.242053F, -2.16083F, -0.714301F, -0.15456F, -0.263084F, - -0.280016F, -0.257777F, -1.21622F, -0.265797F, 0.968951F, 0.162985F, - 0.676469F, 0.655545F, 0.810888F, 0.487314F, 0.4752F, 0.247879F, -0.140592F, - 0.764472F, -0.179613F, -0.403715F, -0.173125F, 0.476959F, 0.103758F, - -0.180204F, 0.633259F, -0.260488F, 0.422568F, -0.247612F, -0.168682F, - -0.264864F, -0.72458F, -0.996515F, -0.207389F, 0.0104264F, -0.207003F, - -0.259704F, -0.169155F, 0.0833812F, 0.00381601F, 0.558928F, 0.405104F, - 0.574466F, 0.322373F, -1.27078F, 0.864134F, 0.816153F, -0.725967F, - -0.173979F, -0.165024F, 0.148206F, 0.413054F, 0.41004F, 0.389344F, - -0.243205F, -0.18639F, -0.202564F, -0.673044F, -0.581382F, -0.206572F, - -0.866134F, -0.156914F, -0.386881F, -0.879551F, -0.800788F, -0.157867F, - -0.890868F, 0.184281F, 1.03112F, 0.683013F, 0.482723F, 0.178126F, -0.709556F, - 0.932452F, -1.21588F, -0.381767F, 0.512526F, -1.12849F, -0.158104F, - -0.72452F, -0.489656F, -0.172455F, -0.210583F, -0.210538F, -1.20585F, - 0.825964F, 0.703706F, 0.455914F, -0.851548F, 0.0741099F, -0.0571138F, - -1.23341F, 0.74275F, 0.639261F, 1.03164F, 0.885506F, -0.499196F, -0.290325F, - -0.883972F, -0.0650937F, 0.711197F, 0.375152F, -0.844722F, 0.659745F, - -0.436335F, -1.33225F, -1.42327F, 3.09288F, -0.489984F, -0.0624362F, - 0.335563F, -1.31097F, -1.22101F, -1.49767F, -1.32352F, 0.114105F, 1.28125F, - 1.15707F, 0.409913F, 0.461268F, 1.27035F, 0.723616F, 0.580528F, 8.03553F, - 1.44502F, 1.27205F, 0.957852F, 0.905437F, -0.0408411F, 0.0146593F, 0.819581F, - 0.576791F, 0.582279F, 0.280465F, 0.422815F, 0.399645F, -0.184872F, 0.20881F, - 0.272289F, 3.09515F, 1.11898F, 1.43525F, 2.78316F, 0.862938F, -0.858365F, - 1.2123F, -0.18152F, 0.901362F, -0.0674431F, 0.0102118F, 0.711017F, 0.383423F, - -0.517407F, 0.926853F, 1.07592F, -0.268628F, -0.192588F, -1.32343F, - -1.12962F, 0.327791F, 0.424342F, -0.241936F, 1.95672F, -0.259689F, 0.292545F, - 0.882515F, -0.255708F, 0.856235F, -0.208639F, 1.50531F, -1.03519F, 2.94699F, - 1.25225F, 1.7815F, -0.716035F, 1.632F, -1.01335F, 0.0111331F, -0.0419209F, - 0.581762F, 0.0614642F, -0.806844F, -0.742176F, -0.686468F, 0.737734F, - -1.46044F, -0.155577F, -1.06267F, 0.345654F, 1.59478F, -0.238834F, - -0.628351F, -0.139512F, -0.179279F, -0.281367F, -0.243615F, -0.262662F, - -0.303974F, 0.100012F, 0.256207F, -0.416182F, -0.547758F, 1.1258F, - -0.738956F, -0.75022F, 0.304924F, 0.437851F, -1.33785F, -0.225709F, 1.1305F, - 1.14622F, -1.16743F, -0.66795F, -0.0651348F, -0.281364F, -1.19596F, - 0.511235F, 0.472101F, -0.41393F, 0.0443736F, 0.912181F, 0.927747F, 1.01705F, - -0.0434686F, 0.038567F, 1.13855F, -1.20528F, 1.72873F, 0.385466F, 0.912698F, - 0.596473F, -0.503811F, -1.23236F, -1.8281F, 0.151456F, 0.40449F, -0.002083F, - -0.179733F, -1.30917F, 0.295446F, -0.872426F, -0.279802F, -0.0465756F, - 0.465193F, 0.482647F, 0.118335F, 0.147645F, 0.796636F, -0.193182F, - -0.000477764F, -0.136068F, 0.419211F, -1.11816F, -0.269151F, 0.783436F, - -0.135853F, -0.811461F, -0.237291F, -0.275247F, -0.272482F, -0.278242F, - -0.0931979F, -0.0354825F, 0.675485F, 0.795391F, 0.308154F, 0.408895F, - 0.146036F, -0.419355F, 0.288883F, 0.224684F, 0.643431F, 0.37575F, 0.137941F, - 0.499559F, -0.164028F, 0.0782191F, -0.994784F, -0.82001F, -1.25553F, - -0.270296F, 0.0185581F, 0.139358F, 0.0567041F, 0.091934F, 0.614364F, - -1.48059F, 0.0517422F, -0.0385974F, -0.828164F, -1.04519F, -0.260627F, - -0.162662F, 0.757774F, -0.0322297F, -0.237103F, 0.367566F, 0.607891F, - 0.71585F, -1.27324F, -1.24622F, 0.1636F, -1.35026F, 2.03015F, 2.50173F, - 2.39788F, 2.3245F, 2.74561F, -0.00847785F, -0.152673F, 2.66161F, 1.98631F, - 3.55023F, 2.65858F, -0.00960693F, -0.109412F, -0.266771F, -0.221566F, - -0.238777F, 1.688F, -0.256277F, -0.268452F, -0.252389F, -0.205401F, - 0.172919F, -0.266791F, -0.257978F, -1.09076F, 2.40503F, 2.53127F, 2.40866F, - 2.01822F, 2.77979F, 2.20662F, 2.63285F, 2.7907F, 2.46034F, 2.60238F, - 2.73145F, 2.57097F, 2.36329F, 2.42131F, 1.92348F, 0.532564F, 2.25573F, - 2.465F, -0.146504F, 2.52472F, -0.108515F, -0.158251F, -0.256609F, -0.251635F, - -0.248539F, -0.252373F, 1.99658F, -0.16347F, -0.273285F, 2.67655F, 2.26516F, - 2.28617F, 2.65645F, 2.17356F, 2.09502F, 2.03206F, 1.94632F, 2.20973F, - 1.94605F, 2.55842F, 2.18415F, 2.347F, 1.91795F, 2.005F, 3.10395F, 1.38798F, - 0.607084F, -0.142814F, 1.28724F, 2.33044F, 1.2927F, 1.81196F, 1.87838F, - 1.72125F, 1.89158F, 1.99326F, 1.87071F, -0.165389F, 2.21488F, 4.91632F, - 13.0395F, 2.54269F, 3.33082F, 2.4303F, 9.77386F, 2.92417F, 2.37895F, - 2.35666F, 2.76362F, 1.95341F, 2.39318F, 2.37859F, 2.30339F, 2.44812F, - 0.868682F, 1.9844F, 1.88241F, 2.52695F, 2.44844F, 2.45743F, 2.07481F, - 2.08192F, 1.23389F, 1.34756F, 1.01227F, 3.2675F, 1.16283F, 2.71589F, - 2.93666F, 3.18251F, 3.28281F, 2.49386F, 3.37514F, 2.52087F, 4.52518F, - 3.15403F, 7.43215F, 8.39406F, 0.876342F, 3.40378F, 3.74917F, 5.38111F, - 2.86984F, 6.79714F, 2.6619F, 1.8861F, 1.78027F, 0.927027F, 2.60814F, - 2.07077F, 2.12268F, 2.87124F, 1.94219F, 1.82645F, 0.593296F, 3.65595F, - 1.92242F, 2.53864F, 4.69278F, 2.17976F, 1.31809F, 6.43325F, 1.32007F, - 2.11055F, 2.16733F, 2.15309F, 2.76167F, 2.4478F, 2.78841F, 0.930153F, - 0.938881F, 2.53246F, 2.49487F, 2.22657F, 4.91137F, 0.950331F, 2.20082F, - 6.25547F, 2.05952F, 2.38112F, 3.09983F, 2.39703F, 6.29483F, 3.69768F, - 1.58749F, 4.00878F, 2.58844F, 2.10073F, 2.00385F, 3.49721F, 0.919744F, - 2.59851F, 1.39561F, 6.50589F, 2.96717F, 3.72025F, 2.53547F, 0.915591F, - 0.920076F, 4.07598F, 7.44973F, 1.90085F, 2.19544F, 1.94917F, 2.53475F, - 2.97254F, 2.0934F, 5.32959F, 2.75639F, 3.08364F, 2.27963F, 2.98622F, - 1.99352F, 4.91722F, 3.99801F, 0.95752F, 3.22176F, 17.8577F, 18.3009F, - 9.75073F, 7.77551F, 14.3216F, 14.92F, 14.0229F, 3.95192F, 1.97267F, 2.61639F, - 1.65541F, 4.5337F, 2.83313F, 0.925979F, 2.32221F, 1.67174F, 2.11162F, - 4.95211F, 0.940099F, 0.948242F, 2.86291F, 2.97256F, 0.948425F, 3.85613F, - 2.92707F, 4.18637F, 2.37044F, 1.65295F, 4.62556F, 2.84096F, 2.27015F, - 1.86088F, 2.99998F, 2.73133F, 3.66864F, 25.2578F, 1.65024F, 3.194F, 6.93305F, - 1.79832F, 23.2587F, 1.98679F, 6.55365F, 3.79002F, 2.19018F, 2.54995F, - 0.856689F, 0.789743F, 0.844804F, 6.50872F, 2.90571F, 4.77652F, 2.68389F, - 3.36621F, 2.54922F, 3.8235F, 2.68422F, 2.56756F, 5.30089F, 2.18853F, - 3.35321F, 5.5692F, 1.86892F, 2.33201F, 2.3092F, 5.88774F, 2.38517F, 2.40017F, - 3.15471F, 3.19196F, 2.52847F, 2.81909F, 4.84367F, 2.8449F, 2.20068F, - 2.95302F, 3.54238F, 1.96984F, 2.45456F, 3.50413F, 4.11733F, 2.36855F, - 5.85934F, 4.00861F, 3.67387F, 4.11029F, 1.66502F, 3.2302F, 4.99563F, - 3.37449F, 3.12866F, 3.84694F, 2.48084F, 5.38479F, 2.67437F, 5.03797F, - 3.31733F, 4.17851F, 5.35855F, 3.89376F, 4.85289F, 2.91635F, 3.61346F, - 2.79773F, 4.24368F, 3.53913F, 2.77875F, 3.20056F, 2.63767F, 2.49769F, - 5.34575F, 2.15723F, 5.08306F, 2.93361F, 2.14033F, 2.23626F, 3.52532F, - 2.8327F, 2.27228F, 2.88174F, 2.2164F, 7.48348F, 5.51205F, 5.25401F, 4.8542F, - 2.23675F, 4.78095F, 2.44335F, 4.258F, 2.44916F, 2.29403F, 2.68407F, 3.17042F, - 2.94593F, 4.37997F, 3.13554F, 4.15045F, 3.31418F, 3.26522F, 4.14013F, - 3.99797F, 6.30728F, 3.1692F, 2.99065F, 1.68145F, 3.99436F, 5.1029F, 5.25705F, - 4.76298F, 4.88007F, 4.96835F, 2.78445F, 5.01459F, 4.75126F, 2.90728F, - 1.73773F, 1.87598F, 5.04987F, 5.06768F, 5.15661F, 4.21151F, 2.19853F, - 5.05539F, 1.77404F, 1.66013F, 5.46251F, 4.36409F, 5.38734F, 5.26584F, - 1.96942F, 2.08545F, 5.00147F, 4.31386F, 3.03829F, 3.25627F, 5.3625F, - 2.10935F, 2.75737F, 5.26413F, 5.35639F, 2.26058F, 4.23336F, 4.57553F, - 1.75545F, 1.9349F, 2.31484F, 5.03153F, 4.1257F, 3.60648F, 2.45393F, 2.7487F, - 2.14444F, 2.5081F, 4.33536F, 2.26889F, 4.81676F, 2.18281F, 2.47306F, - 5.06671F, 4.87234F, 4.64461F, 2.99518F, 3.26872F, 5.07853F, 2.28946F, - 5.48679F, 1.92145F, 4.95621F, 4.44807F, 4.32559F, 4.995F, 4.74841F, 2.31715F, - 3.55616F, 2.60144F, 3.77362F, 1.85024F, 2.31294F, 2.19098F, 5.29014F, - 2.44493F, 3.77292F, 4.87092F, 2.54354F, 4.24799F, 2.60309F, 3.54514F, - 2.65528F, 4.193F, 6.42157F, 4.5473F, 4.64152F, 3.21974F, 7.0186F, 1.74488F, - 5.32573F, 2.37582F, 2.7435F, 2.9691F, 1.61283F, 1.66854F, 3.91271F, 5.79555F, - 7.6205F, 3.24771F, 3.48572F, 1.89916F, 2.69112F, 3.03088F, 3.1662F, 2.35253F, - 2.78608F, 4.34249F, 5.31517F, 1.66568F, 1.93253F, 4.69195F, 6.90205F, - 3.12186F, 2.12283F, 4.80679F, 5.41881F, 3.45665F, 2.98301F, 2.4282F, - 2.15901F, 2.80246F, 2.50135F, 3.63372F, 5.19083F, 7.08061F, 4.32534F, - 5.06826F, 4.89726F, 2.99587F, 1.65767F, 2.2156F, 2.77952F, 2.7581F, 2.94981F, - 1.54536F, 10.5218F, 5.03254F, 5.83377F, 2.12764F, 5.17599F, 4.70726F, - 5.41488F, 5.26536F, 3.53641F, 4.85714F, 2.36533F, 3.10782F, 3.92415F, - 3.84448F, 1.73757F, 2.72628F, 4.39755F, 2.77443F, 4.41447F, 2.47824F, - 2.69356F, 3.43697F, 5.73092F, 3.22214F, 3.74863F, 2.76035F, 3.05131F, - 1.66377F, 5.25942F, 5.08913F, 3.12629F, 2.56918F, 3.05242F, 3.25352F, - 3.24701F, 4.75067F, 5.46631F, 5.33089F, 3.22286F, 3.17003F, 4.01207F, - 1.66665F, 5.14237F, 5.06616F, 5.23402F, 4.06946F, 2.5339F, 2.52367F, - 2.18326F, 3.90624F, 3.72371F, 4.1106F, 1.70278F, 1.65647F, 4.6393F, 2.67679F, - 2.7407F, 2.85069F, 2.43038F, 3.21447F, 2.25866F, 3.76269F, 3.61842F, - 4.25984F, 4.71923F, 5.00398F, 2.3066F, 5.43337F, 2.1817F, 5.41154F, 2.31772F, - 2.94752F, 3.49326F, 5.0805F, 1.72671F, 2.76085F, 2.6119F, 2.22968F, 2.1963F, - 4.36037F, 3.0109F, 6.22985F, 2.8241F, 4.64154F, 3.27478F, 2.25667F, 1.67563F, - 1.67586F, 1.64781F, 1.82468F, 5.05402F, 4.86937F, 5.03605F, 3.88877F, - 3.3678F, 3.56895F, 2.21295F, 1.63104F, 1.65663F, 3.07637F, 4.84123F, - 2.12581F, 1.82304F, 1.91936F, 4.45037F, 3.92385F, 3.12612F, 3.41458F, - 3.14824F, 2.26677F, 2.19406F, 2.61436F, 1.64941F, 3.32868F, 1.64919F, - 1.7226F, 3.73403F, 4.06064F, 5.04998F, 1.96723F, 2.10344F, 2.95566F, - 2.80058F, 2.75638F, 4.34606F, 1.63494F, 4.46593F, 4.97125F, 4.69314F, - 3.83415F, 2.75506F, 2.15099F, 2.94277F, 4.33812F, 4.82584F, 3.34654F, - 1.74523F, 2.20019F, 2.13672F, 3.5204F, 3.26142F, 3.94499F, 3.91598F, - 3.11616F, 3.85955F, 2.69535F, 2.8138F, 2.91225F, 2.14577F, 2.27884F, - 3.80726F, 5.02621F, 1.90295F, 3.73381F, 7.10816F, 2.37983F, 4.30386F, - 3.8112F, 3.40809F, 4.22689F, 4.99676F, 2.77228F, 1.66303F, 3.45082F, - 2.22334F, 2.51449F, 4.77036F, 3.00939F, 1.77846F, 2.4272F, 2.21029F, - 1.80399F, 2.12666F, 2.38639F, 2.16495F, 16.06F, 5.78484F, 3.88555F, 2.21019F, - 4.68007F, 3.33717F, 3.77355F, 2.22332F, 2.38304F, 1.79724F, 5.88569F, - 5.92577F, 2.53401F, 1.84083F, 3.61066F, 3.3754F, 2.51345F, 1.64845F, 4.527F, - 3.63925F, 2.08316F, 2.18063F, 3.66175F, 2.25446F, 2.18873F, 1.86495F, - 2.23665F, 2.23249F, 3.65588F, 3.51021F, 5.28167F, 2.09354F, 3.45754F, - 2.36075F, 1.61569F, 2.24674F, 2.6273F, 4.11615F, 1.76855F, 1.67297F, - 1.77293F, 3.59146F, 2.5591F, 2.34601F, 1.66552F, 3.69078F, 5.04356F, 4.3791F, - 2.93058F, 2.25939F, 2.18151F, 1.66975F, 1.64146F, 4.2815F, 2.27672F, 2.3275F, - 1.76655F, 3.02583F, 5.2805F, 2.97556F, 6.46274F, 1.69574F, 2.8977F, 2.17671F, - 1.75024F, 3.96225F, 3.54427F, 1.84592F, 6.41214F, 2.18265F, 4.14544F, - 2.30874F, 1.67324F, 1.70153F, 1.71931F, 3.39752F, 3.14831F, 1.66858F, - 1.60717F, 5.02615F, 4.23432F, 3.06409F, 2.8916F, 2.27332F, 2.38209F, - 1.71949F, 1.80173F, 2.00194F, 3.34948F, 2.82393F, 2.26621F, 2.19792F, - 6.19703F, 1.71879F, 1.78988F, 2.02814F, 3.76095F, 1.8273F, 1.77593F, - 3.12063F, 2.25724F, 1.84828F, 6.5935F, 2.29866F, 2.19355F, 2.21086F, - 2.86398F, 1.65319F, 1.96899F, 2.23958F, 2.48944F, 1.91593F, 3.97594F, - 3.06191F, 1.33893F, 1.65552F, 2.18582F, 1.61739F, 4.52682F, 1.94883F, - 2.60191F, 3.15375F, 2.11737F, 1.74792F, 2.42547F, 7.81615F, 3.13098F, - 1.82952F, 1.04036F, 1.65701F, 2.63882F, 4.53531F, 2.96911F, 4.28026F, - 2.22554F, 3.83327F, 2.74593F, 2.24631F, 2.13785F, 2.04001F, 5.16047F, - 1.78367F, 2.25764F, 4.0561F, 1.96715F, 1.96272F, 5.19474F, 1.98128F, - 2.02991F, 2.23766F, 2.33169F, 2.29891F, 7.20532F, 2.39713F, 22.6659F, - 3.97144F, 2.48738F, 1.66057F, 4.39253F, 3.71972F, 2.36986F, 1.67204F, - 1.65802F, 3.43101F, 3.35682F, 1.22497F, 1.27073F, 4.9538F, 2.25483F, - 2.21096F, 2.32354F, 1.81076F, 1.92275F, 2.03531F, 1.76811F, 2.17551F, - 2.27965F, 2.19611F, 2.13626F, 2.1772F, 3.5769F, 6.34109F, 7.8008F, 2.325F, - 2.58464F, 2.43113F, 1.65987F, 5.34328F, 5.33462F, 2.24531F, 1.64784F, - 1.72097F, 5.14134F, 2.93284F, 1.28549F, 1.56255F, 4.94585F, 1.29581F, - 2.00862F, 3.16971F, 3.81889F, 2.26965F, 2.40712F, 5.82323F, 3.95439F, - 1.32237F, 2.40689F, 2.19326F, 1.9756F, 4.08179F, 4.23045F, 1.31892F, - 1.29518F, 2.09099F, 1.91665F, 5.92867F, 2.29577F, 1.97603F, 2.05262F, - 2.13642F, 2.1454F, 2.26623F, 2.14522F, 2.2049F, 2.26262F, 1.08833F, 7.54843F, - 2.44889F, 2.1678F, 1.65753F, 2.01755F, 2.93323F, 1.09667F, 1.09309F, - 1.79755F, 1.96016F, 4.96725F, 2.05847F, 5.35103F, 1.28292F, 1.09458F, - 1.61275F, 1.87335F, 1.81614F, 5.53206F, 2.32466F, 2.31742F, 1.84607F, - 1.50239F, 5.21088F, 2.95213F, 2.78474F, 1.18272F, 3.22705F, 3.05654F, - 2.58163F, 1.27707F, 2.10403F, 1.90903F, 1.94984F, 2.20894F, 1.69826F, - 5.08002F, 2.05057F, 2.10941F, 2.10401F, 2.10065F, 2.04722F, 2.19434F, - 2.16106F, 2.27481F, 1.66122F, 1.62219F, 3.11901F, 2.97146F, 2.07487F, - 1.66022F, 4.79727F, 5.64952F, 1.31237F, 1.18391F, 1.0304F, 1.65804F, 1.2818F, - 1.29119F, 1.30138F, 1.12599F, 1.08818F, 1.65173F, 1.62668F, 4.98187F, - 1.31147F, 1.31515F, 5.20391F, 2.76191F, 1.2894F, 1.10412F, 1.71052F, - 2.12557F, 3.2583F, 1.85067F, 2.27984F, 1.29121F, 2.24907F, 2.06076F, - 2.09173F, 2.0332F, 2.1067F, 2.17748F, 2.10077F, 2.22455F, 4.4197F, 2.25919F, - 6.54138F, 2.33532F, 4.29076F, 2.72764F, 4.13588F, 4.75332F, 2.27417F, - 4.59705F, 4.29348F, 1.28851F, 2.97062F, 1.32415F, 1.3018F, 2.04767F, - 2.62883F, 2.25805F, 4.15829F, 2.0227F, 1.87855F, 1.22924F, 1.26897F, - 1.29632F, 1.2573F, 1.68879F, 1.08755F, 1.76116F, 2.54526F, 1.74107F, 2.1008F, - 2.07257F, 1.46096F, 3.03897F, 2.55599F, 1.29899F, 1.88614F, 1.18593F, - 1.04924F, 2.02098F, 3.68713F, 2.10451F, 2.07682F, 2.10749F, 6.02599F, - 6.06729F, 5.00809F, 5.46155F, 2.31329F, 2.53372F, 1.29839F, 2.28966F, - 2.67455F, 1.76609F, 1.82345F, 4.94588F, 0.912888F, 0.911269F, 5.34665F, - 1.30341F, 1.34202F, 2.18894F, 1.70418F, 3.43084F, 1.02667F, 2.98495F, - 2.8456F, 6.55835F, 1.22587F, 5.04914F, 2.08768F, 1.99523F, 2.09937F, - 1.92267F, 1.87614F, 2.02976F, 2.01546F, 7.21039F, 4.2139F, 2.0184F, 9.27539F, - 1.93179F, 4.36047F, 8.06712F, 3.13155F, 2.08884F, 2.01464F, 4.61993F, - 4.82281F, 4.5512F, 1.65807F, 1.89542F, 1.89436F, 1.02175F, 0.937168F, - 2.04827F, 2.26807F, 2.07052F, 7.67695F, 1.25215F, 1.01034F, 2.47761F, - 34.1646F, 1.00232F, 1.71063F, 1.31423F, 1.78205F, 0.963115F, 1.93537F, - 1.66898F, 3.1736F, 2.8009F, 0.887085F, 4.25185F, 5.12407F, 1.85698F, - 0.927209F, 1.32607F, 4.50718F, 2.48285F, 1.75306F, 2.2098F, 2.47454F, - 5.96499F, 20.1166F, 2.09548F, 5.21916F, 8.19078F, 9.82286F, 4.95371F, - 8.34782F, 2.04998F, 0.849525F, 1.0346F, 1.63484F, 1.6802F, 5.59009F, - 0.919817F, 1.99913F, 5.58412F, 21.7253F, 1.805F, 1.3078F, 10.7926F, 1.6561F, - 2.60054F, 1.74828F, 1.28833F, 2.63439F, 5.19707F, 5.05388F, 5.30302F, - 0.894313F, 0.936761F, 0.928432F, 2.1949F, 2.7148F, 0.858255F, 1.29992F, - 3.28342F, 1.79761F, 3.09884F, 12.8181F, 1.87771F, 1.45888F, 7.88258F, - 11.5499F, 3.86091F, 15.0174F, 2.65564F, 10.1782F, 4.26292F, 1.06381F, - 0.851983F, 4.62842F, 2.70466F, 1.32944F, 1.0707F, 0.876094F, 6.4717F, - 2.35445F, 1.99883F, 1.86952F, 1.30141F, 1.19128F, 1.71173F, 1.73938F, - 3.97554F, 3.18333F, 1.95594F, 3.10826F, 1.70999F, 0.900302F, 12.4895F, - 4.22024F, 2.05009F, 5.18877F, 3.66882F, 12.5922F, 3.02139F, 1.12453F, - 5.57775F, 1.34358F, 2.46563F, 0.887004F, 1.61348F, 1.33721F, 0.870958F, - 1.88125F, 6.28196F, 5.39235F, 1.51269F, 1.1378F, 1.60425F, 9.8782F, 0.92646F, - 0.94121F, 1.82249F, 2.0629F, 3.61018F, 1.89347F, 0.959454F, 1.02162F, - 2.86047F, 3.93163F, 3.88827F, 2.37679F, 2.69511F, 3.12992F, 5.38857F, - 1.77312F, 1.34459F, 2.27851F, 4.29878F, 2.43276F, 31.2875F, 3.91816F, - 5.33844F, 1.13744F, 0.906546F, 0.606953F, 1.66115F, 1.03232F, 0.893424F, - 1.65065F, 1.32713F, 0.899109F, 0.922336F, 0.621553F, 1.27735F, 1.06826F, - 0.943404F, 1.64911F, 5.31725F, 3.85546F, 1.96162F, 2.14739F, 1.38319F, - 2.20473F, 5.98356F, 6.22916F, 4.92424F, 1.66483F, 3.3541F, 3.95651F, - 2.90848F, 0.834949F, 1.13856F, 1.2951F, 1.03117F, 2.01729F, 1.11422F, - 1.07173F, 0.917648F, 0.37111F, 0.465713F, 0.617688F, 0.703311F, 1.77269F, - 1.84557F, 1.01821F, 3.13251F, 2.8587F, 1.10859F, 0.903827F, 2.1935F, - 3.16113F, 1.20136F, 0.940002F, 0.452031F, 5.14925F, 0.877398F, 0.964966F, - 0.974923F, 1.13012F, 0.458606F, 1.59891F, 21.797F, 1.4254F, 5.1845F, - 0.86717F, 1.81708F, 2.72053F, 3.22016F, 2.32673F, 3.90172F, 5.61969F, - 16.136F, 5.37475F, 3.5908F, 0.823444F, 1.35072F, 1.36813F, 0.612129F, - 0.895355F, 5.12427F, 1.93809F, 0.959381F, 1.0628F, 0.972996F, 0.88766F, - 1.0218F, 0.908501F, 0.856986F, 1.86305F, 1.20393F, 0.604688F, 1.35395F, - 1.02822F, 1.82753F, 2.41526F, 1.12188F, 1.03038F, 1.64032F, 3.58224F, - 2.10723F, 1.34783F, 2.72391F, 4.56054F, 6.21761F, 13.783F, 1.35714F, - 0.415904F, 2.39143F, 1.91475F, 0.862255F, 1.01715F, 1.354F, 0.47F, 1.83473F, - 1.9422F, 0.920247F, 0.950411F, 5.40847F, 0.824415F, 0.832241F, 0.66613F, - 0.642889F, 1.9523F, 1.77853F, 1.70723F, 3.14361F, 2.79555F, 1.11325F, - 3.16685F, 0.897205F, 0.927628F, 0.935385F, 0.454428F, 1.9532F, 11.8096F, - 1.45427F, 1.58416F, 4.84958F, 2.1242F, 2.49145F, 1.17572F, 4.69777F, - 1.26052F, 1.28725F, 1.25273F, 1.27194F, 1.22176F, 1.28133F, 1.20341F, - 1.3141F, 0.914788F, 2.55911F, 0.971745F, 1.27093F, 1.38135F, 1.35703F, - 1.45998F, 1.31374F, 1.04067F, 0.758762F, 0.50217F, 1.51704F, 1.52727F, - 0.835608F, 4.09227F, 1.862F, 3.93785F, 0.965482F, 2.56778F, 5.44666F, - 1.14175F, 1.6794F, 5.7015F, 0.497394F, 3.61688F, 3.75196F, 3.50768F, - 1.21708F, 1.95203F, 1.43892F, 1.30765F, 1.20469F, 1.23044F, 1.19596F, - 0.804118F, 0.910478F, -0.869721F, 5.01203F, 1.01383F, 1.28445F, 1.38292F, - 1.72827F, 0.837269F, 0.386092F, 0.869509F, 1.02989F, 1.38243F, 2.58251F, - 0.965995F, 0.993676F, 1.21252F, 1.57616F, 1.0089F, 0.964794F, 1.5955F, - 1.68526F, 4.60338F, 2.56098F, 1.52916F, 2.07437F, 1.8014F, 1.12784F, - 3.87724F, 4.40897F, 1.1308F, 8.32549F, 1.70549F, 1.17149F, 2.30556F, 3.8108F, - 2.6482F, 6.41771F, 3.24502F, 2.23391F, 2.76965F, 1.85319F, 6.56783F, - 0.853737F, 3.03829F, 6.11606F, 1.21708F, 1.33294F, 2.26922F, 1.20084F, - 1.0874F, 0.507535F, 1.87568F, 5.00193F, 0.880479F, 0.438717F, 1.15783F, - 1.74186F, 0.981198F, 1.65689F, 0.371797F, 4.12547F, 1.79514F, 0.425421F, - 1.00644F, 1.83725F, 1.8965F, 1.94171F, 1.61687F, 3.35125F, 1.78979F, - 3.01711F, 8.46583F, 1.08722F, 1.6945F, 3.93952F, 5.44484F, 2.41933F, - 3.58168F, 1.25399F, 1.27165F, 0.911584F, 1.15014F, 1.23116F, 0.377026F, - 0.478148F, 0.946994F, 1.32802F, 0.49419F, 0.504506F, 0.866717F, 1.85747F, - 0.985738F, 1.01687F, 3.61779F, 2.06474F, 0.437876F, 7.65684F, 2.54646F, - 9.80792F, 2.07064F, 5.38887F, 4.67248F, 1.24514F, 3.14298F, 2.47098F, - 0.631288F, 3.09921F, 0.947651F, 2.28833F, 0.704169F, 1.30403F, 1.16504F, - 1.29878F, 1.11874F, 1.07967F, 1.16869F, 1.26545F, 1.21608F, 1.21522F, - 1.17608F, 1.24794F, 6.16587F, 1.2432F, 1.70789F, 0.899323F, 0.470053F, - 0.42754F, 1.68096F, 1.6597F, 0.958528F, 1.3536F, 0.374373F, 1.18099F, - 0.956742F, 1.79057F, 0.879338F, 4.84588F, 0.929177F, 1.02302F, 1.50272F, - 1.96093F, 0.961436F, 1.3024F, 1.73731F, 2.42101F, 1.81917F, 3.05624F, - 3.71007F, 4.74531F, 4.67866F, 1.49649F, 2.19806F, 2.18377F, 2.81073F, - 2.94523F, 4.02202F, 2.18643F, 0.828202F, 6.53283F, 1.03433F, 2.26986F, - 3.58821F, 3.20532F, 2.56397F, 4.81535F, 3.1324F, 3.15385F, 0.368369F, - 2.52601F, 0.581132F, 1.98212F, 0.916513F, 0.795606F, 1.03949F, 2.17177F, - 1.6704F, 2.46077F, 0.934389F, 1.02069F, 2.11401F, 3.90645F, 2.33405F, - 3.40364F, 3.87646F, 3.74751F, 7.6998F, 1.01509F, 0.849303F, 0.367713F, - 0.482495F, 0.609483F, 0.868134F, 0.926824F, 4.91963F, 0.909323F, 2.98862F, - 0.892118F, 0.897017F, 2.94993F, 2.41483F, 7.14743F, 1.99707F, 5.16566F, - 1.70405F, 1.90219F, 0.975077F, 0.994443F, 3.83658F, 1.00474F, 1.16478F, - 1.23715F, 1.2652F, 3.96414F, 1.19201F, 1.84166F, 0.981712F, 0.970848F, - 0.819604F, 1.0235F, 3.4365F, 3.25613F, 4.67083F, 1.34883F, 2.37459F, - 2.06572F, 2.0646F, 2.74385F, 2.98925F, 2.56461F, 3.72238F, 3.77107F, - 2.42767F, 3.97852F, 3.1084F, 6.74892F, 2.90575F, 1.83117F, 1.7294F, 5.66865F, - 3.08176F, 0.920207F, 1.27893F, 0.771793F, 2.51153F, 2.44684F, 0.796534F, - 0.864284F, 2.09819F, 2.10305F, 2.08356F, 1.55743F, 0.879379F, 4.77424F, - 7.27427F, 0.852319F, 3.90512F, 2.75825F, 1.8452F, 3.16647F, 0.993707F, - 0.917568F, 2.43085F, 1.03929F, 4.40514F, 3.05365F, 0.847703F, 2.93272F, - 5.68086F, 2.2171F, 0.440534F, 0.601272F, 0.648351F, 0.77193F, 0.827754F, - 0.801791F, 0.653691F, 0.779327F, 2.18531F, 5.00361F, 2.56256F, 2.5044F, - 6.16266F, 0.648716F, 0.704636F, 0.692393F, 2.23423F, 2.2754F, 0.916785F, - 1.81591F, 3.81218F, 1.24785F, 0.541075F, 0.73399F, 2.14968F, 4.14284F, - 2.70521F, 1.78822F, 0.933106F, 1.26243F, 0.961555F, 1.30182F, 2.6054F, - 2.42614F, 1.85417F, 2.62066F, 1.93295F, 1.96824F, 0.997358F, 2.31485F, - 2.2797F, 2.44558F, 2.13284F, 2.77794F, 2.31633F, 2.12607F, 2.23911F, - 0.956449F, 0.970304F, 0.542047F, 0.929618F, 0.79729F, 2.55592F, 0.906643F, - 4.62007F, 0.504594F, 0.703333F, 0.906577F, 0.753935F, 1.16187F, 2.26743F, - 0.916653F, 2.15424F, 2.30393F, 0.816888F, 1.25555F, 2.4381F, 2.40313F, - 0.898902F, 2.2412F, 1.60512F, 2.14461F, 1.06292F, 1.50093F, 2.4732F, 2.0315F, - 5.36066F, 6.06084F, 1.93127F, 0.971847F, 1.24402F, 2.37326F, 2.22311F, - 2.62946F, 1.15639F, 3.5845F, 2.37404F, 2.15442F, 5.65724F, 3.8873F, 2.50504F, - 3.63662F, 1.82161F, 1.68936F, 2.71051F, 2.70748F, 2.01298F, 2.21265F, - 0.305257F, 0.685632F, 0.615335F, 1.65111F, 3.86958F, 4.23029F, 3.38448F, - 2.44304F, 2.42285F, 2.33323F, 2.34792F, 0.889794F, 0.995415F, 0.641799F, - 0.738596F, 1.60455F, 0.765133F, 3.49842F, 2.75742F, 2.90493F, 3.08751F, - 2.7332F, 2.90841F, 1.71525F, 2.38297F, 2.72878F, 1.46163F, 1.73956F, - 0.880417F, 1.33656F, 2.19647F, 1.43636F, 3.58172F, 2.64574F, 2.50531F, - 1.48373F, 1.76215F, 0.894388F, 2.74924F, 2.23968F, 2.73244F, -2.3568F, - 1.6012F, 2.30324F, 3.46245F, 1.95673F, 3.26173F, 1.99816F, 2.26928F, - 3.36634F, 2.31411F, 2.62411F, 0.722298F, 4.46792F, 2.24374F, 2.7657F, - 2.21835F, 1.2506F, 2.57888F, 3.73877F, 0.660382F, 0.722027F, 0.684506F, - 0.898147F, 0.808557F, 2.62016F, 3.01041F, 2.39019F, 2.47335F, 3.90642F, - 0.715921F, 0.812906F, 2.77454F, 2.31896F, 2.85055F, 0.757213F, 3.21314F, - 1.48692F, 2.15504F, 2.67553F, 3.55913F, 2.02046F, 2.27549F, 0.871018F, - 2.66312F, 1.07698F, 1.18451F, 2.81465F, 3.48382F, 1.25559F, 2.7526F, - 2.95763F, 3.45659F, 4.14813F, 2.02552F, 1.09949F, 1.592F, 7.49662F, 1.97494F, - 1.0371F, 1.23907F, 2.73048F, 2.90633F, 2.54492F, 2.44785F, 0.895309F, - 0.8884F, 1.33443F, 2.55235F, 3.76464F, 5.13735F, 2.78437F, 3.04032F, - 1.54532F, 2.9437F, 0.906667F, 0.188481F, 1.99797F, 4.42347F, 3.02541F, - 4.01229F, 1.67766F, 1.16078F, 2.03099F, 1.2116F, 3.08067F, 2.39555F, - 4.16808F, 2.16821F, 5.12428F, 3.5071F, 3.86615F, 1.0002F, 5.41533F, 2.4825F, - 4.71853F, 3.97969F, 3.06596F, 0.858406F, 0.76346F, 0.908832F, 0.710107F, - 2.70351F, 2.88376F, 3.39474F, 2.54179F, 3.91959F, 0.706786F, 2.0386F, - 3.43321F, 3.19957F, 2.30284F, 9.98939F, 2.70522F, 2.85344F, 3.13411F, - 0.989218F, 1.15486F, 2.51416F, 2.93058F, 2.33472F, 1.71509F, 1.16403F, - 2.39933F, 2.84882F, 1.9654F, 2.21494F, 1.25302F, 2.86676F, 2.01055F, - 5.60441F, 0.830005F, 3.89843F, 4.13481F, 0.871394F, 0.833841F, 0.954191F, - 1.65134F, 2.05859F, 2.04764F, 2.69219F, 15.7614F, 3.55439F, 2.58075F, - 6.95998F, 0.941874F, 1.08338F, 0.982054F, 0.981785F, 0.984591F, 0.937703F, - 0.853609F, 1.14349F, 4.26015F, 0.983743F, 0.994958F, 0.890723F, 1.06341F, - 1.99608F, 2.04196F, 2.76199F, 2.07001F, 0.890124F, 0.879058F, 0.906512F, - 1.66627F, 1.01518F, 0.781906F, 2.02963F, 0.459886F, 2.18885F, 2.29468F, - 3.54069F, 1.80915F, 1.23133F, 0.897124F, 1.99601F, 1.17691F, 12.5775F, - 1.21783F, 4.45259F, 4.52816F, 2.66881F, 0.577012F, 1.06932F, 0.84392F, - 2.48333F, 0.906399F, 2.17492F, 2.11537F, 2.09651F, 2.17074F, 4.45233F, - 2.12914F, 1.99042F, 4.9328F, 3.35714F, 0.87221F, 4.77814F, 0.864589F, - 0.940005F, 0.945264F, 2.05822F, 3.22375F, 0.946818F, 0.855489F, 2.09248F, - 0.399084F, 1.51252F, 2.18357F, 3.24375F, 2.06438F, 2.7502F, 2.01213F, - 2.06075F, 1.89691F, 3.98053F, 0.974001F, 2.1786F, 2.92098F, 3.22454F, - 2.43249F, 2.66567F, 2.44299F, 5.10583F, 2.36903F, 1.77917F, 4.12664F, - 2.06583F, 2.13876F, 1.86169F, 0.589583F, 2.25324F, 0.936534F, 2.29514F, - 1.06493F, 2.7614F, 1.18004F, 0.952375F, 2.26587F, 1.09806F, 2.6033F, - 1.23941F, 1.72593F, 2.15989F, 1.24814F, 3.0468F, 2.9667F, 5.27096F, 4.00193F, - 3.00728F, 2.5653F, 2.58393F, 22.9054F, 2.72889F, 2.59147F, 3.33543F, - 2.42703F, 2.5734F, 2.70571F, 2.4498F, 2.55172F, 2.37068F, 2.76259F, 2.3212F, - 0.584597F, 2.21821F, 2.14515F, 0.858384F, 0.985825F, 0.401742F, 1.25998F, - 1.5252F, 1.29929F, 0.724109F, 0.752244F, 0.879109F, 0.820667F, 1.12351F, - 0.777897F, 1.89649F, 1.85951F, 2.52698F, 1.99041F, 2.46903F, 2.52175F, - 0.979894F, 0.971753F, 2.35178F, 0.162009F, 1.6435F, 5.61435F, 0.976696F, - 1.09351F, 1.03768F, 1.44865F, 0.958177F, 1.87373F, 2.12858F, 3.04174F, - 6.2818F, 0.900691F, 6.07736F, 0.384642F, 1.4934F, 0.705246F, 0.990645F, - 1.01091F, 2.06032F, 0.821255F, 1.25106F, 2.34385F, 1.22546F, 0.882351F, - 1.28539F, 1.79195F, 9.2983F, 4.24399F, 1.02783F, -1.08883F, -1.12094F, - 1.12761F, -1.1708F, -0.961676F, 1.27031F, 0.930019F, 0.941369F, 0.656791F, - 1.14546F, 0.662617F, 0.944645F, 1.26034F, 0.715325F, 1.03191F, 1.53086F, - -1.18102F, -1.311F, -0.675393F, -2.19503F, 0.791133F, -0.527696F, -0.928341F, - -0.671852F, -1.19158F, -0.678815F, 1.14088F, -0.962616F, -1.37915F, - -1.13259F, 1.24909F, -1.24375F, -0.615117F, -1.21736F, -0.853206F, -1.36102F, - -2.12429F, -1.14126F, -0.874701F, -1.11793F, 0.371345F, 0.359989F, - -0.906192F, 1.23692F, -3.82364F, -0.650538F, -0.763966F, -1.07657F, - 0.801021F, 0.245686F, -1.02787F, 1.8691F, -1.12179F, -1.11809F, 0.911927F, - 1.10886F, -2.11089F, -1.57161F, -1.65903F, 1.38225F, 1.20414F, 0.824738F, - -1.12108F, -2.02436F, -1.36442F, -1.21887F, -1.15922F, 0.900214F, -1.5885F, - 1.09346F, -0.698931F, 1.12895F, 0.880588F, 1.16275F, 1.06925F, 0.979468F, - 0.742184F, 0.408841F, 0.776874F, 0.242323F, 2.53514F, -1.17341F, 1.0279F, - 0.851633F, 1.07923F, 1.07722F, -1.22991F, 1.14976F, 1.19272F, -1.23486F, - -1.38762F, 1.19994F, -1.22962F, -1.868F, -1.88496F, -1.87605F, -2.04883F, - -2.06101F, -1.30942F, 0.877436F, 1.31334F, 0.948319F, 1.13612F, -0.96261F, - 0.377427F, 0.541013F, 0.208524F, -1.43637F, 0.374375F, -1.19085F, -1.21447F, - -2.13386F, -1.14223F, -2.08644F, -1.14571F, 0.872576F, 0.931533F, 0.425799F, - -1.07269F, -1.37741F, -1.11521F, -2.08955F, 1.09925F, -1.28148F, 1.13707F, - -1.38632F, -1.27031F, 1.13513F, -1.19232F, 0.16767F, -1.17532F, -1.0758F, - -1.07169F, 0.941718F, 1.59154F, 1.23855F, -2.10609F, 0.449706F, 0.968628F, - 0.852846F, 0.914383F, 0.89671F, 0.827258F, 1.40829F, 0.499799F, 1.07533F, - 0.16077F, -0.956875F, -2.07029F, -2.07953F, -1.15639F, -2.04082F, 0.918805F, - 0.88888F, 0.267136F, -0.976624F, 1.12981F, -1.19551F, -1.23134F, -1.25819F, - 1.02504F, 0.169297F, 0.998187F, -1.24258F, 0.896699F, -1.06495F, 1.17479F, - 1.2449F, 0.587191F, 0.389499F, 0.281248F, 1.05796F, -1.33111F, -1.60668F, - -1.22071F, 0.430927F, 1.70117F, 1.299F, 0.417693F, 0.640835F, 0.911573F, - 1.30275F, 1.28334F, 0.362745F, 0.477198F, 1.13186F, 1.07959F, 0.996755F, - 1.11549F, -1.17186F, -1.19415F, -1.22738F, -1.14781F, -1.04884F, -1.22114F, - -1.163F, -1.09138F, -2.10968F, -1.55705F, 1.03573F, 1.31171F, 1.32982F, - 0.246822F, 0.501326F, 1.14265F, -1.30407F, -1.2031F, -2.12084F, -1.11552F, - 0.64586F, 0.165275F, 1.16993F, 1.1616F, 0.889478F, 0.256274F, -1.45142F, - -1.16367F, -1.13051F, -2.21938F, -1.28706F, -0.921787F, -1.17938F, 1.71233F, - 0.652797F, -1.83683F, -1.228F, 0.675895F, -0.630967F, 1.04018F, 0.252438F, - 0.953F, 0.183764F, 1.87274F, 0.262344F, -1.08371F, -1.21338F, -1.26055F, - 1.17526F, -1.099F, -1.18384F, 0.858948F, 0.918584F, 0.916351F, 0.978149F, - 0.396753F, 0.905758F, -1.55111F, -1.1994F, 0.683641F, 0.88477F, -1.19983F, - -1.90973F, -1.51484F, 0.911599F, 0.35258F, 0.321606F, 12.443F, 0.235814F, - -1.20597F, -1.68489F, -1.30243F, -1.19226F, -1.19093F, 1.07885F, 0.904523F, - -1.20165F, 0.905456F, -2.03665F, 0.854895F, 0.895983F, 0.917692F, 0.951583F, - 0.315415F, 0.899281F, 0.894139F, -1.134F, -1.60228F, 0.399497F, 0.587329F, - -3.4038F, -1.27955F, -1.21748F, -2.09331F, 0.88504F, -1.23851F, -1.52156F, - -1.56697F, -2.34518F, 0.581039F, 0.168588F, -2.11134F, -1.25676F, 0.657572F, - 1.36647F, 0.664086F, -1.43019F, 0.436533F, 0.941885F, 0.909473F, 0.916429F, - 0.931148F, 0.471184F, 0.47834F, 0.144069F, 0.358651F, 0.411479F, 0.893438F, - 0.456584F, 0.908687F, 0.965802F, 0.473738F, 0.135643F, -1.77028F, 1.58227F, - 0.418469F, 0.955704F, -1.23317F, 0.715075F, 0.133439F, 0.907633F, 0.939579F, - 0.502692F, 0.143543F, 0.210071F, -1.15664F, -1.1495F, 0.591519F, -1.13382F, - -1.53422F, 1.64924F, 1.0781F, 0.936108F, 0.146329F, -1.16834F, 0.926892F, - -2.18741F, -1.37902F, 0.91384F, 0.987691F, 0.670294F, -1.32113F, 0.886373F, - -1.25719F, -0.00706801F, 1.32542F, 1.48668F, -1.21338F, -1.21933F, -1.2804F, - -2.18601F, -1.63414F, 0.894357F, -1.94341F, -1.50863F, 1.04257F, 1.32395F, - 1.22079F, 1.24385F, 1.05543F, 1.2204F, 1.84917F, 1.25667F, 1.13083F, - 1.07825F, 1.03717F, 1.1197F, 1.29706F, -1.28952F, -1.19023F, 1.0012F, - 0.453578F, 1.15873F, -1.43586F, 0.994986F, 1.07396F, -1.19701F, 0.872289F, - 0.702F, -1.21519F, -1.91728F, 1.25226F, 1.32248F, 0.901891F, 0.916083F, - 0.713951F, -2.23235F, 1.35559F, 1.21851F, 0.946289F, -2.19074F, 0.38332F, - 1.53434F, 1.306F, 0.93767F, 0.747851F, 1.29727F, 1.07112F, -0.683319F, - 1.40755F, 0.25321F, 1.07782F, 0.896754F, 1.68258F, 1.06573F, 0.883255F, - 0.167743F, -1.14673F, -1.54293F, 1.0793F, 1.16281F, 1.24305F, 1.07441F, - -1.3008F, -1.7237F, -1.54357F, 1.14484F, 1.58713F, 1.06291F, -1.06977F, - 0.98787F, 1.20655F, 0.684023F, -1.16394F, -1.31983F, 1.65581F, -1.176F, - 1.29681F, 1.30347F, 0.827582F, 1.03592F, 0.932552F, 0.667535F, 1.04847F, - 0.821583F, -1.1677F, 0.60694F, -1.26689F, 1.65932F, 1.65739F, -1.44718F, - -1.99076F, 1.04578F, 0.91427F, -2.26912F, 1.27816F, 1.155F, -1.67128F, - -1.56735F, 0.408466F, 0.676617F, -1.30614F, -3.35925F, -1.22484F, -1.25483F, - 0.592572F, -1.17816F, -1.38871F, 1.64555F, 0.93816F, 0.889758F, 1.08794F, - -1.23951F, -1.30344F, -2.31521F, 0.39674F, 1.28651F, 0.982144F, 1.02399F, - 0.589772F, 1.32827F, 1.27188F, 1.02626F, -1.27461F, 1.1617F, -0.0353421F, - 0.988079F, -1.634F, -6.39448F, -2.25205F, -4.01507F, 0.609457F, -7.12533F, - -2.23643F, 0.052304F, -1.02921F, 1.61709F, 1.0655F, 1.02919F, 0.649895F, - 1.63488F, 1.32234F, 1.02188F, 1.06647F, 1.05188F, -1.12257F, 1.66237F, - -1.06364F, -1.9318F, 0.370379F, 1.27401F, 1.05721F, 1.07869F, 0.595357F, - -1.34777F, 1.32039F, 1.50908F, 1.05518F, 1.21F, 0.909014F, 0.90312F, - 0.59336F, 1.37942F, 0.909446F, 1.29256F, 1.03937F, 1.19317F, 1.32642F, - 1.21245F, 0.902974F, 0.900965F, 0.790175F, 1.07922F, 1.02237F, 1.10883F, - -1.76528F, -1.14011F, -1.21958F, -1.24821F, -1.14712F, 0.598512F, 0.65905F, - 0.608429F, -1.30086F, -1.14641F, 0.905993F, 0.909945F, 1.287F, 1.21483F, - -1.21059F, 0.616628F, 0.431297F, -1.36096F, -1.12341F, -1.22465F, -1.20945F, - 0.625693F, 0.639267F, 0.40253F, -1.202F, -1.23572F, 0.641013F, 0.161256F, - 1.08164F, -1.1503F, -1.18699F, -1.19058F, -1.29764F, 0.615972F, 0.5937F, - -1.12183F, 0.614949F, -1.12118F, -1.1134F, -1.20992F, -1.2607F, -1.11093F, - -1.16743F, -1.43461F, -29.9404F, -31.7065F, -1.47532F, 0.0613903F, 0.620343F, - -1.46726F, -1.37074F, -1.19863F, -1.23484F, 0.012053F, -1.26478F, 1.54618F, - 0.790146F, 0.424671F, 0.621647F, -1.20642F, 0.462634F, 0.419073F, -1.24204F, - 0.803051F, -1.15299F, -1.24261F, 0.0737276F, 0.0441383F, -1.3327F, 0.382532F, - 0.132327F, 0.805337F, 0.944179F, -1.24609F, 1.04003F, 0.711491F, 1.30862F, - -0.0522921F, 0.783499F, 0.625942F, -1.16661F, -2.25348F, -12.5307F, - -8.81254F, -8.71247F, -9.69338F, -11.5119F, 0.0725644F, 0.4165F, -1.71416F, - 0.423845F, 0.615704F, 0.451583F, -1.81055F, -1.50488F, -1.51521F, 0.612657F, - 0.59502F, 0.621504F, -1.76832F, -1.363F, -1.55233F, -1.2181F, -1.50341F, - 0.387085F, -1.58259F, 0.174815F, 0.610964F, 1.06788F, 0.278487F, 0.169427F, - 0.637834F, 0.594764F, 0.170938F, 0.119828F, 0.754306F, 0.629062F, 1.07948F, - 0.169116F, 0.788259F, -0.729988F, 0.162624F, 0.619436F, 0.907184F, - -0.0163545F, -2.36464F, 0.429201F, 1.02543F, -1.161F, 0.0506958F, -1.4776F, - -1.73101F, 0.938423F, -0.820989F, 0.11897F, 0.519695F, 0.552759F, 1.0109F, - 1.07807F, 0.256204F, 0.0142833F, -5.44207F, -1.80329F, -0.873884F, -4.63612F, - -5.92659F, -1.54572F, -0.532432F, -1.7713F, 0.0151867F, 0.713059F, 0.759421F, - 1.0135F, -0.523688F, -0.447455F, -0.911572F, -0.882638F, -1.18475F, - -2.77875F, -1.01853F, 0.119966F, -1.51821F, 0.614332F, -0.593545F, 0.126015F, - 0.119831F, 0.099151F, -0.00567992F, 0.717564F, -0.637765F, 0.108522F, - 0.667741F, 0.363047F, 1.07696F, -1.44627F, -1.73826F, -1.56171F, 0.464037F, - 0.010491F, -2.00677F, 0.131625F, -1.76667F, 0.121941F, -0.863222F, -1.28828F, - -0.889045F, -6.09771F, -5.91266F, -1.93859F, -1.79661F, -1.9343F, 0.693301F, - -6.02313F, -5.98479F, -1.89748F, -1.86325F, -2.01231F, -1.79185F, 0.0403527F, - -0.580298F, -1.44286F, -1.89452F, 0.486044F, 0.646334F, 0.591757F, 0.884032F, - -1.01741F, 0.633984F, -0.807581F, -1.37903F, -1.33303F, -0.58238F, -4.50124F, - -2.07684F, 0.65341F, -0.961444F, -0.812381F, -0.883697F, -4.23901F, 0.59235F, - -0.385341F, -0.829392F, -0.849603F, -0.746979F, -0.780269F, -2.02023F, - -1.9735F, -1.90128F, 0.473951F, 0.00965456F, -0.76751F, -0.748468F, - -0.343626F, -0.767976F, 0.175586F, 0.164269F, -0.817611F, -0.857289F, - -0.937352F, -0.815427F, -0.847465F, -0.789462F, -0.96781F, -0.849592F, - -0.693331F, 0.424737F, -0.705047F, -0.407482F, -1.17831F, 1.41226F, - -2.10977F, -2.30176F, -0.906922F, -0.827178F, -0.8279F, -0.482686F, -2.0449F, - 0.469005F, -0.116554F, -0.997933F, -0.822499F, -0.838689F, -0.821609F, - -0.831223F, -0.823787F, 0.159876F, -0.910944F, -0.92166F, -0.95307F, - -0.84983F, -0.471617F, -0.825309F, -0.846848F, -0.976838F, -0.93043F, - -0.866504F, -0.894813F, -0.809349F, -2.19401F, 0.61551F, -1.3041F, -0.11502F, - -0.65481F, -0.815475F, -0.808678F, -0.933046F, -2.38502F, -2.0279F, 2.39466F, - 0.585217F, -0.656634F, -0.808334F, -1.41721F, -0.808625F, -0.775516F, - -0.925769F, -0.717033F, -0.869496F, -0.472964F, -0.843711F, 0.44402F, - -0.941387F, -0.858894F, -0.34342F, -0.787342F, -0.810703F, -0.946456F, - -1.91194F, 0.793488F, -2.03286F, 0.751605F, 1.27415F, 1.31742F, 1.23305F, - -2.51587F, 1.29393F, 1.13973F, 1.29479F, 1.26073F, 1.33357F, 1.22901F, - 1.13439F, 1.15449F, -1.36666F, -1.12266F, -1.05794F, -1.83545F, 1.41551F, - 1.77803F, 1.25171F, -1.24548F, -0.863757F, -0.414812F, -1.34462F, -0.142916F, - -0.84355F, -0.808906F, -1.07986F, -0.937393F, -0.949708F, -0.918631F, - -0.863032F, 2.56472F, -0.784763F, -0.855338F, -0.678025F, -0.84398F, - -0.802893F, -0.841441F, -0.826076F, -0.803988F, -0.815383F, -0.797193F, - -0.910747F, -0.8598F, 0.201167F, -0.655066F, 1.19839F, 1.21471F, 1.22304F, - 1.67682F, 0.966205F, 5.40995F, -0.956272F, 1.28779F, 1.24549F, 0.827652F, - -0.862455F, -0.804601F, -0.853902F, -0.112104F, 1.07944F, 1.22248F, - 0.392724F, -0.960902F, -0.88088F, -1.12873F, -1.13197F, -0.96762F, - -0.00394696F, -0.0482301F, -1.05966F, 1.68746F, -0.992277F, -0.0739016F, - 1.45539F, -1.20274F, 0.0637726F, -0.844829F, -0.886266F, -0.915907F, - -1.00456F, 0.41079F, 0.0391919F, -0.903724F, -0.877862F, -0.792097F, - 0.60518F, 0.662657F, -0.834298F, 1.20798F, 1.14352F, -2.02835F, 1.27926F, - 1.2948F, 1.6921F, -2.0922F, 1.13532F, -2.05704F, 1.22095F, -0.67231F, - -1.07773F, 1.26116F, 1.25925F, -2.0506F, -2.05345F, -2.07396F, 0.836058F, - 0.474065F, -1.57865F, 0.857787F, 0.866043F, 0.524201F, -0.903757F, 0.811193F, - -0.995782F, -0.972874F, -1.12878F, -1.29654F, -0.922914F, -0.678909F, - -0.860993F, -1.14496F, 1.48436F, -0.798233F, -1.19082F, -0.878928F, - -0.873509F, -0.664407F, -0.814695F, -0.911977F, -0.819817F, -0.992184F, - -1.5198F, 1.31962F, -1.57483F, -2.0591F, -2.04544F, -2.10065F, -2.05225F, - -2.05494F, 1.15389F, -2.17233F, -2.15527F, 1.51524F, -2.24992F, 1.19805F, - -1.26412F, -0.965117F, -1.24807F, -1.19504F, 1.28716F, -2.04171F, -2.14604F, - 2.32018F, -2.14219F, -0.8946F, -0.927996F, -0.813694F, -0.950558F, -0.96415F, - -1.23612F, -0.002031F, -0.0702192F, -0.988157F, -0.989009F, 1.31072F, - -2.04633F, 1.30272F, 1.28657F, 1.13328F, 0.596608F, -0.884057F, -0.99703F, - -0.797194F, -0.823385F, -0.962877F, -1.23679F, -0.774659F, -0.857037F, - -0.867897F, -1.18356F, -0.0403819F, -0.0466432F, -0.0685278F, -1.00165F, - 6.27356F, 1.29117F, 0.453348F, -0.881211F, -0.916757F, -0.432724F, -1.00916F, - -0.820028F, 0.722816F, -0.824109F, -0.90032F, -0.846422F, -1.24439F, - -1.08971F, -1.06548F, -0.798089F, -0.420356F, 0.486991F, 0.21033F, - -0.895891F, -0.624709F, -0.650574F, -0.856028F, -1.15638F, 1.35825F, - -0.576724F, -0.893131F, -0.865608F, -0.983798F, -0.662773F, -0.798289F, - 1.24962F, 1.23272F, 1.22459F, -1.54798F, 1.19932F, 2.0361F, -2.06095F, - -2.05024F, -2.05905F, 1.1519F, 0.874632F, 0.390783F, -0.951007F, -1.02051F, - -0.807661F, -1.26679F, -0.0427102F, -0.112647F, -0.976739F, 1.16035F, - 1.25745F, 1.19899F, -2.0019F, -2.06939F, -2.09916F, -2.07251F, 0.579111F, - -1.23231F, -1.24608F, -1.23142F, -1.11677F, -1.12376F, 1.16813F, 1.36635F, - -1.50567F, 1.14233F, -2.12093F, 1.1138F, -1.99532F, -0.804619F, -0.839253F, - -0.999521F, -0.860792F, -1.13112F, 1.28423F, -1.2487F, -0.962353F, - -0.886605F, -0.929792F, -0.414852F, -0.8175F, -0.0249277F, -0.814799F, - -0.971806F, -2.1326F, -0.00299455F, -0.865623F, 1.04849F, 0.67371F, - -1.13725F, -0.801019F, 1.27161F, -2.11506F, -2.11761F, 1.3347F, -2.56417F, - 1.26906F, -2.09601F, 1.35593F, -2.10596F, -2.12F, -0.814981F, -1.08244F, - -2.09611F, -2.06392F, 0.856398F, 1.14099F, -1.47571F, 0.673026F, -1.21862F, - -1.16153F, 0.391585F, -0.793871F, 0.41881F, 0.544296F, -1.07207F, 1.21057F, - -2.01831F, -1.92867F, -2.01416F, -1.97581F, -2.04452F, -1.53934F, -1.97843F, - -2.0051F, -1.91709F, -2.01025F, -2.01793F, -2.04938F, -1.21082F, -2.10618F, - -2.07551F, -2.08286F, -1.87939F, -2.01695F, 0.0466832F, -1.12341F, -1.15434F, - -1.79792F, 0.826131F, -2.11258F, -1.95586F, -0.994179F, -1.20306F, -1.17442F, - -2.03574F, -1.10617F, -0.860037F, -1.18779F, -1.11998F, -0.70821F, - -0.974558F, -1.10504F, -0.884573F, -1.97387F, -1.14566F, -1.97687F, - 0.648899F, -0.0129709F, -1.26895F, -1.55904F, -1.28172F, 0.433544F, - 0.248857F, -1.11871F, -1.1561F, -1.12602F, -0.998085F, -2.07773F, 0.286515F, - 1.66164F, -2.08723F, -2.00517F, -2.1086F, 0.905581F, 0.758946F, 0.872852F, - 0.996636F, -2.03021F, -0.837931F, 0.753791F, -2.07218F, 1.52928F, 0.871144F, - 0.943759F, 0.949516F, -2.10926F, -2.04479F, -2.08767F, -2.10822F, -2.00642F, - 1.09176F, -0.736756F, -1.29557F, -1.48774F, -1.10562F, -1.09208F, -2.13273F, - -1.91181F, -2.06343F, -1.88712F, 1.96804F, 1.00339F, 0.936001F, -4.02693F, - 0.987072F, -2.0775F, -2.07938F, -2.08922F, -1.99697F, -1.29116F, -1.57618F, - -1.56441F, -1.11855F, -1.54663F, -1.12141F, -2.0012F, -1.27211F, -1.24928F, - -2.14069F, -2.06468F, 0.924468F, 0.143418F, -0.651399F, -2.14205F, 0.789119F, - 0.647747F, -1.19382F, -2.03183F, -1.98195F, -16.4763F, -2.00254F, -3.45614F, - -2.08744F, 0.927921F, -2.06113F, -1.72018F, -1.17035F, -1.92425F, -2.13089F, - -1.54453F, -1.29755F, -7.58849F, -2.16722F, 1.1625F, -1.32545F, -1.12685F, - -1.42207F, 0.825499F, 0.161622F, -1.40935F, -1.4898F, -1.56716F, -1.20757F, - -0.86983F, 1.02207F, -1.27596F, 1.26404F, 0.656851F, 1.21231F, -1.17404F, - 1.16476F, 0.822585F, -1.90067F, 0.388339F, -1.33563F, 1.2509F, 0.641375F, - 0.553601F, 0.0391753F, -1.18967F, 0.936725F, 0.462712F, -1.07101F, -1.1336F, - 0.950039F, 4.69264F, 1.06444F, 1.8659F, 0.301931F, 1.05984F, 0.175142F, - 0.25425F, -1.1801F, -16.7317F, -1.99041F, -1.09995F, -2.06323F, -1.18426F, - -1.22556F, -1.14184F, -1.18932F, -1.03605F, -1.11107F, -2.09278F, -1.06234F, - -1.10194F, -2.17451F, -1.80463F, -2.08388F, -1.57382F, -1.07719F, -2.88654F, - -1.54095F, 2.01479F, -2.13712F, -1.00464F, -1.11475F, -1.1771F, -1.14321F, - 0.140866F, -1.91106F, -1.1283F, -1.0128F, -0.834905F, 0.899862F, -1.07455F, - -1.13114F, -1.25584F, 0.843383F, 0.164424F, 0.462596F, 0.0768235F, -1.07836F, - 0.381844F, -1.73801F, 0.407608F, 1.22912F, -1.08661F, -1.07981F, -1.19342F, - 0.192529F, 0.778709F, 0.244678F, 0.218988F, 0.205806F, 0.650088F, 0.507282F, - 1.0067F, -1.15562F, 0.516665F, 0.207722F, -1.15926F, 0.241149F, -1.10317F, - -0.999455F, -4.15003F, -1.17604F, -1.18228F, -3.04318F, -1.13669F, - -0.991145F, -1.06643F, -1.58518F, -1.1436F, -1.52743F, -1.99052F, -1.07001F, - -1.88897F, 0.26694F, 0.153394F, -1.89191F, -1.97327F, -1.22912F, -1.03274F, - -1.16672F, -2.03732F, -1.13615F, 1.14004F, -3.03203F, -1.11375F, -1.98203F, - -1.15686F, -1.02706F, -1.15777F, -1.80848F, 0.235112F, -1.12026F, -1.0816F, - 0.199088F, 0.239338F, -1.08939F, -2.90794F, -1.05252F, 1.16667F, 0.88634F, - -1.11981F, -1.18243F, 1.04085F, -1.53804F, 0.38056F, -0.89851F, -1.00678F, - 0.0238583F, 3.81621F, 3.551F, -1.07624F, -1.01163F, -1.05457F, -1.11749F, - 0.262165F, -1.15352F, 0.288554F, 0.183169F, -1.08874F, -1.05154F, -1.15418F, - -1.03378F, -1.04775F, -1.0958F, -1.14993F, -1.02304F, -1.12732F, 0.195319F, - 0.17145F, -2.05561F, 0.407449F, -1.21432F, -2.16948F, -1.11637F, -1.86606F, - -1.11684F, 0.909403F, -1.07917F, 0.801294F, -1.07957F, -1.06444F, -1.1334F, - -1.00814F, -1.04888F, -1.0742F, -3.64394F, -1.14011F, -1.84398F, -1.04924F, - -1.15541F, -1.38378F, -1.12105F, -1.11721F, -1.14412F, -1.12456F, -0.78831F, - -1.1273F, 1.27707F, -0.639789F, -1.00349F, -1.23285F, -0.611711F, 0.727909F, - -1.18386F, -0.978312F, 1.04693F, -1.00049F, -1.09516F, -1.15825F, -1.16605F, - -1.10392F, -1.31893F, -1.30983F, -1.14165F, -1.18031F, -1.81845F, -1.13294F, - -2.10761F, -1.0567F, 0.818327F, 0.908051F, -1.07624F, -1.15884F, -1.12171F, - -1.10547F, -1.17467F, 0.985973F, 0.144476F, 0.216313F, -1.21899F, -1.16024F, - -2.08103F, -1.09329F, -1.14743F, -2.19711F, 1.60385F, -1.1691F, 0.876939F, - -1.17302F, 1.24461F, -1.15884F, -0.0139481F, -1.48711F, 1.25145F, 0.819169F, - 0.859677F, 0.461955F, 0.390404F, -1.12525F, -0.971317F, -0.994643F, - 0.810176F, 0.581863F, -6.27153F, -1.11999F, -1.27231F, -2.86202F, -1.39943F, - -1.47037F, -1.77406F, -1.83098F, -1.11666F, -0.978868F, -1.15921F, 1.04339F, - 0.921315F, -0.998018F, -1.04781F, -1.05349F, -1.19429F, -1.47284F, 0.870561F, - -1.01522F, 0.901857F, -1.15092F, -1.10835F, -1.49013F, 0.496854F, 0.746508F, - 0.456267F, -1.10579F, -1.29839F, -1.76782F, -1.67533F, -1.38418F, -1.05146F, - 0.138288F, -1.02833F, -1.2937F, -1.72091F, -1.33428F, -1.42177F, -1.13739F, - -1.09234F, 0.146262F, -1.15735F, -2.0645F, -1.12817F, -1.49497F, -5.11875F, - -1.19445F, -1.67897F, 0.194132F, -1.41698F, -0.773226F, -1.7791F, 0.143276F, - 0.130326F, -2.03106F, -1.07133F, -1.23548F, -1.15499F, -1.12275F, -1.14523F, - -1.15314F, -1.1587F, -1.08048F, -1.1624F, -1.14641F, -0.74994F, -1.19995F, - -1.18266F, -1.78144F, -1.41028F, -1.96548F, -1.49764F, -1.2044F, -1.17103F, - -1.05274F, -1.21733F, -1.24011F, -1.18198F, -1.90512F, 0.202737F, -2.03684F, - -1.33609F, -1.07623F, -1.23063F, -1.07436F, -1.13756F, -2.84144F, -1.37689F, - -1.16767F, -2.17571F, -1.21598F, -1.1604F, -1.15934F, -1.76653F, -2.86072F, - -1.12511F, -0.897044F, -1.16485F, -1.11774F, -1.37957F, -1.97616F, -2.70234F, - -2.08409F, -1.12351F, -1.5259F, -1.85299F, -2.2709F, -2.68183F, -0.883669F, - -2.08594F, -1.2655F, -2.91931F, -1.14458F, -0.804208F, -1.08624F, -1.06107F, - -1.1749F, -0.859937F, -1.08512F, -2.21889F, -1.10414F, -1.94443F, -1.41224F, - -2.80027F, -2.05989F, -2.09631F, -2.85233F, -2.16468F, -0.628634F, -1.09029F, - -2.06747F, -2.78477F, -2.80867F, -2.30905F, -0.649697F, -1.2323F, -1.45913F, - -1.44095F, -1.19192F, -2.00268F, -2.14858F, -2.23913F, -2.13049F, -1.99862F, - -1.22228F, -2.13545F, -1.38702F, -2.08926F, -2.40086F, -2.75224F, -2.20227F, - -2.11244F, -1.23983F, -2.10538F, -2.88369F, -1.13178F, -1.96801F, -2.87656F, - -2.24256F, -2.2108F, -3.00618F, -3.01238F, -1.17442F, -1.20763F, -1.21777F, - -1.13081F, -1.99074F, -2.00368F, -1.22564F, -1.62107F, -1.2946F, -0.723515F, - -2.63946F, -2.6966F, -2.67715F, -2.21762F, -2.44241F, -1.15571F, -2.37134F, - -1.94115F, -2.05372F, -2.77589F, -1.36754F, -1.16613F, -1.04598F, -2.03531F, - -1.14108F, -1.3495F, -1.68854F, -1.73154F, -2.79981F, -2.83712F, -2.00662F, - -1.48431F, -2.13362F, -1.24914F, -1.01167F, -2.78886F, -2.08923F, -1.82611F, - -2.77972F, -2.19607F, -1.24064F, -1.68205F, -1.59859F, -1.92568F, -2.73419F, - -12.3328F, -1.96727F, -2.17216F, -2.27179F, -1.23136F, -1.14711F, -2.17651F, - -2.09979F, -1.11481F, -1.12181F, -1.10888F, -2.22594F, -2.30163F, -2.40925F, - -2.81662F, -2.36631F, -1.6127F, -1.08045F, -2.02558F, -1.74324F, -2.00893F, - -1.21295F, -1.25386F, -1.23016F, -1.065F, -1.17725F, -1.12147F, -1.88373F, - -1.55287F, -2.72919F, -1.14858F, -1.18144F, -4.71485F, -1.17432F, -2.15582F, - -1.1855F, -1.14628F, -2.42887F, -2.54758F, -3.90993F, -2.58808F, -1.18792F, - -1.45105F, -2.20195F, -1.10279F, -1.03205F, -1.27255F, -1.05279F, -1.1169F, - -1.24188F, -1.28403F, -1.19531F, -1.89143F, -2.71996F, -3.12619F, -2.81186F, - -1.86173F, -1.57849F, -1.19935F, -2.4571F, -1.43698F, -4.03836F, -1.80404F, - -4.80941F, -2.90492F, -2.94333F, -1.09597F, -5.29534F, -1.16231F, -1.93078F, - -2.07139F, -2.8711F, -7.65427F, -1.10374F, -1.17965F, -3.52744F, -1.05043F, - -1.16209F, -1.52689F, -1.98253F, -2.23558F, -3.03292F, -2.70426F, -1.26752F, - -2.70773F, -2.66125F, -2.61535F, -2.78191F, -2.10216F, -1.15416F, -2.75614F, - -2.73784F, -2.39264F, -1.04627F, -2.10281F, -1.09197F, -2.32046F, -1.09273F, - -2.79992F, -2.48229F, -1.20486F, -2.78466F, -1.12285F, -1.6843F, -1.63115F, - -0.791246F, -1.72334F, -1.27923F, -1.2959F, -2.04643F, -2.16653F, -1.65945F, - -1.19155F, -1.68201F, -1.57291F, -1.66147F, -1.6682F, -1.0484F, -1.08943F, - -1.54709F, -1.09068F, -2.11133F, -2.19369F, -2.25519F, -1.65652F, -1.10941F, - -0.959616F, -2.8241F, -2.74282F, -2.70408F, -1.98464F, -1.22514F, -1.15325F, - -1.22556F, -1.66845F, -1.43834F, -0.914837F, -2.83596F, -2.80572F, -1.65948F, - -1.21881F, -1.22543F, -2.05192F, -1.74596F, -2.81993F, -2.2992F, -2.32877F, - -1.76944F, -2.11511F, -2.08646F, -2.04877F, -1.45736F, -1.03495F, -1.18083F, - -1.58613F, -2.61119F, -2.12246F, -1.13959F, -1.82418F, -1.71789F, -1.82341F, - -2.20721F, -2.88129F, -2.35816F, -2.26018F, -2.25511F, -2.64441F, -2.24129F, - -2.8856F, -1.41996F, -2.88806F, -1.21741F, -2.89227F, -1.17545F, -1.16641F, - -1.16267F, -1.17193F, -1.1776F, -1.35272F, -2.07453F, -2.74183F, -2.79585F, - -2.76039F, -2.53761F, -2.91653F, -2.72123F, -2.66298F, -2.22295F, -1.12624F, - -2.22188F, -2.81661F, -2.41527F, -7.55476F, -2.24644F, -2.30518F, -2.87701F, - -2.4751F, -2.58756F, -2.47317F, -2.63802F, -2.52677F, -1.16714F, -1.17922F, - -1.09981F, -1.21151F, -1.06049F, -0.490961F, -1.0324F, -1.18263F, -1.41114F, - -1.05018F, -1.38417F, -1.88814F, -2.29476F, -2.19529F, -2.74398F, -2.53034F, - -2.85581F, -2.3519F, -1.55895F, -2.02837F, -1.16885F, -2.0801F, -3.23814F, - -2.90173F, -2.6366F, -1.0941F, -2.09436F, -1.30898F, -2.72175F, -2.77998F, - -2.55342F, -1.17362F, -2.46409F, -2.95309F, -2.70088F, -1.47336F, -7.27414F, - -2.43105F, -2.82295F, -2.85231F, -2.61846F, -1.83361F, -1.05424F, -1.12597F, - -1.06863F, -2.55837F, -2.76417F, -1.28619F, -1.30729F, -1.31998F, -1.9083F, - -2.08238F, -2.91849F, -2.53687F, -1.18759F, -1.06651F, -2.44093F, -1.66735F, - -1.18489F, -1.8308F, -2.05531F, -2.21303F, -2.80215F, -1.21878F, -1.27874F, - -1.48914F, -2.72496F, -1.12515F, -3.29605F, -2.6713F, -2.87851F, -2.36923F, - -2.6866F, -2.54617F, -3.10089F, -1.20735F, -1.23957F, -1.14188F, -1.25055F, - -1.3139F, -1.33103F, -1.13277F, -1.2372F, -1.13746F, -2.74632F, -2.11418F, - -2.70553F, -2.8281F, -2.44546F, -2.68793F, -2.73422F, -2.67912F, -2.18624F, - -2.23182F, -1.68836F, -1.19623F, -1.20707F, -1.45722F, -2.06677F, -2.70656F, - -2.21786F, -2.79356F, -1.16729F, -1.08255F, -1.11808F, -1.18479F, -1.24322F, - -1.31018F, -1.43153F, -7.80414F, -7.67269F, -1.91433F, -2.08938F, -2.45396F, - -1.19909F, -2.08826F, -2.82396F, -1.13374F, -1.12542F, -2.01036F, -2.53752F, - -1.39554F, -2.07017F, -2.10694F, -1.46419F, -1.53873F, -1.49363F, -2.76322F, - -1.46027F, -2.6574F, -2.69111F, -1.28859F, -2.06069F, -2.75681F, -2.69764F, - -1.39435F, -2.30044F, -1.10768F, -1.11941F, -2.44429F, -1.22598F, -3.87152F, - -2.44916F, -1.91351F, -1.53345F, -14.9337F, -2.07037F, -2.74979F, -2.47432F, - -1.69099F, -2.70506F, -2.60723F, -2.14717F, -2.14879F, -2.54902F, -2.48349F, - -2.18403F, -1.48626F, -1.39504F, -2.42713F, -2.88168F, -3.06653F, -2.38536F, - -1.27263F, -1.44318F, -1.7047F, -1.48631F, -1.85845F, -8.89602F, -1.8976F, - -1.88031F, -1.93576F, -2.53055F, -2.80387F, -2.44991F, -0.867885F, -2.22475F, - -1.18573F, -1.11408F, -2.11854F, -1.24015F, -1.4931F, -1.61159F, -7.97098F, - -1.84042F, -1.71891F, -1.61654F, -0.832708F, -1.65082F, -1.20577F, -1.11406F, - -1.13937F, -1.29707F, -2.25489F, -8.58936F, -1.72302F, -2.0605F, -2.86124F, - -1.17853F, -1.17509F, -0.945078F, -1.11585F, -0.534603F, -2.70587F, - -1.11174F, -1.14006F, -1.50944F, -1.56876F, -1.76942F, -2.80289F, -2.78339F, - -2.5302F, -1.11121F, -2.51442F, -1.14453F, -1.0893F, -2.90908F, -1.12461F, - -1.39824F, -2.21201F, -2.01713F, -2.04245F, -2.10426F, -2.21293F, -1.08649F, - -1.99218F, -2.47285F, -1.22037F, -2.07661F, -1.93172F, -1.29851F, -1.19869F, - -1.84852F, -1.94574F, -2.83622F, -2.54103F, -1.41594F, -1.26497F, -1.6368F, - -1.96813F, -2.74065F, -1.11622F, -1.14222F, -1.32175F, -1.17297F, -1.33184F, - -1.3308F, -2.26357F, -1.11796F, -3.09912F, -1.06004F, -15.7956F, -17.7037F, - -2.11417F, -2.72895F, -2.72269F, -1.11783F, -1.61172F, -10.3798F, -3.52636F, - -2.45252F, -1.20235F, -1.15412F, -1.27463F, -1.05462F, -1.14806F, -2.25535F, - -1.90345F, -2.81126F, -1.10851F, -1.12727F, -1.18734F, -1.0941F, -3.05388F, - -1.11782F, -1.97738F, -1.70085F, -2.22615F, -2.81748F, -0.85125F, -1.19939F, - -1.81194F, -2.41709F, -2.32703F, -1.19893F, -2.40796F, -2.72322F, -2.64914F, - -1.923F, -10.432F, -16.6046F, -6.3708F, -3.60232F, -2.08398F, -3.56466F, - -10.2453F, -2.11401F, -2.36435F, -2.05984F, -0.851167F, -0.843075F, - -2.66947F, -8.64763F, -8.51968F, -1.90197F, -2.31052F, -0.950344F, -2.19395F, - -1.2629F, -2.13497F, -1.07978F, -3.52288F, -2.81839F, -1.85044F, -2.17691F, - -2.19725F, -1.10973F, -1.48396F, -1.08154F, -1.15877F, -1.77452F, -1.91706F, - -2.08275F, -1.7097F, -2.16717F, -2.81265F, -2.26942F, -1.80209F, -2.8821F, - -1.07544F, -2.4713F, -1.32772F, -5.82879F, -11.4555F, -11.5646F, -9.55314F, - -2.05415F, -2.81539F, -2.80814F, -2.80763F, -2.68879F, -2.57374F, -1.22983F, - -11.5468F, -2.13163F, -11.5379F, -11.6053F, -2.58649F, -2.5422F, -1.75972F, - -6.61528F, -5.18469F, -2.17415F, -2.12348F, -2.52993F, -2.76034F, -1.25993F, - -1.25245F, -2.16409F, -2.15894F, -2.78781F, -1.94495F, -3.04952F, -2.77248F, - -2.29718F, -1.41625F, -2.67583F, -1.31391F, -2.50601F, -2.82433F, -1.82101F, - -13.3197F, -3.4673F, -11.3492F, -2.7286F, -2.65062F, -1.21803F, -1.17698F, - -1.07907F, -2.34504F, -1.75808F, -19.5545F, -2.87099F, -1.28893F, -1.13443F, - -1.46185F, -1.83153F, -2.69905F, -2.74131F, -1.33382F, -2.88296F, -1.31253F, - -3.10476F, -2.75653F, -1.85423F, -1.80984F, -1.78313F, -1.83857F, -1.78272F, - -1.90864F, -1.8399F, -1.20592F, -1.21544F, -1.97523F, -2.77449F, -2.76575F, - -2.51828F, -1.5656F, -1.14705F, -1.2695F, -1.10711F, -2.2649F, -1.84435F, - -1.67004F, -1.7989F, -2.5132F, -1.33528F, -2.12324F, -1.44819F, -2.01007F, - -2.93565F, -2.81376F, -2.55907F, -2.06095F, -1.80836F, -1.82099F, -1.77796F, - -1.75516F, -1.79473F, -1.83082F, -2.70734F, -2.72194F, -1.13234F, -2.6177F, - -2.70895F, -2.04383F, -1.2023F, -1.22636F, -1.17681F, -1.78486F, -1.55763F, - -1.75747F, -2.81216F, -2.52835F, -2.81543F, -1.53232F, -1.11349F, -1.46443F, - -2.05283F, -1.95997F, -2.09106F, -1.92049F, -1.95962F, -1.9904F, -1.78362F, - -1.76791F, -1.89986F, -1.75718F, -2.52289F, -2.6476F, -1.82068F, -2.46305F, - -1.83857F, -2.38543F, -1.817F, -8.76185F, -2.05396F, -1.87876F, -1.94339F, - -1.87946F, -2.09066F, -1.9668F, -1.92872F, -2.02953F, -2.05057F, -2.05532F, - -2.07009F, -2.80171F, -2.85078F, -2.79825F, -2.79486F, -2.43037F, -2.10174F, - -1.1975F, -2.07386F, -2.00339F, -1.8828F, -2.31868F, -2.43361F, -1.85516F, - -1.77634F, -1.90857F, -2.71161F, -2.86469F, -2.00811F, -1.76962F, -1.73871F, - -2.18386F, -2.54578F, -2.82666F, -2.82621F, -2.70581F, -2.43599F, -1.2767F, - -1.24976F, -1.23116F, -2.64964F, -2.74717F, -2.70579F, -2.44733F, -2.2085F, - -2.10146F, -2.44347F, -2.85741F, -1.47114F, -2.80364F, -1.30615F, -1.20337F, - -2.35608F, -1.29599F, -2.06536F, -2.42029F, -2.71016F, -2.67408F, -2.05845F, - -2.87562F, -2.79777F, -2.47058F, -3.00812F, -2.39139F, -2.59378F, -2.3649F, - -2.40439F, -2.20235F, -2.74242F, -2.57067F, -2.79733F, -2.17201F, -2.41939F, - -1.86247F, -2.78326F, -2.81132F, -2.83219F, -1.25804F, -1.93073F, -2.46244F, - -2.29645F, -1.25335F, -1.23757F, -1.17796F, -2.95632F, -2.37167F, -1.2595F, - -1.24444F, -1.26745F, -1.18912F, -1.14582F, -1.19819F, -2.87207F, -2.25818F, - -1.25285F, -2.6677F, -2.24066F, -1.30522F, -2.59171F, -1.51886F, -2.19596F, - -2.19F, -2.20569F, -2.07134F, -2.08499F, -2.13834F, -2.17574F, -2.90224F, - -2.67768F, -2.80291F, -2.50242F, -6.03915F, -2.7434F, -2.22792F, -2.06778F, - -2.05467F, -2.18926F, -2.80324F, -2.56691F, -2.7988F, -2.48296F, -2.20513F, - -2.22615F, -2.57479F, -2.63923F, -2.67628F, -2.58807F, -2.50953F, -2.408F, - -2.22849F, -2.55622F, -2.6312F, -2.79695F, -2.62318F, -2.37839F, -2.70856F, - -1.74127F, -1.89242F, -1.89848F, -3.12564F, -3.73432F, -2.05504F, -2.11376F, - -2.69925F, -2.48626F, -1.76269F, -1.97713F, -2.81324F, -2.91962F, -2.66813F, - -2.66898F, -2.55468F, -2.34049F, -2.64277F, -2.86568F, -2.65613F, -2.79211F, - -2.79574F, -3.23192F, -2.26326F, -2.46421F, -2.21069F, -2.57201F, -3.13322F, - -2.71252F, -2.31421F, -2.26246F, -2.3517F, -1.94143F, -1.97268F, -1.92128F, - -2.03945F, -2.00793F, -2.05869F, -2.13559F, -2.76647F, -2.78423F, -2.34626F, - -2.32623F, -1.91756F, -1.90674F, -2.05669F, -2.04263F, -2.54841F, -2.72612F, - -2.71815F, -2.51651F, -2.72417F, -2.44228F, -2.38294F, -2.2309F, -2.11483F, - -2.80601F, -2.70607F, -2.67597F, -2.37385F, -2.82017F, -2.4191F, -2.33767F, - -2.84359F, -2.82136F, -2.09249F, -2.50129F, -2.42886F, -2.83655F, -2.79565F, - -2.73382F, -1.83697F, -1.81649F, -2.05413F, -2.22768F, -2.03537F, -2.4233F, - -2.81033F, -1.93768F, -2.05898F, -2.16859F, -2.63991F, -2.51773F, -2.73706F, - -2.90155F, -2.77106F, -2.79183F, -2.60178F, -2.46722F, -2.54032F, -2.83954F, - -2.43317F, -2.76056F, -2.68118F, -2.71044F, -2.22131F, -2.22225F, -2.20255F, - -1.50895F, -2.55999F, -2.76277F, -2.03129F, -2.18207F, -2.09694F, -2.12817F, - -2.85793F, -2.8665F, -2.70788F, -2.76717F, -2.64675F, -2.79014F, -2.91179F, - -3.04758F, -2.80776F, -2.7405F, -2.65629F, -2.80078F, -2.8151F, -2.82885F, - -2.46094F, -2.87793F, -2.39581F, -2.06198F, -2.16824F, -2.83659F, -3.10759F, - -2.85196F, -2.73646F, -2.71259F, -2.97618F, -2.60597F, -2.63726F, -2.45218F, - -2.34338F, -1.83596F, -2.70667F, -3.07859F, -2.80917F, -2.7297F, -2.68124F, - -2.71029F, -3.48468F, -2.52152F, -2.61956F, -2.69052F, -2.53609F, -2.54582F, - -2.29279F, -2.40575F, -2.75452F, -2.3902F, -2.48182F, -2.28336F, -2.81932F, - -2.55573F, -2.52309F, -2.53548F, -2.24984F, -2.53613F, -2.68904F, -2.6394F, - -2.48727F, -1.54531F, -2.28628F, -2.96576F, -2.92617F, -2.56753F, -2.01472F, - -2.08343F, -1.8642F, -2.07885F, -2.05981F, -2.13836F, -2.45075F, -2.46584F, - -2.73682F, -2.89566F, -2.74517F, -2.52232F, -2.71411F, -2.55154F, -2.63071F, - -2.4561F, -2.62599F, -2.84539F, -2.80885F, -2.63745F, -2.63431F, -2.60811F, - -2.59073F, -2.60246F, -2.56387F, -2.42521F, -2.31775F, -1.27953F, -1.28224F, - -2.04017F, -2.05785F, -2.15165F, -2.48572F, -2.75859F, -2.79742F, -2.53241F, - -2.71157F, -2.70539F, -2.69179F, -2.49856F, -2.59488F, -2.59304F, -2.54825F, - -2.54949F, -2.45891F, -2.2185F, -1.28076F, -2.16299F, -2.45149F, -2.65289F, - -2.58833F, -2.75306F, -2.78344F, -2.52237F, -2.82156F, -2.40914F, -2.82392F, - -2.53404F, -2.57599F, -2.51992F, -2.65074F, -2.34448F, -2.21335F, -2.21702F, - -2.0511F, -2.41214F, -2.43069F, -2.67745F, -2.62075F, -2.77429F, -2.62543F, - -2.08405F, -2.42952F, -2.81978F, -2.45155F, -2.41938F, -2.17176F, -2.21692F, - -2.67015F, -2.7628F, -2.40981F, -2.63753F, -2.82662F, -1.6059F, -2.19587F, - -1.58875F, -2.80505F, -2.77225F, -2.31091F, -2.53004F, -2.40477F, -3.1437F, - -2.00347F, -1.89318F, -1.97242F, -1.96444F, -2.04666F, -2.0844F, -2.18615F, - -2.17031F, -2.44816F, -2.6577F, -2.68839F, -2.73732F, -2.45943F, -2.81527F, - -3.98929F, -2.7626F, -2.33839F, -2.95695F, -2.70236F, -2.53625F, -2.68143F, - -3.14222F, -2.49934F, -2.89846F, -2.48969F, -2.21775F, -2.07234F, -2.10343F, - -2.12845F, -2.28123F, -2.18398F, -2.06348F, -2.19442F, -2.82844F, -2.29895F, - -2.46737F, -2.50781F, -3.10659F, -2.78248F, -3.00142F, -2.70148F, -2.44286F, - -1.90305F, -2.54609F, -2.83145F, -2.77506F, -2.6376F, -1.9167F, -2.58865F, - -2.93157F, -2.72556F, -2.49091F, -2.94026F, -2.39145F, -2.67973F, -2.63782F, - -2.24645F, -2.2832F, -2.54818F, -2.6968F, -2.78157F, -2.87743F, -2.4952F, - -2.51638F, -2.38623F, -2.21651F, -2.34052F, -2.30471F, -2.41856F, -2.78774F, - -2.81576F, -2.75011F, -2.50924F, -2.36173F, -2.19483F, -2.30396F, -2.32489F, - -3.0995F, -2.82825F, -2.93332F, -3.11297F, -2.31129F, -2.28959F, -2.95115F, - -2.29562F, -2.54243F, -2.33079F, -2.244F, -2.08446F, -1.95721F, -2.13949F, - -2.05147F, -2.10881F, -2.06158F, -2.81825F, -2.98667F, -2.05524F, -2.16982F, - -2.06753F, -3.12991F, -2.82426F, -2.35503F, -1.86671F, -2.10106F, -2.30728F, - -2.14746F, -2.81448F, -2.2951F, -2.45488F, -2.69135F, -2.73346F, -2.69902F, - -2.94795F, -2.17777F, -2.81012F, -2.38F, -1.73713F, -2.59518F, -1.98652F, - -2.36121F, -2.34496F, -2.23388F, -2.79131F, -2.90143F, -2.86284F, -2.49341F, - -2.36043F, -2.19488F, -2.20768F, -2.71727F, -2.32339F, -2.33159F, -2.58217F, - -2.16928F, -2.83572F, -2.78776F, -2.98311F, -2.72998F, -2.76973F, -2.83131F, - -2.81287F, -2.5101F, -2.6796F, -2.23575F, -2.83768F, -2.71812F, -2.71508F, - -2.72614F, -2.82504F, -2.95064F, -2.60722F, -2.49526F, -2.82762F, -2.54394F, - -2.19616F, -2.28124F, -2.76447F, -2.33074F, -1.94201F, -2.01812F, -2.46134F, - -2.70905F, -2.79768F, -2.82848F, -2.64919F, -2.35138F, -2.70721F, -2.73621F, - -2.40338F, -2.79386F, -2.17877F, -2.86243F, -2.68016F, -1.89913F, -2.77912F, - -2.73236F, -2.88722F, -2.5932F, -2.3708F, -2.81534F, -2.82502F, -2.8048F, - -2.75064F, -2.66143F, -2.67278F, -2.7421F, -2.76686F, -2.83612F, -2.68085F, - -2.58179F, -2.14326F, -2.97533F, -2.56828F, -2.62077F, -2.61724F, -2.64038F, - -2.34988F, -2.28541F, -2.18561F, -2.27273F, -2.11042F, -2.3127F, -2.73828F, - -2.71117F, -2.37344F, -2.26565F, -3.2715F, -4.47156F, -2.74237F, -2.03066F, - -3.9721F, -2.16289F, -3.16533F, -2.68977F, -2.2926F, -2.08925F, -2.07965F, - -2.51805F, -2.85649F, -2.74961F, -2.35685F, -2.46852F, -2.93668F, -2.1469F, - -2.61833F, -2.31077F, -2.05743F, -2.20604F, -2.69128F, -1.99608F, -2.30002F, - -2.08563F, -2.13714F, -2.66291F, -2.71722F, -2.68739F, -2.22516F, -1.42857F, - -2.98055F, -2.83408F, -2.82041F, -2.61235F, -1.04706F, -2.8476F, -1.07413F, - -1.43079F, -1.37732F, -2.80701F, -2.06393F, -1.07521F, -0.985954F, -1.31627F, - -0.840405F, -0.00116933F, 1.15226F, 0.201103F, -0.793756F, 0.102779F, - -0.494478F, -0.228721F, -0.259069F, 1.35548F, 1.19643F, -0.205978F, - -0.741256F, -1.00316F, -0.142431F, 0.00455529F, -0.791627F, 0.111402F, - -0.232897F, -0.233276F, -0.242294F, -0.237872F, -0.258398F, -0.17004F, - -0.495784F, -0.17111F, 1.20658F, 1.19704F, -0.12978F, -0.379862F, -1.3038F, - -1.59584F, -0.275191F, -0.16704F, -0.263017F, 1.12341F, -1.05902F, - -0.789307F, -0.794405F, -0.168287F, -0.168243F, -0.238264F, -0.161618F, - -0.233371F, -0.258205F, -0.23724F, -0.253426F, -0.260499F, -2.26441F, - -2.17017F, -2.27275F, -2.20407F, 1.07195F, -0.166667F, -0.208693F, -0.17971F, - -0.874567F, -0.26434F, -0.26922F, -0.259043F, -0.16175F, -0.264445F, - -0.168558F, -0.124308F, -0.181689F, -0.44109F, 0.320927F, -0.70981F, - -0.669569F, -0.186747F, -0.254641F, -0.247643F, -0.23787F, -0.233344F, - -0.268455F, -0.180905F, -0.16903F, -0.239183F, -0.229193F, -1.2845F, - -0.20302F, -0.146874F, -0.166995F, -0.280251F, -0.270025F, -0.160708F, - -0.887549F, -0.50395F, -0.177385F, -1.38772F, -0.519574F, -1.12174F, - -0.235063F, -0.173678F, -0.25338F, -2.51259F, -1.07626F, -1.23767F, - -1.08873F, -0.277144F, -0.204538F, -1.38965F, -0.168141F, -0.13381F, 2.4979F, - 2.15759F, 1.68817F, 1.01644F, -0.150591F, -0.157273F, -0.211847F, -0.257971F, - 0.811926F, 0.62925F, -0.167124F, -0.149721F, -0.125594F, -0.254646F, - 1.73701F, 1.073F, -0.160936F, -0.237634F, -0.140959F, -0.15995F, 2.18547F, - -0.236676F, -0.25541F, -0.18029F, -0.214935F, -0.200966F, -0.166914F, - -0.252363F, 1.78334F, -0.24087F, -0.216498F, -0.178096F, -0.167616F, - -0.253452F, 1.36506F, 1.28034F, -0.206738F, 1.27415F, 1.19665F, -0.166888F, - 1.01568F, -0.20368F, -0.215835F, -0.203907F, -0.0502188F, -0.813273F, - -0.577087F, -0.133519F, 0.594161F, 0.593889F, -0.391713F, -0.142506F, - -0.634379F, -0.372729F, -0.367297F, -0.369359F, -0.329433F, -0.196926F, - -0.156864F, 0.595845F, -0.152074F, 0.784138F, -1.56078F, -0.435741F, - -0.432009F, -0.421876F, -0.42231F, -0.117693F, -0.386794F, -0.375323F, - -0.38112F, -0.525194F, -0.503004F, -0.458821F, -0.373731F, -0.434423F, - -0.803903F, -0.575878F, -1.15991F, -0.828789F, -0.60302F, -1.32803F, - -1.01811F, 0.604378F, 0.588286F, -0.398783F, -0.380371F, -0.347146F, - -0.823108F, -0.82173F, -0.4053F, -0.38503F, -0.829961F, -0.827612F, - -0.533017F, -0.41378F, -0.183596F, -0.599941F, -0.162676F, -1.10892F, - -0.518942F, -0.252496F, -0.238068F, -1.3899F, -0.646125F, -0.471069F, - -0.466529F, -0.394088F, -0.361516F, -1.27136F, -0.684103F, -0.841172F, - -0.608866F, -0.481872F, -0.394288F, 0.000309513F, 0.0128667F, -0.857638F, - -0.105016F, -1.38968F, -0.509441F, -0.478337F, -0.425578F, -0.234893F, - -0.139807F, -0.466622F, -0.932648F, -0.505585F, -0.411996F, -0.123173F, - -1.32219F, -1.019F, -1.06631F, -0.967831F, -0.640283F, -0.471838F, - -0.234483F, -1.14029F, -0.173653F, -0.283188F, -0.205938F, 0.030572F, - -0.551287F, -0.471551F, -0.434774F, -0.417599F, -0.406057F, -0.244345F, - 0.0254183F, 0.250703F, -0.878258F, -0.262106F, 0.0394478F, -1.39821F, - -0.77626F, -0.51377F, -0.0186296F, -1.46728F, -1.38117F, -0.149468F, - -1.58637F, 0.0159187F, -0.69684F, -1.82569F, -2.13481F, -2.12908F, - -0.599935F, -0.524611F, -0.0427025F, -1.79154F, -0.437372F, -0.0449773F, - -0.0174857F, -0.3292F, 0.0416402F, -0.374796F, -0.651771F, -0.378711F, - -1.83629F, -1.85654F, -1.88541F, -0.506655F, -0.372573F, 0.090869F, - 0.0136462F, -0.08037F, -1.77983F, -8.15658F, -0.529789F, -0.435071F, - 0.00425459F, -0.0481661F, -0.012263F, -1.76669F, -0.482121F, -0.473893F, - -0.312375F, -0.280391F, -0.0741739F, -0.395308F, -0.371631F, -0.276568F, - -3.58078F, -0.135255F, 0.0759224F, 0.00958602F, -0.372698F, 0.0371242F, - -0.0235706F, -1.80543F, -1.8503F, -4.8776F, -0.4261F, -0.424025F, - -0.0157182F, -0.405114F, -0.0856256F, -0.432157F, -0.411464F, -0.0897871F, - -2.02098F, -7.38075F, -0.72576F, -0.476634F, -0.417934F, -0.337416F, - -0.256159F, -0.074747F, -0.0329521F, 0.124527F, -1.82545F, -1.11699F, - -1.6514F, -0.477121F, -0.454755F, -0.449213F, -0.0875954F, 0.0315617F, - -1.78145F, -1.67975F, -2.65552F, -0.474679F, -0.420273F, -1.82354F, - -1.89837F, -1.42387F, -0.567675F, -0.475366F, -0.477554F, -0.438229F, - -0.00885184F, -0.758667F, -0.424097F, -2.76764F, -1.83711F, -2.52743F, - -0.527803F, 1.1895F, -0.17208F, -0.550744F, -0.349339F, 0.0279761F, - 0.0249127F, -0.691789F, -0.60738F, -0.57559F, -0.435833F, 0.0179878F, - 0.333601F, -1.99441F, -1.86262F, -1.91711F, -1.89456F, -0.467972F, - -0.422228F, -0.403005F, -0.362751F, 0.0928073F, 0.00566493F, -1.81458F, - -1.75287F, -1.55221F, -1.24425F, -1.2123F, -1.19075F, -1.13737F, -0.441897F, - -0.02517F, -0.0226982F, 0.234103F, -0.0445284F, -0.197385F, -2.89133F, - -0.859711F, -1.93849F, -0.699552F, -0.468571F, -0.456021F, -0.528971F, - -0.432208F, -0.426492F, -0.420876F, -0.400934F, -0.224595F, -0.242295F, - -0.141361F, 0.0261844F, 0.0746324F, 0.0528424F, 0.0209277F, 0.453863F, - -0.0166163F, -1.78535F, -0.84899F, -1.237F, -1.38363F, -0.425101F, - -0.356116F, -0.104621F, -0.187072F, -0.199056F, -1.93555F, -1.34162F, - -1.18085F, -0.595292F, -0.374351F, -0.160266F, -0.202736F, -1.02391F, - -0.308651F, -0.117808F, -2.08498F, -0.827692F, -0.412142F, -0.204349F, - 0.879318F, -0.257031F, -0.22323F, -0.288403F, -0.0748574F, -0.039168F, - -1.00546F, -1.05529F, -0.609727F, -0.478585F, 0.0422583F, 0.426083F, - -0.461453F, -0.404158F, 0.0541008F, 0.0112319F, 0.120175F, 0.451373F, - -1.6365F, -1.85987F, -1.16456F, -0.731004F, -0.604993F, -0.42801F, - -0.403714F, -0.29337F, -0.167899F, -0.0104219F, -0.00199712F, 0.168971F, - 0.813012F, -0.199002F, -0.79133F, -0.0941831F, 0.0316178F, -1.09432F, - -0.424376F, 0.0511908F, 0.637748F, -2.55008F, -1.08123F, -0.35387F, - -0.0510066F, -0.000116328F, -0.90301F, -0.419826F, -0.365152F, -0.995584F, - -0.32808F, -0.19708F, -0.147329F, -1.84351F, -1.84154F, -1.09546F, - -0.479108F, -0.0483498F, 0.0798054F, 0.737086F, -1.75704F, -2.05901F, - -0.836785F, -0.452632F, -0.403025F, -0.260465F, -0.226448F, -0.0453334F, - 0.0215242F, -2.04094F, -0.55294F, -0.476052F, -0.450778F, -0.00558304F, - 0.103189F, 0.729482F, 0.427483F, -1.13031F, -0.470592F, -0.424937F, - -0.410127F, -0.0113193F, -0.163795F, -0.010886F, -1.72468F, -1.61369F, - -0.512213F, -0.320024F, -0.0312382F, -0.0617981F, 0.0374096F, -1.77426F, - -2.06072F, -1.65676F, -1.46791F, -1.27667F, -0.515828F, -0.415372F, - -1.81178F, -0.525458F, -0.444972F, 0.038121F, 0.9369F, 0.843917F, -0.464501F, - -0.407172F, 0.0147575F, -0.0069798F, -1.7684F, -0.444044F, 0.00960877F, - 0.0102367F, -0.0287164F, 0.139911F, 0.433012F, 0.424049F, -0.377262F, - -0.397795F, -0.0610284F, -0.0092092F, 0.115152F, 0.42559F, 0.400505F, - -1.15233F, -0.480513F, -0.409851F, -0.407686F, -0.103782F, -0.0331289F, - -0.0127875F, 0.196798F, 0.137687F, -1.74331F, -2.55259F, -1.60523F, - -0.611787F, -0.475462F, -0.474105F, -0.102774F, 0.346298F, 0.11335F, - 0.365615F, -1.76952F, -1.60286F, -1.05186F, -0.767989F, -0.882275F, - -0.602771F, -0.454532F, -0.422335F, -0.266352F, -0.0600146F, 0.0404781F, - -0.08377F, 0.00330391F, -0.0265314F, -0.280899F, -0.122297F, 0.0394171F, - 0.100533F, 0.0722163F, 0.790815F, 0.490765F, 0.0288569F, 0.846129F, - 0.402455F, 0.875132F, -0.575746F, -0.51436F, -0.639804F, -0.413825F, - -0.407514F, -0.333817F, -0.149896F, 0.00976875F, 0.280828F, -0.922299F, - -0.811259F, -0.67185F, -0.511449F, -0.493878F, -0.26914F, 0.146512F, - -0.0226726F, 0.0826738F, -0.0123994F, -1.83284F, -1.44825F, 0.0632444F, - 0.556184F, 0.27788F, -1.81857F, -1.07795F, -1.04724F, -1.13286F, -0.671982F, - -0.410346F, 0.0045922F, 0.080636F, 0.0558886F, -0.577527F, -0.107649F, - -0.00108423F, 0.0111365F, 0.148213F, -0.0327546F, 0.0901404F, 0.751416F, - 0.434478F, -1.11563F, -1.77145F, -1.38374F, -0.728302F, -0.568429F, - 0.0900387F, 0.425463F, -1.89024F, -1.70402F, -0.771191F, -0.600645F, - -0.425161F, -0.00904647F, 0.318908F, -1.82942F, -0.747275F, 0.020737F, - 0.559031F, -2.12456F, -2.26034F, -1.99353F, -1.71315F, 0.0206687F, - 0.0716288F, 0.0324676F, 0.52786F, 0.906775F, -1.25276F, 0.00576959F, - -1.29273F, -0.533464F, 0.155126F, -2.34706F, -0.540239F, 0.770096F, - -0.398058F, -1.13298F, -0.541954F, 0.152848F, -0.0246836F, 0.429153F, - 0.439843F, 0.418623F, -1.00795F, -0.393913F, -0.000420162F, -0.0286444F, - -0.376019F, 0.811283F, -1.04469F, 0.419301F, 0.0231855F, 0.0128388F, - 0.0421263F, 0.175946F, 0.372547F, 0.419285F, -3.38909F, -1.78168F, -1.24852F, - -0.0545446F, -0.0480663F, 0.444521F, -0.0132058F, -0.0338886F, -0.0541608F, - -0.0393654F, 0.185607F, 0.433184F, -0.0562799F, 0.440539F, -0.0180453F, - -0.0463548F, -0.0193724F, -1.16749F, -1.09875F, -0.00964839F, -1.30825F, - -1.28838F, -1.2074F, -1.20818F, -1.18681F, -1.19877F, -1.16257F, -1.14428F, - -1.07248F, -0.972146F, -0.0586474F, -0.0565374F, -0.0324376F, -1.20975F, - -1.17514F, -0.354156F, -0.0598895F, -0.0619771F, -0.0552536F, -0.0553534F, - -1.23208F, -1.18063F, -1.1612F, -0.808564F, -0.774573F, -0.730751F, - -0.738515F, -0.578299F, -0.588622F, -0.556471F, -0.542992F, -0.399751F, - -0.352872F, -0.1151F, -0.0676921F, -0.0594651F, -0.0636043F, -1.24402F, - -0.748292F, -0.614341F, -0.526871F, -0.386617F, -0.342123F, -0.0674396F, - -0.0548553F, -0.767419F, -0.734666F, -0.722442F, -0.725599F, -0.617733F, - -0.518813F, -0.147356F, -0.0453061F, -0.0306743F, -0.077821F, -0.0782671F, - -0.958409F, -0.64576F, -0.0743454F, -0.105523F, -0.727913F, -0.114931F, - -1.20251F, -1.18037F, -1.1677F, -1.26929F, -1.11038F, -1.12639F, -1.11205F, - -1.27855F, -0.363896F, -0.21838F, -0.77403F, -0.716453F, -0.548638F, - -0.327053F, -0.0646611F, -1.32843F, -1.23028F, -0.223604F, -0.62004F, - -0.600414F, -0.544246F, -1.17599F, -0.186793F, -0.495041F, -1.20402F, - -1.15943F, -1.1409F, -0.760143F, -0.667733F, -0.561356F, -0.548505F, - -1.24372F, -0.687104F, -0.544582F, -1.34478F, -0.645259F, -0.571378F, - -1.6706F, -1.8021F, -9.74684F, -1.33095F, -1.21751F, -0.758851F, -1.74099F, - -1.68431F, -0.93693F, -0.64409F, -0.293781F, -0.636614F, -0.311869F, - -0.833932F, -1.3204F, -2.14471F, -0.651668F, -0.621477F, -1.7089F, -1.85636F, - -1.86576F, -1.42336F, -1.22639F, -0.143427F, -6.11033F, -1.75862F, -1.8392F, - -1.88441F, -1.80384F, -1.89043F, -0.390512F, -0.286185F, -0.282383F, - -0.336845F, -0.0884828F, -0.137622F, -0.0423561F, -0.440127F, -0.0521268F, - -0.279738F, -0.35195F, -0.0585687F, -1.82544F, -1.56431F, -1.78649F, - -1.72438F, -1.68636F, -0.465706F, -0.140538F, -0.326734F, -1.72741F, - -1.86524F, -1.70625F, -0.0943635F, -1.75178F, -1.69215F, -1.71106F, - -1.32686F, -0.566377F, -0.171405F, -0.172118F, -0.604134F, -0.276391F, - -0.182389F, -0.393264F, -0.335193F, -0.338458F, -1.70795F, -1.76007F, - -1.80015F, -1.74003F, -1.67782F, -0.247331F, -0.17218F, -0.116468F, - -1.73936F, -0.355908F, -0.168461F, -0.17774F, -0.172596F, -0.176704F, - -0.170116F, -0.136158F, -1.82169F, -1.7458F, -1.80641F, -0.424131F, - -0.344934F, -0.165313F, -0.170635F, -0.0865236F, -0.105278F, -0.0349952F, - -0.80771F, -0.464745F, -0.377538F, -0.330108F, -0.175858F, -0.165335F, - -0.176129F, -0.0899005F, -0.46333F, -0.390734F, -0.371955F, -0.376292F, - -0.35351F, -0.0949706F, -0.164707F, -0.11268F, -0.101807F, -0.101174F, - -0.606178F, -0.332601F, -0.334454F, -0.322277F, -0.294947F, -1.72328F, - -1.87419F, -1.2905F, -0.0337652F, -1.75717F, -0.243337F, -0.796128F, - -0.465586F, -1.26311F, -0.114559F, -0.0970518F, -0.818169F, -1.62414F, - 0.0451152F, -0.050451F, -0.963136F, 0.132541F, -0.0477107F, 0.0606509F, - -0.600729F, -1.01982F, -0.00255711F, -0.623458F, -0.900612F, -1.71438F, - -1.6901F, -1.00651F, -0.987401F, -0.730467F, -0.750026F, 0.00938517F, - -0.161014F, -1.66417F, -1.24525F, -0.160782F, -0.861565F, -0.964024F, - 0.0141086F, 0.0404631F, -0.165764F, -0.162595F, -0.16235F, -0.157274F, - -1.20016F, -0.901701F, 0.00275721F, 0.242072F, -1.59397F, -1.01224F, - -0.948158F, -0.88987F, -0.914542F, 0.00639365F, -1.47563F, -1.0183F, - -0.966142F, -1.05388F, -0.169345F, -0.968707F, -0.972815F, 0.0414859F, - 0.260986F, -0.976856F, 0.031734F, 0.0376001F, -0.944252F, -0.998879F, - 0.134079F, -1.01882F, -0.872808F, -0.411302F, -0.0267561F, 0.0426694F, - -0.235992F, -1.66313F, -1.08853F, -1.01527F, -1.00881F, -0.727072F, - -0.692049F, 0.00786367F, -1.29532F, -1.02642F, -1.02397F, -0.964748F, - -0.690139F, -1.03617F, -1.00164F, -0.969462F, -0.896932F, -0.845072F, - -0.820201F, 0.235012F, -1.03542F, -0.0248551F, -0.0176886F, 0.0472831F, - -1.01067F, -0.0972669F, 0.00130679F, -0.764601F, -0.63967F, 0.145104F, - -0.877458F, -1.02958F, -0.705077F, -0.699666F, -0.693308F, -0.698492F, - -0.653493F, -0.0527313F, 0.0364782F, 0.0410694F, -0.968091F, -1.01878F, - -0.879943F, -0.851149F, -0.728124F, -0.436224F, -0.0125997F, -0.020835F, - -1.29874F, -1.04053F, -0.972725F, -0.932542F, -0.71489F, -0.686115F, - -0.231596F, 0.0326831F, 0.04075F, -0.0155041F, 0.0428834F, -0.874712F, - -0.769922F, -0.730865F, -0.673264F, -0.636457F, -0.553113F, -0.259247F, - -1.55027F, -1.03941F, -0.85735F, -0.842685F, -0.633782F, -0.227061F, - -0.0266802F, -0.95706F, -0.694641F, -0.0418942F, -1.03233F, -1.07348F, - -0.848526F, -0.71123F, -0.569713F, 0.0320749F, 0.142666F, 0.0307223F, - -0.894514F, -0.913096F, -0.0725891F, 0.00995804F, -0.00697048F, 0.242467F, - 0.234841F, -0.762798F, -0.668711F, 0.0684846F, -0.665464F, -0.652904F, - 0.365545F, -1.01885F, -0.43829F, -0.894374F, -0.798513F, -0.556778F, - 0.375146F, -0.648262F, -0.967173F, -0.678345F, -0.0295319F, 0.24637F, - -1.07524F, -0.846847F, -0.705755F, -0.654469F, -0.654262F, -0.6111F, - -1.29706F, -0.953087F, -0.705748F, -1.06254F, -0.697013F, -0.654181F, - 0.0301679F, -0.0233504F, 0.0418374F, -1.39067F, -0.986362F, -0.905114F, - -0.872149F, -0.822297F, -0.0564491F, -0.0271198F, 0.0453409F, -0.0419725F, - -1.13306F, -1.46335F, -0.918696F, -0.744745F, -0.693609F, 0.0635947F, - 0.0481563F, 0.237707F, -1.4311F, -1.06109F, -0.894652F, -0.694009F, - -0.0272829F, 0.0984052F, 0.0191555F, -1.16037F, -0.973091F, -0.673666F, - -0.593712F, 0.0624298F, -0.869879F, -0.667684F, -0.688852F, -0.702218F, - -0.554783F, -0.448125F, -0.0337886F, -0.985453F, -0.708422F, -0.763367F, - -0.655969F, -0.772888F, -0.0325186F, -0.057067F, -0.0367226F, -0.927248F, - -0.810927F, -0.0455977F, 0.138916F, -0.856517F, -0.921802F, -0.695951F, - -0.68081F, -0.0118955F, -0.801474F, -1.0034F, 0.057875F, -0.971987F, - -0.705699F, -0.696191F, -1.11533F, -1.00476F, -0.907117F, -0.872972F, - -0.745658F, -0.688F, -0.61686F, -0.896876F, -0.237669F, -0.0473697F, - 0.037953F, -0.0227121F, -0.0212661F, -0.971067F, -0.743904F, -0.7545F, - -0.0206022F, -1.01497F, -0.642906F, -0.893236F, -0.911132F, -0.582471F, - -0.705778F, -0.32626F, -0.235598F, -0.147573F, -0.72156F, -0.628244F, - -0.624474F, -0.332783F, -0.139029F, -0.0152179F, -0.0434418F, -1.14663F, - -1.11873F, -0.740136F, -0.698193F, -0.745926F, -0.462701F, -0.239961F, - -0.054289F, -0.0233088F, -1.10479F, -1.06733F, -0.702098F, -0.487249F, - -0.396776F, -0.0599214F, -0.0515236F, -0.0137974F, -0.0048505F, -1.01226F, - -0.862739F, -0.775281F, -0.694199F, -0.721518F, -0.740561F, -0.45865F, - -0.432423F, -0.330589F, -0.288549F, -0.0437556F, -0.0386972F, -0.933682F, - -0.628206F, -0.604023F, -0.514599F, -0.159614F, -0.0620124F, -0.114123F, - -0.123493F, -0.038238F, -0.610986F, -0.0378951F, -0.0379262F, -0.0310817F, - -0.0174352F, -0.0146748F, -1.06818F, -0.604071F, -0.0174294F, -0.0173028F, - -0.0328579F, -0.0207511F, -0.0188552F, -0.692592F, -0.602998F, -0.024959F, - -0.620104F, -0.765879F, -0.0384606F, -0.0202286F, -1.10356F, -0.0377225F, - -0.0339773F, -0.0432154F, -0.0195253F, -0.841512F, -0.0266286F, -0.00875218F, - -0.0312316F, -0.0254587F, -1.2832F, -0.692884F, -0.0226731F, -0.857179F, - -1.3587F, -1.3599F, -1.35244F, -1.34614F, -0.89314F, -0.742036F, 0.411266F, - -0.0302467F, -1.45776F, -1.38885F, -1.28756F, -0.161118F, -0.153196F, - -0.119141F, -0.0359721F, -1.46626F, -1.48247F, -1.38254F, -1.32066F, - -0.864611F, -0.845246F, -0.739068F, -0.676535F, -0.66675F, -0.442695F, - -0.391301F, -0.158095F, 0.342252F, 0.115899F, 0.576726F, -0.0267287F, - -0.0328233F, -0.0209763F, -1.46526F, -1.67529F, -0.861689F, -0.798805F, - -0.767884F, -0.715116F, -0.683704F, -0.387434F, -0.188481F, -1.45864F, - -1.22392F, -0.82664F, -0.740486F, -0.422408F, -0.127448F, 0.566409F, - -0.872755F, -0.849629F, -0.740958F, -0.682162F, -0.646423F, -0.546234F, - -1.16369F, -1.13723F, -0.905346F, -0.125903F, -0.956294F, -1.16458F, - -0.154672F, -0.776028F, -0.0790706F, -0.0779506F, 0.395983F, -0.0260377F, - -0.0158444F, -1.47814F, -1.45657F, -0.148564F, 0.386884F, 0.577912F, - 0.564111F, -0.0276357F, -1.25584F, -1.02684F, -0.418093F, -0.237564F, - -0.186347F, -0.221658F, -0.155992F, 0.327493F, 0.335995F, 0.389173F, - 0.559806F, 0.52137F, -0.0298225F, -1.37609F, -1.00901F, -0.240564F, - -0.735626F, -0.441578F, -0.416451F, -0.420437F, -0.423137F, -0.3813F, - -0.245468F, -0.685997F, -0.160248F, -0.981258F, 0.0791523F, -1.43981F, - -0.302844F, -1.46131F, -0.682008F, -0.266043F, -0.149251F, -1.2272F, - -0.420672F, -0.544614F, -0.682475F, -0.103973F, -0.0490736F, -1.26505F, - -0.737802F, -0.608426F, 0.586795F, -0.15342F, -0.150769F, 0.664293F, - 0.640373F, -0.484508F, -0.149743F, -0.121848F, 2.11713F, -0.395654F, - 0.81009F, 0.580583F, -0.505161F, -0.42674F, -0.349252F, -0.591309F, -1.0742F, - -0.498253F, -0.489057F, -0.39561F, -0.385439F, -0.380145F, -0.34326F, - 0.839723F, -0.5438F, -0.519804F, -0.857254F, -0.494518F, -0.47203F, - -0.43586F, -0.43006F, -0.42273F, -0.399731F, 0.818583F, -0.531905F, - -0.566031F, -0.398207F, -0.35172F, -0.339543F, -0.480287F, -0.565915F, - -0.459814F, -0.934261F, -0.663392F, -0.651955F, -0.598641F, 0.932012F, - -0.99966F, -0.711493F, -0.54285F, -0.507529F, -0.469312F, -0.50916F, - -0.46241F, -0.36687F, -0.418674F, -1.16275F, -0.994881F, -0.494759F, - -0.48379F, -0.437091F, -0.438336F, -0.432714F, -0.427711F, -0.424809F, - -0.391543F, 0.533014F, 0.815235F, -0.522165F, -0.511833F, -0.498353F, - -0.494078F, -0.555778F, -0.464312F, -0.472731F, -0.432702F, -0.426185F, - -0.34857F, -0.395449F, -0.387499F, -0.381171F, -0.386426F, -0.376125F, - -0.372386F, -0.210468F, -0.160079F, 0.869083F, -0.571932F, -0.52581F, - -0.510296F, -0.503406F, -0.927578F, -0.457102F, -0.388347F, -0.368593F, - -0.361737F, -0.353612F, -2.1832F, -0.522876F, -0.515822F, -0.501271F, - -0.472048F, -0.462885F, -0.438083F, -0.435136F, -1.48967F, -1.09932F, - -1.01138F, -0.585784F, -0.550248F, -0.532488F, -0.515641F, -0.365358F, - -0.339195F, 0.0337876F, 0.047966F, 0.111709F, 0.250601F, -1.48557F, - -1.11514F, -1.02106F, -1.04646F, -0.720585F, -0.65564F, -0.396206F, - -0.352246F, -0.278684F, 0.0344846F, 0.0908172F, 0.262438F, 0.255492F, - 0.525438F, 0.55586F, 0.613358F, 0.176925F, -1.44958F, -1.23636F, -1.10607F, - -0.925335F, -0.512426F, -0.72961F, -0.707717F, -0.699209F, -0.693399F, - -0.682276F, -0.570414F, -0.540423F, -0.454275F, -0.426838F, -0.147053F, - -0.0889604F, 0.549186F, 0.626102F, -0.512054F, -0.488174F, -0.383486F, - -0.367845F, 0.577889F, -1.39622F, -1.02092F, -0.538229F, -0.533453F, - -0.531039F, -0.526646F, -0.52039F, -0.507486F, -0.504946F, -0.422904F, - -0.40093F, -0.374603F, -0.348754F, 0.525538F, -1.29844F, -0.546237F, - -0.518568F, -0.48898F, -0.472363F, -0.471976F, -0.45862F, -0.426284F, - -0.435083F, -0.438474F, -0.0183294F, -0.351773F, -0.512702F, -0.492736F, - -0.488477F, -0.435444F, -0.402614F, -0.370279F, -1.31455F, -1.17675F, - -1.15086F, -1.1045F, -1.099F, -1.05131F, -0.859535F, -0.860853F, -0.829765F, - -0.714878F, -0.501857F, -0.50413F, -0.371598F, -0.347042F, -0.306764F, - -0.294679F, -0.0495947F, 0.0606752F, 0.59879F, 0.596131F, -1.51614F, - -1.19035F, -1.1626F, -1.16006F, -1.00648F, -1.03963F, -0.945975F, -0.549776F, - -0.142212F, -0.097456F, 0.0487391F, 0.277527F, 0.33118F, -1.38941F, - -1.48623F, -1.36735F, -1.26277F, -1.24016F, -1.24312F, -0.993862F, - -0.995109F, -0.748392F, -0.815014F, -0.725876F, -0.583217F, -0.579108F, - -0.480843F, -0.452114F, -0.340297F, -0.241007F, -0.241634F, 0.328648F, - 0.594717F, 0.560788F, -1.47766F, -1.41054F, -1.23403F, 0.238568F, -1.50755F, - -1.45513F, -1.30047F, -1.24918F, -1.22677F, -0.102998F, 0.0223884F, 0.16106F, - 0.562332F, -1.0291F, -0.553326F, -0.448236F, -0.383027F, -0.36662F, - -1.28928F, -0.666512F, -0.57073F, -1.48443F, -1.29396F, -1.23867F, -1.24267F, - -1.1755F, -1.15272F, -1.04157F, -0.994692F, -0.996146F, -0.9868F, -0.991639F, - -0.856408F, -0.850591F, -0.793351F, -0.71873F, -0.596484F, -0.547663F, - -0.537219F, -0.511057F, -0.459149F, -0.41649F, -0.419086F, -0.408875F, - -0.401477F, -0.291133F, -0.283724F, -0.282026F, -0.26617F, -0.22499F, - -0.231442F, -0.225876F, -0.19962F, -0.114514F, 0.0435929F, 0.116939F, - 0.333334F, 0.384631F, 0.374156F, 0.573945F, 0.628547F, -1.48877F, -1.3802F, - -1.3821F, -1.43762F, -1.34971F, -1.25869F, -1.15797F, -1.10908F, -0.994266F, - -0.99251F, -0.996599F, -0.854683F, -0.978259F, -0.786958F, -0.785655F, - -0.584084F, -0.53838F, -0.525973F, -0.502978F, -0.399479F, -0.280422F, - -0.263091F, 0.377147F, 0.381934F, 0.424917F, 0.583385F, 0.587743F, 0.564297F, - -1.45115F, -1.42859F, -1.4262F, -1.34491F, -1.24295F, -1.09562F, -0.995335F, - -0.858032F, -0.756838F, -0.865114F, -0.585666F, -0.533666F, -0.37145F, - -0.361013F, -0.317807F, -0.280453F, -0.214892F, -0.236888F, 0.32106F, - 0.380882F, 0.417784F, 0.445342F, 0.496491F, 0.801412F, -1.19646F, -0.607565F, - -0.633584F, -0.55411F, -0.08413F, 0.325979F, 0.318319F, 0.573016F, 0.557505F, - 0.717187F, -1.50417F, -1.49802F, -1.24158F, -0.995983F, -0.944978F, - -0.783769F, -0.41516F, -0.24477F, -0.217631F, 0.277388F, 0.600068F, -1.2422F, - -0.583561F, -0.353389F, -0.286757F, 0.57981F, -0.67697F, -1.1695F, -1.16332F, - -1.11336F, -1.10967F, -1.11037F, -0.99988F, -1.0011F, -0.984209F, -0.948138F, - -0.93513F, -0.933435F, -0.878348F, -0.772918F, -0.652912F, -0.580978F, - -0.567228F, -0.552733F, -0.526588F, -0.53458F, -0.528813F, -0.505705F, - -0.476465F, -0.475622F, -0.463417F, -0.439937F, -0.401487F, -0.393884F, - -0.374168F, -0.360809F, -0.352552F, -0.279763F, -0.267065F, -0.249323F, - -0.233572F, -0.21544F, -0.173789F, 0.595992F, 0.598669F, 0.589001F, - -1.48206F, -1.44724F, -1.37162F, -1.11594F, -0.955699F, -0.889167F, - -0.838896F, -0.58615F, -0.541237F, -0.54841F, -0.521448F, -0.52627F, - -0.516366F, -0.402497F, -0.329986F, -0.281366F, -0.280346F, -0.271072F, - -0.220093F, -0.187795F, -0.0859848F, 0.066074F, 0.0985655F, 0.241208F, - 0.608603F, 0.58695F, 0.795582F, -1.4644F, -1.03974F, -0.798307F, -0.897869F, - -0.542699F, -0.549084F, -0.509088F, -0.402949F, -0.399018F, -0.355088F, - -0.218072F, -0.189386F, 0.315475F, 0.33351F, -0.00251075F, 0.381521F, - 0.64038F, 0.590014F, 0.580348F, 0.587515F, 0.581602F, 0.575504F, 0.833837F, - -1.41415F, -1.35547F, -1.25735F, -1.07219F, -0.997863F, -1.2148F, -0.883864F, - -0.371869F, -0.344744F, -0.3407F, -0.195194F, -0.0744407F, 0.041299F, - 0.597505F, 0.586491F, 0.797696F, -1.42038F, -1.37232F, -1.3502F, -1.23908F, - -1.32341F, -1.03731F, -0.68925F, -0.46002F, 0.176819F, -0.0746491F, - 0.252965F, 0.41199F, -1.26185F, -0.936364F, -0.212364F, -0.168603F, - 0.106179F, 0.549266F, -1.22823F, -0.613535F, -0.439138F, -0.366231F, - 0.192192F, 0.0361184F, -0.215881F, -0.137526F, -0.943743F, 0.0746686F, - -0.851795F, -0.557611F, -0.408538F, -0.326992F, -1.4659F, -1.01436F, - -0.430368F, -0.992021F, -0.124964F, 0.00469787F, -0.584789F, -0.534866F, - -0.706548F, -0.553159F, -0.22925F, -0.183578F, -1.09536F, -1.00163F, - 0.572114F, -1.09622F, -0.375419F, -0.920547F, -0.40405F, -0.39262F, - -0.246166F, -1.16425F, -1.18882F, -0.673033F, -0.393215F, -0.330246F, - 0.664147F, -1.29364F, -0.232206F, -0.44919F, -0.186947F, -1.11094F, -0.6252F, - -0.467343F, -0.999082F, -0.999521F, -0.549771F, -0.399845F, -0.742248F, - -0.0386018F, -0.0246902F, -1.16843F, -0.452177F, -0.247858F, -1.28846F, - -0.412576F, -0.329591F, -0.608304F, -0.226301F, -0.812266F, -0.809136F, - -0.580184F, -0.494537F, -0.480582F, -0.464955F, -0.47967F, -0.477166F, - -0.178678F, 0.121164F, -1.16666F, -0.381489F, -0.280206F, 0.00499551F, - -0.00954968F, -0.0163079F, -0.444907F, -0.177694F, -0.0908236F, -0.0978387F, - -0.00784763F, -0.0278555F, -1.42898F, -0.559883F, -0.432338F, -0.334622F, - -0.0276069F, -1.46999F, -1.42843F, -1.2055F, -0.702189F, -0.657009F, - -0.695363F, -0.437104F, -0.856885F, -0.84605F, -0.844463F, -0.607557F, - -0.474425F, -0.430931F, -0.406126F, -0.378874F, -0.114847F, 0.0184218F, - -0.0481231F, 0.0295664F, -0.0386407F, -0.0159455F, -0.0111628F, -1.05072F, - -0.574935F, -0.537958F, -0.503415F, -0.507262F, -0.504659F, -0.450358F, - -0.413277F, -0.242822F, -0.049474F, 0.036363F, -0.0245871F, -0.00921581F, - -0.0118579F, -0.914982F, -0.606884F, -0.55775F, -0.510585F, -0.477591F, - -0.476957F, -0.475707F, -0.423034F, -0.422492F, -0.421819F, -0.406125F, - -0.0990578F, -0.0211226F, -0.197148F, -0.155952F, -0.147744F, -0.037546F, - -0.0178019F, -0.586627F, -0.402983F, -0.342915F, -0.329317F, -0.201595F, - -0.0375272F, 0.0225311F, -0.199223F, -0.161477F, -1.32615F, -1.16811F, - -0.2407F, -0.0362817F, -0.00934883F, -0.892458F, -0.825663F, -0.448914F, - -0.327245F, -0.330274F, -0.977509F, -0.374899F, -0.186352F, -0.852845F, - -0.622997F, -0.580891F, -0.529246F, -0.505247F, -0.479196F, -0.462877F, - -0.45187F, -0.404656F, -0.4023F, -0.224194F, -0.106792F, -0.204659F, - -0.163886F, -0.0376837F, -0.0202866F, -0.583678F, -0.604482F, -0.475249F, - -0.489618F, -0.46358F, -0.47045F, -0.465048F, -0.410833F, -0.407792F, - -0.40451F, -0.0546614F, 0.0101367F, -0.0475919F, 0.255428F, -0.155804F, - -1.02362F, -0.984652F, -0.994292F, -0.868485F, -0.582222F, -0.496401F, - -0.504175F, -0.453626F, -0.184149F, -1.20089F, -0.871449F, -0.35051F, - -0.17171F, -1.55839F, -1.16532F, -0.431128F, -0.0165888F, -0.0180002F, - -0.487973F, 0.00934632F, -1.29569F, -0.181062F, -0.969227F, -0.87064F, - -0.870385F, -0.731342F, -0.618219F, -0.52571F, -0.503781F, -0.474912F, - -0.468616F, -0.428048F, -0.423481F, -0.376024F, -0.168347F, -0.0458108F, - -0.0171624F, -0.0064916F, 0.0359132F, -0.0153007F, -1.14671F, -0.967109F, - -0.889035F, -0.856053F, -0.873146F, -0.683148F, -0.621446F, -0.617757F, - -0.587187F, -0.513076F, -0.456081F, -0.479596F, -0.449995F, -0.37836F, - -0.114686F, -0.0755788F, -0.205993F, -0.608263F, -0.61409F, -0.492257F, - -0.463059F, -0.427264F, -0.378662F, -0.150306F, -0.177415F, -0.0126943F, - -0.204284F, -0.142595F, -0.0624395F, -0.064607F, -1.3746F, -0.546446F, - -0.751129F, -0.526188F, -0.463317F, -0.491649F, -0.47662F, -0.434709F, - -0.447351F, -0.400009F, -0.277476F, 0.0187986F, -0.0277157F, -1.02599F, - -0.499704F, -0.588141F, -0.553231F, -0.495496F, -0.4274F, -0.0824451F, - -0.0237849F, -0.908425F, -0.351512F, -0.347916F, -0.0839708F, -0.037324F, - -1.28265F, -1.0704F, -0.821062F, -0.438566F, -0.174587F, 0.432191F, 0.3627F, - -1.81115F, 0.00096451F, -1.84199F, -0.610607F, -0.505357F, -0.0134829F, - -0.0382552F, -0.0822838F, -0.818554F, -0.821816F, -0.396976F, 0.0122168F, - -0.418458F, -0.216045F, -1.83735F, -1.82318F, -1.82885F, -1.7808F, -1.82345F, - -1.83938F, -1.83156F, -1.81002F, -1.81682F, -1.79266F, -0.477466F, - -0.00951892F, -0.0104582F, -1.80394F, -2.34145F, -1.81158F, -1.99495F, - -1.79892F, -1.82518F, -1.7987F, -0.476922F, -0.0519167F, -1.83043F, - -0.473554F, -0.589894F, -0.43888F, -0.640395F, -0.284761F, -2.10371F, - -2.13265F, -1.8022F, -1.85119F, -2.64544F, -1.8173F, -1.78294F, -1.82307F, - -1.83545F, -1.79303F, -1.85326F, -0.479095F, -0.126146F, -0.0144962F, - -2.18223F, -1.79905F, -2.6704F, -1.81856F, -3.47169F, -2.63925F, -1.81107F, - -1.81204F, -1.81511F, -1.82547F, -1.83302F, -1.75372F, -1.8128F, -1.80919F, - -1.82841F, -1.7691F, -1.82529F, -1.82671F, -1.84924F, -1.8357F, -1.81833F, - -1.86196F, -1.7982F, -0.575404F, -0.474843F, 0.044271F, -2.23915F, -1.96168F, - -1.8901F, -1.85331F, -1.8474F, -1.81904F, -0.667941F, -0.47433F, -0.452202F, - -0.425308F, -0.291082F, -0.168231F, -0.0368526F, -0.050332F, -0.0560913F, - -0.361981F, -0.562631F, -0.47116F, -0.404366F, -0.19978F, -0.0253806F, - -0.592814F, -0.161125F, -0.009358F, -0.919414F, -0.457207F, -0.979063F, - -0.268378F, -3.70109F, -12.4605F, -2.44126F, -2.4114F, -2.70381F, -1.81373F, - -1.83276F, -1.83474F, -2.43324F, -1.82105F, -1.83018F, -1.82845F, -1.83646F, - -1.8352F, -1.84162F, -0.474474F, -2.4541F, -2.63256F, -1.81556F, -1.85462F, - -2.37715F, -1.8277F, -1.83504F, -1.80566F, -1.78373F, -0.83243F, -0.468631F, - -0.2309F, -2.22081F, -2.36792F, -1.81048F, -3.67646F, -1.81293F, -1.7794F, - -1.92705F, -1.79155F, -1.85484F, -0.236362F, -0.0761755F, 0.00103268F, - -2.48582F, -3.66606F, -0.383377F, -0.0787035F, -0.0407372F, -0.00456171F, - -0.450308F, -0.443249F, -0.0747159F, 0.0620367F, -0.191632F, 0.0061753F, - -0.936908F, -6.84684F, -3.0607F, -0.507373F, -0.13326F, -0.474112F, - -0.405732F, 0.00709987F, -2.58318F, -0.669903F, -0.605017F, -0.472717F, - -0.453172F, -0.367107F, -0.287095F, -0.123122F, 0.0171588F, 0.00660647F, - 0.0364742F, -0.372712F, 0.281656F, -1.4577F, -0.624616F, -1.85609F, - -1.85343F, -0.819799F, 0.0294556F, -1.90528F, -0.577024F, -1.90437F, - -2.14269F, -1.8342F, -0.552273F, -0.599337F, -0.321468F, -0.23442F, - -0.0645556F, -0.816281F, -0.393371F, -0.127495F, -0.758462F, -0.354096F, - -0.427471F, -0.177579F, -0.163262F, 0.0181231F, -0.518968F, -0.51888F, - -0.57733F, -0.03656F, -0.035858F, -0.537695F, -0.540683F, -0.498819F, - -0.392075F, -0.463602F, -1.47267F, -1.45388F, -1.80267F, -0.818014F, - -0.609274F, -0.467443F, -0.46449F, -0.449532F, -0.38772F, -0.102106F, - 0.131374F, -0.467761F, -0.386377F, -0.785361F, -1.46423F, -0.374726F, - -0.491884F, -0.382819F, -0.0678497F, -0.491862F, -0.356869F, -1.76287F, - -1.80883F, -1.7439F, -1.81117F, -0.581189F, -0.602801F, -0.594899F, - -0.00279571F, -1.0168F, -0.647739F, -0.59568F, -0.47538F, -0.118716F, - -0.00216691F, -0.640448F, -0.393371F, -0.355828F, -1.01473F, -0.98181F, - -0.754524F, -0.597017F, -0.199622F, -0.207957F, 0.0408074F, -1.72445F, - -1.81333F, -1.97255F, -1.82589F, -1.85413F, -1.83359F, -1.82491F, -0.832953F, - -0.519382F, -0.578798F, -0.463971F, -1.11748F, -1.34061F, -0.646843F, - -0.593742F, -0.541951F, -0.395343F, -0.394382F, -0.35063F, -0.352521F, - -0.0946502F, -0.0736678F, -1.06987F, -0.224043F, -0.115012F, -0.659179F, - -0.511287F, -0.464147F, -0.359436F, -0.0664837F, -0.524598F, 0.0704247F, - -1.37318F, 0.0558682F, -1.84588F, -1.79114F, -1.89439F, -1.78588F, -1.81203F, - -1.80023F, -0.888499F, -0.808787F, -0.523529F, -0.627915F, -0.612394F, - -0.479267F, -0.519592F, -0.476546F, -0.477034F, -0.475747F, -0.476806F, - -0.413589F, -0.401593F, -0.390667F, -0.118715F, -0.0673952F, -0.247054F, - -0.0188193F, -0.0126074F, -0.0066377F, -0.0716394F, -0.0559167F, -0.0698618F, - -1.84993F, -1.03708F, -0.968322F, -1.5049F, -1.50514F, -1.04916F, -0.66207F, - -0.732395F, -0.665499F, -0.605086F, -0.505295F, -0.528773F, -0.469787F, - -0.456512F, -0.421758F, -0.403008F, -0.405503F, -0.369202F, -0.228326F, - -0.0806051F, -0.0812527F, -0.263389F, -0.0186193F, -0.0173653F, 0.0927227F, - -0.241729F, -0.225872F, -0.0901326F, -0.0716179F, -0.0689564F, -0.159168F, - -0.142594F, -0.0190412F, -0.101301F, -0.0780631F, -1.74107F, -1.02534F, - -1.07517F, -0.951282F, -1.51434F, -1.54426F, -1.16655F, -0.764379F, - -0.783646F, -0.790804F, -0.679358F, -0.693331F, -0.740136F, -0.833192F, - -0.550808F, -0.561402F, -0.545098F, -0.573652F, -0.638314F, -0.660821F, - -0.589471F, -0.480294F, -0.462114F, -0.437751F, -0.424499F, -0.425339F, - -0.388329F, -0.334892F, -0.390154F, -0.329366F, -0.278701F, -0.235521F, - -0.293944F, -0.232072F, 0.0224128F, -0.0225987F, -0.104442F, 0.0229512F, - 0.145341F, -0.249848F, -0.0957105F, -0.121076F, -0.0159129F, 0.0086495F, - -0.0576147F, -0.0626633F, -0.0604048F, -0.0904895F, -2.02633F, -0.847534F, - -0.933709F, -1.46209F, -0.740936F, -0.773291F, -0.716285F, -0.547527F, - -0.522346F, -0.420705F, -0.382525F, -0.348325F, -0.367653F, -0.0214407F, - -0.13211F, -0.101277F, -0.150063F, 0.0361229F, -0.044141F, -0.126717F, - -0.058855F, -1.04717F, -1.0776F, -1.47658F, -1.34959F, -1.61141F, -1.03001F, - -1.22803F, -0.813897F, -0.69765F, -0.700897F, -0.594563F, -0.601436F, - -0.551015F, -0.615241F, -0.431378F, -0.39396F, -0.385712F, -0.344733F, - -0.220768F, -0.347787F, -0.348169F, -0.146818F, -0.177393F, 6.1327F, - -0.254806F, -0.0572476F, -0.0718106F, -0.115806F, -0.0374026F, -0.114349F, - -1.05201F, -1.26826F, -0.687803F, -0.732441F, -0.543194F, -0.47353F, - -0.390399F, -0.218366F, -0.153608F, -0.177637F, -0.115749F, -0.0709013F, - -0.010508F, -0.854394F, -1.06885F, -1.12977F, -0.37461F, 0.458937F, - 0.130239F, -0.121577F, -0.112649F, 0.026151F, 0.430548F, -0.00499517F, - 0.0284549F, -1.37048F, -1.8455F, -4.60889F, -1.86212F, -1.80892F, -1.79974F, - -1.82755F, -0.832882F, -0.492435F, -0.39765F, -0.333803F, -0.10254F, - -0.117077F, -0.0592302F, -0.180833F, -1.82715F, -0.885051F, -1.41072F, - -1.05199F, -0.62096F, -0.543208F, -0.47139F, -0.391744F, -0.33506F, - -0.396321F, -0.332169F, -0.382869F, -0.279755F, -0.367703F, -0.352346F, - -0.10037F, -0.0663186F, 0.0504705F, -0.202882F, -0.0249305F, -0.117444F, - -0.922099F, -1.38821F, -1.35457F, -1.36766F, -0.749822F, -0.588312F, - -0.500459F, -0.503066F, -0.456294F, -0.428643F, -0.3873F, -0.349357F, - -0.120491F, -0.118063F, -0.292252F, -0.184585F, -0.17132F, -1.22516F, - -1.43584F, -0.876398F, -1.32528F, -0.539242F, -0.733023F, -0.462879F, - -0.145364F, -0.0730918F, -0.993917F, -1.80104F, -1.00633F, -1.34679F, - -1.46778F, -1.0783F, -0.638214F, -0.541951F, -0.445383F, -0.391033F, - -0.371066F, -0.361535F, -0.137456F, -0.143747F, 0.00723361F, -1.05443F, - -1.49359F, -1.44751F, -1.39041F, -0.792746F, -0.678408F, -0.162524F, - -0.170184F, -0.661091F, -0.595632F, -0.483763F, -0.119906F, -1.11277F, - -0.424249F, -0.103749F, -0.0791263F, 0.118457F, -1.85944F, -0.536096F, - -0.476898F, -0.458841F, -0.113228F, -1.03228F, -0.782362F, -0.521201F, - -0.669026F, -0.498014F, -0.471089F, -0.491589F, -0.398253F, -0.368802F, - -1.0943F, -1.05106F, -0.531957F, -0.459948F, -0.479768F, -0.383222F, - -0.395937F, -0.400102F, -0.384766F, -0.160661F, -0.0796598F, -0.0370799F, - -1.65272F, -0.643461F, -0.644779F, -0.535155F, -0.573345F, -0.46105F, - -0.395379F, -0.391209F, -0.222597F, -0.242473F, -0.373304F, -0.352591F, - -0.360082F, -0.0385094F, -0.123712F, -1.42398F, -1.44054F, -0.546831F, - -0.441651F, -0.356134F, -0.388666F, -0.358486F, -0.354704F, -0.00993529F, - -1.19778F, -1.69974F, -0.0630096F, -1.22217F, -0.517295F, -0.458462F, - -0.397939F, -0.359822F, -0.000143568F, 0.0122863F, 0.0203662F, -0.171884F, - -1.10835F, -0.594589F, -0.525234F, -0.514842F, -0.497564F, -0.457835F, - -0.477041F, -0.434176F, -0.360888F, 0.0287833F, -0.481759F, -0.431259F, - -0.331589F, -0.23959F, -0.207601F, -0.00116004F, 0.0571601F, -0.020089F, - -0.936874F, -0.541512F, -0.541096F, -0.5431F, -0.461418F, -0.433683F, - -0.377054F, -0.240863F, -0.119447F, -0.576214F, -0.422766F, -0.351866F, - -0.025466F, -0.0404094F, -1.45483F, -0.156031F, -0.0465803F, -0.854461F, - -0.90292F, -0.496848F, -0.474976F, -0.175505F, -0.546926F, -0.533791F, - -0.396865F, -0.386289F, -0.379913F, -0.348259F, -0.126372F, -0.0487411F, - -0.220137F, -0.0165328F, -0.0108233F, -0.871759F, -0.76414F, -1.03898F, - -0.490697F, -0.47136F, -0.432552F, -0.39394F, -0.424896F, -0.424536F, - -0.399788F, -0.243375F, -0.0119106F, -0.149145F, -0.88462F, -0.242194F, - -0.0249619F, -0.0836149F, -0.0335732F, -0.396677F, -0.39204F, -0.396095F, - -0.387719F, -0.358337F, -0.177489F, -0.312731F, -0.129554F, -0.823691F, - -0.570807F, -0.0295948F, -0.156731F, 0.0701657F, -1.29108F, -0.484026F, - -0.98496F, -0.384145F, -0.238097F, -0.125111F, -0.974924F, -0.532507F, - -0.492003F, -0.364645F, -0.491459F, -0.366434F, -0.105936F, -0.0233599F, - -0.0271898F, -0.54365F, -0.46957F, -0.46048F, -0.38529F, -0.242884F, - 0.00146083F, -0.029111F, -0.171686F, -0.0325053F, -0.840696F, -0.121438F, - -0.0329663F, -0.833605F, -0.805799F, -1.47566F, -0.186687F, -0.967304F, - -0.969129F, -0.958755F, -0.36418F, -0.374486F, -0.160061F, -0.771137F, - -0.769478F, -0.615238F, -0.398765F, -0.164252F, -0.367096F, -0.366974F, - -0.361741F, -0.795427F, -0.775111F, -1.397F, -0.889004F, -0.490406F, - -0.418865F, -0.434836F, -0.354093F, -0.398667F, -0.398385F, -0.3637F, - -0.351193F, -0.0401246F, -0.0201858F, -0.769018F, -0.92188F, -0.495724F, - -0.478414F, -0.358485F, -0.154502F, -0.0266831F, -0.189695F, -0.0358973F, - -0.0384504F, -0.167659F, -0.395523F, -0.193655F, -0.0516169F, -0.0561287F, - -0.847626F, -0.628042F, -0.58264F, -0.807671F, -0.413611F, -0.21126F, - -0.44849F, -0.459169F, -0.397505F, -0.398545F, -0.3863F, -0.368017F, - -0.0850907F, -0.149115F, -0.184713F, -0.176194F, -1.47531F, -1.2979F, - -0.543757F, -0.522045F, -0.480196F, -0.459059F, -0.459637F, -0.42353F, - -0.398535F, -0.111984F, -0.591005F, -0.460364F, -0.14836F, -0.463341F, - -0.0294716F, -0.182496F, -1.49168F, -0.324359F, -0.316798F, -0.128162F, - -1.38468F, -0.261496F, -0.262989F, -0.349939F, -0.118412F, -0.0219736F, - -0.523034F, -0.609429F, -0.582633F, -0.588984F, -0.479622F, -0.435998F, - -0.436486F, -0.422876F, -0.207863F, -0.223564F, -0.209154F, -0.162309F, - 0.317463F, -0.0167752F, -0.0274087F, -0.0198908F, -0.186683F, -1.31518F, - -1.29189F, -1.24265F, -0.899865F, -0.544456F, -0.459344F, -0.377206F, - -0.373754F, -0.399201F, -0.0455146F, -0.0313352F, -1.19627F, -0.859476F, - -0.728789F, -1.32421F, -0.422801F, -0.0500276F, -0.00906393F, -1.54055F, - -1.28793F, -1.43093F, -0.548406F, -0.106225F, -1.38579F, -1.50144F, - -1.16905F, -0.795181F, -0.766253F, -0.398999F, -0.398288F, -0.147645F, - 0.325997F, 0.321129F, 0.586894F, -1.37714F, -1.28775F, -1.24206F, -1.3125F, - -1.00887F, -0.745192F, -0.719359F, -0.301633F, 0.332219F, 0.37152F, - 0.563819F, -1.47923F, -1.43345F, -1.37992F, -1.25654F, -0.870752F, - -0.758775F, -0.666471F, -0.714443F, -0.655435F, -0.119766F, 0.0611408F, - 0.316053F, -1.55566F, -1.43428F, -1.42788F, -1.20353F, -1.25917F, -0.815296F, - -0.737044F, -0.834838F, -0.335317F, -0.374427F, -0.300801F, -0.283011F, - -0.386434F, -1.55774F, -1.28308F, -0.762639F, -0.822814F, -0.701869F, - -0.369671F, 0.606078F, -0.847673F, -0.842085F, -0.600484F, 0.12158F, - -1.54141F, -1.49592F, -1.2252F, -0.659009F, 0.194901F, -0.554676F, -1.38305F, - -1.36827F, -1.38321F, -1.05505F, -0.771143F, -0.525458F, -0.425919F, - -0.307518F, -0.38835F, -0.387661F, -0.387971F, -0.382204F, -0.0892048F, - -0.35913F, 0.0144313F, 0.377088F, 0.545964F, 0.556682F, 0.599251F, 0.596557F, - 0.587338F, 0.50982F, -1.26239F, -1.43797F, -0.859998F, -0.817679F, - -0.747547F, -0.787017F, -0.684648F, -0.543962F, -0.784889F, -0.524936F, - -0.706761F, -0.524492F, -0.505503F, -0.421924F, -0.268838F, -0.243195F, - -0.399166F, -0.234399F, -0.384013F, -0.381482F, -0.36217F, 0.353198F, - 0.382703F, 0.555828F, 0.529023F, 0.592506F, 0.574155F, -1.24591F, -1.24649F, - -1.02797F, -0.86592F, -0.745018F, -0.545319F, -0.718077F, -0.730197F, - -0.728097F, -0.672528F, -0.478595F, -0.470976F, -0.457443F, -0.316803F, - -0.317739F, -0.421394F, -0.240304F, -0.387632F, -0.179428F, -0.144505F, - -0.209185F, -0.360919F, -0.127138F, 0.4022F, 0.4904F, 0.558869F, 0.589067F, - -1.51613F, -1.24429F, -1.52814F, -1.15998F, -0.774128F, -0.803601F, - -0.702067F, -0.42087F, -0.271289F, -0.231312F, -0.398837F, -0.398661F, - -0.385391F, -0.00216129F, 0.591811F, 0.586806F, 0.590216F, 0.629662F, - -1.48406F, -1.36452F, -1.192F, -1.24655F, -1.13164F, -1.13394F, -1.05839F, - -0.776496F, -0.541605F, -0.125901F, 0.384529F, 0.539936F, 0.483367F, - 0.623783F, -1.83804F, -1.27919F, -0.85103F, -0.760166F, -2.59243F, - -0.363961F, -0.258268F, -1.28457F, -0.582506F, 0.0308779F, -0.903757F, - -0.529469F, -1.33884F, -1.10317F, -1.00202F, -0.766173F, -0.752998F, - -0.649184F, -0.586764F, -0.470898F, -0.485915F, -0.393504F, -0.326846F, - 0.104423F, 0.255638F, 0.356685F, 0.39409F, 0.571965F, 0.563591F, 0.591778F, - 0.58077F, 0.593256F, -1.51992F, -0.740241F, -0.735358F, -0.565278F, - -0.307433F, -0.375954F, -0.359676F, -0.26068F, -0.264059F, -0.119356F, - 0.120138F, -0.348814F, 0.254519F, 0.326031F, 0.364631F, 0.603544F, 0.505203F, - -1.47332F, -1.25382F, -1.07346F, -0.83662F, -0.74699F, -0.739279F, -0.71113F, - -0.756621F, -0.745614F, -0.499189F, -0.70411F, -0.488992F, -0.566894F, - -0.565112F, -0.477511F, -0.496864F, -0.46579F, -0.475882F, -0.389677F, - -0.26159F, -0.335708F, -0.295682F, -0.299612F, -0.157306F, -0.143607F, - 0.361721F, 0.353195F, 0.563051F, 0.657977F, 0.646256F, -1.23812F, -1.23696F, - -0.801868F, -0.691792F, -0.53216F, -0.64746F, -0.665366F, -0.448604F, - -0.458053F, -0.372496F, -0.433473F, -0.422502F, -0.386379F, -0.364325F, - -0.359074F, -0.354087F, 0.33487F, 0.527033F, 0.740408F, -1.45878F, -1.69589F, - -1.31679F, -1.28277F, -1.2977F, -0.837039F, -0.733011F, -0.541089F, - -0.446855F, -0.439893F, -0.320484F, -0.323642F, -0.289849F, -0.398096F, - -0.0277849F, 0.426106F, 0.508648F, 0.763081F, -1.35831F, -1.50999F, - -0.0843191F, 0.266588F, 0.513645F, -0.426687F, 0.152558F, 0.0197423F, - -1.8032F, -1.44273F, -1.42033F, -1.29249F, -0.390947F, -0.0844869F, - 0.498027F, 0.364273F, 0.568075F, -1.36521F, -0.544143F, -0.734067F, - -0.567615F, -0.154469F, -0.376469F, 0.350717F, 0.576571F, 0.600171F, - -1.4803F, -1.4398F, -1.01847F, -0.891598F, -0.57259F, -0.775912F, -0.767926F, - -0.705229F, -0.488568F, -0.341453F, -0.399933F, -0.148446F, -0.399229F, - -0.38741F, -0.373825F, -0.367086F, 0.315399F, 0.364056F, 0.552943F, - 0.592136F, 0.575547F, 0.516296F, 0.535364F, 0.647397F, -1.51708F, -1.4349F, - -1.47661F, -1.41585F, -1.24566F, -0.704682F, -0.434669F, -0.281719F, - -0.210375F, -0.147662F, -0.0720154F, -0.371234F, 0.330303F, -1.48197F, - -1.3549F, -1.21521F, -1.28867F, -0.779529F, -0.224411F, -0.207804F, - -0.133503F, -0.106204F, -0.385966F, -0.345873F, -0.77328F, -0.210987F, - -0.394998F, 0.704258F, 0.629327F, -1.32375F, -0.574399F, -0.0581115F, 0.472F, - -0.070251F, -0.607451F, -0.491983F, -0.407354F, -0.237688F, -0.0147375F, - -1.33957F, -0.770665F, -0.493377F, -0.391919F, -0.46833F, 0.0756188F, - -1.36935F, -1.30072F, -0.524757F, -0.505346F, -0.486972F, -0.0909742F, - -0.374307F, -0.487696F, -0.483824F, -0.399696F, -1.51826F, -0.523456F, - -0.478643F, -0.0698441F, -0.815881F, -0.63039F, -0.602538F, -0.521541F, - -0.497487F, 0.000289753F, -0.641572F, -0.645319F, -0.599483F, -0.577909F, - -0.470322F, -0.46724F, -0.00890862F, -0.58036F, -0.423628F, 0.345779F, - -0.785153F, 0.0160281F, -0.533388F, -0.590668F, -0.39224F, -0.376456F, - -0.37407F, -0.368308F, -0.498806F, -0.485542F, -0.158728F, -0.533348F, - -0.363054F, -0.461773F, -0.417833F, -0.378913F, -0.612322F, -0.47506F, - -0.397976F, -0.364866F, -0.208838F, 0.0393697F, -0.597834F, -0.631919F, - -0.365056F, -0.232551F, -0.228628F, -0.511614F, -0.532199F, -0.440105F, - -0.0199633F, -0.523887F, -0.983599F, -0.353875F, -0.301811F, -0.196292F, - -0.100924F, -0.0831491F, -0.872572F, -0.642216F, -0.319625F, -0.531061F, - -1.75025F, -3.03004F, -7.13635F, -1.82493F, 0.0805218F, -0.549804F, - 0.783087F, -2.07856F, -0.471454F, -0.470871F, -0.469544F, -0.432717F, - -0.426827F, -0.474728F, -0.384199F, -0.216405F, -0.802142F, -0.501304F, - -0.497948F, -0.460159F, -0.463702F, -0.431615F, -0.3912F, 0.00685542F, - -0.173793F, -0.439298F, -0.429527F, -0.310308F, -0.422905F, -1.15208F, - -0.235387F, -1.786F, -1.82144F, -0.600214F, -0.615737F, -0.472967F, - -2.98689F, -2.14475F, -1.0794F, -0.532316F, -0.260223F, -1.88218F, - -0.677226F, -0.704543F, -0.466284F, -0.538399F, -0.410555F, -0.376157F, - -0.165304F, -0.0410297F, -1.82618F, -0.627179F, -0.63187F, -0.532023F, - -0.38629F, -0.112428F, -0.0750774F, -1.97751F, -0.428879F, -0.433998F, - 0.146957F, -0.46361F, -0.218851F, -0.31941F, -0.213975F, -1.83167F, - -1.68396F, -1.64989F, -2.006F, -1.8172F, -1.95734F, -0.0505971F, -0.0488872F, - -1.77731F, -1.75051F, -0.581075F, -0.675634F, -0.0421438F, -1.9485F, - -1.13123F, -0.589176F, -0.579586F, -0.593709F, -0.4289F, -0.213836F, - 0.0251724F, -0.212536F, -3.12158F, -1.87979F, -0.589243F, -0.397287F, - 0.341907F, -0.132761F, -0.467809F, -0.207127F, -4.80146F, -2.09357F, - 0.0127887F, -0.599442F, -0.0367846F, -0.0452865F, -2.18196F, -1.11447F, - -0.0336475F, -0.288818F, -0.054104F, -0.000653219F, -2.37275F, -1.79599F, - -3.48092F, -1.98676F, -1.75929F, -0.241254F, -0.00327217F, -0.877556F, - 0.190664F, -1.05543F, -0.0282896F, -0.0186265F, -13.4798F, -13.1949F, - -7.18888F, -5.66619F, -10.1098F, -10.6991F, -9.73549F, 0.0729048F, -1.79118F, - -1.82527F, -0.748895F, -1.06024F, 0.0068788F, -0.0417065F, -1.64478F, - -0.75563F, -0.470779F, 0.110754F, -0.0298082F, -0.0427242F, -1.94738F, - -0.115557F, -0.0243473F, 0.398607F, -1.68598F, -2.02824F, -0.159752F, - -0.166954F, 0.551306F, 0.0399052F, 0.0399754F, -1.45893F, -2.23255F, - -2.03223F, -2.67879F, -17.8345F, -0.74554F, -2.41399F, -5.05081F, -1.38593F, - -16.6738F, -1.51594F, -1.11459F, -0.437036F, -0.0585316F, -1.89361F, - -0.476346F, -0.275921F, -0.225566F, -1.10185F, -0.119675F, 0.137194F, - -0.125097F, 0.207142F, -0.989695F, -0.992786F, -1.48752F, -1.29833F, - -0.291058F, -0.202524F, 0.0116968F, 0.755327F, -1.62917F, -1.74868F, - -1.68879F, -4.22862F, -1.74806F, -1.75391F, 0.153033F, -2.32324F, -0.467553F, - -0.0447416F, -0.573272F, 0.00459359F, 0.040057F, -0.389799F, 0.00826518F, - -0.0310854F, -1.23566F, -0.925211F, -0.181309F, -1.7778F, -4.30727F, - -1.91982F, -1.77459F, -1.77932F, -1.077F, -0.464312F, 0.0763377F, 0.0796182F, - 0.155618F, -2.1288F, -0.818158F, -1.21502F, -0.473062F, -0.0496759F, - 0.0779925F, -2.3085F, -1.39621F, -0.400874F, 0.217362F, -0.00711439F, - 0.168426F, 0.506399F, -1.82329F, -0.314688F, -0.0865325F, -0.0409483F, - -1.81573F, -1.3964F, -1.6198F, -0.357057F, 0.126037F, -0.0721527F, 0.30816F, - 0.411723F, -1.75327F, -1.33645F, -0.598078F, -0.426419F, -0.42771F, - -0.202049F, -0.0794349F, -0.00251928F, -0.0119337F, -0.220952F, 0.144218F, - -0.121392F, -1.73042F, -0.796856F, -0.66003F, -0.446701F, -0.317616F, - 0.146856F, -1.73606F, -1.04045F, 0.433038F, -1.85459F, -1.80615F, -1.96622F, - -1.88046F, -2.85748F, -1.80302F, -1.0573F, -0.779806F, -0.442852F, - -0.110017F, -0.0535884F, -0.0485818F, -0.0245943F, 0.0530908F, 0.607016F, - -0.954464F, -0.557913F, -0.461896F, -0.383007F, -0.353497F, -0.00561111F, - -0.001778F, 0.0125068F, 0.110509F, 0.391018F, -1.55709F, -0.658649F, - -0.654862F, -1.05284F, -0.549011F, -0.218679F, -0.0416034F, -0.319539F, - -0.258397F, 0.200791F, 0.128761F, -0.000154844F, 0.135182F, 0.578129F, - 0.373745F, 0.79363F, -1.6029F, -1.55693F, -0.778994F, -0.943598F, -0.813945F, - -0.380372F, -0.34839F, -0.310644F, -0.0196752F, 0.165096F, 0.112911F, - 0.0445832F, 0.493476F, 0.414421F, -1.74792F, -1.96487F, -0.767474F, - -0.94889F, -0.551329F, -0.29469F, 0.0232329F, 0.174224F, 0.201701F, - 0.000319237F, 0.0871491F, 0.697504F, 0.0978221F, -1.64482F, -0.895582F, - -0.957521F, -0.542713F, -0.470275F, -0.208621F, -0.0282766F, -0.270061F, - -0.125158F, -0.159847F, 0.0240132F, -0.149827F, 0.0736361F, 0.385913F, - -1.3014F, -0.859639F, -0.156998F, 0.101425F, -0.0388139F, -1.29719F, - -0.510692F, -0.538654F, -0.245535F, -0.102598F, 0.74403F, 0.51179F, - -2.19704F, -1.78089F, -1.11496F, -0.381224F, -0.022788F, 0.426966F, - 0.726944F, -1.69428F, -1.0893F, -0.774403F, -0.911291F, -1.09304F, -1.17637F, - -0.605654F, -0.487843F, -0.286734F, -0.112945F, 0.0195759F, 0.198361F, - 0.42476F, 0.761125F, -1.84856F, -1.5649F, -0.758104F, -0.71152F, -0.674458F, - -0.655191F, -0.408765F, -0.389747F, 0.0481693F, 0.274175F, 0.0719241F, - 0.275991F, 0.513644F, 0.357326F, 0.75941F, -1.765F, -1.7662F, -1.03305F, - -1.11066F, -0.468855F, 0.028414F, 0.0923949F, 0.163539F, 0.0753931F, - 0.398365F, 0.750898F, 0.807272F, -1.56879F, -0.713222F, -0.732039F, - -0.521745F, -0.563458F, -0.394238F, -0.181631F, -0.0953167F, 0.115392F, - 0.141422F, -0.0557747F, 0.142698F, 0.0256789F, -0.45862F, 0.144074F, - 0.325348F, 0.208154F, 0.626687F, -2.03221F, -0.357042F, 0.129709F, - 0.00328214F, 0.508901F, 1.30879F, -2.16548F, -0.673853F, -0.226954F, - -0.0990078F, 0.775041F, -1.17597F, -0.184743F, 0.17991F, -0.00491623F, - 0.178698F, 0.72899F, -0.677449F, -0.609601F, -0.675971F, -0.0418312F, - -0.0237793F, 0.867309F, -1.80188F, -1.78246F, -1.09003F, -0.394038F, - 0.00228453F, 0.0868799F, 0.139602F, -0.00747393F, 0.160134F, 0.445715F, - -1.87768F, -1.73547F, -2.09799F, -0.764698F, -0.776736F, -0.849177F, - -0.600814F, -0.14515F, -0.0384104F, -0.0389099F, 0.427116F, 0.421376F, - -1.8228F, -1.69676F, -0.963664F, -0.667031F, -0.53088F, -0.232498F, - 0.103207F, -0.230022F, 0.776861F, 0.459047F, -1.57624F, -1.6404F, -1.52972F, - -0.757134F, -0.11028F, -0.0438428F, 0.0424984F, 0.390135F, -1.87201F, - -1.46634F, -1.81235F, -0.592665F, -0.44801F, -1.27553F, 0.0448705F, - -1.13473F, -1.1175F, -0.746406F, -0.374932F, -0.0246016F, -0.0262844F, - 0.0847609F, 0.0558378F, 0.137401F, 0.182013F, 0.433101F, -1.10102F, - -0.610717F, -0.477341F, -0.510935F, -0.391526F, -0.375402F, -0.324194F, - 0.185188F, 0.12078F, 0.12065F, 0.148053F, 0.768411F, 0.431191F, 0.43948F, - 0.658745F, -1.10992F, -1.8236F, -0.751346F, -0.633263F, -0.436608F, - -0.411632F, -0.00458106F, -0.319546F, -0.219804F, -0.0255624F, 0.647177F, - 0.78789F, -1.86202F, -0.953337F, -0.953537F, 0.0319533F, 0.057352F, - 0.351658F, 0.668332F, 0.428068F, 0.858791F, -0.513099F, -0.54915F, 0.15405F, - 0.183441F, 0.390754F, 0.419517F, -1.74143F, -1.51002F, -0.841634F, - -0.382094F, -0.279497F, -0.126321F, -0.134257F, -0.10567F, 0.0406543F, - 0.430027F, 0.465877F, -1.86193F, -1.47416F, -0.808276F, -0.765657F, -1.1057F, - 0.512462F, -1.46238F, -1.07338F, -0.884062F, -0.834293F, -0.306435F, - -0.16192F, -1.07465F, -1.8476F, -0.841457F, -0.47726F, -0.513801F, - -0.455571F, -0.272476F, -1.76335F, 0.39657F, -0.56854F, -0.209881F, - 0.502011F, 0.408409F, -2.16466F, -1.04799F, 0.0155358F, -0.85566F, -0.66307F, - -0.0136978F, 0.0584044F, 0.432307F, -0.965381F, -0.521916F, -0.397327F, - 0.0314558F, -0.119785F, -0.576132F, 0.137263F, 0.155028F, 0.0263051F, - -1.11548F, 0.146535F, 0.199296F, -0.373633F, -0.205651F, 0.149302F, - 0.419081F, 0.421004F, -0.537167F, -0.129688F, 0.403643F, 0.213224F, - -2.03209F, -0.156916F, -0.335769F, 0.0979053F, 0.421314F, -1.11817F, - -0.176256F, -0.0745811F, -1.18971F, -0.422726F, -1.1148F, -0.53901F, - 0.132112F, 0.175465F, -1.78596F, -1.12927F, 0.153925F, 0.0770581F, 0.171799F, - 0.668735F, 0.412702F, 0.414073F, -1.05376F, -0.745556F, 0.098033F, 0.434866F, - 0.467192F, -0.77678F, -0.473665F, -0.541505F, 0.0653864F, -2.97021F, - -0.760207F, -0.017434F, -1.05679F, -0.757333F, -0.735735F, -1.61591F, - -0.446255F, -0.0538085F, -0.341534F, 0.182316F, -1.74768F, -1.11293F, - -0.768157F, -0.633159F, 0.154138F, 0.784399F, -1.17729F, -0.732937F, - -0.00740687F, 0.147044F, 0.124573F, 0.645072F, 0.416057F, -1.81708F, - -0.633779F, -0.542252F, -0.316157F, 0.137374F, 0.612303F, 0.427622F, - 0.425324F, -2.50772F, -0.634537F, -0.532584F, -0.399016F, 0.010249F, - 0.179106F, -0.808607F, 0.0249395F, 0.492525F, -0.470387F, -0.217646F, - -0.270047F, 0.439622F, -0.351932F, -2.28651F, -1.25928F, -0.861872F, - -0.12733F, -1.03555F, -0.365262F, 0.219296F, 0.632147F, 0.00364466F, - -1.10983F, -1.10945F, -0.735968F, -0.246531F, -0.383003F, -0.0357547F, - 0.136093F, -1.02791F, -0.565326F, -0.351097F, 0.682687F, 0.0948036F, - 0.174133F, -0.0597393F, -1.2512F, -0.669995F, -0.513338F, -0.493378F, - -0.274083F, -0.386826F, 0.0307196F, 0.76984F, 0.422782F, -1.11967F, - 0.303428F, -0.27319F, -0.369066F, 0.460703F, -1.83905F, -0.889581F, - -0.867326F, -0.410863F, -0.395294F, -0.222383F, -1.83313F, -1.84781F, - -1.81311F, -5.91705F, -1.87577F, -18.3448F, -3.06938F, -1.85319F, -1.07396F, - 0.170213F, 0.16427F, -1.83927F, -1.18159F, -1.11984F, 0.104415F, 0.137691F, - -1.12883F, -0.0471888F, 0.0677591F, 0.425233F, 0.398153F, -0.998495F, - -0.457443F, -0.568258F, 0.336921F, -0.538968F, -1.84402F, -1.86276F, - -1.75507F, -1.69867F, -1.70502F, -2.90042F, -5.1009F, -6.4026F, -1.77795F, - -1.96575F, -1.83884F, -1.10456F, -0.0819822F, -0.0528562F, -1.74221F, - -1.10168F, -0.633787F, -0.141145F, 0.673843F, -0.040052F, -0.601295F, - -0.279305F, -0.400878F, -0.320799F, 0.800063F, 1.17983F, 0.429238F, - -1.84884F, -0.167894F, 0.0423056F, -0.00576224F, 0.448168F, -0.64643F, - -0.40352F, 0.0288439F, 0.100742F, -0.00529074F, -0.024362F, -0.564622F, - 0.3479F, -0.332821F, -0.403247F, -1.77407F, -1.79834F, -1.77197F, -1.73855F, - -1.82531F, -1.61038F, -1.73543F, -1.7898F, -0.0166849F, -6.09123F, -1.828F, - -1.59917F, -1.12825F, -0.317576F, 0.676986F, -0.0417318F, -0.0166001F, - -0.569959F, -0.435879F, -0.242549F, -0.313676F, 0.0940131F, -0.0375891F, - -0.0417951F, -0.609093F, -0.57832F, -0.534308F, -0.212023F, 0.440815F, - -1.08699F, -0.768857F, -0.59567F, -0.315747F, 0.0104527F, 0.0231428F, - -0.0395237F, -1.76841F, 0.0092364F, -0.034707F, -0.0234502F, 0.364085F, - -0.788651F, -0.226003F, 0.434534F, -0.742017F, -0.132232F, -1.82193F, - -1.84582F, -1.77228F, -1.73337F, -1.69249F, -1.796F, -1.70453F, -1.74244F, - -1.07442F, -0.734978F, 0.781372F, 0.725603F, -1.72113F, -1.14099F, - -0.237551F, -0.163827F, -0.0188647F, -0.0464203F, -0.046668F, -1.12419F, - -1.20184F, -0.402577F, -0.0245231F, -0.0122902F, -0.0220063F, -1.16356F, - -0.611879F, -0.219425F, -0.399701F, -0.398736F, 0.175558F, -0.0814209F, - -0.0280027F, -0.0397039F, -1.1645F, 0.40667F, -1.75786F, -0.76847F, -1.1172F, - -0.0174189F, -0.397578F, -1.83309F, -1.86463F, -1.77841F, -1.81266F, - -1.87739F, -1.8073F, -1.82363F, -3.42289F, -1.78059F, -5.05306F, -1.84218F, - -3.365F, -2.14145F, -3.36005F, -3.66856F, -1.66936F, -3.54059F, -0.998254F, - -0.40133F, 0.725278F, -0.00914177F, -0.021295F, -1.80995F, -1.96032F, - -1.6596F, -0.922985F, -0.632995F, -0.436968F, -0.409063F, -0.403422F, - -0.361856F, -0.0231648F, 0.155125F, -0.0264104F, -0.786263F, -1.20919F, - -0.811319F, -0.732997F, -0.640012F, -0.383687F, -0.193815F, -0.0421567F, - -0.0229887F, 0.221054F, -0.0384537F, -0.0723992F, -0.596172F, -0.542812F, - -1.83967F, -1.82145F, -1.8488F, -4.68222F, -4.49696F, -0.0729393F, -4.48153F, - -1.75391F, -2.00151F, -0.0256001F, -1.7558F, -1.95299F, -0.642116F, - -0.439249F, -0.271068F, -0.350866F, -0.350465F, 0.772623F, -0.0227251F, - 0.00387784F, -1.85407F, -0.649769F, -1.8806F, -0.0531885F, -1.5581F, - -1.5699F, -0.306269F, -0.0487961F, -2.22947F, -1.90762F, -1.82078F, - -1.82028F, -1.69991F, -1.71265F, -1.80613F, -1.79706F, -5.80173F, -3.33446F, - -1.8051F, -7.02761F, -1.72886F, -3.43529F, -6.16502F, -2.43683F, -1.7732F, - -1.76847F, -3.52455F, -3.72594F, -3.3738F, -1.10493F, -0.639805F, -0.311086F, - -0.0529495F, -0.0386731F, -1.8027F, -2.00889F, -1.81573F, -6.19073F, - -0.030172F, -0.0635215F, -2.14323F, -25.4968F, -0.427424F, -0.33183F, - -0.358593F, 0.152309F, -0.0115135F, -1.61184F, -1.05554F, -0.804895F, - -0.22173F, -0.326211F, -3.32673F, -1.35564F, -0.617661F, -0.345363F, - -0.0439059F, -1.97616F, -0.715737F, 0.144863F, -0.615818F, -2.2119F, - -4.55771F, -14.5365F, -1.84391F, -4.13898F, -6.25226F, -7.36594F, -3.93911F, - -6.36706F, -1.80342F, -1.20528F, -1.24849F, -1.12949F, -0.773738F, -1.19932F, - -0.350698F, -1.81815F, -4.33667F, -16.1089F, -0.426939F, -0.0151576F, - -7.51271F, -0.768817F, -0.739502F, -0.569555F, -0.399734F, -0.243269F, - 0.0767902F, 0.0744609F, 0.105888F, -0.327869F, -0.0395036F, -0.0454128F, - -1.21283F, -0.595822F, -1.22094F, -0.400661F, -0.0528908F, -0.590769F, - -1.70428F, -2.12792F, -0.425064F, 0.0720209F, -1.27979F, 0.181427F, - -2.98912F, -10.3019F, -2.12083F, -7.00034F, 0.0683505F, -0.0453432F, - -1.21066F, -0.379716F, -0.298213F, -0.356701F, -0.0440885F, -1.38278F, - -4.72543F, -1.93109F, -1.70379F, -0.402695F, -0.359507F, -0.0410153F, - -0.743802F, -0.666752F, -0.902551F, -0.0714009F, -1.08761F, 0.00752451F, - -0.805125F, -1.24623F, -8.74731F, -3.22886F, -1.78628F, -3.87246F, -2.83997F, - -8.65645F, -2.40531F, -0.745223F, -0.266803F, -0.398103F, -0.230343F, - -0.333605F, -0.872146F, -0.396845F, -0.356337F, -1.73885F, -4.67965F, - -4.0324F, -0.850732F, -0.72209F, -0.806833F, -1.60268F, -0.420989F, - -0.418074F, -0.356907F, -0.313263F, -0.113689F, -0.0240377F, -0.114093F, - -0.0566382F, -2.0484F, -2.17873F, -1.73388F, -0.278376F, -0.208198F, - -0.0694188F, -1.17601F, -0.62717F, -0.357995F, -1.21588F, -0.0296327F, - -0.254864F, -3.14004F, -0.641796F, -0.48658F, -1.68935F, -1.26289F, -1.2282F, - -1.12454F, -0.806181F, -1.25496F, -1.16329F, -1.84984F, -1.2487F, -1.2227F, - -1.13206F, -0.022116F, -0.0398402F, -1.23927F, -1.12316F, -1.16238F, - -1.7514F, -0.58719F, -0.621497F, -0.394937F, -0.289373F, 0.110091F, - -1.55512F, -0.00813335F, -1.05983F, -0.750011F, 0.0824358F, 0.0268463F, - -1.66769F, -1.71079F, -1.81286F, -1.2423F, -0.584379F, -0.0424979F, - -0.0412723F, -0.0457866F, -1.26914F, -1.19707F, -1.13004F, -1.09557F, - -1.08009F, -0.614261F, -0.412104F, -0.200486F, -0.175272F, -0.0809358F, - -1.24258F, -0.243981F, -0.0572911F, 0.0276169F, -1.22518F, -1.17505F, - -0.300258F, -0.419738F, -0.342829F, -0.312312F, -0.0397644F, -1.26358F, - -0.627495F, 0.239159F, -0.400164F, 0.0254609F, -0.352397F, -0.041656F, - -2.18304F, -1.77666F, -0.388312F, 0.0754304F, -1.05221F, -1.55019F, - -0.389886F, 0.287907F, -1.74411F, -1.85548F, -1.86779F, -1.13365F, -1.13247F, - -0.298749F, -0.326124F, -0.349994F, -0.0404301F, -0.345844F, -0.338485F, - -0.0620605F, -0.0484341F, -1.2197F, -0.60187F, -0.0463705F, -1.19232F, - -0.356322F, -0.0629335F, -0.549769F, 0.0138733F, -0.0598935F, -1.3748F, - -1.56322F, -0.337077F, -0.258239F, -0.531809F, -0.44414F, -0.0282686F, - -2.86528F, 0.324569F, -1.86164F, -1.34549F, -0.433773F, -0.37241F, - -0.339083F, -0.0506374F, -1.86131F, -1.20379F, -0.636882F, -0.371597F, - -0.352211F, -0.34631F, 0.205036F, -1.75199F, -1.73421F, -1.19482F, -1.07573F, - -0.636982F, -0.609551F, -0.381374F, -0.150839F, -1.89439F, -0.415208F, - -0.14709F, -0.31558F, -0.0445581F, -0.0402119F, -1.09095F, -0.96735F, - -2.4517F, -0.621891F, -0.515253F, -1.09691F, -0.579033F, -0.426709F, - -0.716718F, -0.806605F, -1.82524F, -1.80125F, -1.78778F, -1.80508F, - -1.75342F, -1.81844F, -1.78526F, -1.83117F, -1.26865F, -1.79705F, -0.346884F, - -1.79558F, -0.592878F, -0.574879F, -0.331824F, -0.0154435F, -0.0878214F, - -1.71168F, -1.30182F, -0.733934F, -0.538763F, -1.75996F, -0.360732F, - -1.71263F, -2.70915F, -1.23383F, -1.10866F, -1.41789F, -0.809672F, - -0.560996F, -0.295526F, -1.31105F, 0.0396742F, -1.92639F, -0.550002F, - -0.764604F, -0.679176F, -0.226845F, -1.81889F, -1.75232F, -1.80942F, - -1.78038F, -1.19317F, -0.558386F, -0.44901F, -0.0789365F, -0.0655116F, - -1.80336F, -1.87827F, -2.1517F, -1.66907F, -1.28221F, -1.22297F, -0.67754F, - -0.559646F, -0.577165F, -0.0817858F, -0.0698106F, -1.7551F, -0.390314F, - -0.0715028F, -0.0533125F, -0.768311F, -0.621064F, 0.106833F, -0.122323F, - -2.04234F, -0.834678F, -0.796316F, -0.405779F, 0.44779F, 0.682203F, - -0.00500219F, -4.96372F, -0.749653F, -0.606208F, -0.443499F, -0.379048F, - 0.000481946F, 1.11691F, -1.56612F, 0.0109996F, -1.50229F, -1.34401F, -2.558F, - -0.682627F, -0.761436F, 1.34859F, -1.79024F, -1.84766F, -2.5711F, -1.81183F, - -1.69294F, -1.24053F, -0.570846F, -0.0853022F, -0.341691F, -1.18152F, - -0.526772F, -0.267288F, -1.31055F, -0.546069F, -1.08287F, -0.378105F, - -0.267144F, -1.27997F, -0.685923F, -0.792614F, -0.584187F, -0.588141F, - -0.53072F, -1.80297F, -0.281572F, 0.00344088F, -4.93276F, -0.697632F, - -0.648127F, 0.0659089F, -1.08171F, -0.018888F, 0.186888F, -1.78804F, - -1.79952F, -1.58089F, -1.7817F, -1.82306F, -1.30895F, -1.1898F, -0.346789F, - -1.83695F, -1.28581F, -1.21012F, -0.351663F, -0.260307F, -0.0436045F, - -0.409033F, 0.755368F, -1.81581F, -1.35935F, -4.75597F, -0.583704F, - -5.82243F, -1.77829F, -0.0231319F, -3.29623F, -0.737772F, -0.0177995F, - -0.0167613F, -1.10026F, -0.380626F, -0.584255F, 0.0326498F, -1.75742F, - -1.88233F, -1.88753F, -1.85234F, -1.80747F, -1.78347F, -1.75766F, -1.79454F, - -1.79581F, -1.86676F, -1.76442F, -1.84957F, -5.6596F, -1.85702F, -2.17786F, - -1.25855F, -1.20935F, -1.06405F, -1.17732F, -0.770417F, -0.344132F, - -1.85765F, -1.49606F, -1.75347F, -1.28065F, -0.869515F, -0.324888F, - 0.137413F, -1.26588F, -0.951333F, -0.751757F, -0.551118F, -0.338451F, - -0.0161976F, -1.16952F, -0.65935F, -0.556156F, -0.0226007F, 0.783538F, - -0.2928F, 0.215084F, -0.781242F, -0.650569F, -1.884F, -1.88614F, -1.64053F, - -0.89078F, -0.651462F, -0.590028F, 1.36615F, -0.704189F, -0.460081F, - -0.110802F, 0.0146704F, 0.140479F, 0.961754F, -3.41447F, -3.36709F, - -1.49801F, -1.79074F, -1.11447F, -0.393948F, -0.345576F, -1.74577F, - -0.0287362F, -1.78472F, -0.774846F, -0.378949F, -0.344761F, -1.63037F, - -0.564255F, -1.86915F, 0.0118641F, -0.131033F, -1.95466F, -0.118186F, - 0.856519F, -0.0516445F, -1.75954F, -1.49046F, -1.22043F, -1.22637F, - -1.48943F, -0.319776F, 0.0833896F, -1.61932F, -1.77688F, -1.12928F, - -0.349332F, -1.76647F, -1.8958F, -4.29461F, -0.45458F, 0.0284364F, - -0.454649F, -0.0440857F, -1.81195F, -1.83019F, -4.48129F, -1.77259F, - -1.82691F, -1.85385F, -1.84681F, -1.74779F, -1.77055F, -0.39825F, -0.188736F, - -0.0501342F, -1.74461F, -1.61188F, -2.4253F, -0.496569F, 0.0628064F, - -0.004924F, -0.880985F, -0.235702F, -0.851267F, -0.397985F, -0.321468F, - -1.86885F, -1.85774F, -0.392311F, -0.924944F, -0.565945F, -0.411124F, - 0.722971F, -1.03357F, -0.853998F, -0.629061F, -0.309754F, 0.132374F, - -2.46699F, -2.40755F, -1.79713F, -1.79991F, -1.77103F, -1.86753F, -1.51201F, - -1.78929F, -1.77666F, -1.75618F, -0.764336F, -1.52104F, -3.14241F, -4.40024F, - -1.22317F, -0.507654F, -0.0979279F, -1.62581F, -1.76065F, -0.0466757F, - -0.0522663F, -1.77772F, -0.0774806F, -1.90115F, -0.312255F, -0.089979F, - -1.85058F, -0.982542F, -0.0691714F, -1.80966F, -1.873F, -1.77255F, -1.89093F, - -1.81365F, -1.79768F, -1.78615F, -1.55967F, -1.83427F, -3.17294F, -1.82294F, - -1.7996F, -3.42815F, -1.80962F, -1.83126F, -1.75608F, -1.859F, -1.83348F, - -1.26192F, -0.568575F, 0.179713F, -0.0307328F, -1.872F, -1.96727F, -1.79566F, - -1.00206F, -0.596727F, -0.566425F, -0.343597F, -0.0248708F, -0.0576456F, - -1.8835F, -1.8563F, -1.73179F, -0.471167F, -0.821236F, -0.559788F, - -0.408856F, -0.0303545F, -1.70403F, -1.35755F, -0.963144F, -0.663184F, - -0.522145F, -0.0312038F, -0.685299F, -0.518556F, -0.434821F, -0.0626222F, - -1.84675F, -2.24337F, -1.79269F, -1.92161F, -1.26589F, 0.124708F, -1.78853F, - -1.90367F, -1.82497F, -1.59342F, -1.82263F, -1.87709F, -1.27553F, -0.584612F, - -0.0271195F, -1.50876F, -1.9102F, -1.74006F, -1.72488F, -1.26144F, - -0.753918F, -0.601304F, -0.0425933F, -0.20673F, -0.108131F, -0.588F, - -0.579691F, -0.51438F, -0.35932F, -0.315902F, -0.0976208F, -1.85539F, - -1.86725F, -1.70088F, -1.82752F, -0.453847F, -0.38874F, -0.400855F, - -1.71273F, -2.09505F, -0.964549F, -0.387514F, -0.294684F, -0.192615F, - -0.816604F, -0.277796F, -1.21273F, -0.163588F, -0.0384185F, -1.41204F, - -1.86354F, -1.85049F, -2.95963F, -2.66234F, -2.78728F, -2.3358F, -1.77915F, - -1.7607F, -1.72022F, -1.72936F, -1.23835F, -0.0673497F, -1.84826F, -1.80638F, - -2.80854F, -1.57314F, -2.44485F, -2.01573F, -2.09319F, -2.17264F, -1.99056F, - -2.16148F, -1.5124F, -1.74553F, -0.836076F, -0.503005F, -0.470543F, - -0.443892F, -0.415023F, -0.369103F, -0.157028F, -1.95635F, -0.0290322F, - 0.0205013F, -0.109314F, -2.39296F, -1.2491F, -1.68073F, -0.915837F, - -0.683078F, -0.466939F, -0.386908F, -0.344096F, 0.24876F, -1.50595F, - -1.89244F, -1.11798F, -0.910342F, -0.969833F, -0.701048F, -0.574139F, - -0.119853F, -1.78353F, -0.52917F, -0.415133F, -0.0598502F, -0.166888F, - -1.47288F, 0.211585F, -1.85713F, -1.89072F, -1.83317F, -1.8221F, -1.60342F, - -2.00605F, -2.15124F, -1.74849F, -1.78349F, -0.385361F, -1.8715F, -1.63918F, - -2.05788F, -1.83682F, -2.09416F, -1.55564F, -2.30236F, -0.549269F, -0.59768F, - -0.205563F, -2.48022F, -0.488567F, -1.22045F, -0.569221F, -0.622663F, - -0.424202F, -0.421907F, -0.0909962F, -0.00342685F, -0.155261F, -0.337712F, - -1.66679F, -1.62497F, -0.752356F, -0.479495F, -0.706144F, -0.846673F, - -1.69845F, -0.556722F, -2.27421F, -1.82242F, -2.02795F, -2.09506F, -1.82734F, - -1.77294F, -1.25028F, -1.23571F, -2.68164F, -1.93981F, -2.46393F, -3.18208F, - -2.04837F, -0.681628F, -0.548555F, -0.479002F, -1.56136F, -0.925886F, - -0.625882F, -0.512486F, -2.16784F, -1.98742F, -0.605783F, -0.500945F, - -0.560382F, -0.436632F, -0.392344F, -0.355802F, -0.144931F, 0.0256955F, - -3.34291F, -1.82599F, 0.175837F, -0.0412635F, -2.57607F, -1.48423F, - -1.06769F, -0.233731F, -5.41602F, -1.81976F, -1.59315F, -1.83738F, -1.44671F, - -1.91187F, -2.00183F, 0.112218F, 0.634658F, 1.19623F, -1.47387F, -1.84229F, - -2.75782F, 0.158992F, -1.83911F, -6.23953F, -1.9761F, -0.0508034F, 0.141119F, - -0.427048F, -0.42309F, -0.128104F, -2.07053F, -1.72771F, -0.532477F, - -0.42156F, -0.139588F, -0.00544025F, -0.439949F, -0.915666F, -0.587137F, - -0.321354F, -0.214658F, -0.0250515F, -0.348766F, 0.366632F, 0.421789F, - -1.83813F, -1.62059F, -1.84747F, -3.33492F, -1.83181F, -1.81165F, -2.2922F, - -11.1107F, -2.45678F, -1.88909F, -4.26943F, -0.0461216F, 0.0211176F, - -0.0200179F, -0.0204997F, -0.0192866F, -0.0358446F, -1.80302F, -1.21918F, - -2.78757F, -0.019335F, -0.0143956F, -1.61265F, -1.64271F, -1.80586F, - -1.80931F, -2.34003F, -1.80442F, -1.25325F, -1.23291F, -1.40094F, -1.12696F, - -0.0949408F, -1.63637F, -1.82707F, -1.18363F, -0.217661F, -0.0636714F, - -1.95524F, -0.561732F, -0.444869F, -0.0590094F, -1.80298F, -1.68989F, - -5.44908F, -0.0520035F, -1.97057F, -1.68099F, 0.470632F, -1.78886F, - -2.35262F, -1.68616F, -2.12718F, -1.63792F, -1.8949F, -1.84117F, -1.80531F, - -1.85405F, -3.41058F, -1.81388F, -1.78443F, -3.25527F, -2.32938F, -1.23789F, - -0.275701F, -1.77121F, -1.98092F, -1.68366F, -1.82622F, -2.26119F, - -0.0378355F, -1.59282F, -1.78908F, -1.32716F, -0.720514F, -0.227873F, - 1.12118F, -1.85652F, -1.89756F, -0.808161F, -0.603668F, -0.577665F, - -0.379859F, -1.67412F, -0.586595F, -0.587966F, -0.518922F, -0.403889F, - -1.02977F, -0.310974F, 0.00993536F, -1.40585F, -0.904645F, -1.01837F, - -1.83406F, -1.83375F, -1.64075F, -1.67532F, -1.95163F, -1.62128F, -1.98104F, - -1.66517F, -4.83674F, -0.0587F, -1.6367F, -1.89244F, -1.71469F, -1.82438F, - -0.814793F, -0.528576F, -0.607272F, -0.524564F, 0.140674F, -0.777666F, - -0.695035F, -1.76717F, -0.870272F, -2.18522F, -2.18236F, -16.3866F, - -2.25097F, -2.1563F, -2.69466F, -2.02743F, -2.1286F, -2.20012F, -2.04734F, - -2.0803F, -2.00048F, -2.20531F, -1.96979F, -0.756311F, -1.96476F, -1.87897F, - -1.64018F, -1.65953F, -1.95815F, -1.81799F, -1.26242F, -1.92126F, -1.91479F, - -1.9338F, -1.90318F, -1.7564F, -1.72471F, -1.67726F, 0.469728F, 0.451789F, - -2.08104F, -1.73847F, -2.01127F, -2.04851F, -0.023411F, -1.77188F, -1.74537F, - -1.58089F, -0.74753F, -3.57294F, -1.54243F, -1.43378F, -0.735073F, - -0.587333F, -0.51692F, -0.388213F, -0.219962F, -2.05063F, -2.83605F, - -0.508635F, -0.545769F, -1.28255F, -0.603178F, -0.315022F, -0.743178F, - -0.0825165F, -0.906214F, -0.706851F, -0.629883F, -1.94961F, -1.74622F, - -1.2484F, -0.569148F, -0.562176F, -0.616221F, -1.73545F, -1.65893F, - -1.73558F, -1.30957F, -1.63854F, -1.66417F, -1.46703F, -1.86782F, -0.343145F, - -1.65885F, -0.049865F, -1.71812F, -0.0478456F, -1.50963F, -1.98704F, - -1.31221F, -1.58192F, -2.06601F, -1.83859F, -1.8947F, -1.22019F, -2.65914F, - -1.31598F, -1.0795F, -1.402F, -1.19434F, -1.84952F, -1.19731F, -1.7565F, - -1.40694F, -1.8555F, -1.78009F, -1.9135F, -1.89951F, -1.1037F, -1.82992F, - -1.42915F, -1.83897F, -2.6367F, -1.8165F, -1.48253F, -1.36512F, -1.25938F, - -1.80122F, -1.40554F, -2.90379F, -3.8695F, -1.22761F, -1.26346F, -1.24737F, - -1.88252F, -1.79594F, -1.35361F, -2.62742F, -1.79021F, -1.31407F, -1.56449F, - -1.68282F, -1.85324F, -1.73482F, -1.63756F, -0.780355F, -1.83817F, -1.7625F, - -1.37828F, -1.75159F, -0.183626F, -0.186356F, -0.121092F, -1.26363F, - -1.70404F, -0.558229F, -1.21464F, -1.69254F, -1.56452F, -1.677F, -1.61289F, - -1.52012F, -1.33243F, -1.67141F, -1.34476F, -1.81261F, -4.00397F, -1.83286F, - -1.87334F, -1.83663F, -1.88553F, -1.87233F, -1.88839F, -1.80772F, -1.82156F, - -1.79057F, -1.89964F, -1.82334F, -1.86627F, -1.85156F, -1.86167F, -1.8033F, - -1.81682F, -1.80404F, -0.177556F, -1.6026F, -1.88387F, -1.51207F, -1.8735F, - -1.55153F, -1.77467F, -2.03379F, -1.80329F, -2.0203F, -1.0805F, -1.78593F, - -1.77245F, -1.85061F, -1.34472F, -1.78373F, -0.122488F, -1.54987F, -1.5294F, - -2.39315F, -1.59895F, -1.98531F, -1.36615F, -1.68259F, -0.769557F, -0.18136F, - -1.67164F, -1.51591F, -1.42755F, -0.722057F, -0.192245F, -1.81339F, - -1.52667F, -1.2873F, -1.2294F, -0.675736F, -0.604602F, -0.036551F, -1.82041F, - -1.26265F, -0.76213F, -1.55184F, -1.51556F, -1.48238F, -1.77108F, -2.42972F, - -1.78585F, -1.81618F, -1.79658F, -1.48789F, -1.80792F, -1.82364F, -1.35649F, - -1.41971F, -1.51441F, -1.5236F, -1.78911F, -1.56571F, -1.8437F, -1.80209F, - -0.188394F, -0.174114F, -1.80123F, -1.79125F, -0.72989F, -0.179175F, - -1.53202F, -1.61217F, -1.75884F, -0.820122F, -0.0393219F, -1.80677F, - -1.80568F, -0.752098F, -1.48965F, -1.70236F, -0.205285F, -1.24475F, - -0.813359F, -0.00390948F, -1.17282F, -1.18456F, -1.53074F, -1.84575F, - -1.80084F, -1.79994F, -1.74982F, -1.83476F, -1.83581F, -1.74609F, -1.83543F, - -1.83963F, -1.858F, -1.85325F, -1.81626F, -1.57299F, -1.8125F, -1.8166F, - -1.76206F, -1.84891F, -1.70333F, -0.0483519F, -1.83631F, -1.87853F, - -1.79769F, -1.85598F, -1.7413F, -1.82562F, -1.83444F, -1.83926F, -1.27812F, - -2.46876F, -1.81393F, -1.82064F, -1.83447F, -0.682461F, -1.83226F, -2.07076F, - -1.34722F, -1.32593F, -1.91401F, -1.37017F, -1.47194F, -1.77424F, -1.17918F, - -0.0552872F, -1.64977F, -0.179953F, -1.16176F, -1.159F, -0.610693F, - -0.614253F, -1.45638F, -1.78721F, -3.56167F, -1.80703F, -1.57146F, -1.8497F, - -1.44261F, -1.81285F, -1.66711F, -0.110657F, -1.55713F, -1.437F, -1.41317F, - -1.43111F, -2.00988F, -1.24716F, -1.72065F, -0.198101F, -1.28365F, -1.30658F, - -1.53063F, -1.69926F, -1.68165F, -0.757384F, -1.77987F, -1.79503F, -30.0244F, - -1.79858F, -1.86467F, -2.16378F, -1.81656F, -1.36723F, -0.111931F, - -0.0396875F, -1.49262F, -1.83369F, -1.21954F, -1.79647F, -1.56959F, - -1.55145F, -1.50376F, -1.38707F, -1.79059F, -1.3075F, -1.25512F, -1.42619F, - -1.57823F, -1.23893F, -0.0424791F, -3.65713F, -1.52182F, -1.73025F, - -1.78562F, -0.674322F, -1.87394F, -1.62272F, -1.61116F, -1.76706F, - -0.0375031F, -1.71941F, -1.80814F, -1.42075F, -0.11765F, -0.561738F, - -0.148924F, -1.52825F, -1.07037F, -1.50826F, -1.51201F, -1.49536F, -1.82251F, - -1.80565F, -1.78808F, -1.81567F, -1.78653F, -1.82378F, -1.27587F, -3.2889F, - -1.28068F, -1.55062F, -2.34219F, -1.80866F, -1.33169F, -0.602222F, -1.89454F, - -1.22721F, -0.105414F, -0.143426F, -1.63046F, -1.51069F, -1.81981F, - -1.75802F, -1.86542F, -1.79719F, -1.7927F, -1.78949F, -0.0386438F, -1.80586F, - -1.52176F, -0.747871F, -0.0182935F, -0.0467936F, -1.79365F, -1.4509F, - -0.719578F, -1.85032F, -1.52975F, -0.456847F, -0.0645377F, 0.00314176F, - -0.166198F, -0.710695F, -0.0936774F, -0.481863F, -1.89108F, -2.79232F, - -1.86983F, -1.84022F, -1.878F, -1.92909F, -1.80163F, -1.44933F, -1.80333F, - -1.70135F, -1.64551F, -1.95511F, -1.7817F, -0.816628F, -0.82312F, -0.803933F, - -0.574111F, -0.522651F, -1.95232F, -1.55982F, -0.545184F, -0.511422F, - -1.79849F, -1.83683F, -1.69464F, -0.69775F, -0.562504F, -1.60228F, -1.66297F, - -0.0631858F, -0.0851359F, -1.78858F, -0.707135F, -0.335164F, -1.82796F, - -1.70442F, -0.0317939F, -0.757157F, -0.474809F, -0.462821F, -0.220129F, - -1.90386F, -0.401025F, -0.726764F, -0.479709F, -1.9312F, -1.12934F, - -0.756406F, -0.583053F, -0.511361F, -0.393112F, -0.0253081F, -0.75801F, - -1.21659F, -0.430635F, -0.908897F, -0.799192F, -0.32777F, -1.08574F, - -0.788687F, -0.0625925F, -1.73108F, -1.32924F, -1.65503F, -0.822792F, - -0.780673F, -0.56963F, -0.0183592F, -1.84154F, -1.66705F, -1.68191F, - -0.771438F, -0.618707F, -0.0256832F, -1.56809F, -0.692082F, -0.579258F, - -0.0515283F, -1.82475F, -1.87809F, -1.13838F, -1.83668F, -0.826834F, - -0.0252859F, -1.80277F, -0.0487872F, -0.690789F, -0.0391633F, -0.502528F, - -1.75839F, -1.82369F, -1.12566F, -1.83765F, -1.10551F, -1.139F, -1.69714F, - -1.7565F, -0.779244F, -0.463917F, -1.91149F, -0.745172F, -0.555517F, - -1.76013F, -1.62084F, -1.26446F, -0.126655F, -1.86675F, -3.3251F, -1.84583F, - -1.8611F, -0.0378916F, -1.83938F, -1.90433F, -1.09767F, -0.973703F, - -0.331915F, -0.025385F, -1.87664F, -1.89105F, -1.90294F, -1.18075F, - -0.0357356F, -0.055254F, -0.0552788F, -0.0346648F, -0.00673116F, -0.0411403F, - -0.0605123F, -1.83914F, -0.0562384F, -0.516441F, -0.102105F, -2.18034F, - -5.3015F, -2.53544F, -3.83085F, -1.14022F, -6.26587F, -2.56023F, -1.28839F, - -1.72341F, -0.609957F, -0.0473145F, -0.0484614F, -1.29676F, -1.0342F, - -0.412742F, -0.0647111F, -0.0535587F, -0.0442809F, -1.65402F, -0.774358F, - -1.75482F, -2.2753F, -1.25564F, -0.0153783F, -0.0543167F, -0.031506F, - -0.0443631F, -1.96291F, -0.606946F, -0.622807F, -0.525686F, -0.0203173F, - -0.0403363F, -0.0393192F, -0.0419493F, -0.621584F, -0.353804F, -0.024025F, - -0.0428101F, -0.794765F, -0.574221F, -0.51851F, -0.472994F, -0.464831F, - -0.272491F, -0.0352031F, -0.0580998F, -0.158843F, -2.26042F, -1.80985F, - -1.80281F, -1.79959F, -1.79899F, -1.21894F, -1.18273F, -1.14341F, -1.84363F, - -1.804F, -0.355747F, -0.354608F, -0.0346881F, -0.0180304F, -1.73528F, - -1.11818F, -1.18975F, -1.96126F, -1.79773F, -1.81519F, -1.78009F, -1.30254F, - -1.28968F, -1.23073F, -1.85622F, -1.81664F, -1.29418F, -0.595253F, - -0.927258F, -1.80401F, -1.80282F, -1.80747F, -1.78517F, -1.21925F, -1.18068F, - -1.79273F, -1.2119F, -1.79519F, -1.75038F, -1.74815F, -1.83825F, -1.78574F, - -1.74284F, -1.99658F, -21.5351F, -22.3747F, -1.83375F, -1.24553F, -1.17053F, - -1.96305F, -1.86709F, -1.77048F, -1.74522F, -1.34682F, -1.76249F, -0.626808F, - -0.2639F, -1.20663F, -1.17475F, -1.86466F, -1.19627F, -1.17501F, -1.88592F, - -0.265315F, -1.80596F, -1.82259F, -1.30876F, -1.26515F, -1.50622F, -1.24678F, - -0.685831F, -0.262766F, -0.0926027F, -1.46301F, -0.739678F, -0.188885F, - -0.580753F, -0.539331F, -0.271869F, -0.162444F, -1.78456F, -2.56317F, - -10.5391F, -7.08801F, -6.85436F, -7.4148F, -8.49878F, -1.26378F, -1.24938F, - -1.83872F, -1.20687F, -1.21918F, -1.20339F, -1.84573F, -1.69F, -1.6936F, - -1.17934F, -1.17671F, -1.17215F, -1.73654F, -1.85358F, -2.01219F, -1.77008F, - -1.94124F, -1.25345F, -1.64145F, -0.60079F, -1.20719F, -0.826064F, - -0.736716F, -0.651655F, -0.0823654F, -1.19142F, -0.654721F, -0.569812F, - -0.214766F, -0.190108F, -0.764517F, -0.652297F, -0.247682F, -1.20452F, - -0.648639F, -0.249986F, -0.0923839F, -0.522126F, -2.69831F, -1.23741F, - -1.24541F, -1.73875F, -1.28228F, -1.67508F, -1.71962F, -0.973769F, - -0.366842F, -0.559736F, -1.16087F, -1.16807F, -0.728628F, -0.644183F, - -0.493857F, -0.51909F, -5.07375F, -1.96357F, -0.362249F, -4.34477F, - -5.30117F, -1.65722F, -1.1044F, -1.79525F, -0.497773F, -0.222445F, - -0.180594F, -0.00165324F, -0.3784F, -0.387411F, -0.416119F, -0.288851F, - -1.67068F, -2.73406F, -0.869482F, -0.559241F, -1.59996F, -1.21491F, - -0.403079F, -0.640748F, -0.57033F, -0.637313F, -0.496877F, -0.0701621F, - -0.938876F, -0.642075F, -0.311652F, -1.24974F, -0.927302F, -1.62664F, - -1.85954F, -1.74722F, -1.20414F, -0.568802F, -1.79754F, -0.684044F, - -1.75258F, -0.644066F, -0.463845F, -0.636379F, -0.461754F, -4.78515F, - -4.62609F, -1.69854F, -1.61489F, -1.6751F, -1.19937F, -5.17947F, -4.85338F, - -1.73301F, -1.68635F, -1.757F, -1.58466F, -0.669598F, -0.394113F, -1.53427F, - -1.68917F, -1.31226F, -1.30141F, -1.19509F, -1.12808F, -0.38634F, -1.14978F, - -0.076083F, -1.25987F, -0.641052F, -0.189614F, -3.73066F, -1.77832F, - -1.24164F, -0.370208F, -0.278135F, -0.110977F, -3.5353F, -1.17609F, - -0.354385F, -0.365771F, -0.385945F, -0.305539F, -0.100357F, -1.77623F, - -1.7446F, -1.66328F, -1.19939F, -0.549349F, -0.43881F, -0.400722F, - -0.315422F, -0.295711F, -0.961793F, -0.592364F, -0.466972F, -0.437876F, - -0.434064F, -0.196523F, -0.13193F, -0.463298F, -0.434838F, -0.369945F, - -0.403843F, -1.26507F, -0.448703F, -0.112388F, -0.607456F, -1.90254F, - -1.79238F, -1.85357F, -0.949969F, -0.276257F, -0.184396F, -0.0132953F, - -1.73097F, -1.19321F, -0.544001F, -0.410002F, -0.313555F, -0.271813F, - -0.249244F, -0.182231F, -0.111155F, -0.595687F, -0.467758F, -0.43272F, - -0.403248F, -0.289602F, -0.283342F, -0.310513F, -0.465468F, -0.46116F, - -0.447899F, -0.335801F, -0.0942983F, -0.335394F, -1.76265F, -1.1418F, - -0.798852F, -0.544342F, -0.281732F, -0.293377F, -0.145172F, -0.0217765F, - -1.94448F, -1.64091F, -2.6839F, -1.15242F, -0.280645F, -0.299447F, - -0.272076F, -0.0891157F, -0.103308F, -0.0330757F, -1.14142F, -0.46599F, - -0.282114F, -0.131452F, -1.3328F, -0.437347F, -0.379533F, -0.178318F, - -0.0912226F, -0.0720918F, -0.443472F, -1.73997F, -1.72208F, -1.70372F, - -1.6905F, -1.80167F, -1.83339F, -1.80765F, -2.01931F, -1.81275F, -1.75931F, - -1.82997F, -1.79326F, -1.90866F, -1.80135F, -1.71593F, -1.74287F, -1.23379F, - -0.265676F, -0.277109F, -1.51181F, -1.91121F, -2.17867F, -1.82173F, - -0.563161F, -0.430324F, -0.367251F, -0.248872F, -0.538263F, -0.346257F, - -0.308945F, -0.276211F, -0.131727F, -0.19615F, -0.175579F, -0.122876F, - -2.7496F, -0.426631F, -0.365394F, -0.310097F, -0.34135F, -0.372836F, - -0.331791F, -0.255969F, -0.173072F, -0.144584F, -0.20502F, -0.0529656F, - -0.455906F, -0.521867F, -0.145431F, -1.77654F, -1.81667F, -1.76214F, - -2.11093F, -0.988163F, -1.1158F, -0.131615F, -1.81533F, -1.81824F, -1.67047F, - -0.469035F, -0.147858F, -0.119062F, -0.0344644F, -1.69967F, -1.78406F, - -1.2333F, -0.465695F, -0.283133F, -0.279577F, -0.279698F, -0.116476F, - -0.0283403F, -0.0341536F, -0.0368655F, -2.12809F, -0.460238F, -0.032339F, - -1.96638F, -0.611961F, -0.48831F, -0.465741F, -0.441242F, -0.409829F, - -0.381985F, -1.16091F, -0.575117F, -0.4622F, -0.373906F, -0.0760334F, - -1.25342F, -1.11936F, -0.332274F, -1.83702F, -1.79282F, -1.69216F, -1.82256F, - -1.81716F, -2.15096F, -1.70447F, -1.76638F, -1.70774F, -1.77035F, -0.361712F, - -0.268718F, -1.82339F, -1.84413F, -1.71097F, -1.7344F, -1.70779F, -1.79281F, - -1.22162F, -1.70579F, -1.78844F, -1.7733F, -1.12802F, -0.456809F, -0.265817F, - -0.0322111F, -0.0380737F, -0.0354641F, -0.617658F, -0.445808F, -0.175988F, - -0.0753331F, -0.0297316F, -1.96206F, -0.173558F, -0.109827F, -0.450323F, - -0.321123F, -0.149832F, -0.19073F, -0.182043F, -0.00446556F, -0.639513F, - -1.70604F, -1.83117F, -1.69493F, -1.76082F, -1.73496F, -1.7847F, -1.70735F, - -1.71909F, -1.81106F, -1.78861F, -1.78332F, -1.96492F, -1.8338F, -1.76583F, - -0.167198F, -0.117644F, -0.174904F, -0.107856F, -1.83416F, -1.73626F, - -1.81371F, -2.6161F, -1.78141F, -0.431446F, -0.42989F, -0.363812F, - -0.193726F, -0.117825F, -0.180956F, -0.0258487F, -0.0258212F, -0.0360559F, - -0.0355373F, -1.85892F, -1.74182F, -1.87421F, -1.8337F, -1.72414F, -1.18407F, - -0.433428F, -0.375508F, -0.211242F, -0.141247F, -0.119937F, -0.175829F, - -0.10589F, -0.077285F, -0.0750051F, -0.11159F, -0.0254511F, -0.0304495F, - -0.0270948F, -0.0275101F, -5.45376F, -1.85328F, -1.27694F, -0.463733F, - -0.463514F, -0.364933F, -0.383636F, -0.349492F, -0.223533F, -0.22167F, - -0.118883F, -0.120524F, -0.175349F, -0.0400031F, -0.0449953F, -0.0193748F, - -1.27416F, -1.30314F, -1.11209F, -0.4419F, -0.410263F, -0.166045F, - -0.113132F, -0.0223231F, -1.81198F, -0.312025F, -0.0777429F, -0.0478626F, - -0.447035F, -0.16872F, -0.333877F, -1.8399F, -1.80732F, -1.85676F, -1.68458F, - -1.80408F, -2.39722F, -1.73443F, -1.71204F, -1.72415F, -1.80747F, -1.70587F, - -1.1839F, -0.465982F, -0.329132F, -0.158442F, -0.164981F, -0.0303756F, - -0.0341993F, -0.0374774F, -1.82622F, -1.82512F, -1.8262F, -1.75604F, - -1.78656F, -1.77104F, -1.71782F, -1.15715F, -0.175342F, -0.170987F, - -0.182318F, -0.0594912F, -0.0684891F, -1.8092F, -1.90755F, -1.69042F, - -1.7397F, -1.81901F, -1.71326F, -1.42467F, -0.352867F, -0.341937F, - -0.338779F, -0.0739402F, -0.0335816F, -1.83108F, -0.764874F, -0.677527F, - -0.440459F, -0.402264F, -0.337448F, -0.157866F, -0.042377F, -0.0532381F, - -0.0310961F, -1.66886F, -0.0259452F, -0.0360871F, -1.60782F, -1.25705F, - -0.0225665F, -0.0155365F, -1.80328F, -1.80811F, -1.77939F, -1.86318F, - -2.18251F, -1.81983F, -1.83263F, -1.8554F, -1.809F, -1.78736F, -0.371915F, - -0.33578F, -1.76863F, -1.74278F, -1.78871F, -1.77184F, -1.711F, -1.33516F, - -0.183491F, -0.127988F, -1.13535F, -0.0197254F, -1.25063F, -1.08941F, - -1.16681F, -1.7594F, -1.76481F, -1.83079F, -1.75908F, -1.80335F, -1.77662F, - -1.76554F, -1.80688F, -1.79694F, -1.78062F, -1.76906F, -1.74627F, -1.75039F, - -1.795F, -1.96925F, -1.81634F, -1.82456F, -1.78461F, -1.43597F, -1.28528F, - -0.140658F, -1.74504F, -1.7312F, -1.76251F, -1.67988F, -1.38878F, -0.362923F, - -0.202437F, -1.74661F, -1.42996F, -0.200322F, -0.330342F, -0.198826F, - -0.0556783F, -0.377374F, -0.368551F, -1.73919F, -0.464213F, -1.68231F, - -0.182544F, -1.69346F, -1.30906F, -0.511563F, -0.17086F, -0.146143F, - -0.171471F, -1.20792F, -0.70276F, -1.43782F, -1.44521F, -1.43698F, -1.32659F, - -1.8089F, -1.47916F, -3.5147F, -1.81661F, -1.80572F, -1.836F, -1.8615F, - -1.79021F, -1.85785F, -1.8988F, -1.42485F, -0.368201F, -1.50385F, -1.80836F, - -2.68424F, -1.9068F, -1.93806F, -1.83105F, -1.84234F, -1.77546F, -1.58771F, - -1.84778F, -1.53351F, -1.76632F, -0.366415F, -0.285829F, -0.150445F, - -1.49657F, -1.42491F, -1.88729F, -1.81642F, -1.83975F, -1.75602F, -2.92453F, - -1.77925F, -1.80982F, -3.63124F, -1.79603F, -1.8062F, -1.80644F, -1.81587F, - -1.39264F, -1.7813F, -1.72291F, -1.72322F, -0.194266F, -0.160312F, - -0.193738F, -1.5273F, -0.183762F, -0.169286F, -1.69335F, -1.574F, - -0.0675591F, -1.81802F, -0.173054F, 0.662441F, -1.49246F, -1.164F, -0.65609F, - -1.84329F, -1.76096F, -16.6458F, -1.8816F, -4.04634F, -1.81704F, -1.53622F, - -1.79106F, -1.82761F, -1.77081F, -1.80196F, -1.85734F, -1.69504F, -1.73332F, - -9.58086F, -2.06882F, -1.85648F, -1.78682F, -1.80023F, -2.46083F, -1.59195F, - -1.77342F, -1.67701F, -1.66773F, -1.70806F, -0.94356F, -0.459064F, - -0.336249F, -0.29319F, -0.011089F, -1.43941F, -1.85889F, -0.92347F, - -0.00295222F, -1.59164F, -1.65203F, -1.24466F, -1.04547F, -1.77271F, - -1.14772F, -1.28379F, -0.532163F, -1.44291F, -1.82663F, -1.77406F, -1.28285F, - -1.67974F, -1.82356F, -5.73395F, -1.84376F, -2.84254F, -1.78491F, -1.76084F, - -1.8118F, -1.37693F, -1.8059F, -15.6962F, -2.1397F, -1.27643F, -1.85911F, - -1.82073F, -1.82543F, -1.79736F, -1.83012F, -1.36593F, -1.35903F, -3.09826F, - -1.29151F, -1.31641F, -2.28279F, -1.79786F, -1.80712F, -1.72961F, -1.6878F, - -2.77436F, -1.81913F, -4.24048F, -1.9311F, -1.35743F, -1.33204F, -1.41411F, - -1.67256F, -1.79022F, -1.74148F, -1.27952F, -0.335685F, -0.34359F, - -0.0744395F, -1.57881F, -1.79926F, -1.38756F, -1.51769F, -1.74794F, - -1.21557F, -1.6079F, -1.31871F, -1.23239F, -0.0982522F, -1.28622F, - -0.583717F, -1.54072F, -1.51329F, -1.45512F, -1.83321F, -1.52905F, -1.77358F, - -1.79698F, -1.78972F, -1.80743F, -1.80363F, -1.77962F, -1.81854F, -1.82058F, - -1.8221F, -1.40124F, -1.81768F, -1.45083F, -1.41591F, -4.16776F, -1.80428F, - -1.80478F, -3.18507F, -1.43121F, -1.41148F, -1.40971F, -1.76131F, -1.40163F, - -1.65703F, -1.84921F, -1.40998F, -1.80559F, -1.87113F, -1.78203F, -1.80819F, - -1.78146F, -1.83901F, -1.39675F, -1.81346F, -1.77826F, -1.39335F, -1.79559F, - -3.28759F, -1.30907F, -1.82669F, -1.80947F, -1.42318F, -1.42462F, -1.76589F, - -1.80657F, -0.191241F, -1.50741F, -1.80096F, -1.76551F, -1.43522F, -3.06498F, - -1.41279F, -1.83146F, -1.24916F, -0.19266F, -0.111524F, -0.0597781F, - -0.159652F, -1.26614F, -0.366573F, -0.860374F, -0.628306F, -5.56405F, - -4.6411F, -1.49635F, -1.4629F, -1.49725F, -1.45541F, -1.79237F, -1.79587F, - -1.81392F, -1.80236F, -1.62718F, -1.47714F, -1.81636F, -1.49317F, -1.4701F, - -1.47818F, -1.80935F, -1.47211F, -1.78265F, -1.80953F, -1.82404F, -2.06742F, - -1.82851F, -1.80673F, -2.09205F, -1.78779F, -2.5633F, -1.30866F, -1.26275F, - -1.53937F, -1.56688F, -1.5047F, -1.47861F, -1.48296F, -1.45952F, -1.45925F, - -1.45895F, -3.77977F, -1.43628F, -1.83529F, -1.4694F, -1.786F, -1.66389F, - -1.32482F, -0.210416F, -1.8248F, -1.80695F, -0.0374576F, -1.79293F, - -0.767864F, -0.166859F, -0.83252F, -0.619582F, -0.363994F, -0.102527F, - -1.84191F, -1.49334F, -1.74227F, -1.4836F, -1.53232F, -1.80515F, -1.78465F, - -1.61733F, -1.84049F, -1.82795F, -1.80874F, -1.81297F, -1.77948F, -1.80973F, - -1.83168F, -1.42605F, -1.75092F, -1.26639F, -1.56739F, -1.81376F, -1.50315F, - -1.4784F, -1.50284F, -1.60682F, -1.79455F, -1.59607F, -1.82868F, -1.82515F, - -1.83004F, -1.53729F, -1.82652F, -1.91387F, -0.771094F, -0.217082F, - -1.53313F, -1.4639F, -0.801292F, -1.38791F, -0.526738F, -0.864475F, - -1.96284F, -1.48424F, -1.50717F, -1.64848F, -2.09603F, -1.76292F, -1.4803F, - -1.49387F, -1.67613F, -1.81327F, -8.07783F, -1.70634F, -1.53523F, -3.05542F, - -1.88777F, -1.72694F, -1.8691F, -1.8266F, -1.32415F, -1.18619F, -1.30369F, - -1.69942F, -1.63786F, -1.45203F, -1.52593F, -1.50095F, -1.85461F, -1.7375F, - -1.55807F, -1.4871F, -1.26191F, -1.34516F, -1.31209F, -1.38326F, -1.3087F, - -1.41787F, -1.09534F, -1.29274F, -1.86345F, -1.60309F, -1.70464F, -1.55412F, - -0.828058F, -0.558964F, -1.48853F, -1.78564F, -2.05485F, -1.61028F, - -1.90895F, -1.80749F, -1.44421F, -1.77264F, -1.52503F, -1.84805F, -0.213376F, - -1.16929F, -5.474F, -1.78001F, -1.54113F, -1.75643F, -0.182984F, -1.05663F, - -1.36679F, -1.8345F, -1.77375F, -1.77486F, -1.55003F, -1.61602F, -1.71015F, - -1.70904F, -1.78866F, -1.82975F, -1.712F, -1.70204F, -1.67519F, -1.81411F, - -1.19091F, -1.73998F, -1.65923F, -1.8075F, -1.77251F, -1.9696F, -1.71953F, - -1.84187F, -1.77755F, -1.66771F, -1.74627F, -1.7299F, -1.42945F, -1.70535F, - -1.92647F, -1.79802F, -2.02344F, -1.51122F, -1.90486F, -1.27307F, -1.3236F, - -0.931741F, -0.198019F, -0.191807F, 0.130225F, -1.68414F, -1.44794F, - -1.34647F, -1.60751F, -0.927169F, -1.77827F, -1.27566F, -1.40578F, -1.79236F, - -1.39449F, -1.41867F, -0.893197F, -0.0355906F, -1.50874F, -1.53533F, - -1.66093F, -1.61798F, -0.883824F, -0.286165F, -2.48568F, -1.51559F, - -0.216961F, -0.665387F, -1.21759F, -1.52983F, -1.47134F, -1.45311F, - -1.22161F, -1.58686F, 0.0339316F, -1.74783F, -1.7702F, -1.56596F, -0.923178F, - -1.78277F, -1.9011F, -0.747264F, 0.12788F, -0.953992F, -1.51037F, -1.03037F, - -0.98986F, -0.912372F, -0.00295994F, -0.896938F, -0.196176F, -0.168761F, - -0.159331F, -1.68323F, -1.74782F, -1.79992F, -1.86622F, -1.7876F, -1.69515F, - -1.43541F, -1.09521F, -1.7764F, -1.75975F, -1.98952F, -1.03724F, -0.0217749F, - -1.81849F, -1.41452F, -1.11788F, -1.00419F, -0.224995F, -1.43295F, - -0.896742F, -1.11414F, -1.07224F, -0.175643F, -0.751828F, -1.78742F, - -1.78361F, -1.66894F, -1.55803F, -1.75619F, -1.73151F, -1.80585F, -1.83101F, - -1.82524F, -1.05995F, -0.990357F, -0.974603F, -0.659929F, -0.0732362F, - 0.0319416F, -1.76521F, -1.99699F, -1.79013F, -1.08733F, -1.03795F, -2.11171F, - -1.64723F, -1.39724F, -1.78706F, -1.47292F, -1.5292F, -1.53006F, -1.52746F, - -1.02288F, -0.923053F, -1.786F, -1.73122F, -1.09401F, -1.48112F, -1.17625F, - -1.04845F, 0.102777F, -0.925846F, -0.952217F, 0.0112424F, -1.80037F, - -1.81151F, -1.65255F, -1.83263F, -0.775232F, -9.1855F, -0.0771739F, - -1.90055F, -1.96355F, -1.49913F, -1.36327F, -1.89806F, -1.06891F, -0.226187F, - -1.33261F, -1.26946F, 0.0320607F, -1.14559F, -1.11931F, -0.845432F, - -0.439906F, -0.127677F, -0.0787388F, -0.776911F, -0.100105F, -1.8048F, - -1.52024F, -1.53157F, -1.50676F, -1.34296F, -1.39679F, -1.34078F, -1.76859F, - -1.64121F, -0.919916F, -1.80919F, -1.8043F, -3.90941F, -1.78902F, -1.91122F, - -0.450812F, -1.80604F, -2.17759F, -2.24831F, -3.12896F, -2.14355F, -1.5092F, - -0.448366F, 0.13846F, -1.72615F, -1.38867F, -1.51068F, -1.40561F, -1.43185F, - -0.178577F, -1.55867F, -1.39139F, -1.04794F, -1.02944F, -0.786F, 0.0979574F, - -1.66934F, -0.928443F, -0.205779F, -1.03112F, 0.0485816F, -0.954136F, - -1.81303F, -3.84728F, -2.52588F, -2.5483F, -1.57491F, -4.14463F, -1.51326F, - -1.94967F, -1.04078F, -0.960588F, -5.97671F, -1.58184F, -1.3859F, -2.96567F, - -1.42361F, -1.43131F, -1.75306F, -1.79777F, -1.06648F, -0.808022F, - -0.674666F, -1.60356F, -1.18997F, -0.95386F, -0.906286F, -0.81638F, - -1.82516F, -1.51862F, -1.03659F, -1.01397F, -0.438539F, -1.43173F, -1.677F, - -0.583171F, -0.438616F, -0.58831F, -0.975688F, -0.0668301F, -0.628628F, - -0.652481F, -1.78944F, -1.7627F, -1.72608F, -1.3614F, -1.76693F, -1.79146F, - -1.77441F, -1.78344F, -0.0767669F, -0.133439F, -1.77467F, -1.7615F, - -1.70898F, -1.74604F, -1.75071F, -1.41384F, -1.41754F, -1.71599F, -1.26592F, - -1.85148F, -0.0238243F, 0.0377324F, -1.87295F, -1.4861F, -1.2799F, -1.06105F, - -0.76356F, -0.657895F, -1.73847F, -1.48396F, -1.37218F, -0.18236F, -1.90168F, - -1.4416F, -0.606687F, -0.831802F, -0.701511F, -1.74727F, -1.80762F, - -1.77776F, -1.76826F, -1.76431F, -0.783085F, -0.341768F, -0.0504082F, - -1.8198F, -1.82069F, -1.44487F, -1.07142F, -0.148401F, -1.70492F, -1.74573F, - -1.70746F, -0.779347F, -0.0462762F, -0.0782369F, -1.85137F, -1.63729F, - -1.66364F, -1.1136F, -0.741072F, -0.00892591F, 0.056398F, -1.88175F, - -0.58332F, -1.89364F, -0.777261F, -0.654504F, -0.759826F, -0.0307598F, - -0.575142F, -1.81785F, -1.81504F, -1.81677F, -1.82882F, -1.80503F, -1.7904F, - -1.80846F, -1.03177F, -1.02135F, -1.00458F, -0.923511F, -0.967328F, - -0.70521F, -0.700246F, 0.0512943F, -1.75577F, -1.10776F, -1.03219F, - 0.308739F, -5.58072F, -1.10853F, -1.09431F, -0.641214F, -0.433241F, - -0.000700959F, 0.00658013F, 0.0330467F, 0.0773511F, -0.115888F, -0.115353F, - -0.0665223F, -0.0549686F, -0.0868003F, -0.23963F, -0.0591418F, -1.84443F, - -1.67158F, -1.33045F, -1.45974F, -1.37662F, -1.08765F, -1.1059F, -1.12725F, - -1.05192F, -0.762618F, -0.428362F, -0.129805F, -0.0548223F, -0.0651426F, - -1.79174F, -1.10759F, -0.872484F, -0.0177983F, -0.0776333F, -1.80543F, - -1.42882F, -0.957496F, -0.74264F, -0.00258643F, -0.099647F, -0.449652F, - -0.229007F, 0.00165914F, 0.0271142F, -2.05532F, -0.984795F, -0.979149F, - -0.726005F, 0.184764F, -0.0176013F, -1.73322F, -1.79098F, -1.73641F, - -3.11023F, -3.27878F, -1.82893F, -1.84717F, -1.82554F, -1.8138F, -1.81059F, - -1.05068F, -0.0638457F, -1.85255F, -0.451797F, -0.0327988F, -2.04967F, - -1.79303F, -1.81774F, -1.90802F, -1.10921F, -1.01354F, -1.83893F, -1.81912F, - -2.12169F, -0.948195F, -0.20416F, -1.05975F, -0.798816F, -0.711307F, - -0.0162295F, 0.112413F, -1.11524F, -1.12267F, -1.80304F, -1.80844F, - -1.79977F, -1.79874F, -1.82309F, -1.80666F, -1.79619F, -1.79647F, -1.78721F, - -1.00909F, -1.0855F, -0.902407F, -0.822036F, -0.912979F, -0.933342F, - -0.900948F, -1.00542F, -1.06435F, -0.979911F, -2.06422F, -1.85004F, - -1.85778F, -1.82024F, -1.78729F, -0.904782F, -0.0206391F, -0.966643F, - -0.0954912F, -1.73697F, -1.77157F, -1.77363F, -1.8457F, -1.85195F, -1.83047F, - -5.06504F, -4.99295F, -1.72422F, -1.78402F, -0.0748158F, -1.56637F, - -1.79283F, -0.792639F, -1.79398F, -1.6654F, -1.75818F, -0.929281F, -1.60649F, - -1.79224F, -1.8081F, -1.78162F, -1.813F, -1.79251F, -0.739413F, -1.82635F, - -1.01641F, -0.89684F, -1.95776F, -1.7772F, -1.00785F, -0.93964F, -1.64454F, - -1.10823F, -0.0743721F, -0.0621604F, -1.12428F, -0.202645F, 0.723305F, - -1.10788F, -0.0889089F, -1.86101F, -10.4133F, -1.80748F, -0.728655F, - -0.0697561F, -1.84248F, -0.778864F, -0.941679F, -1.8302F, -1.83991F, - -0.985021F, -0.912917F, 0.0424465F, -1.55569F, -1.39997F, -1.00373F, - -0.880273F, -2.33767F, -1.0764F, -1.62872F, -1.67544F, -1.84791F, -1.67829F, - -1.87879F, -6.56955F, -1.70766F, -1.70029F, -1.72258F, -0.9319F, -0.811925F, - -0.0733367F, -0.229798F, 0.0516017F, -0.113855F, -0.125057F, 0.115498F, - -1.59297F, -1.72375F, -1.78497F, -6.18853F, -1.86485F, -1.78067F, -1.5451F, - -0.369796F, -0.136284F, -0.0998574F, -0.0606369F, -1.5208F, -1.59894F, - -2.13717F, -6.45607F, -1.58877F, -1.79512F, -0.846726F, -0.214512F, - -0.11402F, -0.148576F, -0.0613143F, -1.0791F, -0.781884F, -0.221516F, - -0.0556343F, -1.62652F, -1.62587F, -1.61413F, -0.993997F, -0.978549F, - -0.940533F, -0.207924F, 0.0837283F, -0.127499F, -0.0720193F, -0.943357F, - -0.317176F, -0.0605464F, -1.85823F, -1.7813F, -1.79566F, -1.78612F, - 0.0297057F, -0.141929F, -1.72539F, -0.0707206F, -0.176479F, -1.97148F, - -1.86081F, -0.16716F, -0.163313F, -1.86961F, -1.71449F, -0.985463F, - -0.674768F, -0.176181F, -0.0687413F, -1.70657F, -1.72631F, -0.969177F, - -0.224818F, -0.179484F, -0.0770662F, -0.316713F, -0.179692F, -0.159669F, - 0.0301248F, -0.0613339F, -1.25824F, -1.44778F, -11.9526F, -13.1022F, - -1.8428F, -0.98201F, -0.985059F, -0.335923F, -0.148453F, -7.65285F, - -2.98935F, -1.0468F, -0.329382F, -0.321681F, -0.168699F, -1.4463F, -1.48902F, - -2.1234F, -1.69582F, -0.794644F, -0.349617F, -0.127572F, -0.0862543F, - -1.49395F, -0.996279F, -0.324294F, 0.0751855F, -1.76317F, -2.12381F, - -0.796728F, -0.351325F, -0.106197F, 0.0400807F, -1.98825F, -0.995049F, - -0.0275643F, -2.08647F, -0.727456F, -1.14651F, -1.81746F, -7.70769F, - -12.2984F, -4.94412F, -3.18808F, -1.78465F, -2.77804F, -7.16892F, -1.81458F, - -1.97974F, -1.79025F, -0.372697F, -0.365754F, -2.43394F, -6.47671F, - -6.36354F, -1.91061F, -1.94165F, -0.267005F, 0.0438641F, -0.146677F, - 0.11913F, -1.44588F, -3.07054F, -2.56975F, -1.7769F, -1.85361F, -1.84115F, - -0.326088F, -0.172492F, -0.229192F, -0.186649F, -1.60893F, -1.6979F, - -1.7904F, -0.0811009F, -1.84982F, -0.843463F, 0.165801F, -1.61853F, - -1.11516F, -0.222004F, -1.06371F, -1.45595F, -4.65825F, -8.50886F, -8.42145F, - -7.02892F, -1.78814F, -0.997484F, -0.995847F, -0.972903F, -0.765962F, - 0.0163255F, -0.0922194F, -8.41158F, -2.04602F, -8.4043F, -8.44142F, - -0.958055F, -0.0602346F, -1.82321F, -5.20635F, -4.0009F, -2.11444F, - -1.08684F, -0.952365F, -0.972438F, -0.203133F, -0.170723F, 0.125385F, - -1.11059F, -0.983784F, -0.991174F, -0.947164F, -0.742133F, -0.435836F, - -0.128801F, -0.714292F, -0.154846F, -0.951024F, -0.734106F, -1.79959F, - -10.0039F, -2.84383F, -8.38392F, -0.988705F, -0.88614F, -0.160588F, - -0.17119F, -1.26703F, -2.11821F, -1.81173F, -14.5712F, -0.855993F, - -0.165009F, -0.0644608F, -1.55763F, -1.84111F, -0.928418F, -0.92781F, - -0.167631F, -0.797592F, -0.203949F, -0.923781F, -0.83532F, -1.81023F, - -1.79034F, -1.79957F, -1.78444F, -1.79799F, -1.90261F, -1.84608F, -0.2023F, - -0.183194F, -1.70139F, -1.01763F, -0.99802F, -0.630683F, -0.157171F, - -0.22783F, -0.172551F, -0.0774996F, -1.94885F, -1.61472F, -1.73039F, - -1.81241F, -0.686511F, -0.14654F, -1.83251F, -0.175606F, 0.0775973F, - -0.902967F, -0.695259F, -1.14436F, -1.74237F, -1.81217F, -1.82351F, - -1.78535F, -1.77509F, -1.77776F, -1.80841F, -0.986817F, -0.984361F, - -0.196581F, -0.843398F, -0.745031F, -1.77223F, -0.190463F, -0.182166F, - -0.0947647F, -1.79917F, -1.61338F, -1.81115F, -0.914794F, -0.0580914F, - -0.976283F, -0.151991F, -0.220418F, -0.114581F, -1.75059F, -1.65857F, - -1.7817F, -1.62856F, -1.65317F, -1.68093F, -1.82336F, -1.73182F, -1.82082F, - -1.75355F, -2.00683F, -2.09798F, -1.79186F, -1.95946F, -1.78405F, -1.90585F, - -1.8335F, -6.92655F, -1.8208F, -1.85513F, -1.84527F, -1.82062F, -1.81859F, - -1.86437F, -1.83071F, -1.79672F, -1.80052F, -1.80468F, -1.79703F, -1.0258F, - -0.967492F, -0.867859F, -0.750808F, 0.0639562F, -0.00205407F, -0.166082F, - -1.76329F, -1.6951F, -1.80779F, -1.86409F, -1.93724F, -1.79717F, -1.77238F, - -1.84494F, -0.930189F, -0.824732F, -1.70748F, -1.76385F, -1.79461F, - -1.88925F, -0.941899F, -0.871474F, -0.79397F, -0.750973F, 0.0327934F, - -0.161754F, -0.168074F, -0.150719F, -0.93802F, -0.769397F, -0.75943F, - 0.0423172F, 0.139274F, -1.81532F, -1.05698F, -0.845163F, -0.178515F, - -0.904306F, -0.15416F, -0.106534F, 0.066624F, -0.172958F, -1.76417F, - -1.25573F, -0.986988F, -0.769641F, -1.7284F, -1.03563F, -0.423035F, - -1.27489F, -1.04405F, 0.0378221F, -0.940442F, 0.00105505F, 0.0334907F, - 0.0208631F, -0.952519F, -0.727774F, -1.0143F, -1.72137F, -1.9417F, -1.85001F, - -1.00452F, -1.00078F, -0.981073F, -0.16205F, -1.94347F, -1.27211F, - -0.873341F, -0.164997F, -0.138827F, -0.180608F, -0.419731F, 0.0104228F, - -0.168002F, -0.173036F, -0.163225F, -0.117837F, -0.181571F, -0.0836961F, - -1.04409F, 0.0461135F, -0.165251F, -1.00235F, 0.0580481F, -0.147462F, - -1.03373F, -0.156362F, -1.80028F, -1.77771F, -1.75258F, -1.68728F, -1.70052F, - -1.72F, -1.75079F, -1.04764F, -0.947628F, -0.811059F, -0.726988F, -0.372695F, - -0.427559F, 0.373542F, -1.71386F, -1.70127F, -1.76515F, -0.973465F, - -0.905387F, -0.792107F, 0.0252469F, 0.235999F, 0.370358F, -0.910151F, - -0.701203F, -0.426992F, 0.021185F, 0.0323524F, 0.00170888F, 0.0531132F, - -0.945784F, -0.766058F, -0.977546F, -0.792317F, -0.00120621F, -0.733625F, - -1.76059F, -1.93897F, -1.76224F, -3.22232F, -3.29361F, -1.81718F, -1.84237F, - -1.01609F, 0.0261229F, -1.76501F, -1.80902F, -1.02308F, -1.02577F, - -0.902672F, -0.954131F, -0.700886F, -0.102619F, 0.00997866F, -0.83589F, - -1.01757F, -0.984407F, -0.780333F, -0.771114F, 0.0448299F, -0.943071F, - 0.14222F, -1.04615F, -0.821349F, -0.704103F, 0.053824F, 0.176681F, -1.07123F, - -1.95516F, -1.91306F, -1.76824F, -1.9003F, -1.83299F, -1.8461F, -1.70939F, - -0.975385F, -0.852572F, 0.0408826F, 0.0375896F, -1.90915F, -1.84219F, - -1.81108F, -1.78038F, -2.24241F, -0.977105F, -0.888879F, -0.726377F, - -0.771911F, -0.0776824F, 0.00872239F, 0.0416436F, -1.81489F, -0.791386F, - -0.759496F, -0.700374F, -0.00809458F, -0.944397F, -0.706914F, 0.00694264F, - -0.830665F, -0.79023F, 0.0863806F, -0.644037F, 0.0517624F, -1.03708F, - -0.868401F, -0.657393F, -1.79882F, -1.78375F, -1.67467F, -1.88882F, - -1.78992F, -0.900999F, -0.959898F, -1.85871F, -1.86794F, -1.97587F, - -1.01859F, -0.915625F, -0.937467F, -0.831135F, -0.985702F, -0.975111F, - -0.815319F, -0.0777181F, -0.859936F, -0.924775F, -0.682662F, -0.910351F, - -0.827301F, -0.778252F, -0.0132044F, 0.0507967F, 0.238009F, -0.173263F, - -0.974116F, -1.01094F, -1.80662F, -1.8196F, -1.71262F, -1.79985F, -1.3093F, - -1.05723F, -0.994254F, -0.975048F, -0.862901F, -0.973426F, -0.921563F, - -0.922668F, -0.811945F, -0.763569F, -0.69941F, -0.720939F, -0.723545F, - -0.020173F, -0.0714721F, 0.090628F, 0.482351F, -1.70242F, -1.81705F, - -1.32423F, -1.07956F, -0.941324F, -0.875046F, -0.812731F, -0.784287F, - -0.693587F, -0.545039F, -0.0418986F, 0.0409014F, -1.36149F, -1.0151F, - -1.10486F, -0.974295F, -0.972375F, -0.865587F, -0.8986F, -0.927593F, - -0.778914F, -0.783426F, -0.774903F, -0.729164F, 0.0068746F, -0.0119935F, - 0.000842061F, -0.827631F, -0.706123F, -0.0687628F, -0.0155201F, -0.970162F, - -0.736372F, -0.00302366F, 0.0474228F, 0.0411089F, -0.971357F, -0.983896F, - -0.80671F, 0.0353346F, -0.0391243F, 0.00354427F, -1.33383F, -0.900117F, - -0.646146F, -1.74655F, -1.7847F, -1.39133F, -1.74003F, -1.72782F, -1.77835F, - -1.00827F, -0.980091F, -1.02186F, -1.03684F, -0.985076F, -0.914647F, - -0.886989F, -0.654873F, -0.649612F, -0.102969F, -0.06653F, -0.00296827F, - -0.0101199F, -0.0469652F, -0.0390812F, -0.0444154F, -0.0268019F, 0.0114767F, - 0.0126354F, -0.0160092F, 0.0314856F, -0.0168326F, -0.0137861F, -1.71271F, - -1.71433F, -1.7758F, -1.28289F, -0.976083F, -0.790276F, -0.745469F, - -0.740454F, -0.694387F, -0.656403F, -0.090825F, -0.0395444F, -0.0232224F, - -0.0238405F, 0.01018F, 0.0312758F, 0.0538787F, -0.0156844F, -1.88809F, - -0.970954F, -0.972931F, -0.937372F, -0.848163F, -0.751654F, -0.0856987F, - -0.00877044F, -0.0911909F, 0.0349807F, -0.0158788F, -0.0192486F, -0.0307987F, - 0.0100526F, 0.0107439F, 0.239645F, -1.83708F, -1.4264F, -1.00402F, - -0.961683F, -0.832307F, -0.654531F, -0.62696F, -0.634405F, -1.47575F, - -0.707311F, -0.59288F, -0.430834F, 0.00595142F, -0.0279571F, 0.366334F, - -1.01542F, -1.01008F, -0.695998F, -0.0662156F, 0.0348176F, -0.120722F, - 0.132728F, -0.00710089F, -1.02969F, -0.984874F, -0.6128F, -0.0111008F, - -0.00505494F, -0.685623F, -0.0967757F, -1.82377F, -1.83164F, -1.77228F, - -1.88263F, -1.87778F, -1.90923F, -1.88994F, -1.0387F, -1.00024F, -1.00799F, - -0.985667F, -0.883254F, -0.951133F, -1.22152F, -0.750652F, -0.677711F, - -0.738921F, -0.705825F, -0.679773F, -0.657801F, -0.704536F, -0.0779488F, - 0.117007F, 0.0314346F, 0.366265F, -1.81724F, -1.85586F, -1.84357F, -1.97147F, - -1.8783F, -1.70155F, -1.87121F, -1.32554F, -1.09831F, -1.02656F, -0.9905F, - -1.09707F, -0.968131F, -1.05649F, -0.901389F, -0.726491F, -0.611869F, - -0.713996F, 0.00966763F, 0.00106078F, 0.0223579F, -1.58174F, -0.953694F, - -1.04881F, -0.960692F, -0.725238F, 0.0617256F, -0.0585653F, 0.0247693F, - 0.0289233F, -0.0192945F, -1.13517F, -1.20668F, -0.988388F, -0.968259F, - -0.830665F, -0.0425946F, -0.0224232F, -1.26196F, -1.0975F, -1.23568F, - -0.824047F, -0.814359F, -0.875398F, -0.794309F, -0.636372F, 0.0122636F, - -0.0101512F, 0.231938F, -1.2151F, -1.22381F, -1.01946F, -0.962057F, - -0.895056F, 0.245739F, 0.00903446F, 0.052317F, -0.780804F, 0.0122925F, - -0.992874F, -0.0102769F, 0.17581F, -1.87202F, -1.79829F, -1.93454F, - -1.87517F, -1.874F, -1.84123F, -1.00775F, -1.03087F, -1.87959F, -1.92487F, - -1.76441F, -1.06958F, -0.929399F, 0.011093F, -1.54139F, -1.88039F, -2.06274F, - -1.88995F, -0.792663F, -0.116428F, 0.037039F, -0.921572F, -0.784689F, - -1.02625F, -0.979708F, -0.0262522F, -0.75698F, -0.438704F, -0.749893F, - -0.72404F, -1.76359F, 0.038504F, 0.0435115F, 0.3763F, -0.983584F, -1.01213F, - -0.928331F, -0.0723056F, 0.0352489F, -0.0201912F, 0.240944F, -0.791734F, - 0.00875615F, 0.0591171F, 0.160215F, 0.123281F, -0.872952F, -0.868314F, - -0.870402F, -1.00225F, -0.993F, -0.759257F, -0.744119F, -1.29466F, - -0.650362F, -1.77228F, -1.08079F, -0.995358F, -0.920756F, -0.87458F, - -0.794333F, -0.74321F, -0.845034F, -0.063651F, -1.04725F, -0.686511F, - 0.349566F, -0.823422F, -0.841392F, 0.0373423F, -1.74711F, -1.69822F, - -1.26556F, -1.01457F, -0.995865F, -0.954185F, -0.758017F, 0.0468718F, - -1.01135F, -1.00021F, 0.00195807F, -0.979082F, 0.226474F, -1.08635F, - -0.722048F, -1.00583F, -1.00492F, -0.951737F, -0.984078F, -0.842986F, - 0.0262699F, -1.00122F, -0.956845F, -0.801251F, -0.729236F, -0.622848F, - -0.626199F, -1.02033F, -1.00252F, -0.89084F, -0.702159F, -0.646787F, - -1.13397F, -1.02466F, -0.490852F, -0.0334982F, -0.0146892F, 0.0111381F, - -0.0530686F, -0.0431916F, -0.0702447F, -1.16724F, -1.11163F, -1.21942F, - -0.965962F, -0.960561F, -0.0325205F, -0.020612F, -1.68733F, -2.26993F, - -1.382F, -1.06948F, -2.02581F, -1.06497F, -0.997279F, -0.696415F, - -0.0668608F, -1.10134F, -1.02487F, -0.639942F, -0.678053F, 0.0847969F, - -0.0494755F, -1.25997F, -0.824637F, -1.82258F, -0.6519F, -0.313288F, - -0.438493F, -0.0195352F, -0.636346F, -0.44231F, -0.0638977F, -0.0561983F, - -1.09642F, -1.01714F, -0.892989F, -0.694871F, -0.0403027F, -0.0199917F, - -0.910511F, -0.798242F, -0.757851F, -0.65219F, -0.0319613F, -0.962602F, - -0.0388502F, -0.862515F, -0.692607F, -0.757017F, -0.445387F, -0.110368F, - -0.155085F, -0.583465F, 0.630537F, -1.34304F, -1.1075F, 0.584037F, - -0.0279378F, -1.4994F, -0.798088F, -0.50476F, -0.484196F, -0.359509F, - -0.35027F, -0.426532F, -0.258063F, -0.208506F, -0.382431F, 0.382285F, - -0.0289351F, -1.44225F, -0.508686F, -0.504386F, -0.498512F, -0.494459F, - -0.483515F, -0.463288F, -0.383652F, -0.433007F, -0.238415F, -0.216631F, - -0.40082F, -0.148234F, 0.0408773F, 0.257556F, -0.380534F, -0.366147F, - -0.36874F, 0.0664891F, -0.0209607F, -0.0285925F, -0.0268862F, -0.531973F, - -0.513754F, -0.503531F, -0.506325F, -0.498585F, -0.488863F, -0.489171F, - -0.484009F, -0.477385F, -0.417779F, -0.410994F, -0.400413F, -0.379207F, - -0.412386F, -0.435556F, -0.431322F, -0.421486F, -0.113845F, -0.388107F, - -0.385695F, -0.385261F, -0.375992F, -0.374292F, -0.368083F, -0.344889F, - -0.350688F, -1.49559F, -1.52306F, -1.49351F, -1.24584F, -0.592927F, - -0.568635F, -0.522372F, -0.503652F, -0.497953F, -0.489289F, -0.471631F, - -0.458533F, -0.436118F, -0.421438F, -0.0260691F, -0.385955F, -0.38482F, - -0.380774F, 0.0457762F, -0.372454F, -0.36208F, 0.719506F, -1.4636F, - -0.543112F, -0.870785F, -0.880657F, -0.596498F, -0.488934F, -0.471431F, - -0.423369F, -0.0995026F, -0.193489F, -0.0873761F, -0.0677469F, -0.371176F, - -0.54279F, -0.572039F, -0.459842F, -0.397252F, -0.811256F, -0.395884F, - -0.0711181F, 0.837998F, -0.52619F, -0.522662F, -0.396133F, -0.37517F, - 0.627094F, -1.43128F, -0.519721F, -0.510326F, -0.494175F, -0.483219F, - -0.133213F, 0.0463838F, -0.361941F, -0.353298F, -0.529141F, -0.503432F, - -0.898714F, -0.487881F, -0.480056F, -0.471919F, -0.43485F, -0.436497F, - -0.435059F, -0.421782F, -0.213375F, -0.398989F, -0.398978F, -0.402969F, - -0.380516F, -0.368304F, -0.724908F, -0.56863F, -0.459498F, -0.380321F, - -0.333752F, -0.423307F, -0.261483F, -0.398058F, -0.385883F, -0.386728F, - -0.305521F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 100.0F, - 0.2F, - - { 0.0F, 0.0F, 1.0F }, - - { -0.00120033463F, 0.00031723376F, 0.00179909123F, -0.00048965367F, - 0.000269401469F, 0.0025175761F, -0.00190733571F, 0.00228447909F, - 0.000180226212F, 0.00098133483F, 0.000551727368F, 0.000386494328F, - -0.00195541326F, -0.000818271481F, 0.00164427236F, 0.000525198237F, - -0.0017084172F, 0.000345568522F, -0.00101569027F, 0.000292662735F, - 0.00290596648F, 0.00196349225F, 0.000523894851F, 0.00205791183F, - 0.000595766585F, -0.000247043354F, -0.0012349321F, 0.00239970302F, - 0.0011094081F, 0.000801738526F, -0.000941737031F, 0.00216702791F, - 0.000601915119F, -0.00146362395F, -0.00184427609F, -0.00233109924F, - 0.000772954139F, -0.00121653418F, -0.000723667152F, -0.000196539302F, - -0.000712475F, 0.00184352102F, 0.00286921184F, 0.00195826078F, - -0.0023105552F, -0.0014248942F, 0.00172429369F, -0.000112811249F, - -0.00132816343F, -0.00281090476F, 0.00260269409F, 0.00098686642F, - -0.00241556857F, -0.000488087F, -0.000413487491F, -0.000362857099F, - -0.00131262874F, -0.00166683691F, 0.00129565957F, 0.000354458956F, - 0.00471123494F, -0.00139130966F, 0.00218376913F, 0.000945636246F, - -0.00103406142F, -0.000937014644F, -0.00356039521F, -0.000133825393F, - -0.00442352053F, 0.00152033847F, 0.00126741827F, 0.00126768264F, - 0.00316153117F, -0.000201241797F, 0.000540823967F, 0.000367185508F, - 0.00242681755F, 0.00111695239F, 0.00189569267F, 0.00209069182F, - -0.00112366758F, 0.000581074913F, -0.00073541858F, -0.0017684584F, - -0.000919447804F, -0.00283068628F, -0.00235894276F, 0.00106838974F, - 0.000591082382F, 0.00290304748F, -0.00174680445F, 0.000588165422F, - -0.000487660465F, 0.000647300621F, -0.00216221856F, 0.00148709607F, - 0.0050815F, 0.00309167849F, 0.00111093244F, 0.00290464447F, -0.000903867069F, - 0.00190173881F, 0.00167121133F, -0.00171434309F, -0.00117393304F, - 0.000154924885F, 0.00132939359F, -0.00178920431F, -0.00368691748F, - -0.000229367986F, 0.00062803016F, -0.00187031F, -0.000613596174F, - -0.000242869937F, -0.00220378232F, -0.00125137705F, 0.00374428253F, - -0.000684294151F, 0.0017608481F, 0.000841345289F, -0.0027268352F, - 0.00235245936F, -0.00386777031F, 0.00251566805F, 0.00122595194F, - -0.000661745376F, 0.000835447689F, 0.000565801165F, 0.00203406718F, - 0.00140093081F, 0.0015109824F, -0.00100814702F, -0.00168579328F, - 0.000485990779F, -0.00214976608F, -0.00197317172F, -0.000788644247F, - -0.0024704861F, -0.000137471841F, -0.00382696255F, 3.85259591E-5F, - 0.0004621158F, 0.00140210672F, 0.00198964891F, -0.000974376162F, - -2.60005781E-5F, 0.00040425008F, 0.000395389361F, -0.00264978618F, - 0.000996275456F, 0.00122917199F, 0.00261748838F, -0.000572452554F, - -0.00264905416F, -0.0019799571F, 0.00160241651F, 0.0031624164F, - 5.5090346E-5F, -0.000560144894F, 0.00369408051F, 0.00239271601F, - -0.00436752103F, 0.00189914252F, 0.00135392556F, -0.00278125214F, - -0.00168851251F, -0.00025398648F, -0.00218894216F, 0.00568651082F, - 0.000613791752F, -0.000459730887F, -0.00029524893F, 0.000551600882F, - -0.00284213643F, -0.00104786863F, 0.00283992779F, -0.000349502981F, - 0.00088893238F, -0.00133517385F, 0.000916219433F, -0.00131599139F, - 0.00266167545F, -0.000836377032F, -0.000249189674F, 0.000919571146F, - -0.00291851955F, 0.000908892136F, -0.00464374758F, 0.00175524422F, - 0.000143594501F, -0.000406738109F, 0.00430891057F, -0.000846700743F, - 3.58683064E-5F, -0.00105342804F, -0.00112863048F, 0.00198664446F, - 0.00232568756F, 0.000121921621F, 0.00273794425F, 3.23149725E-5F, - -0.00152201275F, 0.0009260386F, 0.000582259847F, 0.00117965881F, - -0.000961091369F, -0.00247303559F, -9.08016082E-5F, 0.00146386342F, - 0.000458232797F, -0.00367861846F, 0.00309828296F, -0.00115498703F, - 0.0024271016F, -0.00182749156F, 0.0035399776F, 0.00188906293F, - 0.000343339925F, 0.000452513166F, -0.00051911606F, 0.00117860106F, - 0.00112872629F, -0.00157833099F, -0.00142855709F, 0.00134401361F, - 0.00253007025F, -0.00232766243F, 0.000675269752F, 0.000953700684F, - -0.00380863389F, 0.00125859049F, 0.00107551529F, -0.00376268197F, - 0.00124778808F, -0.00222700695F, -0.000980164157F, 0.0020010923F, - 0.00218044734F, 0.00267404341F, -0.000885373855F, 0.00474456325F, - -0.000818251108F, 0.00190644572F, -0.00404736446F, -0.00157408696F, - 0.00244216411F, -0.00122755417F, 0.00030545058F, 0.00101727515F, - -0.000773854146F, -0.000135378607F, 0.00077783328F, -0.000156645328F, - 0.000341354491F, -0.00173809403F, -0.000477054477F, -0.000490876788F, - 0.000747794751F, -0.000637169462F, -0.00101695035F, -0.00157906732F, - 0.000162965516F, 0.0020659992F, -0.00183239963F, -0.000485551485F, - -0.000814158353F, -0.000295134465F, -0.00194528629F, 0.000586913782F, - -2.49333825E-5F, -0.00399446394F, 0.000442727964F, -0.000763713382F, - 2.97588358E-5F, 0.00082433346F, -0.00148945209F, -0.00159956142F, - -0.00316673913F, 0.00299726753F, 0.00208598142F, -0.00167959277F, - 0.000177103662F, 0.00170830765F, -0.00122772146F, 0.00115692639F, - -0.000817719323F, -0.000211367675F, -0.000331704F, 0.00212746253F, - 0.00342331291F, -0.00182230596F, -0.00234454591F, 0.00249147927F, - -0.00456712348F, -0.000579890097F, 0.0012938428F, -0.00228192F, - 0.00192168087F, -0.00137892331F, -0.000260579574F, -0.000297626335F, - -0.000138075833F, -0.00286118267F, 0.00364333251F, -0.00288702245F, - 0.00083135569F, -0.00249504042F, 0.00352674373F, -0.00266406336F, - -0.00191910693F, -0.000228088742F, 0.00195711409F, 0.00427616853F, - -0.00289809937F, -0.00131726265F, 0.000774575397F, 0.00104097824F, - 0.00164392893F, -0.00118163135F, 0.0010792925F, -0.000779933587F, - 0.00404726341F, 0.00062394113F, 0.00154473959F, 0.0006822492F, - -0.00223801192F, 0.00255911541F, -3.66426102E-5F, 0.00377679174F, - 0.00160837453F, 0.00371160149F, 0.000744176272F, 0.00100286072F, - 7.16315662E-6F, 0.00474413205F, -0.00204819953F, -0.000184541437F, - 0.000402938778F, -0.000748232473F, -7.24486235E-5F, 0.000748138933F, - 0.00257617515F, -0.000666672247F, -0.00188559364F, -0.00112883106F, - 0.0019131822F, 0.00129100319F, -0.000684920407F, -0.00175982632F, - -0.00021884765F, -0.00284739654F, 0.00143297622F, 0.000908654416F, - -0.000750021543F, 9.35741E-6F, -0.00568592409F, -0.00160820177F, - 0.00159349677F, -0.00260995631F, 0.00131270546F, 0.000746795966F, - -0.000708856038F, 0.00371741084F, -0.000926926034F, 0.00410630228F, - -0.000486166304F, 0.00267063291F, -0.00415411452F, 0.00145045668F, - -0.00328556099F, 0.000938747602F, -0.00215144642F, -0.00121427607F, - 0.00265847379F, -0.0025510164F, -0.000251495512F, -0.000460548F, - -0.00235597021F, -0.000123980892F, -0.00166018051F, -1.26463301E-5F, - -0.00126949803F, -0.00145347626F, 0.00203701947F, 8.54380778E-5F, - -0.00137076806F, 0.00322923926F, -0.00139187544F, 0.000936943165F, - -1.25445895E-5F, 0.00142887793F, -0.000206548109F, 0.00222745235F, - 0.000317273254F, -0.00351673667F, -0.000287639676F, 0.000677903823F, - 0.000656775839F, 0.00143122114F, 0.000438729767F, 0.00386759592F, - -0.000353695854F, -0.000991439214F, 0.00255872915F, 0.00210955553F, - 0.000661419879F, -0.000105965242F, -0.00230995356F, 0.00398798706F, - 6.85833E-5F, 4.65773264E-5F, 0.000155806076F, -0.000759483781F, - 0.000893606339F, -0.00110379257F, 0.000565042428F, -0.00270183268F, - 0.00024491566F, 0.00031092338F, -0.000360455073F, -0.00387012702F, - 0.000964669744F, -0.000859529886F, 0.000645960739F, 2.66402658E-5F, - 0.000347481488F, 0.00147409283F, -0.00196297863F, -0.000663478335F, - -0.00225480553F, 0.00208311505F, -0.000282880123F, -0.000311099226F, - 0.00153220014F, 0.00353624974F, -0.00082043634F, 0.000653751253F, - 0.000301538297F, -5.94286303E-6F, 0.00282875914F, 0.000556453655F, - 0.00268495199F, -0.00014298594F, 0.000146991864F, -0.00248965458F, - 0.00117943424F, -0.00141622033F, 0.000815272797F, -0.000811418169F, - 0.0024771872F, 0.000411177782F, 0.00117468403F, 0.000230787831F, - 0.00238163676F, 0.000288915704F, 0.00476958975F, -0.00058676285F, - -0.00225071912F, 1.53215842E-5F, -0.000604026834F, -0.00175921293F, - 0.000333891367F, 0.00040339955F, -3.030486E-5F, -0.00100507319F, - -0.00218418F, -0.00145709433F, -0.00155516015F, -0.00298570469F, - -0.000186855701F, 0.00211003702F, 0.00110385672F, -0.000917916186F, - -0.00331942947F, 0.00187710742F, -0.00292150583F, 0.000342179352F, - -0.001291272F, 0.000441475946F, 0.000262206537F, 0.00204382837F, - 0.00076605106F, -0.00507750083F, -0.00509927748F, 0.000914124481F, - 0.00190231868F, 0.00375512778F, -0.00101459841F, -0.00231593638F, - 0.000822221162F, 0.00117264013F, -0.00142994791F, 0.000380744226F, - 0.00198584865F, -0.00169723271F, 0.00366749708F, -0.00133891986F, - -0.00251878425F, -0.00208036089F, 0.00438469602F, -0.000545591116F, - -5.15626E-5F, 0.000384083018F, -0.00181669137F, -0.000293222809F, - -0.00277312798F, 0.00140175619F, 0.00036476212F, -0.000937288511F, - -0.000312224292F, -0.000163920558F, -0.00113473425F, 0.00125151488F, - 0.000833292259F, -0.00172145141F, 0.00136278942F, 0.00070231F, - 0.00600180123F, 0.000490321836F, -0.00319285854F, -0.000154740148F, - 0.000206695127F, -0.00117970188F, 0.00104856223F, -0.00123484596F, - 0.00276350882F, 0.00303634512F, 0.00104958389F, -0.00167755049F, - 0.000104377381F, -0.00234041573F, 0.000270931283F, 0.00186339894F, - 0.00244557229F, -2.21241E-5F, -0.00219599972F, -0.000462981086F, - 0.000101575279F, 0.000257811771F, -0.000644995598F, 0.00089480984F, - -0.00143653201F, 0.000886524F, 0.00387692056F, 0.00112266536F, - 0.00132980314F, -0.00174456323F, -0.000938551675F, -0.00012144239F, - 0.00233617984F, -0.00229866663F, -0.000508763187F, 0.00278811716F, - -0.001706574F, 0.000703544298F, -0.0016602308F, 0.000551981619F, - -0.00147905154F, -0.00061230507F, 0.00139311631F, -0.00267177261F, - -0.0010821349F, -0.000308389572F, -0.00252017844F, 5.14805106E-5F, - 0.00139870378F, -0.00049261807F, 0.00345301861F, -0.00267260266F, - -0.00141874049F, 0.00265707751F, -0.00203037518F, 9.48231082E-6F, - 0.000975244504F, -0.000939946F, -0.00209936546F, -0.00406858092F, - 0.000945721F, -0.000733794936F, 0.00122030091F, -0.0017225903F, - 0.000944443513F, 0.00119082467F, -0.00246377499F, 0.0033180113F, - 0.00417110324F, -0.00593069382F, -0.00034187964F, 0.000703751866F, - 0.00155501708F, -0.00108855427F, 0.000986301457F, -0.000752811611F, - 0.000299982523F, -0.000616057194F, 0.00132423418F, 0.0010699355F, - 0.00310018682F, -0.00249475F, -0.000321192754F, 0.000363734667F, - -0.00256955042F, 0.000161377946F, -0.0010037981F, -0.00096530281F, - 0.00180906651F, -0.00152415538F, 0.00223588035F, 0.00198637717F, - 0.00406722864F, -0.00029643107F, 0.00123207446F, -0.000860818953F, - 0.000261140696F, -5.77170322E-5F, -0.00240443437F, -0.00164404768F, - -0.00146111788F, -0.00104286009F, 0.00520714652F, -2.08394576E-5F, - 4.95478162E-5F, 0.00213191286F, -0.000103522791F, -0.00618431484F, - -1.67452363E-5F, 0.000833679282F, 0.0014373255F, -0.000844986702F, - -0.000806784665F, -0.000573807338F, 0.000110739813F, -0.00159416534F, - -0.000999216572F, -0.00034079561F, -0.000255459599F, 0.000186798992F, - -0.000125951614F, 0.00256175827F, -0.00446866F, 0.00170673744F, - 0.00311731058F, -0.00155760336F, 0.0050599589F, 0.00334217306F, - -0.000929035712F, -0.00128341024F, 0.000612609379F, -0.00148774404F, - -0.00366578507F, 0.00280380202F, 0.00293688942F, 0.00188938912F, - 0.0021242525F, -0.000778369722F, -0.0037618808F, 0.00144570193F, - -0.00150343531F, -0.00156783359F, 0.000113061506F, 0.00364953582F, - -0.000214029409F, 0.000465072517F, -0.000531474478F, 0.000892818847F, - -0.000937979261F, 0.00333925383F, -0.00336217275F, 0.00178477797F, - -0.00222229399F, -0.00049514F, -0.00246979878F, -0.00263304939F, - -0.00145867164F, -0.000495747197F, 0.00345226424F, 0.00345610525F, - -0.0039259633F, 0.000791927159F, -0.00117424107F, -0.00103941152F, - 0.000262169604F, 0.000225317519F, 0.000728206243F, -0.000124654587F, - -0.00145545637F, 0.00139032898F, -0.000594378682F, -0.0023764018F, - -0.00020938844F, -0.0019864582F, -0.000236987777F, -0.00132654898F, - 0.00256401137F, 0.00200066157F, 0.000800975F, 0.00193713023F, 0.00136238267F, - -0.00226882659F, 0.00244031381F, -0.00333335F, 0.00224895892F, - -0.00305932551F, -0.000881262124F, -0.0018369162F, 0.00145886152F, - -0.000119368291F, 0.00182895712F, 0.00302489568F, 0.000651903916F, - 0.00222151936F, 0.00161842874F, -0.00254142541F, 0.00109092938F, - -0.00314570847F, -0.000406487496F, -0.00217253785F, 0.000299198553F, - 0.00248045824F, -0.00353520573F, -0.00295592356F, 0.000991393F, - -0.00310293864F, 0.00417273305F, -0.00271473778F, -0.00194737886F, - -0.00153439457F, 0.000821281224F, 0.00312354811F, 0.00257412181F, - -0.00162399269F, -0.00107727991F, 0.000790675054F, -0.00216116733F, - -0.00102800399F, 0.00185955281F, -0.00193241204F, 0.00381841674F, - -0.00020652407F, 0.0010225299F, -0.000926310953F, -0.00308464747F, - -0.000957714685F, -0.000760557828F, -0.00110674533F, -0.00217140326F, - 0.000497946749F, 0.00352652324F, 0.000927589776F, -0.000228090343F, - 0.00211267988F, 0.00269386731F, 0.000337339327F, -7.80180781E-5F, - -0.00199551811F, 2.29047728E-5F, -0.00168152212F, -0.00436116941F, - 0.00116734218F, -0.0034280119F, -0.000810561527F, 5.36450034E-6F, - -0.0037968934F, -0.000384107087F, -0.00144968193F, 0.00156174344F, - 2.45347856E-5F, 0.00023924229F, -8.60242435E-5F, 0.000322681F, - 0.00262954039F, 0.0028647224F, -0.0016918995F, 0.000989177846F, - -0.000379809615F, 0.00163569674F, 0.000603063265F, 0.00261669513F, - 0.000427527324F, 0.00214348966F, 0.00132241799F, 0.0025153684F, - 0.00141312985F, -0.00304832775F, 6.03902663E-5F, -0.00025418913F, - 0.00270270417F, -0.00197259104F, 0.000595546F, -0.00274112565F, - 0.00233405805F, -0.00103089667F, -0.00310003641F, -0.000589925854F, - 0.000272013684F, 0.00126218353F, 0.00182856328F, -0.000691888679F, - -0.000851314689F, -0.000786111283F, 0.00174873299F, 0.00108402898F, - 0.00074392464F, 0.00360839465F, 0.000535186788F, 0.0039325105F, - -0.00118188804F, 0.00316036213F, -0.00049343548F, -0.000675681222F, - -0.000638158235F, 0.002220375F, -0.00141870149F, -0.00103213161F, - 0.00285850605F, 0.00192254444F, 0.0016844304F, 0.00106352649F, - -0.00218621572F, 0.00125208916F, 0.0019257731F, -0.00243153214F, - -0.00388761982F, -0.000559504493F, 0.000620032486F, -0.00106279249F, - 0.00112996227F, 0.00235216413F, -0.00140649744F, -0.000713319692F, - 0.000150631953F, -0.00163042219F, 0.002500684F, -0.000962255115F, - 0.00295119616F, -0.00136222271F, -0.000305221794F, -0.000123051053F, - 0.000471239298F, 0.00145215401F, 0.000821090245F, 0.00339313876F, - -0.000528975739F, 0.00241469499F, -0.000556679384F, -0.00258774566F, - -0.000718369905F, -0.00237847376F, -0.000624014589F, 0.000712225679F, - -0.000234321793F, -0.00330306287F, -0.000669803F, 0.00126968476F, - -0.00200734101F, 0.00149360509F, -0.00111517147F, 0.00103712711F, - 0.000415320741F, -0.000425287028F, -0.0024486687F, 0.00116440642F, - 0.00200982578F, 0.000201469244F, 0.00163283397F, 0.00160640967F, - 0.000961519603F, -0.00235093269F, 9.55203432E-5F, -0.000268061558F, - 0.00415697694F, 0.000828851422F, 0.00234207069F, -0.00292470842F, - 0.00150626793F, 0.00180186704F, 0.000453624234F, 0.00377037516F, - -0.00158186594F, -0.000364979438F, -0.00118849101F, 0.000157368864F, - 0.00205634953F, -0.00260290457F, -0.00349211483F, 0.00141407223F, - -0.000270963967F, -0.000509747711F, -0.0018042837F, 0.000576740073F, - 0.00115372217F, -0.00258643436F, 0.00105030451F, 0.00209515635F, - -0.00250819139F, -0.00205118186F, 0.00148246891F, -0.00468340842F, - -0.00286841975F, 0.000411599845F, -0.00239321496F, -0.00181211764F, - -0.00152578671F, 0.00280459714F, 0.00186344702F, -0.000416104856F, - -0.00154219428F, 0.000256237865F, -0.00041637168F, 0.00236476865F, - -0.00258447719F, -0.00219868449F, -0.000579054875F, -0.000162989614F, - -0.000398490141F, -0.00138024322F, -0.0011130044F, -0.000580932945F, - -0.00106241892F, 0.00128302735F, -0.00104095112F, 0.000725450751F, - 0.000347336463F, 5.42453745E-5F, -0.00113926758F, -0.00272238837F, - 0.00244467845F, -0.00064952916F, -0.0020292215F, 0.000418535434F, - -0.00231292588F, 0.00177453866F, -0.00213082531F, -0.0037414697F, - -0.00206708396F, -0.00117873563F, -0.00224591489F, -0.00109466584F, - -0.00239103148F, 0.00376629201F, 0.00189702F, 0.00237709493F, - -0.00255378149F, -0.00187708891F, -0.000262578891F, -0.00296355388F, - -2.55586037E-5F, 0.000286403F, -0.000335439225F, 0.00202925061F, - 0.000411903311F, 0.0010856241F, 0.00379960868F, -0.000176657471F, - 0.000187801721F, -0.000992899411F, 0.00325011F, 0.00100457342F, - -0.00369929336F, -0.00073467585F, 0.00192880118F, 0.00388774369F, - 0.000564566872F, -5.49859542E-5F, 0.000935267832F, 0.00171843427F, - -0.000100396916F, -0.002612137F, -0.00200498127F, 0.00120498135F, - -0.0020577251F, -0.00206689327F, -6.70054069E-5F, 0.0014398091F, - -0.00116097974F, 4.09532622E-5F, 0.00235764193F, -0.00121427048F, - -0.000471881445F, -0.00147631532F, 0.00125465018F, -0.00116165751F, - -8.65119182E-6F, -0.00113378919F, -0.00203060335F, -0.000987363397F, - -0.000279147353F, -0.00074609922F, -0.0019882163F, -0.00213516923F, - 0.00182886852F, 0.00112882326F, -0.00427987799F, 0.00336623681F, - 0.0040021264F, -0.000841731613F, -0.00165345764F, -0.00104981079F, - -0.00019062894F, -0.00172478473F, -0.00227690767F, -0.00158154976F, - -9.84445069E-5F, -0.000522146933F, -0.00355805061F, -0.00187109748F, - 2.88406977E-6F, -0.00190580613F, -0.00170167361F, -0.00255671982F, - 0.00156532135F, -0.0014291713F, 0.00255178777F, -3.04365385E-5F, - 0.00204580487F, 1.86608504E-5F, -0.00151844183F, -0.0012329024F, - -0.00175464875F, 0.000760695257F, 5.49282558E-5F, -0.00027081091F, - -0.00285263266F, 0.002353366F, -0.00098571158F, 0.00222727098F, - -0.000993737835F, -0.00359743903F, -0.00202716351F, 0.000169570732F, - 0.00463688606F, 0.000335575605F, 0.00128373597F, 0.00020997737F, - 0.00078590709F, -0.00296912F, 0.00139666442F, 0.00231415802F, 0.00204563932F, - -0.000730471045F, -0.00299879536F, -0.001183177F, -0.00152002764F, - -0.0014197909F, -0.00297456724F, 0.000330994662F, -0.00317081017F, - -0.000280272332F, 0.000771096675F, -0.00360990944F, -0.00153642637F, - 0.0034171422F, 0.00103201915F, 0.000857994717F, 0.00167444139F, - -0.00149446575F, 0.000124541592F, 0.00296334201F, 0.00373977027F, - -0.000592973782F, 0.00159648526F, 0.000914861565F, 0.000685395498F, - -0.00279864529F, -0.00240157917F, 0.000564667047F, 0.00049096829F, - -0.000233393439F, 0.00208659773F, -0.000692637288F, 0.00131963647F, - -0.000461392803F, 0.00011680273F, 0.0010524994F, 0.00120591687F, - 0.0018800199F, -0.00322161335F, 0.00368265924F, 0.000409881031F, - -0.0026560449F, -0.00117552979F, -0.00171709142F, 0.00108323887F, - -0.000732090673F, 0.00106313021F, -3.25106048E-5F, -9.25931163E-5F, - 0.00277623208F, 0.00191121211F, 0.00243059825F, -0.00160375086F, - 0.00265953923F, 0.000935243268F, -0.000537929533F, 0.000593858073F, - -0.00121930777F, -0.000642991217F, -6.67998174E-5F, -0.00144142145F, - 0.00101699366F, 0.0043029692F, -0.00201371429F, -0.000103792823F, - -0.00167013006F, 0.00102344819F, -0.00171678141F, -0.000625085086F, - -0.00156635081F, 0.002292349F, -0.00152366445F, 0.00235445192F, - -0.00373119512F, 0.002438033F, 0.000713277725F, 0.000770934217F, 0.00125745F, - 0.00289461901F, -0.00036612316F, -0.0037969884F, 0.00285213161F, - 0.00333118183F, -0.000783541822F, -0.00211625756F, 0.00147788599F, - -0.0015561817F, -0.00128853216F, -0.00247483305F, 0.00206785416F, - -0.00349983247F, -0.000657819F, 0.00449349266F, -0.00333154015F, - -0.00165419513F, -0.00180429313F, 0.00202455767F, 0.00322345435F, - 0.00333121326F, 0.000740502379F, 0.000589193194F, -0.000868175528F, - -0.00158721069F, -0.000380747457F, -0.001435988F, -2.43466839E-5F, - 0.0025564197F, 0.00259610452F, -0.000284169044F, -3.17708073E-5F, - 0.00138465478F, 0.00194051722F, -0.00200859434F, 0.00184980151F, - 0.000872969627F, 0.00265571056F, -0.00111778115F, 0.00171169615F, - -0.000200716357F, 0.0013520252F, 0.00395408133F, -0.00210623187F, - -0.0032937536F, 0.000698977383F, 0.00130240538F, 0.000578812906F, - 0.00267785834F, -0.00012557341F, -0.000600337749F, -0.000276006525F, - 7.37639421E-5F, 0.00331965159F, 0.00354253151F, -0.00315106451F, - -0.000223916053F, 0.000564691087F, -0.000922406209F, -0.00139784813F, - 0.00287001906F, 0.000525990326F, 0.000994485221F, 0.000262450834F, - 0.0006583574F, 0.000699699332F, -0.00111117901F, -0.00243194285F, - -0.00303075672F, 0.00182660681F, -0.00248327199F, 0.00145218265F, - -0.000315814046F, 0.00126052508F, -0.000201446528F, -8.50767174E-6F, - -0.00244402932F, 0.00182260387F, 0.00400978699F, -0.00132076931F, - 0.00158859428F, 0.00066823978F, -0.00132247561F, 0.00226200582F, - 0.000710332359F, 0.00146379601F, -0.00144324475F, 0.00606835214F, - -0.00409904402F, 0.00127794349F, -0.00194648432F, -0.000739202835F, - 0.000639330421F, -0.0011143802F, 0.000790996826F, 0.0037252137F, - -0.000581451401F, 0.00363462209F, 0.00102684414F, 0.000621564046F, - 0.000492373F, -0.00200353586F, 0.00193076802F, 0.000275246101F, - -0.00222644745F, 0.000523156952F, 0.00338592078F, -0.00169597322F, - 0.00117972656F, -0.000251837F, -0.00284885825F, -0.00413120026F, - 0.00172772189F, 0.00243731379F, -0.000908546208F, 0.000145192302F, - 0.000687714724F, 0.00240196148F, 0.000152687164F, -0.00310662249F, - 0.000771327817F, 0.00238729594F, -0.00111773668F, -0.00101186067F, - 5.29664649E-5F, 0.00199418608F, 0.00139400142F, 0.00302185304F, - -0.00178908743F, -9.50615067E-5F, 0.00141857332F, -0.0016991559F, - 0.00270374562F, 2.83746867E-5F, 0.00104887097F, -0.00135492079F, - 0.00150359666F, -0.00106130564F, -0.000609777111F, 0.000668871F, - 0.00206218963F, 0.000829201133F, 0.00129430008F, -0.00115024648F, - -0.00366343744F, -0.00100221159F, 0.000147456303F, 0.00170232623F, - 0.00172980595F, -0.000555447419F, 0.00248570531F, 0.00056591304F, - 0.00322244223F, -0.000984385842F, -0.00247790711F, 0.00153929158F, - 0.000371021888F, 0.00215686834F, -0.00318215811F, 0.00279704109F, - 0.000220176866F, 0.00200258475F, 0.0033946808F, -0.00171448907F, - 0.00114661991F, -0.00209633773F, -0.00155943877F, -0.00218588347F, - 0.0023148295F, 0.00187725679F, -0.000102219266F, 0.00151918293F, - -0.00224907906F, -0.00160818896F, 0.00193453615F, 0.000896741345F, - 0.00352948532F, -0.0030408348F, 0.00236688834F, 0.00064995396F, - -0.00141707272F, -0.000125186067F, -0.000217502544F, -0.00010631718F, - -0.00133083202F, -0.0012750827F, -0.00124032458F, 0.000864496629F, - -0.00251110899F, -0.000378629251F, 0.000115166455F, -0.00216294546F, - 0.000233203347F, -0.000525269541F, -0.0017910013F, 0.00292988704F, - 0.001132234F, 0.00453769509F, 0.000457222224F, -0.00131511106F, - -0.00160292513F, 0.00456277421F, 0.00165783544F, 0.00380459661F, - 0.000319349172F, -0.000137913899F, 0.00170685688F, 0.000575419283F, - 0.00191018346F, 9.49166788E-5F, 0.00106716063F, 0.00323209632F, - 0.00109869987F, 0.00191856315F, 0.00197580899F, -0.000689514214F, - 0.000828075048F, -0.00401313836F, -0.00134380255F, 0.0014798562F, - -0.000624739041F, -0.0011148944F, -0.00216544792F, 0.0017594992F, - 0.00143415062F, 0.00183940178F, 0.000427501625F, 0.00281122723F, - 0.00158756855F, -0.00373280165F, 0.00115094101F, 0.00081410032F, - 0.000775292516F, -0.00105535716F, 0.00100419275F, -0.00446527684F, - -0.00349584897F, 0.000149913307F, 0.000942075334F, 0.00365097239F, - -0.00177576928F, -0.00332885F, 0.00188465416F, 0.00175016443F, - 0.00383619731F, -0.00368412305F, -0.00037119491F, -0.00174731493F, - -0.00252208952F, 0.0015260902F, -0.00143245014F, 0.00202733837F, - 0.000705467071F, -0.000471564621F, 0.00157581316F, 0.00164028828F, - 0.00197945652F, -0.0070005036F, 0.002762652F, -0.000961343118F, - 0.00189666974F, -0.00017730832F, -0.000216231332F, -0.00258357171F, - 0.00173435442F, 0.00179577293F, -0.000951797585F, 0.000990887638F, - 0.00214442494F, 0.00268617133F, 0.000298319035F, 0.00176495442F, - 0.00300629111F, 0.00440991F, -0.00337861315F, 0.00147685455F, 0.00147454545F, - -0.003434435F, 0.00282321707F, -0.00118756457F, -0.00208020839F, - -0.000371985254F, -3.78700424E-5F, 7.6336335E-5F, 0.00285028154F, - -0.00174782646F, 0.00247766729F, -9.96911258E-5F, -0.000140034346F, - -0.00337009295F, 0.00256420719F, 0.00350640737F, 0.00220146589F, - -0.00137679686F, -0.00124896318F, 0.000334890559F, 0.00163270708F, - -0.00284488709F, -0.00311372196F, 4.43362442E-5F, 0.00481867837F, - -0.000550466706F, 0.0016088963F, -0.0014551346F, 0.00326213078F, - 0.00256534317F, 0.00266851136F, 0.000152786219F, 0.00233424664F, - -0.00332759856F, 0.000631026342F, -0.00243170303F, -0.00240348349F, - 0.000403942F, 0.000572114543F, -0.000366761553F, -0.00383988977F, - 0.000420894823F, 0.00300559F, 0.00211500656F, -0.000288327516F, - 0.000694016635F, 0.000247825665F, 0.00060851546F, 0.000589737319F, - 0.000258500775F, 0.00129596493F, 0.00379403867F, 0.00148535857F, - -0.00270003593F, 0.0030223F, -0.00138864061F, 0.001585313F, -0.00151518814F, - 0.00150429562F, 0.000197171539F, -0.00178710371F, -0.0035975338F, - 0.00252699316F, -0.00120296259F, 0.00127267023F, 0.00366031914F, - -0.00288507319F, 0.000329436181F, 0.00236289017F, 0.00514791533F, - -0.00157259149F, 0.00372715271F, -0.00211247383F, 0.00317128049F, - -0.000789746293F, -0.00199433696F, 0.00393922487F, -0.00094180688F, - 0.00471026311F, 0.00510932785F, -0.00111773598F, 0.00353265298F, - -0.000667279412F, -0.00182774884F, 0.00017240076F, 0.00117224618F, - 0.000761933683F, 0.000907345384F, -0.000964994077F, -0.00201538787F, - -0.00471707201F, 0.00389494933F, -0.0021094F, -0.00454117404F, - -0.00281825499F, -0.00176810508F, -0.00101463927F, 0.00222143717F, - 0.000499647169F, 0.00179292355F, 0.00171474915F, 0.00083059154F, - -0.000488042395F, -0.001950067F, 0.000314076809F, 0.00141148863F, - -0.00253657F, 0.00111886719F, -0.000279066502F, -0.00417275587F, - 0.000554099388F, 0.000313388096F, 0.000114230475F, -0.00257013831F, - -0.000308098533F, -0.00249527791F, -0.00133017718F, -0.000992793939F, - -0.00104239734F, 0.00386581197F, -0.000219165799F, 0.00073204661F, - 0.00218917825F, 0.000238178676F, -0.00117643236F, -0.00313279056F, - 0.000340750441F, -0.00078447588F, 0.00291647459F, 0.000613529177F, - -7.46653677E-5F, -0.00124426268F, -0.00427298062F, -0.00247971481F, - -0.00261217938F, -0.00137704576F, -0.00120748172F, 0.00236248621F, - -0.00134920829F, 0.00335346302F, -0.00049130962F, 0.000654028496F, - 0.00209923903F, -0.000872331206F, -0.000514235406F, 7.02203106E-5F, - 0.000876060338F, 0.00297414744F, 0.00357965869F, -0.00180852483F, - 0.000548975659F, -0.000386753702F, -0.00129820616F, -0.000886797789F, - 0.00169061369F, 0.002913523F, 0.000717159885F, 0.00138167467F, - 0.00195148063F, -0.000555612613F, 0.00193975074F, 0.00309141329F, - 0.000878558087F, 0.000420914119F, 0.000626883237F, 0.00123477285F, - -0.000549432F, 0.000301838067F, 0.00298390025F, -0.00123999233F, - -8.6828346E-5F, -0.00324921543F, -0.000356809644F, 0.00206934218F, - -0.000668162189F, 0.000939324906F, -0.00249981228F, -0.00304185133F, - -0.00133450096F, 0.000459843053F, -0.00088127231F, 0.000612642209F, - -0.00131453073F, -0.000388373621F, 0.00404766481F, -0.000886562455F, - -0.00198945752F, 0.00110284274F, -0.000485523546F, -0.0012933613F, - 0.000234684165F, -0.000431892113F, 0.00170970429F, 0.000254255108F, - 0.00115322322F, 0.00321482215F, 0.00199662312F, -0.00168576196F, - 0.00354856811F, 0.000494134612F, 0.000726134866F, 0.0017385406F, - -0.00282640429F, 0.00312143704F, 0.00235355832F, 0.000561418419F, - 0.00341563905F, 0.00332198176F, 0.00376563519F, -0.00222174753F, - -0.00197312445F, 0.00148785533F, -0.000303711306F, -0.00146468601F, - 0.00107057113F, 0.000406870327F, 0.00214534509F, 0.000484633871F, - -0.0013839826F, 0.00418333896F, -0.00033058651F, -0.000669227F, - 0.000228477205F, -0.000528119737F, 0.000170945306F, -0.0031319051F, - -0.00382732018F, -0.00418534363F, -0.00371257472F, -0.000153828325F, - -0.00144763442F, -0.00391660351F, -0.000906608184F, 0.000520584F, - -0.00120503281F, -0.00138364616F, 0.00140878092F, 0.000420498138F, - -0.0030909346F, -0.000813011196F, -0.0020665871F, 0.00211010501F, - -0.00254884176F, 8.35035171E-5F, 0.0003130392F, 0.00174522144F, - 0.00223648897F, -0.000958159391F, -0.000106308711F, -0.00188615813F, - 0.000530136283F, -2.9419698E-5F, 0.000376285519F, 0.00428138161F, - -0.000188720966F, 0.00104342354F, -0.000761151081F, -0.00326883723F, - 0.00124542555F, -0.000770604645F, 0.00299598533F, -0.00200795778F, - -0.000362134684F, 0.000698297925F, -0.00373463286F, -0.00155403919F, - -0.00127653731F, -0.000611278869F, -0.0019162182F, 0.00249482132F, - -0.00062514958F, 0.000522857532F, 0.000513558218F, 0.000691877969F, - 0.00139699F, -0.00112699787F, -0.00129973877F, 0.000940967409F, - -0.0032343443F, 0.00116318464F, 0.00108533492F, 0.00295175589F, - 0.000158818453F, -6.89510343E-5F, -0.00266504358F, -0.000261353358F, - -0.000448611274F, -0.000714180816F, -0.000274674298F, 0.00164557551F, - 0.00251864921F, -0.000382751547F, -3.8719405E-5F, 0.0027870622F, - 0.00218299194F, -0.00152107025F, -0.00310241082F, -0.00283374335F, - -0.000342402433F, -0.00535774045F, 0.00343885948F, -0.000605901F, - 0.0023824533F, 0.00109803805F, -8.29370765E-5F, 0.00268199458F, - -0.00176311552F, 0.000739531417F, -0.000719770789F, -0.0009352247F, - 0.000371572358F, 0.00127759739F, -0.00324530271F, 0.00342580932F, - -0.000676285184F, 0.00266379584F, 0.000173316919F, 0.0045074583F, - -0.00216619717F, 0.00436406443F, 0.00220478536F, -0.00333039789F, - 0.000437144656F, 0.000103348153F, -0.00164062402F, 0.00100831327F, - 0.00153215975F, 0.000143761135F, -0.000894560479F, -0.000142480858F, - -0.0046897619F, -0.0058971988F, -0.00370993582F, 0.00068868621F, - -0.000920575287F, -0.000175985362F, -0.00159883173F, 0.000893880147F, - -0.000542731257F, 0.00125168008F, 0.000865766779F, -0.000899011386F, - 0.00310820458F, -0.00360691873F, -0.00172905379F, -0.000906277273F, - -7.68246E-5F, -0.000790329592F, 0.000144706399F, 0.000299129926F, - -0.000686376938F, -0.00246470189F, 0.00118691F, -0.0007073179F, - 0.00243903766F, 0.000320776162F, -0.00389357191F, -0.00142150791F, - -0.000616158941F, -0.0016628236F, -0.000667630055F, 0.000123878359F, - 0.00103324791F, -0.000986796F, -0.00450771395F, 0.00423180498F, - -0.00124792499F, 9.62863778E-5F, 0.00143682794F, -0.0027303868F, - -0.000877040962F, 0.00103396992F, 0.00270299055F, 0.0015272008F, - 0.00382570503F, -0.00459928857F, 0.00283108233F, 0.000282984227F, - 0.00215641456F, -0.00211860868F, -0.000633094402F, 0.00062916032F, - 0.000206693294F, 0.00182436977F, -0.00292343274F, 0.00194162328F, - -0.00304707745F, 0.00102058495F, -0.00266215368F, -0.0017483976F, - 0.0025887494F, -0.000558897154F, -0.00118332717F, -0.00112572068F, - -0.00293008192F, 0.000431521767F, -0.0015913794F, 0.00122130266F, - -0.00052960281F, -0.00126190158F, -0.000493915053F, 0.00224199728F, - 0.000199264658F, 0.00226842705F, 0.00335375383F, 0.000403532031F, - 0.00451382203F, 0.000618756691F, 0.00140748813F, 0.000925887376F, - 0.00236586831F, 0.00148593134F, -0.000205883858F, 0.000288256968F, - 0.00119972811F, 0.00337082776F, -0.00185590773F, 0.00391032454F, - 0.00165872229F, 0.00173911836F, 0.00338519202F, 0.00419145F, -0.00210460485F, - 0.000143476631F, 0.000661257363F, -0.00131871412F, -0.00272030104F, - 0.00134638499F, 0.00498574413F, -0.00233332068F, -0.0019458587F, - 9.88746906E-5F, -0.000990191707F, -0.000193534695F, 0.00262294943F, - 0.00229358068F, -0.00219420157F, -0.00377643318F, 0.00228122249F, - -0.00354034896F, 0.00206509791F, -0.00305105443F, 0.0014483569F, - 0.000827266776F, 0.00124804385F, 0.000933737F, -0.00298929634F, - 0.00197144877F, -0.00362777896F, 0.00293018227F, -0.00323894294F, - -0.00120331114F, 0.00177579687F, -0.00100504048F, 0.000784465694F, - 0.00252742344F, -0.00123399275F, 0.00306910975F, 0.00234334893F, - 0.00301269279F, 0.000781771378F, -0.000472500455F, -0.00326963654F, - -0.000461884483F, 0.00163853518F, 0.0028768864F, -0.0047179237F, - 0.00111986801F, 0.000378954544F, 0.000556959945F, -0.00107917341F, - 0.00188864791F, 0.00264323666F, -8.15625899E-5F, 0.000535694184F, - -0.000711416302F, -0.00128856383F, 0.00245243753F, -0.0034585637F, - 0.000444504345F, -6.16410616E-5F, -0.000383569306F, -0.00480351551F, - -0.00145961239F, 0.000449219311F, 0.00163663516F, -0.00215731F, - 0.000599067891F, -0.00204802165F, -0.00279050972F, -0.00353513658F, - -0.00212454074F, -0.00110274891F, 0.00126050087F, 0.0015819422F, - 0.00295410817F, -0.000538111199F, 0.00139213703F, -0.000397684373F, - -0.00328732817F, -0.00124491134F, -0.000231814425F, 0.000591451942F, - 0.000982948113F, 0.000293645746F, 0.0022363856F, -0.0033463682F, - -0.00364354253F, -0.000912022428F, -0.000130610759F, 0.00190547609F, - -0.000879408966F, 0.000217791618F, -0.00125403493F, -0.00228411355F, - -0.00203714264F, -0.00170681207F, -0.0023558354F, 0.000352260133F, - 0.00083010341F, -0.0013667501F, -3.05152244E-5F, 0.00244703679F, - -0.000914216F, 0.000254370563F, -0.00248826575F, 0.000860513188F, - -0.000462764059F, 0.000217503577F, 0.000329205563F, -0.00145125657F, - -6.36633E-5F, 0.000642672F, -0.00476523582F, 0.00299081858F, 0.000943921856F, - -7.08880398E-5F, 0.00147002842F, 0.00142139231F, 0.000230724196F, - -0.000977134F, 0.000255070132F, -0.000520317699F, 0.000345474604F, - 0.00379036972F, 0.00057786284F, -0.00162298325F, -0.000713038316F, - 0.000253619422F, 0.00263852F, -0.0042198617F, -0.00047475574F, 0.0010732084F, - 0.00100854516F, 0.00454055099F, -0.00242447807F, 6.38520069E-5F, -0.0026858F, - -2.6576623E-5F, -0.00244162697F, -0.000253792095F, 0.000640882878F, - -0.00133527699F, 0.00363475038F, 0.00302518881F, 2.20515467E-5F, - -0.00115123799F, 0.000605957583F, 0.00198588753F, 0.000596064783F, - 0.0016150166F, 0.00227224105F, 0.00152708206F, 0.00328308297F, - -0.00139178778F, 0.00369014149F, -0.00247952319F, 0.00158032356F, - -0.00242785038F, -0.00114979316F, 0.00130167394F, 0.00170114369F, - -0.000649677F, -0.00212723436F, -0.00264636637F, 0.00312140677F, - -0.00153237651F, -0.00186547812F, -0.000481665571F, 0.00228654779F, - 0.00185574859F, 0.00381902489F, 0.00334506924F, 0.000343949039F, - 0.00163465517F, -0.00487442035F, -0.00244317227F, -0.000176469403F, - 0.00162949937F, 0.000899533858F, -0.000898534141F, -0.00160527404F, - -0.00193976704F, -0.000615470344F, 0.00144385651F, 0.00184465328F, - 0.00223636115F, 0.00256420812F, -0.000240201873F, 0.00100407773F, - 0.00154953904F, -0.00176716899F, -0.00400425447F, -0.00629541278F, - -0.000436868548F, 0.000343561289F, 0.00020161907F, 0.00135108363F, - 0.000715080067F, -0.000453032728F, -0.000952133967F, -0.000958165678F, - -0.000351096183F, -0.00297843944F, 0.00162944756F, 0.000722727389F, - 2.54422E-5F, 5.19153909E-5F, 0.0060887686F, 0.000744173187F, - -0.000801893184F, 0.00142513053F, -0.00170640915F, 0.000200190101F, - 0.00269190478F, -0.00143410836F, 0.00280404091F, -0.00345009821F, - -0.000527738826F, 0.000179667564F, 0.00117255468F, -0.00230608415F, - -0.00361546921F, -3.49205129E-6F, -0.000711539586F, -0.00147259829F, - -0.00175801269F, 0.00273378054F, 0.001437956F, 0.00168688444F, - -0.00050317467F, -0.00361952814F, -0.00201148726F, 0.00109419087F, - 0.00168341433F, -0.00172793632F, -0.00194337219F, 0.00410298258F, - 0.001948972F, -0.00209280662F, -0.000735624752F, 0.000284779817F, - 0.0031158498F, 0.00266518211F, 0.00123415736F, 0.00152454828F, - -0.000489013561F, 0.00178507855F, 0.000929838803F, 0.0040635257F, - 0.000230600519F, -0.00125956431F, -0.0013381684F, -0.00271976832F, - 0.00177144865F, -0.000869580952F, 0.000923034211F, 0.00447297655F, - -0.00188114727F, 0.00143332488F, -0.00171075901F, 0.0014719069F, - -0.000847277115F, 0.0021791223F, 0.000597056467F, 0.0010002373F, - -0.00239204F, 0.000590246578F, 0.00103056827F, -0.00153277116F, - -0.00056078186F, 0.00471923733F, 0.00125006F, -0.000599138148F, - -0.00216824026F, -0.00316616916F, -0.00449610408F, 0.000999545795F, - -0.00193953468F, 0.00187256059F, 0.00104082946F, 0.00117182429F, - 0.00259457109F, -0.00135027326F, -0.000108394874F, 0.00149275688F, - -0.00163247471F, 0.00161992328F, 0.00185436453F, -0.00199182262F, - -0.00080050074F, -0.000414694368F, -0.000647919718F, -0.00170638552F, - -0.00298783905F, 0.00142041908F, -0.00171502656F, 0.00145626091F, - 0.00169501523F, -0.00140950596F, -0.00115062657F, -0.000234165811F, - -0.000937516103F, -0.000702849182F, 0.0030719717F, -0.0016004449F, - -0.0042945384F, -0.00090568763F, -0.000362906372F, -0.000436528353F, - 0.00163752993F, -0.00286469562F, -0.000547808304F, -0.00324018206F, - -0.00115929858F, -0.000359205122F, -0.00231499691F, 0.00129940792F, - -0.000423124759F, -0.000876345381F, -0.000183374985F, 0.000434668065F, - -0.000823976647F, -0.000833372411F, 0.00219943631F, 0.000676403F, - -0.00226675766F, 0.00100461068F, 0.000108385175F, 0.00218742341F, - -0.003824946F, 0.00367104099F, 0.00125929283F, 2.56449712E-5F, - -5.66070921E-5F, 0.00362974545F, 0.000735478F, 0.00131531537F, 0.00207923F, - -0.00192355551F, 0.00355722615F, -0.00111983146F, -0.0032785635F, - 0.00331563293F, -0.00167860917F, 0.000337859208F, 0.00135244464F, - 0.00209109508F, 0.00147777656F, 0.00215247367F, -0.00133960112F, - 0.00296308077F, -0.000102911159F, -0.00220534694F, 0.00167425175F, - -0.000925400935F, 0.000232010832F, -0.00074182637F, -0.00197409303F, - -0.00127968134F, -0.000210581813F, 0.000942381623F, 0.00271612755F, - -0.00139753718F, 0.000481517F, 0.000160705153F, -0.000349224836F, - -0.00276444037F, -0.0032228257F, 0.00301426742F, -0.00089981F, - 0.000418550393F, 0.00152262743F, 0.000420741242F, 0.000685954874F, - -0.00286348863F, 0.000787312107F, -0.00336625963F, -0.000630409573F, - 0.00352254137F, 0.00323640183F, 0.000878743769F, 0.000368335983F, - -0.00130934059F, -0.00111666264F, -0.000230581922F, 0.00058546F, - -0.00233361265F, -0.00159496791F, -0.00165332761F, -0.00194387115F, - 0.000422696059F, 0.00594696356F, 0.00305052404F, 0.000238344466F, - 0.00117385015F, -0.00247416459F, -0.00245935144F, 0.00204911618F, - -0.00201485679F, 0.000219456109F, 0.000378326338F, 0.000836267369F, - 0.00190865993F, -0.00146696588F, 0.000102894381F, -0.00132132776F, - -0.000312514516F, -0.00130727561F, -0.00254809042F, -0.0027264494F, - 0.00408467231F, -0.000377054268F, 0.000125667721F, -0.00131943449F, - 0.00104802195F, 0.00156951929F, -0.00269490317F, 0.000392958755F, - 0.00633619633F, -0.00295610982F, -0.00107062166F, 0.00217046286F, - -0.00164730696F, -0.000913554104F, 0.00163232733F, -0.00210120063F, - 0.00265595876F, 0.00265024207F, 0.000131644687F, 0.00244506239F, - 0.0026946757F, -0.000284970243F, 0.00135729008F, -0.000533067505F, - -0.000737362134F, 0.00144025777F, -0.00137085316F, -0.000164290672F, - 0.00124896958F, -0.00399291F, 0.0017736149F, 0.00233368203F, - -0.000405607687F, 0.00176396861F, -0.000257747452F, -0.000139095F, - 0.000480850431F, 0.00197957712F, 0.000615705678F, 0.000760999916F, - -0.00313409627F, 2.2599188E-5F, 0.00241927756F, 0.00206815568F, - -0.0013713123F, -0.00102515693F, -0.000316552469F, 0.000123674894F, - 0.000678665121F, 0.00233934796F, -0.00120455876F, 0.00234366534F, - -0.000444085686F, 0.00162191864F, 0.00392142311F, -0.00231002038F, - -0.00168747292F, -0.00399155635F, -2.00599E-5F, -0.00124137709F, - 0.00190659193F, 0.00244901702F, -0.000603329565F, 0.00314144581F, - 0.00438788766F, 0.000903540116F, 0.00104969507F, -0.000683035236F, - 0.000110132642F, -0.000285494316F, 0.000959010213F, -0.00178020133F, - 0.000706681167F, 0.000526838063F, 0.00167765154F, -0.00111631257F, - -0.000347615511F, -0.00199324917F, 0.000209380029F, -0.0021662137F, - 0.00144905457F, -0.00397731503F, -0.00149067643F, 0.00179038814F, - 0.00195572106F, -0.00337364664F, 0.00225340785F, 0.00011623466F, - 0.000364531326F, 0.00307721272F, 0.000128219908F, -0.000840475434F, - -0.00275745825F, -0.000532962207F, -5.36135376E-5F, -0.000563104055F, - 0.00126188993F, 0.00116297347F, -0.000206147961F, -0.000480646821F, - -0.000987392F, 0.00134433131F, 1.80994539E-5F, 0.00180449954F, - 0.000261022913F, -0.00164660951F, 0.0036770443F, 0.00136815547F, - -0.00186579989F, -0.00105527136F, -0.000194332824F, -0.00102130859F, - 0.00167895178F, -0.00374279241F, 0.000668851193F, -0.000180684234F, - 0.00186601502F, 0.00256681046F, 0.000288234442F, -0.000688405649F, - -0.000797994086F, -0.000715980656F, -0.0019877092F, -0.00242203102F, - 0.00145042816F, -0.00245801848F, -0.00169577391F, -0.0009029F, - -0.00181548798F, -0.00172342174F, -0.000597302511F, -0.001016489F, - 0.00145225937F, 0.00404053275F, 0.00241254037F, -0.0044895648F, - -0.00148941576F, 0.00511888508F, -0.00058449019F, -0.000538790599F, - -0.00311311753F, -0.000323579035F, 0.00179736794F, -0.00079021597F, - 0.00113137F, 0.000162256736F, -0.00147864351F, -0.00508900965F, - -0.00216765096F, -0.00272677024F, 0.000277530664F, -0.000926966139F, - 0.000302909582F, 0.00334054464F, 0.000543567236F, -6.44969332E-5F, - 0.00128462596F, 0.00136708515F, 0.000967359461F, -0.00522200251F, - 0.00168364262F, 1.99084366E-6F, 0.00399650773F, 0.000579279847F, - -0.00138000667F, -0.000282513502F, -0.00102089241F, -0.00120938139F, - -0.00269105681F, 0.000484841556F, -0.00116925139F, -0.00222140714F, - -0.00243262947F, -0.000918157806F, 0.00300681707F, -0.00269341539F, - 0.00146303629F, 0.00187680242F, 0.000650750357F, -0.000823730428F, - -0.00211148104F, -0.00300218863F, 0.00218326435F, 0.000489048602F, - 0.000425330858F, -0.00130577199F, 0.00223330432F, -0.000649263849F, - -0.000376615353F, 0.00241780304F, 0.00110381842F, -0.00440158F, - -0.00180355541F, -0.00111242349F, -0.00190291239F, -0.000471119216F, - 0.00145679619F, 0.00142479548F, 0.00209460617F, 0.00279219169F, - -0.00233334373F, -0.00106302113F, 0.000830252247F, -0.0033082671F, - 0.00167693791F, 0.00149179704F, 0.000104866805F, -0.00105214573F, - 0.00252050511F, -0.000119585122F, 0.000978057F, 2.61467794E-5F, - -0.000418136391F, -0.00193099235F, 0.00185310061F, 0.00166847336F, - 0.00401255814F, -0.00499085849F, 0.000786350574F, -0.00357611524F, - -0.00190537039F, 0.00149165827F, 0.000590599666F, 0.00078776211F, - -0.0020472738F, 0.00291362638F, -0.00114876067F, 0.000919874699F, - 0.00325617241F, -0.000866809161F, 0.00231811707F, 0.00129080238F, - 0.00134406833F, -0.000918175909F, 0.00365534145F, -0.00177082652F, - 0.00375732384F, 0.000219505018F, -0.00260031782F, 0.000962932419F, - -0.000966686814F, 0.000819135807F, 0.00123568845F, 0.00156808179F, - 0.00139848876F, -0.00184694887F, -0.00282855122F, -0.000626067747F, - 0.000806374417F, 0.000672040449F, -0.00373505056F, -0.00030874298F, - -0.000241702801F, -0.00164238864F, -0.00149449985F, 0.000727246574F, - -0.00221869675F, -5.10546561E-5F, -0.00305675087F, 0.0018929377F, - -0.0022409095F, -0.00149302906F, 9.81183839E-5F, -0.00460958527F, - 0.000559068401F, -0.00062887813F, 0.00150578516F, 0.000187783357F, - -0.00172067387F, -0.00132923771F, 0.00293483469F, 0.00143615203F, - 0.000619673403F, -0.00166872935F, 0.00245045894F, -0.00129564526F, - -0.0020120556F, 0.000791861909F, 0.000656543125F, 0.0020645326F, - -0.000586122856F, -0.00266152434F, -0.000498710142F, -0.000244825671F, - -0.00280244625F, 0.00222801021F, 0.000142320219F, 0.0006908098F, - 0.000839023967F, -0.00139794883F, 0.00421224209F, -0.00112342287F, - -0.00182857108F, 0.00356030068F, 0.000972627371F, 0.00126993342F, - 0.000943265F, 0.00155573781F, 0.0036493612F, 0.00395464F, -0.00095365895F, - -0.00211360375F, -0.00255449745F, 0.00205625454F, -0.00267771422F, - -0.00226075F, -0.000788823061F, -0.00251989649F, -0.000578157837F, - 0.00649022171F, -0.00213635177F, -0.00191464217F, -0.00296043488F, - 0.000168442843F, -0.00228110398F, -0.00348606333F, -0.000751098443F, - -0.00193154626F, 0.000793100509F, 5.6586814E-5F, -0.000849950535F, - -0.00391255366F, 0.00452193571F, -0.00186254538F, -0.000934378419F, - -0.00187674351F, -0.000587725197F, 0.00208198861F, -0.0062374617F, - -0.00293080369F, 0.000740826887F, -0.00037486924F, -0.000571232871F, - -0.00190602499F, -0.00136226346F, -0.00119186693F, 0.000380983955F, - -0.000874623132F, 0.000153848159F, -0.00305304094F, -0.00216512172F, - 0.00330552761F, 0.00163239846F, -0.00039097335F, -0.00286641391F, - 0.000410086242F, -0.000195198561F, 0.000630681636F, -0.000347461493F, - -0.00283030025F, 0.00152724993F, -0.000148365682F, -0.00189132988F, - -0.000413248345F, 0.00114879734F, -1.04027677E-5F, -0.000778721529F, - -0.0049014329F, 0.000290589494F, -0.00256336923F, -0.00177867943F, - -0.00226680958F, 0.00126553432F, -0.00260892813F, -0.001003301F, - -0.000158414F, 0.00296102162F, 5.09986967E-5F, 0.00245636073F, - 0.00155099062F, -0.00117955613F, -6.35880569E-5F, -0.000418832031F, - 0.00281143771F, 0.00101011246F, -0.00145543297F, -5.23233502E-5F, - 0.00147331716F, 0.00194345985F, 0.000108385037F, 0.00270904251F, - -0.00272547686F, 0.0012750465F, -0.00167422078F, -0.00226230919F, - 0.000600107946F, -0.000153554254F, -0.00347369397F, -0.00142707676F, - -0.000273374782F, 0.000683062593F, 0.00144186756F, -0.000757461879F, - 0.00144120259F, 0.000600084F, 0.00295962929F, 0.000335012854F, - -0.00145796081F, -0.00154045399F, 0.000460559968F, -0.000810781377F, - -0.00147727F, 0.000318718492F, 0.000846549461F, -0.00219194381F, - 0.000791305851F, -0.00142177462F, 0.000372234063F, -0.000386417814F, - 0.00163570652F, -0.00385158625F, 0.000596034923F, 0.000282450725F, - -0.00117801432F, -0.00192953064F, 7.44912177E-5F, 0.000308221584F, - 0.00089061755F, 0.000951905036F, 0.000489774451F, -0.00126210239F, - -0.00111396925F, 0.000132441361F, -0.000653966796F, 0.000485750323F, - 0.00327205518F, 0.000888427196F, -0.00116442889F, 0.00259458949F, - 0.00238763075F, -0.0020880322F, -0.00110166124F, -0.000264080882F, - -0.000977396732F, -0.00187907123F, -0.00215436774F, -0.00109279295F, - 0.00161351485F, -0.00288684154F, 0.00149016117F, -0.00463413121F, - -0.00475350907F, 0.000260516157F, -0.00123440812F, -0.000317156955F, - 0.00122040161F, -0.00167791417F, -0.00173913967F, -0.00151203305F, - 0.000310038798F, 0.00186599989F, -0.000850186392F, -0.000418572687F, - 5.17336557E-5F, 0.00234159222F, 0.00384913827F, 0.00179501693F, - -0.000989661F, -0.000487748475F, -0.00167598366F, -0.00288368179F, - -0.000732357323F, -0.00252706301F, 0.000947506924F, -0.000804819691F, - -0.000292271F, 0.00015151831F, 0.000878328F, -0.00269829319F, - -0.00310428487F, 0.00194880355F, 0.00124531693F, 0.000548739F, - -0.000861521112F, 0.00155170634F, -0.00299877324F, 0.000280999229F, - -0.00319112721F, 0.00128988933F, -0.000841023051F, -0.000465338351F, - 0.00234205718F, 0.00108236214F, -0.00180763786F, -0.00161093508F, - -0.00172333722F, 0.00152288389F, 0.00229984452F, 7.11709872E-5F, - -0.000569094147F, 0.000729578373F, 0.0018362914F, 0.00274308957F, - 0.00226495299F, 0.00132946821F, -5.28036944E-5F, 0.00436805049F, - -0.00153744046F, -0.00474370224F, 0.0027635F, 0.00181186211F, 0.0025665611F, - -5.58063512E-5F, 0.00194783404F, -0.00172863458F, -0.00240783137F, - 0.00138349435F, 0.0024091634F, -4.39586438E-5F, 0.00218784367F, - 0.00142306555F, -0.00148444937F, 0.00338943629F, -0.00160155864F, - -0.000545880641F, 0.00221842527F, -0.00187359762F, -0.000438701F, - -0.00120751269F, -0.000539517845F, 0.0020957F, 4.65486346E-6F, - -0.00421750313F, 0.000212202882F, -0.000996245304F, 0.000760140421F, - 0.000485277851F, -0.00102777733F, 0.00020979211F, -0.00250731315F, - -0.00278629945F, -0.0018926874F, 0.0011706301F, -0.00332850614F, - -0.00137696264F, -0.00305483886F, -1.95930215E-6F, 0.00151408475F, - 0.00124885258F, -0.00172277412F, -9.01503E-5F, 0.000281833141F, - -0.000739896F, 0.00176247745F, 0.00345802493F, -0.0033043474F, - -1.78043465E-5F, 7.28893647E-5F, -0.00167705F, -0.00194755F, 0.000445441401F, - -0.00271744188F, -0.000450266351F, -0.000762412441F, 0.00181647739F, - 0.000484882185F, -0.00101457071F, -0.00178187201F, 0.00174147263F, - -0.00308182905F, 0.00195187016F, 0.00174716173F, -0.00123136234F, - -0.00479246F, -0.00352890138F, 0.00296211871F, -0.00182924187F, - 0.000338650949F, 0.000247660471F, -0.00290933531F, 0.000726907107F, - 0.000529183249F, -0.000893763267F, -0.00452474551F, -0.0042708884F, - 0.00263076066F, -0.00244784937F, 0.00156402087F, 0.000935776625F, - 0.000932952098F, 0.00157339941F, -4.16396489E-7F, -0.00172704901F, - 0.000637863F, 0.0014952831F, -0.00208232272F, -0.000658408448F, - -0.000520706701F, -4.80956E-5F, -0.000508586061F, -0.00029784959F, - -0.000557111751F, 0.00135345466F, -0.00503894174F, -0.000683343795F, - -0.00177927327F, -0.00148147508F, 0.00126428064F, -0.000289146934F, - -0.000835217244F, 0.00256288145F, 0.00101012364F, 0.000536380219F, - 0.000907195848F, -0.0055099763F, 0.000464911544F, 0.000865930633F, - -0.000328531489F, -2.17689631E-5F, 0.00242952863F, 0.000981584773F, - 0.00030362379F, -0.00292387488F, 0.000791071914F, -0.00185319968F, - -0.000389784633F, 0.000684485596F, -0.00112937018F, -0.00304786023F, - -0.00231296872F, 0.00112030318F, 0.00341032748F, -0.000606777437F, - -0.00118813722F, 0.00173106533F, 1.29953332E-5F, 0.00259605702F, - 0.00107552775F, -0.00054005906F, 0.00164316292F, -0.000849367F, - 0.00283287396F, 6.25715E-5F, -0.000891639153F, 0.00190017791F, - 0.00230324222F, -0.000471826846F, 0.0031077459F, -0.00218876242F, - -0.00184907322F, 0.00203341548F, -0.00158363848F, 0.000960121804F, - -0.00034701245F, 0.00117365282F, -0.000344592379F, -0.000621932675F, - 0.00125260721F, -0.00215775566F, -0.000332656258F, 0.000137775278F, - -0.00103195582F, -0.0005255086F, 0.000784802542F, -0.00249361875F, - -0.000785297772F, -0.00214695348F, -0.00199309178F, 0.000103973965F, - 0.00145243562F, 0.00379424286F, -0.00073526206F, -0.00273798476F, - 0.00178055512F, 0.00119190372F, -0.00221921294F, -0.000850329467F, - 0.00027554261F, 0.00152178318F, -0.00299583306F, -5.44793147E-5F, - -0.00136655406F, 0.000332223513F, -0.00187984481F, -0.00284117F, - -0.000848873227F, -0.000242137728F, -0.00061366416F, -0.000290571916F, - 0.000915910932F, -7.97232933E-5F, 7.55915826E-5F, -0.000840390217F, - -0.00076642487F, 0.00312306406F, -0.00148542994F, -0.00404121401F, - -0.00170166593F, -0.00102888863F, 0.00279751839F, -0.0012410481F, - -0.0010038747F, -0.00412359741F, -0.0027634718F, 0.00279380381F, - 2.39368146E-5F, 0.00263505825F, 0.00122187031F, 0.00164885947F, - -0.00159698981F, 0.000186812758F, -0.0044096373F, -0.00256854063F, - 0.00168270094F, 0.000370092981F, 0.000275995815F, 0.000717516814F, - -0.00102757174F, -0.00152151939F, 0.000745823083F, 0.00364578073F, - 0.000248970115F, -0.00324330735F, -0.00117528124F, 0.00244380673F, - -0.000649136608F, 0.00145035528F, 0.00122404564F, -0.00152283546F, - -0.000261287903F, -0.0014645023F, 0.0024352218F, 0.000292659126F, - 0.000101576101F, -0.00199017185F, 0.000800056965F, -0.00165146473F, - -0.000688194705F, -0.00234168302F, 0.00169462198F, 0.00216960814F, - 0.00205896748F, 0.00197268603F, 0.00194076216F, 0.00109304511F, - -0.00114134571F, -0.00174312235F, 0.00347338943F, -0.00309373415F, - 0.000509035075F, 0.00188276649F, 0.00104243401F, 0.00199277024F, - -0.000864895F, -0.000364747219F, -0.00183804752F, 0.000852785073F, - -0.00453157909F, -0.000289443065F, -0.00108066632F, 0.000752460386F, - 0.00129304035F, 0.00114133663F, -0.00099185342F, 0.000472949789F, - 0.00134129566F, -0.000225355805F, -0.000445556885F, 0.000623507F, - 0.000421942095F, -0.00126505853F, 0.00111219345F, 0.000161937409F, - 0.000111652866F, 0.00165921706F, 0.000207756137F, -0.000476134679F, - 0.000772520958F, -0.000840381603F, 0.00174058671F, -0.00199560518F, - 0.000903958688F, -0.00251988764F, 0.0026772446F, -0.000691408059F, - 0.000809711695F, 0.00034100347F, 0.00134249276F, 0.00123366027F, - -0.0003054275F, 0.00154639827F, -0.00270388275F, 0.000433030742F, - 0.00163165026F, -0.000552187266F, -0.00146616262F, 0.00107848621F, - 0.000305589172F, -0.00073330669F, 0.00223302911F, 0.000579437648F, - 0.00222024368F, 0.00344271795F, 8.8637833E-5F, 0.00219419F, 0.00181647588F, - -0.00108519581F, -0.00109392195F, 0.00406386331F, 0.000214280764F, - -0.00181532896F, 0.0013353012F, -0.0016320541F, 0.000905523426F, - 0.00125979178F, -0.00307408674F, -0.0025782953F, -0.0027226042F, - -0.00052552484F, 0.0015375287F, -0.00346314162F, 0.000398418313F, - -0.00267206598F, 0.000607275404F, 0.000263966504F, -0.00112910813F, - 0.00242307037F, 0.000925662811F, -0.00121437525F, -0.00229360419F, - -0.00202449202F, -0.00211803755F, 0.0012286352F, 0.00041505316F, - 0.000115611241F, -0.000241290763F, -0.00166614202F, 0.00254923361F, - -0.00159735233F, 0.00228528655F, -0.00169438252F, -0.00227930513F, - 0.0016709601F, 0.00183573249F, -0.00209951191F, -0.0020905782F, - -0.00217067148F, -0.00165417872F, 2.09115824E-5F, 0.000422007404F, - 0.00172627589F, -0.00123124511F, 0.0012230135F, -0.000539842062F, - -0.000130653527F, -0.000457527145F, -0.00168978644F, 0.00222431403F, - -0.00165615603F, -0.00539173093F, -0.00044465F, -0.00169087655F, - 0.000341930252F, -0.00249376241F, 0.00176753267F, 0.00150628318F, - 0.00267604133F, 0.00185625523F, 0.000626358087F, 0.000526932185F, - -0.000999795273F, 5.56061859E-5F, 0.00225024507F, -0.00127234228F, - 0.00212765066F, -0.000701466401F, 4.78755E-5F, -0.00352725107F, - 0.00280799181F, 0.00158164208F, 0.00329141342F, 0.00151088042F, - 0.00111485249F, -0.00131521781F, 0.000453859742F, 0.00212299102F, - 0.000100528021F, -0.00120077911F, -0.00216176733F, -0.00195437809F, - 0.00263003516F, 0.00110903382F, 0.000745803176F, 0.000716095266F, - -0.00244647497F, 0.00109205791F, -0.00175818126F, 3.39378857E-6F, - 0.000142906749F, -0.000884061F, 0.0001178612F, 0.000365697168F, - 0.00255388464F, 0.00313969073F, -0.000266842893F, 0.000214801636F, - 0.000119391058F, -0.000449421437F, 0.00243512215F, 0.00170524826F, - 0.00499174651F, 0.00499768695F, 0.000586631824F, -0.00148753147F, - -0.00194369501F, 0.00347930612F, -0.00166145118F, -0.00222149584F, - 0.000321285974F, -0.00144295092F, 0.000368104811F, 0.000959150842F, - 0.00082868163F, -0.00295291562F, 0.000368959387F, 0.00345996651F, - -0.001183972F, 0.00189727964F, -0.000223744428F, 0.000637580699F, - -0.00241758628F, 0.000207716337F, -0.000968749402F, -0.000814216328F, - 0.00056472671F, -0.00143687904F, -0.00253777322F, 0.000404522376F, - 0.000743957527F, 0.00126709091F, -0.00238064514F, -0.00232331967F, - 0.000440264121F, -0.000320587656F, -0.000856955943F, 0.00061428512F, - -0.00045438041F, -0.00115479622F, -0.000300994579F, -0.00312850741F, - -8.46541127E-7F, -0.00377654796F, 0.00125629141F, -0.00107737258F, - -0.00511267688F, -0.000935717952F, -0.000843566668F, -0.000592738157F, - 0.00162498839F, 0.00152828323F, -0.00331181078F, 0.00160155364F, - 0.00187037454F, 0.00120213046F, 0.0020363098F, 0.00162810495F, - 0.00401503174F, 0.000198021153F, -0.0014457294F, 0.000620959327F, - 0.000255475898F, 0.00521752704F, 0.00154749199F, 0.0043755658F, - -0.000843854912F, 0.00475921F, -0.000212752915F, 0.00290184561F, - -0.00043559834F, 0.00178015674F, 0.00100909476F, -0.000348464644F, - 0.003146359F, -0.00248283963F, -0.00282679079F, -0.00157107518F, - 0.00132396258F, 0.000610545336F, 0.000558196567F, -0.00152737519F, - -0.00124805851F, -0.00218830514F, 0.0018443194F, -3.64644256E-5F, - 0.00117212092F, -0.000214129221F, -0.00284404703F, 0.00109894702F, - 0.00321980333F, -0.0011362863F, -0.002915547F, -4.87160869E-5F, - 0.00193086895F, -0.00475526648F, 0.00108369754F, 0.00118689192F, - -0.000555041945F, 0.00230785413F, -0.00211893953F, 0.000993911177F, - 0.000445642945F, 0.0009467036F, -2.95358932E-5F, 0.0007323639F, - 0.000541571237F, 0.00188534334F, 0.00219797343F, -0.00274146954F, - 0.00339576858F, 0.00119483983F, 0.000384833926F, 0.000260220957F, - -0.00498639559F, -0.00175516878F, -0.00252350466F, -0.0026256016F, - -0.00182345428F, -0.000790838909F, 0.00150344579F, -9.38227731E-6F, - -0.00126717635F, 5.06709839E-5F, -0.000646000495F, 0.00090980361F, - -0.00179286837F, 0.00334682548F, 0.000482115982F, 0.00238752621F, - 0.00562824076F, 0.00279941177F, -0.000640341896F, -0.00160267181F, - -0.000640723738F, 0.000786694116F, -0.00242138817F, 0.000319985847F, - 0.00102764438F, 0.000950279064F, -0.000209536112F, -6.05319547E-5F, - 0.000503781368F, 0.000457276125F, -0.000380316633F, 0.00252330047F, - 0.000432259694F, -0.00139283529F, -0.00159029465F, -0.0040762336F, - -0.00427129539F, 0.00185740669F, 0.000826881849F, 0.001638997F, - -0.000953880721F, 0.00208506943F, -0.00180613855F, 0.00039985F, - -0.00372535596F, -0.00283977692F, -0.000410377223F, 0.0027477541F, - -0.00141828065F, -0.000656189106F, 0.00159246451F, -0.00250869687F, - -0.00335304113F, 0.00149745645F, -0.00119258882F, -0.00170693814F, - -0.00220914464F, -0.00167660054F, 0.00259639602F, -0.000868837116F, - 0.00366171403F, -0.000743776734F, 0.00154546427F, -0.000870522577F, - 0.00108570128F, -0.00120304897F, 0.000274427177F, -0.00158084941F, - -0.00340602919F, 0.00103097816F, -0.00193932548F, -8.61724402E-5F, - -0.000227466895F, -0.000472787564F, -0.00326500973F, 0.00146433641F, - 0.00345514459F, 0.00202369434F, 0.00226211082F, -0.00296787662F, - -0.00161251065F, -4.77292124E-5F, -0.000742559147F, -0.000449034589F, - 0.0018333F, -0.00237326417F, 0.00178257027F, 0.000863357098F, 7.92610372E-5F, - 0.000275360158F, 0.00115192682F, 0.000543946167F, -0.0008828789F, - -0.00227433071F, 0.00291807624F, 0.0022546642F, -0.0011106031F, - -0.000381555146F, 4.37061462E-5F, 0.0023424353F, 0.00117853412F, - 0.001236276F, -0.00130063447F, 0.00041579461F, -0.00192499463F, - -0.000597839535F, -0.00210963213F, -0.00440366F, 9.24896667E-5F, - -0.00231079408F, 0.00100342603F, -0.000843074406F, -5.39374059E-6F, - 0.000367884786F, 0.00409101136F, -0.00191018067F, 0.0030648543F, - 0.000561824243F, 0.000738453353F, -6.25125358E-6F, -0.000513969921F, - 0.000454124471F, -0.000804395298F, 0.00036805027F, -0.00119903416F, - -0.000943046471F, -0.00125078147F, 0.00288324407F, -0.00094315788F, - -0.000440309435F, -0.000572502497F, -0.000645444205F, -0.00269618025F, - 0.0017753375F, -0.00131543516F, -0.00121359597F, -0.000782208517F, - -0.00124104798F, -0.00269346102F, 0.00164685887F, 0.00185337942F, - -0.00044793592F, 0.000647762441F, -0.00123647007F, -0.000637359684F, - -0.00133192679F, 0.0015139794F, 0.00230984413F, -0.00231138477F, - -0.00126035628F, 0.000261691166F, -5.3790176E-5F, 0.000535468222F, - 0.00224379939F, 0.000326847425F, 0.00144999078F, -0.00215906557F, - -0.00282221683F, -0.000528372533F, 0.000591225864F, 0.000760683266F, - 0.00356319291F, 0.000491860614F, 0.00138717575F, 0.000934011885F, - -0.00141461683F, -0.000877654355F, 0.00164804503F, -0.00116413063F, - 0.00154945266F, -0.0013165765F, 0.000553760969F, 0.00117281941F, - -0.0018921938F, -0.00160773075F, -0.000659058918F, 0.002856693F, - 0.00252062152F, -0.00158860744F, -0.000668551656F, -0.000126233368F, - -0.00228235638F, 0.00320864748F, -0.0006888723F, -0.000347602385F, - -0.000349027745F, -0.00317915855F, -0.00102761586F, 0.00246054656F, - -0.00184873701F, 2.21041264E-5F, -0.00107789133F, 0.000934495823F, - -0.00174744753F, -0.00126173743F, -0.00155859569F, -0.00417043408F, - -0.00177078659F, -0.00244583027F, 0.00193574466F, -0.000277521904F, - 0.000227139739F, 0.00234930636F, -0.00274288701F, 0.000684752828F, - 0.00290632388F, 0.00186216808F, 0.000535636907F, 0.00170434313F, - 0.00469019916F, 0.000231951723F, -0.00451747607F, -0.000582520326F, - 0.00276366598F, -0.00241063F, -6.01641805E-5F, -0.0025251715F, - -3.53154178E-6F, 0.00138789322F, -0.00042467998F, -0.00208212831F, - -0.00135197141F, -0.00254662801F, 0.000978767523F, 0.000266946096F, - -0.00173288805F, -0.000535935163F, 0.00133525697F, 0.00294002052F, - -0.00264999107F, 0.00304274727F, 0.00204568123F, 0.000134079251F, - -0.00470581418F, 0.00316066272F, 0.000337282923F, -0.000215180204F, - 0.00299750897F, 0.00143422815F, -0.00108612783F, -0.000952000963F, - 0.000571689743F, -0.00193158688F, 0.00255559688F, 0.00024179199F, - -0.000409930653F, 0.00100623548F, -0.00310283666F, 0.0014342576F, - 0.00282180309F, -0.00294487691F, -0.00179589354F, 0.000830629375F, - 1.89153106E-5F, 0.00343278027F, -0.00282560289F, 0.000403203914F, - -0.00363410218F, 6.97629439E-5F, -0.000906860223F, -0.00306819496F, - -0.00182705407F, -0.000326128327F, -0.00124665664F, -0.00295751402F, - 0.000941775623F, -0.000974277151F, -0.00136223715F, -0.000801951566F, - -0.0012348108F, 0.00300790835F, -0.000501441886F, -0.00123240903F, - -0.00173798623F, -0.00056629657F, 0.000732296729F, -0.00519892946F, - -0.00218431326F, -0.00335746398F, 0.00254038814F, 0.00119431457F, - -0.00271176128F, -9.59091922E-5F, -0.00142677862F, 0.000870135729F, - -0.000526757038F, 0.000768947299F, -0.00165998796F, 0.0014371362F, - -0.00341386185F, 7.93781801E-5F, -0.00383222802F, -0.000610675663F, - 0.0027193923F, -0.00131335296F, -0.0043134233F, 0.000705771905F, - 0.00270993705F, -0.000597921608F, -0.00436047325F, 0.0017537385F, - 0.00104535639F, 2.05812466E-5F, 0.00164824515F, -0.003050945F, - 0.00251267874F, 0.0005651096F, -0.000186341058F, -0.000800433394F, - 0.000234932508F, -0.00178382767F, -0.000453436893F, -0.00391700957F, - -0.000521271955F, 0.00245940732F, -0.00036064742F, 0.000383047649F, - 0.0033868833F, -9.37402729E-5F, 0.000744218F, -0.000672691909F, - -0.00286797481F, 5.22474547E-5F, -0.00118208642F, -0.00364032621F, - -0.00132574688F, 0.00151162525F, -0.000115307143F, -0.000229238329F, - 0.00125155295F, 2.69109678E-5F, -0.000236415392F, -0.000526013086F, - 0.00221618428F, 0.00128609443F, 0.00266311434F, -0.000356413919F, - 0.00337064173F, 0.00440957537F, 6.34260723E-5F, -0.000191388186F, - 0.000796021719F, 0.00131254224F, 0.00279352884F, 0.00245552347F, - -0.00290936348F, 0.00053195568F, -0.00137627008F, 0.000201805175F, - 0.000243524191F, -0.000790208578F, 0.00281430804F, 0.00127953477F, - 0.00149464165F, -0.00224212394F, -0.00317386654F, -0.00101531425F, - 0.00197532494F, -0.00113020313F, 0.000550317171F, -0.00144659402F, - -0.00431263167F, 0.00532669481F, 0.00121148617F, -0.00187835272F, - 0.000914167322F, -0.000136488263F, -0.00127848063F, 0.000539722678F, - 0.0030121468F, -0.000564467628F, -0.000853693171F, -0.000697950949F, - -0.00104544125F, -0.000233096085F, 0.00212701689F, -0.000399721815F, - -0.000432882574F, 0.00243870239F, -0.00404112227F, 0.00105539977F, - -0.000942961837F, -0.000232151913F, -0.000844538794F, -0.0015611297F, - 0.000103979604F, 0.000426722137F, 0.000160313546F, 0.00272263261F, - -0.00196822733F, 0.001625153F, 0.0031714F, -0.000708518375F, -0.00671380153F, - -0.00030504071F, 7.80861519E-5F, -0.00212567579F, 0.00360347098F, - 0.00229798397F, -0.00115470029F, -0.00214062468F, 0.00435877638F, - 0.000821258698F, 0.00143473851F, -0.000334391574F, 0.000900716754F, - 0.000574166479F, 0.00153697061F, 0.00113832171F, 0.000164656536F, - 0.00125412794F, -0.000611703435F, 0.00045044205F, 0.000379208737F, - -0.00149253034F, 0.00370358932F, 0.00175045989F, 0.0034804293F, - 0.00295506348F, 0.00282353698F, -0.00308039249F, 0.00128523656F, - 0.00187178457F, -0.00152850477F, 0.000763339049F, -0.000680055935F, - -0.00116509222F, 0.00261183782F, -0.00250467355F, -0.000465071382F, - -0.000225476499F, 0.000323363172F, -0.0025596919F, -0.00207460695F, - 0.00221860269F, -0.00335259433F, -0.000623739848F, 0.00181972911F, - 0.00179258978F, 0.00115935958F, -0.00234343158F, -0.000449898827F, - 0.00133855687F, -0.00197272166F, 0.000792964711F, 0.00345379254F, - -0.00272820261F, 0.000491656305F, -0.000856204308F, -0.000181251278F, - 0.00230562245F, 0.000154113353F, 0.00262066675F, 0.00150259701F, - -0.0015056004F, 0.00149176829F, 0.000306827977F, 0.000526014483F, - 0.00169173558F, -0.00429380126F, 0.00236746762F, 0.000790340244F, - 3.58420971E-6F, 2.75131279E-5F, -0.000726117578F, -0.00371253025F, - 0.000946331245F, -0.00149078714F, 0.00240605464F, 0.00272359233F, - 0.00155218667F, 0.000631415809F, 0.00322090881F, -0.00268521509F, - 0.000647491F, 0.000356380042F, -0.00264300709F, 0.000569600146F, - -0.00462651718F, 4.37916242E-5F, 0.00129182776F, -0.00232314551F, - 0.00180185214F, -0.00179891894F, 2.7908E-5F, 0.00122456194F, -0.00106874935F, - -0.00148408685F, -0.00120170868F, 0.00104681286F, 0.00166812935F, - 0.00161161006F, 0.00229518465F, -0.000453842193F, -0.00336607336F, - -0.00102386938F, -0.00280562835F, 0.00284894672F, -0.000295223261F, - -0.00145566557F, 0.00383237354F, 0.00326868636F, 0.000276491017F, - 0.00197276892F, 0.000159160583F, 0.00109490741F, -0.00206343178F, - 0.00141724234F, 2.84463613E-6F, 0.00104024156F, 0.000808708719F, - 0.00220456859F, -8.56465194E-5F, -0.00228306279F, -0.000711421133F, - -0.00359210791F, 0.00269385264F, 0.00190290436F, -0.000807000964F, - 0.000516892644F, 0.000617043697F, -8.84108886E-5F, 0.000109461253F, - 0.000813025108F, 0.000279373489F, 0.0023482875F, 0.000681026F, - -0.00032963691F, 0.00101199967F, 0.00175952911F, -0.00195401232F, - -0.00034311635F, -0.00123134418F, 0.00126871408F, -0.00217267103F, - 0.000591006712F, 5.97968719E-5F, -0.000260725559F, -0.00140785251F, - 0.00189627532F, 0.000983474893F, -0.00135257805F, 0.00128910341F, - -0.000172783388F, -0.000536513748F, 0.000519907277F, -0.00114359113F, - -0.000725932652F, -0.000113674512F, -0.000949352048F, -0.00181762571F, - 0.000412296591F, -0.00119008624F, 0.000705487502F, 0.00132317725F, - 0.00106360309F, -0.00264735403F, 0.000266080402F, -0.00168634742F, - -0.000442067249F, -0.00039068528F, 0.00284898165F, -0.00388672459F, - 0.000543079223F, -0.000645822F, 0.000532967388F, -0.000451952888F, - 0.00157602143F, -0.00133422203F, 0.00252750237F, -0.000281321874F, - -0.000290107884F, 0.00122555741F, -0.00071909558F, -0.000115101815F, - 0.00329703628F, 0.000873530109F, 0.000971603789F, -0.00102676917F, - -0.00384093821F, -0.00229944056F, 0.00390282017F, -0.00132816227F, - 0.00271034311F, 0.00108374644F, -0.00171595695F, -0.000146615959F, - -0.00524187973F, -0.0015246215F, 0.000910241739F, -0.00289367256F, - 0.00384813384F, 0.00113983313F, -0.000999365351F, -0.0027473662F, - -0.000929476286F, 0.000139468306F, 0.00202640123F, 0.000191735991F, - 0.00216002809F, -0.000869266107F, -7.76590896E-5F, -0.00166965008F, - -0.00195227819F, -0.00200830959F, 0.00144687702F, 4.39008036E-5F, - 0.000512586674F, -0.000918588F, -0.00427119946F, 0.000916972698F, - 0.00191378058F, 0.000713209505F, 0.00274614966F, 0.000701636483F, - 0.00301973801F, 0.0042130989F, -0.000167342849F, -0.00214348687F, - -0.0015913574F, 0.000285978807F, -0.00337533467F, -0.000858547166F, - -0.00154148659F, 0.000633716292F, -2.07495104E-5F, -0.00344734127F, - -0.000372534065F, 0.00337084F, 0.00101442984F, 0.00285366829F, - 0.00169736263F, -0.00444632117F, -0.000511048071F, -0.000125575141F, - 0.0016702608F, -0.00755716348F, 0.000610181654F, -0.00106706261F, - 0.0020220743F, -0.000863307272F, -0.003008737F, 0.00428013084F, - -0.00220751157F, 0.000984767918F, 0.00139778317F, -0.000536406704F, - -0.00214847829F, -0.00110188848F, 0.00150259701F, -9.3668983E-5F, - -0.0028018197F, 0.00151464145F, 0.000455451227F, -0.000884795852F, - -0.00154143816F, -0.000134524351F, 0.00186087028F, -0.00142368651F, - 0.00128317985F, -0.00182369724F, 0.000256457453F, 0.0013828571F, - 0.000242606591F, -0.00127596478F, -3.34905635E-5F, 0.00098922872F, - 0.00388906873F, -0.000901681895F, 0.000583436457F, 0.00204604398F, - 0.00262652733F, 0.00101913896F, -0.000176446774F, 0.00147515337F, - -0.0014980546F, 0.00201297365F, -0.000936748402F, -0.00309469062F, - 0.00119677803F, 0.000245905452F, 0.000743372075F, -0.0033498921F, - -0.000404019578F, -0.00151628698F, -0.00448310468F, 0.00109809788F, - 0.000606869929F, -0.000647341367F, 0.00162147335F, 0.000348879112F, - -0.000532916863F, -0.000119401426F, 0.00204189867F, -0.000465551944F, - -0.000704466F, -0.00115434499F, -0.00182594778F, 0.00107904943F, - 0.00515731378F, 0.0012672533F, -0.000659861544F, -0.0013453206F, - -0.00248619542F, 0.000421683566F, -0.00321297324F, 3.9987649E-6F, - 0.000822636357F, -0.00153124717F, -0.00199251366F, -0.00201410521F, - 0.00369022181F, 0.00221468F, -0.000450693973F, -0.000346661807F, - 0.00145634927F, 0.00147619343F, -0.0016975957F, -0.000206504512F, - 0.00349298189F, 0.00107782159F, -0.00112475082F, 0.00207929639F, - -0.00245822151F, -6.67452405E-5F, -0.00096206629F, -0.00229073106F, - 0.000359723F, 0.00148627034F, 0.00282103778F, -0.0010935656F, - -0.000615051715F, -0.00425721193F, -0.00185861404F, 0.00107608573F, - 0.00234730984F, -0.00136056356F, 0.000703343481F, 0.00346044637F, - 0.00238477904F, -0.00059873F, -0.00242080097F, -0.000904965214F, - 0.000126881219F, -0.000553308346F, -0.00247947709F, -0.00166209356F, - -0.00112561847F, -0.000190978943F, -0.00410293601F, 0.00124187861F, - -0.000545160088F, -0.00042098813F, 0.000712559209F, 0.00172746403F, - 0.000253000442F, -0.0014410303F, 0.0025710233F, 0.000243574905F, - -0.00147274125F, -0.00202761637F, 0.00444468204F, 0.000302238477F, - -0.00312186033F, -0.000201967283F, 0.00232901052F, -0.000922995445F, - 0.0028806862F, -0.000870407035F, 0.00240933266F, 0.00369430962F, - -0.00182858366F, 0.00123279821F, -0.00202956423F, -0.000844407303F, - -0.00159469817F, -0.000850357465F, -0.00207406306F, -0.00249878783F, - -0.000638368248F, -0.000727840932F, -0.00179338362F, 0.00191755709F, - -0.00158486725F, -0.000155279544F, -0.00141449529F, 0.00305876276F, - 0.000325875153F, -0.00105845951F, 0.000147562794F, 0.000319896877F, - -0.000811270089F, -0.00125861983F, -0.00258379639F, -0.00447943481F, - -0.00139740633F, -0.000585273141F, -0.00186415797F, -0.00242932281F, - -0.000350410963F, 0.000909085094F, -0.00102829793F, 0.00289789052F, - 0.00145369454F, -0.000133081223F, -0.00169162557F, 0.000355301105F, - -0.00222407794F, -0.00109029363F, -0.000411054527F, -0.00647239201F, - -0.00110081572F, 0.00168068823F, 0.000446042424F, 0.00164670416F, - -0.00410898775F, 0.00105741876F, 0.00187350227F, 0.00104293006F, - -0.000506105542F, -0.00144700939F, 0.00180120487F, -0.00177169743F, - 0.00120241253F, 0.00263756164F, -0.000954961753F, 0.0026037842F, - 0.00200261455F, 0.000574336096F, -0.000925718632F, -0.00277112052F, - -0.000873404788F, -0.000314423727F, -0.000141004726F, 0.00366807519F, - -0.00121506397F, 0.000105540661F, 0.00088634796F, 7.62741474E-5F, - -0.0014366738F, -0.000796290522F, 0.00308072218F, -0.000113718605F, - -0.00163588522F, 0.00189636904F, 0.00243317778F, 0.002375F, -4.79635219E-6F, - 0.000116399133F, 0.00207360927F, -0.00257564755F, -0.000389076711F, - -0.0028916935F, 0.00129296677F, -0.000969690562F, -0.00111763948F, - -0.000975253817F, -0.000258231652F, -0.00139819144F, -0.00239003776F, - -0.00204602093F, -0.00188492122F, -0.00217244215F, -0.00176148175F, - -0.000392657472F, 0.00162862183F, 0.00122822309F, -0.000847588526F, - -0.000991138513F, 0.00105734938F, 0.00120680046F, 0.000685923616F, - 0.00260907947F, 0.000442266552F, 0.00336027937F, 0.002528538F, - -0.0023034038F, 0.00253828987F, -0.00385035388F, -0.00282198726F, - -0.0064815674F, -0.000192613821F, -0.000986366533F, 0.00056943018F, - 0.00151472597F, 0.00253130333F, -0.00180260721F, 0.00217044796F, - 0.00061523309F, 0.00148957188F, -0.00103484315F, 0.0016789271F, - -0.00218039844F, -0.00158881082F, 0.00175904983F, -0.00169937883F, - 5.3941E-5F, -0.002238299F, 0.00103551906F, -0.00280597596F, 0.0013540081F, - 0.000161030446F, -0.000149323547F, 0.000442349527F, -0.00536430255F, - -0.000299605832F, 0.000805216609F, -0.00108076714F, -0.00154960889F, - 0.00178245071F, 0.00292280293F, -0.00334103941F, -0.000631282513F, - 0.000895222533F, 0.00166874914F, -0.00114068342F, -0.000107835032F, - -0.0027415636F, 0.000575983489F, 0.000752973196F, 0.000918191159F, - 0.00216832082F, -0.0039054025F, -0.000558600877F, -0.00242728041F, - -0.000738122675F, -8.75105761E-5F, -0.00151327089F, 0.00249304529F, - 0.00316359685F, -0.00170116732F, 0.00111718255F, -0.00127923815F, - -0.00217852951F, 3.96409414E-5F, 0.00157969922F, -0.00184777775F, - -0.00288080354F, 0.000586231588F, -0.00455626147F, 0.000108157277F, - -0.00188035856F, -0.00242772233F, -0.000509864825F, -0.00103168958F, - -0.000766506419F, -0.00203455775F, 0.00374074839F, 0.00222608401F, - 0.000772328174F, 0.000147413055F, -0.000603916706F, 0.000204020049F, - -0.00201485306F, -0.000983274658F, 0.000109800232F, 0.00332977949F, - 0.00156294985F, 0.00124884F, -0.000318500097F, -0.00431105588F, - 0.00165757618F, -0.000757405884F, -7.41842159E-5F, 0.00365672144F, - 0.00064329023F, 0.00158703735F, 0.000181044132F, 0.000846967625F, - -0.000360581937F, -0.000466085126F, 0.000222915332F, -0.000548657146F, - 0.000198259862F, 3.8759812E-5F, -0.00280271145F, -0.00087070372F, - -0.00147616F, 0.000248275814F, -0.00218556891F, -0.000410465582F, - 8.55042381E-5F, 0.00255619944F, 7.68125465E-5F, -0.00211599539F, - -0.000999049284F, -0.000840514898F, -0.000350424612F, -0.000602656568F, - 0.00155525538F, -0.00314826099F, -0.00373315858F, -0.00233345665F, - 0.000230030491F, -0.0015803572F, -0.00143193884F, 0.000465093472F, - -0.00146248459F, 0.00241121207F, -0.0009553727F, 0.000942772662F, - -0.000908642425F, -0.00119184493F, -0.00145986967F, -0.00207510334F, - -0.000229607875F, 0.00183246122F, -0.000420083554F, 8.27875701E-5F, - -0.00173966633F, 0.000293724559F, 0.00277664978F, -0.000200070193F, - 0.00278755184F, -0.000808271521F, -0.00376315927F, -0.00434349384F, - -0.00177024025F, -3.87661E-5F, -0.000330040319F, 0.00417577708F, - 0.00165090896F, -0.00059428F, -0.00117952458F, 0.00339764915F, - 0.00243228721F, 0.0014731026F, -9.34341806E-5F, 0.00225222949F, - 0.00282212021F, 0.00185852672F, -0.00413268898F, 0.00189911353F, - 0.000833154F, 0.0021811882F, 0.000956444477F, -0.00168222934F, - 0.00198666798F, -0.00124670588F, -0.000394351955F, -0.00551302778F, - 0.00179122947F, -0.000253903796F, -0.000241059941F, -0.000737900089F, - -0.00206683623F, 0.00423880573F, -0.000125689199F, 0.000583148561F, - -0.00111732283F, -0.00348003185F, 0.000718680792F, -0.00201799325F, - -0.000272115954F, -0.000731271808F, -0.000762563839F, 0.00112326688F, - 0.00270879571F, 0.000505897624F, 0.000305723195F, -0.00211429619F, - -0.00429965137F, -0.00177631201F, -0.00128969445F, -0.00218152138F, - 0.00101671461F, -0.00019948915F, 0.000306361413F, -0.00175302767F, - 0.00198600534F, -0.0013007907F, -0.00100579369F, -0.00106870558F, - -0.00318330526F, -0.000176588466F, 0.00381304976F, -0.00216407352F, - 0.00179072039F, -0.000463281147F, -0.000379340112F, -0.000137634343F, - -0.00169307692F, -0.00169337657F, -0.00288549205F, -3.91938556E-5F, - 0.000350081216F, 0.00199396955F, 0.000761814532F, 0.00121458585F, - 0.00229833298F, -0.00223059463F, 0.00331610884F, -0.00287147867F, - -0.00053179817F, -0.000554720347F, 0.00121656805F, 0.00271336432F, - -0.00200972357F, -0.000241312751F, 0.0010331003F, -0.00286937365F, - 0.000647709414F, 0.00129807531F, -0.00242657214F, 0.000619645405F, - -0.00163040042F, 0.00151655287F, 0.00304057845F, -0.000101851663F, - -0.000336624886F, -0.000191694577F, -0.00166077807F, 0.00189272745F, - -0.00186448253F, 0.00156243215F, 0.00183797604F, 0.0003186416F, - 0.00205642148F, 0.00156391761F, 0.000971797912F, 0.00128051522F, - 0.00117961469F, -0.00334555982F, 0.000285257F, 0.00178581942F, - -0.000636322715F, -0.00168477814F, 0.00530360779F, 0.0017279837F, - 0.00281240838F, 0.000936958357F, 0.00179923058F, 0.00233367691F, - 0.00665633334F, 0.00026332878F, 0.00412373757F, -0.0021451253F, - 0.00231998018F, -0.00171014643F, 0.000123511447F, 0.001167209F, - -0.000289501477F, -0.00095108035F, 0.000781342504F, 0.00181388308F, - 0.00101886434F, -0.00230144407F, -0.00578785734F, 0.000239734858F, - 0.00285483547F, -0.00358483219F, 0.00285276398F, 0.00144502614F, - 0.00204429962F, -0.00212085177F, -0.00421012752F, -0.000326147274F, - -0.00280386233F, 0.00322030112F, 0.00369293801F, 0.00262946216F, - 0.00364257605F, 0.00200761762F, 0.000655247946F, 0.00356508722F, - -0.00098865293F, 2.57253578E-5F, 0.00156241783F, 0.00127432402F, - -0.00181282009F, -0.00293245935F, -0.00163032464F, 0.000390812085F, - -0.00284087821F, -0.00196270342F, 0.00422601122F, -0.000746061793F, - -0.00147371F, -3.8972983E-5F, 0.000598657411F, 0.0010540993F, 0.00186842913F, - -0.00631694077F, 0.00257927622F, 0.000160628377F, -0.00133130909F, - -0.000155046073F, 0.00104712124F, 0.00169280206F, -0.00148516987F, - -0.0006872128F, -0.00183937314F, -0.00153882231F, -0.000195308545F, - 0.00271735131F, 0.00140587694F, 0.00343985064F, 0.00102149381F, - 0.000909523573F, -0.00236794562F, 0.00236682571F, -0.00416016858F, - 0.00230781571F, -8.27658914E-6F, 0.000637337565F, -0.000459090283F, - -0.000890590425F, -0.00288739521F, 0.00065045763F, 0.00238119205F, - 0.000847391435F, -0.0032509435F, -0.000284874928F, 0.00150849274F, - -0.000707124302F, 0.000282575F, 0.00493335305F, 0.000838891137F, - -0.00242337422F, 0.000534343359F, -0.00124468794F, -0.000285592658F, - 0.00389017141F, 0.00295916F, 0.00279103289F, -0.00229312433F, - -0.00365784066F, 0.00155009399F, -0.00260592508F, 0.00046339602F, - 0.000573781959F, -0.00154120009F, -0.0021303934F, 0.00199794513F, - 0.0011623333F, 0.00016515849F, 0.000113090013F, -0.00567919947F, - -0.000749941217F, 0.00102968595F, 0.000187132478F, -0.000748974911F, - -0.0015768467F, 0.00147214532F, 0.00388730806F, -0.000344290107F, - 0.00416903617F, 0.000384500687F, -0.00248499028F, -0.00297441753F, - 0.000520423753F, 0.00113928178F, 0.00039308652F, 0.00289396802F, - 0.000491896062F, 0.00114038482F, -0.00155918021F, -0.00140273722F, - 0.00127314555F, 0.000629192102F, -0.00302826241F, 0.00303291599F, - -0.000853456615F, -0.00366037642F, -0.00102856348F, -0.00211975491F, - 0.00286786398F, 0.00129679963F, -0.00339703169F, 0.00127434451F, - -0.000100494122F, -0.0071064611F, -0.0010149969F, 0.00327332341F, - -0.00225068F, -8.7606837E-5F, 0.00014667651F, 0.00109383778F, 0.00125385181F, - 1.58049261E-5F, -0.000199601694F, 0.0024432F, -0.00096632971F, - -0.00133958529F, -0.00174269883F, 0.000410276232F, -0.00166196399F, - 0.000504729338F, 0.000687544874F, -0.00108231686F, 6.75453E-5F, - 0.0014497363F, -0.0021489705F, -0.000593362085F, -0.00309479074F, - -0.000427676627F, 0.00203963974F, -0.00027713392F, -1.46321245E-5F, - -0.000687589869F, -0.00288066152F, -0.00159254728F, 8.55306571E-5F, - 0.00261539384F, 0.000888110604F, 2.40887348E-5F, 0.0034543986F, - -0.00208824314F, 0.00284784F, 0.0024469553F, -0.00134087889F, -0.000743078F, - -0.00176611531F, 0.00178156793F, -0.00130875409F, -0.00317915366F, - -0.00205308245F, 0.000218845424F, -0.00223290687F, -0.0028851761F, - 0.00194675685F, 0.00129501021F, -0.00288999942F, -8.36904728E-5F, - -0.00162001164F, -0.00635185651F, -0.00158330926F, 0.00310757454F, - 0.0057691033F, -0.00146057957F, -0.0027506575F, 0.00503286254F, - -0.00038513192F, -0.00194950739F, -0.00240551028F, -0.00047212487F, - -0.000490480103F, 0.00158391835F, -0.000852799567F, 0.00317176036F, - -0.000265265902F, 0.00280326954F, -0.000674508628F, -0.00313750259F, - -0.00270141172F, -0.00492459396F, 0.000943924591F, 0.00199209875F, - -0.00155280554F, 0.00160268298F, -0.00140361325F, -0.000964170962F, - 0.000759585877F, 0.000316767051F, 0.00177617185F, -0.000551107456F, - -0.00107367022F, -0.000646396482F, -0.000355570461F, -0.00218883133F, - -0.00224387203F, 0.00404763781F, -0.00136692682F, 0.00172761024F, - 0.00272697164F, 0.00129196048F, -0.00291747204F, -0.000912384829F, - 0.000265177223F, -0.00295476941F, 0.00349048572F, -0.00148227555F, - 0.000811653794F, -0.00191938924F, 0.00064391928F, -0.00140597159F, - -0.00103090412F, -0.000400976714F, -0.000971348432F, 0.00274430425F, - -0.00120383326F, 0.00153369573F, -0.00126845494F, 0.00149551407F, - 0.00188330421F, -0.00118425244F, -0.00241750479F, 0.00118075742F, - -0.000293927122F, -0.00439849123F, -0.000326838577F, -0.000884800218F, - 0.000915294862F, 0.00130807061F, 0.00507416623F, -0.00275587663F, - 0.000441137847F, 3.76108546E-5F, -0.00184012519F, -0.00161532965F, - -0.000323110464F, 0.00195122161F, -0.00256261113F, -0.00145966874F, - 0.00230572955F, 0.00109838706F, 0.00353570026F, 0.00123853749F, - -0.000578910811F, 0.00115735177F, 0.000234525884F, 0.00210948801F, - 0.00144399446F, 0.00214089174F, 0.00421856949F, -7.67814563E-5F, - 0.000559363805F, -0.000776850851F, -2.72427933E-5F, -0.000915121F, - 0.00303390482F, 0.00194791588F, -0.00265004719F, 0.000851057179F, - -0.00168036867F, 0.00118043122F, -0.00186313502F, 0.000603368157F, - -0.00227009272F, 0.000495022337F, -0.00249085389F, 0.00173495419F, - 0.00394993089F, -0.000171149542F, -0.00285821082F, -7.7833196E-5F, - -0.000984288868F, 0.00116429466F, 0.000160210984F, 0.000963353086F, - -1.69342438E-5F, 0.00083265407F, -0.00217429432F, -0.00102947524F, - 0.00255679875F, -5.00449096E-5F, 0.000109211302F, -0.00159859296F, - -0.00150338165F, -0.000273687358F, -0.00284920936F, 0.00396718F, - 0.000443271681F, 0.000684704573F, -0.00352920662F, 0.00176307256F, - -0.000523397117F, 0.000623462955F, -0.00137317681F, -0.00161646481F, - 0.00106545212F, -0.00170321495F, -0.00135053543F, 0.00151502853F, - 0.00177143421F, 0.00137504388F, 0.00218537217F, 0.00118105148F, - 0.000578792591F, 1.03856974E-5F, -0.000176008863F, -0.00148462341F, - -0.00156816153F, 0.00200893776F, 0.00342816929F, -0.000482347154F, - 0.000716968614F, 0.004309176F, 0.00022431738F, 0.000308259827F, - -0.00156814605F, -0.00238970877F, 0.00445567071F, 0.00101169758F, - 3.04528348E-5F, 0.000646719825F, 0.00215152232F, 0.000772647152F, - 0.00042357287F, -0.000231228F, 0.00120635831F, -0.00139220175F, - -0.00500378292F, 0.000306991045F, 0.000415313931F, 0.00423402851F, - -0.000147692568F, -0.000532347418F, -0.000455730042F, -0.000520668109F, - 0.00260043819F, 0.00220264494F, -0.00220148894F, 0.00380142569F, - -9.83267237E-5F, 0.000684348634F, -0.000928618829F, 0.00248445314F, - 0.00233244197F, 0.0022939262F, 0.000166256854F, -0.00190699717F, - 0.00167938799F, 0.00160119415F, -0.000789914862F, -0.00111289811F, - -0.000307779032F, -0.000390976929F, -0.00293061743F, -9.70919718E-5F, - -0.0029329441F, -0.003939569F, -0.00123334664F, 0.00203253282F, - -0.0020286392F, -0.00183742295F, -0.00393744651F, -0.00273454841F, - 0.00110844825F, 0.00101514265F, -0.00159563043F, 0.000539054919F, - 0.00157428277F, -0.00152973179F, -0.000503389339F, -0.00124605407F, - -0.000917411293F, -0.00139074947F, 0.00215527532F, -0.00384597154F, - -0.00445177034F, 0.00114596006F, -0.0011099996F, 0.000419480086F, - -0.00246383506F, 0.00285642268F, -0.0020890641F, -0.000417431409F, - -0.000599636696F, 0.00155980326F, -0.00374717335F, 0.000240150941F, - -0.00194717944F, 0.00107587141F, 0.000369863759F, -0.000356242555F, - -0.000459890958F, 0.00313341129F, 0.00182714162F, 0.000265902345F, - -0.0016467612F, 0.00159014412F, -0.000579625717F, 0.000134991904F, - 0.000208201251F, -0.000423051883F, -0.000239462635F, -0.00375062064F, - 0.000248451193F, 0.00492447754F, -0.00175983598F, 0.00124342553F, - -0.00220013736F, -0.00113748037F, 0.0037811019F, -0.00118106417F, - 0.000283151137F, 0.00197350979F, 0.00172250555F, -5.42410926E-5F, - -0.00144276407F, -0.00015999889F, 0.000304538262F, -0.00218433863F, - 0.00437487615F, 0.000382710074F, 0.00027529095F, 0.00105313014F, - -0.00102716382F, -0.000453605171F, -0.00015171511F, -0.00101332075F, - 0.001148129F, -0.0035351729F, -0.00106517097F, -0.00250881515F, - 6.57290511E-5F, -0.00191122026F, 0.00262791687F, -0.00116401713F, - 0.0021644521F, 0.000452274835F, -0.00170257618F, -0.00197723974F, - -0.00215528067F, -0.000451218482F, 0.000678511511F, 5.70584343E-5F, - 0.00114176131F, 0.00367360748F, 0.00060009F, 0.000143775396F, - -0.00248257513F, 0.000558985106F, 0.00241582352F, -0.000894796511F, - 0.00164256629F, -0.000107016684F, 0.000352736912F, -0.00214520353F, - 0.00160463713F, -0.00278690504F, -0.00349585689F, -9.19767408E-5F, - 0.000257640844F, 0.00038227273F, 0.00182005146F, -0.00113712647F, - 0.00236488436F, -0.000125721417F, 0.00181297131F, 0.00114165363F, - -0.000521859387F, 0.000202541356F, 0.000604183762F, -0.000166514612F, - 0.00195391802F, -0.00156704523F, 0.00111700525F, 0.00196536258F, - -0.00140822446F, -0.000177172624F, 0.00125718047F, -0.000504917756F, - 0.00175192067F, 0.000352779636F, 0.000857337494F, -0.000698052347F, - 0.00127159967F, 0.00361995236F, 0.000539962726F, -0.00379144051F, - -0.00112394977F, -0.00120203709F, -0.00192865497F, 0.00154451246F, - 0.000432001398F, -0.00234010047F, 0.00017235284F, 0.0018040709F, - 0.00154936523F, -0.000373940682F, -4.7902904E-5F, -0.00376138161F, - 1.7313474E-5F, 0.000375974021F, 0.00110917806F, -0.000562819303F, - -0.00466491375F, 0.000835149549F, 0.00290863821F, -0.00128291245F, - -0.00219000713F, -0.000708641834F, -0.000812008569F, 0.000304268062F, - -0.00357957091F, 0.00035458908F, 0.00728464499F, -0.00102899922F, - 0.000655620534F, -0.00297023659F, -0.00146278169F, -0.000832496502F, - 0.00168098742F, -0.00106289634F, -0.00355091481F, 0.00190814154F, - 4.64362638E-5F, -0.000651483482F, -1.97050576E-6F, 0.000790710445F, - -0.00237813569F, 0.000382079423F, -0.00105705496F, -0.00124357734F, - -0.00193981268F, -0.000890800555F, 0.00549478969F, 0.00218123174F, - 0.0024279051F, 0.000775037217F, 0.00112820847F, 0.000787821831F, - 0.00127820345F, 0.00108916708F, 0.00202841125F, 0.00479320949F, - -0.00123647356F, 6.91075E-5F, -0.00398460822F, -0.00136189593F, - 0.000104310042F, -0.00274178875F, -0.000734706293F, 0.000183825789F, - 5.52343918E-5F, -0.0021371406F, 0.00385087077F, 0.00182881195F, - -0.000200773429F, -0.000808016339F, 0.00252317218F, -0.00038484775F, - -0.000814989908F, -0.00129396771F, 0.00155178551F, -0.00170492392F, - -0.00167272659F, 0.000671048358F, 0.000275886472F, -0.00161838741F, - 0.000126449304F, 0.00222911523F, -0.0013287802F, -0.00438452233F, - -0.00156177301F, -0.00179334392F, 0.00410496F, -0.00242240168F, - -0.000666301872F, 0.000732253888F, -0.000819462643F, -0.00270292629F, - -0.00181421218F, -0.00274180784F, 0.00297624106F, 0.000400424702F, - -0.000296732585F, 0.00193929137F, -5.26277981E-5F, 0.00104745117F, - 0.000161351651F, 0.000517373206F, -0.000954445102F, -0.00301047834F, - -0.000319788931F, 0.000282254041F, 0.00191053771F, -0.000739857F, - 0.000318528764F, -0.0045213094F, -0.0019731F, -0.00148415565F, - 0.000588705239F, -0.00300152134F, -0.00336355716F, -0.00177521876F, - -4.69895422E-5F, 0.000521033769F, -0.000865054142F, -0.000235402884F, - 0.00022228845F, 7.05478815E-5F, 0.00072712492F, -0.00145693787F, - -0.000511023682F, -0.000628092734F, 0.000584923F, -0.000372722658F, - 0.00543329539F, -0.00102252827F, -0.00282259495F, -0.00055278081F, - -0.0028624956F, -0.00191996491F, -0.000139124371F, 0.000553976046F, - 0.00170955074F, -0.000849802804F, 0.00331439613F, -0.00107066927F, - -0.0014781768F, -0.0013563768F, -0.00115915644F, -0.000563444337F, - -0.00118025893F, -0.000660812075F, 0.00421668263F, -0.000231272745F, - 0.000263155205F, 0.00185166602F, 0.000767240708F, -0.000805564574F, - 0.000526081305F, 0.00127832883F, 0.00267776428F, -0.0010591785F, - 0.00014537129F, 0.00201608031F, 0.00224645622F, 1.32703881E-5F, - -0.000272983685F, 0.00167725049F, 0.00272289384F, -0.000108271423F, - 0.00243431516F, -0.00105951156F, -0.00080729695F, -0.00119924208F, - -0.00153080083F, 0.00210562255F, -0.0017687626F, -0.00171442586F, - 0.00109212799F, 0.00071856362F, 0.000744326448F, -0.0034651889F, - 0.00128200022F, -0.00290547754F, -0.000616006495F, -0.000234273553F, - -0.00146153511F, -0.000754639332F, 0.00164294324F, 0.00172496447F, - -0.00304869469F, 0.00041699881F, -0.00039314953F, -0.00039557353F, - -9.45892825E-5F, -0.00200349814F, -0.00130531436F, -0.00373352156F, - 0.000908075657F, 0.00293885288F, 0.00225423696F, 0.00343630742F, - 0.00106557272F, 0.00266323471F, 0.00200435589F, -0.000661142229F, - -0.00192255934F, 2.61427922E-5F, 0.0016578784F, 0.00253531593F, - 0.000367161381F, 0.00280770357F, -0.00178289704F, 0.00209431536F, - 0.00160109601F, 0.000288400654F, -0.00256082579F, 0.000885325833F, - 0.000369946065F, -0.00226803776F, -0.00191025657F, 0.00248827017F, - -0.00330985151F, 0.00215322617F, -0.000904741872F, 0.00132710219F, - -0.00157920457F, 0.00253865542F, 7.62615455E-5F, -0.00106456527F, - -0.00282553351F, -0.0021767593F, -0.00151727383F, -0.00135167688F, - -0.00145961286F, -0.00276659569F, -0.00229028636F, -0.00101633545F, - 0.000167703605F, -0.000494117965F, -0.00119043456F, -0.00185243157F, - -0.00235964567F, 0.000413369795F, 0.00064571196F, 0.00393597642F, - -0.00146200461F, -0.00179990462F, 0.000574536913F, -0.000538369059F, - 0.000233858969F, 0.00131201628F, 0.000634231896F, -0.00287614972F, - -0.00036289834F, -0.0012541156F, -0.00094128761F, 0.00167302194F, - -0.00120597915F, -0.000482009607F, -0.00193222391F, -0.00258417754F, - 0.000652046932F, 0.00175436703F, -0.000478832895F, -0.000401332509F, - -0.00152494223F, -0.00011049828F, 0.000373849616F, 0.00134567381F, - 0.00237222039F, 0.00149905833F, -0.0013058699F, 0.00166553725F, - 0.00139271945F, -0.000216375731F, -0.0033334149F, 0.00115884584F, - -0.00176403415F, 0.00110907F, 0.00587496674F, -0.00112204289F, - 0.00131727743F, 0.00157258555F, 0.00407233229F, -0.000130362663F, - 0.00186609384F, 0.0024315773F, 0.00297084614F, -0.00110758131F, - 0.000820738438F, -0.00352513115F, 0.0017395654F, 0.000397383F, - -0.00166472362F, 0.00229929551F, 0.00138033717F, 0.000231454469F, - -0.0020259351F, 0.000171381762F, -0.000286322582F, 0.000157772025F, - -0.000104376828F, 0.00192616077F, -0.000333718723F, 0.000989335938F, - 0.00190882909F, -0.001843084F, -0.000224006217F, 0.00112898089F, - 0.000662706501F, -0.00166917406F, 0.00144730671F, 0.00132781512F, - -0.000633251388F, -0.00142574881F, 0.00199498539F, -0.000855431135F, - -0.00188788411F, -0.000157441551F, 0.000377533317F, -0.000297580438F, - -0.000457594055F, 0.00333949737F, 0.00164020131F, -0.00123326562F, - 0.00189958618F, -0.00284500513F, -0.00126287818F, 0.00129750127F, - -0.000990565401F, -0.00542915054F, -0.0004826F, -0.00243212702F, - -0.00283284392F, 0.000730157888F, 0.00454489328F, 0.00222660089F, - 0.00322956196F, 0.000632173149F, 0.000850163866F, -0.00257586595F, - 0.000847010175F, 0.00221860153F, -0.000211632432F, 0.000459817675F, - 0.00227837241F, -0.000612825388F, -0.00297602103F, -0.00207474036F, - 0.000461466669F, -0.00209684833F, 0.00116918015F, -0.00057847274F, - -0.00234263833F, -0.000512941333F, 0.0015508642F, -0.000523151248F, - -0.00243994268F, -4.52440509E-5F, 0.00138941372F, 0.00138775678F, - 0.00272746291F, 0.00114110368F, 0.00127366534F, 0.000867417315F, - -0.00205394113F, 0.000579430256F, 0.000555638806F, -0.00155183766F, - -0.000870825432F, 0.000636401935F, -0.000555699226F, -0.000357427052F, - 0.00119344972F, -0.00255059218F, -0.00321854046F, 0.00265159737F, - -0.00120503211F, 0.00230831234F, -0.00123464142F, -0.00224535307F, - 0.000599701F, 0.000647524954F, 0.00239672419F, 0.000508274243F, - -0.000831587706F, 0.00205680053F, -0.000993710128F, 0.00036563896F, - 0.00199926621F, 5.47204672E-6F, -0.000472653424F, 0.000458800729F, - -0.00176343287F, 0.000743376F, 0.0025251992F, -0.000730597181F, - -0.00181449053F, 0.00120519788F, -0.0020660283F, 0.000602788699F, - -1.17549152E-5F, 0.000584003457F, 0.00173703034F, 0.00373799191F, - 9.94909715E-5F, 0.000861271867F, -0.000726926F, -0.000624862092F, - 0.00114634272F, 0.000527703F, 0.000916543417F, 0.000306162256F, - 0.00205186987F, 0.000256555562F, 0.000216689674F, 0.000562511734F, - 0.00102607161F, -0.002099487F, -0.00130514556F, -0.00153598934F, - 0.00422620168F, -0.000119688273F, -0.00108150521F, 0.00383306877F, - -0.00116131082F, 0.000436389266F, -0.00135590672F, 0.00265536434F, - -0.00193034555F, -0.00106212287F, 0.000114323579F, 0.00140496786F, - 0.00324285915F, -9.2542512E-5F, -0.00133236474F, 0.00235538278F, - -0.000676368F, -0.000394194969F, 8.40716384E-5F, 0.00273943623F, - 0.00122482865F, -0.00246292772F, -0.000956220378F, 0.00113178464F, - 0.000903626205F, -0.00229950296F, 0.00158836087F, -0.000555022969F, - 0.000600516854F, 0.00253501325F, -0.00106522697F, -0.00351602398F, - -0.000985893072F, 0.000680344878F, 0.00162446348F, 0.0018886826F, - -0.000499033544F, 0.00242488715F, -0.00192914438F, 0.00182983163F, - -0.00600094534F, 0.00111416378F, -0.000338956947F, 0.00133410632F, - 0.00135707192F, -0.000721584889F, -0.00151510665F, -0.00320810569F, - 0.00417922949F, 0.00414786115F, 0.00182283809F, -0.00451089209F, - 0.00194827304F, -0.00249765813F, 6.02057867E-7F, 0.00127891556F, - 0.00252157589F, -0.00109989487F, 0.000926535577F, 0.00115318201F, - -0.00124218059F, -0.00154831284F, 0.00186275237F, -0.00245319284F, - 0.000676941709F, -0.00051497994F, -0.00157294667F, -0.00184654596F, - -0.000235952786F, 0.00203687372F, -0.00235796161F, 0.00400502421F, - 0.000224364689F, -6.85807299E-6F, 0.00191781309F, -0.00188435323F, - 0.00158788858F, -0.00149078318F, -0.000438610761F, 0.0026111817F, - 0.00173870136F, -0.000863291032F, 0.000128979184F, -0.00164763688F, - -0.00141013064F, -0.000289744494F, -0.00085171737F, -0.000249987381F, - -0.00315899425F, -0.00102527812F, 0.00261718198F, 0.00125098648F, - -0.00242340961F, -0.00385286962F, -0.000388460234F, 0.000543320668F, - 0.000353026262F, 0.000991816516F, -0.000459621515F, -2.86630584E-5F, - 0.000655218784F, 0.00158431032F, 1.48620429E-5F, 0.0035067054F, - -0.000572449353F, -0.00362022221F, 0.000585049449F, -0.00258885114F, - 0.00208294624F, 0.00127144076F, 0.000681915844F, 0.00213198876F, - 0.00013453314F, 0.0011636616F, -0.00130656152F, -0.00171742623F, - -0.000442539633F, -0.000448687642F, -0.0012405013F, 0.000257963926F, - 0.00128199358F, 0.000439817435F, -0.000549541262F, -0.000331418938F, - -0.00107268023F, 0.00226193713F, -0.000558453554F, 0.00289123389F, - 0.00192041439F, 0.00167375046F, 0.00121718855F, 0.00504396437F, - -0.00219919859F, -0.00278909388F, -0.00140391348F, -0.000903780106F, - -0.000927794F, 0.000245014235F, -0.00412018923F, -0.00171864405F, - -0.000495899818F, 0.000841091038F, 0.00308675389F, 0.000379895035F, - -0.00200116192F, -0.00119921798F, -0.00337163755F, -0.00271788496F, - -0.00180890423F, 0.00137388741F, -0.000637599383F, -0.00120614504F, - -0.000667778484F, -0.000586883398F, 0.0013239017F, -0.00354311778F, - -0.000659869926F, 0.000492627558F, 0.00161865365F, 0.00110915536F, - -0.00106756017F, -0.00403293828F, -0.00204426795F, -0.000154523441F, - 0.00111919665F, 0.00161797123F, -0.000956847449F, 0.000786406221F, - -0.0010665115F, -0.00118669763F, -0.000799426F, -0.000908914546F, - 0.00217583892F, 0.00232953974F, 0.00217700819F, -0.00040628569F, - -0.00210878463F, -0.00278129592F, 0.000402068195F, 0.000416332F, - -0.00233413745F, 0.00267947977F, -0.00157778745F, -0.00228413125F, - 0.000263005146F, 0.0029785044F, -0.000409875327F, 0.000159539035F, - -0.000170463696F, -0.00188694522F, 0.0007643727F, -0.000616749167F, - -0.000507526216F, -0.000787721947F, -0.00148821319F, -0.000177274313F, - 0.00129312801F, 0.00116875966F, -0.00150443346F, -0.00231140107F, - 0.000880717533F, 0.000151159562F, 0.000296713231F, -0.00366245699F, - 0.0001817187F, -0.0014602642F, 0.000692938047F, -0.00149081706F, - 0.00377620314F, 0.00235801283F, 0.000732720189F, -0.0022710464F, - 0.000665368338F, -0.00136119954F, 0.00075467641F, -0.000870499585F, - 0.00080808741F, 6.21683866E-5F, 0.00186664215F, -0.00119351351F, - 0.00117405434F, 0.00164004054F, -0.00221659383F, -0.00142707827F, - 0.00324398186F, -0.00109510694F, 0.000869205338F, -0.00127029733F, - 0.00121624675F, 0.000883405271F, -0.00262109982F, 0.000327554852F, - 0.000643689709F, -0.000162640747F, 0.0017586801F, 0.000533686485F, - 0.00161771558F, 0.00254705898F, -0.0025011613F, -0.00269739167F, - 0.000359494F, 0.000152673776F, -0.00460823392F, 0.00200590654F, - -0.00280158711F, -0.000675767544F, -0.00158255466F, -0.00416788831F, - -0.0019469714F, -0.000165519145F, 0.00136556139F, 0.00144411228F, - 0.00306425057F, -0.00210015685F, 0.00313786278F, -0.000131383218F, - 0.00118813675F, -0.000137965777F, 0.00143604272F, 0.00151034223F, - -0.000881300832F, -0.000958534F, -0.00058989122F, -0.00093230675F, - 0.00105407566F, -0.00100445969F, 0.00194931205F, 0.00209878897F, - 0.000554036524F, -0.00212560943F, -0.000902628817F, -0.0032776969F, - 0.000657556404F, -0.00114751852F, -0.00181660813F, 0.000614201243F, - 0.000646095781F, -0.00276856241F, 0.0010685334F, -0.00184515736F, - -0.00232490874F, 0.00242072134F, -0.000776017841F, 0.00102016178F, - 0.00209366204F, 0.00240504602F, -0.000220025759F, -0.00274434919F, - 0.001291655F, 0.00167750346F, -0.000839609944F, 0.00215802412F, - 0.000688999717F, 0.00191656442F, -0.00409687776F, 0.00167847576F, - -0.00104434113F, 0.00116478349F, -0.00237875711F, 0.00190534163F, - 3.7693957E-5F, 0.00284487498F, 0.000527651282F, 0.00564147765F, - -0.000561844034F, -0.00442138873F, 0.000163485602F, -0.00234165648F, - 0.00238988874F, -0.00275747408F, 0.00174612692F, 0.000448670879F, - -0.00216783467F, -3.08538183E-5F, 0.00271215988F, 0.000378963508F, - 0.00235756487F, 0.00160176598F, 0.000440631149F, -0.000357567F, - 0.00045661148F, -0.000269273442F, -0.000571812852F, 0.0029640079F, - -0.00228692661F, 0.00210395479F, 0.00139967282F, -0.000146242513F, - -0.00154289627F, 0.00171942252F, -0.00225779857F, -0.000443952F, - 0.000523556839F, 0.000850394485F, 0.000497282366F, -0.00188710203F, - 0.00354100135F, -0.00230548158F, -0.00176666537F, -0.00256259646F, - -0.000361938291F, -0.00323883654F, -0.00136526651F, -0.000812909857F, - 7.14635E-5F, 0.00434292527F, 0.00120350637F, -0.00073626783F, -0.0024304972F, - -0.000583018584F, 0.00222134124F, 0.00158877391F, 0.00139140524F, - 0.00257246685F, -0.00219454872F, -0.00377865112F, -0.00177747966F, - -0.000777566805F, 0.000936748518F, -0.000118732591F, 0.00177539175F, - -0.00141114672F, 0.00221388764F, -0.00123610708F, 0.00016217232F, - -0.00226869F, -0.000128301312F, -0.00165174F, 0.000943905674F, - 0.00212783809F, -0.00216478645F, -0.000367151311F, 0.00232943357F, - -0.000420059398F, 0.00299638F, 0.00114562141F, 0.00228626933F, - -0.000365540327F, 0.00115954573F, -7.02923E-5F, -0.00101407629F, - 0.000783474185F, 0.00060165883F, 0.00464719487F, -0.00213123695F, - 9.86887826E-5F, 0.000966053805F, -0.00125705951F, -0.000521191512F, - 0.000395930459F, -0.00245458586F, -0.00386788836F, 0.00270558894F, - 0.000389031135F, -0.00172087888F, 0.00262188562F, 0.00230994867F, - -0.00106781954F, -0.00347533775F, 8.7487846E-5F, 0.00169619697F, - -0.00176863559F, 0.00236996403F, 0.00184579007F, -0.00428604847F, - -0.000983062782F, 0.00039306382F, 0.00575956842F, -0.00104668469F, - 0.00292160548F, 0.00253553642F, 0.000900489569F, 0.000245336F, - 0.000222009854F, -0.000158507726F, 0.00183238357F, -0.00344459456F, - -0.00189553958F, -8.41395304E-5F, -0.00117788254F, 0.00174139673F, - -0.000133942944F, 0.000516993401F, -0.00274472241F, 0.000997601775F, - 0.00178702222F, -0.00126517576F, -0.0023740856F, 0.00222486979F, - 0.00202741218F, 0.00165808899F, 0.00288692908F, 0.000872489065F, - -0.00234520761F, -0.000189702259F, 0.0028932523F, 0.00141496025F, - 0.000942744373F, 0.00198565773F, 0.00116340385F, 0.00269466662F, - 0.000413616712F, 0.000158066789F, 0.00478198F, 0.000376239797F, - -0.00453252532F, -0.000438055111F, -0.00325160963F, 0.000264997187F, - 6.17440237E-5F, 0.00176881591F, 0.000314247329F, -0.00118291541F, - 0.000288764451F, -0.00088390219F, 0.00268680532F, 0.000149929459F, - -0.00090293237F, -0.00123920257F, 0.00152446434F, 0.00253679231F, - 0.00265399925F, -0.00173557352F, -0.00343628787F, 0.000875650498F, - 0.00354847405F, -0.000629955786F, -0.00347712403F, -0.00115556514F, - -0.00101994793F, 0.00268620136F, 0.00267732283F, -1.06546831E-5F, - 0.000415955437F, -0.000102971826F, -0.000584549503F, -0.00180905568F, - 0.00147677923F, 0.000483569573F, 0.000402313482F, 0.00186062371F, - -0.00130130642F, -0.00318378233F, -0.00144508691F, -0.00332324952F, - 0.000427618914F, 0.000254456536F, -0.00146310462F, 0.00213119F, - 0.00244606822F, -0.00132596807F, 0.00342899F, 0.000328149035F, -0.00102499F, - 3.46003653E-5F, 0.00285332301F, 0.00135007547F, 0.0014361901F, - 0.000278213294F, 7.05196499E-6F, 0.00332277617F, -0.00200594519F, - 0.000126689105F, 0.00212718244F, 0.00182970788F, -0.00255610491F, - 0.00382579467F, -0.00208501797F, 0.00392846F, -0.00089673535F, 0.00140237F, - -6.2442261E-5F, -0.0022168709F, 0.00149424607F, 3.34497272E-5F, - 0.00158410729F, 6.62104139E-5F, -0.00286883698F, 0.00349048432F, - -0.00016448331F, -0.000543190225F, 0.00307720783F, 0.000117502859F, - 0.00270009576F, 0.00220947433F, -0.00315174274F, 0.00152627507F, - -0.00051714736F, -0.0023953896F, 0.000704782258F, -0.00142149976F, - -0.00316741806F, 0.000609361567F, 0.00219613104F, -0.00140622573F, - -0.000496888766F, -0.00209984835F, -0.00284918933F, 0.00311995693F, - 0.00236658589F, -0.00201128446F, 0.00314050727F, 0.000481145369F, - 0.000841320259F, 0.0022424988F, -0.00132147747F, 0.00105625857F, - 0.00324923592F, 0.00124405383F, -0.000184701465F, 0.00119624916F, - -0.000527685334F, -0.00107158453F, 0.000568365795F, -0.000220719332F, - -0.00271675666F, 0.00125028193F, 0.00129811873F, 0.00184953073F, - 0.000381120364F, 0.000320476858F, -0.0028481835F, 5.16950386E-5F, - 0.000283574336F, -0.00275759725F, -0.000720337674F, 2.42925544E-5F, - -0.000190717416F, 0.00453800475F, -0.00157569454F, 0.00151194329F, - -0.000213928477F, -0.00121107453F, 8.01231436E-5F, -0.00250692619F, - 0.00111336715F, 0.000406358391F, -0.000634178519F, 0.000663057319F, - 0.00131629745F, -0.00376896048F, 0.0013730292F, -0.00251562218F, - -7.26799772E-5F, -0.00122788025F, -0.000379739766F, -0.00193048094F, - 0.00173908507F, 0.00305923773F, 0.000118785742F, -0.00108676637F, - -0.00145608536F, -0.00299653155F, -0.00215218938F, 0.00124776422F, - 0.00152655458F, -0.000842247F, -0.000947465946F, -0.00234900485F, - 0.000516755623F, -0.000886311231F, 0.00238947943F, 0.000885998306F, - 0.00104895153F, 0.00397097133F, 0.00182921148F, 0.000511814F, - -8.02143113E-5F, 0.00471337F, 0.00110194017F, -0.000494901382F, - 0.00163739582F, -0.00173494138F, 0.000111945177F, 0.000437725103F, - 0.000293108926F, -0.00171152363F, -0.000486831937F, 0.00119038357F, - -0.00304403878F, -0.000574877951F, 0.000580941F, -0.00303054089F, - -0.0015041302F, -0.00192364142F, 0.00335054216F, -0.0033667176F, - -0.000111385227F, -0.00148709677F, 0.000226952543F, -0.00143785495F, - 0.00255653355F, 0.000352099974F, -0.00105997093F, -0.000280537468F, - -0.00292973476F, 0.00186427531F, -0.000292370445F, -0.00360227027F, - -0.00361790205F, 0.00473829871F, 0.000405669212F, -0.00160646765F, - 0.0035300774F, 0.00136430049F, 0.00142391725F, 0.000710497377F, - 0.000233688566F, 0.00195360882F, -7.72272542E-5F, 0.000589615956F, - 0.00046912473F, -0.000357647688F, 0.00151147007F, -0.000337495876F, - -0.00204414595F, 0.00032970836F, -4.78777838E-5F, -0.00105829607F, - -0.00104149291F, -0.00279929466F, 0.0011990827F, -0.00164779602F, - 0.00189628243F, -0.000108927336F, -0.00112388399F, -0.00175066141F, - 0.00126584247F, -0.000430850807F, 0.000601232867F, -0.00207620347F, - 0.00113208219F, 0.00138287153F, 0.00101483415F, -0.000188910199F, - -0.00179858878F, -0.000658840116F, -0.00305355573F, -2.08453912E-5F, - 0.000205831064F, 0.000940531143F, -0.000263854192F, -0.000150324326F, - -0.0016709296F, 0.00182890426F, 0.000885489688F, -0.000852800673F, - -0.00152908743F, -0.00376185728F, 0.00135718903F, 0.00157927594F, - 0.000666410895F, -0.00163848081F, -0.0011943382F, -0.000532264763F, - -0.000394919F, 0.000545625575F, 3.88271765E-5F, 0.00228729681F, - 0.00335934176F, -0.00186993077F, -0.00160418416F, -0.00171705626F, - -0.00145025412F, 0.00139931589F, 0.00122162292F, -0.00216659065F, - -0.00283340178F, -0.00237566582F, 0.00059343694F, -0.00338980393F, - -0.00142493215F, 0.000337086909F, 0.00103051285F, -0.00164504768F, - 0.00119521632F, 0.000953896262F, -0.00106313429F, 0.002735927F, - 0.00128391746F, -0.00153827423F, -0.00351021532F, 0.000687041087F, - 0.000263562775F, -0.00244148F, 0.00388514926F, -0.00228349445F, - 0.00231666933F, -0.00116847549F, -0.000154511537F, -0.000788292557F, - 0.000315764191F, 0.00190970907F, 0.00155719672F, -0.000488210237F, - 0.00296905078F, -0.00329125882F, 0.000307238748F, 0.00223568105F, - -3.65337246E-6F, 0.00030047883F, 0.00276911492F, -0.00354058784F, - -0.00132416573F, 0.00135966286F, 0.000678082637F, 0.000365986227F, - -0.000841993431F, 0.0030992846F, -0.00233660196F, -0.0046649361F, - -0.00176347373F, -0.00125875429F, -0.0035705918F, -0.00324685F, - -0.000291001517F, 0.0017705221F, 0.00167071261F, 0.00110180583F, - 0.00264521828F, -0.00073311344F, -0.000130616056F, 0.000559826905F, - -0.00332918297F, 0.00187432114F, 0.00198371988F, -0.00203048158F, - 0.00212439056F, -0.00191899424F, 0.000311325595F, -0.00169301219F, - -0.000762816228F, 0.000963257451F, 0.000708219712F, -0.000634415715F, - 0.000968866749F, 0.00293562631F, -0.000413379923F, -0.00120261742F, - 0.00305435644F, -0.000936322554F, 0.00268098433F, -0.00222324557F, - -0.00182228431F, 0.00548621174F, 0.00046322128F, -0.000718097319F, - -0.000509191887F, -0.000698537915F, -0.00131954288F, 0.00110059092F, - -3.99098099E-5F, -3.9097511E-5F, -7.18858137E-5F, 0.00194199942F, - -0.00150935922F, 0.000246068463F, -0.00117502315F, 0.000569293625F, - -0.00405175053F, 0.000282546302F, -0.00111263827F, 0.000149904779F, - -0.00431066658F, 0.00134833215F, -0.0024604022F, 0.00315336976F, - 0.00128528138F, 0.00131049857F, 5.96444879E-5F, 0.00138486084F, - 0.00206266157F, 0.00125250476F, -0.000103062797F, -0.000389118242F, - 0.00198505353F, -0.000638589F, 0.00155513594F, -0.00142613857F, - 0.000123268139F, -0.000121215009F, 0.00117401162F, 0.00304351584F, - -0.000233956685F, 0.00146307377F, -0.000165334204F, 0.0011037623F, - -0.00219699345F, 0.000287763949F, 0.0015217131F, -0.00268645561F, - -0.000613226148F, 0.00105688116F, -0.000669244269F, -0.00117190112F, - -0.00192384713F, -0.000136778865F, 0.000873337267F, 0.00120402535F, - -0.000299401931F, -0.00147442345F, -0.000255243824F, -0.000723858888F, - -0.00246068393F, -0.000675007817F, 0.000561514753F, -0.00105920783F, - -0.00390608166F, -1.90692554E-5F, -0.00351666193F, -0.000437086856F, - -0.000542144116F, -0.00137938338F, 0.000819960143F, 0.00189011148F, - 0.00291714375F, -0.00104750239F, 0.000648959423F, 0.0024681706F, - 0.000986904139F, -0.000468202517F, 0.00102291664F, 0.00219460181F, - -0.00320344F, 0.000350418675F, 0.00281772506F, -0.00479155546F, - 0.0010552766F, -0.000725866412F, -0.00143821107F, 9.70773253E-5F, - 0.000429732958F, -0.00152279402F, 5.00390342E-5F, -0.00272891601F, - 0.00118896412F, -0.00162804162F, 0.00131304748F, -0.00149279239F, - 0.00120791537F, 0.00195018563F, 0.00119689142F, 0.000428696949F, 0.00110477F, - -0.00208876468F, 0.00191360468F, -0.00077190937F, 0.000390362635F, - -0.00104825851F, -0.000989336171F, 0.000504737312F, 0.00287697581F, - -0.00219876389F, 4.86240097E-5F, -9.16997597E-5F, -0.000434878719F, - 0.000183997021F, -0.000432330213F, -0.000900531071F, -0.000938566925F, - -0.00283840415F, 0.000764497905F, -0.000321186963F, 0.00403696857F, - 0.000303534267F, 0.00110989576F, 0.000779251F, -0.000521259673F, - -0.00159570714F, -0.00245454838F, 0.0015868363F, -0.00127820764F, - 0.00196794071F, -0.00372075662F, -0.000164215046F, -0.00086188619F, - 0.00065434858F, -0.000344736414F, 0.000498772424F, -0.00306028919F, - -0.00318819378F, 0.00096805254F, 0.00184968801F, -0.00308747077F, - -0.00172794773F, -0.000684790895F, 0.00223753206F, 0.00127146044F, - 0.000260014698F, 0.000959596F, 0.00221810956F, -0.0017544853F, - -0.00161576993F, 0.00132933655F, 0.000484747579F, 0.00341931148F, - 0.00179285812F, 0.00295514683F, 0.00129838171F, -0.00181753363F, - 0.000943296182F, 4.07533935E-6F, 0.000389028166F, 0.00102143188F, - 0.000299257081F, 0.00130571751F, -0.000127718798F, -0.00188569771F, - -0.00223572901F, -9.95712544E-5F, 0.0029284542F, 0.00174672506F, - -0.00244690641F, 2.8522194E-5F, 0.000184099772F, -0.000133394686F, - -6.46658518E-5F, 0.000472594926F, -0.00192956137F, 0.000281338493F, - 0.00136475486F, -0.00121931173F, -9.88921147E-6F, -0.00100299716F, - 0.000976976124F, -8.74223551E-5F, -0.00138821348F, -0.00182911858F, - -0.00318857189F, -0.000851272896F, 0.00266058068F, 0.00213009887F, - 0.00180156261F, -0.000340871891F, -0.000579350395F, 0.00209733145F, - 0.00319293304F, 0.000172250671F, -0.00310760201F, -0.00200389861F, - 0.000923343585F, 0.00135641207F, -0.00200556102F, 0.00133000873F, - 0.00256555155F, -0.00205890811F, -0.0020097848F, -8.93067481E-5F, - 0.000653035357F, -0.00131896487F, -0.000354680757F, 0.00157454133F, - 0.00196422078F, -0.00288774446F, -0.00172353722F, -0.00221136422F, - 0.00212226599F, -0.00202317047F, 0.00101654278F, 0.000864669448F, - 0.00204115617F, 0.000795313739F, 0.00110865606F, -0.00194292527F, - 0.000169619801F, -0.00127454358F, -0.00150766422F, -0.000730828557F, - -0.0018094593F, 2.60145425E-5F, -0.00392291928F, 0.00163259264F, - -0.00179555407F, -0.00175600604F, 0.00195235969F, -0.00366905099F, - 0.00166346703F, -0.000730760104F, 0.000279544882F, 0.00122356287F, - -0.00140013394F, 0.00176010444F, 0.00259587076F, -0.00329294964F, - 0.00130670681F, 0.000872877077F, -0.00260049221F, 0.00109839195F, - 0.000943696825F, 0.00196110248F, 0.00111705135F, 0.00316220289F, - 0.000299034582F, 0.00023545677F, 0.00163018913F, -0.000231879021F, - 7.44302888E-5F, 0.00228889915F, -0.000204943091F, -0.0025331818F, - -0.000616868609F, -0.000478658389F, 0.000996376155F, 0.00154011475F, - 0.00380797056F, 0.000967589673F, -0.000190269973F, -0.00135696167F, - -0.00259928615F, -0.00420809444F, 0.00404605363F, -0.00167381356F, - 0.00240995456F, -0.00149269635F, 0.00299862307F, 0.0023713375F, - 0.000657549128F, -0.00141049025F, -0.00364020164F, 0.00208034506F, - -0.00223948085F, -0.00195467216F, -0.00133005448F, -0.000514850719F, - 0.00332748983F, -0.00084992341F, -0.00191561074F, 0.00064539304F, - -0.00154459965F, -0.00418246631F, 0.000449139217F, 0.00010843129F, - -0.00162430655F, -0.00141722232F, 0.000371994392F, 9.09662776E-5F, - 0.00186018681F, 0.000736071845F, 0.00216177129F, 9.6294636E-5F, - 0.00347343856F, -0.000842689362F, 0.00171366287F, -0.000576649618F, - 0.0025717176F, -0.00130352983F, -0.0023403035F, 0.00204810849F, - -0.000311364682F, 0.00321626F, 0.00347788236F, -0.000596792554F, - 0.00259058946F, 0.000750619336F, 0.00295758154F, -0.00306839519F, - 0.00208334974F, -0.000250681798F, 0.00239749555F, -0.00434006657F, - 0.00163677416F, -0.00357422815F, 0.000364564359F, 6.50619186E-5F, - -0.00193013612F, 0.00391829107F, -0.00205746409F, 0.000608551723F, - -0.00301784044F, 0.000552282436F, 0.000337835721F, -0.00135925785F, - 0.0010695887F, -0.00375785329F, -0.00205894187F, -0.000174033092F, - -0.0042679864F, 0.00166787091F, -0.00162338966F, 0.00180769898F, - 0.00255116285F, 0.00433540298F, 0.00195064431F, -0.000507565041F, - -0.00047302086F, -0.000368799461F, 0.00111104117F, 0.00148929108F, - 0.00104956247F, 0.000583491637F, 0.00203529978F, -0.00303056138F, - 0.000879628467F, -0.00136729854F, 0.00142899633F, 0.000696305477F, - -0.00160306657F, 0.00121809077F, -4.89914019E-5F, 0.00114919536F, - 0.00109819404F, -0.000814015395F, -0.000412940601F, -0.00033164589F, - -0.00405734591F, -0.00142779888F, -0.000449778F, 0.00261636847F, 0.00265118F, - 0.00359494146F, 0.00131131196F, -0.0015539024F, -0.00201994227F, - 0.00109226909F, 0.000414976734F, 0.00221998105F, 0.000214929751F, - -0.00177207321F, -0.000888283248F, -0.00337155769F, 0.00104981381F, - -0.00150872429F, 0.00136624731F, 3.2393531E-5F, -0.00102863251F, - 0.00144656783F, 0.00113427057F, -0.00198298879F, 0.00231367163F, - 0.000261044333F, 0.00113847887F, -0.000776025292F, 0.000867024704F, - -0.00462878309F, 0.00448127231F, -0.00112671091F, 0.00154576404F, - 0.00153356942F, -0.00022976505F, -0.000342745945F, 0.00216622814F, - 0.000732122629F, 0.000939822814F, 0.00149791F, 0.000160391588F, - -0.00177755451F, 6.4805783E-5F, -0.00319323386F, -0.00045334379F, - 0.0015506309F, -0.00025330935F, -0.00218680664F, -0.00196753233F, - -0.00273288018F, 0.00239036488F, -0.0022813566F, -0.00301224808F, - -0.00170842884F, 0.00302458578F, -0.00161086628F, 0.00141012773F, - -0.00122889038F, -0.00178184302F, -0.00322269043F, -0.00134290277F, - -0.000817533408F, -0.000117135234F, 0.00248313043F, -0.000525765412F, - -0.00263687898F, -0.00197917246F, -0.00229044F, 0.000877362036F, - -0.00245154649F, 0.00404232228F, 0.000385018095F, -0.000471970212F, - -0.00159073155F, -0.00115958333F, 0.000339774124F, 0.00379483309F, - -0.00146395375F, 0.000620199193F, 0.00124071597F, -0.00228733337F, - -0.0038470414F, -0.000411852554F, 0.00218915683F, -0.00274502742F, - -0.000682360085F, 0.00234213634F, 0.00114780653F, 0.00307561643F, - -0.000411721849F, -0.00236419123F, -0.00173742662F, 0.000162924247F, - -0.0012559935F, 0.00300109084F, -0.00116777711F, 0.00229524495F, - -0.00108616578F, -0.000582930865F, 0.00228715735F, -6.46353801E-5F, - -0.00284238951F, 0.000678726588F, 0.000477969821F, 0.00362349767F, - -0.00246394123F, 0.00130052422F, 0.00036609589F, -0.00100801268F, - 0.00095731084F, -0.00171189127F, -0.00118229073F, -0.000406303443F, - 0.00147661765F, 0.000893949822F, -0.00300705945F, 0.00103084068F, - 0.000832581834F, 0.000901876541F, -0.00246347953F, 0.000397034833F, - -0.00058606F, -0.000689722539F, -0.00115896831F, -7.15323185E-5F, - 0.000546303F, -2.87825242E-5F, 0.00335967843F, -0.00139819761F, - 0.000141313023F, -0.00138727087F, 0.00143444387F, -0.00387467491F, - -3.50299488E-5F, 0.00123296294F, 0.00117126992F, -0.00245024962F, - -0.00062961824F, -0.000471948617F, 0.000982336234F, -0.00100089912F, - -0.00114547601F, 0.00102450792F, -0.00256620906F, 0.00061298F, - -0.00183406286F, -0.000389203546F, -0.00211693603F, 0.00138069724F, - -0.00169828499F, -0.000168842205F, -0.00222009909F, -0.00396739878F, - -0.00329164788F, 0.00105932925F, 0.00132878195F, 0.00165921601F, - -0.000323000277F, -0.00130247348F, -0.00283161807F, 0.0027001563F, - -0.0035631361F, 0.000337608042F, -0.00220859284F, 2.00847571E-5F, - 0.00108419789F, 0.00334301917F, -0.00160139194F, 0.00126521313F, - 0.00213297829F, 0.00321948272F, -0.00105171651F, -0.00111244095F, - -0.000123922349F, -0.000195789486F, -0.00320943235F, 0.00142874452F, - -0.00107179675F, 0.000486726407F, -0.00088225567F, -0.00298681F, 0.00078645F, - 0.000160280528F, 0.00270688441F, 0.00171649549F, 8.08018813E-5F, - 0.00134681282F, -0.00213394081F, -0.00237180316F, 0.00151983823F, - 0.00221028761F, 0.00294536026F, -0.000911497104F, 0.00333689526F, - 0.00189197925F, 0.00313795195F, 0.00284334505F, 0.000676973315F, - -0.0015854761F, -0.0013517749F, 0.00121840066F, 8.07824472E-5F, - 0.00303347013F, -0.000475655339F, 0.00249571307F, -0.00122785778F, - 0.00096041814F, 0.00474387268F, 0.00156537024F, 0.00397527451F, - -0.00196053064F, -0.00116360863F, 0.00171029661F, 0.0040571834F, - -0.000344622676F, 0.0027189164F, 0.00225892779F, 0.00216582045F, - 0.000237779241F, -0.00158547063F, 0.00084750721F, -0.00250705F, - -0.00153009046F, 0.00267723156F, 0.000368538778F, -0.00258507323F, - 0.000178183735F, 0.00083220104F, -3.91298163E-5F, -0.00348512782F, - 0.00137290813F, -0.00234722719F, -0.00172468554F, 0.00079269649F, - 0.00131907943F, -0.000694586837F, 0.00406161137F, 0.000581196626F, - 0.000344197528F, -0.000130786691F, 0.0012556134F, -0.000970223686F, - 0.00227060332F, 0.00196746155F, 0.000426222658F, 0.00153507083F, - 4.82207797E-5F, -0.000769429083F, -0.000823543465F, 0.00112543057F, - 0.000352793897F, -5.1870953E-5F, 0.00188880623F, -0.00128302141F, - -0.00078025437F, -0.00285273395F, -0.000329174713F, -0.00324980821F, - 0.00169345737F, -0.00232554344F, -4.72349238E-5F, 0.00514199492F, - 0.00145487452F, -0.00134179136F, -0.000924131309F, 0.000642340863F, - -3.74856863E-5F, -0.00061785907F, 0.0022495219F, 0.00130418944F, - -0.00278979121F, -0.000720708573F, -0.00206792029F, 0.00196593557F, - 0.00119285134F, 0.00183081662F, -0.00102153F, 0.000905480236F, -0.000698488F, - -0.00094819197F, 0.000652214396F, -0.00190082588F, 0.000364801468F, - 0.00161438785F, -0.00190770661F, 0.00191087415F, 0.00140699453F, - 0.00182811206F, 0.00179393589F, -0.00258958014F, -0.00156603183F, - 0.00320492242F, -0.00083196105F, -0.00158652302F, -0.00216739951F, - 0.000150251217F, 0.00380158F, -0.00250799395F, -0.00277516339F, - -0.00149470347F, -0.000285158807F, -9.70829E-5F, -0.000362755032F, - -0.000789170444F, -0.0012919131F, -0.00203724F, -0.00134649046F, - -0.00116190768F, -0.0023163415F, -0.00218521012F, -0.000373110903F, - -0.00117220602F, -0.00445853174F, -0.000905473775F, -0.00138699205F, - 4.80189374E-5F, 0.00038894976F, 0.000636365847F, 0.000239918474F, - 0.00397238508F, 0.00267518F, 0.000147144354F, 0.000609222509F, - -0.00182991824F, -0.000637615274F, -0.00129858486F, 0.00240410771F, - -0.0026369479F, -8.31160578E-5F, 0.00164631254F, -0.00398578541F, - 0.000799869827F, -0.0004897365F, -0.00265580602F, 0.0015915453F, - 0.00256833131F, 0.000688790809F, 0.000755966757F, 0.00216082833F, - -0.00361476722F, -0.00105826964F, -0.00180485367F, -0.000595269F, - -0.000502507959F, 8.3286046E-5F, -0.00129504281F, 0.00216497015F, - -0.000881702523F, 0.00130382541F, 0.000975180068F, -0.00384614128F, - 0.000607685826F, -0.00312161143F, -0.00157344609F, -0.0040228907F, - 0.000671029789F, 0.000866203743F, 0.00188312971F, 0.00430627773F, - 0.000720277545F, -0.00171486475F, 0.000998827512F, 0.00385108707F, - 0.00134926417F, 0.000161700926F, -0.000578138F, 0.000883577683F, - 0.00165666163F, -0.00210765796F, 0.00192782911F, -9.2758557E-5F, - 0.00071398553F, -0.000872507459F, 0.00104234822F, 0.00410345709F, - 0.00322344457F, 0.000348282134F, 0.00220384239F, -0.00126060308F, - 0.000567112118F, 0.000322142878F, 0.000193613363F, -0.00325366901F, - 0.00169483712F, 0.0023270105F, 0.00138400192F, 0.000521124F, 0.000450383901F, - -0.000411081448F, 0.00269186054F, -0.00256067677F, 0.00298928958F, - -0.00307330489F, -6.22432708E-5F, 0.00272642192F, -0.00284306728F, - 0.001649649F, 0.000483638665F, 0.00250558555F, -0.00144987216F, - 0.000141428187F, 0.00118350831F, -0.00283875526F, -0.00113977154F, - -0.000737433089F, -0.003376988F, -0.00271815597F, 0.000745020399F, - 0.000856862F, 0.000937103934F, 0.000870047777F, 0.00219006208F, - -0.00117750058F, 0.0019932366F, 0.00153961475F, -0.000575307116F, - -0.000438337622F, -0.00308871618F, -0.00312534766F, -3.19940591E-5F, - 0.00389358F, -0.00100763724F, 0.000690223416F, 0.0012720474F, - -0.000783076335F, 6.42696541E-5F, -0.000643156643F, 0.00238990691F, - 0.00212869933F, -0.00262077642F, -0.00212239078F, -0.000606352114F, - -0.000425537291F, 0.00143397273F, -3.07341652E-5F, 0.000240285968F, - 0.00159244309F, -1.16747315E-5F, -0.000339742284F, 0.00237721973F, - 0.00281125191F, -3.51488634E-5F, -0.00196348061F, -0.00174199988F, - 0.00278085331F, 0.0022532295F, -0.00211103423F, -0.00400268799F, - -0.000955263386F, -0.000906992413F, 0.00455502514F, -0.00183818699F, - 0.000771366293F, 0.00248528295F, -0.00384402508F, 0.00263972161F, - 0.00126474141F, -0.0033037134F, -0.00117994717F, 0.00366951944F, - -0.00148409337F, 0.0013037317F, -0.00239093788F, -0.00192276994F, - -0.00136955664F, -0.000177198948F, -0.00196489645F, 0.00175334F, - 1.77377788E-5F, 0.000843203685F, 0.000390296249F, 0.000588507624F, - -0.00102255808F, 0.00128744519F, -0.00012089128F, -0.00221717637F, - -0.00402375311F, -0.00296052662F, 0.00223723287F, -0.00131202419F, - -0.00105701212F, 0.00111546507F, -0.000843845482F, -0.000684737402F, - -0.00447962619F, -0.0012073929F, 0.00168296241F, -0.000746575592F, - -0.00123000494F, 0.000630034F, -0.000291467906F, 0.00196013669F, - 0.00248707877F, 0.0010073774F, 0.000188243808F, 0.000487016077F, - -0.00129215617F, 0.000670704234F, 0.000322322274F, 0.00138126465F, - -0.000481060182F, 0.000348310685F, 0.00109727692F, 0.00167887588F, - -3.46178931E-5F, -1.8446417E-5F, 0.00416188408F, -0.00285966019F, - 0.000166653859F, -0.00029941008F, 0.000572165416F, 0.00306236255F, - -0.00177570782F, -0.000988920452F, -0.000191691899F, -0.00191001827F, - -0.000282810477F, -0.00160295807F, 0.0040378808F, -0.00205085031F, - 0.000366845867F, -0.00284912623F, -0.00345480349F, 0.000687441614F, - 0.00323387887F, 0.00171909365F, -0.00260388129F, -0.00111013721F, - 5.72955742E-6F, -0.00291628F, 0.00172333559F, 0.000288504758F, - -0.000309532159F, 0.00312945689F, -0.000252453174F, 0.000633774791F, - -0.000244609459F, 0.00280439784F, -0.00067636196F, -0.00344592216F, - -0.00571938744F, 0.000971549831F, 0.00107826106F, -5.35592371E-5F, - -0.000467537349F, -0.00375017966F, 0.000848506927F, 0.000292370649F, - -6.11034557E-5F, -0.00217997632F, 0.00050848542F, 0.0033742066F, - 0.00131364737F, 0.002672954F, 0.00127684523F, -0.000879881438F, - -0.00259484421F, -0.00285692606F, -0.000546892406F, -0.00239554979F, - 0.000262676243F, 0.00102426764F, 0.00124230993F, 0.000630047813F, - 0.0021386731F, -0.000472600688F, -0.000296187442F, 6.1646635E-5F, - 0.000952073198F, 0.000652458111F, -0.00359677384F, -0.00107863848F, - 0.00214183168F, 0.00196623965F, 0.00135889451F, -0.00394637464F, - -0.00125410024F, -0.00064110267F, 0.00166784856F, -0.0017133269F, - 0.000427558378F, 0.000708043168F, 0.00327966572F, -0.00127302F, - 0.00215516821F, -0.000219171023F, -0.00040409551F, 0.00215777545F, - -0.00204443233F, -0.00249456428F, 0.000145840066F, -0.000789815793F, - -0.0032138112F, -0.00222720439F, 0.00237367069F, -0.00149091857F, - -0.00246009254F, -0.00119675521F, -0.00327060511F, -0.00129012426F, - 0.000614713412F, -0.00361177209F, -0.000366160035F, -0.00114946603F, - -0.000427443592F, -0.00281407894F, -0.000307528622F, -0.00131104351F, - 0.00113817176F, -0.000820526446F, -0.00131493318F, -0.000971521076F, - -0.00169537542F, 0.00226779841F, -0.00102815323F, 0.000193683387F, - -0.00148501282F, 0.00121731451F, -0.000375827716F, 0.000128481042F, - -0.00103686017F, -0.00153895549F, -0.00202626619F, 0.00242769415F, - 0.00463228F, -0.00404667F, -0.000121521858F, -0.000554959348F, - 0.00145204691F, 0.0007115021F, -1.13566357E-5F, -0.000965493848F, - 0.000336020224F, 0.00135404477F, -0.000631209288F, 6.98381518E-6F, - 0.000424262427F, 0.00122065772F, 0.00142087473F, -1.77904658E-5F, - -0.002449936F, 0.0014466726F, -0.00133954303F, -0.00127420505F, - -0.000246747368F, -0.00194627733F, 0.000254213897F, -0.00255810632F, - -0.000577436876F, -0.00310756336F, -0.00114977721F, -5.13282721E-5F, - -0.00166426878F, 0.00122128252F, -0.000814127212F, 0.00126099621F, - 0.000166754573F, -0.000955186319F, -0.00169795542F, 0.00044165054F, - -0.00102396554F, -0.000214400759F, -0.00115450122F, 0.000556542422F, - -9.09300361E-5F, 0.00107944722F, -0.00120788603F, -0.00467877695F, - -0.000762704411F, 0.00151682459F, -0.000692319649F, 0.00300183659F, - -0.00387836061F, -0.00232456578F, 0.000575984304F, 0.00303210644F, - -0.00405799504F, -0.000503436604F, -0.000161551041F, 0.000235370273F, - 0.00258786604F, -0.000380945916F, 0.00273484364F, -0.00500345649F, - -0.00178588647F, -0.00117046F, -0.0036811817F, 0.000325955421F, - 0.00161320309F, 0.00233417214F, -0.00231923652F, 0.00246409141F, - 0.000943755324F, 0.00153316453F, -0.00312438677F, 0.000157060087F, - -0.00308309915F, 0.00125349F, 0.00178382313F, 0.0023478684F, -0.00095643138F, - 0.000579047541F, -0.000439240597F, -0.000499712653F, -0.00161031575F, - 0.000406200939F, -0.00262536411F, -0.00105552142F, -0.00148407696F, - -0.00223349035F, -6.70541E-5F, -0.00255638873F, 0.000537754968F, - 0.00131197914F, -0.00140305853F, 0.00262880931F, -0.000878710591F, - -0.00121827039F, 0.00126098294F, -0.00149047619F, -0.000247792021F, - 0.00020068718F, -0.00221006246F, 0.00151330582F, -7.33599154E-5F, - 0.000772930682F, 0.00225936645F, -0.00295352889F, 0.00302776438F, - -0.00282665808F, 0.000397446332F, 0.000501887465F, -0.00265198108F, - 0.00103145698F, 0.00067239115F, -0.00315004075F, 4.73339387E-5F, - -0.000540914596F, -3.1968113E-5F, -0.00464563817F, 0.000994621543F, - -0.000589493662F, -0.00284133083F, -0.000218110159F, -0.000581561937F, - 0.00164830766F, -0.00110024272F, 0.00125265296F, 0.00189338543F, - -0.000491337676F, -0.0015181622F, 0.000835145649F, 0.0043467083F, - -0.00318468059F, -0.0007747169F, 7.13350528E-5F, -0.00182841741F, - 0.000536269F, -0.00256112916F, 0.00333069847F, -0.00456443196F, - -0.00314339972F, 0.00096040935F, -0.00024485981F, -0.00159451365F, - 0.000427357067F, -0.00199820846F, 0.00157868094F, -0.00102303654F, - -0.000247871503F, 0.00149510952F, -0.000522879709F, 0.00324458582F, - -0.000249351055F, -0.00327195367F, 0.000153833F, 0.00106382172F, - 0.00315474602F, 0.000523711496F, -0.00374024827F, 0.00365701271F, - 0.00176584662F, 0.00298054796F, 0.00270194653F, 0.00204167981F, - -0.0010107297F, 0.00149206992F, 0.00149485574F, 0.0022029879F, - 0.00397337973F, 0.00257851812F, -0.000179101553F, -0.00109632604F, - 0.00119660061F, -0.000374256F, 0.0005565295F, 0.00117878465F, 0.00384452124F, - -0.00210581953F, -0.00166165212F, -0.00130307151F, 0.00240945606F, - 0.000172586486F, -0.00210298621F, 0.00155016966F, 0.00132879347F, - -0.000425160106F, -0.000153792964F, -0.00200647814F, 0.00137256936F, - 0.0020896194F, 0.00119539245F, 0.00158734643F, 0.000355919678F, - -0.00174824067F, -0.00112674956F, -0.000100576763F, -0.000724083628F, - 0.00300760427F, 0.000901636202F, -0.00156897656F, 0.00268104509F, - 0.00104238151F, 0.00112239807F, -0.000187150697F, 5.41772861E-5F, - 0.000369909F, 0.000271297089F, 0.00040806926F, -4.14499045E-5F, - 0.00140759593F, 0.00413016789F, -0.000363104F, 0.000144886479F, - 0.00201032893F, -0.000117072101F, -0.00213763583F, -0.00105041068F, - 0.00163482F, -0.00201596762F, -0.00302701187F, -0.000218728688F, - 0.00101227534F, -0.000622003572F, -0.000869442418F, 0.000469739491F, - -0.000844958413F, 0.00158735237F, -6.94294431E-5F, -4.23248312E-5F, - 0.0012094971F, 0.000101341255F, 0.0018002087F, -0.00195986964F, - 0.00244292873F, -0.0017946749F, 0.00194583961F, -0.000884334848F, - -0.00719034672F, -0.00173281867F, -0.000639825943F, -0.0011903086F, - 0.00149707019F, 0.000957352866F, -0.00114880595F, -0.00224805088F, - 0.00356944534F, -0.00110553158F, -0.00131537009F, -0.000460744195F, - 0.000231687867F, 0.00210575F, -0.000488225662F, 0.000387508509F, - 1.2190385E-5F, -5.08697412E-5F, 0.00113350886F, 0.000813032791F, - 0.00218686531F, -0.00113612844F, -0.00158120447F, -0.00220090733F, - -0.000109089822F, -0.000313911849F, 0.00133132352F, -0.00517163938F, - -0.000742386386F, -0.00112734F, -0.00278627314F, -0.00306675863F, - -0.00183066702F, 0.000459913223F, -0.00222590356F, 0.00202419655F, - 0.00106096012F, -0.00174907292F, -0.00123176444F, 0.00434078695F, - -0.0003058539F, 0.00185439258F, 0.00249961391F, -0.00155827974F, - -0.003239549F, -0.00060010131F, -0.000380556332F, -0.000970562571F, - -0.00128187658F, -0.00402787374F, -0.00170229934F, 5.47486961E-5F, - -0.00223631086F, -0.000963421073F, -0.000490802748F, -0.00171525346F, - -0.0004996403F, 0.00303290482F, 0.0021050924F, 0.000953995332F, - -0.000996158924F, -0.00113859086F, -0.00100600044F, 0.00353315054F, - 0.000823553244F, -0.0032934146F, -0.00115248282F, -0.00166712794F, - -0.000399265788F, 0.000839718617F, 0.000980242854F, -0.00109364407F, - -8.07584729E-5F, -0.00119908981F, -0.00208808552F, -0.00111716182F, - -0.00207484677F, 0.00213866495F, -0.00166369684F, 0.00116161513F, - -0.000310460804F, -0.000633308489F, 9.61059268E-5F, -0.00293313293F, - -0.000891643285F, 0.000245976931F, -0.000171942011F, 0.000103492988F, - 0.00167836517F, 0.00160890922F, -0.000213202075F, 0.00304035074F, - -0.00117313163F, 0.000186589357F, 0.000311178126F, -0.00178274664F, - -0.00303236116F, -0.000261821464F, -0.000847361924F, -0.000200484923F, - -0.000756069261F, 0.00162983174F, -0.00122204167F, 0.000424905622F, - -0.000718644238F, -0.00121036125F, -0.00113980868F, 0.000669112313F, - 0.00511188712F, -0.000723928621F, 0.000207797886F, 0.00119151408F, - -0.000596777943F, 0.00200995849F, -1.55181933E-5F, 0.000571984F, - -0.00424576132F, 0.00142241584F, -0.00119849865F, -0.00129922712F, - -0.000570883567F, 0.00107308803F, -0.0015812791F, 0.00168153609F, - -0.00146982F, -0.000408729131F, 0.00124959205F, -7.94729713E-5F, - 0.00128421013F, 0.00282091205F, -0.00195596321F, 0.000575332553F, - -0.00164615782F, -0.0013094286F, -0.000555545441F, -0.000935854216F, - 0.00102322048F, 0.00400467776F, 0.00105000788F, -6.54894757E-5F, - 0.000296806043F, 0.00126931991F, -0.000222641436F, -0.00053623144F, - 0.00164103427F, -0.00126991177F, -0.000464800949F, 0.00252912194F, - 0.00219826307F, -0.00125346519F, 0.00425176555F, 0.000186033125F, - -0.00106354477F, -0.000194910768F, 0.00239356887F, -0.000774091051F, - 1.90378778E-5F, 0.00174709316F, -0.000524087402F, -0.000539368135F, - 0.00127121923F, -0.00256536971F, -0.00300619681F, 0.00485853944F, - 0.00476018805F, -0.00318799564F, -0.00327901589F, -0.00184506446F, - 0.00105026527F, 0.00146812352F, -0.00280184252F, 0.00138122868F, - -0.000214791798F, -0.00189406343F, 0.00107957411F, -0.0013059692F, - 0.00157458289F, -0.00176348072F, 0.000853827514F, -0.000292086916F, - 0.00174034329F, 0.000681588F, 0.00258997013F, 0.000639888458F, - 0.00310715032F, -0.00204018527F, 0.0017437035F, -0.000890933326F, - 0.000164477242F, 0.00291376305F, 0.00195419346F, -0.00209439755F, - 0.000522091F, -0.000747460581F, 0.00103200134F, 0.00148289429F, - -0.00267008622F, 0.00262268609F, 0.000280322216F, 0.000726041035F, - -0.00109723548F, -0.00249252119F, 0.00320049748F, 0.000306878093F, - -0.000403596845F, 0.00096040318F, 0.00121779973F, -0.00206839456F, - -0.00126638054F, 0.00181000878F, -7.8246223E-5F, -0.000758931F, - -0.000950284535F, -0.00285197748F, -0.002935305F, -0.000998651492F, - -0.000663730956F, -0.000548717915F, 0.000592263823F, -0.00134827511F, - -0.00246171397F, 0.00190225639F, 0.0020067F, -0.00178461894F, 0.00127793918F, - 0.0027516596F, 0.00235333946F, 0.000561153F, 0.00339724985F, -0.00108655309F, - -0.00164244312F, -0.00495933555F, 0.000578587584F, -0.00324104889F, - 0.000246294338F, -0.000997781521F, -0.00104035402F, -0.00284696789F, - 0.00230306503F, -0.000264418952F, -0.000901508203F, 0.00240674545F, - 0.000486675155F, -0.00297996565F, -0.000412759837F, 0.000473628228F, - -0.00273564062F, -0.00280857808F, -0.00286112865F, 0.000893179211F, - 0.000343655964F, 0.00128536928F, -0.00215059891F, 0.0020874152F, - -0.00177231047F, 0.000470046885F, -0.00394197367F, -0.000617208192F, - 0.000433714187F, 0.000848703843F, 0.00126782944F, 0.00100249867F, - -0.000843023474F, 0.00147484441F, -0.0023065235F, -0.00126061519F, - 0.00245320844F, -0.00103865471F, -0.00249424344F, -0.002065012F, - 0.00201685401F, -0.00376645336F, -0.00103838928F, 0.00170390424F, - 5.4417651E-5F, -0.00150587375F, -0.000714751543F, 0.00138331938F, - -0.00126793934F, 0.000124043392F, -0.000775590946F, -0.00195451453F, - -0.00206060521F, -0.00329995016F, 0.00107438583F, 0.000776480127F, - 0.000978815253F, -0.000504315482F, -0.002498477F, -0.00153934374F, - -0.000411667046F, -0.000734218629F, -8.26846808E-5F, 0.00270512048F, - -0.000308029237F, -0.00157212152F, -0.00146798696F, -0.000302581931F, - 0.00286018685F, -0.00129494176F, 0.00327732111F, -0.00100453733F, - -0.00364612159F, 0.000139516298F, -0.00204216945F, 0.000326620851F, - 0.00241752411F, -0.000143068071F, -0.00155628566F, 0.000811950595F, - 0.000496053777F, -0.000830419303F, -0.000193948523F, -0.00256036525F, - -0.00102946069F, -0.00266362284F, 0.00041439003F, -0.00107800204F, - -0.00169298693F, 0.00144985563F, 0.000412732654F, 0.000151415719F, - -0.00241996185F, -0.000692441245F, -2.3018581E-6F, 0.000926142675F, - 0.00156924105F, -0.00341983535F, 0.00430143299F, 0.000698072487F, - 0.000977744465F, 0.000109727953F, 0.000764339F, 0.00248117768F, - 0.00378282648F, -0.00366650941F, 0.000676618307F, 0.00353932125F, - 0.000428074796F, 3.0928255E-5F, 0.00056890439F, -0.000827403215F, - -0.000652321F, 0.00187021785F, 0.000537088665F, 0.00116473716F, - 0.000605402747F, 0.00125371967F, -0.00158627483F, 0.00293969968F, - -0.00047283934F, -0.000355704367F, 0.00217114529F, 0.00234451215F, - -0.0011195672F, 0.00124444079F, -0.00206545275F, -0.00178430509F, - -0.00153281935F, 0.00332712196F, 0.00249150442F, 0.00220800773F, - 0.00172503712F, -0.00118803361F, 0.000207681092F, 0.000560198212F, - 0.00305382162F, 0.000424958678F, 0.00125927047F, 0.00347123947F, - 4.52369131E-5F, 0.000697870855F, 0.000605369103F, -0.000955848955F, - -0.000108253756F, 0.00170485443F, -0.00372069818F, 0.00069050194F, - 0.00211317837F, 0.000198232898F, 0.00066481973F, -0.000514729065F, - -0.000618324149F, -0.00205215532F, 0.00265391567F, -0.00142137066F, - 0.000613796175F, 0.000981071615F, -0.00332808727F, 0.000317620521F, - -0.00312106125F, -0.000568338F, -0.00115473242F, -0.00192275736F, - -0.00139598583F, 0.00316576753F, 0.00112486584F, -0.00231503136F, - 0.00452579046F, -0.000480996649F, 0.000302709464F, -0.00120882667F, - 0.00206221524F, 0.000831703306F, -0.00216366351F, -0.00280241692F, - 0.000963417173F, -0.000778849761F, -0.0030704767F, 0.000743372482F, - -0.000680352619F, -0.00143613294F, -0.00103902689F, 0.00118496106F, - -0.000474458939F, -0.00115897076F, 0.000969368324F, -0.00188037904F, - 0.00204911199F, -0.000324143941F, -0.000612122414F, -0.000147937651F, - -0.00160545274F, -0.00121033459F, -0.000910806586F, 0.00117693399F, - 0.00176275F, 0.00109723781F, -0.00207895762F, -0.00111424224F, - 0.00142752286F, 0.00234593241F, -0.000302280125F, 0.00134498579F, - 0.0034195534F, 0.000900380546F, -0.00250737532F, 0.00100886857F, - 0.00148966862F, -0.00145642203F, -0.00101158523F, 0.000698326912F, - 8.71802622E-5F, -0.00136865163F, -0.000563596783F, -0.0011104088F, - 8.92698808E-5F, -0.000528207282F, 0.000430209679F, -0.000163274919F, - -0.00148829608F, 0.00461518485F, -0.00187394058F, 0.00330216065F, - -0.000518563669F, 0.00277776876F, 0.00294781569F, 0.00027006265F, - 3.58226171E-5F, 0.00356146879F, 0.000960138F, -0.0017023785F, - -0.00233702804F, 0.00105467963F, 0.0008050201F, 0.00064277224F, - 0.000543087139F, 0.00112495839F, 0.00139566488F, 0.00113063573F, - 0.00152598554F, 0.00117812969F, -0.000674550771F, 0.000579250685F, - -0.000174743822F, 0.00329119759F, 0.000647781475F, 0.00154129008F, - 0.00201320462F, 1.62822525E-5F, 0.000819868525F, 0.00316976709F, - 0.00389671139F, -0.000914786651F, -0.00294409646F, 0.0024561726F, - 0.000645924F, -0.000161854681F, 0.000354217016F, 0.00208890461F, - -0.00302336481F, -0.00191037753F, 0.000181488038F, 0.00308354874F, - 0.00122532342F, 0.000853466685F, 0.00146595016F, -0.00101697317F, - 0.0042019072F, -0.00423618592F, 0.00205874979F, -0.00214696955F, - -0.000496074324F, -0.00427375594F, -0.00050162F, 0.000655468204F, - -0.0019813152F, -0.00273770094F, 0.00125717511F, 0.000413022586F, - -0.00447874796F, 1.44393207E-5F, 0.000129194959F, -0.00160090439F, - 0.00228055473F, -0.00338040804F, 0.000548146723F, -0.000557236141F, - -0.000184831428F, 0.0014245807F, 0.00236013276F, -0.00240071723F, - -0.00285507739F, 0.000480020943F, -0.00142010278F, 0.000675750256F, - -0.00321643706F, -0.0016335533F, 0.00222742418F, -0.00268675317F, - -0.00167002971F, -0.0019167664F, -0.00118743803F, 0.00118175847F, - -0.00080857164F, -0.00113123376F, -0.000271141529F, 0.000138257892F, - -8.79821237E-5F, 0.00230953959F, -0.00219648727F, -0.00115665176F, - 0.00444232F, 0.00287375716F, -0.000644125394F, -0.000122893165F, - -0.00227731117F, -0.00131747243F, -0.00173622498F, 0.00177453028F, - -0.000134331771F, -0.0018224651F, 0.000360990729F, 0.000680855475F, - 0.00119021675F, -0.00208857399F, -0.00432155468F, -0.000842754671F, - -0.00353576499F, -0.000102428625F, -0.00399916852F, -0.00187129F, - -0.000381111982F, 9.55877331E-5F, -1.96498149E-5F, -0.000247237796F, - 0.001127603F, -0.000694611808F, 0.000218527901F, 0.00377274514F, - 0.00168233656F, 0.000784562377F, -0.00112369738F, 0.00240064715F, - 0.00137051055F, -0.000728784536F, -0.000535083353F, 0.000256950967F, - -0.00142881204F, 0.00158115081F, 0.00042818996F, 0.00151318533F, - 0.00181130681F, 0.00354584F, -0.00304056425F, 0.00140231778F, - -0.00244277902F, -0.000364083564F, 0.00164482265F, 0.000986746862F, - -0.000703572528F, -0.00366664212F, -0.00172180973F, -0.00395098282F, - 0.00174546079F, -0.000179735245F, -0.00397134293F, 0.0017800458F, - 0.00351504819F, 0.000943804043F, 0.0030804642F, -0.000181275827F, - -9.58610763E-5F, -0.000589216535F, -0.000714699621F, 0.000561787514F, - -0.00199054601F, -0.00127913919F, 0.00327138F, 0.00342406798F, - -0.00183572248F, 0.00134042988F, -0.00157585961F, -0.000207799312F, - 0.000626505702F, -0.000144727426F, -0.00632766727F, 0.00395986624F, - -0.000356260047F, 0.00508024497F, -4.62304342E-5F, -0.00204307167F, - -0.00126098865F, -0.00100512675F, -0.00180928071F, 0.00025049629F, - 0.0013682706F, -0.00114167179F, -0.000236944543F, -0.00216451962F, - -0.00371561153F, 0.00132023613F, 0.00092246104F, -0.00132885168F, - 0.00330070662F, -0.0015746312F, -0.00258016144F, 0.00342204794F, - -0.00151304109F, -0.00247529917F, -0.0021856036F, 0.00512804417F, - 0.000239396992F, 0.0017574135F, -0.00133667141F, 0.00558555918F, - 0.00201072567F, 0.000571527751F, 0.00131461374F, 0.00244161743F, - -0.000520491099F, 0.000306375208F, 0.00134371768F, -0.000178284259F, - -0.00184837822F, 0.00306819123F, 0.000602618966F, 0.00092898088F, - 0.000338286278F, 0.000435376103F, -0.00363792107F, 0.00367792975F, - -0.00142833334F, 0.000961063255F, 0.00228510029F, 0.000105509673F, - 0.000127330539F, -0.0030318296F, -0.00113170256F, 0.00152922748F, - 0.000689037202F, -0.000282059802F, 0.000111724417F, 0.0036127232F, - -0.00473053567F, 0.00263110409F, 0.00429400569F, -0.000401526748F, - -0.000756164431F, 0.00106574397F, -0.000393993832F, 0.00234918948F, - -0.00118517852F, -0.000465003715F, 0.00148528919F, -0.001007371F, - -0.00137551955F, 0.00233360543F, -0.000950620859F, 0.00101392844F, - -0.00172566122F, -0.00145957328F, 0.000171247375F, -0.00012329196F, - 0.000127650579F, 0.00200570771F, -0.00158528797F, 0.000410901557F, - 0.00098675664F, 0.00203031837F, 0.00155889941F, 0.000650176487F, - 0.00350306416F, 0.00259269541F, 0.000705546F, 0.000791146711F, - 0.00131868653F, -0.00200776639F, -0.00176472927F, 0.00361068547F, - 0.00281503727F, -0.00221490674F, -0.00220054411F, -0.000598061655F, - 0.00256585865F, -0.00274910103F, 0.00175934611F, -0.000159659583F, - -0.00127313274F, -0.000363080966F, 0.00151479291F, 0.00262006F, - -0.000564602553F, 0.000762910582F, 0.000815950043F, -0.00330986804F, - 0.00312324916F, -0.00278523215F, 0.000897521037F, 0.000556657556F, - 0.001439563F, -0.00175383897F, -0.000362510211F, 0.00116587267F, - 0.000420842F, -0.00217298791F, -0.000462330849F, -0.00367233297F, - 0.0010109843F, 0.00185580051F, -0.000993660302F, 0.00188741169F, - 0.00121858739F, 0.000122616184F, 0.00198084465F, 0.00164912944F, - -0.00241277483F, 0.00119043991F, -0.00194936176F, 0.000534609891F, - 0.000602406042F, -0.00331429252F, -0.000143558282F, 0.00109007617F, - 0.00317275035F, -0.00237400038F, -0.00273278356F, -0.000603151799F, - 0.00102634763F, -0.00178070634F, -0.000141727345F, 0.000357659126F, - 0.0033576556F, 0.000665740576F, -0.000523344846F, -0.000209181133F, - 0.00185719132F, 0.00192413328F, -0.00185033854F, 0.00222942419F, - 0.00143398158F, -0.00159951195F, 0.000906006899F, -0.00134499779F, - 0.00122561213F, 0.00109053438F, 0.000122438927F, -0.000544297F, - 0.00279373978F, -0.000191814281F, 0.00290060742F, 0.000576580758F, - 0.00222435873F, 0.000899345789F, -0.0021249603F, 0.00148937327F, - 0.00297916401F, -0.00186813832F, 0.00471616536F, -1.07840924E-5F, - -0.00119252247F, 0.00230019027F, -0.00268376176F, 0.00166148483F, - 0.00335544255F, -0.00212998176F, 0.00352738705F, 0.00247247983F, - -0.0013901809F, -0.000849670905F, 0.00105268206F, 0.000118603246F, - 0.000671637885F, 0.000686066283F, 0.00279880245F, 0.00162430538F, - -3.09343523E-5F, -0.000768786122F, 0.00114053756F, -0.00087679975F, - -0.00249875546F, 0.000620122126F, -0.00144769973F, 0.00157773355F, - -0.000598308688F, 0.00290526124F, -0.00384526211F, 0.00227404316F, - 0.00209551421F, 0.00141424173F, -0.000159880437F, 0.00142581214F, - -0.00205234601F, -0.00160127878F, -0.00172891293F, -0.000423808582F, - -0.00118287199F, 0.00368615682F, 0.00241735275F, -0.000599624531F, - 0.0025737111F, -0.000254111248F, -0.00139960356F, -0.00259820232F, - -0.00126752595F, 0.000965255138F, -0.00129443523F, -0.000410910929F, - 0.000545561896F, -0.00144572312F, -0.00248097233F, 0.000545923947F, - -0.00279028062F, 0.00230851234F, -0.000379537494F, 0.00323302927F, - -0.000669961853F, -0.0003595374F, 0.00130858994F, 0.000824627583F, - 0.000566902221F, -0.00168909528F, 0.00125958689F, -0.00127658236F, - 0.000686468149F, -0.00173199712F, 0.00294547109F, 0.00548481662F, - 0.000529250305F, -0.000607300317F, 0.00018351202F, -0.00036668891F, - 0.000764219498F, -0.0026653097F, 0.0022591895F, 0.00209839665F, - -0.00368491979F, 0.00146782713F, 0.00123608566F, 0.000236862485F, - 0.00188542332F, 0.0013191374F, 0.00125313806F, -0.0003118983F, - -0.000946151267F, 0.000321453932F, 0.00203000382F, -0.00272310898F, - 3.37395E-5F, -0.00222043158F, 0.00211039162F, 0.000830642181F, - 0.000297366176F, -0.00100654725F, 9.32017138E-5F, 0.000542050519F, - -0.0030074527F, 0.00116318767F, -0.000880795F, -0.000687345164F, - 0.00224789837F, -0.00355522614F, 0.000267767464F, 0.000316148944F, - -0.00101369072F, 0.00013459794F, -0.00229923264F, 0.00136041851F, - 0.00220685219F, 0.000410012377F, -0.00131317426F, 0.000929337868F, - -0.000717419549F, 0.00204285048F, -0.0013799757F, -0.000504335039F, - -0.00238809292F, -0.00103596773F, -0.00221103872F, -0.00113002595F, - 0.00319685298F, -0.00394378742F, 0.00100196723F, -0.000339954F, - -0.00027945693F, 0.00219819974F, -0.00238727243F, 0.00165519048F, - -0.00224113371F, 0.00247445912F, -0.00531792734F, 0.000336265948F, - -0.000295781152F, 0.000553092512F, -0.000772802567F, -0.000163634904F, - 0.00393948471F, 0.00208875537F, 0.00350977527F, -0.00173656875F, - 0.00181691209F, 0.000753767905F, -0.00445104297F, -0.000145860686F, - -0.0025286912F, 0.00210715528F, 0.00170684F, 0.0010775819F, 0.00115957472F, - -0.001134819F, -0.00263869949F, 0.00169901748F, -0.00308663491F, - 0.00100177445F, -0.00120854203F, -0.00277244183F, 0.00224368973F, - -0.00294506038F, 0.00164275477F, 0.000565750059F, 0.000880454842F, - -0.00148730737F, -0.000161971475F, -0.000963855651F, 0.00145661633F, - -0.00122099661F, 0.00173592707F, -0.000365233223F, 0.00103017397F, - 0.00199387269F, 0.00151522225F, -0.00412288588F, -0.00166507461F, - -0.00344523019F, 0.00177574472F, 0.002004195F, 0.00196713139F, - 0.000479312439F, 0.000156877737F, 0.000955807394F, -0.00245073973F, - 0.000495857734F, -0.00084039988F, 0.0017540179F, -0.000982967F, - -0.00106344814F, 0.00347987213F, -0.00246885722F, -0.00174164202F, - -0.00103436783F, 0.000762860291F, -2.46532873E-5F, 0.00095282367F, - 0.00048950559F, 0.00172598823F, -0.000591506192F, 0.000330900541F, - 6.90209563E-5F, 0.000544056296F, 0.00191645918F, 0.000880738487F, - 0.000144804391F, 0.000180248084F, -0.00306130573F, 3.57687713E-5F, - -0.000531675352F, 0.00146593351F, -0.000578187872F, 0.00211810484F, - -0.000189398808F, 0.00136230991F, 0.00189717161F, -0.00173603266F, - 0.000262681919F, -2.29848538E-5F, 0.00174819375F, -0.000568812131F, - -0.00470705F, -0.00201954204F, 0.00128453353F, 0.0011141448F, - 0.000138282456F, -5.55636943E-5F, -0.000651173119F, 0.0012740352F, - -0.000655219366F, -0.000128448766F, -0.00215882459F, -0.00185483112F, - -0.00127235858F, -0.00251125637F, -0.00163667183F, 0.00108724204F, - -0.0016804589F, 0.00174022408F, -0.000283429486F, 0.000718339172F, - 9.14522461E-5F, 0.00289286533F, 0.00073458167F, -0.00131436891F, - 6.87837746E-5F, -0.000923828F, 0.00155327143F, -0.000718728639F, - 0.00297065265F, 0.000244208582F, 0.00189144036F, 0.00258371467F, - 3.69118752E-5F, 0.00115612F, 0.00183735904F, 0.00617454899F, 0.00342784077F, - -7.83862342E-5F, -0.000906776346F, -0.00257158652F, -0.00207869F, - -0.00203000708F, -0.00215919618F, -0.00187312032F, -0.000185763391F, - -0.00057909562F, -0.00040988109F, 0.000329606875F, -0.00254742196F, - 0.00142344332F, 0.00228859158F, -0.00181233312F, -0.00219102809F, - -0.000101249818F, 0.00115657062F, 0.000967377273F, -0.0017446141F, - 0.000486166246F, -0.000146089515F, -0.000570061093F, -0.00605714647F, - -0.00292870472F, 8.58459243E-5F, -0.00101307884F, 0.000913663185F, - -0.000855362392F, -0.00239539519F, -0.00238291523F, -0.000467712758F, - -0.00247509615F, -0.00120216957F, -0.0022534749F, 0.0003042873F, - -0.000711281202F, -0.00410634652F, 0.0003736004F, -0.00257506524F, - -0.000739551266F, -0.00199634675F, 0.00270843878F, 0.00113806082F, - 0.00268038153F, 0.00335604418F, -0.00112923898F, -0.00225939928F, - 0.000637107878F, 0.001756093F, -0.0015292F, -0.00144236279F, - -0.000842460198F, -0.000558966422F, -0.00107511692F, -0.0030953011F, - 0.00249707745F, -0.0017937097F, -0.00185879273F, 0.00300532184F, - -0.0010742594F, 0.00507508405F, -0.00199851F, -0.00075014279F, - 0.000726926548F, -0.00348221674F, 0.00206751074F, -0.00125424354F, - 0.00114115386F, -0.0015844038F, 0.00155466993F, 0.00111571013F, - -0.000135667462F, -0.00229890109F, -0.00124355964F, -0.00375960837F, - 0.000799541478F, -0.00306139F, -0.00234823627F, 0.00249018869F, - 0.00106957147F, 0.00343998894F, 0.000739360112F, 0.000110671281F, - 0.00082076306F, -0.00344887795F, 0.00266360072F, -0.00362300756F, - 0.00112971535F, -3.13675519E-5F, 0.000728170678F, 0.00189777615F, - -0.00152116944F, -0.00199494814F, -0.00104360236F, -0.00177436753F, - 0.0010071916F, 0.00101365498F, -0.00301828561F, 0.00170837913F, - -0.00235964847F, -0.0017365244F, 0.000744912773F, 0.00112162915F, - 0.00174488209F, -6.38933125E-5F, -0.00114822283F, -0.000129849781F, - -0.00113056623F, -0.00451624906F, 0.00195525936F, 0.00305947545F, - 0.000219428883F, -0.000530924357F, -0.00236013718F, 0.00154756045F, - 0.000687578751F, 0.0010109914F, 0.00197922F, -0.00232762028F, - -0.00403158646F, 0.0011731165F, 0.00142484822F, 0.00272495695F, - -0.00264063198F, -0.00162505708F, 0.000742687902F, 0.0022554188F, - 0.000223658048F, -0.000729669759F, 0.00373613858F, 0.000775037683F, - -0.00316104316F, -0.000611153489F, 0.00173707807F, 0.000722919765F, - 0.0011542493F, -0.00276463409F, 0.000200928887F, -6.07859911E-5F, - 0.0017008367F, 0.00229731598F, -0.00119461922F, -0.00394124864F, - 0.00344398082F, -0.000800373964F, 0.0017744191F, 0.00163926405F, - 0.000289879245F, -0.000388296699F, -0.00156766758F, 6.41268198E-5F, - 0.00186100276F, -0.00244326238F, 0.00265689171F, -0.00045460448F, - 0.00250489474F, -0.00377918105F, -0.00151207589F, 0.00127606594F, - 0.00129686692F, 0.000651457172F, 0.000126566942F, -0.00284886546F, - -0.000264054921F, -0.00130103866F, -0.00211146707F, -0.000183011143F, - -0.000734780915F, 0.00130236312F, 0.00063733384F, 0.000497042434F, - 0.00189962157F, 0.00142581342F, -0.00283210189F, 0.00116684753F, - -0.000555928913F, -0.000692870351F, 0.000356802513F, -0.000901617226F, - 0.000738139264F, 0.000381117134F, -0.000920497754F, -0.0026371642F, - 0.000335050165F, 0.00157951587F, -0.00217848923F, -0.000252821163F, - -0.00330551108F, 0.000672649068F, 0.000342580781F, 3.00704028E-6F, - -0.000274867547F, 0.000154097404F, 0.00235333759F, 0.000147640894F, - -0.00144790753F, -0.00196082518F, -0.000859669177F, -0.000416414463F, - -0.000850988727F, -0.00175946206F, -0.00208339631F, -0.00371236261F, - -0.000442219229F, 0.000484030432F, 0.0020406486F, -0.00364858122F, - 0.0010414978F, -0.000332715601F, 0.00182016694F, -0.00206504203F, - -0.000452020089F, 0.00311963819F, -0.00187616493F, 7.51127809E-5F, - -0.000967573316F, 0.00247312174F, -0.00116428442F, 4.41726806E-5F, - 0.00385784195F, -0.00355318864F, -0.00435625529F, 0.000526590273F, - 0.00262213708F, -0.00386964041F, -0.000166595317F, -0.00362738478F, - 0.00275024096F, 0.00204900093F, 0.00014630612F, 0.0022549436F, - -0.00317772082F, -0.00235687895F, 0.00128294528F, -0.00235964172F, - -0.000571771059F, -0.000792078383F, -0.000840677938F, 5.64173861E-5F, - 0.000130493689F, -0.000743993965F, -0.0012799605F, -0.00195445144F, - 0.000285324524F, 0.00521346228F, 0.00154918234F, 0.00125461596F, - 0.000141041746F, 0.000449241459F, 0.000776357076F, 0.00170360785F, - -0.0027346455F, -0.000175869514F, -0.00541390199F, 0.00024052494F, - -0.00301034143F, 0.000188860402F, -0.00306073343F, 0.00177995267F, - 0.00073945825F, -0.00230007037F, 0.00177641038F, -8.43815E-5F, - 0.00027517954F, 0.000343438238F, 0.000545987918F, -0.000344242464F, - -0.00217352808F, -0.00355915492F, -0.000998163F, -0.00356590794F, - 0.00386586157F, 0.000609311F, -0.00149146293F, -0.00238061766F, - 0.000223841722F, -0.00209484971F, 0.00142404612F, 0.0017309566F, - -0.000789293495F, 0.0030911956F, -0.000679417863F, 0.00160625647F, - -0.0019168657F, -0.00193644781F, 0.000364530337F, -0.00155499834F, - -0.000262878544F, -0.0041995435F, -0.000132583271F, -0.000950247864F, - -0.000295876205F, 0.000348078873F, -0.0043044663F, 0.001289921F, - -0.000821455033F, 0.000782269577F, 0.000107419743F, 0.000766241632F, - -0.000995828421F, 0.00315283099F, -0.00190035743F, -0.00409242883F, - 0.00267581828F, 0.00053196511F, -0.000256216532F, 0.000477746711F, - 0.000660527206F, 0.000301302061F, -0.0024099187F, 0.000246476935F, - 0.000352970237F, 1.37205252E-5F, 0.00183130719F, -0.000506127079F, - 0.0011592923F, -0.00380220497F, -0.000850078126F, 0.00102244131F, - 0.00113676803F, -2.67163232E-5F, -0.000915205048F, -0.0034873262F, - 0.00335638388F, -7.05857519E-6F, 0.00101885782F, -0.000791642291F, - -0.000833704195F, 0.000927011773F, -0.00159751682F, 0.000536308624F, - -0.00200522249F, 0.00135118759F, -0.00216062646F, 7.79402326E-5F, - 0.000387367676F, -0.00242776284F, 0.00178976485F, 0.000638631929F, - 0.000608966395F, 0.00071643322F, 0.000937861565F, -0.00386443781F, - -0.00112144276F, -8.75655605E-5F, -0.00347925047F, -0.00169901655F, - -0.00218723249F, 0.000932082476F, -0.00250370684F, 0.00183343317F, - -0.00130627397F, -0.00155440636F, 0.00152723736F, -0.000273328478F, - 0.000434781774F, -0.000895236153F, 0.00594395F, -0.000375356467F, - -0.00116661342F, 0.000580882654F, -0.000853202364F, 0.00154882879F, - -0.00100820104F, 0.000306938513F, 0.000130701461F, -0.00372866704F, - -0.00143443141F, -0.0018628313F, 0.00347551494F, -0.000135543261F, - -0.000373121846F, 0.00199607597F, -0.000533653947F, 0.000655755226F, - 0.00066478888F, -0.00265195128F, 0.000217134177F, 6.32690426E-5F, - -0.00197805976F, -0.000899104634F, 0.00322748511F, 0.00181535864F, - 8.19589186E-5F, 0.00166394794F, -0.00194337103F, -0.000202591F, - 0.00297110807F, 0.00370155135F, 0.00111722341F, 0.00179754F, 0.000156915601F, - -0.00050699967F, 0.00210595597F, 0.00108950061F, -0.00161308853F, - -0.000594461337F, -0.00104083109F, 0.000211590319F, 0.00336227496F, - -0.00405111443F, 0.000449339946F, 0.00101553F, 0.000568379066F, - 0.00225370401F, -0.00186788943F, -0.00306713022F, -0.00102268881F, - -0.00184624491F, 0.000467151724F, 0.00231843279F, 0.000532816048F, - 0.00437707035F, 0.000782548275F, 0.00090207078F, 0.00256444607F, - 0.000702142541F, 0.00411956431F, 0.00049299642F, -6.76657291E-5F, - -0.00104665617F, -0.00131721725F, 0.00119896105F, 0.00195216853F, - 0.0031076055F, -0.00120066921F, -0.00139862148F, -0.00252941437F, - 0.00124025426F, 0.00147761137F, -7.86536184E-5F, -0.00146251009F, - 0.0010031776F, -0.00306723546F, 0.00170595781F, 0.00266534463F, - -0.00104016985F, -0.0048800637F, 0.00214447035F, -0.00158884295F, - -0.00336088496F, 0.00142640865F, -0.00175294222F, -0.0012622776F, - -0.00222328631F, 0.00248311693F, -0.00335176568F, -0.000412309513F, - 0.00251890183F, -0.000111130961F, 0.000636835466F, 0.00184213568F, - 0.00141462986F, 0.00338520063F, -0.000762214186F, -0.000825633411F, - -0.00141069537F, 0.00430610962F, -0.00261306902F, 0.000116506424F, - -0.000343773223F, -0.00239615957F, -0.00353232538F, -0.000657459663F, - -0.00244562188F, 0.000153526868F, 6.55401382E-5F, -0.00183416542F, - -0.00372732431F, 0.00190136919F, 0.00193064532F, 0.000522008F, - 0.00169166259F, 0.00403192919F, 0.00126954331F, 0.00153735839F, - 0.000628518115F, 0.00377573073F, -0.00460343435F, 0.000611359312F, - 1.02469749E-5F, 0.00136385346F, -0.00158499659F, 0.00343212788F, - -0.000245682138F, -0.00318449782F, 0.00248389342F, -0.0029040433F, - -0.000727084815F, 0.00337849231F, 0.000402872975F, 0.00124085986F, - -0.0012292735F, 0.000842419453F, -0.000151589222F, -0.00242045149F, - 0.000181487107F, -0.00135217502F, 0.00161088258F, -0.00297954958F, - 0.000738839677F, -0.000467502512F, 0.00117231417F, -0.000365078216F, - 0.00164747448F, -8.95349E-5F, 0.00249063456F, -0.00169077131F, - 0.00050622446F, 0.00473503815F, -0.00172495749F, 0.000815522159F, - -0.000662177452F, 0.00167377724F, -0.000107388201F, -0.00232741027F, - 0.000282886496F, 0.000495907152F, 0.00219624536F, 0.00130827201F, - 0.0021643003F, -0.00307762553F, 0.00187843537F, -0.00107035204F, - -0.00168782938F, 0.00189229241F, 0.00079907052F, -0.00116845861F, - -0.00107370038F, 0.00129099679F, -0.000124585902F, 0.00173890044F, - 0.001872494F, -0.00190924562F, 0.00119595707F, 0.00153748156F, - 0.000988585642F, 0.000308397255F, 5.34590472E-5F, -0.00016592152F, - -0.000394889328F, -0.00238518766F, -0.00092676637F, -0.00207917416F, - 0.00101141F, 0.00314754318F, -0.00017980962F, 0.00178769731F, - -0.00150295533F, -0.000678932527F, 0.00278861029F, 0.000669688F, 2.66056E-5F, - 0.00105982972F, 0.00360269914F, -0.00026295305F, -0.000944179425F, - 0.00596945174F, 0.000371149741F, 0.000724917685F, -0.00118494686F, - 0.00104171829F, 0.000332526222F, 0.00115901302F, -0.0014134713F, - -0.00225564046F, 0.00132031785F, -0.00277401973F, -0.000991579378F, - -0.00141329598F, -0.00120981294F, 0.000441125158F, 0.00225785305F, - 0.00555302203F, 0.00114016677F, 0.00184126699F, -0.000177133086F, - 0.00182174356F, -0.00141121645F, 0.0019166664F, 0.002120313F, - -0.00289539224F, -0.00251534395F, 0.000406179781F, -0.0018015001F, - -0.00224126899F, 0.00294853584F, 0.000611491F, 0.000868897187F, - 0.000321979809F, -0.00238669058F, 0.00199442962F, -0.00149735273F, - -0.0029232474F, -0.00158793631F, 0.00235227635F, -0.00160606881F, - -0.000726434635F, 0.000389778434F, -0.000109070083F, -0.00033677608F, - -0.00223899051F, 0.00177503144F, -0.000213897452F, -0.00376010709F, - -0.000128296422F, -0.000549324322F, 0.00106168538F, 0.00176004739F, - -0.000533911F, -0.000325582281F, -0.0028476154F, -0.00172990025F, - -0.00100479065F, -0.00167150726F, -0.000321882253F, 0.000236938038F, - 0.00354332523F, 0.00192015525F, 3.79845296E-5F, 0.000296251732F, - -0.000564829679F, -0.00183285784F, -0.0027369028F, 3.97368931E-5F, - -0.00247999234F, 0.00224956917F, -0.0012958938F, -8.7484048E-5F, - -0.00112119713F, -0.0023105396F, -0.000318969978F, 9.93969297E-5F, - 0.00469980901F, 0.00144453521F, 0.000529963581F, -0.00221491326F, - 0.00013450133F, -0.000536033884F, -0.00120052963F, -1.81590913E-5F, - -0.000912892167F, 0.0020338078F, -0.00249176426F, -0.0010529476F, - 0.00137691083F, 0.00120447634F, 0.00475297682F, -0.000529994315F, - 0.000992454123F, -0.000656772288F, -0.00233804365F, 0.000461795804F, - 0.00393077126F, -0.00148316834F, -0.000613278477F, -0.00152994471F, - 0.00120891607F, -0.000177312F, -0.000812454906F, 0.00121080666F, - -0.00359084574F, 0.00104629854F, -0.00275772554F, -0.000430255284F, - -0.00338971498F, 0.00141321961F, 0.000292001467F, 0.00251697958F, - 0.00232259091F, -0.00252398034F, 0.00113886432F, -0.00136586465F, - 0.0033681225F, 0.0029492951F, -0.00190071505F, 0.000525819254F, - -0.00404383242F, 0.000494887703F, 0.00177936442F, 0.000261588895F, - 0.00205830601F, -0.00126973423F, -0.0031625207F, -0.000152736931F, - 0.00331630232F, 0.00111503503F, -0.00157349405F, -0.00148092187F, - -0.00102326542F, 0.00360968104F, 0.000966374879F, 0.000945868785F, - -0.00200671609F, 0.00137409335F, -0.000371796137F, -0.00257383753F, - 0.00150586187F, -0.00186279323F, 0.00272563309F, -0.00289737131F, - 7.16228E-5F, -0.000948878645F, -0.0020343822F, -0.00222284719F, - 0.00024241193F, -0.00337287341F, -0.00109951384F, 0.00174483797F, - -0.000248621189F, 1.59327392E-5F, 0.00355314533F, 0.0018186802F, - -0.00165935943F, 0.000619484868F, 0.000983755F, -0.00059250294F, - 0.00074418413F, -0.00264226086F, 0.00321315718F, 0.000387403823F, - -0.00425403751F, -0.000214709726F, 0.000224109215F, -0.00213899906F, - -0.000350642571F, 0.000593114295F, 0.00307187135F, 0.000855772756F, - 0.000365370041F, -0.00287089916F, 0.00328931795F, -0.00161623466F, - -0.00201053172F, -0.000713293557F, -0.00166629907F, 0.000636272831F, - 0.00207489566F, 0.00349223334F, -0.000818794768F, 0.000312882417F, - -0.000743336626F, -3.47602267E-8F, -0.00140229031F, 0.00199775235F, - -0.00269894791F, -0.000161467804F, -0.0018183582F, -0.000860062253F, - 0.000510641432F, -0.0012199796F, 0.000490480161F, 0.00060411F, - -8.60365162E-6F, -0.00138815457F, -0.00301822089F, 0.000878553779F, - 0.000575592741F, -0.00178522232F, -9.3274808E-5F, -0.00120407145F, - 0.00188022666F, -0.000870006857F, 0.000127846157F, 0.00208833721F, - 0.000626473513F, -0.00123140914F, 0.00148499897F, 0.0014834766F, - 0.00292279711F, 0.00392189F, 0.0001946345F, 0.00141568342F, -0.00140565191F, - 0.0019082207F, -0.00047693323F, -0.000200733863F, 0.00108472852F, - -0.000175788431F, 0.00115590065F, -0.00117736857F, -0.00177493214F, - -0.000901921885F, 0.00242555281F, 0.00244574458F, -0.000899010396F, - 0.00298877805F, -0.00262106722F, 0.0036524462F, -0.000728092273F, - 0.00289561343F, -0.000768104219F, 0.00116890646F, -0.000303218461F, - -0.0023371214F, 0.00104967586F, 0.00230768137F, 1.59936226E-5F, - 0.00196701963F, -0.00319774472F, -0.00176706235F, 0.00128545926F, - 0.00168520934F, 0.0027988411F, -0.00323822722F, -0.00114003813F, - 0.000940075959F, -0.00121467549F, 0.00011948045F, -0.00173669693F, - -0.00100090099F, -0.00146774296F, -0.00092410011F, -0.000181586438F, - -0.000159585688F, -0.00154171512F, -5.45749535E-5F, -0.00132020516F, - -0.000809325953F, -0.00151012803F, -0.00188123726F, 0.00108026282F, - 0.00223872F, -0.00218854262F, -0.00227406621F, 0.00432394631F, - 0.00379469059F, 0.00182128756F, 0.00134224258F, -0.00111604959F, - 6.37572375E-5F, 0.00131165027F, -0.00174315891F, 0.00356312166F, - 0.00169651839F, -0.000950384652F, 0.000394969597F, 0.00437373621F, - 0.000368870853F, -0.000673843431F, 0.0019844037F, 0.00141740451F, - -0.00304879225F, 0.00289580785F, -0.00248536048F, -0.000173247332F, - 0.000730982632F, 0.0011111995F, -0.000430520944F, -0.0042368616F, - 0.0015405087F, -0.00113674614F, 0.00253426447F, -3.61454513E-5F, - -0.000414932554F, -0.00517580286F, 0.00105127587F, 0.00105876068F, - -0.000232964085F, 0.00172616006F, 0.00175548799F, 0.000279472326F, - -1.60376221E-5F, -0.000798178546F, 0.00138958218F, -0.00467880489F, - -0.00294018816F, -0.00170733046F, 0.00107746362F, 0.00539489184F, - 0.000650385511F, -0.00148441095F, 0.000730823143F, 0.00313303526F, - -5.35385261E-5F, 0.000926125038F, -0.00366278808F, 0.000852740486F, - -0.000609195034F, -0.00263706502F, 0.00299699185F, 0.000357686687F, - -0.00224624434F, -0.000891693751F, 0.00252269651F, 0.000957876036F, - -0.00247977534F, 0.00129195873F, 0.00136371935F, 0.0028538953F, - 0.00215578428F, -0.00288438F, 0.000641588937F, 0.000588999246F, - -0.00162950275F, 0.00143976032F, 0.000182514152F, -0.00427794782F, - -0.00106571347F, -0.00175043056F, 0.00167742977F, 0.000205424411F, - -0.00108545413F, 0.000617723796F, 0.00016662947F, -0.00221385085F, - -0.00185694173F, 0.00243275F, -0.00138779706F, 0.00245314091F, -0.000729316F, - -0.0021383597F, -0.000609223498F, 0.000265537325F, 0.000210740167F, - 0.00131946628F, 5.9780923E-5F, -0.000504792435F, 0.00234655058F, - -0.00150109944F, 0.00030525183F, 0.00292555825F, 0.00202671322F, - 0.00154437625F, 0.0011700996F, 0.00116516475F, 0.00521302503F, - 0.00141791371F, -0.00180045527F, -0.00061996025F, 0.000130981629F, - -0.000487516721F, -0.000976753188F, 0.00290902518F, -0.00229062582F, - -0.000641649181F, -0.000389524474F, -0.000764125434F, -0.00178923272F, - -0.00255905627F, -0.001287658F, 0.000172094777F, 0.000783052F, - 0.00155584991F, -0.00306658819F, -0.00314813177F, -0.0026597213F, - -0.0026042969F, 0.00105086213F, -0.00274246139F, 0.00141889113F, - -0.00103530695F, -0.000952873903F, 0.00330397906F, -0.00133154681F, - 0.00194181048F, -0.00354764215F, -0.00128662516F, 0.0012427785F, - 0.00215952937F, 0.000519031135F, -0.00111969875F, -0.000568075164F, - 0.00274858926F, 0.00098272F, -0.00154129148F, -0.0013280987F, 0.00114533014F, - 0.00047170819F, 0.00111045304F, 0.000477705849F, 0.00205357559F, - -0.00142986479F, -0.000664216583F, 0.00225642277F, 7.03972401E-5F, - 0.00198551151F, 0.00118832977F, -0.00165174843F, -0.00215537613F, - -0.00137078948F, -0.00169617648F, -0.00111926859F, 0.00284069218F, - -0.0024530387F, 0.00105117389F, 0.00165356637F, 0.000411357265F, - -0.00040206319F, -0.00106261775F, 0.00221788976F, -0.000774942513F, - -0.00128224411F, 8.82880486E-5F, -0.00261184527F, 0.000927187561F, - -0.000169249688F, 0.00405400805F, 0.00453416258F, 0.000950211077F, - -0.000975071511F, 0.00118701649F, 0.00205426058F, -0.000937824661F, - 0.00185980054F, 0.00133789249F, 0.000632149109F, 0.00048252492F, - -0.000396465242F, 0.00132435979F, 0.0021730219F, -0.000904845132F, - -0.00460711122F, 0.00314863981F, -0.000701208599F, 0.00227357424F, - 0.00496739522F, 0.00161683047F, -0.0029247764F, -0.000286244671F, - 0.00128423423F, 0.00215015607F, -0.000442650053F, -0.000884260167F, - -0.00117410254F, 0.0014832801F, -0.00108450348F, 0.00118550344F, - 0.000351488416F, 0.000733293F, -0.00258928048F, 0.00107682042F, - -0.0034601572F, 0.00214733556F, -0.00240264181F, -0.000194781111F, - 0.00043141772F, -0.000253331644F, 0.000635084114F, -0.00230280915F, - -0.000851033546F, -0.000699175871F, -0.00177648268F, 0.00130416779F, - 0.00237652753F, 0.00015596475F, 0.000200015464F, -0.000389888824F, - 0.00154404121F, 0.0016131209F, 0.00107855385F, 0.00210743374F, - 0.00159965444F, -0.00235518906F, -0.00243853382F, -0.00107415486F, - 6.94740447E-5F, 0.00117438822F, -0.000936896F, -0.0030665081F, - 0.000176742426F, 0.000843102F, 0.000133051F, -0.000318552484F, - -0.00154526171F, -0.00204769452F, 0.000300458196F, -0.000393231603F, - -0.00359659223F, 0.00207672128F, -0.00189666252F, 0.00281529338F, - 0.000602984335F, 0.0040063276F, 0.000388000917F, -0.00153137953F, - -0.00186204491F, -0.00149401464F, 0.00159134029F, -6.29210044E-5F, - -0.00200073258F, -0.00193331193F, 0.0019060435F, 0.000884993526F, - 0.00099339569F, -0.00215858221F, -0.00328060286F, 0.000488972873F, - -0.000920083607F, -0.000154923371F, 0.00221978966F, 0.00228011981F, - -0.0030764949F, -0.00473175F, -0.00136870577F, -0.00294995937F, - -9.67302331E-5F, 0.00338532357F, 0.000491884944F, -0.000147075159F, - 0.000962026475F, -0.00136954698F, -0.000398960139F, 0.00130138185F, - 0.00316373864F, 0.00438204082F, -0.00368843577F, 0.000197749687F, - 0.0034618068F, 0.00261763693F, 0.0011741597F, 0.00102980365F, 0.00164566736F, - -0.00177879515F, 0.00013652553F, 0.00155362347F, -0.00278198044F, - 0.00130810856F, 0.00250029122F, -0.00189683191F, 0.00240290212F, - 0.00167252135F, 0.000423643098F, 0.00127535721F, -0.00187185477F, - -0.00140876602F, 0.00163059728F, 0.00132701779F, -0.0009709968F, - 0.00375226676F, 0.000234187479F, 0.000330666109F, -0.000167925158F, - -0.00165751681F, -0.0010244929F, 0.00137981062F, 0.00122967502F, - -0.00219557574F, 0.000690624F, 0.00172244105F, -0.00313762762F, - -0.000692862144F, 0.000922658073F, 0.000185863682F, -0.00391470036F, - -0.000150962398F, -0.00166116329F, 0.000345317327F, -0.00196303218F, - -0.000345528737F, -0.000327203044F, -0.00150309224F, 0.00101548946F, - -0.000830630946F, 0.00149647077F, -0.00060453621F, 0.000453948247F, - 0.000589294359F, -0.00141856633F, 2.90371372E-5F, 0.000670710928F, - -0.000142572084F, 0.00236321404F, -0.000475204404F, -0.00141812849F, - -0.00451781042F, 0.00116245821F, -0.00170718471F, 0.00169498625F, - 0.00109419576F, 0.00219566328F, 0.00018650954F, 0.000630436291F, - -0.00187844248F, 0.000803541159F, -0.00102061301F, 0.00444771862F, - 0.0021353676F, -0.00210936018F, -0.00198719348F, -0.00169548485F, - 0.00257458747F, -0.000383455132F, -0.000203931399F, 0.000966405671F, - 0.00241464679F, -0.000765987555F, 0.000461084128F, 0.000699542346F, - -0.00186009204F, -0.00263914419F, 0.000611602562F, 0.00333547F, - 0.0030272631F, 0.00241310522F, 0.00155367644F, -0.000676203519F, - 0.0016749237F, -0.00205656886F, 0.00109614676F, -0.00437415158F, - -0.000872314558F, -0.00174712972F, -0.00063059252F, 0.00157990481F, - 0.000370041816F, -0.000453492074F, 0.00161058141F, -0.00342810294F, - 0.00209231861F, 0.00121457828F, -0.00269155973F, 0.00354748336F, - 0.000176873029F, 0.0018067538F, -0.000152957175F, -0.00143659697F, - 0.00334782712F, -0.00038983507F, 0.000508376281F, -0.00180628255F, - 0.00101600122F, 0.00201908359F, -0.00221106899F, -0.00162837678F, - -0.00169589219F, -0.00143896742F, -0.000995404553F, -0.00402059918F, - 0.00108491047F, -0.00255312584F, -0.00158707297F, -0.00439132F, - -0.000421777484F, 0.00213835388F, -0.00206735334F, 0.000102537211F, - 0.00129840965F, -0.00106171938F, 0.00310621061F, 0.000927453162F, - -0.00223976676F, -0.00230418122F, -0.00205589086F, -5.11121198E-6F, - 0.00235690223F, -0.000955946452F, -2.93542835E-5F, -0.00427383557F, - 0.000688431F, 0.00314867869F, 0.000364225F, -0.000803761417F, 0.00336122536F, - 0.00122115808F, -0.00135867507F, -0.000618245802F, -0.000346456625F, - 0.00436252961F, -0.00289616967F, 0.00261492282F, -0.00317784236F, - -0.0015704562F, -0.000373487681F, 0.00137646869F, 0.00148808968F, - -0.00119366345F, 0.00119629363F, 0.00106852653F, 0.00104095088F, - 0.000523077324F, 0.00115067547F, 0.00200532773F, 0.00394058414F, - -0.000650714559F, -0.000678415934F, 0.000127111402F, -4.10338798E-5F, - -0.000263457274F, 0.00209268369F, -0.00592647213F, -0.00259536435F, - -0.00343894889F, 0.00208424707F, -0.00234909402F, -0.00157658756F, - -0.000155200338F, 0.00214580074F, 0.00201647799F, -0.00068088033F, - -0.000169649167F, 6.70058216E-5F, -0.00366157526F, -0.00222082413F, - -0.00118239655F, 0.000975311792F, 0.00317277457F, -0.00122335635F, - 0.00482298061F, 0.000275440863F, 0.00456706109F, 0.000181710639F, - 0.00339886406F, -0.000329753791F, 0.00157404365F, 0.00128720631F, - -0.00114930037F, -0.00140370708F, -0.00140624703F, -0.00138723943F, - -0.00196344382F, -0.00124588131F, -0.00176923256F, -0.00369255641F, - -0.000961608835F, -0.000206227036F, -0.00537909241F, -0.00022604948F, - 0.00139304332F, -0.00103754236F, -0.00138646865F, 0.000445334503F, - 0.00280996086F, 0.000109277178F, 0.000402541482F, -0.000221719136F, - 3.0836145E-5F, -0.00500084879F, 0.000750616076F, 4.5364719E-5F, - -0.000371786795F, -0.00248534209F, 0.00259764562F, 0.00257389341F, - -0.00101583544F, -0.000445340527F, 0.00443103444F, -0.0016664773F, - 0.000942053506F, -0.0045698951F, -0.0015764758F, 0.00368918269F, - 0.00147138431F, 0.00215776707F, -0.00277875457F, -0.000586176931F, - -9.41878243E-5F, 0.000761783158F, -0.00253725168F, 0.000500906317F, - -0.00295352656F, -0.00182090013F, 0.00314033707F, -1.81934302E-5F, - 0.0019218619F, -0.000777882175F, 0.0010709943F, 0.00177742331F, - -6.14832461E-5F, -0.000546066F, -0.000919633138F, 0.000187961588F, - -0.000167479709F, -0.00335097522F, -0.00442807F, 0.00350057916F, - -3.18151833E-5F, -0.000795406173F, -0.00101684697F, -0.00150463264F, - 0.000246889133F, -0.00237745838F, 4.33130226E-5F, 0.000469869381F, - -0.0012591721F, 0.00201652432F, -0.00102422549F, 0.00187542348F, - -0.00242000748F, 0.00374416704F, -0.003284059F, 0.000561018765F, - 0.00180272246F, -0.00273264316F, -0.000140714837F, 0.00284786569F, - 0.000481905F, 0.00212963764F, 0.000682283251F, 0.000402523379F, - 0.000168425322F, 0.000210884784F, 0.00127536815F, -0.000613354146F, - -0.00108704041F, 0.000738720584F, 0.00141089875F, 0.00464905566F, - 0.00147086207F, 0.00224774936F, 0.000585289672F, -0.000389534427F, - -0.00137314794F, 0.0023824228F, -0.00232555694F, -0.00235574F, - -0.00220034341F, -0.00169362559F, 0.000966522493F, -0.000999292824F, - 0.00162696186F, 0.00142213493F, -0.00328843133F, -0.00102240825F, - -0.0033582272F, 0.00355949881F, -0.000127900756F, -0.00121778413F, - -0.000234541716F, -0.00075284153F, 0.00515048858F, 0.000311360025F, - -0.000866523595F, 0.00128459744F, -0.00170035358F, 0.00337549695F, - -0.000169821564F, 0.000588447612F, 0.00135031296F, -0.00242946297F, - 0.000227788041F, -0.000505068107F, -0.00295178778F, -0.00305578392F, - -0.00136010174F, -0.00137301884F, 0.00066382F, -0.000943462481F, - 0.00271122111F, 0.00169845961F, 0.000449161715F, -0.00192584062F, - 0.00282401172F, -0.00257356116F, 0.000325578905F, -0.00290669035F, - 0.00268867682F, -0.00042557728F, -0.00155517901F, 0.000423249032F, - -0.000715662434F, 0.00309782755F, 2.28522877E-5F, -0.00149748777F, - -0.00054852519F, -0.00414774613F, -0.00290831737F, -0.00227268599F, - -0.00196590135F, -0.00235812F, 0.00117206888F, 0.000429183943F, - -0.00265312754F, 0.000430321146F, 0.000192226988F, -0.00130632531F, - 0.000633968564F, -0.00193130935F, 0.000921490311F, 7.71094055E-5F, - -0.000734118337F, 0.0016428791F, 0.00055170519F, 0.00139548408F, - 0.000622864289F, -0.00278962869F, 0.000354089076F, -0.000476748217F, - -0.00288536539F, -0.000137177543F, -0.000515573425F, -0.00224733888F, - 0.00162989076F, 0.000151208718F, -0.000504219264F, 0.000424238824F, - -0.000679458608F, 0.00182354439F, 0.000352393778F, 0.000898190134F, - 2.81988887E-5F, 0.00266554719F, 0.000957053155F, 0.000553949387F, - 0.00442913268F, -0.000751063169F, -0.00246381597F, -0.00198479043F, - -0.00309439213F, 0.000295574922F, -0.0018579131F, 0.00192298763F, - 0.00195802166F, 0.000520226138F, 0.00151615543F, -0.00020661125F, - 0.00102457637F, 0.00111999235F, -0.000756799F, -0.000594174198F, - -0.000417835923F, 0.00166179205F, -0.00403402653F, -0.000927768182F, - -0.00166356447F, 0.000729263527F, -0.00155845773F, 0.000230435951F, - -0.00285285F, -1.453405E-5F, -0.000629388262F, 0.000581496395F, - -0.0019335415F, -7.10677705E-5F, 0.00223485334F, 0.00229157298F, - 0.00126765808F, 0.00158165628F, -0.000601718668F, -0.00153838459F, - -0.00283380551F, 0.000493949046F, 0.000437656039F, -1.59004248E-5F, - -0.000471734587F, -0.000712784415F, 0.00166418753F, 0.000800435F, - -3.81912796E-5F, -0.0025309117F, 0.00195317F, -0.00206156471F, - 0.00141990348F, 0.00340341241F, 0.000871442433F, -0.000583512185F, - -0.00267650816F, 0.00123769743F, -0.000218074F, -0.000983311F, - 0.00391578162F, -0.000521117239F, 0.00153209351F, 0.00154362048F, - 0.00317520415F, -0.00104823487F, 0.0024822359F, -0.000204697F, - 0.000181599171F, -0.000829647586F, 0.000916722463F, -0.00106973771F, - -0.00292616757F, -0.000703613F, -0.00254601869F, 0.0035643389F, - -0.000823662F, 0.0011318248F, 0.00119461573F, 0.00128238369F, 0.00077480037F, - -0.00351531198F, 0.000158624054F, 5.70670963E-5F, 9.27366364E-6F, - -0.00220710458F, 0.000873087323F, -0.00113291F, -0.000234765452F, - -0.00212514424F, 0.000634890806F, 0.00271181972F, 0.0013117342F, - -0.00190255954F, -0.00126655889F, -0.00177870027F, 0.00318525033F, - -0.000612858566F, -0.0015461453F, 0.000599990599F, 0.000883546891F, - 0.00424664095F, -0.00277898484F, 0.00177232432F, -0.00188404194F, - -0.0019904885F, -0.000178722708F, 0.00155075744F, 0.000369485904F, - -0.000534724619F, 0.00153522007F, 0.00164271356F, -0.000646383385F, - -0.00358987134F, 0.00180932542F, -0.00400311919F, 0.0038764067F, - -0.00273870211F, 0.00170952757F, 0.00433245394F, 0.0017849264F, - 0.00301327556F, 0.00169282989F, 0.0022354417F, 0.0016627931F, - -0.000753452128F, 0.00143651047F, 0.00147884188F, -0.00143764017F, - -0.00215771166F, 0.00165701727F, -0.00376219256F, -0.00573804043F, - 4.22078519E-5F, -0.000550523575F, 0.0028546867F, 0.00136758108F, - 0.0034053463F, 0.000805253221F, 0.00158833386F, 0.00339458068F, - -0.00124387396F, -0.00247154944F, 0.000713343208F, -0.000677329F, - 0.000724358892F, -0.000650984F, -0.00255218497F, -0.00188501307F, - -0.00173808681F, 0.000625995395F, -0.0017904829F, -0.000959700381F, - -0.00454170769F, -0.000493447063F, 0.00254690764F, 0.00134841597F, - -8.24247836E-5F, 0.00216591451F, -0.000251792517F, -0.00241848524F, - -0.00163747743F, -0.00118132657F, 0.00258772587F, 0.00262415758F, - 0.00283078314F, 0.00109522545F, -0.000331216375F, -0.00260392972F, - 0.000144074642F, -0.002180896F, -0.00102200871F, -0.000420294498F, - -0.0023014883F, -0.000386995031F, 0.000395039737F, -0.000342058629F, - -0.00162199431F, -0.00131694495F, -0.000594595214F, -0.000308793562F, - -0.00129741407F, -0.00205542194F, -0.00296538929F, -0.00301628537F, - -0.00239380868F, 0.00219559646F, 0.00170559145F, -0.00198486354F, - -2.50784651E-5F, -0.000399448909F, -0.00193477969F, 0.00304245832F, - -0.00176978286F, 0.00236754725F, -0.00246903626F, 0.00419486221F, - -0.000812315557F, -0.000422971818F, 0.000225604264F, 0.00135393871F, - -0.00100368448F, 0.000537527492F, -0.00142165634F, -0.000235982792F, - -0.00161055115F, 0.0019442077F, -0.000386659551F, 0.000334406737F, - -0.00302550965F, 0.00187820755F, -0.00181849662F, 0.0025417041F, - 0.00046267896F, -0.00115579704F, 0.00296645891F, -0.000805920688F, - -0.0011786049F, -0.00193308387F, 0.00393852266F, 0.00212422735F, - 0.00137905241F, 0.00343522127F, -0.00053957873F, 0.00109381659F, - -0.00246347138F, 0.00347829517F, -0.00345063023F, -0.0019566596F, - 0.00158291787F, 0.00134951191F, 0.00124622893F, -0.00132874527F, - 0.00264311396F, 0.00209396449F, 0.00101069047F, 0.00184413651F, - -0.00205207733F, -0.00310009951F, -6.13073E-5F, 0.00140020705F, - -0.000183816F, -0.00148744718F, -0.00140175514F, 0.00253967661F, - -0.00227510207F, -0.00222253473F, -0.00310890772F, -0.00109509483F, - 0.00119821948F, -0.00618667528F, 0.00015707525F, 0.0030453112F, - -0.00253147213F, -0.00184258691F, 0.00137765193F, -0.00510709686F, - 0.00195464864F, 0.00177544274F, -0.00159948668F, 0.000463706849F, - 0.000933869102F, -0.0017117688F, -0.00250590639F, -0.00137624727F, - -0.000560844084F, -0.000640166749F, -0.00130338792F, 0.00232075155F, - -0.00079990353F, 0.000717504823F, -0.00378400343F, 0.0035755767F, - -0.000958507822F, 0.00109217048F, 0.000199933813F, 0.00344097242F, - 0.00271164277F, 0.00011537275F, 0.00483893324F, -0.0011018893F, - 0.00667162845F, -0.00117548904F, -0.00118623429F, -2.52955706E-5F, - 0.000838567968F, -0.00103899266F, 0.00276835333F, 0.00205644174F, - 0.00214431621F, -0.000654317555F, -0.000639970414F, -0.000985221239F, - -0.0038915514F, -0.0011846096F, 0.00152606156F, -0.000928365102F, - 0.00134274189F, -0.00264164433F, 0.00289518246F, -0.00168360269F, - 0.00286492775F, -0.000861114531F, 0.000174847984F, -0.00069250993F, - 0.00172836438F, 0.0024995734F, 0.00279173697F, 0.000879846863F, - -0.00105098856F, 0.00162173226F, 0.00203116448F, -0.0017205925F, - 0.00213381625F, -0.00265157828F, -0.000749428291F, -1.62580982E-5F, - 0.00359898899F, -0.00176056463F, -6.9345886E-5F, 0.00325549417F, - -0.00256282813F, -0.00215182221F, -0.000198794907F, 0.000489547558F, - 0.00440846663F, -0.00218451698F, -0.00464085164F, 0.00287850457F, - 0.00129323266F, 0.00130962604F, -0.000593561F, 0.00120443176F, - 0.00169877312F, -0.000637384364F, -3.79886878E-5F, -0.00380604528F, - 0.00242854399F, 0.00258707674F, 0.000416794355F, 0.00149435317F, - -0.000728155777F, -0.00135196431F, -0.00474716676F, -0.00111959118F, - -0.00220435904F, -0.000352637959F, 0.000149817017F, 0.00226771575F, - 0.0011651268F, -0.00332200737F, -0.00165548292F, -0.000664077874F, - 0.00044773714F, 0.000781185867F, -0.00168022432F, -0.00111467345F, - 0.00166342047F, -0.000167286984F, -0.0002146857F, -0.00261041033F, - 0.00235091569F, -0.0014937548F, 0.00232838094F, 0.00261477195F, - -0.00108950329F, 0.00149307016F, 0.00119166088F, -0.00214418164F, - -0.000805220276F, -0.00205227849F, -0.000752757071F, -0.00212423061F, - -0.00197104621F, 0.00135815714F, -0.000666719279F, -0.00143343909F, - -0.000653187046F, 0.00254742661F, -0.00111941143F, -0.000324126973F, - -0.000523389783F, 0.00146586844F, 0.000312883378F, -0.00168486114F, - -0.00127748924F, -0.00175599544F, 0.000384137238F, -0.00324464636F, - 0.00174405868F, 0.000136259652F, 0.00111416436F, 0.00227643014F, - 0.000759491348F, -0.00150738342F, 0.0010493506F, 0.00338957598F, - 9.92501373E-5F, -0.00355120096F, -0.000555946142F, 0.00141006988F, - 0.00279831863F, -0.000569356896F, -0.000830365403F, 0.000754458073F, - -3.66133027E-5F, -0.000496788707F, 0.00181459438F, 0.000985894934F, - 0.00166399207F, 0.00083984161F, 0.00157766021F, 0.00278465217F, - 0.00326681067F, -0.00086678128F, 0.00172301743F, -0.0023156025F, - 0.00158654421F, -0.00415274F, -0.000218084155F, -0.000614125223F, - -0.00291274139F, 0.00134986499F, 0.000519646041F, -0.00147242111F, - 0.000678104523F, 0.000904092856F, 0.00251226919F, 0.000286563736F, - -0.000832611811F, 0.00011484581F, -0.00351999421F, 0.00111012848F, - 0.00139849808F, -0.000140083095F, 0.000797735178F, 0.000926519162F, - 0.000549993361F, -0.00432107272F, 0.00228010048F, -0.00372174545F, - -8.58093481E-5F, 0.00111595227F, -0.000767083955F, 0.00169138552F, - -0.00102101243F, -0.000227027078F, -0.00447866786F, 0.00130507967F, - 0.00359140057F, 0.0014094275F, 0.00382333319F, -0.0037690124F, - -0.00339316903F, 0.00189024699F, 0.000201829054F, -0.00412495481F, - -0.000270998222F, -0.000625416054F, 0.000422950019F, -0.0026119086F, - 0.000387857901F, -0.000663143F, -0.000335562829F, 0.0013139596F, - 0.00301071024F, -0.000263297727F, 0.00180143735F, 0.000680919387F, - 0.00117482664F, 0.000785601675F, -0.000816917687F, -0.00187947194F, - -0.00237266F, -0.0014669979F, -0.00175711175F, 0.000783059804F, - 0.000330353039F, 0.00232114014F, 0.00287764496F, -0.00042321725F, - -0.000960109348F, 0.00309313135F, -0.00326709449F, -0.00070646021F, - -0.00108351558F, 0.000652982388F, 0.00175477029F, -0.00016673893F, - 0.000432487577F, 1.18654571E-5F, 0.00316636055F, -0.00111948326F, - -0.000797866436F, 0.00183677627F, -0.00205042539F, -0.0022508353F, - -0.0021736084F, -0.000667551067F, 0.000636594137F, -0.000663167797F, - 0.00283622113F, 0.000609587238F, 0.000419913384F, 0.00175202184F, - -0.00196304475F, -0.00164231716F, 0.00265625119F, 0.00259427656F, - -0.00279904483F, 0.00244969805F, 0.00403621513F, -0.00167102506F, - 0.00109740533F, -0.000875614816F, -0.000437598821F, 0.000493277621F, - -0.000110892855F, -0.00240009092F, 0.00107593939F, 2.64908667E-5F, - -0.0004426339F, -0.00137778802F, 0.000343448715F, 0.00151455181F, - -0.000185130179F, 0.00382062187F, -0.000255518826F, -0.00124185055F, - 0.00208206451F, -0.00185679155F, -0.00241026795F, 0.000202936411F, - -1.55407251E-5F, -0.00170257606F, 0.00346665806F, 0.000552605197F, - -0.00129477191F, -0.00275638141F, -0.00171468186F, 0.0031465604F, - 0.00104252179F, 0.00432452559F, -0.00360099226F, 0.00123285688F, - -0.00203123596F, 0.00139470887F, 0.000253646926F, 0.00354420207F, - -0.00219378201F, 0.00103458855F, 0.00136393693F, -0.00168281235F, - -0.00131563225F, 0.00253828336F, 0.000329681236F, 0.0034342357F, - 0.000767707417F, 0.00154672319F, 0.000964924169F, 0.00328742689F, - 0.00175519253F, -0.000337116304F, -0.000639811275F, 0.00272920588F, - -0.000823021343F, -0.00185947702F, -0.00190304278F, 0.000119828328F, - 0.00169924705F, -0.00191263319F, 0.00101083075F, 0.00019358311F, - -0.00221091043F, 0.00125237578F, 0.000559290173F, -0.0022567478F, - -0.000875532394F, 0.000143082216F, 0.00212709815F, 0.00178834423F, - 0.000326338399F, -0.00352865551F, -0.000624473265F, -0.00184651173F, - 0.00256511825F, -0.00253425329F, 0.0040575373F, 0.00132347911F, - -0.00082559319F, -0.00145925314F, 0.00355515163F, -0.00346436794F, - 0.00169305026F, 0.00112791534F, 0.00117547822F, -0.00143836497F, - 0.00279906555F, 0.000726538943F, 0.000148256717F, 0.000889110437F, - 0.00140266365F, -0.00231561624F, -0.00160542957F, 0.00191464624F, - -0.00211486F, -0.00325558428F, -0.00111451559F, 0.0011914823F, - 1.69232972E-5F, -0.00195743446F, -0.00290915859F, -0.000371814327F, - -6.8250949E-5F, -0.00145510782F, -0.00169603643F, 0.000107897242F, - -0.00233175908F, -0.000412185531F, 0.0005628207F, -0.000639861217F, - -0.00300480239F, 0.00203856919F, -0.000673481089F, 0.000343777647F, - -0.000366567197F, -3.11349504E-5F, 0.000867614348F, -0.00154859445F, - -0.00222198F, -9.75382573E-5F, 0.0014883437F, 0.00108765182F, 0.00392675167F, - 0.00176925247F, 0.000588513096F, 0.000765768F, -0.000537231506F, - -0.003700634F, 0.000196487468F, -0.000104965169F, 0.00104351121F, - 0.00135329412F, 0.000978123F, 0.00297151227F, -0.00187573396F, - 0.000896283193F, 0.00132445502F, 0.000616063888F, -8.36076506E-5F, - -0.000705325045F, 0.00179682975F, 0.000838441367F, -9.36252691E-5F, - 0.00150186545F, -0.000474977103F, 0.00440527173F, -0.00131145085F, - -0.00144462427F, 0.00293727149F, -0.00230212393F, -1.48857171E-5F, - 0.00099272537F, -0.00186099322F, -0.000645899156F, 0.000509717851F, - -0.000618757331F, -0.00387274148F, -0.000711006054F, 0.00206560711F, - -0.000809297431F, 0.00334346853F, -0.00170914119F, 0.000168069149F, - -0.00168718782F, -0.00154413062F, 0.00034093362F, 0.00109307771F, - 0.00112844503F, 0.00228630565F, -0.000955223804F, -0.00323720928F, - 0.000655948417F, -0.00162398827F, 0.000294579222F, 0.000666471082F, - 3.0917523E-5F, 0.000356829638F, -0.000268092088F, -0.000503337651F, - 0.00172347354F, -0.00369208702F, -0.00541151455F, -0.0019133453F, - -0.00117431942F, 0.00225136662F, 0.000756446563F, 0.00224020844F, - -8.8866429E-5F, 0.00100598589F, -0.00280174147F, 0.000284635084F, - -0.000944792293F, 0.00309719844F, 0.00186810887F, 0.00187224173F, - 0.00090964837F, 0.000900705229F, -0.00248810253F, 7.44599602E-5F, - -0.00213826494F, -0.00103311497F, 0.00433504F, -0.00274906796F, - 0.000689844484F, -0.000723069243F, 0.00286909821F, -5.73622128E-5F, - 0.0036593508F, -0.000317282829F, -0.00201041973F, -0.00120463793F, - -0.00150899449F, 0.00219395175F, 0.00327606546F, -0.00037121214F, - -0.00293465029F, -0.000665614789F, 0.00297871535F, -0.00161835947F, - 0.000632435724F, -0.000831521F, -0.00287257647F, -0.000710158143F, - 0.0033318433F, -0.000337758538F, 0.00196208386F, -0.00062374468F, - 7.39108873E-5F, 0.000238724402F, -0.00122331479F, -0.00225863443F, - 0.000400011922F, 0.00183574972F, -5.42098169E-5F, 0.00132597564F, - 0.001362824F, 0.00101134682F, 0.000970666F, -0.000409572764F, 0.00105065515F, - -0.0028443581F, 0.00353450258F, 0.00241456646F, 0.00281704077F, - 0.0012795357F, 0.000921880594F, -0.00107040326F, 0.00257408898F, - -0.00311551709F, 0.00366700487F, -0.00095100177F, 0.0015103952F, - -0.00157721073F, -0.00202892441F, -0.000255157065F, 0.00169558229F, - 0.00101307058F, 0.00128423981F, -3.12293778E-5F, 0.000200880299F, - 0.00133345858F, -0.000785542827F, -0.00284799258F, -0.00134362932F, - -0.00229198602F, -0.00279102195F, -0.000591835415F, 0.0013013162F, - -0.00065411F, 0.00135237095F, -0.00240933569F, -0.000377771445F, - 0.000431927794F, 0.000840969558F, -0.00148680818F, 0.00433838647F, - 9.54933348E-5F, -0.00087655423F, 0.000640358834F, 0.00236781687F, - 3.62465107E-6F, 0.00184579741F, -0.000360683247F, -0.00148763077F, - 0.0047220625F, -0.00178633968F, 0.00112995028F, -0.000644131796F, - 0.00306488434F, -0.00156585302F, -0.00035938347F, 0.00199671276F, - 0.00031770012F, 0.00107277371F, 0.000764514902F, 0.000554202357F, - -0.00570004852F, -0.000573490921F, 0.00109744875F, 0.00191245577F, - -0.000521488604F, 0.00525988F, 0.000969365763F, 0.00232895697F, - -0.000674833485F, -0.00127233588F, -8.02588E-6F, 0.000774747692F, - 0.000933423173F, 0.0022323567F, -0.0025021031F, 0.000768653641F, - -0.0023852759F, -0.000251634599F, -0.000736776041F, -0.00285015209F, - 0.00193151645F, 0.00283103297F, -0.000429127977F, 0.00134628417F, - -0.00346931792F, -0.00486826384F, 0.000815629435F, 0.0024079096F, - 0.00205246639F, -0.00197626138F, -0.000334178127F, -0.000333428412F, - 0.00184112124F, -0.000184598684F, -0.00293751387F, -0.0013305F, - -0.00042007977F, 0.00370000466F, -0.000243487113F, 0.00186076679F, - 0.000874040416F, 0.0038731182F, 0.000703974336F, 0.000305669731F, - -0.00109573232F, -0.00325053162F, 0.000543432194F, 0.000730733678F, - 0.000806987577F, -0.00154419697F, -0.00291914423F, -0.000656858436F, - -0.000915632758F, -0.00188783475F, 0.00191470922F, -0.00131618662F, - -0.00126154511F, -0.00301362434F, 0.000775783905F, -7.62263653E-7F, - 0.000232270584F, 5.98437946E-5F, 0.00234073075F, 0.00112513662F, - 0.00057057105F, 0.00476752454F, 0.000535083294F, 0.00239714887F, - 0.000546880241F, 0.00208007614F, -0.00144613232F, 0.00307585206F, - -0.00186849094F, -0.00038321098F, -0.00152668299F, -0.00130003458F, - 0.000460948941F, -0.00124628644F, -0.00161415304F, -0.00149987591F, - 0.000502887298F, 0.000588633353F, 0.00108932436F, 0.00202446571F, - -0.000421173463F, 0.00147993595F, 0.000293388555F, -0.0021864376F, - -0.000876023551F, -0.000985012273F, 0.00179976888F, 0.000297640159F, - 0.00486173062F, 0.00354747241F, 0.00205412367F, -0.000987571664F, - -0.00166504132F, -0.00151375914F, 0.00250707F, 0.00121625257F, - 0.000440236094F, -0.000731687469F, -0.000231923987F, 0.00220838096F, - 0.00433237F, -0.00289329421F, -0.00156462367F, 0.00126339716F, - -0.000287822186F, -0.00129015977F, 0.00286792801F, 9.81033E-5F, - -0.00176051026F, 9.78265743E-5F, 0.000758705952F, 0.00396336522F, - 0.00344987423F, -0.000876415346F, -0.0006936391F, 0.00324853347F, - 0.00104972883F, 0.0005611261F, -0.000340769358F, 0.00256423419F, - 0.000168220708F, -0.000802388939F, 0.0024035559F, 0.00287383585F, - 0.000448494742F, 0.00293076F, 0.000609645562F, -0.00150336779F, - -0.00343317422F, 0.00262729614F, -0.00134065689F, 0.00174553017F, - 0.00286940858F, -0.00138229027F, -0.00030448605F, -0.00112709787F, - -0.00224577F, -0.000796107226F, 0.00318848761F, -0.000346135697F, - -0.00239571626F, 0.000346934568F, -0.00156456116F, -0.000401252357F, - 0.00188926631F, -0.000285980233F, 0.00246300548F, -0.000379474455F, - 0.00151602738F, -0.000486494304F, 0.000882524182F, 0.00282407086F, - 0.00244842051F, -0.00302299811F, 0.00450022798F, -0.0028757134F, - 0.00140842993F, -0.00207094406F, 0.000328466675F, -0.0030386718F, - 0.000184836899F, 0.000142366131F, -0.00174354948F, -0.00177824323F, - -0.00127960299F, -0.0027420253F, -0.000396215561F, -0.00149207469F, - 0.000712633657F, -0.0021070959F, -0.000235730098F, -0.00260800752F, - -0.00287885196F, 0.000902776548F, 0.000319279585F, 0.0020040744F, - 0.000217886176F, 0.00136269885F, 0.00134865707F, -0.00400164258F, - -0.00204371312F, 0.0013283781F, -0.00179558853F, -0.000691657362F, - -0.00237242412F, 0.000252408528F, 0.00217399769F, -0.00174413587F, - -0.00114924891F, 0.000757565314F, -0.00292287883F, 0.00424216827F, - 0.00115246524F, 0.00133484544F, -0.00245264592F, 0.00174300373F, - 0.000999431591F, 0.00230021658F, -0.00112853455F, 8.36629697E-5F, - -0.00305394805F, -0.000565788476F, 0.00122205145F, 0.00155838556F, - -0.00123014674F, -0.000340755127F, 0.00146965554F, 3.33360695E-5F, - 0.00298399408F, -0.00231252145F, 0.000309868308F, -0.00197204016F, - 0.000668498746F, -0.00120169704F, -0.00172656646F, -0.000353221927F, - -0.000574460195F, 0.000837786472F, -0.000916342367F, 0.00112374313F, - 0.000682802347F, -0.000197302521F, -0.00214825175F, 0.00225572428F, - 0.001789317F, 0.00169244315F, 0.00400734739F, 0.000972023059F, - 0.000631044153F, 0.00199552067F, 0.000347536057F, -0.000956783F, - 0.00303861964F, 0.000381903053F, -0.00248538074F, 0.000168341532F, - 0.00179346872F, 0.000122351717F, 0.00118722615F, 0.00122340128F, - 0.00381439272F, 0.000505321193F, -0.00160025479F, 0.00132898032F, - -0.00253935368F, 0.000644456F, -0.00195188727F, 0.00218450674F, - -0.00104356313F, 0.000852009F, 0.000598834769F, 0.000837873667F, - -0.000150749009F, 0.00118852418F, 0.00472139F, -0.00111108087F, - -0.00124522729F, -0.0022688068F, 0.000577959523F, 0.0012639215F, - -0.0007230388F, -0.00156317034F, 0.000880194129F, -0.00222838856F, - -0.00264163339F, -0.00280421064F, 0.000972407F, -0.00100042112F, - 0.00150570855F, -0.000859317603F, 0.00175689103F, -0.00142715254F, - -0.00120420218F, 0.00631178962F, 0.00058567978F, -0.000947227585F, - -0.00201211288F, 0.00186125748F, -0.00118233915F, -0.000830060337F, - -0.00158220425F, -0.000113067719F, -0.000963542843F, 0.0028886525F, - 0.00143661222F, 0.00202206452F, -0.00313319173F, -0.00133703568F, - -0.00105738791F, 0.0012118764F, -0.00184657634F, -0.000416769501F, - -0.000352054019F, -0.00150517945F, -0.000612505537F, 0.00090566522F, - -0.00234642322F, 0.000747475F, -0.00339006423F, 0.00139564858F, - 0.00066569238F, -0.00132235803F, 0.00143468566F, -0.00249433587F, - -0.00153885584F, 0.000480366056F, -0.00122995721F, 0.00324290688F, - 0.000684235711F, 0.00209849142F, -0.00160052825F, -0.00193896273F, - 0.00153237674F, -0.00106084452F, 0.00146802189F, 0.00113181269F, - -0.00159860402F, -0.000192391119F, 0.00205371156F, -0.00196833489F, - -0.00111266319F, -0.000940748549F, -0.00297442428F, -0.00162044447F, - -0.000859090884F, -0.00175645901F, -0.000841958856F, 0.0008813811F, - 0.00227765692F, -0.00167872664F, 0.000541685265F, 0.00388430827F, - 0.000979107805F, 0.000234547362F, -0.00316024898F, 0.00292920764F, - 0.000478823669F, 0.00231923372F, -0.000560248794F, -0.00183146913F, - 0.00248130062F, 0.000114395567F, -0.00181111775F, 0.000256781786F, - 0.00400817301F, -0.000570163F, -0.00447990047F, -0.00410951534F, - -0.00226038322F, -0.000300216721F, -0.00198553572F, -0.00411218032F, - -0.00098411378F, -0.000970649882F, -0.00207249052F, 0.00118922081F, - 0.00301282F, -0.0034871269F, 0.00384513987F, 0.000924004882F, -0.0013787864F, - -9.84422804E-5F, -0.00162907189F, 0.00232059369F, -0.00517543871F, - -0.00418251473F, 0.00118373369F, -0.0004187554F, -0.000683136168F, - 0.000712906301F, -0.00136742159F, 0.00228037522F, -0.000395814248F, - -0.00360879255F, 0.000726771716F, -0.00108156097F, 0.00170116231F, - -0.000316079881F, 0.000602089101F, -0.0015977195F, 0.0010615295F, - 0.00242039375F, -0.00158399949F, 0.000816309068F, -0.00197237171F, - -0.00065893057F, -0.00136199628F, 0.000171846928F, 0.00242319168F, - -1.13732895E-5F, 0.00152320613F, 0.00175972923F, -0.00366631546F, - 0.00140499498F, 0.000613331096F, -0.000158866402F, 0.00277549447F, - 0.000771332358F, -0.00468556629F, -0.00200762018F, 0.00182916317F, - -0.00339524122F, -0.000900595F, 0.00279592839F, -0.0011889973F, - 0.00163606915F, 0.0026907064F, -0.00200158497F, 0.000197470377F, - 7.71781488E-5F, 0.00137406238F, -0.00342209428F, 5.1639774E-5F, - -0.000339424354F, -0.00138568925F, -0.000270462275F, -0.00142307498F, - 0.000971881265F, -0.000317790458F, 0.000582939712F, -0.00039858208F, - -0.00283595151F, -0.00129602F, -0.00251191971F, 0.00112955808F, - 0.000770119252F, -0.00148707244F, 8.07018805E-5F, 0.000327425834F, - 0.000276950654F, -0.000127535037F, -0.000491308514F, 0.00190858275F, - -0.000414052833F, -0.00372399855F, -0.00154997874F, -0.000630814582F, - 0.000583676272F, 0.0020798496F, 0.00159426872F, 0.00187652151F, - 0.00112848356F, 0.000878995F, -4.35603797E-5F, 0.00244184979F, - -0.00739168655F, 0.0021298856F, 0.000708533858F, 0.00227059727F, - 0.000256688596F, 0.00254843151F, 0.00178695074F, 0.00205770717F, - 2.25657386E-5F, 0.00129769859F, -0.00121998868F, -0.0013203444F, - 0.00249655196F, 0.000767665857F, 0.000951729657F, 0.00317161065F, - -0.0024668111F, -0.00426612375F, -0.00340743153F, 0.00140571746F, - 0.00466331095F, 0.000166407073F, -0.000433516601F, -0.00122354762F, - 0.00329733F, 0.00168334809F, -0.000966917432F, 0.00143767043F, - -0.00334464642F, -0.00410025381F, 0.00105466496F, 0.00260612718F, - 0.00413093902F, 0.00189906894F, 0.00079424266F, 0.00194480631F, - 0.000142270088F, 0.00423295703F, -0.00181447342F, -0.00340324454F, - 0.00300492626F, 0.00231767702F, 0.00012241381F, 0.00146967336F, - -0.00223255064F, -0.000252595579F, -0.0019764551F, -0.000562298053F, - 0.00259537227F, 0.00069833477F, 0.00137890084F, -0.000382595928F, - -0.000479700742F, -0.00291442452F, 0.000905793509F, 0.00161353289F, - -0.00125450804F, -0.00195733644F, 0.000656644232F, 0.00308591919F, - 0.00273604738F, 0.00154541258F, 0.00439200038F, -0.00249181152F, - -0.00156324264F, -0.00357713876F, 0.00141525839F, 0.00377448834F, - -0.00136550842F, 0.00129582337F, -4.68849103E-5F, -0.00144801661F, - 0.000281932502F, -0.00192802108F, -0.00209831237F, -0.000684446073F, - 0.00183920178F, -0.00076606404F, -0.000888079579F, 0.00142073852F, - -0.000146285631F, -0.000565090508F, 0.00124710274F, -0.00311689F, - 0.00375167839F, 0.0021407241F, -0.00396959297F, 0.00105796347F, - 0.00332703581F, 0.00272516417F, -0.00105514575F, 0.00192543119F, - 0.00174127962F, 0.0004819608F, 0.00126428925F, 0.00164588145F, - -0.000112070644F, 0.00174130395F, -0.00273720152F, 0.0018387239F, - -0.00277184392F, -0.00259196293F, -0.000649618858F, -9.71237096E-5F, - -0.00440031243F, -0.00389869791F, -0.000994078582F, 0.0024562527F, - 0.00236004172F, 0.00268464745F, 0.00305898208F, -0.000972255F, - -0.00274583302F, 0.00011038567F, -0.00144387141F, 0.000870236079F, - 0.00275953603F, -0.000180344621F, -0.00234255777F, 0.00100793724F, - 1.58046339E-6F, 0.000848070893F, 0.00104747585F, -0.00181870302F, - -0.000267154712F, 0.00219139527F, 0.00159795943F, 0.00126773352F, - -0.00216348306F, -0.00444761245F, 0.00127053552F, 0.00459522847F, - -0.00293234223F, 0.000780453F, -0.00230323942F, 0.00115417014F, - 0.00107640924F, -0.00050292688F, -0.000303814595F, 0.00062377908F, - 0.00027997751F, 0.000294079859F, 0.00145734637F, 0.00204286887F, - -0.000422010402F, 0.000821145426F, -0.00130417733F, 0.00136583985F, - 8.89603107E-5F, -8.15962412E-5F, -0.000951912138F, -0.00233737729F, - 0.00115084287F, -0.000535019324F, -0.0009683916F, 0.00126193138F, - 0.00148631388F, 0.000690777611F, -0.00420425F, -0.00532144727F, - -0.00139551517F, 0.00107350561F, 0.00241028634F, 0.00141957507F, - 0.00243330607F, -0.00275001652F, 8.43065427E-5F, -0.00236723456F, - 0.00059447973F, -0.000465796853F, -0.000199442598F, -0.00192986289F, - -0.00233591418F, 0.00124473F, -0.00165078242F, 0.000770793471F, - -0.000936429424F, -0.000620734179F, -0.00319665694F, 0.00109424035F, - -0.000804835465F, -0.00196091412F, 0.000118225667F, -0.00156885118F, - -0.00111404504F, -0.00162095262F, -0.00158284721F, -0.00220630108F, - 0.00109551044F, -0.0015950388F, -0.00208858075F, 0.000491088955F, - 0.000854312442F, 0.00215269742F, -0.00117580255F, -0.00128002034F, - 0.0002241654F, -0.00266515207F, 0.00232118112F, 0.000463196891F, - 0.00185984198F, 0.00620778045F, -0.00117205339F, 0.00259272987F, - -0.00185524311F, -0.00135460228F, 0.0008594963F, -0.00482476223F, - 0.00165918376F, 0.000296729413F, -0.0011616461F, 0.0023346711F, - 0.00151249708F, -0.00233053858F, 0.00218114303F, -0.00012803456F, - -0.00214930787F, -0.000456104375F, 9.1383321E-5F, -0.000600432395F, - -0.00136658899F, 0.000855603779F, 4.46620252E-5F, 0.00235178345F, - -0.000264995499F, -0.00352687202F, -0.000104582556F, 0.00250476762F, - 0.00253003463F, -0.00117221347F, -7.04457707E-5F, -0.00134287798F, - -0.00189352722F, 0.000511873048F, 0.00334725762F, 0.00344597967F, - 0.00161829928F, 0.00104333705F, -0.0010358009F, 0.000718348F, -0.0024917738F, - -0.00193441834F, -0.00280496059F, 0.00144844037F, -0.00435151393F, - 0.00143389648F, 0.00326004415F, 0.00113834708F, -0.000565459F, - -0.00126948114F, 0.00211205683F, -0.000821583963F, 0.000944862433F, - -0.00269094133F, -0.000728308049F, -0.00484033115F, 0.000883979199F, - -0.00128634134F, -0.00297917356F, 0.00243260595F, 0.000671062677F, - -0.000256522209F, -0.000371919654F, -0.00235307077F, 0.00145311432F, - 0.000701958954F, -0.00215308415F, 0.00332986983F, 0.00246112584F, - 0.00109843491F, -0.00223508384F, -0.000788574165F, 0.00246457965F, - 0.00334276585F, -0.00141447433F, 0.000828126213F, -0.00243652961F, - 2.22893141E-5F, -0.000726935454F, -0.000413087342F, -3.79379489E-5F, - -0.000322716747F, -0.000385358813F, 0.0012615422F, -0.000537652F, - -0.00037149174F, -0.0028884185F, 0.000839876535F, 0.00136829447F, - -0.000397956872F, 0.00113218441F, 0.00119732379F, 0.00034437445F, - 0.0023872864F, -0.000939929916F, 0.00169434049F, 7.78144895E-5F, - 0.0025932889F, -0.000836178078F, 0.00177535147F, 0.00110522029F, - -7.363841E-5F, 0.000485807308F, -0.00183288264F, -0.00108089065F, - -0.0022221941F, 0.000598393439F, 0.00245919987F, -0.00125908712F, - -0.00169617054F, 0.00128346705F, 0.00107205554F, 0.00306015951F, - -0.000634620956F, -0.00149055477F, -0.00309630577F, 4.82520882E-5F, - 0.00169170205F, -0.00159358676F, -0.000657588127F, 0.00169494F, - -0.00276405388F, 0.00384445512F, -0.000121338933F, -0.000113363159F, - 0.00120630034F, 0.00102488406F, 0.00414983043F, 0.0018422819F, - 0.000755592831F, -0.00254635466F, 9.69017856E-5F, 0.000970572408F, - -0.00125555787F, -0.000455704139F, -0.000673777424F, -0.00128853507F, - -0.00101521297F, 0.00239398517F, 0.00166151114F, -0.00093170692F, - -0.000496828638F, -0.00176409387F, 0.00014567803F, -0.000854142359F, - -0.00478742784F, -0.00167186069F, -0.000809760648F, 0.00159224984F, - 0.00204281183F, 0.00413040398F, -0.000544354902F, 0.00202074298F, - -0.00415563164F, -0.000189240702F, -0.00138273637F, -0.00195375877F, - -0.00315740774F, 0.000209268706F, -0.00128203479F, -0.000883456843F, - -0.0028934991F, 0.00198981934F, -0.00317720813F, -0.000888979353F, - -0.00106265035F, 0.00490917498F, -0.000682450773F, 0.000619911181F, - -7.88002289E-5F, 0.00568212802F, 0.00159060757F, -0.00158745295F, - 0.000360840029F, 0.000855468854F, 0.00364152645F, 0.00104227057F, - -0.000493910164F, -0.0011891746F, 0.00183264527F, 0.00279088924F, - -0.000363007246F, -0.00357164396F, 0.00179175765F, 0.000659411424F, - 2.58535383E-5F, 0.00166096131F, 0.00237925723F, 0.000179306007F, - 0.0001104978F, 0.00435832236F, -0.0014871048F, -0.00116012059F, - -0.000964792853F, -0.0038573083F, -0.000996550778F, -0.00227289274F, - 0.000871290918F, -0.000214135216F, -0.00106255675F, -0.0029552707F, - 0.00155004254F, 0.00152294047F, 0.00271159201F, 0.00105915812F, - -0.000999802F, 0.00159273646F, -0.000861076231F, -0.00157772494F, - 0.000870608957F, -0.000230431935F, -0.00129462942F, 0.00321007986F, - 2.61410378E-6F, -0.00298673F, 0.00131429173F, -0.000200639639F, - 0.00199655257F, -0.00066791859F, -0.00350073935F, -0.00293593341F, - -0.00261393352F, -0.00393762253F, 0.00128660654F, 0.000815102481F, - 0.00136150408F, -0.00500029698F, -0.00306609855F, 0.0014726657F, - -0.000178467249F, 0.000515387801F, 0.00239263242F, 0.000801287824F, - -0.00142183725F, 0.00184052403F, 0.000125353559F, -0.00154410722F, - 0.00181933807F, 0.00199837331F, -0.00158547412F, 0.00385140348F, - 0.00182683393F, -7.29856765E-5F, 0.00459565455F, 0.000412902387F, - 0.000622455147F, -0.000778784393F, 0.00230017863F, 0.00326429214F, - 0.000115343821F, -0.00228372728F, 0.000416488736F, 0.00144944049F, - -0.000464710844F, 0.000389348919F, 0.000436950126F, -0.00342979841F, - -0.000707218423F, 0.00100047549F, 0.00318616768F, 0.000177157301F, - -0.000617010577F, 0.00181672256F, 0.00133065204F, 0.00152466714F, - 0.00182401726F, -8.5861604E-5F, 0.000744368415F, -0.00442926073F, - -0.00142895279F, -0.00201135385F, 0.00191339466F, -0.00108343933F, - -0.00270249345F, -5.94924713E-5F, -0.00431448454F, -0.000820399495F, - 0.0032497975F, -0.000749213272F, 0.000252214057F, -0.00119357731F, - 0.0012502803F, -0.0019293091F, -0.0010618103F, -0.000294363446F, - 0.00138735818F, -0.00024836915F, -0.00371840666F, -0.00144625024F, - -0.000846871582F, 0.00462928228F, 0.000135066439F, -0.00146611733F, - 0.000255161693F, 0.00203817431F, 0.00141304336F, -0.00250455784F, - 0.000234494903F, 0.00152045058F, 0.00112503103F, -0.00182517117F, - -0.00128408242F, 0.000233080704F, 0.000968575536F, 0.0021465146F, - 7.79664188E-5F, -0.00168150023F, 0.00373776187F, -0.000100507321F, - 0.000236854888F, -0.00275734137F, 0.00290792785F, -0.000507567776F, - -0.000390861416F, -0.00285345479F, -0.00191419921F, 0.000845780887F, - 0.000197901361F, 0.000379943638F, 0.00354878511F, 0.00408800738F, - 0.0013355898F, -0.00215602829F, -0.000633156276F, -2.34450563E-5F, - -0.00112936378F, 0.000120764191F, 0.000519632245F, -0.000532277161F, - -0.0033147491F, 0.00154321105F, 0.000601815758F, 0.00203035399F, - -0.000158728581F, 0.000645286171F, 0.0031033433F, -0.000858317711F, - -0.000251858437F, 0.00363865308F, -0.00184052892F, 0.00157480012F, - 0.000869901618F, -0.0035175134F, 0.00146203791F, -0.000532045786F, - -0.00317870639F, 0.00334830093F, 0.00340941665F, -0.00136266381F, - 0.00119637907F, -0.000897058519F, -0.00315099186F, -0.00215397356F, - -0.00243772636F, 0.00266974862F, -0.000163238728F, 0.00235483819F, - -0.00190768263F, -0.00150614115F, -0.000262040412F, 0.000120027267F, - 0.00261150696F, 0.000718681084F, 0.00385504239F, 0.00278014829F, - -0.000355981058F, 0.00202562753F, 0.00171482132F, -0.00177267625F, - -0.000578192936F, 0.00236535631F, -0.00206308719F, -0.00308649708F, - 0.000839344284F, 0.00108542782F, 0.00065590936F, 0.000620626612F, - 0.000119866985F, -0.00114912097F, -0.00109332742F, -0.000180567135F, - 0.00172330008F, 0.00199768809F, 0.00176731788F, -0.00119769305F, - 0.00070740527F, 0.00228031632F, -0.000415331888F, 0.000855541555F, - -0.00240982417F, -0.00141512696F, -0.00220493134F, 0.00448866794F, - 0.000853786885F, -0.00122235203F, -0.00220985571F, -0.0010949868F, - -0.000491243147F, 0.00272486266F, -0.00394560024F, -0.00407606782F, - -0.00489602843F, -0.000830105681F, 0.00203787419F, 0.0018537F, - -0.00244675297F, 0.00213529728F, 0.000464781595F, -0.00188348209F, - 0.000433254056F, 0.00390121527F, -0.00260688551F, -0.0022994082F, - 0.00171891239F, -0.00354344212F, 0.00208257535F, 0.00291705737F, - 0.000331327756F, -0.00239309436F, -0.00010933556F, -0.00154192012F, - 0.0029437819F, -0.00139422982F, -0.00283462368F, -0.000256217492F, - -0.00387088954F, 0.00358239259F, 0.00412928313F, 0.00280626724F, - 0.00120345317F, 4.6353176E-5F, -0.00064711069F, 0.00144488376F, - -0.00122008764F, 6.35526449E-5F, -0.000460657408F, -0.000571578101F, - 0.00186955451F, 0.000175775203F, -0.00264961855F, 0.000800795096F, - -0.00102047238F, 0.00112252438F, 0.000498901121F, 0.00167894026F, - -0.00166006747F, 0.000742965669F, -0.00179712672F, -0.000896385696F, - -0.00061774964F, 0.00213563256F, -0.00348768057F, 0.00314523582F, - -0.00119801902F, 0.000150360356F, 0.000139665251F, -0.000956576841F, - -0.000636363751F, 0.000376468F, 0.00122508092F, -0.000801423099F, - -0.000735554611F, 0.00103800674F, 0.00141044229F, 0.00384576712F, - -0.00320637203F, 0.00133844488F, -0.000834793958F, 0.00125138578F, - 0.00415870454F, -0.0023990483F, -0.000207407909F, 0.000248561817F, - -0.00295451307F, 0.000938306795F, -0.000476416404F, 0.0021085639F, - 0.00110013399F, -0.00328086782F, -0.000525522686F, -0.00159507908F, - -0.00187630858F, -0.00157368137F, -0.00166304572F, -0.0029479994F, - -0.000446041668F, 0.000287246541F, 0.000649801863F, -0.00206371583F, - 0.00132458401F, 0.000510419777F, -0.000454565656F, 0.00360438414F, - -0.00109407713F, -0.000109603512F, 0.00112793793F, -0.00169280346F, - 0.00152842829F, 0.00207424979F, -0.00370053132F, -0.00200583925F, - -0.00185292773F, -0.0027163846F, 0.000616547F, 0.000930190086F, - -0.00205967971F, 0.00437358581F, -1.66705286E-5F, 0.00158822781F, - 0.00227375911F, -0.000208157115F, -0.000206654033F, -0.000425300939F, - -0.000162142591F, -0.00167284894F, 0.00105193013F, -0.000517959415F, - -0.00132336351F, 0.000266980671F, -0.0015352153F, 0.0027462265F, - -0.000201984571F, 0.0029253331F, -0.00208525127F, 0.000866206654F, - -0.00618335605F, -0.00536211347F, -0.00297275721F, -0.00115421833F, - 0.00029507681F, -0.00199922407F, -3.96800569E-5F, -0.00235982868F, - 0.00166247471F, -0.0011569428F, -0.0014606755F, 0.00102137495F, - -0.00200786744F, 0.000847953896F, -0.0021456934F, 0.000707375293F, - -0.000181499447F, -0.000713039713F, -0.000993028167F, 0.000644687F, - -0.00170548994F, 6.13591692E-5F, 0.000444879697F, 0.00487654703F, - -0.000205356293F, -0.000372652779F, -0.000162783908F, 0.00247214199F, - -0.00283652032F, -0.00320523395F, 0.00270032766F, -0.00129775889F, - -0.00171746581F, -0.000977473101F, -0.00114786124F, 0.000316261343F, - 0.00412235316F, -7.86669334E-5F, 0.00131500605F, 0.00538573507F, - 0.00407111831F, 0.00185184251F, 0.00413516536F, -0.00105170766F, - -0.000483899F, -0.00110605673F, 0.000736231F, -0.00021303026F, - -0.00231583905F, 0.000613246404F, -0.00396376895F, -0.000242859562F, - -0.00223562447F, 0.000949443376F, 0.00158529624F, 0.000650667178F, - -0.00308165979F, 0.000369569199F, 0.00187325408F, 0.00133257289F, - 0.000383595267F, 0.000445798447F, 1.27618223E-5F, 0.00126648438F, - 1.61303196E-5F, 0.000717925548F, -0.00221935706F, 0.000246002048F, - -0.000225542113F, 0.00282956683F, -0.00205626711F, -0.000370979396F, - 0.0015737206F, -0.00201496063F, -0.00345242745F, 0.00117979257F, - -0.00146633433F, -0.00101086008F, -0.00243441598F, -0.00152127072F, - -0.000268781499F, 0.00435377751F, 0.000928652356F, 0.000284709298F, - -0.000332019466F, 0.00317499298F, -0.00116921682F, -0.000898489612F, - -0.00127133983F, -0.000235572166F, 0.000487818616F, -0.00201049191F, - 0.00325224036F, 0.00255928631F, -0.000921476807F, 0.00157453597F, - -0.00251803757F, -0.000490852224F, -0.00139023131F, -0.00134389102F, - 0.000315223675F, -0.00408642925F, -0.000804444775F, -0.000393995637F, - 0.00190940208F, 0.00461624144F, -0.00546247279F, 0.000299301231F, - -0.004399308F, -0.000345658016F, 0.000562637928F, 0.000815664767F, - -7.77501482E-5F, 0.00120514724F, -0.00101569947F, -0.00028306077F }, - - { -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, - -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, - -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, - -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, - 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, - 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, - -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, - -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, - 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, - 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, - -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, - -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, - 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, - 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, - -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, - -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, - 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, - 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, - -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, - -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, - 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, - 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, - -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, - -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, - 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, - 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, - -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, - -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, - 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, - 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, - 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, - -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, - -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, - -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, - 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, - 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, - -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, - -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, - -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, - 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, - 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, - -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, - -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, - 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, - 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, - -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, - -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, - 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, - 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, - -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, - -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, - 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, - 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, - -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, - -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, - 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, - 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, - -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, - -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, - 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, - 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, - -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, - -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, - 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, - 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, - 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, - -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, - -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, - -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, - 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, - 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, - -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, - -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, - -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, - 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, - 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, - -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, - -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, - 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, - 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, - -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, - -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, - 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, - 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, - -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, - -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, - 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, - 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, - -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, - -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, - 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, - 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, - -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, - -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, - 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, - 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, -3.8F, -3.45F, - -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, -2.05F, -1.7F, - -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, -0.3F, 0.05F, - 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, - 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, - 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, -4.15F, -3.8F, - -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, -2.4F, -2.05F, - -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, -0.65F, -0.3F, - -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, 1.45F, 1.45F, - 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, 3.55F, 3.55F, - 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, - -4.15F, -3.8F, -3.8F, -3.45F, -3.45F, -3.1F, -3.1F, -2.75F, -2.75F, -2.4F, - -2.4F, -2.05F, -2.05F, -1.7F, -1.7F, -1.35F, -1.35F, -1.0F, -1.0F, -0.65F, - -0.65F, -0.3F, -0.3F, 0.05F, 0.05F, 0.4F, 0.4F, 0.75F, 0.75F, 1.1F, 1.1F, - 1.45F, 1.45F, 1.8F, 1.8F, 2.15F, 2.15F, 2.5F, 2.5F, 2.85F, 2.85F, 3.2F, 3.2F, - 3.55F, 3.55F, 3.9F, 3.9F, 4.25F, 4.25F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, - 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, 4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, - -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.5F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, - -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -4.15F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, -3.8F, - -3.8F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, -3.45F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, -3.1F, - -3.1F, -3.1F, -3.1F, -3.1F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, -2.75F, - -2.75F, -2.75F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, - -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.4F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, - -2.05F, -2.05F, -2.05F, -2.05F, -2.05F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, - -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.7F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, - -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.35F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, -1.0F, - -1.0F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, -0.65F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, -0.3F, - -0.3F, -0.3F, -0.3F, -0.3F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, - 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.05F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, - 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.4F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, 0.75F, - 0.75F, 0.75F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, 1.1F, - 1.1F, 1.1F, 1.1F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, - 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.45F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, - 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 1.8F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, 2.15F, - 2.15F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, 2.5F, - 2.5F, 2.5F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, - 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 2.85F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, - 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.2F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, - 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.55F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, 3.9F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, 4.25F, - 4.25F, 4.25F, 4.25F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, - -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, - -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, - 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, - -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, - -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, - -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, - -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, - -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, - 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, - -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, - -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, - 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, - -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, - -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, - -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, - -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, -4.15F, -3.8F, - -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, - 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, - 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, - -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, - 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, -4.15F, - -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, - -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, - 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, 4.5F, -4.5F, - -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, - -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, - 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, - -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, - 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, - 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, - -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, - 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, - -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, - 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, - -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, - -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, - 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, - -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, - -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, - 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, - -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, - 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, - 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, - -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, - 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, - -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, - 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, - -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, - -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, - 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, - -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, - -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, - 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, - -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, - 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, - 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, - -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, - 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, - -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, - 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, - -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, - -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, - 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, - -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, - 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, - -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, - -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, - 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, - -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, - 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, - -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, - 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, - 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, - -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, - 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, - -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, - 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, 2.85F, 3.2F, 3.55F, 3.9F, 4.25F, - -4.5F, -4.15F, -3.8F, -3.45F, -3.1F, -2.75F, -2.4F, -2.05F, -1.7F, -1.35F, - -1.0F, -0.65F, -0.3F, 0.05F, 0.4F, 0.75F, 1.1F, 1.45F, 1.8F, 2.15F, 2.5F, - 2.85F, 3.2F, 3.55F, 3.9F, 4.25F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 100.0F, - 0.2F, - - { 0.0F, 0.0F, 1.0F }, - 0.0607967712F, - 0.1F, - 0.2F, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, -0.02276F, -2.17724109F, -2.2F, -2.2F, -2.2F, -2.2F, - -2.2F, -2.2F, 0.0F, 0.0F, 0.0F, 0.0F, 0.010346F, 0.989655F, 1.0F, 1.0F, - 1.02500105F, 2.9750011F, 3.00000095F, 3.00000095F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - -0.045518F, -0.045518F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.02069F, 0.02069F, 0.0F, 0.0F, 0.05F, 0.05F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - -0.045518F, 0.0F, 0.045518F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.02069F, 0.0F, -0.02069F, 0.0F, 0.05F, 0.0F, -0.05F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - 12.0F, - - { -0.1061F, -0.054F, -0.0064F }, - - { 0.0247F, 0.0183F, 0.0094F }, - - { 0.1177F, -0.0422F, -0.0826F }, - - { -0.1331F, 0.0509F, -0.0166F }, - - { 0.5F, -0.5F, -0.5F, 0.5F }, - - { 0.5F, 0.5F, 0.5F, 0.5F }, - - { 0.0F, -0.707107F, 0.0F, 0.707107F }, - - { 0.0F, 0.0F, 0.707107F, 0.707107F }, - - { 0.0F, 0.0F, 9.80665F }, - 1.25F, - 1.25F, - 0.0F, - 830.007F, - 829.326F, - 3000.0F, - 1800.0F, - 611.053F, - 610.213F, - 3000.0F, - 1800.0F, - 113.638F, - 86.1833F, - 224.0F, - 171.0F, - - { 1.0F, 1.0F, 1.0F }, - - { 1.0F, 1.0F, 1.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, -0.7F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { -8.265E-7F, 3.0746E-8F, 7.2916E-5F }, - 0.0F, - - { 0.0F, 0.0F, 0.0F }, - - { 1421300000U, 1421300030U, 1421300031U, 0U, 0U, 0U }, - - { 1421300000U, 1421300030U, 1421300031U, 0U, 0U, 0U }, - - { 1421300000U, 1421300030U, 1421300031U, 0U, 0U, 0U }, - - { 1421300000U, 1421300030U, 1421300031U, 0U, 0U, 0U }, - 0U, - 1421300000U, - - { 2842600000U, 2842600030U, 1421300180U, 1421300181U, 1421300182U, 1421300229U, - 1421300230U, 1421300240U, 1421300241U, 1421300280U, 1421300281U, 1421300290U, - 0U, 0U, 0U, 0U, 14U, 332179714U, 332179714U, 332179714U, 332179729U, - 332179118U, 332179134U, 332179714U }, - - { 1421300000U, 1421301000U, 1421301300U, 0U, 0U, 0U }, - - { 1421300000U, 1421301000U, 1421301300U, 0U, 0U, 0U }, - - { 1421300000U, 1421301000U, 1421301300U, 0U, 0U, 0U }, - - { 0U, 0U, 0U }, - - { 2U, 2U, 2U }, - - { 2U, 2U, 2U }, - 0U, - 1U, - 0U, - 0U, - - { 1, 1, 1 }, - - { 1, 1, 1 }, - - { 1, 1, 1 }, - - { 1, 1, 1 }, - 0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.14814814814814811, - 0.14814814814814811, - 0.009259259259259257, - 0.0F, - 0.001F, - 1.26247478F, - 1.26247478F, - 0.0F, - 0.0F, - 1000000000U, - 0U, - 0U, - - { - 0U, // bpm_timestamp_sec - 0U, // bpm_timestamp_nsec - - { - 0.0F, 0.0F } - , // bpm_motor_curr - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // bpm_servo_curr - - { - 0.0F, 0.0F, 0.0F } - , // bpm_torque_B - - { - 0.0F, 0.0F, 0.0F } - , // bpm_force_B - - { - 0.0F, 0.0F } - , // bpm_motor_speed - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - // bpm_nozzle_theta - }, - -1.0, - 0.016, - 0.5, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0 }, - 0.016, - 0.5, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - -1.0, - -1.0, - -1.0, - -1.0, - - { 0.0, 0.0, 0.0, 1.0 }, - 2.0, - 1.0, - 2.0, - 0.0, - -1.0, - -1.0, - -1.0, - 2.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 2.0, - 1.0, - 2.0, - 0.0, - -1.0, - -1.0, - -1.0, - 2.0, - 6.2831853071795862, - 0.0, - 0.00066666666666666664, - 25.0, - 0.016, - 0.016, - 0.0, - 0.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - -1.0, - -1.0, - -1.0, - -1.0, - - { 0.0, 0.0, 0.0, 1.0 }, - 2.0, - 1.0, - 2.0, - 0.0, - -1.0, - -1.0, - -1.0, - 2.0, - 6.2831853071795862, - 0.0, - 2.9088820866572161E-5, - 25.0, - 0.016, - 0.016, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 1.0, - 0.0, - 1.0, - 1.0, - 0.0, - 0.0, - 1.0, - 1.0, - 0.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - 0.0, - 2.9237781375397746E-5, - 0.0, - 0.0014709974999999999, - 0.0, - 1.4082564847130597E-6, - 0.0, - 3.1997702953229377E-5, - - { 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F }, - - { 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F }, - 0.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 0.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - 0.0F, - 0.0F, - 0.016F, - 0.016F, - 0.016F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 0.016F, - 0.0F, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 0.016F, - 0.0F, - - { 0.0F, 0.0F, 0.0F }, - -1.0F, - -1.0F, - -1.0F, - - { 0.0F, 0.0F, 0.0F }, - 25.0F, - 14.0F, - 0.016F, - 14.0F, - 0.016F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 0.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 0.0F, - 0.0F, - 0U, - 0U, - - { 2147483648U, 2147483648U }, - 1U, - 1000000000U, - 1U, - 1U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 1U, - 1U, - 1U, - 1U, - 1U, - 0U, - 0U, - 0U, - 0U, - 0U, - 1U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 12U, - 1U, - 3U, - 1U, - 3U, - 1U, - 3U, - 1U, - 1U, - 3U, - 1U, - 1U, - 3U, - 1U, - 1U, - 3U, - 1U, - 1U, - 3U, - 1U, - 1, - 1, - - // Start of '/servo_model' - { - -6.0F, - 0.016F, - 0.0F, - 0.016F, - 0.0F, - 0.016F, - 0.0F, - 0.016F, - 0.0F, - 0.0F, - 0.0F - } - // End of '/servo_model' - , - - // Start of '/speed_controller' - { - -16.6F, - -1.0F, - 0.0F, - 0.335103214F, - -0.335103214F, - 0.0F, - 0.016F, - 0.0F, - 0.016F, - 0.0F, - 0.0F, - 0.0F - } - // End of '/speed_controller' - , - - // Start of '/latch_nozzle_thrust_matricies' - { - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - 0.0F, - 0.0F, - 0.0F, - - // Start of '/CoreSubsys' - { - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F - } - // End of '/CoreSubsys' - } - // End of '/latch_nozzle_thrust_matricies' - , - - // Start of '/blower_aerodynamics' - { - 0.016, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 2.0F, - 0.0F - } - // End of '/blower_aerodynamics' - , - - // Start of '/servo_model' - { - -6.0F, - 0.016F, - 0.0F, - 0.016F, - 0.0F, - 0.016F, - 0.0F, - 0.016F, - 0.0F, - 0.0F, - 0.0F - } - // End of '/servo_model' - , - - // Start of '/speed_controller' - { - -16.6F, - -1.0F, - 0.0F, - 0.335103214F, - -0.335103214F, - 0.0F, - 0.016F, - 0.0F, - 0.016F, - 0.0F, - 0.0F, - 0.0F - } - // End of '/speed_controller' - , - - // Start of '/latch_nozzle_thrust_matricies' - { - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - 0.0F, - 0.0F, - 0.0F, - - // Start of '/CoreSubsys' - { - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F - } - // End of '/CoreSubsys' - } - // End of '/latch_nozzle_thrust_matricies' - , - - // Start of '/blower_aerodynamics' - { - 0.016, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 2.0F, - 0.0F - } - // End of '/blower_aerodynamics' - , - - // Start of '/pinhole_projection_model' - { - // Start of '/CoreSubsys' - { - 0.0, - 0.0, - - { 0.0F, 0.0F }, - 2.22044605E-16F, - 0.0F - } - // End of '/CoreSubsys' - } - // End of '/pinhole_projection_model' - , - - // Start of '/pinhole_projection_model' - { - // Start of '/CoreSubsys' - { - 0.0, - 0.0, - - { 0.0F, 0.0F }, - 2.22044605E-16F, - 0.0F - } - // End of '/CoreSubsys' - } - // End of '/pinhole_projection_model' - , - - // Start of '/pinhole_projection_model' - { - // Start of '/CoreSubsys' - { - 0.0, - 0.0, - - { 0.0F, 0.0F }, - 2.22044605E-16F, - 0.0F - } - // End of '/CoreSubsys' - } - // End of '/pinhole_projection_model' - , - - // Start of '/pinhole_projection_model' - { - // Start of '/CoreSubsys' - { - 0.0, - 0.0, - - { 0.0F, 0.0F }, - 2.22044605E-16F, - 0.0F - } - // End of '/CoreSubsys' - } - // End of '/pinhole_projection_model' -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_private.h b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_private.h deleted file mode 100644 index 6e83a7cb29..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_private.h +++ /dev/null @@ -1,112 +0,0 @@ -// -// File: sim_model_lib0_private.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:45:47 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_sim_model_lib0_private_h_ -#define RTW_HEADER_sim_model_lib0_private_h_ -#include "rtwtypes.h" -#include "sim_model_lib0.h" -#if !defined(rt_VALIDATE_MEMORY) -#define rt_VALIDATE_MEMORY(S, ptr) if(!(ptr)) {\ - rtmSetErrorStatus(sim_model_lib0_M, RT_MEMORY_ALLOCATION_ERROR);\ - } -#endif - -#if !defined(rt_FREE) -#if !defined(_WIN32) -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((ptr));\ - (ptr) = (NULL);\ - } -#else - -// Visual and other windows compilers declare free without const -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((void *)(ptr));\ - (ptr) = (NULL);\ - } -#endif -#endif - -extern const real_T rtCP_pooled_15LYgUQHWYtd[6]; - -#define rtCP_pooled1 rtCP_pooled_15LYgUQHWYtd // Pooled Parameter (Expression: noz_randn_seed +[1:6]) - // Referenced by: - // '/random_noise' - // '/random_noise' - - -void sim_model_lib0_Normalize(const real32_T rtu_Vec[4], real32_T rtu_Magnitude, - real32_T rty_Normalized_Vec[4]); -void sim_model_lib0_Normalize_e(const real32_T rtu_Vec[3], real32_T - rtu_Magnitude, real32_T rty_Normalized_Vec[3]); -void pinhole_projection_mod_Init(int32_T NumIters, - DW_pinhole_projection_model_s_T localDW[], P_pinhole_projection_model_si_T - *localP, real_T rtp_pixel_noise_var, real_T rtp_pixel_noise_seed); -void pinhole_projection_mo_Start(int32_T NumIters, - DW_pinhole_projection_model_s_T localDW[]); -void si_pinhole_projection_model(int32_T NumIters, const real32_T - rtu_P_points_cam_cam[], real32_T rty_P_points_2D_cam[], boolean_T - rty_points_in_FOV_flag[], DW_pinhole_projection_model_s_T localDW[], - P_pinhole_projection_model_si_T *localP, uint8_T rtp_noise_on_flag, real32_T - rtp_cam_focal_length_Y, real32_T rtp_cam_focal_length_X, real_T - rtp_pixel_noise_var, real32_T rtp_cam_num_X_pixels, real32_T - rtp_cam_num_Y_pixels, real32_T rtp_cam_min_dist, real32_T rtp_cam_max_dist, - const real32_T rtp_cam_pointing[3]); -void latch_nozzle_thrust_ma_Init(B_latch_nozzle_thrust_matrici_T *localB, - P_latch_nozzle_thrust_matrici_T *localP); -void latch_nozzle_thrust_matrici(boolean_T rtu_Enable, const real32_T - rtu_veh_cm[3], B_latch_nozzle_thrust_matrici_T *localB, - P_latch_nozzle_thrust_matrici_T *localP, const real32_T - rtp_nozzle_orientation_offset[24], const real32_T rtp_P_nozzle_B_B[18], const - real32_T rtp_nozzle_orientation_B[18], const real32_T rtp_P_nozzle_B_B_offset - [18], const real_T rtp_P_CG_B_B_error[3], real32_T rtp_noise_on_flag); -void sim_model__calc_nozzle_area(const real32_T rtu_servo_theta[6], - B_calc_nozzle_area_sim_model__T *localB, real32_T rtp_noz_flap_count, real32_T - rtp_noz_min_theta, real32_T rtp_noz_flap_length, real32_T - rtp_noz_intake_height, const real32_T rtp_noz_width[6], real32_T - rtp_noz_gear_ratio); -void sim_model_li_dc_motor_model(real32_T rtu_ctl_voltage, real32_T - rtu_curr_rotor_speed, B_dc_motor_model_sim_model_li_T *localB, real32_T - rtp_motor_kn, real32_T rtp_motor_r, real32_T rtp_motor_kt, real32_T - rtp_motor_friction_coeff); -void sim_m_speed_controller_Init(DW_speed_controller_sim_model_T *localDW, - P_speed_controller_sim_model__T *localP); -void sim_model__speed_controller(real32_T rtu_battery_voltage, uint8_T - rtu_speed_cmd, real32_T rtu_speed_curr, B_speed_controller_sim_model__T - *localB, DW_speed_controller_sim_model_T *localDW, - P_speed_controller_sim_model__T *localP, real32_T rtp_ctl_pwm2speed, real32_T - rtp_ctl_speed_filt_num, real32_T rtp_ctl_speed_cilt_den, real32_T rtp_ctl_kp, - real32_T rtp_ctl_kd, real32_T rtp_ctl_filt_n, real32_T rtp_ctl_max_voltage, - real32_T rtp_ctl_ki); -void sim_model__servo_model_Init(DW_servo_model_sim_model_lib0_T *localDW, - P_servo_model_sim_model_lib0_T *localP, real32_T rtp_servo_min_theta, real32_T - rtp_motor_gear_ratio); -void sim_model_lib0_servo_model(const real32_T rtu_command_PWM[6], - B_servo_model_sim_model_lib0_T *localB, DW_servo_model_sim_model_lib0_T - *localDW, P_servo_model_sim_model_lib0_T *localP, real32_T - rtp_servo_pwm2angle_bias, real32_T rtp_servo_pwm2angle, real32_T - rtp_servo_min_theta, real32_T rtp_motor_gear_ratio, real32_T - rtp_servo_max_theta, real32_T rtp_motor_backlash_deadband, real32_T - rtp_servo_ctl_kp, real32_T rtp_servo_ctl_kd, real32_T rtp_servo_ctl_filt_n, - real32_T rtp_servo_max_voltage, real32_T rtp_motor_k, real32_T rtp_motor_r, - real32_T rtp_motor_friction_coeff, real32_T rtp_motor_gear_box_inertia, - real32_T rtp_servo_ctl_ki); - -#endif // RTW_HEADER_sim_model_lib0_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_ref.rsp b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_ref.rsp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_types.h b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_types.h deleted file mode 100644 index 101ad43c34..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_types.h +++ /dev/null @@ -1,223 +0,0 @@ -// -// File: sim_model_lib0_types.h -// -// Code generated for Simulink model 'sim_model_lib0'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:45:47 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_sim_model_lib0_types_h_ -#define RTW_HEADER_sim_model_lib0_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_act_msg_ -#define DEFINED_TYPEDEF_FOR_act_msg_ - -typedef struct { - uint32_T act_timestamp_sec; - uint32_T act_timestamp_nsec; - uint8_T act_impeller_speed_cmd[2]; - real32_T act_servo_pwm_cmd[12]; - real32_T act_nozzle_theta[12]; - real32_T act_predicted_force_B[3]; - real32_T act_predicted_torque_B[3]; -} act_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmc_state_cmd_ -#define DEFINED_TYPEDEF_FOR_cmc_state_cmd_ - -typedef struct { - uint32_T timestamp_sec; - uint32_T timestamp_nsec; - real32_T P_B_ISS_ISS[3]; - real32_T V_B_ISS_ISS[3]; - real32_T A_B_ISS_ISS[3]; - real32_T quat_ISS2B[4]; - real32_T omega_B_ISS_B[3]; - real32_T alpha_B_ISS_B[3]; -} cmc_state_cmd; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmc_msg_ -#define DEFINED_TYPEDEF_FOR_cmc_msg_ - -typedef struct { - cmc_state_cmd cmc_state_cmd_a; - cmc_state_cmd cmc_state_cmd_b; - uint8_T cmc_mode_cmd; - uint8_T speed_gain_cmd; - uint8_T localization_mode_cmd; - real32_T att_kp[3]; - real32_T att_ki[3]; - real32_T omega_kd[3]; - real32_T pos_kp[3]; - real32_T pos_ki[3]; - real32_T vel_kd[3]; - real32_T center_of_mass[3]; - real32_T inertia_matrix[9]; - real32_T mass; -} cmc_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_env_msg_ -#define DEFINED_TYPEDEF_FOR_env_msg_ - -typedef struct { - real32_T P_B_ISS_ISS[3]; - real32_T V_B_ISS_ISS[3]; - real32_T A_B_ISS_ISS[3]; - real32_T A_B_ISS_B[3]; - real32_T A_B_ECI_B[3]; - real32_T Q_ISS2B[4]; - real32_T omega_B_ISS_B[3]; - real32_T omega_B_ECI_B[3]; - real32_T alpha_B_ISS_B[3]; - real32_T fan_torques_B[3]; - real32_T fan_forces_B[3]; -} env_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ex_time_msg_ -#define DEFINED_TYPEDEF_FOR_ex_time_msg_ - -typedef struct { - uint32_T timestamp_sec; - uint32_T timestamp_nsec; -} ex_time_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_landmark_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_landmark_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_landmarks[150]; - real32_T cvs_observations[100]; - uint8_T cvs_valid_flag[50]; -} cvs_landmark_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_optical_flow_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_optical_flow_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_observations[1600]; - uint8_T cvs_valid_flag[800]; - real32_T cvs_id_tag[50]; -} cvs_optical_flow_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_handrail_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_handrail_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_landmarks[150]; - real32_T cvs_observations[150]; - uint8_T cvs_valid_flag[50]; - uint8_T cvs_3d_knowledge_flag; - real32_T cvs_handrail_local_pos[3]; - real32_T cvs_handrail_local_quat[4]; - uint8_T cvs_handrail_update_global_pose_flag; -} cvs_handrail_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_registration_pulse_ -#define DEFINED_TYPEDEF_FOR_cvs_registration_pulse_ - -typedef struct { - uint8_T cvs_ar_tag_pulse; - uint8_T cvs_landmark_pulse; - uint8_T cvs_optical_flow_pulse; - uint8_T cvs_handrail_pulse; -} cvs_registration_pulse; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_bpm_msg_ -#define DEFINED_TYPEDEF_FOR_bpm_msg_ - -typedef struct { - uint32_T bpm_timestamp_sec; - uint32_T bpm_timestamp_nsec; - real32_T bpm_motor_curr[2]; - real32_T bpm_servo_curr[12]; - real32_T bpm_torque_B[3]; - real32_T bpm_force_B[3]; - real32_T bpm_motor_speed[2]; - real32_T bpm_nozzle_theta[12]; -} bpm_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_imu_msg_ -#define DEFINED_TYPEDEF_FOR_imu_msg_ - -typedef struct { - uint32_T imu_timestamp_sec; - uint32_T imu_timestamp_nsec; - real32_T imu_A_B_ECI_sensor[3]; - real32_T imu_accel_bias[3]; - real32_T imu_omega_B_ECI_sensor[3]; - real32_T imu_gyro_bias[3]; - uint8_T imu_validity_flag; - uint8_T imu_sat_flag; -} imu_msg; - -#endif - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_sim_model_lib0_T_ P_CoreSubsys_sim_model_lib0_T; - -// Parameters for system: '/pinhole_projection_model' -typedef struct P_pinhole_projection_model_si_T_ P_pinhole_projection_model_si_T; - -// Parameters for system: '/blower_aerodynamics' -typedef struct P_blower_aerodynamics_sim_mod_T_ P_blower_aerodynamics_sim_mod_T; - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_sim_model_lib0_a_T_ P_CoreSubsys_sim_model_lib0_a_T; - -// Parameters for system: '/latch_nozzle_thrust_matricies' -typedef struct P_latch_nozzle_thrust_matrici_T_ P_latch_nozzle_thrust_matrici_T; - -// Parameters for system: '/speed_controller' -typedef struct P_speed_controller_sim_model__T_ P_speed_controller_sim_model__T; - -// Parameters for system: '/servo_model' -typedef struct P_servo_model_sim_model_lib0_T_ P_servo_model_sim_model_lib0_T; - -// Parameters for system: '/blower_aerodynamics' -typedef struct P_blower_aerodynamics_sim_m_m_T_ P_blower_aerodynamics_sim_m_m_T; - -// Parameters (auto storage) -typedef struct P_sim_model_lib0_T_ P_sim_model_lib0_T; - -// Forward declaration for rtModel -typedef struct tag_RTM_sim_model_lib0_T RT_MODEL_sim_model_lib0_T; - -#endif // RTW_HEADER_sim_model_lib0_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_tunable_funcs.cpp b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_tunable_funcs.cpp deleted file mode 100644 index 48d735920c..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_tunable_funcs.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Autogenerated file, see write_tunable_param_functions for details - -#include "sim_tunable_funcs.h" -#include -#include -#include - -// Do config setup and add includes -void sim_ReadParams(config_reader::ConfigReader* config, RT_MODEL_sim_model_lib0_T* sim) { - - auto & p = sim->defaultParam; - - if (!msg_conversions::config_read_array(config, "tun_abp_p_dockcam_body_body_sim", 3 , p->tun_abp_p_dockcam_body_body_sim)) - ROS_FATAL("Unspecified tun_abp_p_dockcam_body_body_sim."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_imu_body_body", 3 , p->tun_abp_p_imu_body_body)) - ROS_FATAL("Unspecified tun_abp_p_imu_body_body."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_navcam_body_body_sim", 3 , p->tun_abp_p_navcam_body_body_sim)) - ROS_FATAL("Unspecified tun_abp_p_navcam_body_body_sim."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_perchcam_body_body_sim", 3 , p->tun_abp_p_perchcam_body_body_sim)) - ROS_FATAL("Unspecified tun_abp_p_perchcam_body_body_sim."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2dockcam", 4 , p->tun_abp_q_body2dockcam)) - ROS_FATAL("Unspecified tun_abp_q_body2dockcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2navcam", 4 , p->tun_abp_q_body2navcam)) - ROS_FATAL("Unspecified tun_abp_q_body2navcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2perchcam", 4 , p->tun_abp_q_body2perchcam)) - ROS_FATAL("Unspecified tun_abp_q_body2perchcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_quat_body2imu", 4 , p->tun_abp_quat_body2imu)) - ROS_FATAL("Unspecified tun_abp_quat_body2imu."); - if (!msg_conversions::config_read_array(config, "tun_ase_gravity_accel", 3 , p->tun_ase_gravity_accel)) - ROS_FATAL("Unspecified tun_ase_gravity_accel."); - if (!config->GetReal("tun_bpm_PM1_thrust_error_sf", &p->tun_bpm_PM1_thrust_error_sf)) - ROS_FATAL("Unspecified tun_bpm_PM1_thrust_error_sf."); - if (!config->GetReal("tun_bpm_PM2_thrust_error_sf", &p->tun_bpm_PM2_thrust_error_sf)) - ROS_FATAL("Unspecified tun_bpm_PM2_thrust_error_sf."); - if (!config->GetReal("tun_bpm_noise_on_flag", &p->tun_bpm_noise_on_flag)) - ROS_FATAL("Unspecified tun_bpm_noise_on_flag."); - if (!config->GetReal("tun_cvs_dockcam_focal_length_X", &p->tun_cvs_dockcam_focal_length_X)) - ROS_FATAL("Unspecified tun_cvs_dockcam_focal_length_X."); - if (!config->GetReal("tun_cvs_dockcam_focal_length_Y", &p->tun_cvs_dockcam_focal_length_Y)) - ROS_FATAL("Unspecified tun_cvs_dockcam_focal_length_Y."); - if (!config->GetReal("tun_cvs_dockcam_num_pixels_X", &p->tun_cvs_dockcam_num_pixels_X)) - ROS_FATAL("Unspecified tun_cvs_dockcam_num_pixels_X."); - if (!config->GetReal("tun_cvs_dockcam_num_pixels_Y", &p->tun_cvs_dockcam_num_pixels_Y)) - ROS_FATAL("Unspecified tun_cvs_dockcam_num_pixels_Y."); - if (!config->GetReal("tun_cvs_navcam_focal_length_X", &p->tun_cvs_navcam_focal_length_X)) - ROS_FATAL("Unspecified tun_cvs_navcam_focal_length_X."); - if (!config->GetReal("tun_cvs_navcam_focal_length_Y", &p->tun_cvs_navcam_focal_length_Y)) - ROS_FATAL("Unspecified tun_cvs_navcam_focal_length_Y."); - if (!config->GetReal("tun_cvs_navcam_num_pixels_X", &p->tun_cvs_navcam_num_pixels_X)) - ROS_FATAL("Unspecified tun_cvs_navcam_num_pixels_X."); - if (!config->GetReal("tun_cvs_navcam_num_pixels_Y", &p->tun_cvs_navcam_num_pixels_Y)) - ROS_FATAL("Unspecified tun_cvs_navcam_num_pixels_Y."); - if (!config->GetReal("tun_cvs_perchcam_focal_length_X", &p->tun_cvs_perchcam_focal_length_X)) - ROS_FATAL("Unspecified tun_cvs_perchcam_focal_length_X."); - if (!config->GetReal("tun_cvs_perchcam_focal_length_Y", &p->tun_cvs_perchcam_focal_length_Y)) - ROS_FATAL("Unspecified tun_cvs_perchcam_focal_length_Y."); - if (!config->GetReal("tun_cvs_perchcam_num_pixels_X", &p->tun_cvs_perchcam_num_pixels_X)) - ROS_FATAL("Unspecified tun_cvs_perchcam_num_pixels_X."); - if (!config->GetReal("tun_cvs_perchcam_num_pixels_Y", &p->tun_cvs_perchcam_num_pixels_Y)) - ROS_FATAL("Unspecified tun_cvs_perchcam_num_pixels_Y."); - if (!msg_conversions::config_read_array(config, "tun_env_accel_dof_gain", 3 , p->tun_env_accel_dof_gain)) - ROS_FATAL("Unspecified tun_env_accel_dof_gain."); - if (!msg_conversions::config_read_array(config, "tun_env_alpha_dof_gain", 3 , p->tun_env_alpha_dof_gain)) - ROS_FATAL("Unspecified tun_env_alpha_dof_gain."); - if (!msg_conversions::config_read_matrix(config, "tun_inertia_error_mat", 3, 3, p->tun_inertia_error_mat)) - ROS_FATAL("Unspecified tun_inertia_error_mat."); - if (!msg_conversions::config_read_array(config, "tun_ini_P_B_ISS_ISS", 3 , p->tun_ini_P_B_ISS_ISS)) - ROS_FATAL("Unspecified tun_ini_P_B_ISS_ISS."); - if (!msg_conversions::config_read_array(config, "tun_ini_Q_ISS2B", 4 , p->tun_ini_Q_ISS2B)) - ROS_FATAL("Unspecified tun_ini_Q_ISS2B."); - if (!msg_conversions::config_read_array(config, "tun_ini_V_B_ISS_ISS", 3 , p->tun_ini_V_B_ISS_ISS)) - ROS_FATAL("Unspecified tun_ini_V_B_ISS_ISS."); - if (!msg_conversions::config_read_array(config, "tun_ini_omega_B_ISS_B", 3 , p->tun_ini_omega_B_ISS_B)) - ROS_FATAL("Unspecified tun_ini_omega_B_ISS_B."); - if (!msg_conversions::config_read_array(config, "tun_iss_omega_ISS_ECI_ISS", 3 , p->tun_iss_omega_ISS_ECI_ISS)) - ROS_FATAL("Unspecified tun_iss_omega_ISS_ECI_ISS."); - if (!config->GetReal("tun_mass_error", &p->tun_mass_error)) - ROS_FATAL("Unspecified tun_mass_error."); - if (!msg_conversions::config_read_array(config, "tun_sim_cg_error", 3 , p->tun_sim_cg_error)) - ROS_FATAL("Unspecified tun_sim_cg_error."); - bool ase_gravity_removal; - if (!config->GetBool("tun_ase_gravity_removal", &ase_gravity_removal)) - ROS_FATAL("Unspecified tun_ase_gravity_removal."); - p->tun_ase_gravity_removal = ase_gravity_removal; - bool cvs_noise_on; - if (!config->GetBool("tun_cvs_noise_on", &cvs_noise_on)) - ROS_FATAL("Unspecified tun_cvs_noise_on."); - p->tun_cvs_noise_on = cvs_noise_on; - bool env_drag_disturb_on; - if (!config->GetBool("tun_env_drag_disturb_on", &env_drag_disturb_on)) - ROS_FATAL("Unspecified tun_env_drag_disturb_on."); - p->tun_env_drag_disturb_on = env_drag_disturb_on; - bool epson_report_truth; - if (!config->GetBool("tun_epson_report_truth", &epson_report_truth)) - ROS_FATAL("Unspecified tun_epson_report_truth."); - p->tun_epson_report_truth = epson_report_truth; -} diff --git a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_tunable_funcs.h b/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_tunable_funcs.h deleted file mode 100644 index 68bdc17711..0000000000 --- a/gnc/matlab/code_generation/sim_model_lib0_ert_rtw/sim_tunable_funcs.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef READ_PARAMS_CPP -#define READ_PARAMS_CPP - -namespace config_reader { - class ConfigReader; -} - -#include "sim_model_lib0.h" -void sim_ReadParams(config_reader::ConfigReader* config, RT_MODEL_sim_model_lib0_T* sim); - -#endif \ No newline at end of file diff --git a/gnc/matlab/cxx_functions/cxx_blocks_config.m b/gnc/matlab/cxx_functions/cxx_blocks_config.m deleted file mode 100644 index bee84fd682..0000000000 --- a/gnc/matlab/cxx_functions/cxx_blocks_config.m +++ /dev/null @@ -1,112 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%%generate the mex files used by simulink for the hand coded C++ files listed below -% This file is automated to check the checksums and compile if they have changed - -%setup variables -function cxx_blocks_config(ASTROBEE_ROOT, ase_of_num_aug, ase_of_num_features, ab_verbose) - -if ispc - cxx_blocks_config_windows(ASTROBEE_ROOT, ase_of_num_aug, ase_of_num_features, ab_verbose); -else - - file_names = {'apply_delta_state', 'compute_delta_state_and_cov', 'matrix_multiply', 'of_residual_and_h'}; - func_dec1 = ['void apply_delta_state(single u1[], int32 size(u1, 1), uint16 u2, single u3[4], single u4[3], single u5[3], single u6[3], single u7[3], ' ... - 'single u8[4], single u9[3], uint16 u10, single u11[' num2str(ase_of_num_aug.Value) '][4], single u12[' num2str(ase_of_num_aug.Value) ... - '][3], single y1[4], single y2[3], single y3[3], single y4[3], single y5[3], single y6[4], single y7[3], uint16 y8[1], single y9[' ... - num2str(ase_of_num_aug.Value) '][4], single y10[' num2str(ase_of_num_aug.Value) '][3])']; - func_dec2 = ['int32 y1 = compute_delta_state_and_cov(single u1[], int32 u2, single u3[][], int32 size(u3, 1), int32 size(u3, 2), ' ... - 'uint32 u4, single u5[][], single u6[][], single y2[size(u3, 2)], single y3[size(u3, 2)][size(u3, 2)])']; - func_dec3 = 'void matrix_multiply(single u1[][], int32 size(u1, 1), int32 size(u1, 2), single u2[][], int32 size(u2, 1), int32 size(u2, 2), single y1[size(u1, 1)][size(u2, 2)])'; - func_dec4 = ['int32 y1 = of_residual_and_h(single u1[' num2str(2*ase_of_num_aug.Value*ase_of_num_features.Value) '], single u2[4][' ... - num2str(ase_of_num_features.Value) '], single u3[' num2str(4 * ase_of_num_aug.Value) '][4], int32 u4[' num2str(ase_of_num_features.Value) ... - '][' num2str(ase_of_num_aug.Value) '], int32 u5, int32 p1, int32 p2, single p3, single p4, single p5, single p6, single u6[' num2str(21 + 6 * ase_of_num_aug.Value) ... - '][' num2str(21 + 6 * ase_of_num_aug.Value) '], single y2[', num2str(6 * ase_of_num_aug.Value) '], single y3[' num2str(6 * ase_of_num_aug.Value) ... - '][' num2str(21 + 6 * ase_of_num_aug.Value) '], uint32 y4[1], uint8 y5[1], single y6[' num2str(ase_of_num_features.Value) '], single y7[' num2str(6*ase_of_num_aug.Value) '][' num2str(6 * ase_of_num_aug.Value) '])']; - function_declarations = {func_dec1, func_dec2, func_dec3, func_dec4}; - - - - % Find the eigen search path - eigen_search_path = {'/usr/include/eigen3/', '../../../../../local/eigen3/', '../../../local/eigen3/', [ASTROBEE_ROOT '/../../../../../local/eigen3/']}; % Modify this to add more eigen paths - eigen_search_path_idx = find(cellfun(@(x) ~isempty(dir(x)), eigen_search_path), 1, 'first'); - eigen_path = eigen_search_path{eigen_search_path_idx}; - - - cd(fullfile(ASTROBEE_ROOT)); - %check if the checksum file exists, if not, create it. - if exist('file_checksums.mat', 'file') - load('file_checksums.mat'); - else - file_checksums.exists = 1; - if(ab_verbose) - disp('.....file_checksums.mat did not exist, creating'); - end - end - - - for i = 1:4 - %determine checksums of current '.h' and '.cpp' files - file_cpp_checksum = Simulink.getFileChecksum(which([file_names{i} '.cpp'])); - file_h_checksum = Simulink.getFileChecksum(which([file_names{i} '.h'])); - - %check if files already exist. Note: exist==3 impiles a mex file exists - if(exist(['ex_' file_names{i} '.cpp'], 'file') && exist(['ex_' file_names{i}], 'file')==3 && exist(['ex_' file_names{i} '.tlc'], 'file')) - %check if checksum even contains correct files - if(all(isfield(file_checksums,{[file_names{i} 'cpp'], [file_names{i} 'h']}))) - %check if checksums are correct - if(strcmp(file_cpp_checksum,file_checksums.([file_names{i} 'cpp'])) && strcmp(file_h_checksum,file_checksums.([file_names{i} 'h']))) - continue; %skip if they are equal! - end - end - end - - if(ab_verbose) - disp(['.....Building ' file_names{i}]); - end - - %if any are false, then generate the code - def = legacy_code('initialize'); % initialize def variable - def.SourceFiles = {[file_names{i} '.cpp']}; % Source file for block - def.HeaderFiles = {[file_names{i} '.h']}; % Header file for block - def.SFunctionName = ['ex_' file_names{i}]; % sfunction name - def.OutputFcnSpec = function_declarations{i}; % See this help file: http://www.mathworks.com/help/simulink/sfg/integrating-existing-c-functions-into-simulink-models-with-the-legacy-code-tool.html#bq4g1es-6 - def.Options.language = 'C++'; % Language - % def.SrcPaths = {'./src', './../../external/eigen/Eigen/src'}; % Can add multiple sources as cell arrays of strings - % def.IncPaths = {'./include', './../../external/eigen/Eigen'}; - def.SrcPaths = {'./cxx_functions/src', fullfile(eigen_path, 'Eigen', 'src')}; % Can add multiple sources as cell arrays of strings - def.IncPaths = {'./cxx_functions/include', eigen_path}; - def.Options.singleCPPMexFile = false; % Option need for code generation - def.Options.supportsMultipleExecInstances = true; - - legacy_code('sfcn_tlc_generate', def); - legacy_code('sfcn_cmex_generate', def); - legacy_code('compile', def); - legacy_code('rtwmakecfg_generate', def); - %legacy_code('slblock_generate', def); - - %and store the checksums - file_checksums.([file_names{i} 'h']) = file_h_checksum; - file_checksums.([file_names{i} 'cpp']) = file_cpp_checksum; - end - - %save the checksums file - save('file_checksums.mat', 'file_checksums'); - cd(ASTROBEE_ROOT); -end -return diff --git a/gnc/matlab/cxx_functions/cxx_blocks_config_windows.m b/gnc/matlab/cxx_functions/cxx_blocks_config_windows.m deleted file mode 100644 index 6b97b5aa15..0000000000 --- a/gnc/matlab/cxx_functions/cxx_blocks_config_windows.m +++ /dev/null @@ -1,121 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%%generate the mex files used by simulink for the hand coded C++ files listed below -% This file is automated to check the checksums and compile if they have changed - -%setup variables -function cxx_blocks_config_windows(ASTROBEE_ROOT, ase_of_num_aug, ase_of_num_features, ab_verbose) -file_names = {'apply_delta_state', 'compute_delta_state_and_cov', 'matrix_multiply', 'of_residual_and_h'}; -func_dec1 = ['void apply_delta_state(single u1[], int32 size(u1, 1), uint16 u2, single u3[4], single u4[3], single u5[3], single u6[3], single u7[3], ' ... - 'single u8[4], single u9[3], uint16 u10, single u11[' num2str(ase_of_num_aug.Value) '][4], single u12[' num2str(ase_of_num_aug.Value) ... - '][3], single y1[4], single y2[3], single y3[3], single y4[3], single y5[3], single y6[4], single y7[3], uint16 y8[1], single y9[' ... - num2str(ase_of_num_aug.Value) '][4], single y10[' num2str(ase_of_num_aug.Value) '][3])']; -func_dec2 = ['int32 y1 = compute_delta_state_and_cov(single u1[], int32 u2, single u3[][], int32 size(u3, 1), int32 size(u3, 2), ' ... - 'uint32 u4, single u5[][], single u6[][], single y2[size(u3, 2)], single y3[size(u3, 2)][size(u3, 2)])']; -func_dec3 = 'void matrix_multiply(single u1[][], int32 size(u1, 1), int32 size(u1, 2), single u2[][], int32 size(u2, 1), int32 size(u2, 2), single y1[size(u1, 1)][size(u2, 2)])'; -func_dec4 = ['int32 y1 = of_residual_and_h(single u1[' num2str(2*ase_of_num_aug.Value*ase_of_num_features.Value) '], single u2[4][' ... - num2str(ase_of_num_features.Value) '], single u3[' num2str(4 * ase_of_num_aug.Value) '][4], int32 u4[' num2str(ase_of_num_features.Value) ... - '][' num2str(ase_of_num_aug.Value) '], int32 u5, int32 p1, int32 p2, single p3, single p4, single u6[' num2str(21 + 6 * ase_of_num_aug.Value) ... - '][' num2str(21 + 6 * ase_of_num_aug.Value) '], single y2[', num2str(6 * ase_of_num_aug.Value) '], single y3[' num2str(6 * ase_of_num_aug.Value) ... - '][' num2str(21 + 6 * ase_of_num_aug.Value) '], uint32 y4[1], uint8 y5[1], single y6[' num2str(ase_of_num_features.Value) '])']; -function_declarations = {func_dec1, func_dec2, func_dec3, func_dec4}; - -cd(fullfile(ASTROBEE_ROOT, 'cxx_functions')); -%check if the checksum file exists, if not, create it. -if exist('file_checksums.mat', 'file') - load('file_checksums.mat'); -else - file_checksums.exists = 1; - if(ab_verbose) - disp('.....file_checksums.mat did not exist, creating'); - end -end - -%check the eigen version -cd(fullfile(ASTROBEE_ROOT, '..', '..', 'external', 'eigen')); -eigen_ver_text = fileread('CMakeLists.txt'); -eigen_ver_start = strfind(eigen_ver_text, '/get/')+5; -eigen_ver_end = strfind(eigen_ver_text, '.tar.')-1; -eigen_ver = eigen_ver_text(eigen_ver_start:eigen_ver_end); -cd(fullfile(ASTROBEE_ROOT, 'cxx_functions')); - -%update if necessary -if(~isfield(file_checksums, 'eigen_lib_ver') || exist('Eigen', 'dir')~=7 || ~strcmp(file_checksums.eigen_lib_ver,eigen_ver)) - %if no version in .mat file, no folder exists, or does not equal current version, checkout new version - if(ab_verbose) - disp('.....No Eigen Library Found, performing one time download now'); - end - file_checksums.eigen_lib_ver = eigen_ver; - websave('Eigen', 'http://bitbucket.org/eigen/eigen/get/3.2.2.zip'); - unziped_files = unzip('Eigen.zip'); - new_folder = strsplit(fileparts(unziped_files{1}),{'/','\'}); - movefile([new_folder{1} '/Eigen']); - rmdir(new_folder{1},'s'); - delete('Eigen.zip'); -end - - -for i = 1:4 - %determine checksums of current '.h' and '.cpp' files - file_cpp_checksum = Simulink.getFileChecksum(which([file_names{i} '.cpp'])); - file_h_checksum = Simulink.getFileChecksum(which([file_names{i} '.h'])); - - %check if files already exist. Note: exist==3 impiles a mex file exists - if(exist(['ex_' file_names{i} '.cpp'], 'file') && exist(['ex_' file_names{i}], 'file')==3 && exist(['ex_' file_names{i} '.tlc'], 'file')) - %check if checksum even contains correct files - if(all(isfield(file_checksums,{[file_names{i} 'cpp'], [file_names{i} 'h']}))) - %check if checksums are correct - if(strcmp(file_cpp_checksum,file_checksums.([file_names{i} 'cpp'])) && strcmp(file_h_checksum,file_checksums.([file_names{i} 'h']))) - continue; %skip if they are equal! - end - end - end - - if(ab_verbose) - disp(['.....Building ' file_names{i}]); - end - - %if any are false, then generate the code - def = legacy_code('initialize'); % initialize def variable - def.SourceFiles = {[file_names{i} '.cpp']}; % Source file for block - def.HeaderFiles = {[file_names{i} '.h']}; % Header file for block - def.SFunctionName = ['ex_' file_names{i}]; % sfunction name - def.OutputFcnSpec = function_declarations{i}; % See this help file: http://www.mathworks.com/help/simulink/sfg/integrating-existing-c-functions-into-simulink-models-with-the-legacy-code-tool.html#bq4g1es-6 - def.Options.language = 'C++'; % Language -% def.SrcPaths = {'./src', './../../external/eigen/Eigen/src'}; % Can add multiple sources as cell arrays of strings -% def.IncPaths = {'./include', './../../external/eigen/Eigen'}; - def.SrcPaths = {'./src', './Eigen/src'}; % Can add multiple sources as cell arrays of strings - def.IncPaths = {'./include', './Eigen'}; - def.Options.singleCPPMexFile = false; % Option need for code generation - def.Options.supportsMultipleExecInstances = true; - - legacy_code('sfcn_tlc_generate', def); - legacy_code('sfcn_cmex_generate', def); - legacy_code('compile', def); - legacy_code('rtwmakecfg_generate', def); - %legacy_code('slblock_generate', def); - - %and store the checksums - file_checksums.([file_names{i} 'h']) = file_h_checksum; - file_checksums.([file_names{i} 'cpp']) = file_cpp_checksum; -end - -%save the checksums file -save('file_checksums.mat', 'file_checksums'); -cd(ASTROBEE_ROOT); -return diff --git a/gnc/matlab/cxx_functions/include/apply_delta_state.h b/gnc/matlab/cxx_functions/include/apply_delta_state.h deleted file mode 100644 index 4e271912dd..0000000000 --- a/gnc/matlab/cxx_functions/include/apply_delta_state.h +++ /dev/null @@ -1,28 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -void apply_delta_state( - float* delta_state, int delta_state_length, unsigned short update_flag, - float* quat_ISS2B_in, float* gyro_bias_in, - float* V_B_ISS_ISS_in, float* accel_bias_in, float* P_B_ISS_ISS_in, - float* ml_quat_ISS2cam_in, float* ml_P_cam_ISS_ISS_in, unsigned short kfl_status_in, - float* of_quat_ISS2cam_in, float* of_P_cam_ISS_ISS_in, - float* quat_ISS2B_out, float* gyro_bias_out, - float* V_B_ISS_ISS_out, float* accel_bias_out, float* P_B_ISS_ISS_out, - float* ml_quat_ISS2cam_out, float* ml_P_cam_ISS_ISS_out, unsigned short* kfl_status_out, - float* of_quat_ISS2cam_out, float* of_P_cam_ISS_ISS_out); diff --git a/gnc/matlab/cxx_functions/include/compute_delta_state_and_cov.h b/gnc/matlab/cxx_functions/include/compute_delta_state_and_cov.h deleted file mode 100644 index bad78cae4b..0000000000 --- a/gnc/matlab/cxx_functions/include/compute_delta_state_and_cov.h +++ /dev/null @@ -1,19 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -int compute_delta_state_and_cov(float* residual, int error_in, float* H, int H_rows, int H_cols, unsigned int augs_bitmask, float* R_mat, float* P, float* delta_state_out, float* P_out); diff --git a/gnc/matlab/cxx_functions/include/fault_assert.h b/gnc/matlab/cxx_functions/include/fault_assert.h deleted file mode 100644 index 985319feac..0000000000 --- a/gnc/matlab/cxx_functions/include/fault_assert.h +++ /dev/null @@ -1,33 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_FAULT_ASSERT_H_ -#define GNC_AUTOCODE_FAULT_ASSERT_H_ - -#ifndef MATLAB_MEX_FILE -namespace ff_util { - class FreeFlyerNodelet; -} - -ff_util::FreeFlyerNodelet* get_global_nodelet(void); -void set_global_nodelet(ff_util::FreeFlyerNodelet* n); -#endif - -void fault_assert(unsigned int level, unsigned char* s); - -#endif // GNC_AUTOCODE_FAULT_ASSERT_H_ diff --git a/gnc/matlab/cxx_functions/include/fault_clear.h b/gnc/matlab/cxx_functions/include/fault_clear.h deleted file mode 100644 index 7cc6bac115..0000000000 --- a/gnc/matlab/cxx_functions/include/fault_clear.h +++ /dev/null @@ -1,19 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -void fault_clear(unsigned int level); diff --git a/gnc/matlab/cxx_functions/include/matrix_multiply.h b/gnc/matlab/cxx_functions/include/matrix_multiply.h deleted file mode 100644 index 356094d35a..0000000000 --- a/gnc/matlab/cxx_functions/include/matrix_multiply.h +++ /dev/null @@ -1,19 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -void matrix_multiply(float* m1, int m1_rows, int m1_cols, float* m2, int m2_rows, int m2_cols, float* m_out); diff --git a/gnc/matlab/cxx_functions/include/of_residual_and_h.h b/gnc/matlab/cxx_functions/include/of_residual_and_h.h deleted file mode 100644 index dfc430dde0..0000000000 --- a/gnc/matlab/cxx_functions/include/of_residual_and_h.h +++ /dev/null @@ -1,21 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -int of_residual_and_h(float* of_measured, float* global_points, float* camera_tf_global, int* valid, int num_points, int ase_of_num_aug, - int ase_of_num_features, float tun_ase_mahal_distance_max, float ase_of_r_mag, float ase_inv_focal_length, float ase_distortion, float* P, - float* r_out, float* H_out, unsigned int* augs_bitmask, unsigned char* num_of_tracks, float* mahal_dists, float* R_out); diff --git a/gnc/matlab/cxx_functions/include/ros_log.h b/gnc/matlab/cxx_functions/include/ros_log.h deleted file mode 100644 index 6de8582dce..0000000000 --- a/gnc/matlab/cxx_functions/include/ros_log.h +++ /dev/null @@ -1,24 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#define LOG_LEVEL_DEBUG 0 -#define LOG_LEVEL_INFO 1 -#define LOG_LEVEL_WARN 2 -#define LOG_LEVEL_ERROR 3 - -void ros_log(int level, unsigned char* s); diff --git a/gnc/matlab/cxx_functions/src/apply_delta_state.cpp b/gnc/matlab/cxx_functions/src/apply_delta_state.cpp deleted file mode 100644 index 22a45fdcee..0000000000 --- a/gnc/matlab/cxx_functions/src/apply_delta_state.cpp +++ /dev/null @@ -1,105 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include - -using namespace Eigen; - -void rotate_quat(Quaternionf & q, Vector3f & gibbs_vector, Quaternionf* ret) { - Quaternionf t(2, gibbs_vector.x(), gibbs_vector.y(), gibbs_vector.z()); - // Eigen's multiply doesn't work the same as simulink... - // let's keep this to be safe - ret->x() = q.x() * t.w() + q.w() * t.x() - q.z() * t.y() + q.y() * t.z(); - ret->y() = q.y() * t.w() + q.z() * t.x() + q.w() * t.y() - q.x() * t.z(); - ret->z() = q.z() * t.w() - q.y() * t.x() + q.x() * t.y() + q.w() * t.z(); - ret->w() = q.w() * t.w() - q.x() * t.x() - q.y() * t.y() - q.z() * t.z(); - if (ret->w() < 0) { - ret->w() = -ret->w(); ret->x() = -ret->x(); ret->y() = -ret->y(); ret->z() = -ret->z(); - } - // Eigen normalize function doesn't work here, I don't understand why - if (ret->w() != 0) { - float mag = sqrt(ret->x() * ret->x() + ret->y() * ret->y() + ret->z() * ret->z() + ret->w() * ret->w()); - ret->x() /= mag; ret->y() /= mag; ret->z() /= mag; ret->w() /= mag; - } -} - -void apply_delta_state( - float* delta_state, int delta_state_length, unsigned short update_flag, - float* quat_ISS2B_in, float* gyro_bias_in, - float* V_B_ISS_ISS_in, float* accel_bias_in, float* P_B_ISS_ISS_in, - float* ml_quat_ISS2cam_in, float* ml_P_cam_ISS_ISS_in, unsigned short kfl_status_in, - float* of_quat_ISS2cam_in, float* of_P_cam_ISS_ISS_in, - float* quat_ISS2B_out, float* gyro_bias_out, - float* V_B_ISS_ISS_out, float* accel_bias_out, float* P_B_ISS_ISS_out, - float* ml_quat_ISS2cam_out, float* ml_P_cam_ISS_ISS_out, unsigned short* kfl_status_out, - float* of_quat_ISS2cam_out, float* of_P_cam_ISS_ISS_out) { - int num_augs = (delta_state_length - 21) / 6; - Map ds(delta_state, delta_state_length); - Map quat(quat_ISS2B_in); - Map out_quat(quat_ISS2B_out); - Map gyro_bias(gyro_bias_in); - Map out_gyro_bias(gyro_bias_out); - Map vel(V_B_ISS_ISS_in); - Map out_vel(V_B_ISS_ISS_out); - Map accel_bias(accel_bias_in); - Map out_accel_bias(accel_bias_out); - Map pos(P_B_ISS_ISS_in); - Map out_pos(P_B_ISS_ISS_out); - Map ml_quat(ml_quat_ISS2cam_in); - Map out_ml_quat(ml_quat_ISS2cam_out); - Map ml_pos(ml_P_cam_ISS_ISS_in); - Map out_ml_pos(ml_P_cam_ISS_ISS_out); - - Quaternionf temp_quat1, temp_quat2; - Vector3f temp_vector = ds.segment<3>(0); - temp_quat1 = quat; - rotate_quat(temp_quat1, temp_vector, &temp_quat2); - out_quat = temp_quat2; - out_gyro_bias = gyro_bias + ds.segment<3>(3); - out_vel = vel + ds.segment<3>(6); - out_accel_bias = accel_bias + ds.segment<3>(9); - out_pos = pos + ds.segment<3>(12); - - temp_quat1 = ml_quat; - temp_vector = ds.segment<3>(15); - rotate_quat(temp_quat1, temp_vector, &temp_quat2); - out_ml_quat = temp_quat2; - out_ml_pos = ml_pos + ds.segment<3>(18); - - *kfl_status_out = (kfl_status_in & ~3) | update_flag; - - Map of_quat(of_quat_ISS2cam_in, num_augs, 4); - Map out_of_quat(of_quat_ISS2cam_out, num_augs, 4); - Map of_pos(of_P_cam_ISS_ISS_in, num_augs, 3); - Map out_of_pos(of_P_cam_ISS_ISS_out, num_augs, 3); - - for (int i = 0; i < num_augs; i++) { - Quaternionf t_quat(of_quat(i, 3), of_quat(i, 0), of_quat(i, 1), of_quat(i, 2)); - Quaternionf r; - temp_vector = ds.segment<3>(21 + 6 * i); - rotate_quat(t_quat, temp_vector, &r); - out_of_quat(i, 0) = r.x(); - out_of_quat(i, 1) = r.y(); - out_of_quat(i, 2) = r.z(); - out_of_quat(i, 3) = r.w(); - out_of_pos.row(i) = of_pos.row(i) + ds.segment<3>(24 + 6 * i).transpose(); - } -} - diff --git a/gnc/matlab/cxx_functions/src/compute_delta_state_and_cov.cpp b/gnc/matlab/cxx_functions/src/compute_delta_state_and_cov.cpp deleted file mode 100644 index 70f90b275d..0000000000 --- a/gnc/matlab/cxx_functions/src/compute_delta_state_and_cov.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include - -#include -#include -//#include - -using namespace Eigen; - -int compute_delta_state_and_cov(float* residual_in, int error_in, float* H_in, - int H_rows, int H_cols, unsigned int augs_bitmask, float* R_mat_in, float* P_in, - float* delta_state_out_out, float* P_out_out) { - //struct timeval secs1, secs2; - //gettimeofday(&secs1, 0); - Map H_full(H_in, H_rows, H_cols); - Map residual_full(residual_in, H_rows); - Map R_mat_full(R_mat_in, H_rows, H_rows); - Map P(P_in, H_cols, H_cols); - Map delta_state_out(delta_state_out_out, H_cols); - Map P_out(P_out_out, H_cols, H_cols); - - if (error_in) { - delta_state_out.setZero(); - P_out = P; - //ROS_INFO("compute_delta_state stop 1"); - return 1; - } - - std::vector used_augs; - for (int i = 0; i < (H_cols - 15) / 6; i++) - if (augs_bitmask & (1 << i)) - used_augs.push_back(i); - VectorBlock > residual = residual_full.segment(0, used_augs.size() * 6); - Block > R_mat = R_mat_full.block(0, 0, used_augs.size() * 6, used_augs.size() * 6); - Block > reduced_H = H_full.block(0, 0, used_augs.size() * 6, used_augs.size() * 6); - MatrixXf reduced_P(used_augs.size() * 6, used_augs.size() * 6); - for (int i = 0; i < used_augs.size(); i++) - for (int j = 0; j < used_augs.size(); j++) - reduced_P.block<6, 6>(6 * i, 6 * j) = P.block<6, 6>(15 + 6 * used_augs[i], 15 + 6 * used_augs[j]); - MatrixXf reduced_cols_P(H_cols, used_augs.size() * 6); - for (int i = 0; i < used_augs.size(); i++) - reduced_cols_P.block(0, 6 * i, H_cols, 6) = P.block(0, 15 + 6 * used_augs[i], H_cols, 6); - //gettimeofday(&secs2, 0); - //ROS_INFO("compute_delta_state %d create Ps", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - - MatrixXf T(reduced_cols_P.rows(), reduced_H.rows()), S(reduced_H.rows(), reduced_H.rows()); - T.noalias() = reduced_cols_P * reduced_H.transpose(); - //gettimeofday(&secs2, 0); - //ROS_INFO("compute_delta_state %d multiplies0", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - S.triangularView() = reduced_H * reduced_P * reduced_H.transpose(); - S.triangularView() += R_mat; - //gettimeofday(&secs2, 0); - //ROS_INFO("compute_delta_state %d multiplies1", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - - LDLT chol(S); - if (chol.info() != Success) { - fprintf(stderr, "S not positive definite.\n"); - delta_state_out.setZero(); - P_out = P; - //ROS_INFO("compute_delta_state stop 2"); - return 1; - } - //gettimeofday(&secs2, 0); - //ROS_INFO("compute_delta_state %d cholesky", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - MatrixXf sinv = MatrixXf::Identity(S.cols(), S.cols()); - chol.solveInPlace(sinv); - //gettimeofday(&secs2, 0); - //ROS_INFO("compute_delta_state %d inverse", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - - MatrixXf K; - K.noalias() = T * sinv.selfadjointView(); - delta_state_out.noalias() = K * residual; - //gettimeofday(&secs2, 0); - //ROS_INFO("compute_delta_state %d multiplies2", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - - // F = I - KH; P = FPF' + KRK'; - // P = P - (KHP)' - KHP + K(HPH' + R)K' - // P = P - KHP - P_out.triangularView() = P - K * T.transpose(); // P - KHP - P_out.triangularView() = P_out.transpose(); - //P_out.noalias() = 0.5 * (P_out + P_out.transpose()); - - //gettimeofday(&secs2, 0); - //ROS_INFO("compute_delta_state %d %d stop 0", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)), H_rows); - return 0; -} diff --git a/gnc/matlab/cxx_functions/src/fault_assert.cpp b/gnc/matlab/cxx_functions/src/fault_assert.cpp deleted file mode 100644 index 748ba7fd70..0000000000 --- a/gnc/matlab/cxx_functions/src/fault_assert.cpp +++ /dev/null @@ -1,62 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include "fault_assert.h" - -#include -#include - -#ifndef MATLAB_MEX_FILE -#include -#include -ff_util::FreeFlyerNodelet* global_gnc_nodelet = NULL; - -ff_util::FreeFlyerNodelet* get_global_nodelet(void) {return global_gnc_nodelet;} -void set_global_nodelet(ff_util::FreeFlyerNodelet* n) {global_gnc_nodelet = n;} -#endif - -void fault_assert(unsigned char* key, unsigned char* t) { - char* k = (char*)key; // matlab wants to pass unsigned char - char* s = (char*)t; // matlab wants to pass unsigned char -#ifdef MATLAB_MEX_FILE - printf("Triggered Fault %s: %s\n", key, t); -#else - if (global_gnc_nodelet == NULL) { - ROS_FATAL("Cannot trigger fault, global_gnc_nodelet not set."); - } else { - std::string key = std::string(k); - bool found = false; - int enum_val; - for (int i = 0; i < ff_util::kFaultKeysSize && !found; i++) { - if (key == ff_util::fault_keys[i]) { - found = true; - enum_val = i; - } - } - if (found) { - global_gnc_nodelet->AssertFault(static_cast(enum_val), - std::string(s)); - } else { - std::string err_msg = "GNC: Fault key " + key + - " wasn't recognized. " + std::string(s); - global_gnc_nodelet->AssertFault(ff_util::UNKNOWN_FAULT_KEY, err_msg); - } - } -#endif -} - diff --git a/gnc/matlab/cxx_functions/src/fault_clear.cpp b/gnc/matlab/cxx_functions/src/fault_clear.cpp deleted file mode 100644 index 4849ca7f1e..0000000000 --- a/gnc/matlab/cxx_functions/src/fault_clear.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include "fault_clear.h" - -#include -#include - -#ifndef MATLAB_MEX_FILE -#include -#include -// nodelet defined -#include "fault_assert.h" -#endif - -void fault_clear(unsigned char* key) { - char* k = (char*)key; -#ifdef MATLAB_MEX_FILE - printf("Cleared Fault %s.\n", k); -#else - ff_util::FreeFlyerNodelet* nodelet = get_global_nodelet(); - if (nodelet == NULL) { - ROS_FATAL("Cannot clear fault, global_gnc_nodelet not set.\n"); - } else { - std::string key = std::string(k); - bool found = false; - int enum_val; - for (int i = 0; i < ff_util::kFaultKeysSize && !found; i++) { - if (key == ff_util::fault_keys[i]) { - found = true; - enum_val = i; - } - } - if (found) { - nodelet->ClearFault(static_cast(enum_val)); - } else { - nodelet->ClearFault(ff_util::UNKNOWN_FAULT_KEY); - } - } -#endif -} - diff --git a/gnc/matlab/cxx_functions/src/matrix_multiply.cpp b/gnc/matlab/cxx_functions/src/matrix_multiply.cpp deleted file mode 100644 index e8040e0986..0000000000 --- a/gnc/matlab/cxx_functions/src/matrix_multiply.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include - -using namespace Eigen; - -void matrix_multiply(float* m1, int m1_rows, int m1_cols, float* m2, int m2_rows, int m2_cols, float* m_out) { - Map A(m1, m1_rows, m1_cols); - Map B(m2, m2_rows, m2_cols); - Map out(m_out, m1_rows, m2_cols); - - out.noalias() = A * B; -} - diff --git a/gnc/matlab/cxx_functions/src/of_residual_and_h.cpp b/gnc/matlab/cxx_functions/src/of_residual_and_h.cpp deleted file mode 100644 index 235841272d..0000000000 --- a/gnc/matlab/cxx_functions/src/of_residual_and_h.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include - -#include -#include -#include -//#include - -using Eigen::MatrixXd; -using namespace Eigen; -//using namespace std; - -void apply_givens(MatrixXf & H_f_j, MatrixXf & H_x_j, VectorXf & r_j, int row1, int row2, int col, - int H_x_j_col_start, int H_x_j_num_cols) { - JacobiRotation g; - g.makeGivens(H_f_j(row1, col), H_f_j(row2, col)); - H_f_j.applyOnTheLeft(row1, row2, g.adjoint()); - H_x_j.block(0, H_x_j_col_start, H_x_j.rows(), H_x_j_num_cols).applyOnTheLeft(row1, row2, g.adjoint()); - r_j.applyOnTheLeft(row1, row2, g.adjoint()); -} - -void apply_givens(MatrixXf & H, VectorXf & r, int row1, int row2, int col) { - JacobiRotation g; - g.makeGivens(H(row1, col), H(row2, col)); - H.block(0, col, H.rows(), H.cols() - col).applyOnTheLeft(row1, row2, g.adjoint()); - r.applyOnTheLeft(row1, row2, g.adjoint()); -} - -Matrix3f skew(const Vector3f & v) { - Matrix3f ret; - ret << 0, -v(2), v(1), - v(2), 0, -v(0), - -v(1), v(0), 0; - return ret; -} - -// S is lower triangular -int pinv(const MatrixXf & S, MatrixXf & out) { - LDLT chol(S); - if (chol.info() != Success) { - return 1; - } - out = MatrixXf::Identity(out.rows(), out.cols()); - chol.solveInPlace(out); - return 0; -} - -int of_residual_and_h(float* of_measured_p, float* global_points_p, float* camera_tf_global_p, - int* valid_p, int num_points, int ase_of_num_aug, int ase_of_num_features, - float tun_ase_mahal_distance_max, float ase_of_r_mag, float ase_inv_focal_length, float ase_distortion, float* P_p, - float* r_out_p, float* H_out_p, unsigned int* augs_bitmask, unsigned char* num_of_tracks_out, float* mahal_dists_out_p, float* R_out_p) -{ - //struct timeval secs1, secs2; - //gettimeofday(&secs1, 0); - int covariance_size = 21 + 6 * ase_of_num_aug; - Map of_measured(of_measured_p, ase_of_num_features * 2 * ase_of_num_aug); - Map global_points(global_points_p, 4, ase_of_num_features); - Map camera_tf_global(camera_tf_global_p, 4 * ase_of_num_aug, 4); - Map valid(valid_p, ase_of_num_features, ase_of_num_aug); - Map P(P_p, covariance_size, covariance_size); - Map r_out(r_out_p, 6 * ase_of_num_aug); - Map H_out(H_out_p, 6 * ase_of_num_aug, covariance_size); - Map R_out(R_out_p, 6 * ase_of_num_aug, 6 * ase_of_num_aug); - Map mahal_dists_out(mahal_dists_out_p, ase_of_num_features); - - r_out.setZero(); - H_out.setZero(); - R_out.setZero(); - *num_of_tracks_out = 0; - *augs_bitmask = 0; - - // figure out which augmentations were used (this helps with speed) - std::vector used_augs; - for (int i = 0; i < ase_of_num_aug; i++) { - if ((valid.col(i).array() > 0).any()) { - used_augs.push_back(i); - *augs_bitmask |= 2 << i; - } - } - if (used_augs.size() < 3) { - r_out(1) = 1; - return 1; - } - - MatrixXf reduced_P(used_augs.size() * 6, used_augs.size() * 6); - for (int i = 0; i < used_augs.size(); i++) - for (int j = 0; j < used_augs.size(); j++) - reduced_P.block<6, 6>(6 * i, 6 * j) = P.block<6, 6>(21 + 6 * i, 21 + 6 * j); - - VectorXf r((2 * used_augs.size() - 3) * ase_of_num_features); - MatrixXf H((2 * used_augs.size() - 3) * ase_of_num_features, 6 * used_augs.size()); - H.setZero(); - //gettimeofday(&secs2, 0); - //ROS_INFO("of_residual_and_h %d cast", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - - int cur_row = 0; - int num_tracks = 0; - for (int j = 0; j < num_points; j++) { - int valid_augs = valid.row(j).count(); - if (valid_augs < 2) - continue; - int rows = valid_augs * 2; - VectorXf r_j(rows); - MatrixXf H_f_j(rows, 3); - MatrixXf H_x_j(rows, 6 * used_augs.size()); - H_x_j.setZero(); - int aug_ind = 0; - // construct r_j, H_x_j, and H_f_j - for (int i = 0; i < valid_augs; i++) { - while (!valid(j, used_augs[aug_ind])) - aug_ind++; - assert(aug_ind < used_augs.size()); - - Matrix c_tf_g = camera_tf_global.block<3, 4>(4 * used_augs[aug_ind], 0); - Vector3f camera_landmark = c_tf_g * global_points.col(j); - Vector2f z_est(camera_landmark(0), camera_landmark(1)); - z_est *= 1.0 / camera_landmark(2); - - Matrix prefix; - prefix << 1, 0, -z_est(0), 0, 1, -z_est(1); - prefix *= 1.0 / camera_landmark(2); - Matrix H_theta_ji = prefix * skew(camera_landmark); - Matrix H_p_ji = -prefix * c_tf_g.block<3, 3>(0, 0); - - r_j.segment<2>(2 * i) = of_measured.segment<2>(2 * ase_of_num_aug * j + 2 * used_augs[aug_ind]) - z_est; - H_x_j.block<2, 3>(2 * i, aug_ind * 6) = H_theta_ji; - H_x_j.block<2, 3>(2 * i, aug_ind * 6 + 3) = H_p_ji; - H_f_j.block<2, 3>(2 * i, 0) = -H_p_ji; - aug_ind++; - } - - // do givens rotations to make H_f_j upper triangular - // optimized version only if we observed all 4 augmentations - if (used_augs.size() == valid_augs && valid_augs == 4) { - // first make each block of 2 rows x x x; 0 x x - for (int i = 0; i < rows; i+=2) - apply_givens(H_f_j, H_x_j, r_j, i, i + 1, 0, 6 * (i/2), 6); - // now make each block of 4 rows x x x; 0 x x; 0 0 x; 0 0 0 - for (int i = 0; i < rows; i+=4) { - apply_givens(H_f_j, H_x_j, r_j, i , i + 2, 0, 6 * (i/4), 12); - apply_givens(H_f_j, H_x_j, r_j, i + 1, i + 2, 1, 6 * (i/4), 12); - apply_givens(H_f_j, H_x_j, r_j, i + 1, i + 3, 1, 6 * (i/4), 12); - apply_givens(H_f_j, H_x_j, r_j, i + 2, i + 3, 2, 6 * (i/4), 12); - } - // finally eliminate the second block of four - apply_givens(H_f_j, H_x_j, r_j, 0, 4, 0, 0, 24); - apply_givens(H_f_j, H_x_j, r_j, 1, 4, 1, 0, 24); - apply_givens(H_f_j, H_x_j, r_j, 1, 5, 1, 0, 24); - apply_givens(H_f_j, H_x_j, r_j, 2, 4, 2, 0, 24); - apply_givens(H_f_j, H_x_j, r_j, 2, 5, 2, 0, 24); - apply_givens(H_f_j, H_x_j, r_j, 2, 6, 2, 0, 24); - } else { - // normal version - for (int col = 0; col < 3; col++) - for (int row = rows - 1; row > col; row--) - apply_givens(H_f_j, H_x_j, r_j, row - 1, row, col, 0, H_x_j.cols()); - } - - // then delete the top three rows which are nonzero in H_f_j - Block H_x_j_reduced = H_x_j.block(3, 0, rows - 3, H_x_j.cols()); - VectorBlock r_j_reduced = r_j.segment(3, rows - 3); - - // now check the mahalanobis distance - MatrixXf S(H_x_j_reduced.rows(), H_x_j_reduced.rows()); - S.triangularView() = H_x_j_reduced * reduced_P * H_x_j_reduced.transpose(); - for (int i = 0; i < S.rows(); i++) - S(i, i) += (ase_inv_focal_length * ase_of_r_mag) * (ase_inv_focal_length * ase_of_r_mag); - MatrixXf S_inv(S.rows(), S.cols()); - if (pinv(S, S_inv)) { - fprintf(stderr, "Failed to take inverse for Mahalanobis distance.\n"); - continue; - } - float mahal_dist = sqrt(r_j_reduced.transpose() * S_inv * r_j_reduced); - mahal_dists_out(j) = mahal_dist; - if (mahal_dist > tun_ase_mahal_distance_max) - continue; - - // add to full r and H - r.segment(cur_row, r_j_reduced.rows()) = r_j_reduced; - H.block(cur_row, 0, r_j_reduced.rows(), H_x_j_reduced.cols()) = H_x_j_reduced; - cur_row += r_j_reduced.rows(); - num_tracks++; - } - //gettimeofday(&secs2, 0); - //ROS_INFO("of_residual_and_h %d r and h", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - - // set the rest to uninitialized - for (int j = num_points; j < ase_of_num_features; j++) - mahal_dists_out(j) = std::numeric_limits::quiet_NaN(); - - r.conservativeResize(cur_row); - H.conservativeResize(cur_row, NoChange); - if (r.rows() < 6 * used_augs.size()) { - r_out(1) = 1; - //ROS_INFO("of_residual_and_h end 1"); - return 1; - } - - *num_of_tracks_out = static_cast(num_tracks); - //gettimeofday(&secs2, 0); - //ROS_INFO("of_residual_and_h %d output", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - - // now we do the compression - // do givens rotations to make H upper triangular and remove as many rows as possible - for (int col = 0; col < H.cols(); col++) - for (int row = H.rows() - 1; row > col; row--) - apply_givens(H, r, col, row, col); - //gettimeofday(&secs2, 0); - //ROS_INFO("of_residual_and_h %d compress", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - //secs1 = secs2; - - r_out.segment(0, 6 * used_augs.size()) = r.segment(0, 6 * used_augs.size()); - H_out.block(0, 0, 6 * used_augs.size(), 6 * used_augs.size()) = H.block(0, 0, 6 * used_augs.size(), 6 * used_augs.size()); - for (int i = 0; i < 6 * ase_of_num_aug; i++) - R_out(i, i) = (ase_of_r_mag * ase_inv_focal_length) * (ase_of_r_mag * ase_inv_focal_length); - //gettimeofday(&secs2, 0); - //ROS_INFO("of_residual_and_h %d end 0", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec))); - - return 0; -} - diff --git a/gnc/matlab/cxx_functions/src/ros_log.cpp b/gnc/matlab/cxx_functions/src/ros_log.cpp deleted file mode 100644 index 7e54fd7407..0000000000 --- a/gnc/matlab/cxx_functions/src/ros_log.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include "ros_log.h" - -#ifndef MATLAB_MEX_FILE -#include -#else -#include -#endif - -void ros_log(int level, unsigned char* t) { - char* s = (char*)t; // matlab wants to pass unsigned char -#ifdef MATLAB_MEX_FILE - puts(s); -#else - switch (level) { - case LOG_LEVEL_INFO: - ROS_INFO(s); - break; - case LOG_LEVEL_WARN: - ROS_WARN(s); - break; - case LOG_LEVEL_ERROR: - ROS_ERROR(s); - break; - default: - ROS_DEBUG(s); - break; - } -#endif -} - diff --git a/gnc/matlab/data_reduction/@quaternion/angle_error.m b/gnc/matlab/data_reduction/@quaternion/angle_error.m deleted file mode 100644 index 0f2728c7f8..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/angle_error.m +++ /dev/null @@ -1,22 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Quaternion method for returnig angle error - -function [ total_angle_rad ] = angle_error( quat1, quat2 ) -total_angle_rad = quat_error(quat1.value, quat2.value)/1000; - diff --git a/gnc/matlab/data_reduction/@quaternion/convert_to_dcm.m b/gnc/matlab/data_reduction/@quaternion/convert_to_dcm.m deleted file mode 100644 index 0be52318a8..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/convert_to_dcm.m +++ /dev/null @@ -1,19 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function dcm = convert_to_dcm(q) -dcm = quaternion_to_dcm(q.value); diff --git a/gnc/matlab/data_reduction/@quaternion/convert_to_eulers.m b/gnc/matlab/data_reduction/@quaternion/convert_to_eulers.m deleted file mode 100644 index 7204a879f5..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/convert_to_eulers.m +++ /dev/null @@ -1,21 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function eulers = convert_to_eulers(q) - -eulers = quat_to_eulers(q.value); - diff --git a/gnc/matlab/data_reduction/@quaternion/inverse.m b/gnc/matlab/data_reduction/@quaternion/inverse.m deleted file mode 100644 index f8bf943fe9..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/inverse.m +++ /dev/null @@ -1,21 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function quat_inv = inverse(quat) -% Quaternion inverse -quat = quat.value; -quat_inv = [ -quat(:,1:3) quat(:,4) ]; \ No newline at end of file diff --git a/gnc/matlab/data_reduction/@quaternion/normalize_and_sign_check.m b/gnc/matlab/data_reduction/@quaternion/normalize_and_sign_check.m deleted file mode 100644 index 0b7d7a6658..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/normalize_and_sign_check.m +++ /dev/null @@ -1,27 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function qOut = normalize_and_sign_check(qIn) -% Normalize quaternion and enforce the scalar to be positive - -% If there are negatives we want to multiply by -1 -quatMultiplyer = repmat(sign(qIn.value(:, 4)), 1, 3); -quatMultiplyer(quatMultiplyer == 0) = 1; % Should multiply by 1 if the scalar is zero - -tempQuat = [qIn.value(:, 1:3) .* quatMultiplyer abs(qIn.value(:, 4))]; - -qOut = quaternion(tempQuat ./ repmat(rssrow(tempQuat), 1, 4)); \ No newline at end of file diff --git a/gnc/matlab/data_reduction/@quaternion/quaternion.m b/gnc/matlab/data_reduction/@quaternion/quaternion.m deleted file mode 100644 index 92e82837aa..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/quaternion.m +++ /dev/null @@ -1,91 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -classdef quaternion % telemData is a subclass - - methods - % Constructor - function obj = quaternion(q) - switch nargin % Need to support the constructor with 0 arguments for some reason - case 0 - obj.value = []; - case 1 - - obj.value = q; - otherwise - error('Illegal number of arguments'); - end - end - - - function y = double(obj) - y = double(obj.value); - end - - % When matlab calls disp, return the value of the data property - function disp(obj) - obj.value - end - - function angleOut = minus(q1, q2) - % Returns angle between 2 quaternions (radians) - angleOut = angle_error(q1, q2); - end - - function qOut = mtimes(q1, q2) - % Returns the product of quaternion multiply (q1*q2) - qOut = quaternion(quatmult(q1.value, q2.value)); - end - - function qOut = uminus(qIn) - % -q is quaternion inverse - qOut = quaternion(qIn.inverse); - - end - - function qOut = abs(qIn) - % Normalize and enforce positive scalar - qOut = qIn.normalize_and_sign_check; - end - - function n = numel(A,varargin) - n = 1;%numel(A.value); - end - - function [varargout] = subsref(A,S) - if strcmp(S.type, '.') - %varargout{1} = A.value; - varargout{1} = eval(['A' S.type S.subs]); - else - varargout{1} = quaternion(subsref(A.value,S)); - end - end - - % Whats this? - function dataOut = isempty(dataIn) - dataOut = builtin('isempty',dataIn); - end - - - - end - % Properties - properties - value; - end - -end \ No newline at end of file diff --git a/gnc/matlab/data_reduction/@quaternion/quatmult.m b/gnc/matlab/data_reduction/@quaternion/quatmult.m deleted file mode 100644 index e07eb37c68..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/quatmult.m +++ /dev/null @@ -1,22 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function qOut = quatmult(p, q) - -qOut = quaternion(quatmult(p.value, q.value)); - -end diff --git a/gnc/matlab/data_reduction/@quaternion/rotate_a2b.m b/gnc/matlab/data_reduction/@quaternion/rotate_a2b.m deleted file mode 100644 index 39512b91f9..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/rotate_a2b.m +++ /dev/null @@ -1,21 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function vecOut = rotate_a2b(quat, vector) -% Rotates vector from frame A to B where the quaternion express a rotation -% of a vector from cf A to B. -vecOut = quat_rotation_vec(vector, quat_inv(quat.value)); diff --git a/gnc/matlab/data_reduction/@quaternion/rotate_b2a.m b/gnc/matlab/data_reduction/@quaternion/rotate_b2a.m deleted file mode 100644 index 2751591dcd..0000000000 --- a/gnc/matlab/data_reduction/@quaternion/rotate_b2a.m +++ /dev/null @@ -1,21 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function vecOut = rotate_b2a(quat, vector) -% Rotates vector from frame B to A where the quaternion express a rotation -% of a vector from cf A to B. -vecOut = quat_rotation_vec(vector, quat.value); diff --git a/gnc/matlab/data_reduction/@telemData/minus.m b/gnc/matlab/data_reduction/@telemData/minus.m deleted file mode 100644 index 29529e3f06..0000000000 --- a/gnc/matlab/data_reduction/@telemData/minus.m +++ /dev/null @@ -1,22 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function C = minus(A,B) -% Return the time synced addition of A and B, where A and B are telemData -% classes -[A, B] = sampleUniform(A,B); -C = telemData(A.time, A.data - B.data); diff --git a/gnc/matlab/data_reduction/@telemData/mtimes.m b/gnc/matlab/data_reduction/@telemData/mtimes.m deleted file mode 100644 index 9452253d6b..0000000000 --- a/gnc/matlab/data_reduction/@telemData/mtimes.m +++ /dev/null @@ -1,22 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function C = mtimes(A,B) -% Return the time synced addition of A and B, where A and B are telemData -% classes -[A, B] = sampleUniform(A,B); -C = telemData(A.time, A.data * B.data); diff --git a/gnc/matlab/data_reduction/@telemData/plot.m b/gnc/matlab/data_reduction/@telemData/plot.m deleted file mode 100644 index aed140bcfd..0000000000 --- a/gnc/matlab/data_reduction/@telemData/plot.m +++ /dev/null @@ -1,66 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function varargout = plot(h,varargin) -%PLOT -% Accepts a TelemData with standard plot options. -% Additoinal properties include: -% 't0' - specify a time to set as the zero time -% 'title' - Specify a title for the plot and window -% 'AbsTime' - Absolute Time -% -% Examples: -% plot(telemData) -% plot(telemData, 'property1Name', property1) -% plot(telemData, 'property1Name', property1, 't0', 122) - -% See if a zero time has been specified -t0_indx = find((strcmpi(varargin, 't0')), 1,'first'); -if ~isempty(t0_indx) - h.time = h.time - varargin{t0_indx + 1}; - varargin(t0_indx:t0_indx+1) = []; -end - -% See if a zero time has been specified -title_indx = find((strcmpi(varargin, 'title')), 1,'first'); -if ~isempty(title_indx) - plot_title = varargin{title_indx+1}; - varargin(title_indx:title_indx+1) = []; -end - -abs_plot_indx = find((strcmpi(varargin, 'AbsTime')), 1,'first'); -if ~isempty(abs_plot_indx) - varargin(abs_plot_indx) = []; - h.time = datetime(h.time, 'ConvertFrom', 'posixtime'); -end - - - -pHandle = plot(h.time, h.data, varargin{:}); -grid on; -if ~isempty(title_indx) - title(plot_title); - % Set the figure title - r = gca; - set(gcf, 'Name', r.Title.String) -end - -if nargout>0 - varargout{1} = pHandle; -end - - diff --git a/gnc/matlab/data_reduction/@telemData/plotError.m b/gnc/matlab/data_reduction/@telemData/plotError.m deleted file mode 100644 index 0a50891c7e..0000000000 --- a/gnc/matlab/data_reduction/@telemData/plotError.m +++ /dev/null @@ -1,60 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function plotError(telemData1, telemData2, units, titleStr, legend1, legend2,style2, t0) -% Prep for plot function in the telem class - -error_data = telemData1 - telemData2; - -if t0 > 0 - t1 = telemData1.time - t0; - t2 = telemData2.time - t0; - te = error_data.time - t0; -elseif t0 == 0 - t1 = telemData1.time; - t2 = telemData2.time; - te = error_data.time; -else - t1 = datetime(telemData1.time, 'ConvertFrom', 'PosixTime'); - t2 = datetime(telemData2.time, 'ConvertFrom', 'PosixTime'); - te = datetime(error_data.time, 'ConvertFrom', 'PosixTime'); -end - -figure;%(5); clf; -subplot(2,1,1); -plot(t1, telemData1.data) -hold_on; -plot(t2, telemData2.data, style2) -title(titleStr); grid on; -ylabel(units); xlabel('seconds') -if size(telemData1.data, 2)==1 && size(telemData2.data, 2)==1 - legend(legend1, legend2); -else - legend([legend1 '_x'], [legend1 '_y'], [legend1 '_z'], [legend2 '_x'], [legend2 '_y'], [legend2 '_z']); -end - - -subplot(2,1,2); -plot(te, error_data.data); -hold on; magLine = plot(te, rssrow(error_data.data), '--k'); -legend(magLine, '|error|'); -title([titleStr ' Error']); grid on; -ylabel(units); xlabel('seconds') - -% Set the figure title -r = gca; -set(gcf, 'Name', r.Title.String) \ No newline at end of file diff --git a/gnc/matlab/data_reduction/@telemData/plotQuatsError3.m b/gnc/matlab/data_reduction/@telemData/plotQuatsError3.m deleted file mode 100644 index 5709f67a60..0000000000 --- a/gnc/matlab/data_reduction/@telemData/plotQuatsError3.m +++ /dev/null @@ -1,44 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function plotQuatsError3(telemData1, telemData2, telemData3, titleStr, legend1, legend2,legend3, style2, style3, t0) -% Prep for plot function in the telem class -figure;%(5); clf; -% Combine calcs into analysis file -R1 = convert_to_eulers( telemData1.data ); -R2 = convert_to_eulers( telemData2.data ); -R3 = convert_to_eulers( telemData3.data ); - - - -subplot(2,1,1); -plot(telemData1.time - t0, R1*180/pi) -hold_on; -plot(telemData2.time - t0, R2*180/pi, style2) -hold_on; -plot(telemData3.time - t0, R3*180/pi, style3) -title(titleStr); grid on; -ylabel('Euler Angles, Deg'); xlabel('seconds') -legend([legend1 '_\theta'], [legend1 '_\phi'], [legend1 '_\psi'], [legend2 '_\theta'], [legend2 '_\phi'], [legend2 '_\psi'],... - [legend3 '_\theta'], [legend3 '_\phi'], [legend3 '_\psi']); - -% Minus applied to quaternion calculates angle error -angleError = telemData1 - telemData2; -subplot(2,1,2); -plot(angleError.time - t0, angleError.data*180/pi); -title([titleStr ' Error (' legend1 ' to ' legend2 ')']); grid on; -ylabel('Deg'); xlabel('seconds') diff --git a/gnc/matlab/data_reduction/@telemData/plus.m b/gnc/matlab/data_reduction/@telemData/plus.m deleted file mode 100644 index 5d03c67ec9..0000000000 --- a/gnc/matlab/data_reduction/@telemData/plus.m +++ /dev/null @@ -1,25 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function C = plus(A,B) -% Return the time synced addition of A and B, where A and B are telemData -% classes -[A, B] = sampleUniform(A,B); -C = telemData(A.time, A.data + B.data); - - - diff --git a/gnc/matlab/data_reduction/@telemData/sampleUniform.m b/gnc/matlab/data_reduction/@telemData/sampleUniform.m deleted file mode 100644 index 64ac6a0f0d..0000000000 --- a/gnc/matlab/data_reduction/@telemData/sampleUniform.m +++ /dev/null @@ -1,104 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [A_Out, B_Out] = sampleUniform(A, B) -% resamples the telemData variables A and B according the following rules: -% 1. If one has its masterTime property set, the output is resampled to its -% time base for the period of time that overlaps both variables -% 2. If bother have masterTime or neither has masterTime set then the least -% numerous dataset becomes the masterTime -% 2a. In the event they both have equal number of points the first input -% becomes the masterTime -minTime = max(min(A.time),min(B.time)); -maxTime = min(max(A.time),max(B.time)); -A_indx = find(A.time >= minTime & A.time <= maxTime); -B_indx = find(B.time >= minTime & B.time <= maxTime); - -if A.masterTime && ~B.masterTime - % Use A as masterTime - master = 'A'; -elseif ~A.masterTime && B.masterTime - % Use B as masterTime - master = 'B'; -elseif length(B_indx) > length(A_indx) - master = 'B'; - % Use the most numerous one of the overlapping period as the masterTime -else % if its a tie then use A - master = 'A'; -end - -% Generate the master time vector -masterT = eval([master '.time(' master '_indx);']); - - - - -%% Generate the interpolated data -if strcmp(master, 'A') - slave = 'B'; -else - slave = 'A'; -end - -% Just downsample the master to the overlapping points and output -% First we copy the the telemData structure over then replace the data, -% this will preserve all the other properties -eval([master '_Out = ' master ';']); - -%A_Out = telemData(masterT, A.data(A_indx)); -eval([master '_Out.time = masterT;']); -try - eval([master '_Out.data = ' master '.data(' master '_indx,:);']); -catch - eval([master '_Out.data = ' master '.data(' master '_indx);']); -end -%% Interpolate the slave -slaveTM = eval(slave); - % Add special check for quaternion - if isa(eval( [slave '.data'] ), 'quaternion') % if its a quaternion, - interp_type = 'slerp'; % use SLERP - elseif all(all(mod( slaveTM.data ,1)==0)) - interp_type = 'nearest'; % Use nearest if integer values - else %if non-integer values - interp_type = 'linear'; % Otherwise, use linear - end - if strcmp(interp_type, 'slerp') - try - interpedData = interp_slerp(slaveTM.data, slaveTM.time, masterT); - catch - interp_type = 'nearest'; - interpedData = interp1(slaveTM.time, slaveTM.data.value, masterT, interp_type); - interpedData = quaternion(interpedData); - warning('Using nearest interpolation for quaternions, define interp_slerp on the matlab path'); - end - else - interpedData = interp1(slaveTM.time, slaveTM.data, masterT, interp_type); - end - - % First we copy the the telemData structure over then replace the data, - % this will preserve all the other properties - - - %eval([slave '_Out = telemData(masterT, interpedData);']); - eval([slave '_Out = ' slave ';']); - - %A_Out = telemData(masterT, A.data(A_indx)); - eval([slave '_Out.time = masterT;']); - eval([slave '_Out.data = interpedData;']); - - -end diff --git a/gnc/matlab/data_reduction/@telemData/telemData.m b/gnc/matlab/data_reduction/@telemData/telemData.m deleted file mode 100644 index 3937b9df2c..0000000000 --- a/gnc/matlab/data_reduction/@telemData/telemData.m +++ /dev/null @@ -1,111 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -classdef telemData % telemData is a subclass - - - methods - % Constructor - function obj = telemData(T,Y,varargin) - switch nargin % Need to support the constructor with 0 arguments for some reason - case 0 - obj.time = []; - obj.data = []; - case 1 - if isa(T, 'timeseries') % if the first imput is a timeseries data type, unpack it - obj.time = T.Time; - if size(T.Data , 2) == 4 && ismatrix(T.Data) % If it looks like a quaternion time vector - obj.data = quaternion(T.Data); - else - obj.data = T.Data; - end - else - error('Illegal arguments'); - end - case 2 - obj.time = T; - obj.data = Y; - otherwise - error('Illegal number of arguments'); - end - end - - % Whats this? - function dataOut = isempty(dataIn) - dataOut = builtin('isempty',dataIn); - end - - function y = uminus(obj) - y = obj; - y.data = uminus(y.data); - end - - function y = convert_rad2deg(obj) - y = obj; - y.data = y.data*180/pi; - end - - function y = mag(obj) - y = obj; - y.data = rssrow(y.data); - end - - function y = max(obj) - y = max(obj.data); - end - - function y = maxerror(obj) - [~, idx] = max(abs(obj.data), [], 1); - y = obj.data(sub2ind(size(obj.data), idx, 1:size(obj.data, 2))); - end - - function y = min(obj) - y = min(obj.data); - end - - function y = mean(obj) - y = mean(obj.data); - end - - function y = std(obj) - y = std(obj.data); - end - - function y = mean3sig(obj) - y = abs(mean(obj.data)) + 3*std(obj.data); - end - - function y = fprintf(varargin) - td_indx = find(cellfun(@(a) isa(a, 'telemData'), varargin)); - for ii = 1:length(td_indx) - varargin{td_indx(ii)} = varargin{td_indx(ii)}.data; - end - y = fprintf(varargin{:}); - end - - end - % Properties - properties - data; - time; - masterTime = false; - units; - legendName; - - end - -end \ No newline at end of file diff --git a/gnc/matlab/data_reduction/@telemData/timeRange.m b/gnc/matlab/data_reduction/@telemData/timeRange.m deleted file mode 100644 index d4565140bb..0000000000 --- a/gnc/matlab/data_reduction/@telemData/timeRange.m +++ /dev/null @@ -1,29 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function TMDataOut = timeRange(telemDataObj, minTime, maxTime) -TMDataOut = telemDataObj; -validIndx = TMDataOut.time >= minTime & TMDataOut.time <= maxTime; -TMDataOut.time = TMDataOut.time(validIndx); -switch ndims(TMDataOut.data) - case 2 - TMDataOut.data = TMDataOut.data(validIndx,:); - case 3 - TMDataOut.data = TMDataOut.data(:,:,validIndx); - otherwise - TMDataOut.data = TMDataOut.data(validIndx,:); -end diff --git a/gnc/matlab/data_reduction/ab_populate_ts_field.m b/gnc/matlab/data_reduction/ab_populate_ts_field.m deleted file mode 100644 index 2bcf1af416..0000000000 --- a/gnc/matlab/data_reduction/ab_populate_ts_field.m +++ /dev/null @@ -1,24 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function dataStruct = ab_populate_ts_field(timeIn, dataIn, fieldName, dataStruct) - %dataStruct = dataStruct; - tempField = timeseries(dataIn,timeIn); - tempField.Name = fieldName; - tempField.TimeInfo.StartDate = '01/01/1970 00:00:00'; - dataStruct.(tempField.Name) = tempField; -end \ No newline at end of file diff --git a/gnc/matlab/data_reduction/ab_processTimeData.m b/gnc/matlab/data_reduction/ab_processTimeData.m deleted file mode 100644 index 9a9a20d7fc..0000000000 --- a/gnc/matlab/data_reduction/ab_processTimeData.m +++ /dev/null @@ -1,23 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function time_data = ab_processTimeData(time_data) - %time_data = (time_data-time_data(1))*(10^-9); %convert from nsec to sec - if all(time_data > 1E10) - time_data = (time_data)*(10^-9); %convert from nsec to sec - end - \ No newline at end of file diff --git a/gnc/matlab/data_reduction/astrobee_eval_requirments.m b/gnc/matlab/data_reduction/astrobee_eval_requirments.m deleted file mode 100644 index 9707129dfd..0000000000 --- a/gnc/matlab/data_reduction/astrobee_eval_requirments.m +++ /dev/null @@ -1,32 +0,0 @@ -% Eval Astrobee GN&C requirments on the following data sets -% -% -%% Data sets -dir_names = {'/Users/jfusco1/Desktop/2018-03-27-06-55-06_scn2', ... - '/Users/jfusco1/Desktop/2018-04-04-07-16-02', '/Users/jfusco1/Desktop/2018-04-10-10-24-03'}; - - - -%% Process the data -curr_dir = pwd; -for dir_indx=1:length(dir_names) - cd(dir_names{dir_indx}); - - astrobee_load_data; - fprintf('Loading data from: %s\n', pwd); - - % Determine which data sets exist in the workspace - ds_on.cmd = exist('out_cmd_msg', 'var') && ~isempty(out_cmd_msg) && isfield(out_cmd_msg, 'traj_quat'); - ds_on.env = exist('out_env_msg', 'var') && ~isempty(out_env_msg); - ds_on.kfl = exist('out_kfl_msg', 'var') && ~isempty(out_kfl_msg); - ds_on.act = exist('out_act_msg', 'var') && ~isempty(out_act_msg); - - % Run any init files we need variables from - tunable_init; - - %% Derive Data - calcData(dir_indx) = deriveAstroBeeData; - t0 = calcData.t0; -end -cd(curr_dir); - diff --git a/gnc/matlab/data_reduction/astrobee_generate_stats.m b/gnc/matlab/data_reduction/astrobee_generate_stats.m deleted file mode 100644 index 11fd82e549..0000000000 --- a/gnc/matlab/data_reduction/astrobee_generate_stats.m +++ /dev/null @@ -1,114 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function astrobee_generate_stats(varargin) - -% Options -writeStats = 0; - -if ~isempty(varargin) - t_low = varargin{1}(1); - t_high = varargin{1}(2); -else - t_low = -inf; - t_high = inf; -end - -%% Import From Base -% The try statement is cheesy but easy -try; out_ex_time_msg = evalin('base', 'out_ex_time_msg'); end; -try; out_cvs_reg_pulse = evalin('base', 'out_cvs_reg_pulse'); end; -try; out_cvs_landmark_msg = evalin('base', 'out_cvs_landmark_msg'); end; -try; out_cvs_optflow_msg = evalin('base', 'out_cvs_optflow_msg'); end; -try; out_cvs_handrail_msg = evalin('base', 'out_cvs_handrail_msg'); end; -try; out_cmc_msg = evalin('base', 'out_cmc_msg'); end; -try; out_imu_msg = evalin('base', 'out_imu_msg'); end; -try; out_env_msg = evalin('base', 'out_env_msg'); end; -try; out_vpp_msg = evalin('base', 'out_vpp_msg'); end; -try; out_bpm_msg = evalin('base', 'out_bpm_msg'); end; -try; out_kfl_msg = evalin('base', 'out_kfl_msg'); end; -try; out_kfl_P = evalin('base', 'out_kfl_P'); end; -try; out_cmd_msg = evalin('base', 'out_cmd_msg'); end; -try; out_act_msg_vpp = evalin('base', 'out_act_msg_vpp'); end; -try; out_act_msg = evalin('base', 'out_act_msg'); end; -try; out_ctl_msg = evalin('base', 'out_ctl_msg'); end; -astrobee_version = evalin('base', 'astrobee_version'); -simData = evalin('base', 'simData'); -try; current_file_name = evalin('base', 'current_file_name'); end; -try; calcData = evalin('base', 'calcData'); end; - - - -%% Stats -% Dependent on current_file_name having been generated by -% astrobee_load_p3_data.m - -if simData - titleParsed = {datestr(now, 30)}; -else - titleParsed = strsplit(current_file_name, '_'); -end - -% Stats -fid_array = 1; -if writeStats - fid_array = [fid_array fopen([titleParsed{1} '_stats.txt'], 'w')]; -end - -for ii = 1:length(fid_array) - - fprintf(fid_array(ii),'\n'); - fprintf(fid_array(ii),'True Position Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (m): %f, %f, %f\n', maxerror(calcData.error.pos.total.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Mean (m): %f, %f, %f\n', mean(calcData.error.pos.total.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Mean + 3 Sigma (m): %f, %f, %f\n', mean3sig(calcData.error.pos.total.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Standard Deviation (m): %f, %f, %f\n\n', std(calcData.error.pos.total.timeRange(t_low,t_high))); - - % Calc Euler as well - fprintf(fid_array(ii),'True Attitude Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (deg): %f\n', maxerror(timeRange(calcData.error.att.total_mag.convert_rad2deg, t_low, t_high))); - fprintf(fid_array(ii),'Mean (deg): %f\n', mean(timeRange(calcData.error.att.total_mag.convert_rad2deg,t_low, t_high))); - fprintf(fid_array(ii),'Mean + 3 Sigma (deg): %f\n', mean3sig(timeRange(calcData.error.att.total_mag.convert_rad2deg, t_low,t_high))); - fprintf(fid_array(ii),'Standard Deviation (deg): %f\n\n', std(timeRange(calcData.error.att.total_mag.convert_rad2deg, t_low, t_high))); - - - - fprintf(fid_array(ii),'Position Knowledge Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (m): %f, %f, %f\n', maxerror(calcData.error.pos.knowledge.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Mean (m): %f, %f, %f\n', mean(calcData.error.pos.knowledge.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Mean + 3 Sigma (m): %f, %f, %f\n', mean3sig(calcData.error.pos.knowledge.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Standard Deviation (m): %f, %f, %f\n\n', std(calcData.error.pos.knowledge.timeRange(t_low,t_high))); - - fprintf(fid_array(ii),'Velocity Knowledge Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (m/s): %f, %f, %f\n', maxerror(calcData.error.velocity.knowledge.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Mean (m/s): %f, %f, %f\n', mean(calcData.error.velocity.knowledge.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Mean + 3 Sigma (m/s): %f, %f, %f\n', mean3sig(calcData.error.velocity.knowledge.timeRange(t_low,t_high))); - fprintf(fid_array(ii),'Standard Deviation (m/s): %f, %f, %f\n\n', std(calcData.error.velocity.knowledge.timeRange(t_low,t_high))); - - fprintf(fid_array(ii),'Attitude Knowledge Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (deg): %f\n', maxerror(timeRange(calcData.error.att.knowledge_mag.convert_rad2deg,t_low, t_high))); - fprintf(fid_array(ii),'Mean (deg): %f\n', mean(timeRange(calcData.error.att.knowledge_mag.convert_rad2deg,t_low, t_high))); - fprintf(fid_array(ii),'Mean + 3 Sigma (deg): %f\n', mean3sig(timeRange(calcData.error.att.knowledge_mag.convert_rad2deg,t_low, t_high))); - fprintf(fid_array(ii),'Standard Deviation (deg): %f\n\n', std(timeRange(calcData.error.att.knowledge_mag.convert_rad2deg,t_low, t_high))); - - -end - -if writeStats - fclose(fid_array(2)); -end - diff --git a/gnc/matlab/data_reduction/astrobee_load_data.m b/gnc/matlab/data_reduction/astrobee_load_data.m deleted file mode 100644 index 846be497ff..0000000000 --- a/gnc/matlab/data_reduction/astrobee_load_data.m +++ /dev/null @@ -1,258 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Astrobee Load Data -% -% -%% Load CSVs - -filepath = [pwd filesep]; -%csv_file_list = cellstr(ls([filepath '*.csv'])); -csv_file_list = dir([filepath '*.csv']); -csv_file_list = {csv_file_list.name}; - -if isempty(csv_file_list) - fprintf(2,'No CSV data files found to process\n'); - return; -end - -posixEpoch = '01/01/1970 00:00:00'; - -% Clear Structs to zero -clear out_* - -simData = 0; % Set variable which tells the system this is data off the robot -vive_lock = false; % Semaphore to deal with truth data possibly coming from multiple sources - -for current_file_num=1:length(csv_file_list) - current_file_name = csv_file_list{current_file_num}; %find the full file name - listing = dir(current_file_name); % Get info on the file - if listing.bytes == 0 % If the file is empty, - continue; % skip it - end - - if ~isempty(strfind(current_file_name, 'gnc_ekf')) - rawData = importdata([filepath current_file_name],',',1); - rawTimeData = rawData.textdata(2:end,3); - mat_out = zeros(length(rawTimeData),1); for ii=1:length(rawTimeData); mat_out(ii) = str2double(rawTimeData{ii}); end - time_data = ab_processTimeData(mat_out); %capture time data (in textdata) - - % Sometimes we see duplicate timestamps - duplicate_time_indx = find(diff(time_data) == 0); - rawData.data(duplicate_time_indx,:) = []; - time_data(duplicate_time_indx) = []; - - out_kfl_msg = []; - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3), 'P_B_ISS_ISS', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,4:7), 'quat_ISS2B', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,8:10), 'V_B_ISS_ISS', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,11:13), 'omega_B_ISS_B', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,14:16), 'gyro_bias', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,17:19), 'A_B_ISS_ISS', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,20:22), 'accel_bias', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,23:37), 'cov_diag', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,38), 'confidence', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,39), 'aug_state_enum', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,40), 'kfl_status', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,41), 'update_OF_tracks_cnt', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,42), 'update_ML_features_cnt', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,43:45), 'hr_P_hr_ISS_ISS', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,46:49), 'hr_quat_ISS2hr', out_kfl_msg); - - mahal_dist = rawData.data(:,50:99); - mahal_dist(mahal_dist == 0) = NaN; - out_kfl_msg = ab_populate_ts_field(time_data, mahal_dist, 'ml_mahal_distance', out_kfl_msg); - fprintf('Loaded %s\n', current_file_name); - - elseif ~isempty(strfind(current_file_name, 'loc_ml_features')) - try - [pos, att, num_features, t] = import_ml_features([filepath current_file_name]); - time_data = ab_processTimeData(t); %capture time data (in textdata) - out_loc_ml_features = []; - out_loc_ml_features = ab_populate_ts_field(time_data, pos, 'raw_pose_pos', out_loc_ml_features); - out_loc_ml_features = ab_populate_ts_field(time_data, att, 'raw_pose_quat', out_loc_ml_features); - out_loc_ml_features = ab_populate_ts_field(time_data, num_features, 'num_valid', out_loc_ml_features); - fprintf('Loaded %s\n', current_file_name); - - catch - fprintf(2, 'Unable to import localization mapped landmark features\n'); - end - - elseif ~isempty(strfind(current_file_name, 'gnc_ctl_command')) - - out_ctl_msg = []; - if ~exist('out_cmd_msg', 'var'); out_cmd_msg = []; end - - rawData = importdata([filepath current_file_name],',',1); - - rawTimeData = rawData.textdata(2:end,3); - mat_out = zeros(length(rawTimeData),1); for ii=1:length(rawTimeData); mat_out(ii) = str2double(rawTimeData{ii}); end - time_data = ab_processTimeData(mat_out); %capture time data (in textdata) - - % Sometimes we see duplicate timestamps - duplicate_time_indx = find(diff(time_data) == 0); - rawData.data(duplicate_time_indx,:) = []; - time_data(duplicate_time_indx) = []; - - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3), 'body_force_cmd', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,4:6), 'body_torque_cmd', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,7:9), 'body_accel_cmd', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,10:12), 'body_alpha_cmd', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,13), 'ctl_status', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,14:16), 'pos_err', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,17:19), 'pos_err_int', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,20:22), 'att_err', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,23:25), 'att_err_int', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,26), 'att_err_mag', out_ctl_msg); - - out_cmd_msg = ab_populate_ts_field(time_data, rawData.data(:,27), 'cmd_mode', out_cmd_msg); - fprintf('Loaded %s\n', current_file_name); - - - elseif ~isempty(strfind(current_file_name, 'vive')) - out_env_msg = []; - out_debug = []; - vive_lock = true; - rawData = importdata([filepath current_file_name],',',1); - rawTimeData = rawData.data(:,1); - time_data = ab_processTimeData(rawTimeData); %capture time data (in textdata) - % Some truth systems have duplicate timestamps - duplicate_time_indx = find(diff(time_data) == 0); - rawData.data(duplicate_time_indx,:) = []; - time_data(duplicate_time_indx) = []; - - true_axis_angle_axis = rawData.data(:,5:7); - true_axis_angle_angle = rssrow(rawData.data(:,5:7)); - true_axis_angle_axis = true_axis_angle_axis ./ repmat(true_axis_angle_angle, 1, 3); - small_indx = find(abs(true_axis_angle_angle) < 1E-3); - true_axis_angle_axis(small_indx,:) = repmat([1 0 0], length(small_indx), 1); - true_axis_angle_angle(small_indx,:) = 0; - truth_quat = axis_angle_to_quat(true_axis_angle_axis, true_axis_angle_angle); - out_debug = ab_populate_ts_field(time_data, rawData.data(:,5:7), 'Att_Axis_Angle', out_debug); - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,2:4), 'P_B_ISS_ISS', out_env_msg); - out_env_msg = ab_populate_ts_field(time_data, truth_quat, 'Q_ISS2B', out_env_msg); - - - fprintf('Loaded %s\n', current_file_name); - - elseif ~isempty(strfind(current_file_name, 'loc_truth_pose')) - if vive_lock - fprintf('Using Vive Data instead of loc_truth_pose data') - continue; - end - if ~exist('out_env_msg', 'var'); out_env_msg = []; end - rawData = importdata([filepath current_file_name],',',1); - rawTimeData = rawData.textdata(2:end,3); - mat_out = zeros(length(rawTimeData),1); for ii=1:length(rawTimeData); mat_out(ii) = str2double(rawTimeData{ii}); end - time_data = ab_processTimeData(mat_out); %capture time data (in textdata) - % Some truth systems have duplicate timestamps - duplicate_time_indx = find(diff(time_data) == 0); - rawData.data(duplicate_time_indx,:) = []; - time_data(duplicate_time_indx) = []; - - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3), 'P_B_ISS_ISS', out_env_msg); - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,4:7), 'Q_ISS2B', out_env_msg); - - fprintf('Loaded %s\n', current_file_name); - - - elseif ~isempty(strfind(current_file_name, 'loc_truth_twist')) - if vive_lock - fprintf('Using Vive Data instead of loc_truth_twist data') - continue; - end - if ~exist('out_env_msg', 'var'); out_env_msg = []; end - rawData = importdata([filepath current_file_name],',',1); - rawTimeData = rawData.textdata(2:end,3); - mat_out = zeros(length(rawTimeData),1); for ii=1:length(rawTimeData); mat_out(ii) = str2double(rawTimeData{ii}); end - time_data = ab_processTimeData(mat_out); %capture time data (in textdata) - % Some truth systems have duplicate timestamps - duplicate_time_indx = find(diff(time_data) == 0); - rawData.data(duplicate_time_indx,:) = []; - time_data(duplicate_time_indx) = []; - - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3), 'V_B_ISS_ISS', out_env_msg); - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,4:6), 'omega_B_ISS_B', out_env_msg); - - fprintf('Loaded %s\n', current_file_name); - - - elseif ~isempty(strfind(current_file_name, 'hw_imu')) - out_imu_msg = []; - rawData = importdata([filepath current_file_name],',',1); %import the data - rawTimeData = rawData.textdata(2:end,3); - mat_out = zeros(length(rawTimeData),1); for ii=1:length(rawTimeData); mat_out(ii) = str2double(rawTimeData{ii}); end - time_data = ab_processTimeData(mat_out); %capture time data (in textdata) - - out_imu_msg = ab_populate_ts_field(time_data, rawData.data(:,14:16), 'imu_omega_B_ECI_sensor', out_imu_msg); - out_imu_msg = ab_populate_ts_field(time_data, rawData.data(:,26:28), 'imu_A_B_ECI_sensor', out_imu_msg); - fprintf('Loaded %s\n', current_file_name); - - - - - elseif ~isempty(strfind(current_file_name, 'hw_pmc_command')) - out_act_msg = []; - rawData = importdata([filepath current_file_name],',',1); - rawTimeData = rawData.textdata(2:end,3); - mat_out = zeros(length(rawTimeData),1); for ii=1:length(rawTimeData); mat_out(ii) = str2double(rawTimeData{ii}); end - time_data = ab_processTimeData(mat_out); %capture time data (in textdata) - % Some truth systems have duplicate timestamps - duplicate_time_indx = find(diff(time_data) == 0); - rawData.data(duplicate_time_indx,:) = []; - time_data(duplicate_time_indx) = []; - - out_act_msg = ab_populate_ts_field(time_data, rawData.data(:,[2:7, 9:14]), 'act_servo_pwm_cmd', out_act_msg); - out_act_msg = ab_populate_ts_field(time_data, rawData.data(:,[1 8]), 'act_impeller_spped_cmd', out_act_msg); - - fprintf('Loaded %s\n', current_file_name); - - elseif ~isempty(strfind(current_file_name, 'gnc_control_feedback')) - if ~exist('out_cmd_msg', 'var'); out_cmd_msg = []; end - - rawData = importdata([filepath current_file_name],',',1); %import the data - rawTimeData = rawData.textdata(2:end,3); - mat_out = zeros(length(rawTimeData),1); for ii=1:length(rawTimeData); mat_out(ii) = str2double(rawTimeData{ii}); end - time_data = ab_processTimeData(mat_out); %capture time data (in textdata) - - duplicate_time_indx = find(diff(time_data) == 0); - rawData.data(duplicate_time_indx,:) = []; - time_data(duplicate_time_indx) = []; - - out_cmd_msg = ab_populate_ts_field(time_data, rawData.data(:,3:5), 'traj_pos', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData.data(:,6:9), 'traj_quat', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData.data(:,10:12), 'traj_vel', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData.data(:,13:15), 'traj_omega', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData.data(:,16:18), 'traj_accel', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData.data(:,19:21), 'traj_alpha', out_cmd_msg); - - fprintf('Loaded %s\n', current_file_name); - end - - -end - - -astrobee_load_sim_data; % Convert to telemData - -% Set the truth signals to be used as the masterTime base when -% interpolating -out_env_msg.Q_ISS2B.masterTime = true; -out_env_msg.P_B_ISS_ISS.masterTime = true; - - -%astrobee_post_process; diff --git a/gnc/matlab/data_reduction/astrobee_load_p4_data.m b/gnc/matlab/data_reduction/astrobee_load_p4_data.m deleted file mode 100644 index 54da8d8cca..0000000000 --- a/gnc/matlab/data_reduction/astrobee_load_p4_data.m +++ /dev/null @@ -1,201 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Load rosbag data in the current working directory -% To Do: -% Add delta of valid features (i.e. rejected features) -%% Load CSVs - -% Hack to deal with offsets in the dock room frame coordinates -% truth_offset = [.009166 -0.0253, .00753]; -truth_offset = [0 0 0]; - -filepath = [pwd filesep]; -%csv_file_list = cellstr(ls([filepath '*.csv'])); -csv_file_list = dir([filepath '*.csv']); -csv_file_list = {csv_file_list.name}; - -posixEpoch = '01/01/1970 00:00:00'; -% Sort of a hack -% % Initialize Structs to zero -out_cmd_msg = []; -out_act_msg_vpp = []; -out_ctl_msg = []; -out_env_msg = []; -out_kfl_msg = []; -out_cvs_registration_pulse = []; -out_imu_msg = []; -out_cvs_landmark_msg = []; -out_act_msg = []; - -simData = 0; % Set variable which tells the system this is data off the robot - -for current_file_num=1:length(csv_file_list) - current_file_name = csv_file_list{current_file_num}; %find the full file name - listing = dir(current_file_name); % Get info on the file - if listing.bytes == 0 % If the file is empty, - continue; % skip it - end -% dot_index = strfind(current_file_name,'.'); %find the name of the data -% underscore_indx = strfind(current_file_name,'_'); -% current_file_ID = [current_file_name(1:dot_index(end)-1)]; - - - if ~isempty(regexp(current_file_name, 'pmc_actuator_command.csv', 'ONCE')) - rawData = importdata(current_file_name); - time_data = ab_processTimeData(str2num(cell2mat(rawData.textdata(2:end,1)))); %capture time data (in textdata) - out_act_msg = ab_populate_ts_field(time_data, rawData.data(:,[1,8]), 'act_impeller_speed_cmd', out_act_msg); - out_act_msg = ab_populate_ts_field(time_data, rawData.data(:,[2:7,9:14]), 'act_servo_pwm_cmd', out_act_msg); - % Need to verify order of servos - - - - elseif ~isempty(regexp(current_file_name, 'localization_mapped_landmarks_features.csv', 'ONCE')) - try - rawData = importdata(current_file_name); - time_data = ab_processTimeData(rawData.data(:,1)); - validPoints = isfinite(rawData.data(:,13:5:258)); - out_cvs_landmark_msg = ab_populate_ts_field(time_data, validPoints, 'traj_pos', out_cvs_landmark_msg); - catch - fprintf(2, 'Unable to import localization mapped landmark features\n'); - end - - - elseif ~isempty(regexp(current_file_name, 'ctl_traj.csv', 'ONCE')) - rawData = csvread(current_file_name, 1, 0); - time_data = ab_processTimeData(rawData(:,1)); - - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,3:5), 'traj_pos', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,6:9), 'traj_quat', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,10:12), 'traj_vel', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,13:15), 'traj_omega', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,16:18), 'traj_accel', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,19:21), 'traj_alpha', out_cmd_msg); - - - elseif ~isempty(regexp(current_file_name, 'ctl_shaper.csv', 'ONCE')) - rawData = csvread(current_file_name, 1, 0); - time_data = ab_processTimeData(rawData(:,1)); - - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,3:5), 'cmd_pos', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,6:9), 'cmd_quat', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,10:12), 'cmd_vel', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,13:15), 'cmd_omega', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,16:18), 'cmd_accel', out_cmd_msg); - out_cmd_msg = ab_populate_ts_field(time_data, rawData(:,19:21), 'cmd_alpha', out_cmd_msg); - - -% elseif ~isempty(regexp(current_file_name, 'mapped_landmarks_registration.csv', 'ONCE')) -% rawData = csvread(current_file_name, 1, 0); -% time_data = ab_processTimeData(rawData(:,1)); -% -% out_cvs_registration_pulse = ab_populate_ts_field(time_data, rawData(:,2), 'out_cvs_registration_pulse', out_cvs_registration_pulse); -% - - - elseif ~isempty(regexp(current_file_name, 'ekf.csv', 'ONCE')) - rawData = importdata([filepath current_file_name],',',1); - time_data = ab_processTimeData(str2num(cell2mat(rawData.textdata(2:end,1)))); %capture time data (in textdata) - - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3), 'P_B_ISS_ISS', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,4:7), 'quat_ISS2B', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,8:10), 'V_B_ISS_ISS', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,11:13), 'omega_B_ISS_B', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,14:16), 'gyro_bias', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,17:19), 'A_B_ISS_ISS', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,20:22), 'accel_bias', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,23:37), 'cov_diag', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,38), 'confidence', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,39), 'aug_state_enum', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,40), 'kfl_status', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,41), 'update_OF_tracks_cnt', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,42), 'update_ML_features_cnt', out_kfl_msg); - - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,43:45), 'ml_P_cam_ISS_ISS', out_kfl_msg); - out_kfl_msg = ab_populate_ts_field(time_data, rawData.data(:,46:48), 'ml_quat_ISS2cam', out_kfl_msg); - - mahal_dist = rawData.data(:,78:127); - mahal_dist(mahal_dist == 0) = NaN; - out_kfl_msg = ab_populate_ts_field(time_data, mahal_dist, 'ml_mahal_distance', out_kfl_msg); - - - - - - elseif ~isempty(regexp(current_file_name, 'ctl_command.csv', 'ONCE')) - rawData = importdata([filepath current_file_name],',',1); %import the data - time_data = ab_processTimeData(str2num(cell2mat(rawData.textdata(2:end,1)))); %capture time data (in textdata) - - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3), 'body_force_cmd', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,4:6), 'body_torque_cmd', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,7:9), 'body_accel_cmd', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,10:12), 'body_alpha_cmd', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,13), 'ctl_status', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,14:16), 'pos_err', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,17:19), 'pos_err_int', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,20:22), 'att_err', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,23:25), 'att_err_int', out_ctl_msg); - out_ctl_msg = ab_populate_ts_field(time_data, rawData.data(:,26), 'att_err_mag', out_ctl_msg); - - - - - elseif ~isempty(regexp(current_file_name, 'truth.csv', 'ONCE')) % files that are not just numbers - rawData = importdata([filepath current_file_name],',',1); %import the data - time_data = ab_processTimeData(str2num(cell2mat(rawData.textdata(2:end,1)))); %capture time data (in textdata) - - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3) + repmat(truth_offset,length(rawData.data), 1) , 'P_B_ISS_ISS', out_env_msg); - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,4:7), 'Q_ISS2B', out_env_msg); - fprintf(2, 'Warning: A truth offset of %f, %f, %f was applied to the truth signal\n', truth_offset); - - elseif ~isempty(regexp(current_file_name, 'truth_accel.csv', 'ONCE')) % files that are not just numbers - rawData = importdata([filepath current_file_name],',',1); %import the data - time_data = ab_processTimeData(str2num(cell2mat(rawData.textdata(2:end,1)))); %capture time data (in textdata) - - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3) + repmat(truth_offset,length(rawData.data), 1) , 'A_B_ISS_B', out_env_msg); - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,4:6), 'alpha_B_ISS_B', out_env_msg); - fprintf(2, 'Warning: A truth offset of %f, %f, %f was applied to the truth signal\n', truth_offset); - - elseif ~isempty(regexp(current_file_name, 'truth_twist.csv', 'ONCE')) % files that are not just numbers - rawData = importdata([filepath current_file_name],',',1); %import the data - time_data = ab_processTimeData(str2num(cell2mat(rawData.textdata(2:end,1)))); %capture time data (in textdata) - - %out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,1:3) + repmat(truth_offset,length(rawData.data), 1) , 'A_B_ISS_B', out_env_msg); - out_env_msg = ab_populate_ts_field(time_data, rawData.data(:,4:6), 'omega_B_ECI_B', out_env_msg); - fprintf(2, 'Warning: A truth offset of %f, %f, %f was applied to the truth signal\n', truth_offset); -% elseif ~isempty(regexp(current_file_name, 'fam.csv', 'ONCE')) % files that are not just numbers -% rawData = importdata([filepath current_file_name],',',1); %import the data -% time_data = str2num(cell2mat(rawData.textdata(2:end,1))); %capture time data (in textdata) -% current_file_ID = 'fam'; - - elseif ~isempty(regexp(current_file_name, 'hw_imu.csv', 'ONCE')) - rawData = importdata([filepath current_file_name],',',1); %import the data - time_data = ab_processTimeData(str2num(cell2mat(rawData.textdata(2:end,1)))); %capture time data (in textdata) - - out_imu_msg = ab_populate_ts_field(time_data, rawData.data(:,14:16), 'imu_omega_B_ECI_sensor', out_imu_msg); - out_imu_msg = ab_populate_ts_field(time_data, rawData.data(:,26:28), 'imu_A_B_ECI_sensor', out_imu_msg); - - - - else - fprintf('Unknown File: %s\n', current_file_name) - end - -end - -% Converts time series to telemData -astrobee_load_sim_data; \ No newline at end of file diff --git a/gnc/matlab/data_reduction/astrobee_load_sim_data.m b/gnc/matlab/data_reduction/astrobee_load_sim_data.m deleted file mode 100644 index 4a2990d0b1..0000000000 --- a/gnc/matlab/data_reduction/astrobee_load_sim_data.m +++ /dev/null @@ -1,52 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Currently assumes the Astrobee sim has been run and the sim data is -% sitting in the workspace -% -%% -wsVars = whos; % Find all the variables in the workspace -structVars = strcmp('struct', {wsVars(:).class}); -structIndx = find(structVars); - -% Walk through all the structures in the workspace, looking for structures -% of time series objects -for ii = 1:length(structIndx) - testStruct = eval([wsVars(structIndx(ii)).name]); - % Simulink outputs are structures of timeseries and length 1 - if length(testStruct) ~= 1 - continue; - end - - % Test if the elements of the structure are timeseries - ts_test = structfun(@(x) (isa(x, 'timeseries')), testStruct, 'UniformOutput', true); - if all(ts_test) - % Replace the Time Series structure with the the telemData - % structure - assignin('base', [wsVars(structIndx(ii)).name], structfun(@(x) (telemData(x)), testStruct, 'UniformOutput', false)); - end -end - -%% Hack because it is currently non-recursive -if exist('out_cmc_msg', 'var') - out_cmc_msg.cmc_mode_cmd = telemData(out_cmc_msg.cmc_mode_cmd); - out_cmc_msg.cmc_state_cmd_a = structfun(@(x) (telemData(x)), out_cmc_msg.cmc_state_cmd_a, 'UniformOutput', false); - out_cmc_msg.cmc_state_cmd_b = structfun(@(x) (telemData(x)), out_cmc_msg.cmc_state_cmd_b, 'UniformOutput', false); -end - -clear wsVars structVars structIndx testStruct - diff --git a/gnc/matlab/data_reduction/astrobee_post_process.m b/gnc/matlab/data_reduction/astrobee_post_process.m deleted file mode 100644 index 39dda5d3eb..0000000000 --- a/gnc/matlab/data_reduction/astrobee_post_process.m +++ /dev/null @@ -1,419 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%% Post process the astrobee data -% Takes in simulation or astrobee data. Simulation data can be packed by -% running astrobee_load_sim_data, actual test data can be packaged into this -% format by running astrobee_load_test_data -% -% Currently assumes the data is packed in time series structures in the -% workspace. Supports: -% 'overview' - Overview plots -% 'est' - Estimater Specific plots -% 'cmd' - Command shaper plots -% 'mahal' - Plot mahalanobis distance -% 'stats' - generate stats -% 'traj_ctl' - Plots internal system errors off the trajector (shaper + ctl) -% 'docking' - -% 'saveMCdata' - Save off monte carlo test data -% -% -% To Do: -% Add rejected count due to mahal -% -% -function astrobee_post_process(varargin) -%% Import From Base -% The try statement is cheesy but easy -try; out_ex_time_msg = evalin('base', 'out_ex_time_msg'); end; -try; out_cvs_reg_pulse = evalin('base', 'out_cvs_reg_pulse'); end; -try; out_cvs_landmark_msg = evalin('base', 'out_cvs_landmark_msg'); end; -try; out_cvs_optflow_msg = evalin('base', 'out_cvs_optflow_msg'); end; -try; out_cvs_handrail_msg = evalin('base', 'out_cvs_handrail_msg'); end; -try; out_cmc_msg = evalin('base', 'out_cmc_msg'); end; -try; out_imu_msg = evalin('base', 'out_imu_msg'); end; -try; out_env_msg = evalin('base', 'out_env_msg'); end; -try; out_vpp_msg = evalin('base', 'out_vpp_msg'); end; -try; out_bpm_msg = evalin('base', 'out_bpm_msg'); end; -try; out_kfl_msg = evalin('base', 'out_kfl_msg'); end; -try; out_kfl_P = evalin('base', 'out_kfl_P'); end; -try; out_cmd_msg = evalin('base', 'out_cmd_msg'); end; -try; out_act_msg_vpp = evalin('base', 'out_act_msg_vpp'); end; -try; out_act_msg = evalin('base', 'out_act_msg'); end; -try; out_ctl_msg = evalin('base', 'out_ctl_msg'); end; -astrobee_version = evalin('base', 'astrobee_version'); -simData = evalin('base', 'simData'); -try; current_file_name = evalin('base', 'current_file_name'); end; - - - -%% Configure -%simData = 1; -estPlots = 1; -saveFigs = 0; -writeStats = 0; - -% Determine which data sets exist in the workspace -ds_on.cmd = exist('out_cmd_msg', 'var') && ~isempty(out_cmd_msg) && isfield(out_cmd_msg, 'traj_quat'); -ds_on.env = exist('out_env_msg', 'var') && ~isempty(out_env_msg); -ds_on.kfl = exist('out_kfl_msg', 'var') && ~isempty(out_kfl_msg); -ds_on.act = exist('out_act_msg', 'var') && ~isempty(out_act_msg); - -% Run any init files we need variables from -ASTROBEE_ROOT = evalin('base', 'ASTROBEE_ROOT'); -tunable_init; - -%% Parse Input Arguments -plotConfig = []; -if nargin == 0 - plotConfig.overview = 1; -else - if any(strcmpi(varargin, 'overview')); plotConfig.overview = 1; end - if any(strcmpi(varargin, 'est')); plotConfig.est = 1; end - if any(strcmpi(varargin, 'ctl')); plotConfig.ctl = 1; end - if any(strcmpi(varargin, 'cmd')); plotConfig.cmd = 1; end - if any(strcmpi(varargin, 'rqmnt')); plotConfig.rqmnt = 1; end - if any(strcmpi(varargin, 'stats')); plotConfig.stats = 1; end -end - -%% Derive Data -calcData = deriveAstroBeeData; -t0 = calcData.t0; - -try; out_env_msg = evalin('base', 'out_env_msg'); end; -assignin('base', 'calcData', calcData); -assignin('base', 't0', t0); - -%% Save Monte Carlo Data if desired -if any(strcmpi(varargin, 'saveMCdata')) - errorStats = calcData.error; - assignin('base', 'errorStats', errorStats); -end -%% Overview Plots -if ds_on.cmd && isfield(plotConfig, 'overview') - - plotError(out_env_msg.P_B_ISS_ISS, out_cmd_msg.traj_pos, 'Meters', 'Position', 'true', 'traj', '--', t0) - lims = get(gca, 'XLim'); - hold on; plot(lims, [.2 .2], 'r--', 'LineWidth', 2); plot(lims, [-.2 -.2], 'r--', 'LineWidth', 2); - - plotError(out_kfl_msg.P_B_ISS_ISS, out_cmd_msg.traj_pos, 'Meters', 'Commanded Position', 'est', 'traj', '--', t0) - lims = get(gca, 'XLim'); - hold on; plot(lims, [.1 .1], 'r--', 'LineWidth', 2); plot(lims, [-.1 -.1], 'r--', 'LineWidth', 2); - - - % Plot attitude error - %plotQuatsError(out_env_msg.veh_quat_eci2body.Time,out_env_msg.veh_quat_eci2body.Data, out_cmd_msg.cmd_curr_quat.Time, out_cmd_msg.cmd_curr_quat.Data, '', 'Commanded Attitude', 'true', 'cmd','--') - %plotQuatsError3(out_env_msg.Q_ISS2B, out_cmd_msg.traj_quat, out_cmd_msg.cmd_quat, 'Commanded Attitude', 'true', 'traj','cmd', '--', ':', t0) - plotQuatsError(out_env_msg.Q_ISS2B, out_cmd_msg.traj_quat, 'Commanded Attitude', 'true', 'traj', '--', t0); - lims = get(gca, 'XLim'); - hold on; plot(lims, [20 20], 'r--', 'LineWidth', 2); plot(lims, [-20 -20], 'r--', 'LineWidth', 2); - - - figure; plot(calcData.error.att.total_euler.convert_rad2deg, 't0', t0, 'Title', 'Attitude Error off commanded trajectory'); - grid on; %title('Attitude Error off commanded trajectory'); - legend('\theta_{error}', '\phi_{error}', '\psi_{error}'); - xlabel('time'); ylabel('Deg') - -end - -%% Docking plots -% Still a work in progress - -% Position of the midpoint between the dock lances at the same height as -% the dock lances -% -% n = length(out_env_msg.Q_ISS2B.time); -% t = out_env_msg.Q_ISS2B.time; -% P_dock_ISS = [-.49+.02921-5.75*.0254,.57,-.8573]; -% dock_normal_vec = telemData(t, repmat([1 0 0], n, 1)); % Unit vector normal to the astrobee adaptor, points towards astrobee from the dock in the world frame -% -% % Center point between the receptacles of the dock adaptor in the astrobee -% % body frame. In the same plane as the entrance to the dock receptacles. -% P_dockAdaptor_body_body = [-5.75*0.0254, 0, 0]; -% P_dockAdaptor_body_ISS = telemData(out_env_msg.Q_ISS2B.time, rotate_b2a(out_env_msg.Q_ISS2B.data,P_dockAdaptor_body_body)); -% P_dockAdaptor_ISS_ISS = P_dockAdaptor_body_ISS + out_env_msg.P_B_ISS_ISS; -% -% dock_error_ISS = repmat(P_dock_ISS, length(P_dockAdaptor_ISS_ISS.data), 1) - P_dockAdaptor_ISS_ISS.data; -% dock_error_ISS = telemData(out_env_msg.Q_ISS2B.time, dock_error_ISS); -% -% % Calc in track and out of track error for dock -% dock_inTrack_error = telemData(t, dot(dock_error_ISS.data, dock_normal_vec.data, 2)); -% dock_outTrack_error = dock_error_ISS - telemData(t, (repmat(dock_inTrack_error.data, 1, 3).*dock_normal_vec.data)); - - - -%% Actuator Plots -if ds_on.act && isfield(plotConfig, 'overview') - % Blower System - figure; - nozzle_cmds_r = telemData(out_act_msg.act_servo_pwm_cmd.time, out_act_msg.act_servo_pwm_cmd.data(:,1:6)); - nozzle_cmds_l = telemData(out_act_msg.act_servo_pwm_cmd.time, out_act_msg.act_servo_pwm_cmd.data(:,7:12)); - subplot(2, 1, 1); plot(nozzle_cmds_r, 't0',t0); title('Nozzle PWM Commands'); - ylabel('PWM, Right Side PM'); - legend(cellstr(num2str([1:6]', 'Nozzle %-d'))); - subplot(2, 1, 2); plot(nozzle_cmds_l, 't0',t0); title('Nozzle PWM Commands'); - legend(cellstr(num2str([7:12]', 'Nozzle %-d'))); - ylabel('PWM, Left Side PM'); - %set(h(6:12), 'LineStyle', '--'); - %ylabel('PWM'); legend(cellstr(num2str([1:12]', 'Nozzle %-d'))); - -end - -%% Controller Plots -if isfield(plotConfig, 'ctl') - plotError(out_kfl_msg.P_B_ISS_ISS, out_cmd_msg.cmd_pos, 'Meters', 'Control Position', 'est', 'cmd', '--', t0) - plotQuatsError(out_kfl_msg.quat_ISS2B, out_cmd_msg.cmd_quat, 'Control Attitude', 'est', 'cmd', '--', t0) - - figure; plot(out_ctl_msg.att_err_int, 't0', t0); legend('X_{int}', 'Y_{int}', 'Z_{int}'); - title('Attitude Integrated Error'); xlabel('seconds') - - % Mode - figure; plot(out_ctl_msg.ctl_status, 't0', t0); xlabel('seconds'); title('Control Mode'); -end - -%% Trajectory Control Errors -% Knowledge of how far astrobee is off the commanded trajectory, different -% than what we call control error because that is the output of the shaper -% to commanded. This rolls up hangoff error and shaper response errors. - -if any(strcmpi(varargin, 'traj_ctl')) - plotError(out_cmd_msg.traj_pos, out_kfl_msg.P_B_ISS_ISS, 'Meters', 'Trajectory Position Command', 'traj', 'est', '--', t0) - plotError(out_cmd_msg.traj_vel, out_kfl_msg.V_B_ISS_ISS, 'M/s', 'Trajectory Velocity Command', 'traj', 'est', '--', t0) - plotQuatsError(out_cmd_msg.traj_quat, out_kfl_msg.quat_ISS2B, 'Attitude Command', 'traj', 'est', '--', t0) - plotError(out_cmd_msg.traj_omega.convert_rad2deg, out_kfl_msg.omega_B_ISS_B.convert_rad2deg, 'deg/sec', 'Trajectory Body Rates Command', 'traj', 'est', '--', t0) - -end -%% Shaper Plots -if isfield(plotConfig, 'cmd') - plotError(out_cmd_msg.traj_pos, out_cmd_msg.cmd_pos, 'Meters', 'Position Command', 'traj', 'cmd', '--', t0) - plotQuatsError(out_cmd_msg.traj_quat, out_cmd_msg.cmd_quat, 'Attitude Command', 'traj', 'cmd', '--', t0) -end - -%% Estimator Plots -if isfield(plotConfig, 'est') - - % Attitude - figure; subplot(2,1,1); - plot(calcData.est_euler_deg, 't0', t0); - hold_on; - plot(calcData.truth_euler_deg, '--', 't0', t0) - title('Attitude Truth/Estimate Quaternion'); grid on; - ylabel('Euler Angles, Deg'); xlabel('seconds') - legend('\phi_{est}','\theta_{est}','\psi_{est}','\phi_{truth}', '\theta_{truth}', '\psi_{truth}') - r = gca; - set(gcf, 'Name', r.Title.String) - - subplot(2,1,2); - plot(calcData.error.att.knowledge_mag.convert_rad2deg, 't0', t0); - title('Attitude Error'); grid on; - ylabel('Error Magnitude, Deg'); xlabel('seconds') - - %figure; plot(out_kfl_msg.ml_mahal_distance) - - % Body Rates - plotError(out_kfl_msg.omega_B_ISS_B.convert_rad2deg, out_env_msg.omega_B_ISS_B.convert_rad2deg, 'Deg/sec', 'Body Rates Knowledge Error', 'est', 'truth', ':', t0) - - % % Add bias plot, rework with new IMU model reported bias - % figure%(3); clf; - % plot(out_kfl_msg.gyro_bias.Time, out_kfl_msg.gyro_bias.Data*180/pi); - % hold_on; title('Bias Estimation') - % plot([out_kfl_msg.gyro_bias.Time(1) out_kfl_msg.gyro_bias.Time(end)], [xim_gyro_bias_ic'; xim_gyro_bias_ic']*180/pi, ':'); - % legend('est_x', 'est_y', 'est_z', 'true_x', 'true_y', 'true_z'); ylabel('Deg/sec') - - % Position Error - plotError(out_kfl_msg.P_B_ISS_ISS, out_env_msg.P_B_ISS_ISS, 'Meters', 'Position Knowledge', 'est', 'truth', ':', t0) - - % Velocity - plotError(out_kfl_msg.V_B_ISS_ISS, out_env_msg.V_B_ISS_ISS, 'm/s', 'Velocity Knowledge (Global)', 'est', 'truth', ':', t0) - - plotError(calcData.true_lin_vel_body, calcData.est_lin_vel_body, 'm/s', 'Velocity Knowledge (Body)', 'true', 'est', '--', t0) - - % Acceleration - if simData - plotError(out_kfl_msg.A_B_ISS_ISS, out_env_msg.A_B_ISS_ISS, 'm/s^2', 'Acceleration Knowledge', 'est', 'truth', ':', t0) - end - % - - - figure; plot(out_kfl_msg.update_ML_features_cnt, 'ro','MarkerFaceColor', 'r', 't0', t0); hold on; - plot(out_kfl_msg.update_OF_tracks_cnt, 'bo','MarkerFaceColor', 'b', 't0', t0); - title('Number of features used for vision update'); ylabel('# of observations') - r = gca; legend('ML Features', 'OF Features') - set(gcf, 'Name', r.Title.String) - - % Kfl Status - figure; plot(out_kfl_msg.kfl_status, 'o', 't0', t0) - set(gca, 'YTick', [0 1 2]); - set(gca, 'YTickLabel', {'IMU', 'ML', 'OF'}); - title('Est Update Type') - r = gca; - set(gcf, 'Name', r.Title.String) - - % Acceleration Bias - figure%(7); clf; - plot(out_kfl_msg.accel_bias.time-t0, out_kfl_msg.accel_bias.data); - title('Accel Bias Estimation'); ylabel('M/s^2'); grid on; - r = gca; - set(gcf, 'Name', r.Title.String) - if simData - hold_on; - plot(out_imu_msg.imu_accel_bias, ':'); - legend('est_x', 'est_y', 'est_z', 'true_x', 'true_y', 'true_z'); - else - legend('est_x', 'est_y', 'est_z'); - end - - % Gyro Bias - figure%(7); clf; - plot(out_kfl_msg.gyro_bias.time-t0, out_kfl_msg.gyro_bias.data*180/pi); - title('Gyro Bias Estimation'); ylabel('Deg/s'); grid on; - r = gca; - set(gcf, 'Name', r.Title.String) - if simData - hold_on; - q_body2imu = quaternion(tun_abp_quat_body2imu); - true_imu_bias_body = telemData(out_imu_msg.imu_gyro_bias.time, rotate_b2a(q_body2imu, out_imu_msg.imu_gyro_bias)); - plot(true_imu_bias_body.convert_rad2deg, ':'); - legend('est_x', 'est_y', 'est_z', 'true_x', 'true_y', 'true_z'); - else - legend('est_x', 'est_y', 'est_z'); - end - - -end - -if isfield(plotConfig, 'mahal') - figure; plot(out_kfl_msg.ml_mahal_distance,'v', 't0', t0) - title('Mahalanobis Distance') - r = gca; - set(gcf, 'Name', r.Title.String) -end - -%% Process All Figures -all_axes_h = findobj('type', 'axes'); -if ~isempty(all_axes_h) - linkaxes(all_axes_h, 'x'); % Link all the X axes -end - -% Could apply the figure titles here - -%% Stats -% Dependent on current_file_name having been generated by -% astrobee_load_p3_data.m -if isfield(plotConfig, 'stats') - if simData - titleParsed = {datestr(now, 30)}; - else - titleParsed = strsplit(current_file_name, '_'); - end - - % Stats - fid_array = 1; - if writeStats - fid_array = [fid_array fopen([titleParsed{1} '_stats.txt'], 'w')]; - end - - for ii = 1:length(fid_array) - if ds_on.cmd && ds_on.env - fprintf(fid_array(ii),'\n'); - fprintf(fid_array(ii),'True Position Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (m): %f, %f, %f\n', maxerror(calcData.error.pos.total)); - fprintf(fid_array(ii),'Mean (m): %f, %f, %f\n', mean(calcData.error.pos.total)); - fprintf(fid_array(ii),'Mean + 3 Sigma (m): %f, %f, %f\n', mean3sig(calcData.error.pos.total)); - fprintf(fid_array(ii),'Standard Deviation (m): %f, %f, %f\n\n', std(calcData.error.pos.total)); - - % Calc Euler as well - fprintf(fid_array(ii),'True Attitude Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (deg): %f\n', maxerror(calcData.error.att.total_mag.convert_rad2deg)); - fprintf(fid_array(ii),'Mean (deg): %f\n', mean(calcData.error.att.total_mag.convert_rad2deg)); - fprintf(fid_array(ii),'Mean + 3 Sigma (deg): %f\n', mean3sig(calcData.error.att.total_mag.convert_rad2deg)); - fprintf(fid_array(ii),'Standard Deviation (deg): %f\n\n', std(calcData.error.att.total_mag.convert_rad2deg)); - end - - if ds_on.kfl && ds_on.env - fprintf(fid_array(ii),'Position Knowledge Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (m): %f, %f, %f\n', maxerror(calcData.error.pos.knowledge)); - fprintf(fid_array(ii),'Mean (m): %f, %f, %f\n', mean(calcData.error.pos.knowledge)); - fprintf(fid_array(ii),'Mean + 3 Sigma (m): %f, %f, %f\n', mean3sig(calcData.error.pos.knowledge)); - fprintf(fid_array(ii),'Standard Deviation (m): %f, %f, %f\n\n', std(calcData.error.pos.knowledge)); - - fprintf(fid_array(ii),'Velocity Knowledge Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (m/s): %f, %f, %f\n', maxerror(calcData.error.velocity.knowledge)); - fprintf(fid_array(ii),'Mean (m/s): %f, %f, %f\n', mean(calcData.error.velocity.knowledge)); - fprintf(fid_array(ii),'Mean + 3 Sigma (m/s): %f, %f, %f\n', mean3sig(calcData.error.velocity.knowledge)); - fprintf(fid_array(ii),'Standard Deviation (m/s): %f, %f, %f\n\n', std(calcData.error.velocity.knowledge)); - - fprintf(fid_array(ii),'Attitude Knowledge Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (deg): %f\n', maxerror(calcData.error.att.knowledge_mag.convert_rad2deg)); - fprintf(fid_array(ii),'Mean (deg): %f\n', mean(calcData.error.att.knowledge_mag.convert_rad2deg)); - fprintf(fid_array(ii),'Mean + 3 Sigma (deg): %f\n', mean3sig(calcData.error.att.knowledge_mag.convert_rad2deg)); - fprintf(fid_array(ii),'Standard Deviation (deg): %f\n\n', std(calcData.error.att.knowledge_mag.convert_rad2deg)); - end - - if ds_on.cmd && ds_on.env - [dataMax, dataMin, dataMean] = calcTPMvalues(calcData.error.pos.total.data,95); - fprintf(fid_array(ii),'95 True Position Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (m): %f, %f, %f\n', dataMax); - fprintf(fid_array(ii),'Min Error (m): %f, %f, %f\n', dataMin); - fprintf(fid_array(ii),'Mean Error (m): %f, %f, %f\n\n', dataMean); - - % Calc Euler as well - [dataMax, dataMin, dataMean] = calcTPMvalues(calcData.error.att.total_mag.convert_rad2deg.data,95); - fprintf(fid_array(ii),'95 True Attitude Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (deg): %f\n', dataMax); - fprintf(fid_array(ii),'Min Error (deg): %f\n', dataMin); - fprintf(fid_array(ii),'Mean Error (deg): %f\n\n', dataMean); - end - - if ds_on.kfl && ds_on.env - [dataMax, dataMin, dataMean] = calcTPMvalues(calcData.error.pos.knowledge.data,95); - fprintf(fid_array(ii),'95 Position Knowledge Error Stats:\n'); - fprintf(fid_array(ii),'Max Error (m): %f, %f, %f\n', dataMax); - fprintf(fid_array(ii),'Min Error (m): %f, %f, %f\n', dataMin); - fprintf(fid_array(ii),'Mean Error (m): %f, %f, %f\n\n', dataMean); - - fprintf(fid_array(ii),'95 Velocity Knowledge Error Stats:\n'); - [dataMax, dataMin, dataMean] = calcTPMvalues(calcData.error.velocity.knowledge.data,95); - fprintf(fid_array(ii),'Max Error (m/s): %f, %f, %f\n', dataMax); - fprintf(fid_array(ii),'Min Error (m/s): %f, %f, %f\n', dataMin); - fprintf(fid_array(ii),'Mean Error (m/s): %f, %f, %f\n\n', dataMean); - - fprintf(fid_array(ii),'95 Attitude Knowledge Error Stats:\n'); - [dataMax, dataMin, dataMean] = calcTPMvalues(calcData.error.att.knowledge_mag.convert_rad2deg.data,95); - fprintf(fid_array(ii),'Max Error (deg): %f\n', dataMax); - fprintf(fid_array(ii),'Min Error (deg): %f\n', dataMin); - fprintf(fid_array(ii),'Mean Error (deg): %f\n\n', dataMean); - end - end - - if writeStats - fclose(fid_array(2)); - end -end - -%% Save the figs if necesarry -% Dependent on current_file_name having been generated by -% astrobee_load_p3_data.m -if saveFigs - fig_Handles = findobj('type', 'fig'); - savefig(fig_Handles, [titleParsed{1} '_figs'], 'compact') -end - - - - - diff --git a/gnc/matlab/data_reduction/deriveAstroBeeData.m b/gnc/matlab/data_reduction/deriveAstroBeeData.m deleted file mode 100644 index 4a17fb8686..0000000000 --- a/gnc/matlab/data_reduction/deriveAstroBeeData.m +++ /dev/null @@ -1,328 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function calcData = deriveAstroBeeData -% Derive data from astrobee telemetry logs -%% Import Data from the base workspace -% Find out which data sets are "on", exist in the workspace -ds_on = evalin('caller', 'ds_on'); -simData = evalin('base', 'simData'); - - -firstTime = 0; -out_cmd_msg = evalin('base', 'out_cmd_msg'); -if exist('out_cmd_msg', 'var') && isfield(out_cmd_msg, 'traj_quat') - % If the cmd message is present, use the first command as t0 - firstTime = out_cmd_msg.traj_quat.time(find(rssrow(out_cmd_msg.traj_quat.data.value) ~= 0, 1, 'first')); -end - -if ds_on.env - out_env_msg = evalin('base', 'out_env_msg'); - firstTime = max(firstTime, out_env_msg.P_B_ISS_ISS.time(1)); -end - -if ds_on.kfl - out_kfl_msg = evalin('base', 'out_kfl_msg'); - firstTime = max(firstTime, out_kfl_msg.omega_B_ISS_B.time(1)); -end - -calcData.t0 = inf; -%% Do all ENV only calcs now -if ds_on.env - if ~isfield(out_env_msg, 'V_B_ISS_ISS') || ~isfield(out_env_msg, 'V_B_ISS_ISS') % If it is vehicle data derive the velocities - out_env_msg.V_B_ISS_ISS = telemData(out_env_msg.P_B_ISS_ISS.time(2:end), diff(out_env_msg.P_B_ISS_ISS.data)./ mean(diff(out_env_msg.P_B_ISS_ISS.time))); - %out_env_msg.V_B_ISS_ISS = telemData(out_env_msg.P_B_ISS_ISS.time(2:end), diff(out_env_msg.P_B_ISS_ISS.data)./ repmat(diff(out_env_msg.P_B_ISS_ISS.time), 1,3)); - - out_env_msg.omega_B_ISS_B = telemData(out_env_msg.Q_ISS2B.time, rate_from_quat(out_env_msg.Q_ISS2B.time, out_env_msg.Q_ISS2B.data.value)); - %TODO: Add filtering here - end - % Export this data back to the workspace - assignin('base', 'out_env_msg', out_env_msg); - - out_env_msg = structfun(@(a) timeRange(a, firstTime, inf), out_env_msg, 'UniformOutput', false); - calcData.t0 = min(calcData.t0, out_env_msg.P_B_ISS_ISS.time(1)); - - calcData.truth_euler_deg = telemData(out_env_msg.Q_ISS2B.time, convert_to_eulers(out_env_msg.Q_ISS2B.data) *180/pi); - - [Q_in_quat_time, V_in_quat_time] = sampleUniform(out_env_msg.Q_ISS2B, out_env_msg.V_B_ISS_ISS); - calcData.true_lin_vel_body = telemData(Q_in_quat_time.time, rotate_a2b(Q_in_quat_time.data, V_in_quat_time.data)); - -end - - -%% Docking Calcs -%% Docking plots -% Still a work in progress -if ds_on.env - % Position of the midpoint between the dock lances at the same height as - % the dock lances - % - n = length(out_env_msg.Q_ISS2B.time); - t = out_env_msg.Q_ISS2B.time; - P_dock_ISS = [-.49+.02921-5.75*.0254,.57,-.8573]; - dock_normal_vec = telemData(t, repmat([1 0 0], n, 1)); % Unit vector normal to the astrobee adaptor, points towards astrobee from the dock in the world frame - - % Center point between the receptacles of the dock adaptor in the astrobee - % body frame. In the same plane as the entrance to the dock receptacles. - P_dockAdaptor_body_body = [-5.75*0.0254, 0, 0]; - P_dockAdaptor_body_ISS = telemData(out_env_msg.Q_ISS2B.time, rotate_b2a(out_env_msg.Q_ISS2B.data,P_dockAdaptor_body_body)); - P_dockAdaptor_ISS_ISS = P_dockAdaptor_body_ISS + out_env_msg.P_B_ISS_ISS; - - dock_error_ISS = repmat(P_dock_ISS, length(P_dockAdaptor_ISS_ISS.data), 1) - P_dockAdaptor_ISS_ISS.data; - dock_error_ISS = telemData(out_env_msg.Q_ISS2B.time, dock_error_ISS); - - % Calc in track and out of track error for dock - dock_inTrack_error = telemData(t, dot(dock_error_ISS.data, dock_normal_vec.data, 2)); - dock_outTrack_error = dock_error_ISS - telemData(t, (repmat(dock_inTrack_error.data, 1, 3).*dock_normal_vec.data)); - - calcData.error.docking.inTrack = dock_inTrack_error; - calcData.error.docking.inTrack_final = dock_inTrack_error.data(end); - calcData.error.docking.outOfTrack = dock_outTrack_error.mag; - calcData.error.docking.outOfTrack_final = calcData.error.docking.outOfTrack.data(end); - calcData.error.docking.outOfTrack_vector = dock_outTrack_error; - - % Define cups in the body frame and lances in the ISS frame - P_cup1_B_B = [-5.75, 0, 1.25]*0.0254; - P_cup2_B_B = [-5.75, 0, -1.25]*0.0254; - P_lance1_ISS_ISS = [-.49+.02921-5.75*.0254, .57,-.8573+1.25*0.0254]; - P_lance2_ISS_ISS = [-.49+.02921-5.75*.0254, .57,-.8573-1.25*0.0254]; - - % Rotate the cups into the ISS frame - P_cup1_body_ISS = telemData(out_env_msg.Q_ISS2B.time, rotate_b2a(out_env_msg.Q_ISS2B.data,P_cup1_B_B)); - P_cup2_body_ISS = telemData(out_env_msg.Q_ISS2B.time, rotate_b2a(out_env_msg.Q_ISS2B.data,P_cup2_B_B)); - - % Shift the cup origins to be relative to ISS C.F. - P_cup1_ISS_ISS = P_cup1_body_ISS + out_env_msg.P_B_ISS_ISS; - P_cup2_ISS_ISS = P_cup2_body_ISS + out_env_msg.P_B_ISS_ISS; - - % Calc errors - cup1_error_ISS = repmat(P_lance1_ISS_ISS, length(P_cup1_ISS_ISS.data), 1) - P_cup1_ISS_ISS.data; - cup1_error_ISS = telemData(out_env_msg.Q_ISS2B.time, cup1_error_ISS); - cup2_error_ISS = repmat(P_lance2_ISS_ISS, length(P_cup2_ISS_ISS.data), 1) - P_cup2_ISS_ISS.data; - cup2_error_ISS = telemData(out_env_msg.Q_ISS2B.time, cup2_error_ISS); - - % Calc in track and out of track error for dock - cup1_inTrack_error = telemData(t, dot(cup1_error_ISS.data, dock_normal_vec.data, 2)); - cup1_outTrack_error = cup1_error_ISS - telemData(t, (repmat(cup1_inTrack_error.data, 1, 3).*dock_normal_vec.data)); - cup2_inTrack_error = telemData(t, dot(cup2_error_ISS.data, dock_normal_vec.data, 2)); - cup2_outTrack_error = cup2_error_ISS - telemData(t, (repmat(cup2_inTrack_error.data, 1, 3).*dock_normal_vec.data)); - - - calcData.error.docking.cup1_inTrack = cup1_inTrack_error; - calcData.error.docking.cup1_inTrack_final = cup1_inTrack_error.data(end); - calcData.error.docking.cup1_outOfTrack = cup1_outTrack_error.mag; - calcData.error.docking.cup1_outOfTrack_final = calcData.error.docking.cup1_outOfTrack.data(end); - calcData.error.docking.cup1_outOfTrack_vector = cup1_outTrack_error; - - calcData.error.docking.cup2_inTrack = cup2_inTrack_error; - calcData.error.docking.cup2_inTrack_final = cup2_inTrack_error.data(end); - calcData.error.docking.cup2_outOfTrack = cup2_outTrack_error.mag; - calcData.error.docking.cup2_outOfTrack_final = calcData.error.docking.cup2_outOfTrack.data(end); - calcData.error.docking.cup2_outOfTrack_vector = cup2_outTrack_error; - -end - -%% DO all KFL only calcs now -if ds_on.kfl - out_kfl_msg = structfun(@(a) timeRange(a, firstTime, inf), out_kfl_msg, 'UniformOutput', false); - calcData.t0 = min(calcData.t0, out_kfl_msg.omega_B_ISS_B.time(1)); - - calcData.est_euler_deg = telemData(out_kfl_msg.quat_ISS2B.time, convert_to_eulers(out_kfl_msg.quat_ISS2B.data) *180/pi); - calcData.est_lin_vel_body = telemData(out_kfl_msg.quat_ISS2B.time, rotate_a2b(out_kfl_msg.quat_ISS2B.data, out_kfl_msg.V_B_ISS_ISS.data)); - -end - -%% Do all the CMD only calcs now -if ds_on.cmd - out_cmd_msg = structfun(@(a) timeRange(a, firstTime, inf), out_cmd_msg, 'UniformOutput', false); - calcData.t0 = min(calcData.t0, out_cmd_msg.traj_quat.time(1)); -end -%calcData.t0 = min(min(out_env_msg.P_B_ISS_ISS.time), min(out_kfl_msg.omega_B_ISS_B.time)); - -%% Knowledge Errors -if ds_on.kfl && ds_on.env - calcData.error.att.knowledge_mag = out_env_msg.Q_ISS2B - out_kfl_msg.quat_ISS2B; - calcData.error.att.knowledge_quat = out_env_msg.Q_ISS2B * (-out_kfl_msg.quat_ISS2B); % Takes a long time to calc - calcData.error.att.knowledge_euler = telemData(calcData.error.att.knowledge_quat.time, convert_to_eulers(calcData.error.att.knowledge_quat.data)); - calcData.error.pos.knowledge = out_env_msg.P_B_ISS_ISS - out_kfl_msg.P_B_ISS_ISS; - calcData.error.velocity.knowledge = out_env_msg.V_B_ISS_ISS - out_kfl_msg.V_B_ISS_ISS; -end - -%% Total Errors -if ds_on.env && ds_on.cmd - % Total Errors - calcData.error.att.total_mag = out_env_msg.Q_ISS2B - out_cmd_msg.traj_quat; - calcData.error.att.total_quat = out_env_msg.Q_ISS2B * (-out_cmd_msg.traj_quat); % Takes a long time to calc - calcData.error.att.total_euler = telemData(calcData.error.att.total_quat.time, convert_to_eulers(calcData.error.att.total_quat.data)); - - calcData.error.pos.total = out_env_msg.P_B_ISS_ISS - out_cmd_msg.traj_pos; -end - -%% Trajectory Control Error -if ds_on.kfl && ds_on.cmd - % Total Errors - calcData.error.att.traj_ctl_mag = out_cmd_msg.traj_quat - out_kfl_msg.quat_ISS2B; - calcData.error.att.traj_ctl_quat = out_cmd_msg.traj_quat * (-out_kfl_msg.quat_ISS2B); % Takes a long time to calc - calcData.error.att.traj_ctl_euler = telemData(calcData.error.att.traj_ctl_quat.time, convert_to_eulers(calcData.error.att.traj_ctl_quat.data)); - calcData.error.omega.traj_ctl = out_cmd_msg.traj_omega - out_kfl_msg.omega_B_ISS_B; - - calcData.error.pos.traj_ctl = out_cmd_msg.traj_pos - out_kfl_msg.P_B_ISS_ISS; - calcData.error.velocity.traj_ctl = out_cmd_msg.traj_vel - out_kfl_msg.V_B_ISS_ISS; - -end - - -%% Control Errors -if ds_on.kfl && ds_on.cmd - calcData.error.att.control_mag = out_kfl_msg.quat_ISS2B - out_cmd_msg.traj_quat; - calcData.error.att.control_quat = out_kfl_msg.quat_ISS2B * (-out_cmd_msg.traj_quat); % Takes a long time to calc - calcData.error.att.control_euler = telemData(calcData.error.att.control_quat.time, convert_to_eulers(calcData.error.att.control_quat.data)); - - calcData.error.pos.control = out_kfl_msg.P_B_ISS_ISS - out_cmd_msg.traj_pos; -end - - -%% Requirments -if exist('out_env_msg', 'var') && exist('out_kfl_msg', 'var') - % FFREQ-622 - % The GN&C Subsystem shall estimate pose, velocity & acceleration within tolerances specified in the IRG-FF006-01 L2 Mobility Accuracy table. - calcData.req.FFREQ_622.att.thresh_deg = 10; % Requirement states 10 degrees - calcData.req.FFREQ_622.pos_thresh = .1; % Requirement states 10 cm - % TODO: Is this definition of control and estimation the same as mine? - % Requirment is 10 degrees and 10 cm of estimation error - calcData.req.FFREQ_622.att.maxError = max(calcData.error.att.knowledge_mag.convert_rad2deg); - if calcData.req.FFREQ_622.att.maxError < calcData.req.FFREQ_622.att.thresh_deg - calcData.req.FFREQ_622.att.pass = true; - else - calcData.req.FFREQ_622.att.pass = false; - end - - calcData.req.FFREQ_622.pos.maxError = max(calcData.error.pos.knowledge.mag); - if calcData.req.FFREQ_622.pos.maxError < calcData.req.FFREQ_622.pos_thresh - calcData.req.FFREQ_622.pos.pass = true; - else - calcData.req.FFREQ_622.pos.pass = false; - end - - if isempty(calcData.req.FFREQ_622.pos.pass) || isempty(calcData.req.FFREQ_622.att.pass) - calcData.req.FFREQ_622.pass = []; - elseif calcData.req.FFREQ_622.pos.pass && calcData.req.FFREQ_622.att.pass - calcData.req.FFREQ_622.pass = true; - else - calcData.req.FFREQ_622.pass = false; - end - - % FFREQ-623 - % The GN&C Subsystem shall estimate pose, velocity & acceleration within tolerances specified in the IRG-FF006-01 L2 Mobility Accuracy table. - calcData.req.control.att.thresh_deg = 20; % Requirement states 10 degrees - calcData.req.control.pos_thresh = .2; % Requirement states 10 cm - % TODO: Is this definition of control and estimation the same as mine? - % Requirment is 20 degrees and 20 cm of estimation error - if ds_on.env && ds_on.cmd - calcData.req.FFREQ_623.att.maxError = max(calcData.error.att.control_mag.convert_rad2deg); - % First test attitude error - if calcData.req.FFREQ_623.att.maxError < calcData.req.control.att.thresh_deg - calcData.req.FFREQ_623.att.pass = true; - else - calcData.req.FFREQ_623.att.pass = false; - end - - % Now test position error - calcData.req.FFREQ_623.pos.maxError = max(calcData.error.pos.control.mag); - if calcData.req.FFREQ_623.pos.maxError < calcData.req.control.pos_thresh - calcData.req.FFREQ_623.pos.pass = true; - else - calcData.req.FFREQ_623.pos.pass = false; - end - - % Report requirment as combination of the attitude and position - if isempty(calcData.req.FFREQ_623.pos.pass) || isempty(calcData.req.FFREQ_622.att.pass) - calcData.req.FFREQ_623.pass = []; - elseif calcData.req.FFREQ_623.pos.pass && calcData.req.FFREQ_622.att.pass - calcData.req.FFREQ_623.pass = true; - else - calcData.req.FFREQ_623.pass = false; - end - else - calcData.req.FFREQ_623.pass = []; - end - - - - - % FFREQ-620 - % The GN&C Subsystem shall have a maximum linear velocity of 0.5 m/s. - - if ~exist('out_cmd_msg.traj_vel', 'var') || max(out_cmd_msg.traj_vel.mag) < req.max_velocity - calcData.req.FFREQ_620.pass = []; - %fprintf('FFREQ-620: Not Tested\n'); - else - if max(out_env_msg.V_B_ISS_ISS.mag) >= req.max_velocity - calcData.req.FFREQ_620.pass = true; - %fprintf('FFREQ-620: Passed\n'); - else - calcData.req.FFREQ_620.pass = false; - %fprintf(2, 'FFREQ-620: Failed\n'); - end - calcData.req.FFREQ_620.max_speed_traj_t = [out_cmd_msg.traj_vel.time(1) out_cmd_msg.traj_vel.time(end)]; - %fprintf(' Max Speed: %f m/s\n', max(out_env_msg.V_B_ISS_ISS.mag)) - %plotError(mag(out_env_msg.V_B_ISS_ISS.timeRange(max_speed_traj_t(1), max_speed_traj_t(2))), mag(out_cmd_msg.traj_vel), 'm/s', '|Velocity|', 'true', 'traj', '--', max_speed_traj_t(1)); - %subplot(2,1,1); plot(max_speed_traj_t-max_speed_traj_t(1), [req.max_velocity req.max_velocity], 'r--', 'LineWidth', 2) - end - - - % Reqmn't FFREQ-616 - % The GN&C Subsystem shall halt robot motion within 7 seconds. - % Tested by setting the vehicle into stop mode at Max Velocity - if exist('out_env_msg', 'var') && exist('out_ctl_msg', 'var') - [test_vel, test_mode] = sampleUniform(out_env_msg.V_B_ISS_ISS, out_ctl_msg.ctl_status); - test_vel_mag = test_vel.mag; - test_max_vel_t = test_vel_mag.time(test_vel_mag.data > req.max_velocity); - stop_mode_t = test_mode.time(test_mode.data == 1); - overlap_vel_stop_t = intersect(test_max_vel_t, stop_mode_t); - - - if isempty(overlap_vel_stop_t) - calcData.req.FFREQ_616.pass = []; - %fprintf('FFREQ-616: Not Tested\n'); - else - start_stopping_t = min(overlap_vel_stop_t); % Find the first time we start to stop at max velocity - stopping_vel = timeRange(out_env_msg.V_B_ISS_ISS.mag, start_stopping_t, inf); % Extract just data during the stop - end_stopping_t = min(stopping_vel.time(stopping_vel.data < req.stop_threshold)); % Find the first time we are stopped - stop_time = end_stopping_t - start_stopping_t; - - if stop_time < req.stopping_time - calcData.req.FFREQ_616.pass = true; - %fprintf('FFREQ-616: Passed\n'); - else - calcData.req.FFREQ_616.pass = false; - %fprintf(2, 'FFREQ-616: Failed\n'); - end - %fprintf(' Stop Time: %f s\n', stop_time); - % figure; plot(stopping_vel); title('Min Stopping Time') - % lims = ylim; % Grab the ylimits of the plot - % hold on; plot(repmat(stopping_vel.time(1)+req.stopping_time, 2, 1), lims, 'r--', 'linewidth', 2) - % plot([stopping_vel.time(1) stopping_vel.time(end)], [req.stop_threshold req.stop_threshold], 'r--', 'linewidth', 2) - % ylim(lims); - - end - else - calcData.req.FFREQ_616.pass = []; - end - - -end \ No newline at end of file diff --git a/gnc/matlab/data_reduction/import_ml_features.m b/gnc/matlab/data_reduction/import_ml_features.m deleted file mode 100644 index 065749465b..0000000000 --- a/gnc/matlab/data_reduction/import_ml_features.m +++ /dev/null @@ -1,48 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - - -function [pos, att, num_features, t] = import_ml_features(fname) - -fid = fopen(fname); -tline = fgetl(fid); -tline = fgetl(fid); - -pos = []; -att = []; -num_features = []; -t = []; - -while ischar(tline) - arr = regexp(tline,'\,+','split'); - current_pos = str2double(arr(6:8)); - t = [t; str2double(arr{1})]; - current_pos = str2double(arr(6:8)); - current_att = str2double(arr(9:12)); - - if all([current_pos current_att] == 0) % If there are no features pos and att are all zero - num_features = [num_features; 0]; - pos = [pos; [NaN NaN NaN]]; - att = [att; [NaN NaN NaN NaN]]; - else - num_features = [num_features; (length(arr)-12)/5]; - pos = [pos; current_pos]; - att = [att; current_att]; - end - - tline = fgetl(fid); -end \ No newline at end of file diff --git a/gnc/matlab/data_reduction/interp_slerp.m b/gnc/matlab/data_reduction/interp_slerp.m deleted file mode 100644 index c30569a749..0000000000 --- a/gnc/matlab/data_reduction/interp_slerp.m +++ /dev/null @@ -1,81 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - - - -% Using axis angle as an intermediate step to do quaternion interpretation -% Loosely based on https://en.wikipedia.org/wiki/Slerp, but utilizing -% axis-angle to do linear interpolation of the angle. May yield different -% results than the common SLERP equations - -function q_out = interp_slerp(q_in, time_in, time_out) - -if isa(q_in, 'quaternion') % Check if the input quaternion is a quaternion class, we don't use this functionality presently - q_in = q_in.value; - use_quat_class = true; -else - use_quat_class = false; -end - -if ~all(diff(time_in) > 0) - error('Time is not monotonically increasing in sample') -end - -n = length(time_out); - -q_out = zeros(n,4); -for ii=1:n - if time_out(ii) > max(time_in) || time_out(ii) < min(time_in) - q_out(ii,:) = [NaN NaN NaN NaN]; - continue; - end - - if any(time_out(ii) == time_in) - match_time_indx = find(time_out(ii) == time_in, 1, 'first'); - q_out(ii,:) = q_in(match_time_indx,:); - else - % Consider adding min angle check, below which we just linearly - % interpolate - prev_time_indx = find(time_out(ii) > time_in, 1, 'last'); - q_prev = q_in(prev_time_indx, :); - q_next = q_in(prev_time_indx+1, :); - deltaT = (time_out(ii)-time_in(prev_time_indx))/(time_in(prev_time_indx+1)-time_in(prev_time_indx)); - - delta_q = quatmult(quat_inv(q_prev), q_next); % Calculate a quaternion that rotates from the previous quat to the next quat - % Add singularity protections here - - [axis, angle] = quat2axisAngle(delta_q); % Represent this delta quaternion as an axis/angle - - norm_angle = angle*deltaT; % Find what portion of the angle should used this step - - delta_q = axisAngle2quat(axis, norm_angle); % Calculate the corresponding quaternion - - q_out(ii,:) = quatmult(q_prev, delta_q); % Rotate q_prev by the new delta quaternion to get the interpolated quat - q_out(ii,:) = q_out(ii,:) ./ rssrow(q_out(ii,:)); % Normalize - - end - - - -end - -if use_quat_class - q_out = quaternion(q_out); -end - - - diff --git a/gnc/matlab/data_reduction/plotQuatsError.m b/gnc/matlab/data_reduction/plotQuatsError.m deleted file mode 100644 index d14a5ee8ac..0000000000 --- a/gnc/matlab/data_reduction/plotQuatsError.m +++ /dev/null @@ -1,57 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function plotQuatsError(data1, data2, titleStr, legend1, legend2,style2, t0) -% Prep for plot function in the telem class -figure;%(5); clf; -% Combine calcs into analysis file -R1 = data1.data.convert_to_eulers; -R2 = data2.data.convert_to_eulers; -qError = data1-data2; - -if t0 > 0 - t1 = data1.time - t0; - t2 = data2.time - t0; - te = qError.time - t0; -elseif t0 == 0 - t1 = data1.time; - t2 = data2.time; - te = qError.time; -else - t1 = datetime(data1.time, 'ConvertFrom', 'PosixTime'); - t2 = datetime(data2.time, 'ConvertFrom', 'PosixTime'); - te = datetime(qError.time, 'ConvertFrom', 'PosixTime'); -end - - -subplot(2,1,1); -plot(t1, R1*180/pi) -hold_on; -plot(t2, R2*180/pi, style2) -title(titleStr); grid on; -ylabel('Euler Angles, Deg'); xlabel('seconds') -legend([legend1 '_\theta'], [legend1 '_\phi'], [legend1 '_\psi'], [legend2 '_\theta'], [legend2 '_\phi'], [legend2 '_\psi']); - - -subplot(2,1,2); -plot(te, qError.data*180/pi); -title([titleStr ' Error']); grid on; -ylabel('Deg'); xlabel('seconds') - -% Set the figure title -r = gca; -set(gcf, 'Name', r.Title.String) diff --git a/gnc/matlab/data_reduction/plotQuatsError3.m b/gnc/matlab/data_reduction/plotQuatsError3.m deleted file mode 100644 index 0c3f748d81..0000000000 --- a/gnc/matlab/data_reduction/plotQuatsError3.m +++ /dev/null @@ -1,48 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function plotQuatsError3(time1,data1, time2, data2, time3, data3, titleStr, legend1, legend2,legend3, style2, style3) -% Prep for plot function in the telem class -figure;%(5); clf; -% Combine calcs into analysis file -R1 = quat_to_eulers( data1 ); -R2 = quat_to_eulers( data2 ); -R3 = quat_to_eulers( data3 ); - - - -subplot(2,1,1); -plot(time1, R1*180/pi) -hold_on; -plot(time2, R2*180/pi, style2) -hold_on; -plot(time3, R3*180/pi, style3) -title(titleStr); grid on; -ylabel('Euler Angles, Deg'); xlabel('seconds') -legend([legend1 '_\theta'], [legend1 '_\phi'], [legend1 '_\psi'], [legend2 '_\theta'], [legend2 '_\phi'], [legend2 '_\psi'],... - [legend3 '_\theta'], [legend3 '_\phi'], [legend3 '_\psi']); - - -subplot(2,1,2); -plot(time1, quat_error(data1,data2)/1000*180/pi); -title([titleStr ' Error']); grid on; -ylabel('Deg'); xlabel('seconds') - -% Set the figure title -r = gca; -set(gcf, 'Name', r.Title.String) - diff --git a/gnc/matlab/data_reduction/plotValsError.m b/gnc/matlab/data_reduction/plotValsError.m deleted file mode 100644 index a789538549..0000000000 --- a/gnc/matlab/data_reduction/plotValsError.m +++ /dev/null @@ -1,35 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function plotValsError(time1,data1, time2, data2, units, titleStr, legend1, legend2,style2) -% Prep for plot function in the telem class -interpData = 1; - -figure;%(5); clf; -subplot(2,1,1); -plot(time1, data1) -hold_on; -plot(time2, data2, style2) -title(titleStr); grid on; -ylabel(units); xlabel('seconds') -legend([legend1 '_x'], [legend1 '_y'], [legend1 '_z'], [legend2 '_x'], [legend2 '_y'], [legend2 '_z']); - -error_data = data1 - data2; -subplot(2,1,2); -plot(time1, (data1 - data2)); -title([titleStr ' Error']); grid on; -ylabel(units); xlabel('seconds') diff --git a/gnc/matlab/data_reduction/plot_shaded_error_bar.m b/gnc/matlab/data_reduction/plot_shaded_error_bar.m deleted file mode 100644 index 03acb2eca2..0000000000 --- a/gnc/matlab/data_reduction/plot_shaded_error_bar.m +++ /dev/null @@ -1,30 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function plot_shaded_error_bar(x, y, error_values) - - -for ii = 1:size(y,2) - - colorIndx = get(gca, 'ColorOrderIndex'); - colorOrder = get(gca, 'ColorOrder'); - fill([x; flipud(x)], [y(:,ii) - error_values(:,ii); flipud(y(:,ii) + error_values(:,ii))], colorOrder(colorIndx,:), 'linestyle', 'none', 'FaceAlpha', 0.5); - hold on; -end - -hold_on; -plot(x, y) \ No newline at end of file diff --git a/gnc/matlab/fsw_model/ase_augmented_state_estimator_prep.m b/gnc/matlab/fsw_model/ase_augmented_state_estimator_prep.m deleted file mode 100644 index f65607ccc4..0000000000 --- a/gnc/matlab/fsw_model/ase_augmented_state_estimator_prep.m +++ /dev/null @@ -1,32 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Astrobee Augmented State Estimator (ASE) prep file. Configures -% parameters used in the library file augmented_state_estimator. -% -%% Initial Conditions - -ase_total_num_states = double((15 + 6 + 6 * ase_of_num_aug.Value)); -ase_state_ic_cov_diag = ones(ase_total_num_states,1, 'single'); - -ase_state_ic_of_quat_ISS2cam = single(repmat([0 0 0 1],ase_of_num_aug.Value, 1)); -ase_state_ic_of_P_cam_ISS_ISS = zeros(ase_of_num_aug.Value,3, 'single'); - - %% Indices -ase_of_p_indx = 22:15 + 6 + 6 * ase_of_num_aug.Value; -ase_diag_indx = (1:ase_total_num_states:ase_total_num_states^2)+(0:ase_total_num_states-1); - diff --git a/gnc/matlab/fsw_model/ctl_controller/cex_control_executive/cex_control_executive_lib.slx b/gnc/matlab/fsw_model/ctl_controller/cex_control_executive/cex_control_executive_lib.slx deleted file mode 100644 index 6703a4ef17..0000000000 Binary files a/gnc/matlab/fsw_model/ctl_controller/cex_control_executive/cex_control_executive_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/ctl_controller/clc_closed_loop_controller_lib.slx b/gnc/matlab/fsw_model/ctl_controller/clc_closed_loop_controller_lib.slx deleted file mode 100644 index 70e844cdf8..0000000000 Binary files a/gnc/matlab/fsw_model/ctl_controller/clc_closed_loop_controller_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/ctl_controller/cmd_command_shaper/cmd_command_shaper_lib.slx b/gnc/matlab/fsw_model/ctl_controller/cmd_command_shaper/cmd_command_shaper_lib.slx deleted file mode 100644 index 1c06870522..0000000000 Binary files a/gnc/matlab/fsw_model/ctl_controller/cmd_command_shaper/cmd_command_shaper_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/ctl_controller/ctl_controller.slx b/gnc/matlab/fsw_model/ctl_controller/ctl_controller.slx deleted file mode 100644 index b7bcf818c0..0000000000 Binary files a/gnc/matlab/fsw_model/ctl_controller/ctl_controller.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/ctl_controller/ctl_controller_init.m b/gnc/matlab/fsw_model/ctl_controller/ctl_controller_init.m deleted file mode 100644 index 34d2abbc45..0000000000 --- a/gnc/matlab/fsw_model/ctl_controller/ctl_controller_init.m +++ /dev/null @@ -1,52 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Astrobee controller (CTL) initialization file. Configures -% parameters used in the library file ctl_controller. -% - -%% Block Configuration -ctl_ts = astrobee_fsw_step_size; - -%% Modes -ctl_idle_mode = uint8(0); -ctl_stopping_mode = uint8(1); -ctl_nominal_mode = uint8(2); -ctl_stopped_mode = uint8(3); - -%% Velocity Loop -ctl_linear_vel_limit = single(10000); %m/s, 0.5 m/s reads(3.GNC.22), need to rework - -%% Rate Loop (angular rates) -ctl_omega_limit = single(10000*pi/180*ones(3,1)); %rad/sec Still TBD (3.GNC.23) - -%% Thresholds -% %% Moved to lua conig file [ASTROBEE_ROOT '/../../management/astrobee/config/gnc.config'] -% ctl_stopping_vel_thresh = single(.01^2); % Linear velocity threshold squared below which the vehicle is considered to be stopped, (m/s)^2 -% ctl_stopping_omega_thresh = single(.01^2); % Angular velocity threshold squared below which the vehicle is considered to be stopped, (rad/s)^2 - -%% Moved to lua conig file [ASTROBEE_ROOT '/../../management/astrobee/config/gnc.config'] -% tun_ctl_bypass_cmd_shaper = uint8(0); -% ctl_att_ki = single([0.0013, 0.0013, 0.0013]); -% ctl_att_kp = single([0.2667, 0.2667, 0.2667]); -% ctl_pos_ki = single([0.0167, 0.0167, 0.0]); -% ctl_pos_kp = single([0.5333, 0.5333, 0.0]); -% ctl_pch_pos_k = 8; % 2 -% ctl_pch_att_k = 20; % 8 -% ctl_max_flap_theta = single(45*pi/180); % unrestricted = 64.23 deg -% ctl_pch_max_area = .75*[0.0027, 0.0027, 0.0015 ,0.0015 ,0.0015, 0.0015 ... % Max area before PCH starts clamping commands -% 0.0027, 0.0027, 0.0015, 0.0015, 0.0015, 0.0015]; diff --git a/gnc/matlab/fsw_model/ctl_controller/ctl_controller_prep.m b/gnc/matlab/fsw_model/ctl_controller/ctl_controller_prep.m deleted file mode 100644 index 31b0bc5671..0000000000 --- a/gnc/matlab/fsw_model/ctl_controller/ctl_controller_prep.m +++ /dev/null @@ -1,29 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Astrobee controller (CTL) initialization file. Configures -% parameters used in the library file ctl_controller. -% -%% Moved to lua conig file [ASTROBEE_ROOT '/../../management/astrobee/config/gnc.config'] -% tun_ctl_pos_Tp = single(ctl_pos_kp./tun_ctl_vel_kd); % Actual proportinal gain in Kp/Kd -% tun_ctl_pos_Ti = single(ctl_pos_ki./tun_ctl_vel_kd); % Actual integral gain in Ki/Kd -% ctl_att_Tp = single(ctl_att_kp./tun_ctl_omega_kd); % Actual proportinal gain in Kp/Kd -% ctl_att_Ti = single(ctl_att_ki./tun_ctl_omega_kd); % Actual integral gain in Ki/Kd - -if tun_tun_debug_ctl_use_truth - warning('Controller using ground truth instead of KFL estimate') -end \ No newline at end of file diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_augmented_state_estimator_hrn.slx b/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_augmented_state_estimator_hrn.slx deleted file mode 100644 index 55c1f65bc0..0000000000 Binary files a/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_augmented_state_estimator_hrn.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_augmented_state_estimator_init.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_augmented_state_estimator_init.m deleted file mode 100644 index 4ed8092870..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_augmented_state_estimator_init.m +++ /dev/null @@ -1,102 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Astrobee augmented state estimator (ase) initialization file. Configures -% parameters used in the library file augmented_state_estimator. - -%% Number of Augmented States -ase_of_num_aug = Simulink.Parameter(int32(16)); % Change here and need to update, cov_diag, of_quat_ISS2cam, and of_P_cam_ISS_ISS in the kfl_msg -ase_of_num_aug.CoderInfo.StorageClass = 'Custom'; -ase_of_num_aug.CoderInfo.CustomStorageClass = 'Define'; -ase_of_num_aug.CoderInfo.Alias = 'ASE_OF_NUM_AUG'; - -%also, change the cvs_optflow_num_hist, as well as the optical flow bus def -ase_ml_num_features = Simulink.Parameter(int32(50)); -ase_ml_num_features.CoderInfo.StorageClass = 'Custom'; -ase_ml_num_features.CoderInfo.CustomStorageClass = 'Define'; -ase_ml_num_features.CoderInfo.Alias = 'ASE_ML_NUM_FEATURES'; - -ase_of_num_features = Simulink.Parameter(int32(50)); -ase_of_num_features.CoderInfo.StorageClass = 'Custom'; -ase_of_num_features.CoderInfo.CustomStorageClass = 'Define'; -ase_of_num_features.CoderInfo.Alias = 'ASE_OF_NUM_FEATURES'; - -ase_aug_state_bitmask = uint32(bitsll(1,ase_of_num_aug.Value+1)-2); %bitmask used in est_estimator/camera_update to determine if all augmented states are valid - -%% Known Bias Parameters -ase_gyro_fixed_bias = single([0 0 0]); -ase_accel_fixed_bias = single([0 0 0]); - -%% Data Types -ase_cov_datatype = Simulink.AliasType; -ase_cov_datatype.BaseType = 'single'; - -%% Predictor Parameters -ase_ts = astrobee_fsw_step_size; %[sec] - -%% State enumnerations -ase_status_converged = uint8(0); -ase_status_acquiring = uint8(1); -ase_status_diverged = uint8(2); - -ase_local_mode_map = uint8(0); -ase_local_mode_docking = uint8(1); -ase_local_mode_perching = uint8(2); - -% KFL Status -% 1 = ML Update -% 2 = OF Update - -%% Earth Parameters (for ground testing only) -ase_earth_rate = single(0*[0, 0, 1]*((360/23.9344699)/3600)*pi/180); % Earth rate, rad/sec, in ECEF -ase_pos_vec = single(0*1.0e+06*[-2.6922, -4.2987, 3.8541]); % N269 Lab position in ECEF (lla2ecef([37.415117, -122.058621, 0])) - -%% Noise Parameters - -% Handrail Depth Sensor Errors -% ase_hr_distance_r = 0.01; % [meters] -% ase_hr_r_mag = 0.005; % Moved to tunable parameters, see tun_ase_hr_r_mag - - %% Measurments -ase_H = single([eye(4) zeros(4,12);zeros(3,13), eye(3)]); -ase_H_error = single([eye(3) zeros(3,12);zeros(3,12), eye(3)]); - -ase_minumum_resid_thresh = 1E-2; % Magnitude of the residual below which it is no longer worh it to itterate on the update step - -ase_max_update_iterations = 1; -ase_max_of_update_iterations = 1; - - %% Initial Conditions -% tun_ase_state_ic_quat_ISS2B = single([0 0 0 1]); moved to gnc.config -% tun_ase_state_ic_omega_B_ISS_B = single([0 0 0]); moved to gnc.config -ase_state_ic_gyro_bias = single([0 0 0]); -% tun_ase_state_ic_V_B_ISS_ISS = single([0 0 0]); moved to gnc.config -ase_state_ic_A_B_ISS_ISS = single([0 0 0]); -ase_state_ic_accel_bias = single([0 0 0]); -% ase_state_ic_P_B_ISS_ISS = single([0 0 0]); moved to gnc.config -ase_state_ic_confidence = ase_status_converged; % change this -ase_state_ic_aug_state_enum = uint8(0); -ase_state_ic_ml_quat_ISS2cam = single([0 0 0 1]); -ase_state_ic_ml_P_cam_ISS_ISS = single([0 0 0]); -ase_state_ic_status = ase_status_acquiring; - -%% Indices -% Indices inside the covariance (P) matrix which are associated with the IMU, Mapped Landmarks, and Optical Flow -ase_imu_p_indx = 1:15; -ase_ml_p_indx = 16:21; -% [Q bias_gyro V bias_accel P] - diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_hw_data_test.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_hw_data_test.m deleted file mode 100644 index 2273fcb136..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/ase_hw_data_test.m +++ /dev/null @@ -1,297 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Define the data path and truth paths respectively by defining the following variables in the workspace: -% data_path_in -% truth_path_in - -warning off MATLAB:table:ModifiedVarnames; -if ~exist('data_path_in', 'var') - data_path = 'ekf_inputs.csv'; - truth_path = 'ground_truth.csv'; -else - data_path = data_path_in; - truth_path = truth_path_in; -end - - -% reload parameters if changed -astrobee_version = 'p4'; -if ~exist('disable_config_overwrite', 'var') || ~disable_config_overwrite - tunable_init; - ase_augmented_state_estimator_init; - ase_augmented_state_estimator_prep; -end - -numOF_pts = 50; -numML_pts = 50; - -%% Telem Data -rawData = csvread(data_path); -%rawData = rawData(1:6000, :); -mlpTime = rawData(:,1) + rawData(:,2)*1E-9; -mlpTime = mlpTime(mlpTime > 0); -mlpTime = mlpTime - mlpTime(1); - -%% Truth Data -truthData = readtable(truth_path); % read as a table becasue there is text -truthData = table2cell(truthData); -if size(truthData) == [0 0] - truthTime = []; - truthPos = []; - truthQuat = []; -else - truthTime = [truthData{:,1}]'*1E-9; - truthPos = cell2mat(truthData(:,5:7)); - truthQuat = cell2mat(truthData(:, 8:11)); - %% Sync up truth data - truthTime = truthTime - truthTime(1); - %truthTime = truthTime + mlpTime(end) - truthTime(end); - - out_env_msg.P_B_ISS_ISS = timeseries(truthPos, truthTime); - out_env_msg.Q_ISS2B = timeseries(truthQuat, truthTime); - -end - -% Bus ICs -cvs_registration_pulse_IC = Simulink.Bus.createMATLABStruct('cvs_registration_pulse'); - -n = size(rawData,1); -%of1 -% ase_accel_fixed_bias = single([-0.0275 0.0339 -9.8173]); -% ase_gyro_fixed_bias = single(1e-3 * [-1.1599 0.0446 -0.5281]); -%of2 -%ase_accel_fixed_bias = single([-0.0156 0.0360 -9.8146]); -%ase_gyro_fixed_bias = single(1e-3 * [-1.1653 -0.01316 -0.7607]); - -%original_flat_fast -% ase_accel_fixed_bias = single([-0.0076 0.03759 -9.8149]); -% ase_gyro_fixed_bias = single(1e-3 * [-1.0787 0.00 0.4470]); -ase_accel_fixed_bias = single([-0.1425 0.0852 -9.8201]); -ase_gyro_fixed_bias = single(1e-3 * [-1.0787 0.00 0.4470]); - -% ase_enable_of = 1; -% ase_Q_imu(10:12,10:12) = 1E-12*eye(3,3,'single'); -% ase_of_r_mag = single((10 * ase_inv_focal_length)^2); - -% Might have to find the lowest time of all the times -% imu_t = rawData(:,1)+rawData(:,2)*1E-9; -% t0 = imu_t(1); -% imu_t = imu_t - t0; -%imu_t = mlpTime; -% we have to do this because otherwise matlab deletes points that are -% slighlty off... -imu_t = 0:0.016:0.016* (size(mlpTime, 1) - 1); -% if we don't do this matlab idiotically rounds things with floating point -% errors, causing us to drop registration pulses -imu_t = imu_t + 1e-6; -truthTime = truthTime + imu_t(end) - mlpTime(end); - -% we have to add a repeat of the first at zero because matlab is the -% dumbest program ever and will randomly add zeros as the first entry -% causing us to rapidly accelerate in z -test_imu_timestamp_sec = timeseries([0; uint32(rawData(:,1))], [0 imu_t]); -test_imu_timestamp_nsec = timeseries([0; uint32(rawData(:,2))], [0 imu_t]); -test_imu_A_B_ECI_sensor = timeseries([single(rawData(1,6:8)); single(rawData(:,6:8))], [0 imu_t]); -test_omega_B_ECI_sensor = timeseries([single(rawData(1,3:5)); single(rawData(:,3:5))], [0 imu_t]); - - -% No timestamp for registration found, using IMU timestamp for now -reg_t = imu_t; -test_cvs_landmark_pulse = timeseries(uint8(rawData(:, 9)), reg_t); -test_cvs_optical_flow_pulse = timeseries(uint8(rawData(:,10)), reg_t); - - -% ml_t = rawData(:,11)+rawData(:,12)*1E-9; -% ml_t = ml_t - t0; -ml_t = imu_t; - -test_cvs_timestamp_sec = timeseries(uint32(rawData(:,11)), ml_t); -test_cvs_timestamp_nsec = timeseries(uint32(rawData(:,12)), ml_t); - -landmark_pos = single(rawData(:,13:15)); -landmark_quat = single(rawData(:,16:19)); -cam_to_body = tun_abp_q_body2navcam; -cam_to_body(4) = -cam_to_body(4); -landmark_quat = quatmult(landmark_quat, repmat(cam_to_body, size(landmark_quat, 1), 1)); -landmark_pos = landmark_pos - quat_rotation_vec(repmat(tun_abp_p_navcam_body_body_sim, size(landmark_pos, 1), 1), landmark_quat); % Look at this value closer -landmark_pos = timeseries(landmark_pos, ml_t); -landmark_quat = timeseries(landmark_quat, ml_t); -landmarks = single(rawData(:,20:169)); -test_cvs_landmarks = timeseries(reshape(landmarks',50,3, n), ml_t); -test_obs = single(rawData(:,170:269)); -test_cvs_observations = timeseries(reshape(test_obs',50,2, n), ml_t); -test_cvs_valid_flag = timeseries(uint8(rawData(:,270:319)), ml_t); -if exist('disable_ml', 'var') && disable_ml - test_cvs_valid_flag.Data(:, :) = 0; -end -%test_cvs_valid_flag.Data(40 * 62.5:end, :) = 0; -%test_cvs_landmark_pulse.Data(1000:end, :) = 0; - -% of_t = rawData(:,313)+rawData(:,314)*1E-9; -% of_t = of_t - t0; -of_t = imu_t; - - -test_OF_cvs_timestamp_sec = timeseries(uint32(rawData(:,320)), of_t); -test_OF_cvs_timestamp_nsec = timeseries(uint32(rawData(:,321)), of_t); - -OF_obs = single(rawData(:,322:1921)); -test_OF_cvs_observations = timeseries(reshape(OF_obs', 50,2,16,n), of_t); - -OF_valid = uint8(rawData(:,1922:2721)); - -% % handrail part -hr_t = imu_t; -test_hr_pulse = timeseries(uint8(rawData(:, 2722)), hr_t); -test_hr_timestamp_sec = timeseries(uint32(rawData(:,2723)), hr_t); -test_hr_timestamp_nsec = timeseries(uint32(rawData(:,2724)), hr_t); - -test_hr_obs = single(rawData(:,2725:2874)); -test_hr_observations = timeseries(reshape(test_hr_obs',50, 3, n), hr_t); -test_hr_valid_flag = timeseries(uint8(rawData(:,2875:2924)), hr_t); -test_hr_rep_valid_flag = timeseries(uint8(rawData(:,2875)), hr_t); -hr_local_pos = single(rawData(:,2925:2927)); -test_hr_local_pos = timeseries(reshape(hr_local_pos',3,1, n), hr_t); -hr_local_quat = single(rawData(:,2928:2931)); -test_hr_local_quat = timeseries(reshape(hr_local_quat',4,1, n), hr_t); - -test_hr_3d_knowledge_flag = timeseries(uint8(rawData(:,2932)), hr_t); -test_hr_update_global_pose_flag = timeseries(uint8(rawData(:,2933)), hr_t); - -test_localization_mode_cmd = timeseries(uint8(rawData(:,2934)), hr_t); - -% invalid_corner = OF_obs > -test_OF_cvs_valid_flag = timeseries(reshape(OF_valid', 50,16,n), of_t); -if exist('disable_of', 'var') && disable_of - test_OF_cvs_valid_flag.Data(:, :) = 0; -end - -%% Configure Initial Conditions -if size(truthPos, 1) > 1 - tun_ase_state_ic_P_B_ISS_ISS = single(truthPos(1,:)); - tun_ase_state_ic_quat_ISS2B = single(truthQuat(1,:)); -else - first_obs_row = find(landmark_pos.Data(:, 1), 1); - tun_ase_state_ic_P_B_ISS_ISS = landmark_pos.Data(first_obs_row, :); - tun_ase_state_ic_quat_ISS2B = landmark_quat.Data(first_obs_row, :); -end - -q_ISS2B = quaternion(tun_ase_state_ic_quat_ISS2B); -p_imu_body_iss = rotate_b2a(q_ISS2B,tun_abp_p_imu_body_body); -tun_ase_state_ic_P_EST_ISS_ISS = tun_ase_state_ic_P_B_ISS_ISS + p_imu_body_iss; - -%% Setup Sim Parameters -test_stop_time = imu_t(end); - -%% Run Sim -if ~exist('show_results', 'var') || show_results - tic; -end -sim('ase_augmented_state_estimator_hrn'); -if ~exist('show_results', 'var') || show_results - toc; -end - -% prev_times_sec = test_cvs_timestamp_sec.Data(1:end-1); -% prev_times_nsec = test_cvs_timestamp_nsec.Data(1:end-1); -% changed = zeros(size(test_cvs_timestamp_sec.Data, 1), 1); -% changed(2:end) = or(prev_times_sec ~= test_cvs_timestamp_sec.Data(2:end), ... -% (prev_times_nsec ~= test_cvs_timestamp_nsec.Data(2:end))); -% ml_update = changed; -ml_update = out_kfl_msg.kfl_status.Data == 1; -of_update = out_kfl_msg.kfl_status.Data == 2; -ml_times = out_kfl_msg.ml_mahal_distance.Time(ml_update); -of_times = out_kfl_msg.of_mahal_distance.Time(of_update); -reg_times = ml_times; -% find the registration times of the previous registration pulse -indices = find(ml_update); -for i=1:size(ml_times, 1) - reg_time = 1; - for j = indices(i)-1:-1:1 - if out_cvs_reg_pulse.cvs_landmark_pulse.Data(j) - reg_time = out_cvs_reg_pulse.cvs_landmark_pulse.Time(j); - break; - end - end - reg_times(i) = reg_time; -end - -landmark_pos = getsamples(landmark_pos, ml_update); -landmark_quat = getsamples(landmark_quat, ml_update); - -landmark_pos.Time = reg_times; -landmark_quat.Time = landmark_pos.Time; - -%% Plots -if exist('fusco_plots', 'var') - figure; plot(out_env_msg.P_B_ISS_ISS, '--', 'LineWidth', 2) - hold_on; plot_shaded_error_bar(out_kfl_msg.P_B_ISS_ISS.Time, out_kfl_msg.P_B_ISS_ISS.Data, sqrt(out_kfl_msg.cov_diag.Data(:,13:15))); title('Position'); ylabel('m'); - - figure; plot_shaded_error_bar(out_kfl_msg.accel_bias.Time, out_kfl_msg.accel_bias.Data, sqrt(out_kfl_msg.cov_diag.Data(:,10:12))); title('Accel Bias'); ylabel('m/s^2'); - figure; plot_shaded_error_bar(out_kfl_msg.accel_bias.Time, out_kfl_msg.gyro_bias.Data, sqrt(out_kfl_msg.cov_diag.Data(:,4:6))); title('Gyro Bias'); ylabel('rad/s'); - -end - -% find the time difference between truth and ekf -if size(truthTime, 1) > 0 - truthOffset = 0; - angular_rmse = inf; - for i=-3:0.1:3 - new_times = resample(out_kfl_msg.quat_ISS2B, truthTime+i); - errors = quat_error(new_times.Data, truthQuat) / 1000 * 180 / pi; - errors(isnan(errors)) = []; - error = sqrt(sum(errors.^2) / size(errors, 1)); - if error < angular_rmse - angular_rmse = error; - truthOffset = i; - end - end - truthTime = truthTime + truthOffset; - - pos_truth_times = resample(out_kfl_msg.P_B_ISS_ISS, truthTime); - pos_errors = sqrt(sum((pos_truth_times.Data - truthPos)' .^ 2)); - pos_errors(isnan(pos_errors)) = []; - pos_rmse = sqrt(sum(pos_errors.^2) / size(pos_errors, 2)); - if ~exist('show_results', 'var') || show_results - fprintf('Pos RMSE: %g Quat RMSE: %g\n', pos_rmse, angular_rmse); - end -end - -if exist('show_graphs', 'var') && show_graphs - figure; plot(out_kfl_msg.P_B_ISS_ISS); - hold_on; plot(truthTime, truthPos, '--'); plot(landmark_pos, 'd'); hold off; - - figure; plot(out_kfl_msg.quat_ISS2B.Time, quat_to_eulers(out_kfl_msg.quat_ISS2B.Data) * 180 / pi); - hold_on; plot(truthTime, quat_to_eulers(truthQuat) * 180 / pi, 'x'); - plot(landmark_quat.Time, quat_to_eulers(landmark_quat.Data) * 180 / pi, 'd'); hold off; - - nonzero_ml = out_kfl_msg.update_ML_features_cnt.Data(:, 1)> 0; - nonzero_of = out_kfl_msg.update_OF_tracks_cnt.Data(:, 1) > 0; - title('Number of Observations'); - figure; plot(out_kfl_msg.update_ML_features_cnt.Time(nonzero_ml), out_kfl_msg.update_ML_features_cnt.Data(nonzero_ml), 'ro'); - hold on; plot(out_kfl_msg.update_OF_tracks_cnt.Time(nonzero_of), out_kfl_msg.update_OF_tracks_cnt.Data(nonzero_of), 'bx'); hold off; - - figure; hold on; - title('ML Mahalanobis Distances'); - boxplot(out_kfl_msg.ml_mahal_distance.Data(ml_update, :)', 'labels', ml_times, 'positions', ml_times); - hold off; -% figure; hold on; -% title('OF Mahalanobis Distances'); -% boxplot(out_kfl_msg.of_mahal_distance.Data(of_update, :)', 'labels', of_times, 'positions', of_times); -% hold off; -end diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/camera_update/camera_update_lib.slx b/gnc/matlab/fsw_model/est_estimator_wStateAug/camera_update/camera_update_lib.slx deleted file mode 100644 index 8bea650be3..0000000000 Binary files a/gnc/matlab/fsw_model/est_estimator_wStateAug/camera_update/camera_update_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/compute_hr_tf_and_landmark.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/compute_hr_tf_and_landmark.m deleted file mode 100644 index e7aa6d6c85..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/compute_hr_tf_and_landmark.m +++ /dev/null @@ -1,48 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [hr_global_landmarks, hr_P_hr_ISS_ISS, hr_quat_ISS2hr] = ... - compute_hr_tf_and_landmark(ml_P_cam_ISS_ISS, ml_quat_ISS2cam, ... - hr_P_hr_cam_cam, hr_quat_cam2hr, ... - hr_feature_observations, hr_feature_valid, hr_update_global_pose_flag) - - persistent hr_P_hr_ISS_ISS_pers; - persistent hr_quat_ISS2hr_pers; - - if isempty(hr_P_hr_ISS_ISS_pers) - hr_P_hr_ISS_ISS_pers = ml_P_cam_ISS_ISS + quaternion_to_rotation(ml_quat_ISS2cam)' * hr_P_hr_cam_cam; - end - if isempty(hr_quat_ISS2hr_pers) - hr_quat_ISS2hr_pers = quatmult(ml_quat_ISS2cam', hr_quat_cam2hr')'; - end - - if hr_update_global_pose_flag == 1 - hr_P_hr_ISS_ISS_pers = ml_P_cam_ISS_ISS + quaternion_to_rotation(ml_quat_ISS2cam)' * hr_P_hr_cam_cam; - hr_quat_ISS2hr_pers = quatmult(ml_quat_ISS2cam', hr_quat_cam2hr')'; - end - m = size(hr_feature_observations, 1); - hr_global_landmarks = single(zeros(m, 3)); - for j=1:m - if hr_feature_valid(j) == 0 - break; - end - ph = quaternion_to_rotation(hr_quat_cam2hr) * (hr_feature_observations(j,:)' - hr_P_hr_cam_cam); - hr_global_landmarks(j,:) = hr_P_hr_ISS_ISS_pers + quaternion_to_rotation(hr_quat_ISS2hr_pers)' * ph; - end - hr_P_hr_ISS_ISS = hr_P_hr_ISS_ISS_pers; - hr_quat_ISS2hr = hr_quat_ISS2hr_pers; - \ No newline at end of file diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/delay_ic_lib.slx b/gnc/matlab/fsw_model/est_estimator_wStateAug/delay_ic_lib.slx deleted file mode 100644 index 82809f7cf4..0000000000 Binary files a/gnc/matlab/fsw_model/est_estimator_wStateAug/delay_ic_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/apply_delta_to_state.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/apply_delta_to_state.m deleted file mode 100644 index e62f47929d..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/apply_delta_to_state.m +++ /dev/null @@ -1,31 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Apply delta state to the current state - -function [state] = apply_delta_to_state(state, delta) -%#codegen -state(1:4) = update_quaternion(state(1:4), delta(1:3)); - state(5:16) = state(5:16) + delta(4:15)'; - num_cameras = (size(state, 2) - 16) / 7; - for i=1:num_cameras - os = 17 + (i - 1) * 7; - od = 16 + (i - 1) * 6; - state(os:os+3) = update_quaternion(state(os:os+3), delta(od:od+2)); - state(os+4:os+6) = state(os+4:os+6) + delta(od+3:od+5)'; - end -end diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_augment_camera_of.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_augment_camera_of.m deleted file mode 100644 index 2fcd61885e..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_augment_camera_of.m +++ /dev/null @@ -1,116 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% This function add loads up the most recent -% camera attitude and position, as well as setting the aug_state_byte -function [state_out, P_out, of_vel_aug, of_omega_aug] = eml_augment_camera_of(P_cam_ISS_ISS, quat_ISS2cam, state_in, P_in, tun_abp_q_body2cam, tun_abp_p_cam_body_body, ase_total_num_states, ase_of_num_aug, replaced_aug) -%#codegen -ase_of_num_aug = double(ase_of_num_aug); % Cast this back to double to not cause issues with other integers - % retain the values between function calls - persistent aug_velocity aug_velocity_mag aug_omega aug_omega_mag; - if isempty(aug_velocity) || isempty(aug_velocity_mag) || isempty(aug_omega) || isempty(aug_omega_mag) - % Initialize values to zero and the same data type as state_in.V_B_ISS_ISS - aug_velocity = zeros(ase_of_num_aug, 3, 'like', state_in.V_B_ISS_ISS); - aug_velocity_mag = zeros(ase_of_num_aug,1, 'like', state_in.V_B_ISS_ISS); - aug_omega = zeros(ase_of_num_aug,3, 'like', state_in.omega_B_ISS_B); - aug_omega_mag = zeros(ase_of_num_aug,1, 'like', state_in.omega_B_ISS_B); - end - - - P = P_in; - state_out = state_in; - % Constants/variables I'll need later - imu_q_global = state_in.quat_ISS2B; - camera_rot_body = quaternion_to_rotation(tun_abp_q_body2cam'); - imu_rot_global = quaternion_to_rotation(imu_q_global); - - % Augment flag - % [MSB Oldest_OF_Aug ... Newest_OF_Aug Valid_ML_Augment(LSB)] - of_bit_mask = bitshift((2^ase_of_num_aug-1),1); % Mask off just the OF bits - of_bits = bitand(state_in.aug_state_enum, uint32(of_bit_mask)); % Extract out the bits associated with OF - ml_aug_bit = bitget(state_in.aug_state_enum, 1); - shifted_bits = bitshift(of_bits, 1); % Shift all the OF bits 1 bit more significant - of_valid_bits = bitset(shifted_bits, 2); % Set the newest OF bit as valid - of_valid_bits = bitand(of_valid_bits, uint32(of_bit_mask)); % Mask off the unused bits, remove rollover - state_out.aug_state_enum = uint32(bitor(of_valid_bits, ml_aug_bit)); % Restore the value of the ML aug bit - %new_jf = bitset(bitshift(bitand(jf, bin2dec('111110')), 1), 2) - - % Augment the state vector - % Ive arranged the state vector so it is: - % [IMU location, ML Camera Location, Newest OF Camera Location .... Oldest OF Camera Loc] - replaced_aug = ase_of_num_aug - replaced_aug + 1; % lowest number replaces most recent - kept_augmentations = zeros(1, ase_of_num_aug-1); - of_in_prange = zeros(1, 6 * (ase_of_num_aug - 1)); - j = 1; - for i=1:ase_of_num_aug-1 - if j == replaced_aug - j = j + 1; - end - kept_augmentations(i) = j; - of_in_prange(6 * (i-1)+1:6*i) = 6 * (j-1) + 21 + (1:6); - j = j + 1; - end - state_out.of_quat_ISS2cam(2:end, :) = state_out.of_quat_ISS2cam(kept_augmentations, :); - state_out.of_quat_ISS2cam(1, :) = quat_ISS2cam'; - state_out.of_P_cam_ISS_ISS = [P_cam_ISS_ISS'; state_out.of_P_cam_ISS_ISS(kept_augmentations,:)]; - - % Augment velocities - aug_velocity = [state_in.V_B_ISS_ISS'; aug_velocity(kept_augmentations,:)]; - aug_velocity_mag = [norm(state_in.V_B_ISS_ISS,'fro'); aug_velocity_mag(kept_augmentations,:)]; - aug_omega = [state_in.omega_B_ISS_B'; aug_omega(kept_augmentations,:)]; - aug_omega_mag = [norm(state_in.omega_B_ISS_B,'fro'); aug_omega_mag(kept_augmentations,:)]; - - of_vel_aug = double(aug_velocity); - of_omega_aug = double(aug_omega); - - % Augmenting the covariance matrix - % Move covariances down the stack: - % P = [A B C] then P+1 = [A 0 B] - % [D E F] [0 0 0] - % [G H J] [D 0 E] - imu_ml_prange = 1:21; - of_out_prange = 21 + 6 + 1:21 + 6 * ase_of_num_aug; - % Section D - P(of_out_prange, imu_ml_prange) = P(of_in_prange, imu_ml_prange); - % Section B - P(imu_ml_prange, of_out_prange) = P(imu_ml_prange, of_in_prange); - % Section E - P(of_out_prange, of_out_prange) = P(of_in_prange, of_in_prange); - % Now actually place an augmentation of IMU into OF - % M here is actually just the left part of J as defined in equation 24 in the - % Visinav paper by Mourikis '09 + space for the ML augmentation. - cov_datatype = class(P_in); - M = [cast(camera_rot_body, 'like', P) zeros(3, 12, cov_datatype) zeros(3, 6, cov_datatype); - cast(skew(imu_rot_global' * tun_abp_p_cam_body_body'), 'like', P) zeros(3, 9, cov_datatype) eye(3, cov_datatype) zeros(3, 6, cov_datatype)]; - of_new_prange = 15 + 6 + 1:15 + 6 + 6; -% J = [eye(21, size(P, 2)); M zeros(6, size(P, 2) - size(M, 2)); zeros(size(P, 1) - 27, 27) eye(size(P, 1) - 27, size(P, 2) - 27)]; -% P = J * P * J'; - % P = [A 0 C] then P+1 = [A AMt C ] - % [0 0 0] [MA MAMt MC] - % [G 0 J] [G GMt J ] - % Center section - P(of_new_prange, of_new_prange) = M * P(imu_ml_prange, imu_ml_prange) * M'; - % Top left sections - P(of_new_prange, imu_ml_prange) = M * P(imu_ml_prange, imu_ml_prange); - P(imu_ml_prange, of_new_prange) = P(of_new_prange, imu_ml_prange)'; - % Bottom right sections - P(of_new_prange, of_out_prange) = M * P(imu_ml_prange, of_out_prange); - P(of_out_prange, of_new_prange) = P(of_new_prange, of_out_prange)'; - - P_out = zeros(ase_total_num_states, cov_datatype); - P_out(1:ase_total_num_states,1:ase_total_num_states) = P(1:ase_total_num_states,1:ase_total_num_states); -end diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_delta_state_and_cov.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_delta_state_and_cov.m deleted file mode 100644 index d331cfbb2d..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_delta_state_and_cov.m +++ /dev/null @@ -1,73 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Calculate the delta State -function [error_out, delta_state, P_out] = eml_compute_delta_state_and_cov(residual, error_in, H, R_mat, P) -%#codegen -error_out = error_in; -if error_in - delta_state = zeros(size(P, 1), 1, 'single'); - P_out = P; - return; -end - -T = P * H'; -S = H*T + R_mat; -[C, p] = chol(S); - -% this should never happen! but if it does, fix P to be semipositive -% definite -if p ~= 0 - P = fix_covariance(P); - T = P * H'; - S = H*T + R_mat; - [C, p] = chol(S); -end - -if p ~= 0 - error_out = 1; - delta_state = zeros(size(P, 1), 1, 'single'); - P_out = P; - return; -end -cinv = inv(C); -sinv = cinv * cinv'; -K = T * sinv; % Kalman gain -delta_state = single(K * residual); - -F = (eye(size(P), 'like',P) - K * H); -% Update covariance -P_out = cast(F * P * F' + K * R_mat * K', 'like',P); % Ensure this is the same data type as P - -% Ensure that the covariance stays semipositive definite and symmetric. -P_out = 0.5 * (P_out + P_out');% + 1e-7 * eye(size(P_out)); - -end - -function [P_out] = fix_covariance(P) -% force all eigenvalues to be positive -[Vc, Dc] = eig(P); -V = real(Vc); -D = real(Dc); -for i = 1:size(D, 1) - if (D(i, i) <= 1e-6) - D(i, i) = 1e-6; - end -end -% recompose matrix from positive eigenvalues -P_out = V * D * V'; -end diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_hr_residual_and_h.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_hr_residual_and_h.m deleted file mode 100644 index b2f834010c..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_hr_residual_and_h.m +++ /dev/null @@ -1,105 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [r_out, error_out, H_out, R_mat] = eml_compute_hr_residual_and_h(ml_P_cam_ISS_ISS, ml_quat_ISS2cam, omega, velocity, L, O, valid, ase_of_num_aug, ase_hr_r_mag, ase_hr_distance_r, ase_ml_forward_projection_time, abp_q_body2cam, hr_quat_cam2hr, hr_3d_knowledge_flag) -%#codegen - error_out = int32(0); - if hr_3d_knowledge_flag == 1 - handrail_knowledge_dims = 3; - else - handrail_knowledge_dims = 2; - end - - % Get tf matrix that converts from global position (world frame) - % to local position (camera frame) - C = quaternion_to_rotation(ml_quat_ISS2cam); - camera_ml_tf_global = eye(3, 4); - camera_ml_tf_global(:, 1:3) = C; - camera_ml_tf_global(:, 4) = -camera_ml_tf_global(:, 1:3) * ml_P_cam_ISS_ISS; - - % Remove the invalid features from the observations and landmarks - O = O(logical(valid), :); % Observations from the camera frame - L = L(logical(valid), :); % Landmarks from the world frame - - % Convert global positions of the landmarks to the local positions - camera_landmarks = camera_ml_tf_global * [L'; ones(1, size(L, 1))]; % Landmarks in the camera frame - - % Create rotation matrix to convert between bases - % R rotation matrix that rotates from the camera frame into the handrail frame - % with the axis_body vector as the z axis unit vector - R = quaternion_to_rotation(hr_quat_cam2hr); -% R = eye(3); - - newerr = [R zeros(3, 1); zeros(1, 3) 1] * [O' - camera_landmarks; ones(1, size(L, 1))]; - r = reshape(newerr(1:handrail_knowledge_dims, :), size(O, 1) * handrail_knowledge_dims, 1); - - - - next_ml_tf_global = eye(3, 4); - % move to camera frame and with half a frame rotation - % TODO: we should use the velocity and omega from the time of the - % registration pulse - cam_omega = quat_rotation_vec(omega', abp_q_body2cam); - next_quat = quat_propagate_step(ml_quat_ISS2cam', cam_omega, ase_ml_forward_projection_time); - %camera_angle = eulers_to_quat(cam_omega(1), cam_omega(2), cam_omega(3)); - next_ml_tf_global(:, 1:3) = quaternion_to_rotation(next_quat'); - next_ml_tf_global(:, 4) = -camera_ml_tf_global(:, 1:3) * (ml_P_cam_ISS_ISS + ase_ml_forward_projection_time * velocity); - next_landmarks = next_ml_tf_global * [L'; ones(1, size(L, 1))]; % Landmarks in the camera frame - %z_next = next_landmarks; - % Find the difference between the current landmark locations and the - % distorted landmark locations due to time error, and rotate into the - % handrail frame - landmark_error_rail_frame = (R * (camera_landmarks - next_landmarks))'; - - omega_error = single(abs(reshape(landmark_error_rail_frame(:,1:handrail_knowledge_dims), size(O,1)*handrail_knowledge_dims,1))); - - - - % compute Jacobian - r_vec = zeros(size(O, 1) * handrail_knowledge_dims, 1); - H = zeros(handrail_knowledge_dims * size(O, 1), 6, 'single'); - for i=1:size(O, 1) - %temp = 1.0 ./ camera_landmarks(3, i) * [eye(2, 2) -z_est(:, i)]; - H_theta = eye(3) * skew(camera_landmarks(:, i)); - H_p = -eye(3) * camera_ml_tf_global(:, 1:3); - H_all = R * [H_theta H_p]; % rotate to new coordinate system - H(i*handrail_knowledge_dims-(handrail_knowledge_dims-1):i*handrail_knowledge_dims,:) = H_all(1:handrail_knowledge_dims, :); % drop last row, since we don't have knowledge along handrail axis - - - % make the confidence depend on mapped landmarked error which is function - % of distance - % [??] Should the camera_landmarks distance be the RSS, not just Z? - % r_vec is measurment noise - - r_vec(i*handrail_knowledge_dims-(handrail_knowledge_dims-1) : i*handrail_knowledge_dims) = ... - single(ase_hr_distance_r * norm(camera_landmarks(3)', 'fro') + ase_hr_r_mag ... - + omega_error(i*handrail_knowledge_dims-(handrail_knowledge_dims-1) : i*handrail_knowledge_dims) ... - ).^ 2; - end - - % Compress the r and H - [q1, T_H] = qr(H); - N = 6; - q1 = q1(:, 1:N); - - H = T_H(1:N, :); - r_out = single(q1' * r); - R_mat = single(q1' * diag(r_vec) * q1); - - % Convert H to be in terms of the entire state vector - rows = size(H, 1); - H_out = [H zeros(rows, 15 + 6 * ase_of_num_aug, 'single')]; diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_ml_residual_and_h.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_ml_residual_and_h.m deleted file mode 100644 index b42015e23f..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_ml_residual_and_h.m +++ /dev/null @@ -1,157 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [r_out, error_out, H_out, R_mat, numFeatures, mahal_dists] = eml_compute_ml_residual_and_h(ase_inv_focal_length, ase_distortion, ml_P_cam_ISS_ISS, ml_quat_ISS2cam, is_converged, omega, velocity, L, O, valid, P_in, mahal_distance_max, ase_min_ml_meas_tested, ase_min_ar_meas, ase_of_num_aug, ase_ml_num_features, ase_vis_r_mag, ase_map_error, abp_q_body2cam, ase_ml_forward_projection_time, ML_update, ase_vis_r_mag_AR, max_mahal_reject_frames) -%#codegen - persistent num_consecutive_rejected_meas - if isempty(num_consecutive_rejected_meas) - num_consecutive_rejected_meas = 0; - end - - - O = O(logical(valid), :); - L = L(logical(valid), :); - O = O .* ase_inv_focal_length; - - camera_ml_tf_global = eye(3, 4); - camera_ml_tf_global(:, 1:3) = quaternion_to_rotation(ml_quat_ISS2cam); - camera_ml_tf_global(:, 4) = -camera_ml_tf_global(:, 1:3) * ml_P_cam_ISS_ISS; - - camera_landmarks = camera_ml_tf_global * [L'; ones(1, size(L, 1))]; % Landmarks in the camera frame - - error_out = int32(0); - - z_est = camera_landmarks([1 2], :) ./ camera_landmarks([3 3], :); - - r = single(reshape(O' - z_est, size(O, 1) * 2, 1)); - - % forward propagate the saved state based on velocity to add an error - % term for the timing of the registration pulse - next_ml_tf_global = eye(3, 4); - % move to camera frame and with half a frame rotation - % TODO: we should use the velocity and omega from the time of the - % registration pulse - cam_omega = ase_ml_forward_projection_time * quat_rotation_vec(omega', abp_q_body2cam); - camera_angle = eulers_to_quat(cam_omega(1), cam_omega(2), cam_omega(3)); - next_ml_tf_global(:, 1:3) = quaternion_to_rotation(quatmult(camera_angle, ml_quat_ISS2cam')'); - next_ml_tf_global(:, 4) = -camera_ml_tf_global(:, 1:3) * ml_P_cam_ISS_ISS + ase_ml_forward_projection_time * velocity; - next_landmarks = next_ml_tf_global * [L'; ones(1, size(L, 1))]; % Landmarks in the camera frame - z_next = next_landmarks([1 2], :) ./ next_landmarks([3 3], :); - omega_error = single(abs(reshape(z_est - z_next, size(O, 1) * 2, 1))); - - % compute Jacobian - r_vec = zeros(size(O, 1) * 2, 1); - H = zeros(2 * size(O, 1), 6, 'single'); - for i=1:size(O, 1) - temp = 1.0 ./ camera_landmarks(3, i) * [eye(2, 2) -z_est(:, i)]; - H_theta = temp * skew(camera_landmarks(:, i)); - H_p = -temp * camera_ml_tf_global(:, 1:3); - H(i*2-1:i*2,:) = [H_theta H_p]; - - % account for distortion model - d2 = 2 * tan(ase_distortion / 2); - ru = ase_inv_focal_length * norm(O(i, :)); - f1 = ase_inv_focal_length; - f2 = ase_inv_focal_length; -% if ru > 1e-5 -% distorted = O(i, :) * tan(ru * ase_distortion) / d2 / ru; -% radius = norm(distorted); -% rd = radius * ase_inv_focal_length; -% t = tan(ase_distortion * rd); -% f1 = f1 * 1 / (d2 * radius) * (distorted(2)^2 * t / rd + ase_distortion * distorted(1)^2 * (1 + t^2)); -% f2 = f2 * 1 / (d2 * radius) * (distorted(1)^2 * t / rd + ase_distortion * distorted(2)^2 * (1 + t^2)); -% end - - if ML_update - % make the confidence depend on mapped landmarked error which is function - % of distance - r_vec(i*2-1:i*2) = single([f1; f2] .* (ase_map_error ./ camera_landmarks(3, i)' + ase_vis_r_mag + omega_error(i*2-1:i*2))) .^ 2; - else % AR update - r_vec(i*2-1:i*2) = single( [f1; f2] .* (ase_vis_r_mag_AR + omega_error(i*2-1:i*2))) .^ 2; - end - end - %r_out = zeros(size(L, 1),1); - mahal_dists = nan(ase_ml_num_features,1); - - num_original = size(r, 1); - % only ignore observations via Malanobis check in the following - % conditions: - % 1. The estimator is converged - % 2. The estiamtor is performing a ML update - % 3. The mahalanobis check hasn't previously rejected the maximum number - % of consecutive frame allowed to be rejected set via tun_max_mahal_reject_frames - if is_converged && ML_update && (num_consecutive_rejected_meas < max_mahal_reject_frames) -% S = H*P_in(16:21, 16:21)*H'; -% for i=1:size(S, 1) -% S(i, i) = S(i, i) + r_vec(i); -% end -% S_inv = pinv(S); - for jj = 1:2:size(L, 1)*2 - S = H(jj:jj+1, :) * P_in(16:21, 16:21) * H(jj:jj+1, :)'; - S = S + [r_vec(jj) 0; 0 r_vec(jj+1)]; - S_inv = pinv(S); - mahal_dist = sqrt(r(jj:jj+1)'*S_inv*r(jj:jj+1)); - mahal_dists((jj+1)/2) = mahal_dist; - end - invalid_vec = (mahal_dists(1:size(L, 1))' > mahal_distance_max); - % toss if nothing is within std. dev. of expected, ransac prob. failed -% if sum(mahal_dists < 3) == 0 -% invalid_vec(:) = true; -% end - invalid = reshape([invalid_vec; invalid_vec], size(L, 1)*2, 1); - r(invalid) = []; - H(invalid,:) = []; - r_vec(invalid) = []; - end - - numFeatures = uint8(size(r, 1)/2); - if ML_update - min_features = ase_min_ml_meas_tested; % Sparse map update - else - min_features = ase_min_ar_meas; % AR update - end - - % toss if not enough measurements, (Mahalanobis check removed all valid features) - % features - if size(r, 1)/2 < min_features || size(r, 1) < num_original / 2 - numFeatures = uint8(0); - error_out = int32(1); - total_num_states = 21 + (ase_of_num_aug*6); - r_out = ones(6, 1, 'single'); - H_out = eye(6, total_num_states, 'single'); - R_mat = eye(6, 6, 'single'); - num_consecutive_rejected_meas = num_consecutive_rejected_meas+1; - return; - else - num_consecutive_rejected_meas = 0; - end - - - - % Compress the r and H - [q1, T_H] = qr(H); - N = 6; - q1 = q1(:, 1:N); - - H = single(T_H(1:N, :)); - r_out = single(q1' * r); - R_mat = single(q1' * diag(r_vec) * q1); - - % Convert H to be in terms of the entire state vector - rows = size(H, 1); - H_out = [H zeros(rows, 15 + 6 * ase_of_num_aug, 'single')]; -end diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_of_global_points.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_of_global_points.m deleted file mode 100644 index 34c1a68d19..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_of_global_points.m +++ /dev/null @@ -1,122 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Code is taken from Zack Morrato's prototyped Optical Flow Kalman Filter. -% This code computes the residual and measurment matrices for the optical -% flow measurments. -function [ of_measured, global_points, camera_tf_global, valid_out, num_of_tracks ] = eml_compute_of_global_points(of_quat_ISS2CAM, of_P_cam_ISS_ISS, of_measured_in, valid, ase_inv_focal_length, ase_of_num_aug) -%#codegen - % for some reason matlab doesn't like creating matrix sizes based on "unbounded" parameters -% if ase_of_num_aug ~= 5 -% error('eml_compute_of_global_points is optimized for four augmented states.'); -% end -% ase_of_num_aug = 4; - ase_of_num_aug = double(ase_of_num_aug); - ase_of_num_features = size(valid, 1); - of_measured = zeros(2 * ase_of_num_features * ase_of_num_aug, 1, 'single'); - global_points = ones(4, ase_of_num_features, 'single'); - valid_out = zeros(ase_of_num_features, ase_of_num_aug, 'int32'); - - of_measured_in = of_measured_in .* ase_inv_focal_length; % Convert measurements to normalized units - % Note for some reason of_measured and valid can not match? Why? - - % Select out only features that are seen by three or more cameras - valid_in_all = sum(valid, 2) >= 3; - of_measured_in = of_measured_in(valid_in_all, :, :); - num_of_tracks = int32(size(of_measured_in, 1)); - valid_out(1:num_of_tracks, :) = valid(valid_in_all, :); - - % reshape the measurements so the order is by feature, x measure then y - % measure, with every camera observation one after the other. - of_measured(1:2 * num_of_tracks * ase_of_num_aug) = ... - reshape(reshape(permute(of_measured_in, [3 1 2]), ... - [ase_of_num_aug * num_of_tracks 2])', ... - [2 * ase_of_num_aug * num_of_tracks 1]); - - % Extract the 4x4 transforms which convert points from global frame to camera - % frame. I'll be stacking them vertically so they can all be evaluated - % simultaneously. - camera_tf_global = repmat(eye(4, 4, 'single'), ase_of_num_aug, 1); - for i = 1:ase_of_num_aug - rot_indices = 4*i-3:4*i-1; - camera_tf_global(rot_indices, 1:3) = ... - quaternion_to_rotation(of_quat_ISS2CAM(i,:)'); - camera_tf_global(rot_indices, 4) = ... - -camera_tf_global(rot_indices, 1:3) * of_P_cam_ISS_ISS(i,:)'; - end - - % Generate the transforms from camera 1 into all the other cameras. This is - % used for point triangulation. - camera_tf_camera1 = repmat(eye(4), ase_of_num_aug, 1); - global_tf_camera1 = pinv(camera_tf_global(1:4, 1:4)); % Used to be inv - for i = 1:ase_of_num_aug - camera_tf_camera1(4*i-3:4*i, 1:4) = ... - camera_tf_global(4*i-3:4*i, 1:4) * global_tf_camera1; - end - - % Triangulate all of the points - % for use interanlly, allocate outside for speed - A = zeros(2 * ase_of_num_aug, 3, 'single'); - b = zeros(2 * ase_of_num_aug, 1, 'single'); - for j = 1:num_of_tracks - % Solving for location of the point in the 3D world - - % Forming A and b matrices, used in a Ax=b to solve for alpha, beta, rho, - % an inverse depth parameterization of the points location in the 1st - % camera. - num_augs = sum(valid_out(j, :)); - aug_ind = 1; - for i = 1:num_augs - while ~valid_out(j, aug_ind) - aug_ind = aug_ind + 1; - end - ind0 = 4*aug_ind - 4; - uv = of_measured((j - 1) * 2 * ase_of_num_aug + 2 * aug_ind - 1: ... - (j - 1) * 2 * ase_of_num_aug + 2 * aug_ind, 1); - - A(2*i-1:2*i, 1:3) = ... - [camera_tf_camera1(ind0+1, 1) - camera_tf_camera1(ind0+3, 1) * uv(1), ... - camera_tf_camera1(ind0+1, 2) - camera_tf_camera1(ind0+3, 2) * uv(1), ... - camera_tf_camera1(ind0+1, 4) - camera_tf_camera1(ind0+3, 4) * uv(1); ... - camera_tf_camera1(ind0+2, 1) - camera_tf_camera1(ind0+3, 1) * uv(2), ... - camera_tf_camera1(ind0+2, 2) - camera_tf_camera1(ind0+3, 2) * uv(2), ... - camera_tf_camera1(ind0+2, 4) - camera_tf_camera1(ind0+3, 4) * uv(2)]; - b(2*i-1:2*i, 1) = ... - [camera_tf_camera1(ind0+3, 3) * uv(1) - camera_tf_camera1(ind0+1, 3); ... - camera_tf_camera1(ind0+3, 3) * uv(2) - camera_tf_camera1(ind0+2, 3)]; - aug_ind = aug_ind + 1; - end - % let's weight the oldest augmentation more heavily - A(2*num_augs-1:2*num_augs, :) = 5 * A(2*num_augs-1:2*num_augs, :); - b(2*num_augs-1:2*num_augs, :) = 5 * b(2*num_augs-1:2*num_augs, :); - inv_depth_p = pinv(A(1:num_augs * 2, :)) * b(1:num_augs * 2, :); - % solve Ax = B with guass newton - - if inv_depth_p(3) < 1e-5 - % This is a point that is truly in our face because we have no - % parallax on it. I only see this happen when using perfect camera - % positions and the robot is stationary (our noise usually makes it so - % that inverse depth is non zero). - inv_depth_p(3) = 1e-5; - end - - % Converting the inverse depth parameterization into the global coordinate - % frame. - global_points(1:3, j) = global_tf_camera1(1:3, :) * ([inv_depth_p(1:2) ./ inv_depth_p(3); 1 / inv_depth_p(3); 1]); - end - -end diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_of_residual_and_h.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_of_residual_and_h.m deleted file mode 100644 index 20dce6eb92..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_compute_of_residual_and_h.m +++ /dev/null @@ -1,190 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Code is taken from Zack Morrato's prototyped Optical Flow Kalman Filter. -% This code computes the residual and measurment matrices for the optical -% flow measurements. -function [ r, H, error_out, num_of_tracks, mahal_dists ] = eml_compute_of_residual_and_h(of_measured, global_points, camera_tf_global, valid, ase_of_num_aug, ase_of_num_features, tun_ase_mahal_distance_max, ase_of_r_mag, P) -%#codegen - -% if ase_of_num_aug ~= 5 -% error('eml_compute_of_residual_and_h is optimized for four augmented states.'); -% end -% ase_of_num_aug = 4; - - error_out = 0; - num_of_tracks = size(global_points, 2); - - z_indices = zeros(1, 2 * ase_of_num_aug); - z_indices(1:2:end) = 3:4:4 * ase_of_num_aug; - z_indices(2:2:end) = 3:4:4 * ase_of_num_aug; - - xy_indices = zeros(1, 2 * ase_of_num_aug); - xy_indices(1:2:end) = 1:4:4 * ase_of_num_aug; - xy_indices(2:2:end) = 2:4:4 * ase_of_num_aug; - - % Find the reprojection location for all of the points, vectorized. And also calculate the error. - camera_landmarks = camera_tf_global * global_points; - z_est = camera_landmarks(xy_indices, :) ./ camera_landmarks(z_indices, :); - r_uncompressed = of_measured - reshape(z_est, 2 * ase_of_num_aug * num_of_tracks, 1); - - % forward propagate the saved state based on velocity to add an error - % term for the timing of the registration pulse - %next_ml_tfs_global = zeros(4 * ase_of_num_aug, 4); - % move to camera frame and with half a frame rotation - %for i=1:ase_of_num_aug - %cam_omega = ase_of_forward_projection_time * quat_rotation_vec(of_omega_aug(i, :)', abp_q_body2cam); - %camera_angle = eulers_to_quat(cam_omega(1), cam_omega(2), cam_omega(3)); - %next_ml_tfs_global(4*i-3:4*i-1, 1:3) = quaternion_to_rotation(camera_angle'); - %next_ml_tfs_global(4*i-3:4*i, 4) = [ase_of_forward_projection_time * of_vel_aug(i, :)'; 1]; - %next_ml_tfs_global(4*i-3:4*i, :) = next_ml_tfs_global(4*i-3:4*i, :) * camera_tf_global(4 * i - 3:4 * i, :); - %end - %next_landmarks = next_ml_tfs_global * global_points; % Landmarks in the camera frame - %z_next = next_landmarks(xy_indices, :) ./ next_landmarks(z_indices, :); - %omega_error = single(abs(reshape(z_est - z_next, size(r_uncompressed, 1), 1))); - - r = zeros((2 * ase_of_num_aug - 3) * num_of_tracks, 1, 'single'); - H = zeros((2 * ase_of_num_aug - 3) * num_of_tracks, 6 * ase_of_num_aug, 'single'); - mahal_dists = nan(ase_of_num_features, 1); % Allocate to maximum size - %R_mat = zeros(size(H, 1), size(H, 1), 'single'); - - cur_row = 1; - % Calculate measurement jacobians and compression from Hf - for j = 1:num_of_tracks - % Copy from residuals as we'll do givens in place - r_j = r_uncompressed((j-1) * 2 * ase_of_num_aug + 1: ... - j * 2 * ase_of_num_aug); - r_j = r_j(logical(reshape([valid(j, :); valid(j, :)], size(r_j, 1), 1))); - %R_mat_j = ase_of_r_mag .^ 2 * eye(size(r_j, 1), size(r_j, 1)); - - % Calculate H_theta H_p and H_f - H_f_j = zeros(size(r_j, 1), 3, 'single'); - H_x_j = zeros(size(r_j, 1), ase_of_num_aug * 6, 'single'); - % This is not really all of H_x .. this is just - % the part of H that covers the OF camera augmentations. - aug_ind = 1; - for i = 1:size(r_j, 1) / 2 - while ~valid(j, aug_ind) - aug_ind = aug_ind + 1; - end - ind0 = 4 * aug_ind - 4; - prefix = (1.0 ./ camera_landmarks(ind0+3, j)) * [eye(2, 2) -z_est(2*aug_ind-1:2*aug_ind, j)]; - H_theta_ji = prefix * skew(camera_landmarks(ind0+1:ind0+3, j)); - H_p_ji = -prefix * camera_tf_global(ind0+1:ind0+3, 1:3); - - % Fill in H_x - H_x_j(i*2-1:i*2, aug_ind*6-5:aug_ind*6) = [H_theta_ji H_p_ji]; - - % Fill in H_f - H_f_j(i*2-1:i*2, :) = -H_p_ji; - aug_ind = aug_ind + 1; - end - - % Project everything on to the left nullspace of H_f - % - % The direct approach is: - % A = null(H_f') - % H_x_of = A' * H_x and r_of = A' * r_j - % - % The nullspace can be calculated quickly with QR, we just need to remember - % to remove a few extract columns in Q (which is the nullspace). QR - % decomposition can be calculated quickly with Givens rotations. And .. - % finally Givens rotations can be done in place. So all this mess you see - % next is because we're doing nullspace projection in place using the - % Given's rotation, which is a fix number of observations. - % - % This is really fast in C/C++. Matlab, I dunno .. probably not because of - % the loop. Simulink Autogenerated code? Let's hope? - - % first, make each half of H_f_j (stacked vertically) upper triangular -% for col = 1:3 -% for row = 4:-1:(col+1) -% [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [row-1, row], col, col:3, 1:12); -% end -% end -% for col = 1:3 -% for row = 4:-1:(col+1) -% [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [4+row-1, 4+row], col, col:3, 13:24); -% end -% end -% % now we have [x x x; 0 x x; 0 0 x; 0 0 0; x x x; 0 x x; 0 0 x; 0 0 0] -% [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [1, 5], 1, 1:3, 1:24); -% % [x x x; 0 x x; 0 0 x; 0 0 0; 0 x x; 0 x x; 0 0 x; 0 0 0] -% [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [2, 5], 2, 2:3, 1:24); -% [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [2, 6], 2, 2:3, 1:24); -% % [x x x; 0 x x; 0 0 x; 0 0 0; 0 0 x; 0 0 x; 0 0 x; 0 0 0] -% [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [3, 5], 2, 3:3, 1:24); -% [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [3, 6], 2, 3:3, 1:24); -% [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [3, 7], 2, 3:3, 1:24); - - % original version - for col = 1:3 - for row = size(r_j, 1):-1:(col+1) - [H_f_j, H_x_j, r_j] = apply_givens(H_f_j, H_x_j, r_j, [row-1, row], col, col:size(H_f_j, 2), 1:size(H_x_j, 2)); - end - end - - % We now discard the top 3 rows ... (this follows with the algorithm for - % calculating the nullspace using QR decomposition.) However I believe you - % can also think of this as 3 DoF loss due to the EKF not solving for this - % feature's physical location. We can do this because we projected on the - % nullspace of H_f. - %dim = rank(H_f_j); - dim = 3; - r_j = r_j(1+dim:end); - %R_mat_j = R_mat_j(1+dim:end, 1+dim:end); - H_x_j = H_x_j(1+dim:end, :); - - % check mahalanobis distance now - S = H_x_j * P(22:21 + ase_of_num_aug * 6, 22:21 + ase_of_num_aug * 6) * H_x_j'; - S = S + ase_of_r_mag .^ 2 * eye(size(S, 1), size(S, 2)); - S_inv = pinv(S); - mahal_dist = sqrt(r_j'*S_inv*r_j); - mahal_dists(j) = mahal_dist; - % don't use feature if distance too great - if mahal_dist > tun_ase_mahal_distance_max - continue; - end - - % Place our solution for j in the big r vector and big H matrices. - next_row = cur_row + size(r_j, 1); - r(cur_row:next_row - 1,:) = r_j; - %R_mat(cur_row:next_row - 1, cur_row:next_row - 1) = R_mat_j; - H(cur_row:next_row - 1, :) = H_x_j; - cur_row = next_row; - end - r = r(1:cur_row - 1, :); - H = H(1:cur_row - 1, :); - %R_mat = R_mat(1:cur_row - 1, 1:cur_row - 1); - - if size(r, 1) < 6 * ase_of_num_aug - error_out = 1; - num_of_tracks = 0; - r = ones(6 * ase_of_num_aug, 1, 'single'); - H = zeros(6 * ase_of_num_aug, size(H, 2), 'single'); - %R_mat = eye(6 * ase_of_num_aug, 6 * ase_of_num_aug, 'single'); - end -end - -function [H_f, H_x, r] = apply_givens(H_f, H_x, r, rows, col, H_f_range, H_x_range) - [g, ~] = planerot(H_f(rows, col)); - H_f(rows, H_f_range) = g * H_f(rows, H_f_range); - H_x(rows, H_x_range) = g * H_x(rows, H_x_range); - %R_mat(rows, :) = g * R_mat(rows, :); - %R_mat(:, rows) = R_mat(:, rows) * g'; - r(rows, :) = g * r(rows, :); -end diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_of_residual_and_h_compress.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_of_residual_and_h_compress.m deleted file mode 100644 index f33d74159e..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/eml_of_residual_and_h_compress.m +++ /dev/null @@ -1,77 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Code is taken from Zack Morrato's prototyped Optical Flow Kalman Filter. -% This code computes the residual and measurment matrices for the optical -% flow measurments. -function [ r_comp, H_comp ] = eml_of_residual_and_h_compress(r, H, ase_of_num_aug) -%#codegen - % H is [ x * 12 0 * 12; x * 24; x * 24; x * 24; 0 * 12 x * 12; ...] -% H = H([2:5:size(H, 1), 3:5:size(H, 1), 4:5:size(H, 1), 1:5:size(H, 1), 5:5:size(H, 1)], :); - - rows = 6 * ase_of_num_aug; - for col=1:rows - for row=size(H, 1):-1:1+col - [H, r] = apply_givens(H, r, [row-1, row], col, col:rows); - end - end -% % make first three fifths upper triangular -% for col=1:rows -% for row=size(H, 1)-2*size(H, 1) / 5:-1:1+col -% [H, r] = apply_givens(H, r, [row-1, row], col, col:rows); -% end -% end -% % make next fifth upper triangular -% for col=1:rows/2 -% for row=size(H, 1)-size(H, 1) / 5:-1:size(H,1)-2*size(H, 1)/5+col+1 -% [H, r] = apply_givens(H, r, [row-1, row], col, col:rows/2); -% end -% end -% % make bottom fifth upper triangular -% for col=rows/2+1:rows -% for row=size(H, 1):-1:size(H,1)-size(H, 1)/5+1+col-rows/2 -% [H, r] = apply_givens(H, r, [row-1, row], col, col:rows); -% end -% end -% % eliminate fourth fifth using top part -% for col=1:rows -% for row=1:min(col, min(rows/2, size(H,1)/5)) -% [H, r] = apply_givens(H, r, [col, 3 * size(H,1) / 5 + row], col, col:rows); -% end -% end -% % eliminate fifth fifth using top part -% for col=rows/2+1:rows -% for row=1:min(col - (rows / 2), size(H,1)/5) -% [H, r] = apply_givens(H, r, [col, 4 * size(H,1) / 5 + row], col, col:rows); -% end -% end - - % Now place the r and H in terms of the full state vector - r_comp = r(1:rows); - H_comp = zeros(rows, 15 + 6 + 6 * ase_of_num_aug); - H_comp(:, 22:end) = H(1:rows, :); - %R_mat_comp = R_mat(1:rows, 1:rows); -end - -function [H, r] = apply_givens(H, r, rows, col, H_range) - [g, ~] = planerot(H(rows, col)); - H(rows, H_range) = g * H(rows, H_range); - r(rows, :) = g * r(rows, :); - %R_mat(rows, :) = g * R_mat(rows, :); - %R_mat(:, rows) = R_mat(:, rows) * g'; -end - diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/quaternion_to_rotation.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/quaternion_to_rotation.m deleted file mode 100644 index 27c3f48d77..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/quaternion_to_rotation.m +++ /dev/null @@ -1,28 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% construct rotation matrix from quaternion -% From Zack and Brian's ekf.m Used as a nested function in the optical -% flow update - -function [R] = quaternion_to_rotation(q) -%#codegen - S = skew(q(1:3)); - a = [q(4) * eye(3, 3) - S; -q(1:3)']; - b = [q(4) * eye(3, 3) + S; -q(1:3)']; - R = b' * a; -end \ No newline at end of file diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/residual_and_h_compress.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/residual_and_h_compress.m deleted file mode 100644 index 5f428696dd..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/residual_and_h_compress.m +++ /dev/null @@ -1,33 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% construct rotation matrix from quaternion -% From Zack and Brian's ekf.m Used as a nested function in the optical -% flow update - -function [r, H, R_mat] = residual_and_h_compress(r, H, r_vec) -%#codegen - [q1, T_H] = qr(H); - - N = min(size(H)); - %N = rank(H); - - q1 = q1(:, 1:N); - H = T_H(1:N, :); - r = q1' * r; - R_mat = q1' * diag(r_vec) * q1; -end diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/skew.m b/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/skew.m deleted file mode 100644 index 2fa5574063..0000000000 --- a/gnc/matlab/fsw_model/est_estimator_wStateAug/embedded_functions/skew.m +++ /dev/null @@ -1,27 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% construct swew matrix from a vector -% From Zack and Brian's ekf.m Used as a nested function in the optical -% flow update - -function [M] = skew(o) -%#codegen - M = [ 0 -o(3) o(2); - o(3) 0 -o(1); - -o(2) o(1) 0]; -end \ No newline at end of file diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/est_estimator_lib_wState_lib.slx b/gnc/matlab/fsw_model/est_estimator_wStateAug/est_estimator_lib_wState_lib.slx deleted file mode 100644 index ddff7db8e1..0000000000 Binary files a/gnc/matlab/fsw_model/est_estimator_wStateAug/est_estimator_lib_wState_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/output_prep/output_prep_lib.slx b/gnc/matlab/fsw_model/est_estimator_wStateAug/output_prep/output_prep_lib.slx deleted file mode 100644 index 9cbab657f7..0000000000 Binary files a/gnc/matlab/fsw_model/est_estimator_wStateAug/output_prep/output_prep_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/predictor/predictor_lib.slx b/gnc/matlab/fsw_model/est_estimator_wStateAug/predictor/predictor_lib.slx deleted file mode 100644 index ecabbef3b2..0000000000 Binary files a/gnc/matlab/fsw_model/est_estimator_wStateAug/predictor/predictor_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/est_estimator_wStateAug/state_manager/state_manager_lib.slx b/gnc/matlab/fsw_model/est_estimator_wStateAug/state_manager/state_manager_lib.slx deleted file mode 100644 index 0335ded8ac..0000000000 Binary files a/gnc/matlab/fsw_model/est_estimator_wStateAug/state_manager/state_manager_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_hrn.slx b/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_hrn.slx deleted file mode 100644 index 5f39b2b7e1..0000000000 Binary files a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_hrn.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_init.m b/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_init.m deleted file mode 100644 index 604a16ff87..0000000000 --- a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_init.m +++ /dev/null @@ -1,31 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Force allocation module initialization file. Configures -% parameters used in the library file fam_force_allocation_module. - -%% Nozzle Parameters -fam_nozzle_x_dir = [1 2 7 8]; %[-] Nozzles cooresponding to the null vector about each axis -fam_nozzle_y_dir = [3 4 9 10]; -fam_nozzle_z_dir = [5 6 11 12]; - -fam_nozzle_pm1 = 1:6; %[-] Nozzles cooresponding to each PM -fam_nozzle_pm2 = 7:12; - -%% Fan Impeller -fam_impeller_eq_density = single(0.001); %[-] Density of points in the lookup table equation -fam_impeller_speeds = single([2000; 2500; 2800])*units_RPM_2_rps; %[rad/s] Basic set of possible impeller speeds diff --git a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_lib.slx b/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_lib.slx deleted file mode 100644 index 4ca7ff8388..0000000000 Binary files a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_lib.slx and /dev/null differ diff --git a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_prep.m b/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_prep.m deleted file mode 100644 index 700323c776..0000000000 --- a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_prep.m +++ /dev/null @@ -1,49 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Force allocation module prep file - -%% Nozzles - -%calculate the inverse matrix for thrust to body torques and forces (i.e. force/torques -> nozzle thrust matrix) -fam_P_nozzle_B_B = [abp_PM1_P_nozzle_B_B; abp_PM2_P_nozzle_B_B]; %[m] Combined PM1&2 nozzle positions -fam_nozzle_orientations = [abp_PM1_nozzle_orientations; abp_PM2_nozzle_orientations]; %[m] Combined PM1&2 nozzle orientations - -fam_nozzle_angle2pwm = (abp_servo_max_PWM-abp_servo_min_PWM)/(abp_nozzle_max_open_angle-abp_nozzle_min_open_angle); %[-] Conversion between servo angle (not nozzle angle) command and resulting servo PWM - -%% Impeller: -fam_impeller_speeds_cnt = length(fam_impeller_speeds); - -%find total thrust (normalized by discharge coefficient) as a function of delta P for each impeller speed -%define and calculate equation from data -fam_impeller_eq_CQ_input = 0:fam_impeller_eq_density:abp_impeller_eq_max_CQ; %[m^3/sec] Range of Flow rate coefficients (CQ) to use in equation -fam_impeller_eq_CP_output = polyval(abp_impeller_eq_CQ2CP_poly,fam_impeller_eq_CQ_input); %[-] CP = f(CQ) - -%Calculate thrust from Cq and Cp: T/N^2 = (Cq*D^3*h*rho*sqrt(2*Cp)) - (2*A0*rho*D^2*Cp) [then (2*A0*rho*D^2*Cp) accounts for the leakage area, Need to do this here since thrust changes with pressure] - %note: that by removing the "zero thrust area" the thrust lookup table will have negative values, this is expected. We won't actually have negative thrust. -fam_PM1_impeller_eq_thrust_output = (fam_impeller_eq_CQ_input*(abp_impeller_diameter^3)*abp_impeller_height*const_air_density.*sqrt(2*fam_impeller_eq_CP_output)) - (2*abp_pm1_zero_thrust_area*const_air_density*(abp_impeller_diameter^2)*fam_impeller_eq_CP_output); -fam_PM2_impeller_eq_thrust_output = (fam_impeller_eq_CQ_input*(abp_impeller_diameter^3)*abp_impeller_height*const_air_density.*sqrt(2*fam_impeller_eq_CP_output)) - (2*abp_pm1_zero_thrust_area*const_air_density*(abp_impeller_diameter^2)*fam_impeller_eq_CP_output); - -%calculate max thrust. -[~, fam_PM1_impeller_eq_thrust_max_idx] = max(fam_PM1_impeller_eq_thrust_output); -[~, fam_PM2_impeller_eq_thrust_max_idx] = max(fam_PM2_impeller_eq_thrust_output); - -%create lookup tables with valid values (from 0-max thrust) We use this section of data as it cooresponds to on pulsing, vs off pulsing -fam_PM1_lookup_thrust_breakpoints = fam_PM1_impeller_eq_thrust_output(1:fam_PM1_impeller_eq_thrust_max_idx); -fam_PM2_lookup_thrust_breakpoints = fam_PM2_impeller_eq_thrust_output(1:fam_PM2_impeller_eq_thrust_max_idx); -fam_PM1_lookup_Cdp_data = fam_impeller_eq_CP_output(1:fam_PM1_impeller_eq_thrust_max_idx); -fam_PM2_lookup_Cdp_data = fam_impeller_eq_CP_output(1:fam_PM2_impeller_eq_thrust_max_idx); diff --git a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_test.m b/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_test.m deleted file mode 100644 index c5da888921..0000000000 --- a/gnc/matlab/fsw_model/fam_force_allocation_module/fam_force_allocation_module_test.m +++ /dev/null @@ -1,76 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Test file to stimulate the FAM -% -% -%% Dependency Prep -% Init model parameters -astrobee_init; - - -fam_nozzle_discharge_coeff = single(0.675*ones(12,1)); %[-] Nozzle Discharge coefficient. Determined via Test -% fam_nozzle_discharge_coeff = single(0.6852*ones(12,1)); %[-] Nozzle Discharge coefficient. Determined via Test - - -astrobee_prep; - -sim_stop_time = 150; - -%% Input Prep -test_impeller_speed_idx.signals.dimensions = 1; -test_impeller_speed_idx.time = [0 70]; -test_impeller_speed_idx.signals.values = single([2; 2;]); - -test_cmd_force.signals.dimensions = 3; -test_cmd_force.time = [0 10 20 30 40 50 60 70 80 90 100 110]; -test_cmd_force.signals.values = single([ 0, 0.0, 0; ... - 0, 0.5, 0; ... - 0, 2.5, 0; ... - 0, 3.0, 0; ... - 0, 6.0, 0; ... - 0, 11.5, 0; ... - 0, 15.5, 0; ... - 0, 18.5, 0; ... - 0, 20.5, 0; ... - 0, 22.5, 0; ... - 0, 22.5, 0; ... - 0, 22.5, 0]*0.0098); - -test_cmd_torque.signals.dimensions = 3; -test_cmd_torque.time = [0 10 20 30 40 50]; -test_cmd_torque.signals.values = single([0, 0, 0; ... - 0, 0, 0; ... - 0, 0, 0; ... - 0, 0, 0; ... - 0, 0, 0; ... - 0, 0, 0]); - - -%% run sim -sim('fam_force_allocation_module_hrn'); - -%% Output Display -close all; - -figure; plot(out_act_msg.act_impeller_speed_cmd); -title('commanded impeller speed'); xlabel('time'); ylabel('speed'); - -figure; plot(out_act_msg.act_servo_pwm_cmd); -title('commanded servo PWM'); xlabel('time'); ylabel('PWM'); legend('1','2','3','4','5','6','7','8','9','10','11','12'); - - diff --git a/gnc/matlab/fsw_model/fsw_lib.slx b/gnc/matlab/fsw_model/fsw_lib.slx deleted file mode 100644 index 72bb53ed3b..0000000000 Binary files a/gnc/matlab/fsw_model/fsw_lib.slx and /dev/null differ diff --git a/gnc/matlab/linear_models/astrobee_linear_att_CL.slx b/gnc/matlab/linear_models/astrobee_linear_att_CL.slx deleted file mode 100644 index 1925b6d6be..0000000000 Binary files a/gnc/matlab/linear_models/astrobee_linear_att_CL.slx and /dev/null differ diff --git a/gnc/matlab/linear_models/astrobee_linear_att_OL.slx b/gnc/matlab/linear_models/astrobee_linear_att_OL.slx deleted file mode 100644 index cac247db83..0000000000 Binary files a/gnc/matlab/linear_models/astrobee_linear_att_OL.slx and /dev/null differ diff --git a/gnc/matlab/linear_models/astrobee_linear_pos_CL.slx b/gnc/matlab/linear_models/astrobee_linear_pos_CL.slx deleted file mode 100644 index f7eb5cb366..0000000000 Binary files a/gnc/matlab/linear_models/astrobee_linear_pos_CL.slx and /dev/null differ diff --git a/gnc/matlab/linear_models/astrobee_linear_pos_OL.slx b/gnc/matlab/linear_models/astrobee_linear_pos_OL.slx deleted file mode 100644 index 5d93005126..0000000000 Binary files a/gnc/matlab/linear_models/astrobee_linear_pos_OL.slx and /dev/null differ diff --git a/gnc/matlab/linear_models/astrobee_linear_pos_ctl_lib.slx b/gnc/matlab/linear_models/astrobee_linear_pos_ctl_lib.slx deleted file mode 100644 index 00b53d6070..0000000000 Binary files a/gnc/matlab/linear_models/astrobee_linear_pos_ctl_lib.slx and /dev/null differ diff --git a/gnc/matlab/linear_models/linear_att_ctl_lib.slx b/gnc/matlab/linear_models/linear_att_ctl_lib.slx deleted file mode 100644 index e68708a816..0000000000 Binary files a/gnc/matlab/linear_models/linear_att_ctl_lib.slx and /dev/null differ diff --git a/gnc/matlab/luaconfig/.gitignore b/gnc/matlab/luaconfig/.gitignore deleted file mode 100644 index 7093921ac9..0000000000 --- a/gnc/matlab/luaconfig/.gitignore +++ /dev/null @@ -1 +0,0 @@ -lua_config.m diff --git a/gnc/matlab/luaconfig/export_config.lua b/gnc/matlab/luaconfig/export_config.lua deleted file mode 100644 index 3ef6651738..0000000000 --- a/gnc/matlab/luaconfig/export_config.lua +++ /dev/null @@ -1,48 +0,0 @@ -function script_path() - local str = debug.getinfo(2, "S").source:sub(2) - return str:match("(.*/)") or "./" -end --- allow us to run script from any directory -package.path = package.path .. ";" .. script_path() .. "?.lua" --- add config directory to imports -package.path = package.path .. ";" .. script_path() .. "../../../management/astrobee/config/?.config" -package.path = package.path .. ";" .. script_path() .. "../../../management/astrobee/config/?.lua" -package.path = package.path .. ";" .. script_path() .. "../../../astrobee/config/?.lua" - --- import the config files we need -require "common_lua" ---require "robot" -require "gnc" - -function toMatlab(v) - if type(v) == "number" then - return string.format("single(%g)", v) - elseif type(v) == "boolean" then - return string.format("uint8(%d)", v and "1" or "0") - elseif type(v) == "table" then - local s = "" - for i, x in ipairs(v) do - t = toMatlab(x) - s = s .. t - if i ~= #v then - s = s .. ", " - if #t > 20 then - s = s .. ";\n" - end - end - end - return "[" .. s .. "]" - else - return "Error: Unexpected type." - end -end - -local filename = script_path() .. "lua_config.m" -local f = assert(io.open(filename, "w")) -for k in pairs(_G) do - if k:find("^tun_") then - f:write(k .. " = " .. toMatlab(_G[k]) .. ";\n") - end -end -f:close() - diff --git a/gnc/matlab/physical_props/abp_astrobee_physical_properties_init.m b/gnc/matlab/physical_props/abp_astrobee_physical_properties_init.m deleted file mode 100644 index 8a8ccfb01a..0000000000 --- a/gnc/matlab/physical_props/abp_astrobee_physical_properties_init.m +++ /dev/null @@ -1,130 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Physical Properties: initalizes the physical parameters of the astrobee FSW and model - -%% units and constants -units_in_2_m = single(0.0254); -units_RPM_2_rps = single(2*pi/60); -units_deg_2_rad = single(pi/180); -units_rad_2_deg = single(180/pi); -const_air_density = single(1.225); %[kg/m^3] Air density inside of the ISS - -%% Vehicle Properties -% gnc.config now holds many properties: edit([ASTROBEE_ROOT '../../management/astrobee/config/gnc.config']) -% mass properties (mass, cg, MOI) moved to gnc.config -% sensor properties (location, orientation) moved to gnc.config - -abp_P_CG_B_B_error = [0 0 0]; %[m] Position Error of the CG estimate - -abp_temp = single(300); %[K] Vehicle temperature - -%% Blower Properties -abp_pm1_impeller_orientation = single([0, 1,0]); %[unit vec] PM1 Axis of rotation of the impeller. -abp_pm2_impeller_orientation = single([0,-1,0]); %[unit vec] PM2 Axis of rotation of the impeller. - -abp_impeller_diameter = single(5.5)*units_in_2_m; %[m] Outer diameter of impeller -abp_impeller_height = single(1.6742)*units_in_2_m; %[m] Trailing Edge height of impeller -abp_impeller_eq_CQ2CP_poly = [58.72 -45.013 11.782 -2.1758 0.1278 0.0825]; %[-] Coefficients from Blair Data 2016-03-13 giving Cpth = f(CQh) -abp_impeller_eq_max_CQ = single(0.33319); %[-] Max CQ from Blair Data. If impeller equation data changes, this value needs to be re-verified - -abp_pm1_zero_thrust_area = single([0.0044667]); %[m^2] PM1: If air is leaking from the plenum, the '0 thrust' position (all nozzles closed) will have an effective open area -abp_pm2_zero_thrust_area = single([0.0042273]); %[m^2] PM2: If air is leaking from the plenum, the '0 thrust' position (all nozzles closed) will have an effective open area -%determined from test data - nozzles closed @[2000 2500]rpm => deltaP = [X Y]PA => Cdp = [X Y] => Total leak area = [X Y] - -abp_impeller_speed2pwm = single(.792095); %[CNT/(rad/sec)] Converts impeller speeds into PWM values - -%% Servo Parameters -abp_servo_max_PWM = single(255); %[PWM] Maximum Servo command to avoid hardstops -abp_servo_min_PWM = single(0); %[PWM] Minimum Servo command to avoid hardstops - -%% Nozzle Properties -abp_nozzle_count = single(6); %[-] Number of Nozzles per PM -abp_nozzle_flap_length = single(0.5353)*units_in_2_m; %[m] Length of the nozzle flappers -abp_nozzle_intake_height = single(0.5154)*units_in_2_m; %[m] Height of the intake to the nozle -abp_nozzle_out_height = single(0.4216)*units_in_2_m; %[m] Height of the output of the nozzle -abp_nozzle_flap_count = single(2); %[-] Number of flaps per nozzle -abp_nozzle_gear_ratio = single(1/2); %[-] Gear Ratio between servo and nozzle. theta_s = R*theta_n -abp_nozzle_max_open_angle = single(79.91)*units_deg_2_rad; %[rad] THEORETICAL max angle that a nozzle can open to -% abp_nozzle_max_open_angle = single(73.90)*units_deg_2_rad; %[rad] Measured max angle that a nozzle can open to on P4E -abp_nozzle_min_open_angle = single(15.68)*units_deg_2_rad; %[rad] Min angle that the nozzle can close to - - - -abp_PM1_nozzle_names = ['Rx+'; ... %[-] PM1: String names for each nozzles. Reference Only. - 'Rx-'; ... - 'Fy+'; ... - 'Ay+'; ... - 'Rz+'; ... - 'Rz-']; -abp_PM2_nozzle_names = ['Lx-'; ... %[-] PM2: String names for each nozzles. Reference Only. - 'Lx+'; ... - 'Ay-'; ... - 'Fy-'; ... - 'Lz+'; ... - 'Lz-']; - -abp_PM1_nozzle_widths = single([ 5.0; ... %[m] PM1: Width of each nozzle - 5.0; ... - 2.8; ... - 2.8; ... - 2.8; ... - 2.8])*units_in_2_m; -abp_PM2_nozzle_widths = abp_PM1_nozzle_widths; %[m] PM2: Width of each nozzle - -abp_PM1_P_nozzle_B_B = single([ 6.00, 4.01, -1.56;... %[m] PM1: Position vector of the nozzle locations in the body frame - -6.00, 4.01, 1.56;... - 2.83, 6.00, 2.83;... - -2.83, 6.00, -2.83;... - -2.66, 4.01, 6.00;... - 2.66, 4.01, -6.00])*units_in_2_m; - -abp_PM2_P_nozzle_B_B = single([-6.00, -4.01, -1.56;... %[m] PM2: Position vector of the nozzle locations in the body frame - 6.00, -4.01, 1.56;... - -2.83, -6.00, 2.83;... - 2.83, -6.00, -2.83;... - 2.66, -4.01, 6.00;... - -2.66, -4.01, -6.00])*units_in_2_m; - -abp_PM1_nozzle_orientations = single([ 1, 0, 0; ... %[unit vec] PM1: Pointing Direction of each nozzle - -1, 0, 0; ... - 0, 1, 0; ... - 0, 1, 0; ... - 0, 0, 1; ... - 0, 0,-1]); - -abp_PM2_nozzle_orientations = single([-1, 0, 0; ... %[unit vec] PM2: Pointing Direction of each nozzle - 1, 0, 0; ... - 0,-1, 0; ... - 0,-1, 0; ... - 0, 0, 1; ... - 0, 0,-1]); - -abp_PM1_discharge_coeff = single([ ... %[-] PM1: Nozzle Discharge coefficient. Determined via Test - 0.914971062 ; ... - 0.755778254 ; ... - 0.940762925 ; ... - 0.792109779 ; ... - 0.92401881 ; ... - 0.930319765 ]); -abp_PM2_discharge_coeff = single([ ... %[-] PM2: Nozzle Discharge coefficient. Determined via Test - 0.947114008 ; ... - 0.764468916 ; ... - 1.000000000 ; ... - 0.90480943 ; ... - 0.936555627 ; ... - 0.893794766 ]); diff --git a/gnc/matlab/physical_props/abp_astrobee_physical_properties_prep.m b/gnc/matlab/physical_props/abp_astrobee_physical_properties_prep.m deleted file mode 100644 index ebdeb42ccc..0000000000 --- a/gnc/matlab/physical_props/abp_astrobee_physical_properties_prep.m +++ /dev/null @@ -1,23 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Free Flyer force allocation module initialization file. Configures -% parameters used in the library file ffp_force_allocation_module. -% -% Dependencies: -% ffp_free_flyer_physical_properties_init.m - diff --git a/gnc/matlab/physical_props/phy_physical_properties.m b/gnc/matlab/physical_props/phy_physical_properties.m deleted file mode 100644 index dc0f84ab5a..0000000000 --- a/gnc/matlab/physical_props/phy_physical_properties.m +++ /dev/null @@ -1,22 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Physical Properties and Constants - -%% Air Properties -phy_rho = 1.2922; % At NIST standard -phy_air_specific_gas_C = 287.058; \ No newline at end of file diff --git a/gnc/matlab/readme.md b/gnc/matlab/readme.md deleted file mode 100644 index a36efc4671..0000000000 --- a/gnc/matlab/readme.md +++ /dev/null @@ -1,77 +0,0 @@ -\page matlab GNC Matlab Code - -In this directory, all the matlab, simulink and embedded C++ functions used -therein are stored. - -This includes the EKF code, control, FAM, and simulator, as well as associated -testing utilities. - -# Dependencies - -LUA and Eigen will need to be installed for the MATLAB environment to properly -initialize. On Linux systems this should be taken care by the standard -installation process. The model is designed to run in MATLAB/Simulink without -additional toolboxes, although Simulink Coder and Simulink Embedded Coder are -required to generate code. - -## For mac installs - -Although the full system FSW cannot be run on a mac, MATLAB development and -simulation can be. LUA should be installed from -https://www.lua.org/download.html If LUA does not get added to the default -system path, the following hack seems to work: Right click on the MATLAB -application and go "Show Package Contents". Then navigate to the bin folder and -open the file matlab in a text editor. Change the first 3 lines to: - ->\#!/bin/sh ->export PATH=$PATH:$HOME/Applications/bin ->\# Copyright 1984-2016 The MathWorks, Inc. - ->Where, $HOME/Applications/bin is the directory pointing to your lua binaries. - - -# The Simulink Code - -## Code Organization - -There are 2 Simulink models that can be used to simulate the Astrobee system. -The models are identical, except astrobee_control_sim has had the Kalman filter -and vision navigation models removed due to complications with simulating vision -navigation as well as the increased complexity of the Kalman filter. -Additionally, the full up astrobee model has issues running on windows due to -its use of hand coded optimized functions. For these reasons, it is recomended -that astrobee_control_sim be used whenever possible. -- astrobee.slx file contains the full up control and pose determination software - as well as an IMU, vision navigation, propulsion, and physics model. -- astrobee_control_sim.slx contains the control software as well as an IMU, - propulsion, and physics model. - -## Open and Run in Matlab - -Once the dependencies have been installed as described above simply opening -either the astrobee.slx or astrobee_control_sim.slx file should call all the -necessary initialization files. For example scenario run files look at the file -'run_ctl_scn01' - -## Compilation - -The code can be generated from within matlab by running the command -'build_Proto4' or from a Linux terminal by running the shell script -'astrobee_gen_code.sh' - -## Embedded C++ Functions - -The Kalman filter utilizes several hand coded functions that are highly -optimized using the Eigen library. These have to be built into mex files in -order to simulate with simulink, this should happen automatically, but requires -the eigen library to be installed and the Matlab mex tools (see matlab -documentation on mex) to be configured to use an installed compiler. - -## LUA Configuration in Matlab - -Tunable parameters are managed through LUA scripts, so as to be consistent with -the rest of the Astrobee software. The GN&C tunable parameters are located in -the freeflyer/astrobee/config/gnc.config file. The matlab file tunable_init -runs the LUA script and imports the variables into the MATLAB base workspace. -Note, tunable parameters utilized by the GN&C system must have the tun_ prefix -in order to be properly autocoded as tunable variables. diff --git a/gnc/matlab/scenarios/run_ctl_scn01.m b/gnc/matlab/scenarios/run_ctl_scn01.m deleted file mode 100644 index 8acfbdc993..0000000000 --- a/gnc/matlab/scenarios/run_ctl_scn01.m +++ /dev/null @@ -1,61 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Run the baseline astrobee scenario -% Perching scenario -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee')) - open('astrobee'); -end - -% Configure the scenario -astrobee_init; -proto4_init; -setup_scn01; - -mlp_speed_gain_cmd_list = uint8([3 3 3]'); -% abp_nozzle_max_open_angle = single(79.91)*units_deg_2_rad; -% abp_servo_max_PWM = single(100); - -astrobee_prep; - - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee_control_sim') -toc -display('Simulation Complete'); -%% Post Process Data -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - -trans_times = [60 75]; -slew_times = [100 125]; -scn1_error_vec = [max(calcData.error.att.total_mag.timeRange(trans_times(1), trans_times(2)))*180/pi, calcData.error.pos.total.mag.timeRange(trans_times(1), trans_times(2)).maxerror, max(calcData.error.att.total_mag.timeRange(slew_times(1), slew_times(2)))*180/pi, calcData.error.pos.total.mag.timeRange(slew_times(1), slew_times(2)).maxerror]; -fprintf(1, 'Max att error (translation), deg: %f\n',max(calcData.error.att.total_mag.timeRange(trans_times(1), trans_times(2)))*180/pi); -fprintf(1, 'Max pos error (translation), m: %f\n',calcData.error.pos.total.mag.timeRange(trans_times(1), trans_times(2)).maxerror); -fprintf(1, 'Max att error (translation), deg: %f\n',max(calcData.error.att.total_mag.timeRange(slew_times(1), slew_times(2)))*180/pi); -fprintf(1, 'Max pos error (translation), m: %f\n',calcData.error.pos.total.mag.timeRange(slew_times(1), slew_times(2)).maxerror); diff --git a/gnc/matlab/scenarios/run_ctl_scn02.m b/gnc/matlab/scenarios/run_ctl_scn02.m deleted file mode 100644 index ddf66f5cf6..0000000000 --- a/gnc/matlab/scenarios/run_ctl_scn02.m +++ /dev/null @@ -1,60 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%% Astrobee stopping scenario -% -% -%% Configure Simulation -% Stop time -% Set clock -open_sys = find_system; -if isempty(strfind(open_sys, 'astrobee_control_sim')) - open('astrobee_control_sim'); -end - -% Configure the scenario -astrobee_init; -proto4_init; -setup_scn02; - -mlp_speed_gain_cmd_list = uint8([3 3 3]'); -abp_nozzle_max_open_angle = single(79.91)*units_deg_2_rad; -abp_servo_max_PWM = single(100); - -astrobee_prep; - - -%% Simulate -display('Running Simulation'); -tic -sim('astrobee_control_sim') -toc -display('Simulation Complete'); -%% Post Process Data -display('Converting Simulation Data Stores to telemData'); -astrobee_load_sim_data; -simData = 1; -display('Performing post processing calculations'); -astrobee_post_process; - -% trans_times = [60 75]; -% slew_times = [100 125]; -% scn1_error_vec = [max(calcData.error.att.total_mag.timeRange(trans_times(1), trans_times(2)))*180/pi, calcData.error.pos.total.mag.timeRange(trans_times(1), trans_times(2)).maxerror, max(calcData.error.att.total_mag.timeRange(slew_times(1), slew_times(2)))*180/pi, calcData.error.pos.total.mag.timeRange(slew_times(1), slew_times(2)).maxerror]; -% fprintf(1, 'Max att error (translation), deg: %f\n',max(calcData.error.att.total_mag.timeRange(trans_times(1), trans_times(2)))*180/pi); -% fprintf(1, 'Max pos error (translation), m: %f\n',calcData.error.pos.total.mag.timeRange(trans_times(1), trans_times(2)).maxerror); -% fprintf(1, 'Max att error (translation), deg: %f\n',max(calcData.error.att.total_mag.timeRange(slew_times(1), slew_times(2)))*180/pi); -% fprintf(1, 'Max pos error (translation), m: %f\n',calcData.error.pos.total.mag.timeRange(slew_times(1), slew_times(2)).maxerror); diff --git a/gnc/matlab/scenarios/setup_scn01.m b/gnc/matlab/scenarios/setup_scn01.m deleted file mode 100644 index ffd72bac81..0000000000 --- a/gnc/matlab/scenarios/setup_scn01.m +++ /dev/null @@ -1,63 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%[mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT filesep 'scenarios' filesep 'scn05.fplan']); -%[pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] - -tun_default_mass = single(14.4); -lin_accel = .0325; -[setpoints1, cmd_times1] = calc_trapazoidal_waypoints_with_att_end_point([0 0 -.7], [1 0 -.7], lin_accel, .2, [0 0 0 1],[0 0 0 1], 0, 0, 60); -[setpoints2, cmd_times2] = calc_trapazoidal_waypoints_with_att_end_point([1 0 -.7], [1 0 -.7], lin_accel, .2, [0 0 0 1],[0 0 1 0], 10*pi/180, 10*pi/180, 100); -[setpoints3, cmd_times3] = calc_trapazoidal_waypoints_with_att_end_point([0 0 -.7], [1.0 0 -.7], lin_accel, .2, [0 0 1 0],[0 0 1 0], 10*pi/180, 10*pi/180, 130); - -ini_sim_initial_conditions_init - - -% mlp_command_times = uint32([0 0; cmd_times1; cmd_times2; cmd_times3]); -% mlp_command_list = single([ini_P_B_ISS_ISS 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... -% setpoints1; ... -% setpoints2; setpoints3]); - -init_pos = [0.0 0 -.7]; - -mlp_command_times = uint32([0 0; cmd_times1; cmd_times2]); -mlp_command_list = single([init_pos 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoints1; ... - setpoints2]); - - - -%ini_veh_quat = single([0, 0, .7071, .7071]); - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 100 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -mlp_mode_cmd_list = uint8([2;2;2]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 60 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - - -astrobee_stop_time = 150; diff --git a/gnc/matlab/scenarios/setup_scn02.m b/gnc/matlab/scenarios/setup_scn02.m deleted file mode 100644 index c4f584beff..0000000000 --- a/gnc/matlab/scenarios/setup_scn02.m +++ /dev/null @@ -1,61 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -%[mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT filesep 'scenarios' filesep 'scn05.fplan']); -%[pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -lin_accel = .05; -[setpoints1, cmd_times1] = calc_trapazoidal_waypoints_with_att_end_point([0 0 -.7], [1 0 -.7], lin_accel, .2, [0 0 0 1],[0 0 0 1], 0, 0, 60); -[setpoints2, cmd_times2] = calc_trapazoidal_waypoints_with_att_end_point([1 0 -.7], [1 0 -.7], lin_accel, .2, [0 0 0 1],[0 0 1 0], 10*pi/180, 10*pi/180, 100); -[setpoints3, cmd_times3] = calc_trapazoidal_waypoints_with_att_end_point([0 0 -.7], [1.0 0 -.7], lin_accel, .2, [0 0 1 0],[0 0 1 0], 10*pi/180, 10*pi/180, 130); - -ini_sim_initial_conditions_init - - -% mlp_command_times = uint32([0 0; cmd_times1; cmd_times2; cmd_times3]); -% mlp_command_list = single([ini_P_B_ISS_ISS 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... -% setpoints1; ... -% setpoints2; setpoints3]); - -init_pos = [0.0 0 -.7]; - -mlp_command_times = uint32([0 0; cmd_times1; cmd_times2]); -mlp_command_list = single([init_pos 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;... - setpoints1; ... - setpoints2]); - - - -%ini_veh_quat = single([0, 0, .7071, .7071]); - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 100 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. - -mlp_mode_cmd_list = uint8([2;1;1]); -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 65 , 0; ... - 120 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - - - - -astrobee_stop_time = 150; diff --git a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_hrn.slx b/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_hrn.slx deleted file mode 100644 index b183936187..0000000000 Binary files a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_hrn.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_init.m b/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_init.m deleted file mode 100644 index 266406c977..0000000000 --- a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_init.m +++ /dev/null @@ -1,62 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%env_environment_model_init.m -% enviroment model initialization file - -%% Control -env_grav_disturb_on = boolean(0); %[flag] Turns on/off the gravity disturbance model (to be used in granite table setup only!) -env_rot_effects_on = boolean(1); %[flag] Turns on/off the rotation effects in the dynamic model - -% Moved to gnc.config -%tun_env_drag_disturb_on = boolean(1); %[flag] Turns on/off the drag disturbance model -%note: to keep drag on, but remove the external airflow, set max value below to 0 - -%% Error -env_ini_position_error = single(0.05); %[m] Error in the initial position -env_ini_orientation_error_angle = single(3*pi/180); %[rad] Error in the intial orienetaion - -%% Linear Drag Parameters -%Constants -const_air_density = single(1.2); %[kg/m^3] density of air in ISS with temperature ~20C - -%Drag coefficients -env_cube_drag_coeff = single(1.05); -env_ang_cube_drag_coeff = single(0.80); -env_cube_ref_area = single(0.092903); %[m^2] -env_ang_cube_ref_area = env_cube_ref_area*sqrt(2); %[m^2] - -%Drag force = (1/2)*(drag coeff)*(air density)*(ref area)*(velocity ^2) -%the variables below represent (1/2)*(drag coeff)*(air density)*(ref area), to get drag, simply multiple by velocity^2 -env_cube_force_drag_coeff = (1/2)*env_cube_drag_coeff*env_cube_ref_area*const_air_density; -env_ang_cube_drag_coeff = (1/2)*env_ang_cube_drag_coeff*env_ang_cube_ref_area*const_air_density; -%since the difference between a cube face, and angular cube is so small -%....simply take the average and use that as the coeff. -env_avg_drag_coeff = mean([env_cube_force_drag_coeff, env_ang_cube_drag_coeff]); - -% External Air Velocity Errors -env_ext_air_vel_variance = [0.01 0.01 0.01]; %[m/s^2] (note: variance must be type double for random number block) -env_ext_air_vel_seed = [1 2 3]; -env_max_ext_air_vel = single(0.2); %[m/s^2] Maximum air velocity in ISS: 0.076-0.20 covers ~70% of cases (extreme cases are near vents and along walls) - -%% rotational drag and air error -env_rotational_drag_coeff = 0.001; %[-] (similar to drag coeff) Fractional amount of rotational velocity used in external disturbance torque -env_ext_air_omega_variance = [0.01 0.01 0.01]; %[rad/s] rotational noise -env_max_ext_air_omega = single(0.1); %[rad/s] maximum disturabance rotation - -%% Gravity -const_gravity_local = single([0, 0,-9.80665]); %[m/s^2] Gravity defined in the local room frame (Z axis points up) diff --git a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_lib.slx b/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_lib.slx deleted file mode 100644 index e701be76e9..0000000000 Binary files a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_lib.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_prep.m b/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_prep.m deleted file mode 100644 index 38cd68fbf9..0000000000 --- a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_prep.m +++ /dev/null @@ -1,23 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%env_environment_model_prep.m - -%% Noise -env_ini_orientation_error_1s = sqrt((env_ini_orientation_error_angle^2)/3)/3; -env_ini_orientation_error_Q = eulers_to_quat(env_ini_orientation_error_1s,env_ini_orientation_error_1s,env_ini_orientation_error_1s); - diff --git a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_test.m b/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_test.m deleted file mode 100644 index 30d7b91a4d..0000000000 --- a/gnc/matlab/sim_model/env_enviroment_model/env_environment_model_test.m +++ /dev/null @@ -1,82 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%env_environment_model_test.m - -%% Setup -sim_end_time = 3600*2; - -ini_time_nanoseconds = 0; -ini_time_seconds = 0; - -init_omega_B_ISS_B = single([0, 0, 0]); % Initial body rates relative to ISS -init_Q_ISS2B = single([0, 0, 0, 1]); % Initial body quaternion, ISS to Body CF - -init_P_B_ISS_ISS = single([10, 0, 0]); %[m] Initial body velocity -init_V_B_ISS_ISS = single([0, 0, -0.0113]); %[m/s] Initial body posistion - - -env_granite_lab_flag = boolean(0); %[flag] Indicates if configuration should be for granite lab testing -env_grav_disturb_on = boolean(0); %[flag] Turns on/off the gravity disturbance model (to be used in granite table setup only!) -env_drag_disturb_on = boolean(0); %[flag] Turns on/off the drag disturbance model -env_rot_effects_on = boolean(1); %[flag] Turns on/off the rotation effects in the dynamic model -env_max_ext_air_vel = single(0.2); %[m/s^2] Maximum air velocity in ISS: 0.076-0.20 covers ~70% of cases (extreme cases are near vents and along walls) -env_max_ext_air_omega = single(0.1); %[rad/s] maximum disturbance rotation - -env_environment_model_prep; - -in_prop_torque.signals.dimensions = 3; -in_prop_torque.signals.values = [0, 0, 0; -% 0, 0, 0.0094; %[N*m] = (desired alpha)*ffp_veh_inertia_matrix - 0, 0, 0; - 0, 0, 0]; -in_prop_torque.time = [0; 5; 8]; - -in_prop_force.signals.dimensions = 3; -in_prop_force.signals.values = [0, 0, 0; -% 1.2, 0, 0; %[N] = (desired accel)* ffp_veh_mass - 0, 0, 0; - 0, 0, 0]; -in_prop_force.time = [0; 30; 40]; - -%% run -sim('env_environment_model_hrn'); - -%% plot -close all; - -% figure; -% subplot(211); plot(out_time_msg.timestamp_sec); -% title('nanoseconds'); -% subplot(212); plot(out_time_msg.timestamp_nsec); -% title('seconds'); - -figure; -subplot(311);plot(out_env_msg.P_B_ISS_ISS); -title('Position'); legend('x','y','z'); -subplot(312);plot(out_env_msg.V_B_ISS_ISS); -title('Velocity'); legend('x','y','z'); -subplot(313);plot(out_env_msg.A_B_ISS_ISS); -title('Accel'); legend('x','y','z'); - -figure; -subplot(311);plot(out_env_msg.Q_ISS2B); -title('Quat'); legend('x','y','z'); -subplot(312);plot(out_env_msg.omega_B_ISS_B); -title('Omega'); legend('x','y','z'); -subplot(313);plot(out_env_msg.alpha_B_ISS_B); -title('Alpha'); legend('x','y','z'); \ No newline at end of file diff --git a/gnc/matlab/sim_model/ini_sim_initial_conditions_init.m b/gnc/matlab/sim_model/ini_sim_initial_conditions_init.m deleted file mode 100644 index ba11f84516..0000000000 --- a/gnc/matlab/sim_model/ini_sim_initial_conditions_init.m +++ /dev/null @@ -1,33 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Free Flyer Sim Initial Conditions Init. Configures the intial conditions -% for a simulation - -%% Time -% Time is maintained as unix time, which is maintained as the number of seconds and nanoseconds since 00:00:00 UTC on 1 January 1970. -% Matlab epoch datenum(1970, 1,1, 0,0,0) -ini_time_seconds = uint32(1.4213e+09); -ini_time_nanoseconds = uint32(0); - -%% Moved to lua conig file [ASTROBEE_ROOT '/../../management/astrobee/config/gnc.config'] - -% init_omega_B_ISS_B = single([0, 0, 0]); % Initial body rates relative to ISS -% init_Q_ISS2B = single([0, 0, 0, 1]); % Initial body quaternion, ISS to Body CF -% -% ini_P_B_ISS_ISS = single([0, 0, 0]); %[m] Initial body velocity -% ini_V_B_ISS_ISS = single([0, 0, 0]); %[m/s] Initial body posistion diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/cmc_command_and_mode_control.slx b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/cmc_command_and_mode_control.slx deleted file mode 100644 index 7382908737..0000000000 Binary files a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/cmc_command_and_mode_control.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/gen_cmd_list.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/gen_cmd_list.m deleted file mode 100644 index 78505e15e5..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/gen_cmd_list.m +++ /dev/null @@ -1,98 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [cmd_list, cmd_time] = gen_cmd_list(path) - - -cmd_start_time = 30; %time of the first cmd -min_stop = 10; %minimum stop time at a station? - -fid = fopen(path,'r'); -text = textscan(fid,'%s','Delimiter',''); -text = text{1}; -fclose(fid); - -station_idx = find(~cellfun('isempty',strfind(text,'"type" : "Station"'))); - -% If you can't find any stations try looking for the new name of Stations -% found in the newer versions of GDS -if isempty(station_idx) - station_idx = find(~cellfun('isempty',strfind(text,'"type" : "ModuleBayStation"'))); -end - -segment_idx = find(~cellfun('isempty',strfind(text,'"type" : "Segment"'))); -all_idx = sort([station_idx; segment_idx]); - -curr_cmd_idx = 0; -for i = 1:length(all_idx) - %if the indexed line contaions station, handle as station - if(~cellfun('isempty', strfind(text(all_idx(i)), 'Station'))) -% y_val = sscanf(cell2mat(text(all_idx(i)+3)), '"y" : %g*'); -% x_val = sscanf(cell2mat(text(all_idx(i)+4)), '"x" : %g*'); -% q_val = sscanf(cell2mat(text(all_idx(i)+5)), '"pitch" : %g*'); -% p_val = sscanf(cell2mat(text(all_idx(i)+6)), '"roll" : %g*'); -% r_val = sscanf(cell2mat(text(all_idx(i)+7)), '"yaw" : %g*'); -% z_val = sscanf(cell2mat(text(all_idx(i)+8)), '"z" : %g*'); -% quat_val = eulers_to_quat(r_val,q_val,p_val); -% -% curr_cmd_idx = curr_cmd_idx + 1; -% cmd_list(curr_cmd_idx,:) = [x_val, y_val, z_val, zeros(1,3), zeros(1,3), quat_val, zeros(1,3), zeros(1,3)]; -% - - %the idea here is that stations are useless except as placeholders for the last command in a segment. - %So just capture the 'duration' time command. Then add that to the begining of the next segment - if (curr_cmd_idx>0) - duration = sscanf(cell2mat(text(all_idx(i)+18)), '"duration" : %g*'); - if(~isempty(duration)) - next_time = time_list(curr_cmd_idx) + duration; - else - next_time = time_list(curr_cmd_idx) + min_stop; - end - else - next_time = cmd_start_time; - end - else %it was a segment - is_waypts = 0; %index of where the waypoint line is relative to "Segment" - while(cellfun('isempty', strfind(text(all_idx(i)+is_waypts), '"waypoints"'))) - is_waypts = is_waypts+1; - end - - waypts = strsplit(cell2mat(text(all_idx(i)+is_waypts)), '['); - waypts = waypts(3:end); - for j = 1:length(waypts); - temp = str2double(strsplit(cell2mat(waypts(j)), {',', '[', ']', ' ','\f','\n','\r','\t','\v'})); - - curr_cmd_idx = curr_cmd_idx + 1; - cmd_list(curr_cmd_idx,:) = temp(3:end-1); - if(j==1) - %if the first waypt of a segment, add on the time from the previous station, - %and then capture the relative time for the rest of the waypoints - time_list(curr_cmd_idx) = next_time + temp(2); - first_time = time_list(curr_cmd_idx); - else - %otherwise, just handle the relative timing - time_list(curr_cmd_idx) = first_time + temp(2); - end - end - end -end - -cmd_time = uint32([floor(time_list)', floor((time_list - floor(time_list))*1e9)']); - -cmd_list = single(cmd_list); - - diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/scenario1.fplan b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/scenario1.fplan deleted file mode 100644 index ba4b8bed89..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/scenario1.fplan +++ /dev/null @@ -1,2161 +0,0 @@ -{ - "type" : "FreeFlyerPlan", - "valid" : true, - "site" : { - "type" : "Site", - "name" : "Site", - "id" : "Site" - }, - "visible" : true, - "defaultTolerance" : 1.0, - "defaultSpeed" : 0.05, - "creator" : "DW", - "schemaUrl" : "http://www.example.com/freeFlyerPlanSchema.json", - "planNumber" : 0, - "dateCreated" : "2015-04-29T17:42:38Z", - "dateModified" : "2015-05-20T17:38:56Z", - "libraryURLs" : [ ], - "sequence" : [ { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -12.54, - "x" : 13.5, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "0", - "id" : "Station", - "sequence" : [ { - "type" : "Undock", - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "0.0 Undock", - "id" : "Undock" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 13.5, -12.539999961853027, 0.0, -0.0, 0.0, 0.0, -0.04551832319738831, 0.020690148701691363, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 13.47724083772303, -12.529654887193875, 0.0, -0.04551832387566418, 0.020690149009998604, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 48.3321797014002, 11.322759353011833, -11.55034503651218, 0.0, -0.04551832387566418, 0.020690149009998604, 0.0, 0.04551832319738831, -0.020690148701691363, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 49.33217971630136, 11.300000190734863, -11.539999961853027, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "0-1", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -11.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "1", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "1.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -11.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -11.51499996110797, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 39.99999940395356, 11.300000190734863, -9.564999962598085, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 40.99999941885472, 11.300000190734863, -9.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "1-2", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -9.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "2", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "2.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -9.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -9.51499996110797, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 11.300000190734863, -8.564999962598085, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 11.300000190734863, -8.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "2-3", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -8.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "3", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "3.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -8.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -8.51499996110797, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 11.300000190734863, -7.564999962598085, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 11.300000190734863, -7.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "3-4", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -7.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "4", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "4.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -7.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -7.51499996110797, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 11.300000190734863, -6.564999962598085, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 11.300000190734863, -6.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "4-5", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -6.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "5", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "5.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -6.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -6.51499996110797, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 11.300000190734863, -5.564999962598085, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 11.300000190734863, -5.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "5-6", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -5.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "6", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "6.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -5.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -5.51499996110797, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 39.99999940395356, 11.300000190734863, -3.564999962598085, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 40.99999941885472, 11.300000190734863, -3.5399999618530273, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "6-7", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -3.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "7", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "7.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -3.5399999618530273, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -3.514999961107969, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 11.300000190734863, -2.5649999625980855, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 11.300000190734863, -2.5399999618530273, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "7-8", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -2.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "8", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "8.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -2.5399999618530273, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -2.5649999625980855, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 159.99999761581424, 11.300000190734863, -10.514999961107968, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 160.9999976307154, 11.300000190734863, -10.539999961853027, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "8-9", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -10.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "9", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "9.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -10.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -10.51499996110797, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 119.99999821186069, 11.300000190734863, -4.564999962598086, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 120.99999822676185, 11.300000190734863, -4.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "9-10", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -4.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "10", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "10.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -4.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -4.51499996110797, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 90.79999788403514, 11.300000190734863, -0.025000000745058593, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 91.7999978989363, 11.300000190734863, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "10-11", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "11", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.275000189989806, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 92.0000062584876, 6.724999810010195, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 93.00000627338876, 6.699999809265137, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "11-12", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 6.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "12", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "12.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 6.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 6.724999810010194, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 7.674999808520079, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 7.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "12-13", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 7.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "13", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "13.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 7.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 7.724999810010194, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 8.674999808520079, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 8.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "13-14", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 8.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "14", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "14.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 8.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 8.724999810010194, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 9.674999808520079, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 9.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "14-15", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 9.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "15", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "15.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 9.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 9.724999810010194, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 39.99999940395356, 11.674999808520079, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 40.99999941885472, 11.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "15-16", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 11.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "16", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "16.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.724999810010194, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 12.674999808520079, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 12.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "16-17", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 12.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "17", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "17.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 12.699999809265137, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 12.674999808520079, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 99.9999985098839, 7.724999810010195, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 100.99999852478506, 7.699999809265137, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "17-18", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 7.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "18", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 7.699999809265137, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 7.699999809265137, -0.025000000745058065, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 35.9999985098839, 7.699999809265137, -1.7749999515712263, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 36.99999852478506, 7.699999809265137, -1.7999999523162842, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "18-19", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -1.8, - "x" : 7.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "19", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "19.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 7.699999809265137, -1.7999999523162842, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 7.699999809265137, -1.774999951571226, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 71.9999970197678, 7.699999809265137, 1.774999951571226, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 72.99999703466897, 7.699999809265137, 1.7999999523162842, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "19-20", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 1.8, - "x" : 7.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "20", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "20.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 7.699999809265137, 1.7999999523162842, 0.0, 0.0, -0.0, -0.0, 0.0, -0.03535533905932738, -0.03535533905932738, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 7.699999809265137, 1.7823222822597848, -0.0176776700564993, 0.0, -0.035355339586162986, -0.035355339586162986, 0.0, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 50.911686138089024, 7.699999809265137, 0.01767767005649934, -1.7823222822597848, 0.0, -0.035355339586162986, -0.035355339586162986, -0.0, 0.03535533905932738, 0.03535533905932738, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 51.911686152990185, 7.699999809265137, 0.0, -1.7999999523162842, 0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "20-21", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 7.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : -1.8, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "21", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "21.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 7.699999809265137, 0.0, -1.7999999523162842, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 7.699999809265137, 0.0, -1.774999951571226, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 71.9999970197678, 7.699999809265137, 0.0, 1.774999951571226, 0.0, 0.0, 0.05000000074505806, -0.0, -0.0, -0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 72.99999703466897, 7.699999809265137, 0.0, 1.7999999523162842, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "21-22", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 7.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 1.8, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "22", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "22.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 7.699999809265137, 0.0, 1.7999999523162842, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 7.699999809265137, 0.0, 1.774999951571226, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 35.9999985098839, 7.699999809265137, 0.0, 0.025000000745057926, 0.0, 0.0, -0.05000000074505806, -0.0, -0.0, 0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 36.99999852478506, 7.699999809265137, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "22-23", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 7.7, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "23", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 7.699999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 7.724999810010194, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 72.00000655651083, 11.275000189989806, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 73.00000657141199, 11.300000190734863, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "23-24", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "24", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, 0.025000000745058065, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 49.99999925494195, 11.300000190734863, 2.474999999254942, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 50.99999926984311, 11.300000190734863, 2.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "24-25", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 2.5, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "25", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "25.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 2.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, 2.525000000745058, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 29.99999955296517, 11.300000190734863, 3.9749999992549423, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 30.999999567866332, 11.300000190734863, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "25-26", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 4.0, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "26", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "26.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, 4.025000000745058, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 29.99999955296517, 11.300000190734863, 5.474999999254942, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 30.999999567866332, 11.300000190734863, 5.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "26-27", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 5.5, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "27", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "27.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 5.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, 5.525000000745058, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 29.99999955296517, 11.300000190734863, 6.974999999254942, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 30.999999567866332, 11.300000190734863, 7.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "27-28", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 7.0, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "28", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "28.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 7.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, 7.025000000745058, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 29.99999955296517, 11.300000190734863, 8.474999999254942, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 30.999999567866332, 11.300000190734863, 8.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "28-29", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 8.5, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "29", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "29.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 8.5, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, 8.474999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 169.99999746680265, 11.300000190734863, 0.025000000745057704, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 170.99999748170382, 11.300000190734863, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "29-30", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "30", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.275000189989806, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 106.00000223517414, 6.025000000745059, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 107.0000022500753, 6.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "30-31", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 6.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "31", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "31.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 6.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 5.974999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 5.025000000745058, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 5.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "31-32", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "32", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "32.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 5.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 4.974999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 4.025000000745058, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 4.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "32-33", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 4.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "33", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "33.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 4.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 3.974999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 3.025000000745058, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 3.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "33-34", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 3.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "34", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "34.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 3.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 2.974999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 2.025000000745058, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 2.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "34-35", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 2.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "35", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "35.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 2.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 1.9749999992549419, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 1.0250000007450581, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 1.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "35-36", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 1.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "36", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "36.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 1.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 0.974999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, 0.025000000745058037, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "36-37", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 0.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "37", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "37.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -0.025000000745058065, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, -0.974999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, -1.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "37-38", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -1.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "38", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "38.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -1.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -1.0250000007450581, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, -1.9749999992549419, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, -2.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "38-39", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -2.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "39", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "39.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -2.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -2.0, -0.025000000745058065, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 39.99999940395356, -2.0, -1.974999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 40.99999941885472, -2.0, -2.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "39-40", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -2.0, - "x" : -2.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "40", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -2.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -2.0, -1.9749999992549419, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 39.99999940395356, -2.0, -0.025000000745057926, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 40.99999941885472, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "40-41", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -2.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "41", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -2.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -2.025000000745058, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 24.00000059604644, -3.175000046938658, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 25.000000610947602, -3.200000047683716, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "41-42", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -3.2, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "42", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "42.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -3.200000047683716, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -3.225000048428774, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999994933605272, -4.174999808520079, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999994948506433, -4.199999809265137, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "42-43", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -4.2, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "43", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "43.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -4.199999809265137, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -4.224999810010194, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, -5.174999808520079, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, -5.199999809265137, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "43-44", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -5.2, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "44", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "44.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.199999809265137, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.224999810010194, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, -6.174999808520079, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, 0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, -6.199999809265137, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "44-45", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -6.2, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "45", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "45.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -6.199999809265137, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -6.174999808520079, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 23.99999582767493, -5.025000000745058, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 24.99999584257609, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "45-46", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "46", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -0.025000000745058065, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 16.0, -5.0, -0.7750000111758709, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 17.00000001490116, -5.0, -0.800000011920929, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "46-47", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -0.8, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "47", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "47.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, -0.800000011920929, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -0.825000012665987, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 16.0, -5.0, -1.5750000230967998, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 17.00000001490116, -5.0, -1.600000023841858, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "47-48", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -1.6, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "48", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "48.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, -1.600000023841858, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -1.625000024586916, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 16.00000119209288, -5.0, -2.3750000946223735, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 17.000001206994042, -5.0, -2.4000000953674316, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "48-49", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -2.4, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "49", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "49.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, -2.4000000953674316, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -2.42500009611249, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 15.999998807907124, -5.0, -3.1750000469386577, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 16.999998822808287, -5.0, -3.200000047683716, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "49-50", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -3.2, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "50", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "50.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, -3.200000047683716, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -3.225000048428774, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 15.999998807907124, -5.0, -3.974999999254942, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 16.999998822808287, -5.0, -4.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "50-51", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -4.0, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "51", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "51.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, -4.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -4.025000000745058, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 16.000003576278637, -5.0, -4.775000189989806, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 17.000003591179798, -5.0, -4.800000190734863, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "51-52", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -4.8, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "52", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "52.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, -4.800000190734863, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -4.825000191479921, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 19.999999701976783, -5.0, -5.775000189989806, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 20.999999716877944, -5.0, -5.800000190734863, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "52-53", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -5.8, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "53", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "53.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, -5.800000190734863, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -5.825000191479921, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 11.999997913837465, -5.0, -6.375000094622374, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 12.999997928738626, -5.0, -6.400000095367432, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "53-54", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -6.4, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "54", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "54.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, -6.400000095367432, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, -6.375000094622374, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 128.0, -5.0, -0.025000000745058593, 0.0, 0.0, 0.05000000074505806, 0.0, -0.0, -0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 129.00000001490116, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "54-55", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "55", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -5.0, 0.0, 0.025000000745058065, 0.0, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 39.99999940395356, -5.0, 0.0, 1.974999999254942, 0.0, 0.0, 0.05000000074505806, -0.0, -0.0, -0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 40.99999941885472, -5.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "55-56", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : -5.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 2.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "56", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -5.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, -4.974999999254942, 0.0, 2.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 325.9999989569188, 11.275000189989807, 0.0, 2.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 326.99999897181993, 11.300000190734863, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "56-57", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 2.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "57", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 0.0, 2.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, 0.0, 1.9749999992549419, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 39.99999940395356, 11.300000190734863, 0.0, 0.025000000745057926, 0.0, 0.0, -0.05000000074505806, -0.0, -0.0, 0.05, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 40.99999941885472, 11.300000190734863, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "57-58", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "58", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.300000190734863, -0.025000000745058065, 0.0, 0.0, -0.05000000074505806, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 250.7999954998494, 11.300000190734863, -12.51499996110797, 0.0, 0.0, -0.05000000074505806, 0.0, -0.0, 0.05, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 251.79999551475055, 11.300000190734863, -12.539999961853027, 0.0, 0.0, -0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "58-59", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -12.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "59", - "id" : "Station", - "sequence" : [ ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 11.300000190734863, -12.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0000000149011612, 11.325000191479921, -12.539999961853027, 0.0, 0.05000000074505806, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 43.999995529651706, 13.474999999254942, -12.539999961853027, 0.0, 0.05000000074505806, 0.0, 0.0, -0.05, -0.0, -0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 44.99999554455287, 13.5, -12.539999961853027, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] ], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "59-60", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -12.54, - "x" : 13.5, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "60", - "id" : "Station", - "sequence" : [ { - "type" : "Dock", - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "60.0 Dock", - "id" : "Dock" - } ] - } ], - "name" : "SoundSurvey", - "id" : "SoundSurvey" -} \ No newline at end of file diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/scenario2.fplan b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/scenario2.fplan deleted file mode 100644 index c752373b92..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/scenario2.fplan +++ /dev/null @@ -1,120 +0,0 @@ -{ - "type" : "FreeFlyerPlan", - "valid" : true, - "site" : { - "type" : "Site", - "name" : "Site", - "id" : "Site" - }, - "visible" : true, - "defaultTolerance" : 1.0, - "defaultSpeed" : 0.05, - "creator" : "DW", - "schemaUrl" : "http://www.example.com/freeFlyerPlanSchema.json", - "planNumber" : 0, - "dateCreated" : "2015-04-29T17:42:38Z", - "dateModified" : "2015-05-20T17:38:56Z", - "libraryURLs" : [ ], - "sequence" : [ { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 0.0, - "x" : 0.0, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "0", - "id" : "Station", - "sequence" : [ { - "type" : "Undock", - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "0.0 Undock", - "id" : "Undock" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.045518, 0.020690, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 2.000000015, -0.022760, 0.010346, 0.0, -0.045518, 0.020690, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 49.332179715, -2.177241, 0.989655, 0.0, -0.045518, 0.020690, 0.0, 0.045518, -0.020690, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 50.332179715, -2.200000, 1.000000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "0-1", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : 1.0, - "x" : -2.2, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "1", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "1.0 StationKeep", - "id" : "StationKeep" - } ] - }, { - "type" : "Segment", - "speed" : 0.05, - "tolerance" : 0.0, - "waypoints" : [ [ 0.0, -2.200000, 1.000000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.050000, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 1.000000015, -2.20000, 1.025001, 0.0, 0.0, 0.050000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 39.999999404, -2.200000, 2.975001, 0.0, 0.0, 0.050000, 0.0, 0.0, -0.05000, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 40.999999419, -2.20000, 3.000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ], [ 50.0, -2.200000, 3.000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]], - "waypointType" : "PoseVelAccel", - "stopAtEnd" : true, - "faceForward" : true, - "name" : "1-2", - "id" : "Segment", - "sequence" : [ ] - }, { - "type" : "Station", - "coordinate" : { - "type" : "Point6Dof", - "y" : -9.54, - "x" : 11.3, - "pitch" : 0.0, - "roll" : 0.0, - "yaw" : 0.0, - "z" : 0.0, - "name" : "Point6Dof", - "id" : "Point6Dof" - }, - "stopOnArrival" : false, - "tolerance" : 0.1, - "name" : "2", - "id" : "Station", - "sequence" : [ { - "type" : "StationKeep", - "duration" : 10, - "color" : "#555555", - "blocking" : true, - "scopeTerminate" : true, - "name" : "2.0 StationKeep", - "id" : "StationKeep" - } ] - } ], - "name" : "SoundSurvey", - "id" : "SoundSurvey" -} \ No newline at end of file diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_init.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_init.m deleted file mode 100644 index 91dea0a187..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_init.m +++ /dev/null @@ -1,105 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%csv_computer_vision_system_init.m - -%% Camera Parameters -% camera positions set in gnc.config -% camera orientations set in gnc.config -% image width/height set in gnc.config -% image focal lengths set in gnc.config -% image offsets set in gnc.config - -cvs_navcam_pointing = single([0, 0, 1]); %[unit vector] Vector pointing in the direction of the camera FOV, relative to the camera frame -cvs_navcam_P_B_B_error = single([0 0 0]); %[m] Error on the position of the camera in the body frame -cvs_navcam_Q_B2navcan_error = single(eulers_to_quat(0, 0, 0)); %[m] Error on the Quaternion from camera to body frame -cvs_navcam_min_dist = single(.2); %[m] min distance a point can be seen by the camera -cvs_navcam_max_dist = single(100); %[m] max distance a point can be seen by the camera - -cvs_dockcam_pointing = single([0, 0, 1]); %[unit vector] Vector pointing in the direction of the camera FOV, relative to the camera frame -cvs_dockcam_P_B_B_error = single([0 0 0]); %[m] Error on the position of the camera in the body frame -cvs_dockcam_Q_B2dockcam_error = single(eulers_to_quat(0, 0, 0)); %[m] Error on the Quaternion from camera to body frame -cvs_dockcam_min_dist = single(.2); %[m] min distance a point can be seen by the camera -cvs_dockcam_max_dist = single(100); %[m] max distance a point can be seen by the camera - -cvs_perchcam_pointing = single([0, 0, 1]); %[unit vector] Vector pointing in the direction of the camera FOV, relative to the camera frame -cvs_perchcam_P_B_B_error = single([0 0 0]); %[m] Error on the position of the camera in the body frame -cvs_perchcam_Q_B2perchcam_error = single(eulers_to_quat(0, 0, 0)); %[m] Error on the Quaternion from camera to body frame -cvs_perchcam_min_dist = single(.2); %[m] min distance a point can be seen by the camera -cvs_perchcam_max_dist = single(100); %[m] max distance a point can be seen by the camera - -%% Image processing parameters -cvs_landmark_process_hz = 3; %[Hz] Time interval at which landmark images are processed -cvs_landmark_num_out = single(50); %[-] Number of landmark observations to send -cvs_landmark_map_filename = [ASTROBEE_ROOT '/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/granite_table_map_points.txt']; -cvs_landmark_map_iss = import_map_points(cvs_landmark_map_filename); %[m] Imports the landmark points relative to the ISS frame -% cvs_landmark_map_iss = gen_points_grid([0 0 0],2,10); %[m] Generates the fixed world points relative to the ISS coordinate frame -cvs_landmark_max_pixel_error= 2; %[pixels] maximum noise error in pixels - Note: must be type double -cvs_landmark_max_error = 0.05; %[m] maximum position error in map - Note: must be type double -cvs_landmark_valid_mask = [true, true, true]; %[-] true = use model's output, false = force to zero, testing only -cvs_landmark_valid_times = uint32([0,0; 30,0; 31,0]); %[s,ms] Timing to use cooresponding valid_masks, testing only - -cvs_AR_process_hz = 6; %[Hz] Time interval at which AR images are processed -cvs_AR_num_out = cvs_landmark_num_out; %[-] Number of AR tag observations to report -% cvs_AR_map_iss = gen_points_AR_tags([0,-.5,-.4], [0,1,0]); %[m] Wall perpendicular to Y-axis, origin [0,-.5,-.4] -cvs_AR_map_iss = gen_points_AR_tags2; %[m] Generate fixed AR tags for granite table -cvs_AR_max_pixel_error = 2; %[pixels] maximum noise error in pixels - Note: must be type double -cvs_AR_max_error = 0.01; %[m] maximum position error in map - Note: must be type double -cvs_AR_valid_mask = [true, true, true]; %[-] true = use model's output, false = force to zero, testing only -cvs_AR_valid_times = uint32([0,0; 30,0; 31,0]); %[s,ms] Timing to use cooresponding valid_masks, testing only - -cvs_optflow_process_hz = 3.6765; %[Hz] Time interval at which OF images are processed -cvs_optflow_num_out = single(50); %NOTE: must change in format_optflow_output.m as well %[-] Number of OF observations to keep (number of points) -cvs_optflow_num_hist = single(16); %[-] Number of histories to keep for the optical flow -cvs_optflow_map_iss = gen_points_walls(4.5,.35); %[m] Generates the fixed world points relative to the ISS coordinate frame -cvs_optflow_max_pixel_error = .5; % 0.2 %[pixels] maximum noise error in pixels - Note: must be type double -cvs_optflow_max_error = 0.01; %optflow has 0 map error %[m] maximum position error in map - Note: must be type double -cvs_optflow_valid_mask = [true, true, true]; %[-] true = use model's output, false = force to zero, testing only -cvs_optflow_valid_times = uint32([0,0; 30,0; 31,0]); %[s,ms] Timing to use cooresponding valid_masks, testing only - -cvs_handrail_process_hz = 5; %[Hz] Time interval at which Handrail images are processed -cvs_handrail_num_out = cvs_landmark_num_out; %[-] Number of handrail observations to report -cvs_handrail_vector_nom = single([0,1,0]); %[-] Vector of the pointing direction of the handrail -cvs_handrail_misalign = single([0;0;0]); %[-] handrail misalignment -cvs_handrail_map_iss = gen_points_rail_wall([-.4,0,-.4],cvs_handrail_vector_nom,[-1,0,0],.05,1,.1); %[m] Generates the fixed world points relative to the ISS coordinate frame -cvs_handrail_3d_knowledge = uint8(1); % flag that allows the filter to include knowledge along the handrail vector -cvs_handrail_noise_var = 0.01^2; %[-] Variance of error for the 3D cam output from the handrail model -cvs_handrail_max_pixel_error= 2; %[pixels] maximum noise error in pixels - Note: must be type double -cvs_handrail_max_error = 0.05; %[m] maximum position error in map - Note: must be type double -cvs_handrail_valid_mask = [true, true, true]; %[-] true = use model's output, false = force to zero, testing only -cvs_handrail_valid_times = uint32([0,0; 30,0; 31,0]); %[s,ms] Timing to use cooresponding valid_masks, testing only - -%% General Noise Parameters -%tun_cvs_noise_on set in gnc.config -cvs_noise_seed = 123; %[unitless] seed for noise generator - -%% create the null messages for landmarks/handrails since only one can be sent at a time -cvs_landmark_null_message.cvs_timestamp_sec = zeros(1,'uint32'); -cvs_landmark_null_message.cvs_timestamp_nsec = zeros(1,'uint32'); -cvs_landmark_null_message.cvs_landmarks = zeros(50,3,'single'); -cvs_landmark_null_message.cvs_observations = zeros(50,2,'single'); -cvs_landmark_null_message.cvs_valid_flag = zeros(50,1,'uint8'); - -cvs_handrail_null_message.cvs_timestamp_sec = zeros(1,'uint32'); -cvs_handrail_null_message.cvs_timestamp_nsec = zeros(1,'uint32'); -cvs_handrail_null_message.cvs_landmarks = zeros(50,3,'single'); -cvs_handrail_null_message.cvs_observations = zeros(50,3,'single'); -cvs_handrail_null_message.cvs_valid_flag = zeros(50,1,'uint8'); -cvs_handrail_null_message.cvs_3d_knowledge_flag = zeros(1,1,'uint8'); -cvs_handrail_null_message.cvs_handrail_local_pos = zeros(3,1,'single'); -cvs_handrail_null_message.cvs_handrail_local_quat = single([0 0 0 1]'); -cvs_handrail_null_message.cvs_handrail_update_global_pose_flag = zeros(1,1,'uint8'); diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_lib.slx b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_lib.slx deleted file mode 100644 index 975a6140c1..0000000000 Binary files a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_lib.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_prep.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_prep.m deleted file mode 100644 index f4db104b06..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/cvs_computer_vision_system_prep.m +++ /dev/null @@ -1,58 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%csv_computer_vision_system_prep.m -%% Noise Warning -if ~tun_cvs_noise_on - warning('Vision System Noise OFF') -end - -%% Sample Times -cvs_landmark_process_time = round((1/cvs_landmark_process_hz)/astrobee_time_step_size)*astrobee_time_step_size; -cvs_AR_process_time = round((1/cvs_AR_process_hz)/astrobee_time_step_size)*astrobee_time_step_size; -cvs_optflow_process_time = round((1/cvs_optflow_process_hz)/astrobee_time_step_size)*astrobee_time_step_size; -cvs_handrail_process_time = round((1/cvs_handrail_process_hz)/astrobee_time_step_size)*astrobee_time_step_size; - -cvs_optflow_ids = single(1:size(cvs_optflow_map_iss,1))'; %[-] Assign ID numbers to every optical flow point -cvs_handrail_vector = quat_rotation_vec(cvs_handrail_vector_nom,eulers_to_quat(cvs_handrail_misalign(3),cvs_handrail_misalign(2),cvs_handrail_misalign(1))); %[-] misalign the handrail -cvs_handrail_vector = cvs_handrail_vector/norm(cvs_handrail_vector); - -%map error: -temp_stored_rng = rng; %save off and change rng -rng(cvs_noise_seed+cvs_noise_seed); -cvs_landmark_1s_noise = sqrt((cvs_landmark_max_error^2)/3)/3; %[m] 1sigma value for noise on position of landmarks -cvs_landmark_map_error = single(randn(size(cvs_landmark_map_iss))*cvs_landmark_1s_noise); %[m] error to be added to the map -cvs_AR_1s_noise = sqrt((cvs_AR_max_error^2)/3)/3; %[m] 1sigma value for noise on position of AR tags -cvs_AR_map_error = single(randn(size(cvs_AR_map_iss))*cvs_AR_1s_noise); %[m] error to be added to the map -cvs_optflow_1s_noise = sqrt((cvs_optflow_max_error^2)/3)/3; %[m] 1sigma value for noise on position of world points -cvs_optflow_map_error = single(randn(size(cvs_optflow_map_iss))*cvs_optflow_1s_noise); %[m] error to be added to the map -cvs_handrail_1s_noise = sqrt((cvs_handrail_max_error^2)/3)/3; %[m] 1sigma value for noise on position of landmarks -cvs_handrail_map_error = single(randn(size(cvs_handrail_map_iss))*cvs_handrail_1s_noise); %[m] error to be added to the map -rng(temp_stored_rng); %restore rng values - -%Noise values: assume worst case (max) position error is if all [xyz] are equally wrong. Then divide by 3 to find 1 sigma value -cvs_landmark_pixel_noise = sqrt((cvs_landmark_max_pixel_error^2)/3)/3; %[pixels] 1sigma value for pixel noise on landmarks -cvs_AR_pixel_noise = sqrt((cvs_AR_max_pixel_error^2)/3)/3; %[pixels] 1sigma value for pixel noise on AR tags -cvs_optflow_pixel_noise = sqrt((cvs_optflow_max_pixel_error^2)/3)/3; %[pixels] 1sigma value for pixel noise on optical flow -cvs_handrail_pixel_noise = sqrt((cvs_landmark_max_pixel_error^2)/3)/3; %[pixels] 1sigma value for pixel noise on handrails - -%adjust the given valid mask times to the simulation start time -cvs_landmark_valid_times = [cvs_landmark_valid_times(:,1)+ini_time_seconds, cvs_landmark_valid_times(:,2)+ini_time_nanoseconds]; -cvs_AR_valid_times = [cvs_AR_valid_times(:,1)+ini_time_seconds, cvs_AR_valid_times(:,2)+ini_time_nanoseconds]; -cvs_optflow_valid_times = [cvs_optflow_valid_times(:,1)+ini_time_seconds, cvs_optflow_valid_times(:,2)+ini_time_nanoseconds]; -cvs_handrail_valid_times = [cvs_handrail_valid_times(:,1)+ini_time_seconds, cvs_handrail_valid_times(:,2)+ini_time_nanoseconds]; - diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_AR_tag_output.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_AR_tag_output.m deleted file mode 100644 index 5b68db9771..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_AR_tag_output.m +++ /dev/null @@ -1,63 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%format_AR_tag_output.m - -% inputs: -% num_pts_out = Required number of points to be reported -% points_in iss = ALL [x,y,z] position of points in the ISS frame -% [NOTE: for AR tags, these are grouped in 4's. first 4 = 1 tag, second 4 = another tag] -% points_in_cam = ALL [u,v] pixel location of points in the camera -% valid_in = indicates which points are valid -% -% outputs: -% points_out_iss = output [x,y,z] position of points in the ISS frame -% points_out_cam = output [u,v] pixel location of points in the camera -% valid_out = indicates which values of the output are valid! - -function [points_out_iss, points_out_cam, valid_out] = format_AR_tag_output(num_pts_out, points_in_iss, points_in_cam, valid_in) -%#codegen - -%initialize the outputs -points_out_iss = zeros(num_pts_out,3,'single'); -points_out_cam = zeros(num_pts_out,2,'single'); -valid_out = zeros(num_pts_out,1,'single'); - -for i=1:4:length(points_in_iss) - valid_in(i:i+3) = all(valid_in(i:i+3)); %all 4 tags must be valid, or they are all invalid -end - -%find how many pts are valid, and determine how that compares to required number -valid_pts = (valid_in==1); -needed = num_pts_out-length(valid_in(valid_pts)); - -if(needed==0) %if exactly the right ammount, just report them! - points_out_iss(1:num_pts_out,:) = points_in_iss(valid_pts,:); - points_out_cam(1:num_pts_out,:) = points_in_cam(valid_pts,:); - valid_out(1:num_pts_out,:) = valid_in(valid_pts); -elseif(needed>0) %if we need more points, back fill with zeros, and zero the valid cooresponding valid flag - points_out_iss(1:num_pts_out,:) = [points_in_iss(valid_pts,:); zeros(needed,3)]; - points_out_cam(1:num_pts_out,:) = [points_in_cam(valid_pts,:); zeros(needed,2)]; - valid_out(1:num_pts_out,:) = [valid_in(valid_pts); zeros(needed,1)]; -else %if we have too many points, take a random sample - temp = points_in_iss(valid_pts,:); - random_order = randperm(length(temp)); - points_out_iss(1:num_pts_out,:) = temp(random_order(1:num_pts_out),:); - temp = points_in_cam(valid_pts,:); - points_out_cam(1:num_pts_out,:) = temp(random_order(1:num_pts_out),:); - valid_out(1:num_pts_out,:) = ones(num_pts_out,1); -end diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_handrail_output.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_handrail_output.m deleted file mode 100644 index 48ba23475e..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_handrail_output.m +++ /dev/null @@ -1,55 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%format_handrail_output.m - -% inputs: -% num_pts_out = Required number of points to be reported -% points_in_iss = ALL [x,y,z] position of points in the ISS frame -% points_in_cam = ALL [u,v] pixel location of points in the camera -% valid_in = indicates which points are valid -% -% outputs: -% points_out_iss = output [x,y,z] position of points in the ISS frame -% points_out_cam = output [x,y,z] pixel location of points in the camera frame -% valid_out = indicates which values of the output are valid! - -function [points_out_iss, points_out_cam, valid_out] = format_handrail_output(num_pts_out, points_in_iss, points_in_cam, valid_in) -%#codegen - -%initialize the outputs -points_out_iss = zeros(num_pts_out,3,'single'); -points_out_cam = zeros(num_pts_out,3,'single'); -valid_out = zeros(num_pts_out,1,'single'); - -%find how many pts are valid, and determine how that compares to required number -valid_pts = (valid_in==1); -num_valid = length(valid_in(valid_pts)); -needed = num_pts_out-num_valid; - -if(needed>=0) %if exactly the right ammount or need more, just report what we got! (rest will be zeros) - points_out_iss(1:num_valid,:) = points_in_iss(valid_pts,:); - points_out_cam(1:num_valid,:) = points_in_cam(valid_pts,:); - valid_out(1:num_valid,:) = valid_in(valid_pts); -else %if we have too many points, take a random sample - temp = points_in_iss(valid_pts,:); - random_order = randperm(length(temp)); - points_out_iss(1:num_pts_out,:) = temp(random_order(1:num_pts_out),:); - temp = points_in_cam(valid_pts,:); - points_out_cam(1:num_pts_out,:) = temp(random_order(1:num_pts_out),:); - valid_out(1:num_pts_out,:) = ones(num_pts_out,1); -end diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_landmark_output.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_landmark_output.m deleted file mode 100644 index 641549b34b..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_landmark_output.m +++ /dev/null @@ -1,65 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%format_landmark_output.m - -% inputs: -% num_pts_out = Required number of points to be reported -% points_in iss = ALL [x,y,z] position of points in the ISS frame -% points_in_cam = ALL [u,v] pixel location of points in the camera -% valid_in = indicates which points are valid -% -% outputs: -% points_out_iss = output [x,y,z] position of points in the ISS frame -% points_out_cam = output [u,v] pixel location of points in the camera -% valid_out = indicates which values of the output are valid! - -function [points_out_iss, points_out_cam, valid_out] = format_landmark_output(num_pts_out, points_in_iss, points_in_cam, valid_in) -%#codegen - -%initialize the outputs -points_out_iss = zeros(num_pts_out,3,'single'); -points_out_cam = zeros(num_pts_out,2,'single'); -valid_out = zeros(num_pts_out,1,'single'); - -%Find unique 2D points in the cam can be thought of as either: -% a) points blocked by other points are not visible to the camera, despite technically being in the FOV of the camera -% b) Only can have 1 point reported per pixel location -[points_in_cam, unique_rows, ~] = unique(points_in_cam,'rows'); -points_in_iss = points_in_iss(unique_rows,:); -valid_in = valid_in(unique_rows'); - -%find how many pts are valid, and determine how that compares to required number -valid_pts = (valid_in==1); -needed = num_pts_out-length(valid_in(valid_pts)); - -if(needed==0) %if exactly the right ammount, just report them! - points_out_iss(1:num_pts_out,:) = points_in_iss(valid_pts,:); - points_out_cam(1:num_pts_out,:) = points_in_cam(valid_pts,:); - valid_out(1:num_pts_out,:) = valid_in(valid_pts); -elseif(needed>0) %if we need more points, back fill with zeros, and zero the valid cooresponding valid flag - points_out_iss(1:num_pts_out,:) = [points_in_iss(valid_pts,:); zeros(needed,3)]; - points_out_cam(1:num_pts_out,:) = [points_in_cam(valid_pts,:); zeros(needed,2)]; - valid_out(1:num_pts_out,:) = [valid_in(valid_pts); zeros(needed,1)]; -else %if we have too many points, take a random sample - temp = points_in_iss(valid_pts,:); - random_order = randperm(length(temp)); - points_out_iss(1:num_pts_out,:) = temp(random_order(1:num_pts_out),:); - temp = points_in_cam(valid_pts,:); - points_out_cam(1:num_pts_out,:) = temp(random_order(1:num_pts_out),:); - valid_out(1:num_pts_out,:) = ones(num_pts_out,1); -end diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_optflow_output.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_optflow_output.m deleted file mode 100644 index 85b6d3eeb8..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/format_optflow_output.m +++ /dev/null @@ -1,153 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%format_visOD_output.m - -% inputs: -% num_pts_out = Required number of points to be reported -% points_in iss = ALL [x,y,z] position of points in the ISS frame -% points_in_cam = ALL [u,v] pixel location of points in the camera -% valid_in = indicates which points are valid -% num_augments = Required number of histories kept for Optical Flow (ASSUMED to be >=2) -% flow_ids = ID tag for each optical flow point (OF only) -% -% outputs: -% observati_out = output [u,v] pixel location of points in the camera -% valid_out = indicates which values of the output are valid! -% ids_out = optical flow IDs - -function [observations_out, valid_out, ids_out, registration_pulse] = format_optflow_output(num_points_out, points_in_cam, valid_in, num_augmentations, flow_ids) -%#codegen - -num_points_out = 50; %if number of points out changes, change this number. -augmentation_order = uint8([2 10 13 15]); %order of the replacement augmentations - %note: real FSW has a fancy way to determine the last augmentation to replace that changes between 13, 14 or 15. Here we simplify to just take 15 every time -report_augmentations = 0; -num_augmentations = double(num_augmentations); - -%Define Persistance Variables -persistent int_id_hist; -persistent int_observations; -persistent int_valid_flag; -persistent int_registration_number; -persistent int_initalization_complete; - -%Either Initalize the variables (1st time only), or shift the history over -if isempty(int_id_hist) - int_id_hist = zeros(num_points_out, 1, 'single'); - int_observations = zeros(num_points_out, 2, num_augmentations, 'single'); - int_valid_flag = zeros(num_points_out, num_augmentations, 'uint8'); - int_registration_number = uint8(1); - int_initalization_complete = 0; -else - %until 16 augmentations have been captured, just keep incrementing number. Then after that, start rotating through the registration numbers - if(int_initalization_complete 0) - %initialize new slots with zeros (since we may not fill it, want the remaining to be zeros already) - new_ids = zeros(num_points_needed, 1, 'single'); - new_observations = zeros(num_points_needed, 2, num_augmentations, 'single'); - new_valid_flags = zeros(num_points_needed, num_augmentations, 'uint8'); - - %find the unused valid points (points not already in augmentations) - [~,~,use_these_points] = setxor(int_id_hist, valid_ids); - num_points_avail = length(use_these_points); - - if(num_points_avail > 0) - %shuffle the points avaliable - random_order = randperm(num_points_avail); - shuffled_ids = use_these_points(random_order); - shuffled_valid_points = valid_points_in_cam(shuffled_ids,:); - - %determine if num_points_avail>num_points needed, then verify its 10) - plane_vec1 = cross(norm_vec,[1 0 0]); -else - plane_vec1 = cross(norm_vec,[0 1 0]); -end -plane_vec2 = cross(norm_vec, plane_vec1); - -%location relative to center, in plane defined by vector -%each Tag is represented by (:,:,x) in the matrix. Whole tag must be visible to report. -AR_P_tag_origin_2D(:,:,1) = [-.1 -.1; -.2 -.2; -.1 -.2; -.2 -.1]; -AR_P_tag_origin_2D(:,:,2) = [.1 .1; .2 .2; .1 .2; .2 .1]; -AR_P_tag_origin_2D(:,:,3) = [-.1 .1; -.2 .2; -.1 .2; -.2 .1]; -AR_P_tag_origin_2D(:,:,4) = [.1 -.1; .2 -.2; .1 -.2; .2 -.1]; - -points = []; -AR_P_tag_origin_3D = zeros(4,3,size(AR_P_tag_origin_2D,1)); %[4 corners per tag, 3D position, number of tags] -for i = 1:size(AR_P_tag_origin_2D,3) - for j = 1:4 - AR_P_tag_origin_3D(j,:,i) = AR_P_tag_origin_2D(j,1,i)*plane_vec1 + AR_P_tag_origin_2D(j,2,i)*plane_vec2 + origin; - end - points = [points; AR_P_tag_origin_3D(:,:,i)]; -end - -points = single(points); diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_AR_tags2.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_AR_tags2.m deleted file mode 100644 index e4562ada19..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_AR_tags2.m +++ /dev/null @@ -1,67 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% gen_points_AR_tags2.m -% Computes the position of AR targets on the dock in granite lab frame from -% the specified AR tag positions - -function points_out = gen_points_AR_tags2() - -unit_mm2m = 0.001; - -%% Frames: -% AR frame = AR tag frame - Centered in middle of AR tags -% +X = right (when facing the tags) -% +Y = up -% +Z = out from the tags -% Dock frame = flat surface of the docks - Centered in middle of two docks -% +X = away from dock -% +Y = left (when facing dock) -% +Z = down -% granite lab = room coordinate frame - Centered in middle of granite table -% +X = toward wall with door -% +Y = toward wall with windows -% +Z = down - -DCM_AR2dock = [ 0, 0, 1; ... %[DCM] Converts points in AR frame to dock frame - -1, 0, 0; ... - 0,-1, 0]; -P_AR_dock_dock = [0 0 0]; %[m] Position of the AR frame relative to the dock frame expressed in the dock frame - -DCM_dock2granite = [ cosd(-25), 0,-sind(-25); ... %[DCM] Converts points in dock frame into the granite frame - 0, 1, 0; ... - sind(-25), 0, cosd(-25)]; -P_dock_granite_granite = [ -0.7053 0.3105 -0.8378]; %[m] Position of dock frame relative to the granite frame expressed in the granite frame - -%% AR tag locations -P_tags_AR_AR = [ 71, 48, 0; 91, 48, 0; 71, 28, 0; 91, 28, 0; ... %[m] Position of AR tags relative to AR frame expressed in the AR frame - -91, -28, 0; -71, -28, 0; -91, -48, 0; -71, -48, 0; ... % [top left; top right; bottom left; bottom right] - 151, -15, 0; 171, -15, 0; 151, -35, 0; 171, -35, 0; ... - -171, 35, 0;-151, 35, 0;-171, 15, 0;-151, 15, 0; ... - 91, -15, 0; 131, -15, 0; 91, -55, 0; 131, -55, 0; ... - -131, 55, 0; -91, 55, 0;-131, 15, 0; -91, 15, 0; ... - 131, 48, 0; 171, 48, 0; 131, 8, 0; 131, 8, 0; ... - -171, -8, 0;-131, -8, 0;-171, -48, 0;-131, -48, 0; ... - -48, 48, 0; 48, 48, 0; -48, -48, 0; 48, -48, 0]*unit_mm2m; - -%convert to dock frame -P_tags_dock_dock = DCM_AR2dock*P_tags_AR_AR' + repmat(P_AR_dock_dock', 1, length(P_tags_AR_AR)); % intermediate: P_tags_AR_dock = DCM_AR2dock*P_tags_AR_AR - -%convert to granite frame -P_tags_granite_granite = DCM_dock2granite*P_tags_dock_dock + repmat(P_dock_granite_granite', 1, length(P_tags_AR_AR)); % intermediate: P_tags_dock_granite = DCM_dock2granite*P_tags_dock_dock - -points_out = single(P_tags_granite_granite'); diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_grid.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_grid.m deleted file mode 100644 index 30654a363d..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_grid.m +++ /dev/null @@ -1,35 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function points = gen_points_grid(origin, dist, max_dist) -%returns a grid of points (including [0 0 0]) -%spans from -max to + max with a distance between each point - -i0 = origin(1); -j0 = origin(2); -k0 = origin(3); - -points = []; -for (i=-max_dist+i0:dist:max_dist+i0) - for(j=-max_dist+j0:dist:max_dist+j0) - for(k=-max_dist+k0:dist:max_dist+k0) - points = [points;[i,j,k]]; - end - end -end - -points = single(points); \ No newline at end of file diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_rail_wall.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_rail_wall.m deleted file mode 100644 index 1d9b03b626..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_rail_wall.m +++ /dev/null @@ -1,40 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function points = gen_points_rail_wall(origin, rail_vector, wall_vector, point_delta, rail_length, rail_diam) -%returns a set of points looking like a rail in front of a wall -% origin = center of rail -% rail vector = direciton rail is pointed -% wall vector = direciton from rail to wall -% delta = distance from one point to next -% length = total length of rail -% diam = diameter of the rail - -rail_vector = (rail_vector/norm(rail_vector))'; -point_spread = -rail_length/2:point_delta:rail_length/2; - -left_dir = cross(rail_vector, wall_vector); -right_dir = -left_dir; - -points_center = repmat(origin,length(point_spread),1)' + rail_vector*point_spread; -points_left = repmat(origin+(left_dir*rail_diam),length(point_spread),1)' + rail_vector*point_spread; -points_right = repmat(origin+(right_dir*rail_diam),length(point_spread),1)' + rail_vector*point_spread; - -points_wall = gen_points_wall(origin+(rail_diam*2*wall_vector), wall_vector, point_delta,(rail_length/point_delta)^2); - -points = [points_left'; points_center'; points_right'; points_wall]; -end diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_wall.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_wall.m deleted file mode 100644 index 9159d61cd9..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_wall.m +++ /dev/null @@ -1,45 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function points = gen_points_wall(origin, norm_vec, point_delta, num_points) -%returns a wall of points -% origin = wall center -% norm_vec = vector normal to the wall -% point delta = density of points -% num points = total number of points to have - -cnt = sqrt(num_points)/2; - -%find two vectors in the plane -if(acosd(abs(dot(norm_vec, [1 0 0]))) > 10) - plane_vec1 = cross(norm_vec,[1 0 0]); -else - plane_vec1 = cross(norm_vec,[0 1 0]); -end -plane_vec2 = cross(norm_vec, plane_vec1); - -%use those vectors to populate the points -points = []; -for(j=-point_delta*cnt:point_delta:point_delta*cnt) - for(k=-point_delta*cnt:point_delta:point_delta*cnt) - new_points = [plane_vec1*j + plane_vec2*k + origin]; - points = [points; new_points]; - end -end - -points = unique(points,'rows'); -points = single(points); \ No newline at end of file diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_walls.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_walls.m deleted file mode 100644 index 6bc0e00284..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/gen_points_walls.m +++ /dev/null @@ -1,44 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function points = gen_points_walls(wall_dist, point_delta) -%returns a set of walls surrounding the origin - %will create a box with length 'dist' - -walls = [ 1 0 0; ... - -1 0 0; ... - 0 1 0; ... - 0 -1 0; ... - 0 0 1; ... - 0 0 -1]*wall_dist; - -points = []; - -for(j=-wall_dist:point_delta:wall_dist) - for(k=-wall_dist:point_delta:wall_dist) - new_points = [0,j,k; ... - 0,j,k; ... - j,0,k; ... - j,0,k; ... - j,k,0; ... - j,k,0]; - points = [points; new_points+walls]; - end -end - -points = unique(points,'rows'); -points = single(points); \ No newline at end of file diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/granite_table_map_points.txt b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/granite_table_map_points.txt deleted file mode 100644 index 802cbb69d0..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/granite_table_map_points.txt +++ /dev/null @@ -1,8647 +0,0 @@ -2.03328, 2.67501, -0.813273 -1.99986, 2.49387, -0.577087 -1.95879, 1.86089, -0.133519 -2.19102, 0.848391, 0.594161 -2.0607, 0.801434, 0.593889 -2.16974, 1.39769, -0.391713 -1.9217, 1.78124, -0.142506 -1.95389, 2.31707, -0.634379 -2.15324, 1.36497, -0.372729 -1.91189, 2.06726, -0.367297 -1.91909, 1.96773, -0.369359 -2.16267, 1.2826, -0.329433 -1.94661, 1.81412, -0.196926 -1.94782, 1.81386, -0.156864 -2.19605, 0.845031, 0.595845 -1.97606, 1.84027, -0.152074 -2.28059, 0.837767, 0.784138 -1.96556, 1.80314, -1.56078 -1.04048, -0.252317, -0.435741 -1.03998, -0.207375, -0.432009 -1.04051, -0.23968, -0.421876 -1.03912, -0.20245, -0.42231 -1.97968, 1.78361, -0.117693 -1.03891, -0.202521, -0.386794 -1.03885, -0.256842, -0.375323 -1.04174, -0.150725, -0.38112 -1.03771, -0.160596, -0.525194 -1.03862, -0.237454, -0.503004 -1.04153, -0.204143, -0.458821 -1.04163, -0.161999, -0.373731 -1.03981, -0.214825, -0.434423 -2.00871, 2.47253, -0.803903 -1.99682, 2.62463, -0.575878 -2.14776, 1.15043, -1.15991 -2.0602, 2.55502, -0.828789 -2.01868, 2.49815, -0.60302 -2.29589, 0.619399, -1.32803 -2.27203, 0.7944, -1.01811 -2.0769, 0.807009, 0.604378 -2.2109, 0.769465, 0.588286 -1.04298, -0.18014, -0.398783 -1.04156, -0.167723, -0.380371 -2.29, 1.42501, -0.347146 -2.0226, 2.48875, -0.823108 -2.03793, 2.78125, -0.82173 -1.85776, 2.22973, -0.4053 -1.63752, 2.04486, -0.38503 -1.99214, 2.50171, -0.829961 -2.03555, 2.76215, -0.827612 -2.1472, 1.37711, -0.533017 -1.76625, 2.22677, -0.41378 -1.99231, 1.88928, -0.183596 -2.02144, 2.5179, -0.599941 -1.97929, 1.8973, -0.162676 -2.16567, 1.28017, -1.10892 -1.99138, 2.06848, -0.518942 -2.03631, 1.82452, -0.252496 -2.14821, 1.30917, -0.238068 -2.17994, 0.586163, -1.3899 -2.13416, 1.16775, -0.646125 -2.09999, 1.2604, -0.471069 -1.96761, 1.95861, -0.466529 -1.93364, 1.99728, -0.394088 -1.89622, 2.08817, -0.361516 -2.26265, 0.886085, -1.27136 -2.12192, 1.29239, -0.684103 -2.13802, 2.73402, -0.841172 -2.00095, 2.47967, -0.608866 -1.87654, 2.4498, -0.481872 -2.15126, 1.38746, -0.394288 -1.70026, 2.45353, 0.000309513 -1.69842, 2.40905, 0.0128667 -2.01175, 2.21043, -0.857638 -1.81129, 2.90638, -0.105016 -2.30347, -0.0267789, -1.38968 -1.88704, 2.32719, -0.509441 -1.77487, 2.20325, -0.478337 -1.88012, 2.26255, -0.425578 -1.99795, 1.76601, -0.234893 -1.99149, 1.80305, -0.139807 -1.95757, 1.93395, -0.466622 -2.11817, 2.43413, -0.932648 -1.86796, 1.87073, -0.505585 -2.08149, 2.55914, -0.411996 -2.08613, 2.01226, -0.123173 -2.22466, -0.0134826, -1.32219 -2.27291, 0.717333, -1.019 -1.9835, 1.95371, -1.06631 -2.16079, 2.47104, -0.967831 -2.26095, 1.46721, -0.640283 -1.80577, 2.22956, -0.471838 -2.10157, 1.09091, -0.234483 -2.59112, 0.897493, -1.14029 -1.98879, 1.83189, -0.173653 -1.9532, 1.58129, -0.283188 -2.18284, 1.15923, -0.205938 -2.27601, 0.925845, 0.030572 -2.14676, 1.25911, -0.551287 -1.12125, 0.857883, -0.471551 -1.85891, 4.42449, -0.434774 -2.15084, 1.37595, -0.417599 -1.82798, 2.15677, -0.406057 -1.70656, 2.26204, -0.244345 -1.45604, 2.81133, 0.0254183 -2.22941, 0.980214, 0.250703 -2.04717, 1.99778, -0.878258 -2.03135, 1.79027, -0.262106 -1.60305, 3.47152, 0.0394478 -2.31389, -0.0298761, -1.39821 -1.85433, 1.81934, -0.77626 -2.14977, 1.18657, -0.51377 -1.03556, 0.87023, -0.0186296 -2.30169, 0.0433727, -1.46728 -2.28607, -0.00527282, -1.38117 -1.9436, 1.73617, -0.149468 -1.9853, 1.83499, -1.58637 -2.28149, 0.857724, 0.0159187 -2.15992, 1.3398, -0.69684 -1.16763, 2.12328, -1.82569 -1.22319, 4.23449, -2.13481 -1.22309, 4.40535, -2.12908 -1.84847, 3.61939, -0.599935 -1.79088, 3.51367, -0.524611 -1.51318, 2.7779, -0.0427025 -1.13412, 3.19788, -1.79154 -1.66967, 3.63992, -0.437372 -1.49915, 2.86539, -0.0449773 -1.5553, 2.89738, -0.0174857 -1.81169, 4.15986, -0.3292 -1.71959, 2.42536, 0.0416402 -1.73995, 4.07779, -0.374796 -2.386, 1.52812, -0.651771 -2.13253, 1.2575, -0.378711 -1.025, 3.69069, -1.83629 -0.605321, 3.88781, -1.85654 -1.11333, 3.80702, -1.88541 -1.75505, 3.01727, -0.506655 -1.76271, 4.31633, -0.372573 -1.27652, 4.27127, 0.090869 -1.76078, 3.00108, 0.0136462 -1.55322, 2.12023, -0.08037 -1.09465, 2.31336, -1.77983 -2.68108, 11.8313, -8.15658 -1.79193, 3.20017, -0.529789 -1.7271, 3.85355, -0.435071 -1.55166, 3.65462, 0.00425459 -1.6633, 2.84371, -0.0481661 -1.58804, 2.77301, -0.012263 -1.12114, 3.16037, -1.76669 -1.82853, 2.87562, -0.482121 -1.74376, 3.01611, -0.473893 -1.99249, 3.81885, -0.312375 -1.87671, 3.6041, -0.280391 -1.48944, 2.70718, -0.0741739 -2.15433, 1.3696, -0.395308 -2.14485, 1.35617, -0.371631 -2.10634, 1.59504, -0.276568 -0.736078, 8.11332, -3.58078 -1.68349, 2.76789, -0.135255 -1.68621, 2.96909, 0.0759224 -1.45029, 2.70236, 0.00958602 -1.9497, 1.15616, -0.372698 -1.73265, 2.45317, 0.0371242 -1.03349, 0.869326, -0.0235706 -1.08613, 2.40315, -1.80543 -1.15409, 3.69511, -1.8503 -0.995276, 10.9122, -4.8776 -1.81939, 2.38698, -0.4261 -1.85033, 2.57251, -0.424025 -1.01402, 0.898432, -0.0157182 -1.81449, 2.4912, -0.405114 -1.58416, 2.65139, -0.0856256 -1.73045, 2.73975, -0.432157 -1.76751, 2.71172, -0.411464 -1.72277, 2.44782, -0.0897871 -1.18922, 4.01416, -2.02098 -2.71371, 15.8338, -7.38075 -1.77017, 3.15013, -0.72576 -1.75979, 3.04209, -0.476634 -1.74565, 2.83308, -0.417934 -2.1127, 1.40884, -0.337416 -2.09279, 4.85424, -0.256159 -1.14879, 2.9553, -0.074747 -1.58405, 2.25713, -0.0329521 -1.42856, 4.07964, 0.124527 -1.02074, 3.80276, -1.82545 --0.282087, 5.94113, -1.11699 --2.27743, 10.8779, -1.6514 -1.83459, 3.03201, -0.477121 -1.74529, 3.34819, -0.454755 -1.82239, 3.35525, -0.449213 --0.546783, 5.06035, -0.0875954 -1.81971, 2.16491, 0.0315617 -1.03264, 3.16277, -1.78145 -0.990515, 3.43672, -1.67975 -0.944428, 5.79016, -2.65552 -1.80632, 2.67812, -0.474679 -2.0859, 4.00942, -0.420273 -0.596379, 3.1282, -1.82354 -1.07622, 3.42254, -1.89837 --1.03042, 9.02399, -1.42387 -1.87338, 3.69785, -0.567675 -1.79768, 2.278, -0.475366 -1.83247, 2.85536, -0.477554 -1.77653, 2.84491, -0.438229 -1.72968, 2.52103, -0.00885184 -1.87932, 3.41922, -0.758667 -1.79159, 2.34673, -0.424097 -0.884234, 5.02756, -2.76764 -1.03713, 3.31492, -1.83711 -0.89485, 5.42443, -2.52743 -1.98598, 3.21107, -0.527803 -1.29221, 4.43344, 1.1895 -0.80695, 0.929757, -0.17208 -2.01247, 2.25558, -0.550744 -1.8378, 2.72492, -0.349339 -1.51011, 2.79583, 0.0279761 -1.22979, 2.17763, 0.0249127 -1.96218, 3.22676, -0.691789 -1.85333, 3.2932, -0.60738 -1.92422, 3.5803, -0.57559 -0.953696, 5.63533, -0.435833 -1.55043, 2.71449, 0.0179878 -1.11599, 3.83523, 0.333601 --0.00345637, 2.72967, -1.99441 -0.534234, 2.56893, -1.86262 -0.971214, 4.00935, -1.91711 --3.5774, 11.7925, -1.89456 -1.75878, 3.28661, -0.467972 -1.80845, 2.57266, -0.422228 -1.79593, 2.56982, -0.403005 -1.70944, 2.40238, -0.362751 --1.92208, 6.68366, 0.0928073 -1.60338, 3.09681, 0.00566493 -0.91223, 3.23759, -1.81458 -1.03931, 4.06089, -1.75287 --2.33295, 9.15133, -1.55221 --1.93668, 6.73813, -1.24425 --1.38284, 7.5139, -1.2123 --2.10131, 7.66088, -1.19075 --1.31333, 7.33042, -1.13737 -1.83227, 2.53378, -0.441897 -1.77452, 2.98556, -0.02517 -1.54174, 3.03471, -0.0226982 -0.289154, 4.94944, 0.234103 -1.75089, 2.48432, -0.0445284 -0.834527, 0.936017, -0.197385 -0.908824, 5.19981, -2.89133 -2.00393, 1.98927, -0.859711 --3.65983, 12.1098, -1.93849 -1.86552, 3.53547, -0.699552 -1.89208, 2.34334, -0.468571 -1.82914, 2.77582, -0.456021 --1.85856, 7.36802, -0.528971 -1.76887, 3.93035, -0.432208 -1.82612, 2.29582, -0.426492 -1.78781, 2.73906, -0.420876 -1.7554, 3.10322, -0.400934 -1.67053, 3.73795, -0.224595 -1.70524, 2.26034, -0.242295 -1.44992, 2.70019, -0.141361 -1.57477, 3.64539, 0.0261844 -1.75598, 3.59662, 0.0746324 -1.62242, 2.79301, 0.0528424 -1.80238, 2.12912, 0.0209277 -1.28403, 2.67203, 0.453863 -1.03972, 0.873646, -0.0166163 -0.283913, 3.45066, -1.78535 -1.98303, 1.94283, -0.84899 --1.5404, 7.40544, -1.237 --2.56334, 9.09013, -1.38363 -1.91287, 2.06828, -0.425101 -1.88526, 2.30541, -0.356116 -1.72715, 2.67545, -0.104621 -1.81079, 1.68607, -0.187072 -0.832978, 0.935029, -0.199056 -0.578417, 2.8047, -1.93555 --1.91079, 7.4093, -1.34162 --1.1669, 7.03181, -1.18085 -2.02788, 2.66684, -0.595292 -1.81825, 1.97321, -0.374351 -1.62539, 2.59682, -0.160266 -1.93365, 1.78381, -0.202736 --0.694286, 5.42592, -1.02391 -2.00955, 2.14935, -0.308651 -1.62087, 3.63925, -0.117808 -0.600836, 4.27358, -2.08498 -2.03159, 2.61879, -0.827692 -1.70767, 2.85441, -0.412142 -1.89059, 1.72905, -0.204349 -1.6001, 6.03423, 0.879318 -2.3544, 3.0829, -0.257031 -1.7917, 3.73641, -0.22323 -1.93871, 2.06141, -0.288403 -1.02927, 2.64637, -0.0748574 -1.51581, 1.97236, -0.039168 --0.234797, 5.04237, -1.00546 --1.39833, 5.48932, -1.05529 -1.80804, 3.18663, -0.609727 -1.76428, 3.04073, -0.478585 --3.73251, 12.1933, 0.0422583 --0.374167, 2.35687, 0.426083 -1.73293, 2.96304, -0.461453 -1.87947, 2.30178, -0.404158 --0.626187, 4.95241, 0.0541008 -1.74712, 2.9209, 0.0112319 --0.21656, 3.95991, 0.120175 --0.488956, 2.33311, 0.451373 -0.307896, 3.13864, -1.6365 --1.45529, 3.8142, -1.85987 --1.28969, 6.66328, -1.16456 -1.83343, 3.43018, -0.731004 -2.02926, 2.63816, -0.604993 -1.75661, 3.15795, -0.42801 -1.7492, 3.1564, -0.403714 -0.699203, 4.37746, -0.29337 -1.43038, 2.73518, -0.167899 -1.59511, 3.69204, -0.0104219 -1.65062, 2.7871, -0.00199712 --1.04239, 3.40745, 0.168971 -1.13601, 2.86804, 0.813012 -0.83384, 0.935809, -0.199002 -2.08347, 3.45071, -0.79133 -1.71541, 2.76782, -0.0941831 -1.83589, 2.15624, 0.0316178 -2.03841, 2.28086, -1.09432 -1.78551, 2.34636, -0.424376 -1.85992, 2.74963, 0.0511908 -1.30102, 2.71829, 0.637748 -0.890814, 5.45123, -2.55008 --1.15714, 6.06195, -1.08123 -1.87122, 2.31212, -0.35387 -1.59951, 2.42432, -0.0510066 -1.67606, 2.41919, -0.000116328 -2.01824, 1.97138, -0.90301 -1.94767, 2.31959, -0.419826 -1.76428, 2.07227, -0.365152 -2.15364, 2.37367, -0.995584 --0.511249, 5.45359, -0.32808 -1.71012, 2.95237, -0.19708 -1.81268, 2.3453, -0.147329 -1.14574, 3.29836, -1.84351 -1.07268, 3.98792, -1.84154 --1.43304, 7.52312, -1.09546 -1.80794, 2.98847, -0.479108 -0.135559, 5.10508, -0.0483498 --1.73349, 3.19107, 0.0798054 -1.23395, 2.69869, 0.737086 --1.2484, 3.45801, -1.75704 --2.20619, 4.06969, -2.05901 -2.01883, 2.55901, -0.836785 -1.77215, 3.49782, -0.452632 -1.84048, 2.5142, -0.403025 --1.36619, 5.22068, -0.260465 -1.66434, 3.71773, -0.226448 --0.551968, 5.0749, -0.0453334 -1.70592, 2.12093, 0.0215242 --2.18434, 4.01767, -2.04094 -1.79385, 3.5139, -0.55294 -1.7912, 2.36944, -0.476052 -1.73939, 3.49072, -0.450778 -0.260397, 5.06058, -0.00558304 -1.23772, 3.80895, 0.103189 -1.16842, 2.73933, 0.729482 --0.369194, 2.36054, 0.427483 --1.24336, 6.03476, -1.13031 -1.85858, 2.30949, -0.470592 -1.88658, 2.35596, -0.424937 -1.7297, 2.77839, -0.410127 --1.57977, 4.7859, -0.0113193 -1.4383, 2.74872, -0.163795 -1.66844, 2.3884, -0.010886 --1.826, 4.22379, -1.72468 --0.695101, 5.34261, -1.61369 --1.77628, 4.46463, -0.512213 -1.83465, 3.62245, -0.320024 -0.0593452, 4.98635, -0.0312382 -1.51323, 3.47009, -0.0617981 -1.8069, 2.29701, 0.0374096 -0.591924, 3.6647, -1.77426 --2.14132, 4.06459, -2.06072 --1.05625, 5.52043, -1.65676 --2.22402, 8.60954, -1.46791 --1.22435, 7.56595, -1.27667 -2.18601, 3.36443, -0.515828 -2.02667, 2.22052, -0.415372 -1.03858, 3.96632, -1.81178 -1.68731, 2.5742, -0.525458 -1.6872, 2.7284, -0.444972 --0.234189, 5.36061, 0.038121 -1.18416, 3.78548, 0.9369 -1.15302, 3.07172, 0.843917 -2.03162, 2.269, -0.464501 -1.71733, 3.74927, -0.407172 -1.592, 2.79761, 0.0147575 -1.17447, 2.03901, -0.0069798 -0.803299, 3.20176, -1.7684 -1.79308, 3.44216, -0.444044 --0.98057, 5.11175, 0.00960877 -1.18621, 3.77648, 0.0102367 -1.46985, 2.77918, -0.0287164 --0.97204, 3.26946, 0.139911 --0.731462, 2.15417, 0.433012 --0.529415, 2.16566, 0.424049 --1.22863, 4.83844, -0.377262 -1.67788, 2.12154, -0.397795 -0.355139, 5.0972, -0.0610284 -0.133903, 5.10375, -0.0092092 --0.713221, 4.89719, 0.115152 --0.354969, 2.37046, 0.42559 --0.607241, 2.09923, 0.400505 --0.161171, 5.41276, -1.15233 -1.80764, 2.27641, -0.480513 -1.68214, 3.5437, -0.409851 -1.72678, 2.78462, -0.407686 -1.79282, 2.58781, -0.103782 -1.06338, 2.97832, -0.0331289 -1.77267, 2.64171, -0.0127875 --1.09687, 3.60981, 0.196798 --0.978349, 3.29133, 0.137687 -1.01087, 3.13128, -1.74331 --2.93687, 5.06019, -2.55259 -0.219757, 5.31811, -1.60523 -2.12022, 2.74402, -0.611787 -1.91577, 2.53716, -0.475462 -1.80901, 2.66617, -0.474105 -1.7406, 2.70284, -0.102774 -1.19005, 3.78383, 0.346298 -1.21548, 2.3463, 0.11335 -1.08649, 2.7296, 0.365615 --0.0688065, 3.6187, -1.76952 --0.998716, 5.27457, -1.60286 --1.81441, 4.89594, -1.05186 -2.04113, 3.37037, -0.767989 --1.87495, 4.74451, -0.882275 --1.933, 4.86115, -0.602771 -1.77525, 3.33387, -0.454532 -1.78184, 3.15571, -0.422335 --1.71028, 4.84492, -0.266352 --0.609874, 4.9837, -0.0600146 -0.072312, 5.04962, 0.0404781 -1.86502, 2.93084, -0.08377 -1.84423, 3.71361, 0.00330391 -1.55718, 3.37181, -0.0265314 --0.661683, 1.93442, -0.280899 -1.56921, 2.64003, -0.122297 -1.76984, 3.54543, 0.0394171 -1.23731, 3.79447, 0.100533 -1.76888, 2.9285, 0.0722163 --4.257, 6.4527, 0.790815 -1.46563, 5.02449, 0.490765 -1.22215, 2.14397, 0.0288569 -1.04808, 2.80626, 0.846129 --0.681766, 2.12083, 0.402455 -1.04503, 2.73903, 0.875132 -2.08136, 3.36795, -0.575746 -1.84408, 2.76566, -0.51436 --1.69258, 7.25471, -0.639804 -1.74678, 2.81862, -0.413825 -1.5034, 3.26231, -0.407514 -1.6429, 3.29446, -0.333817 -1.55394, 3.51193, -0.149896 -1.67443, 2.54194, 0.00976875 -1.10253, 3.5783, 0.280828 -2.01887, 2.19791, -0.922299 -1.99082, 2.5672, -0.811259 --1.78465, 4.54423, -0.67185 -1.8427, 3.51886, -0.511449 --1.48713, 5.64919, -0.493878 -2.0182, 4.36158, -0.26914 -0.208609, 4.98827, 0.146512 -1.50768, 3.26871, -0.0226726 -1.39637, 4.07333, 0.0826738 -1.15648, 2.0356, -0.0123994 --1.44025, 4.0982, -1.83284 -0.360326, 4.85874, -1.44825 -2.04364, 4.18435, 0.0632444 -1.55804, 5.69155, 0.556184 -2.37843, 3.12144, 0.27788 --1.52346, 3.24642, -1.81857 --1.08805, 1.66734, -1.07795 --1.11983, 1.66875, -1.04724 --0.275369, 5.34851, -1.13286 --1.77744, 7.41382, -0.671982 --0.83745, 1.86475, -0.410346 --1.27353, 5.05891, 0.0045922 -0.443438, 5.17969, 0.080636 --0.653445, 5.00898, 0.0558886 --0.83728, 1.82395, -0.577527 --0.1875, 5.09234, -0.107649 -0.355772, 5.06315, -0.00108423 --1.29562, 4.98149, 0.0111365 --1.56123, 4.19574, 0.148213 -1.04368, 2.92378, -0.0327546 --1.44892, 2.91284, 0.0901404 -1.19237, 2.79252, 0.751416 --0.662761, 2.19146, 0.434478 --1.00927, 1.64873, -1.11563 --1.25704, 3.91134, -1.77145 -0.216458, 5.35433, -1.38374 -2.00196, 2.25655, -0.728302 --0.808102, 1.77685, -0.568429 -0.95122, 5.3096, 0.0900387 --0.720024, 2.27665, 0.425463 --1.41439, 4.34298, -1.89024 --1.78895, 4.18203, -1.70402 -1.94494, 3.53794, -0.771191 -1.8692, 3.29469, -0.600645 -1.8598, 2.42751, -0.425161 -1.68034, 3.83241, -0.00904647 -1.33395, 5.38108, 0.318908 --0.943335, 4.74728, -1.82942 --0.596567, 1.65824, -0.747275 -1.58819, 3.77695, 0.020737 -1.36399, 5.33209, 0.559031 --1.87457, 3.86582, -2.12456 -1.36663, 4.84829, -2.26034 -0.9428, 4.36668, -1.99353 --1.58294, 3.95942, -1.71315 -0.476328, 5.13775, 0.0206687 -0.250976, 4.80933, 0.0716288 -1.63771, 2.79525, 0.0324676 -1.38415, 5.35594, 0.52786 -1.18164, 3.12581, 0.906775 -0.397741, 4.02166, -1.25276 -1.53013, 3.67059, 0.00576959 --1.92992, 5.0124, -1.29273 -1.90813, 3.15975, -0.533464 --1.66803, 3.36222, 0.155126 -1.43315, 3.18801, -2.34706 --0.817934, 1.79176, -0.540239 -1.31174, 2.73031, 0.770096 --0.827204, 1.86152, -0.398058 --1.10259, 1.68023, -1.13298 --0.827904, 1.79464, -0.541954 --1.32975, 4.95519, 0.152848 -1.04915, 2.94404, -0.0246836 --0.725361, 2.15315, 0.429153 --0.72227, 2.17641, 0.439843 --0.672143, 2.22075, 0.418623 --1.34034, 5.57263, -1.00795 --0.817547, 1.82712, -0.393913 --1.24452, 5.02604, -0.000420162 -1.07169, 2.97911, -0.0286444 --0.592521, 1.82216, -0.376019 -1.34306, 2.79091, 0.811283 --0.986045, 1.6571, -1.04469 --0.638933, 2.13614, 0.419301 -1.63916, 3.10961, 0.0231855 -1.63071, 3.11192, 0.0128388 -1.34058, 3.73613, 0.0421263 --0.897741, 1.73231, 0.175946 --0.654424, 2.16032, 0.372547 --0.469821, 2.24291, 0.419285 -2.33268, 4.27709, -3.38909 -0.946204, 2.42695, -1.78168 --0.92767, 1.64523, -1.24852 --0.980127, 1.02362, -0.0545446 --1.15702, 1.24833, -0.0480663 --0.728818, 2.30878, 0.444521 --1.18656, 1.08811, -0.0132058 --1.17268, 1.10452, -0.0338886 --1.04371, 1.06224, -0.0541608 --1.06457, 1.04676, -0.0393654 --0.899562, 1.7585, 0.185607 --0.644788, 2.17057, 0.433184 --1.04639, 1.07984, -0.0562799 --0.723207, 2.17823, 0.440539 --1.21461, 1.27733, -0.0180453 --1.00438, 1.18369, -0.0463548 --1.16995, 1.26929, -0.0193724 --1.88157, 0.610746, -1.16749 --1.04526, 1.65182, -1.09875 --1.18327, 1.27695, -0.00964839 --1.79123, 0.374143, -1.30825 --1.81881, 0.35416, -1.28838 --1.8069, 0.584333, -1.2074 --1.80636, 0.598935, -1.20818 --1.80229, 0.466719, -1.18681 --1.80693, 0.583593, -1.19877 --1.83661, 0.34487, -1.16257 --1.81252, 0.625086, -1.14428 --1.78457, 0.639051, -1.07248 --1.80984, 0.937232, -0.972146 --0.974018, 1.10766, -0.0586474 --0.9667, 1.02895, -0.0565374 --0.947951, 0.875602, -0.0324376 --1.79795, 0.616895, -1.20975 --1.76884, 0.601398, -1.17514 --0.966374, 0.907174, -0.354156 --1.02085, 1.17328, -0.0598895 --1.03058, 1.15138, -0.0619771 --0.972049, 1.08535, -0.0552536 --0.970031, 1.05948, -0.0553534 --1.8325, 0.616143, -1.23208 --1.80558, 0.591781, -1.18063 --1.80469, 0.584778, -1.1612 --1.81752, 1.26677, -0.808564 --1.80622, 1.10149, -0.774573 --1.8091, 1.00918, -0.730751 --1.80465, 1.24671, -0.738515 --0.755154, 1.78261, -0.578299 --1.79428, 1.09021, -0.588622 --1.79435, 1.28596, -0.556471 --1.79587, 1.25134, -0.542992 --1.08969, 1.30547, -0.399751 --0.970115, 0.909839, -0.352872 --1.81971, 0.970222, -0.1151 --1.02869, 1.16599, -0.0676921 --1.03238, 1.15109, -0.0594651 --1.01693, 1.13627, -0.0636043 --1.92638, 0.671434, -1.24402 --1.82308, 1.06638, -0.748292 --1.81312, 0.978142, -0.614341 --1.80792, 0.977233, -0.526871 --1.80926, 0.754556, -0.386617 --0.961041, 0.895138, -0.342123 --1.02769, 1.17474, -0.0674396 --1.02712, 1.17933, -0.0548553 --1.82233, 1.26509, -0.767419 --1.80901, 1.13603, -0.734666 --1.8001, 1.22608, -0.722442 --1.81271, 1.3121, -0.725599 --1.85117, 1.33501, -0.617733 --1.81253, 1.2208, -0.518813 --1.79233, 0.953708, -0.147356 --1.60435, 1.00999, -0.0453061 --1.08802, 1.32625, -0.0306743 --1.01932, 1.16222, -0.077821 --1.00966, 1.14383, -0.0782671 --1.07371, 1.64798, -0.958409 --1.79385, 1.07511, -0.64576 --1.80669, 0.688164, -0.0743454 --1.4614, 0.951846, -0.105523 --1.82792, 1.23887, -0.727913 --1.78317, 0.923076, -0.114931 --1.81041, 0.602499, -1.20251 --1.80745, 0.492199, -1.18037 --1.80364, 0.618945, -1.1677 --1.80923, 0.468822, -1.26929 --1.81793, 0.363368, -1.11038 --1.08414, 1.67253, -1.12639 --1.05082, 1.66771, -1.11205 --1.77316, 0.086983, -1.27855 --1.79648, 0.718678, -0.363896 --1.82012, 0.627794, -0.21838 --1.81565, 1.10064, -0.77403 --1.8151, 1.12763, -0.716453 --1.79471, 1.01511, -0.548638 --1.81731, 0.943251, -0.327053 --1.81754, 0.889554, -0.0646611 --1.82544, 0.535327, -1.32843 --1.80578, 0.419839, -1.23028 --1.81454, 0.627954, -0.223604 --1.80687, 0.105132, -0.62004 --1.80891, 0.171334, -0.600414 --1.80528, -0.115642, -0.544246 --1.79862, 0.496729, -1.17599 --1.80641, 0.751256, -0.186793 --1.80757, 0.27743, -0.495041 --1.80263, 0.581761, -1.20402 --1.79046, 0.611297, -1.15943 --1.80922, 0.610186, -1.1409 --1.80889, 0.160681, -0.760143 --1.80827, 0.0776296, -0.667733 --1.81592, 0.11987, -0.561356 --1.80812, 0.00994117, -0.548505 --1.80071, 0.422441, -1.24372 --1.808, 0.135358, -0.687104 --1.80988, -0.00956022, -0.544582 --1.91057, -0.848978, -1.34478 --1.81296, 0.100599, -0.645259 --1.8073, 0.117724, -0.571378 --1.69323, -1.4574, -1.6706 --1.77205, -1.67333, -1.8021 --6.85205, -12.2331, -9.74684 --1.79862, -0.942802, -1.33095 --1.80745, 0.596654, -1.21751 --1.80129, 0.134039, -0.758851 --1.74919, -1.53167, -1.74099 --1.70993, -1.49391, -1.68431 --1.80844, -0.637225, -0.93693 --1.80535, 0.12691, -0.64409 --1.8089, -0.851174, -0.293781 --1.80699, 0.102517, -0.636614 --1.80882, -0.340886, -0.311869 --1.84581, -1.02894, -0.833932 --1.80145, -0.935047, -1.3204 --1.933, -2.48336, -2.14471 --1.80856, 0.169681, -0.651668 --1.81488, -1.2846, -0.621477 --1.71928, -1.59211, -1.7089 --1.68459, -2.16153, -1.85636 --1.78205, -2.23601, -1.86576 --1.85439, 0.91728, -1.42336 --1.79248, 0.391576, -1.22639 --1.21232, -0.810223, -0.143427 --4.29649, -7.88111, -6.11033 --1.60449, -2.08659, -1.75862 --1.72898, -2.14996, -1.8392 --1.72383, -2.25843, -1.88441 --1.73419, -2.10155, -1.80384 --1.76496, -2.2645, -1.89043 --1.81043, -0.42949, -0.390512 --1.81269, -0.853058, -0.286185 --1.80732, -0.476083, -0.282383 --1.16874, -0.738964, -0.336845 --1.53234, -0.814578, -0.0884828 --1.21928, -0.818669, -0.137622 --1.28282, -0.791287, -0.0423561 --1.46389, -0.85518, -0.440127 --1.22764, -0.763718, -0.0521268 --1.80605, -0.315142, -0.279738 --1.56978, -0.826854, -0.35195 --1.539, -0.781778, -0.0585687 --1.43827, -2.18853, -1.82544 --1.75203, -1.52533, -1.56431 --1.59063, -2.11214, -1.78649 --1.66752, -2.05744, -1.72438 --1.65651, -1.97881, -1.68636 --1.4939, -0.835075, -0.465706 --1.46785, -0.820764, -0.140538 --1.52606, -0.835097, -0.326734 --1.38004, -2.04855, -1.72741 --1.47394, -2.23896, -1.86524 --1.67804, -2.0235, -1.70625 --1.04129, -2.08923, -0.0943635 --1.48066, -2.07697, -1.75178 --1.58999, -2.05018, -1.69215 --1.62011, -2.07835, -1.71106 --1.04559, -2.58505, -1.32686 --1.8081, -0.392069, -0.566377 --0.9252, -1.24265, -0.171405 --0.855445, -1.23975, -0.172118 --1.80525, -1.02559, -0.604134 --1.82681, -0.310172, -0.276391 --0.912711, -1.22579, -0.182389 --1.8182, -0.420095, -0.393264 --0.959358, -1.09806, -0.335193 --1.83334, -0.341339, -0.338458 --1.69824, -1.86929, -1.70795 --1.73265, -1.93669, -1.76007 --1.67315, -2.19973, -1.80015 --1.70151, -2.08249, -1.74003 --1.6014, -2.08444, -1.67782 --1.56146, -1.35417, -0.247331 --0.854249, -1.24087, -0.17218 --0.919216, -1.18843, -0.116468 --1.72285, -2.15451, -1.73936 --1.56818, -0.813121, -0.355908 --0.953413, -1.25398, -0.168461 --0.917771, -1.23668, -0.17774 --0.924086, -1.23619, -0.172596 --0.848533, -1.24277, -0.176704 --0.850785, -1.25571, -0.170116 --1.19532, -0.836712, -0.136158 --1.76928, -1.95826, -1.82169 --1.72167, -1.91001, -1.7458 --1.75579, -1.99873, -1.80641 --1.43511, -0.968984, -0.424131 --1.57712, -0.846442, -0.344934 --0.917836, -1.31642, -0.165313 --0.948874, -1.2513, -0.170635 --0.933692, -1.18995, -0.0865236 --0.831164, -1.17282, -0.105278 --0.885201, -1.13825, -0.0349952 --1.81311, -1.21051, -0.80771 --1.54231, -0.84953, -0.464745 --1.82523, -0.626828, -0.377538 --0.967856, -1.08995, -0.330108 --0.926988, -1.23305, -0.175858 --0.863688, -1.25269, -0.165335 --0.838998, -1.23381, -0.176129 --0.85207, -1.18756, -0.0899005 --1.53517, -0.850025, -0.46333 --1.83763, -0.591468, -0.390734 --1.81255, -0.584232, -0.371955 --1.58986, -0.827426, -0.376292 --0.958334, -1.0936, -0.35351 --1.83477, -0.594471, -0.0949706 --0.913987, -1.32243, -0.164707 --0.910052, -1.1889, -0.11268 --1.2255, -0.777271, -0.101807 --0.895889, -1.18978, -0.101174 --1.80897, -1.22172, -0.606178 --1.89582, -0.676141, -0.332601 --0.956446, -1.06671, -0.334454 --0.963572, -1.11154, -0.322277 --2.28293, -0.666656, -0.294947 --1.70547, -1.75109, -1.72328 --1.80586, -2.33993, -1.87419 --1.02529, -2.50088, -1.2905 --0.884818, -1.14046, -0.0337652 --1.72572, -1.79021, -1.75717 --1.57052, -1.3614, -0.243337 --1.80893, -1.30659, -0.796128 --1.50736, -0.993933, -0.465586 --1.0492, -2.59363, -1.26311 --0.954315, -1.15727, -0.114559 --0.833813, -1.18309, -0.0970518 --1.77286, -0.982964, -0.818169 --1.58715, -2.0626, -1.62414 --0.133183, -2.67595, 0.0451152 --0.930586, -1.06643, -0.050451 --0.143395, -2.76215, -0.963136 --0.196131, -2.19972, 0.132541 --0.932037, -1.06994, -0.0477107 --0.0645388, -2.24753, 0.0606509 --1.78504, -1.25633, -0.600729 --1.79313, -1.19805, -1.01982 --0.251208, -2.37286, -0.00255711 --1.70021, -1.24412, -0.623458 --0.583566, -2.87228, -0.900612 --1.53834, -2.03696, -1.71438 --1.65971, -2.02397, -1.6901 -0.318993, -2.7888, -1.00651 -0.495046, -2.76035, -0.987401 -0.556562, -2.53499, -0.730467 -0.607381, -2.73514, -0.750026 --0.201254, -2.35555, 0.00938517 --0.879838, -1.46106, -0.161014 --1.62895, -1.9683, -1.66417 --0.997069, -2.409, -1.24525 --0.869552, -1.33639, -0.160782 --1.80285, -1.26923, -0.861565 --0.140338, -2.71973, -0.964024 --0.160034, -2.56272, 0.0141086 -0.222661, -2.23949, 0.0404631 --0.915828, -1.46534, -0.165764 --0.977645, -1.28221, -0.162595 --0.979075, -1.27268, -0.16235 --0.814821, -1.29717, -0.157274 --1.01583, -2.45021, -1.20016 --0.215857, -2.70581, -0.901701 --0.194135, -2.40685, 0.00275721 -0.043671, -2.21268, 0.242072 --1.65215, -2.06897, -1.59397 -0.358673, -2.67199, -1.01224 -0.489251, -2.79401, -0.948158 --0.516199, -2.68809, -0.88987 -0.461077, -2.80497, -0.914542 --0.389743, -2.56851, 0.00639365 --1.63595, -2.07143, -1.47563 --0.761371, -2.26863, -1.0183 --0.692024, -2.66148, -0.966142 --0.804857, -2.37556, -1.05388 --0.935816, -1.26884, -0.169345 --0.109254, -2.78757, -0.968707 -0.217151, -2.80051, -0.972815 -0.118883, -2.33623, 0.0414859 -0.0436387, -2.2473, 0.260986 -0.326215, -2.81312, -0.976856 -0.0574342, -2.45914, 0.031734 -0.110362, -2.32703, 0.0376001 -0.585055, -2.55306, -0.944252 --1.0604, -2.10002, -0.998879 --0.186149, -2.1984, 0.134079 -0.364966, -2.6894, -1.01882 --0.521539, -2.65733, -0.872808 --0.747555, -2.81703, -0.411302 --0.352029, -2.4944, -0.0267561 -0.0430976, -2.38787, 0.0426694 --0.492104, -2.84075, -0.235992 --1.59052, -2.06192, -1.66313 -0.665013, -2.11324, -1.08853 -0.27715, -2.71221, -1.01527 -0.193947, -2.79861, -1.00881 -0.597455, -2.75366, -0.727072 -0.737174, -2.69086, -0.692049 -0.104124, -2.51106, 0.00786367 -0.0236538, -2.82664, -1.29532 -0.401965, -2.68741, -1.02642 -0.0332075, -2.74665, -1.02397 -0.501417, -2.78965, -0.964748 -0.731605, -2.68958, -0.690139 -0.201645, -2.75274, -1.03617 -0.25914, -2.63459, -1.00164 --0.137911, -2.72831, -0.969462 --0.0129434, -2.70558, -0.896932 --0.0696548, -2.611, -0.845072 -0.079581, -2.74287, -0.820201 -0.0449648, -2.20173, 0.235012 -0.29501, -2.70865, -1.03542 -0.6231, -2.1838, -0.0248551 -0.608006, -2.27145, -0.0176886 -0.221215, -2.26631, 0.0472831 -0.408739, -2.65721, -1.01067 -0.458665, -2.38593, -0.0972669 -0.543099, -2.26539, 0.00130679 --0.421864, -2.80038, -0.764601 --0.259012, -2.78893, -0.63967 --0.168725, -2.22864, 0.145104 -0.369017, -2.83149, -0.877458 -0.432392, -2.66932, -1.02958 -0.745765, -2.68396, -0.705077 -0.794159, -2.68478, -0.699666 -0.759263, -2.68492, -0.693308 -0.80831, -2.68244, -0.698492 -0.790123, -2.67009, -0.653493 -0.164973, -2.65075, -0.0527313 -0.0964316, -2.32793, 0.0364782 -0.227379, -2.2332, 0.0410694 -0.295291, -2.78655, -0.968091 -1.41644, -2.82738, -1.01878 --0.0219396, -2.73259, -0.879943 -0.87028, -2.85369, -0.851149 -0.935248, -2.74037, -0.728124 -1.82601, -2.41354, -0.436224 --0.115957, -2.82656, -0.0125997 -0.628203, -2.21337, -0.020835 -0.0273261, -2.83541, -1.29874 -0.57276, -2.4965, -1.04053 -0.0424671, -2.80568, -0.972725 --0.093069, -2.84557, -0.932542 -0.942684, -2.65625, -0.71489 -0.581151, -2.62197, -0.686115 --0.43147, -2.8253, -0.231596 -0.0824267, -2.49329, 0.0326831 -0.0461146, -2.45106, 0.04075 -0.586984, -2.20719, -0.0155041 -0.225304, -2.24504, 0.0428834 --0.131028, -2.60081, -0.874712 -0.666653, -2.65446, -0.769922 --0.0223425, -2.81209, -0.730865 --0.448218, -2.87047, -0.673264 -0.894004, -2.65263, -0.636457 -0.625437, -2.66535, -0.553113 --0.699444, -2.64435, -0.259247 --1.6794, -2.09499, -1.55027 --0.73565, -2.83398, -1.03941 --0.0840643, -2.64999, -0.85735 -0.84061, -2.79639, -0.842685 --0.469388, -2.73945, -0.633782 --0.757155, -2.82406, -0.227061 --0.788962, -1.56334, -0.0266802 --0.118526, -2.70861, -0.95706 -0.743605, -2.66431, -0.694641 -0.499129, -2.32639, -0.0418942 -0.426856, -2.23323, -1.03233 --0.781704, -2.99838, -1.07348 --0.528057, -2.70507, -0.848526 --0.354219, -2.69379, -0.71123 -0.813468, -2.6248, -0.569713 --0.375556, -2.59922, 0.0320749 --0.0145113, -2.22341, 0.142666 -0.0980125, -2.59119, 0.0307223 --0.0605249, -2.71623, -0.894514 --0.0128211, -2.75641, -0.913096 -0.505328, -2.47117, -0.0725891 --0.214279, -2.34467, 0.00995804 -0.589454, -2.22762, -0.00697048 --0.193952, -2.21826, 0.242467 --0.189814, -2.20988, 0.234841 -0.989826, -2.78884, -0.762798 -0.65115, -2.75757, -0.668711 -0.196479, -2.38245, 0.0684846 -0.694362, -2.6891, -0.665464 -0.7399, -2.67761, -0.652904 -0.0449526, -2.21563, 0.365545 -0.301046, -2.72778, -1.01885 -1.83221, -2.41888, -0.43829 --0.0590583, -2.72139, -0.894374 -0.858979, -2.80975, -0.798513 -0.623573, -2.65435, -0.556778 --0.197025, -2.23258, 0.375146 -0.893334, -2.64406, -0.648262 -1.01072, -2.84094, -0.967173 -0.593527, -2.69404, -0.678345 -0.558779, -2.30497, -0.0295319 --0.175324, -2.21452, 0.24637 -0.12501, -2.74729, -1.07524 -0.871458, -2.82531, -0.846847 -0.808911, -2.67417, -0.705755 -0.777192, -2.67109, -0.654469 -1.54576, -2.66331, -0.654262 -0.784551, -2.65144, -0.6111 --1.04357, -2.51151, -1.29706 -0.753928, -2.8252, -0.953087 -0.685638, -2.69803, -0.705748 -0.507717, -2.14415, -1.06254 -1.48556, -2.74412, -0.697013 -1.54602, -2.65567, -0.654181 -0.0750286, -2.60013, 0.0301679 -1.06648, -2.2457, -0.0233504 -0.111307, -2.34118, 0.0418374 -0.937042, -2.68099, -1.39067 -0.317917, -2.80333, -0.986362 --0.159487, -2.72406, -0.905114 --0.0212159, -2.71661, -0.872149 -0.716882, -2.8239, -0.822297 -0.652867, -2.23794, -0.0564491 -0.758165, -2.25974, -0.0271198 -0.0775353, -2.52864, 0.0453409 -0.567399, -2.14006, -0.0419725 -0.661031, -2.1771, -1.13306 -1.16683, -2.73595, -1.46335 -0.466309, -2.8223, -0.918696 --0.0549522, -2.72683, -0.744745 --0.353229, -2.76156, -0.693609 -0.301785, -2.49453, 0.0635947 -0.0231631, -2.27016, 0.0481563 --0.185641, -2.20854, 0.237707 -1.41148, -2.68603, -1.4311 -0.138773, -2.83126, -1.06109 --0.0322683, -2.71386, -0.894652 -0.684281, -2.67465, -0.694009 -0.924019, -2.32614, -0.0272829 -0.0727427, -2.66411, 0.0984052 -0.486772, -2.39858, 0.0191555 -1.02231, -3.4086, -1.16037 -0.986706, -2.86831, -0.973091 -0.878364, -2.59038, -0.673666 -0.864058, -2.67845, -0.593712 -0.0962804, -2.53615, 0.0624298 -0.259775, -2.72699, -0.869879 -0.538307, -2.71222, -0.667684 -0.880052, -2.8845, -0.688852 -1.03088, -2.65834, -0.702218 -0.87389, -2.78361, -0.554783 -2.41125, -2.52988, -0.448125 -0.172705, -2.69303, -0.0337886 -1.37136, -2.83033, -0.985453 -1.00651, -2.61665, -0.708422 -1.50378, -2.83315, -0.763367 -0.730466, -2.67552, -0.655969 -1.37186, -2.80777, -0.772888 -1.69983, -2.22136, -0.0325186 -0.695325, -2.33978, -0.057067 -0.57958, -2.38637, -0.0367226 -0.790023, -2.81705, -0.927248 -0.745665, -2.88689, -0.810927 -0.796135, -2.37377, -0.0455977 -0.0374946, -2.20076, 0.138916 -0.799072, -1.64396, -0.856517 -0.805946, -1.79636, -0.921802 -0.748177, -2.66543, -0.695951 -1.3984, -2.62696, -0.68081 -0.543039, -2.32207, -0.0118955 -0.705427, -2.77805, -0.801474 -0.938688, -1.91912, -1.0034 -0.222947, -2.25014, 0.057875 -0.684118, -2.82211, -0.971987 -0.756, -2.6782, -0.705699 -0.79299, -2.68457, -0.696191 -0.647892, -2.12156, -1.11533 -0.417615, -2.72919, -1.00476 -0.746592, -2.81871, -0.907117 -0.9545, -2.82876, -0.872972 -1.46125, -2.83466, -0.745658 -1.32084, -2.6559, -0.688 -0.752493, -2.64981, -0.61686 -0.759105, -2.81829, -0.896876 -1.94359, -2.27319, -0.237669 -0.69268, -2.36968, -0.0473697 -0.270248, -2.6627, 0.037953 -1.06609, -2.2476, -0.0227121 -0.718092, -2.24189, -0.0212661 -0.323483, -2.74542, -0.971067 -1.42546, -2.74773, -0.743904 -1.53798, -2.81836, -0.7545 -1.21014, -1.43289, -0.0206022 -1.02301, -1.92995, -1.01497 -0.701968, -2.59273, -0.642906 -1.01746, -3.03625, -0.893236 -1.39352, -3.19903, -0.911132 -0.855727, -2.77573, -0.582471 -0.752683, -2.69936, -0.705778 -1.91032, -2.30333, -0.32626 -1.96608, -2.26822, -0.235598 -1.85385, -2.26502, -0.147573 -0.873583, -2.67351, -0.72156 -1.37363, -2.42622, -0.628244 -1.40162, -2.55465, -0.624474 -1.8851, -2.27764, -0.332783 -1.886, -2.30714, -0.139029 -1.7366, -2.22523, -0.0152179 -0.873857, -2.29233, -0.0434418 -0.650498, -2.17197, -1.14663 -0.675489, -2.11862, -1.11873 -0.993203, -2.69115, -0.740136 -0.917567, -2.65135, -0.698193 -1.46643, -2.79585, -0.745926 -1.72731, -2.34673, -0.462701 -1.92566, -2.23178, -0.239961 -0.873461, -2.26176, -0.054289 -1.01578, -2.25274, -0.0233088 -0.648858, -2.12293, -1.10479 -1.1102, -2.02397, -1.06733 -0.919475, -2.73572, -0.702098 -1.80547, -2.53017, -0.487249 -1.91544, -2.30288, -0.396776 -1.61334, -2.15358, -0.0599214 -1.09075, -2.2847, -0.0515236 -0.968354, -2.40713, -0.0137974 -1.05302, -2.29309, -0.0048505 -0.964601, -1.91224, -1.01226 -1.04566, -2.9924, -0.862739 -1.33984, -2.63779, -0.775281 -1.29812, -2.74523, -0.694199 -1.54632, -2.79897, -0.721518 -2.26053, -2.71568, -0.740561 -1.74898, -2.39485, -0.45865 -1.12006, -2.54108, -0.432423 -1.8466, -2.41928, -0.330589 -1.84828, -2.42443, -0.288549 -0.794583, -2.36845, -0.0437556 -0.623801, -2.14043, -0.0386972 -0.871004, -2.76686, -0.933682 -1.57988, -2.60861, -0.628206 -2.36584, -2.70922, -0.604023 -1.05317, -2.54393, -0.514599 -1.81698, -2.42272, -0.159614 -1.35132, -2.59481, -0.0620124 -1.3419, -2.57188, -0.114123 -2.15481, -2.80751, -0.123493 -1.12573, -2.17563, -0.038238 -2.17266, -1.35294, -0.610986 -1.0143, -1.09408, -0.0378951 -1.05254, -1.10759, -0.0379262 -1.02016, -1.09996, -0.0310817 -1.01939, -1.08705, -0.0174352 -1.05892, -1.04553, -0.0146748 -2.15142, -1.33735, -1.06818 -2.17099, -1.30253, -0.604071 -1.01448, -1.38367, -0.0174294 -0.973597, -1.36521, -0.0173028 -1.01021, -1.09953, -0.0328579 -1.04395, -1.05967, -0.0207511 -1.05649, -1.04098, -0.0188552 -1.45406, -2.70775, -0.692592 -2.17718, -1.3073, -0.602998 -1.0101, -1.36831, -0.024959 -1.41484, -2.54415, -0.620104 -1.58862, -2.80564, -0.765879 -1.04664, -1.06112, -0.0384606 -1.05507, -1.04295, -0.0202286 -1.10849, -2.09783, -1.10356 -1.04607, -1.06467, -0.0377225 -1.02175, -1.09646, -0.0339773 -1.05188, -1.05326, -0.0432154 -1.06734, -1.05932, -0.0195253 -2.16819, -1.54466, -0.841512 -0.996804, -1.38375, -0.0266286 -0.983941, -1.36337, -0.00875218 -1.0565, -1.05612, -0.0312316 -1.06372, -1.04723, -0.0254587 -2.3073, -0.742467, -1.2832 -2.18442, -1.36975, -0.692884 -1.01214, -1.35441, -0.0226731 -2.15518, -1.42773, -0.857179 -2.28815, -0.0510482, -1.3587 -2.30858, -0.0518662, -1.3599 -2.29862, -0.0118576, -1.35244 -2.28855, -0.00170406, -1.34614 -2.17825, -1.5408, -0.89314 -2.17764, -1.32125, -0.742036 -4.18043, -1.90019, 0.411266 -1.0069, -1.10883, -0.0302467 -2.2908, 0.0344197, -1.45776 -2.29606, -0.0291711, -1.38885 -2.29307, 0.0361544, -1.28756 -1.82098, -2.21143, -0.161118 -2.2736, -0.877532, -0.153196 -2.27633, -0.856537, -0.119141 -1.009, -1.0976, -0.0359721 -2.28761, -0.476026, -1.46626 -2.32534, 0.0514791, -1.48247 -2.28712, -0.0253184, -1.38254 -2.28976, 0.0592111, -1.32066 -2.17065, -1.5153, -0.864611 -2.18301, -1.34556, -0.845246 -2.16813, -1.32352, -0.739068 -2.16454, -1.35757, -0.676535 -2.16564, -1.38797, -0.66675 -1.90483, -2.25705, -0.442695 -2.24723, -1.02826, -0.391301 -2.24154, -0.865294, -0.158095 -3.90761, -1.67528, 0.342252 -3.01823, -1.18115, 0.115899 -2.1276, 0.413507, 0.576726 -1.07241, -1.09564, -0.0267287 -1.04863, -1.07303, -0.0328233 -1.04358, -1.05954, -0.0209763 -2.28713, -0.501847, -1.46526 -2.07078, -1.97459, -1.67529 -2.16227, -1.4179, -0.861689 -2.09516, -1.32443, -0.798805 -2.1804, -1.54089, -0.767884 -2.13767, -1.30356, -0.715116 -2.18512, -1.50304, -0.683704 -2.24419, -1.03229, -0.387434 -2.03543, -2.13012, -0.188481 -2.30419, 0.388167, -1.45864 -2.24169, -0.739881, -1.22392 -2.17193, -1.3714, -0.82664 -2.24821, -2.6919, -0.740486 -2.30148, -1.0875, -0.422408 -2.28561, -0.889516, -0.127448 -2.20723, 0.298656, 0.566409 -2.1493, -1.52743, -0.872755 -2.15991, -1.37709, -0.849629 -2.16113, -1.39863, -0.740958 -2.04558, -1.31434, -0.682162 -2.28982, -1.07783, -0.646423 -2.1664, -1.54862, -0.546234 -2.19514, -1.21005, -1.16369 -2.08154, -2.03685, -1.13723 -2.13677, -2.10241, -0.905346 -2.07532, -2.15762, -0.125903 -2.27676, -1.58936, -0.956294 -4.7748, 0.154246, -1.16458 -2.23818, -1.19758, -0.154672 -2.27377, -0.145155, -0.776028 -2.2856, -1.24251, -0.0790706 -2.28645, -1.22503, -0.0779506 -2.31605, 0.178928, 0.395983 -1.05245, -1.10715, -0.0260377 -1.06382, -1.07728, -0.0158444 -2.29253, -0.577634, -1.47814 -2.2916, -0.508401, -1.45657 -2.27425, -0.809495, -0.148564 -2.29537, 0.166874, 0.386884 -2.23747, 0.308586, 0.577912 -2.12213, 0.309349, 0.564111 -1.05672, -1.05925, -0.0276357 -2.28976, 0.0531429, -1.25584 -2.18711, -2.34812, -1.02684 -1.92435, -2.281, -0.418093 -1.846, -2.34634, -0.237564 -2.05518, -2.14367, -0.186347 -2.2891, 0.0542366, -0.221658 -2.24698, -0.804053, -0.155992 -2.2895, 0.107933, 0.327493 -2.30532, 0.0343224, 0.335995 -2.29732, 0.169865, 0.389173 -2.15844, 0.311862, 0.559806 -2.06419, 0.286126, 0.52137 -1.05476, -1.05463, -0.0298225 -2.29107, 0.0559099, -1.37609 -2.16589, -2.31302, -1.00901 -1.8532, -2.35604, -0.240564 -2.12697, -1.52256, -0.735626 -1.91412, -2.27005, -0.441578 -1.97004, -2.3372, -0.416451 -1.95126, -2.03848, -0.420437 -2.27147, -1.0787, -0.423137 -1.82864, -2.17869, -0.3813 -1.92175, -2.2512, -0.245468 -2.15475, -1.37558, -0.685997 -2.06551, -2.13958, -0.160248 -2.27066, -1.55685, -0.981258 -2.32575, -1.22227, 0.0791523 -2.29375, -0.53387, -1.43981 -2.27037, -0.715455, -0.302844 -2.28939, -0.501962, -1.46131 -2.17242, -1.49842, -0.682008 -2.28048, -0.723656, -0.266043 -2.26576, -0.380253, -0.149251 -2.269, -0.437948, -1.2272 -2.25545, -1.07307, -0.420672 -1.04136, -0.288977, -0.544614 -2.13328, -1.43243, -0.682475 -2.18633, -1.09767, -0.103973 -2.38511, -1.1098, -0.0490736 -2.29004, -0.707254, -1.26505 -2.15556, -1.31191, -0.737802 -2.16203, -1.29575, -0.608426 -2.20666, 0.430338, 0.586795 -2.27223, -1.00549, -0.15342 -2.27856, -0.878467, -0.150769 -2.31604, 0.751222, 0.664293 -2.27009, 0.506699, 0.640373 -1.04124, -0.259056, -0.484508 -2.27388, -0.809703, -0.149743 -2.26519, -0.854617, -0.121848 -10.4976, -3.71185, 2.11713 -1.04377, -0.208773, -0.395654 -2.17117, -1.58264, 0.81009 -2.14066, 0.319954, 0.580583 -1.04151, -0.222053, -0.505161 -1.04239, -0.20604, -0.42674 -1.0404, -0.181192, -0.349252 -1.04355, -0.186994, -0.591309 -2.32975, -2.56325, -1.0742 -1.04057, -0.243632, -0.498253 -1.04331, -0.263384, -0.489057 -1.04079, -0.211317, -0.39561 -1.04057, -0.259237, -0.385439 -1.0416, -0.26992, -0.380145 -1.0414, -0.31041, -0.34326 -2.15515, -1.35725, 0.839723 -1.04275, -0.178845, -0.5438 -1.04063, -0.168259, -0.519804 -2.18316, -1.34445, -0.857254 -1.04134, -0.17996, -0.494518 -1.03715, -0.182038, -0.47203 -1.04157, -0.166914, -0.43586 -1.04369, -0.245092, -0.43006 -1.04044, -0.180151, -0.42273 -1.04166, -0.203233, -0.399731 -2.14845, -1.61812, 0.818583 -1.04156, -0.166546, -0.531905 -2.11433, -1.46652, -0.566031 -1.04297, -0.203055, -0.398207 -1.04094, -0.287539, -0.35172 -1.04214, -0.309231, -0.339543 -1.04301, -0.257356, -0.480287 -2.2291, -1.44814, -0.565915 -1.0457, -0.288588, -0.459814 -2.15531, 1.33331, -0.934261 -2.16077, 1.39407, -0.663392 -2.17972, 1.18571, -0.651955 -2.13771, 1.20576, -0.598641 -2.45189, 1.00699, 0.932012 -2.15556, 1.15971, -0.99966 -2.06973, 0.168883, -0.711493 -2.34685, 1.60066, -0.54285 -1.04035, -0.228673, -0.507529 -1.04224, -0.169392, -0.469312 -2.16954, 1.40435, -0.50916 -2.14498, 1.14124, -0.46241 -2.13781, 1.233, -0.36687 -1.13818, -0.236503, -0.418674 -2.1603, 1.30467, -1.16275 -2.20962, 0.673446, -0.994881 -1.04157, -0.238831, -0.494759 -1.03964, -0.263076, -0.48379 -1.06022, -0.0201321, -0.437091 -1.04585, -0.0534548, -0.438336 -1.04077, -0.248777, -0.432714 -1.04119, -0.213857, -0.427711 -1.04094, -0.212038, -0.424809 -1.04086, -0.206269, -0.391543 -2.11227, 1.08781, 0.533014 -2.12593, -1.53267, 0.815235 -1.05403, -0.147066, -0.522165 -1.03781, -0.168736, -0.511833 -1.03901, -0.247853, -0.498353 -1.0413, -0.129338, -0.494078 -2.1789, -1.55968, -0.555778 -1.03933, -0.169776, -0.464312 -2.14713, 1.29348, -0.472731 -1.04088, -0.170139, -0.432702 -1.04523, -0.207145, -0.426185 -1.43709, -0.237126, -0.34857 -1.04027, -0.21168, -0.395449 -1.04073, -0.263838, -0.387499 -1.04347, -0.252354, -0.381171 -1.04546, -0.147892, -0.386426 -1.03946, -0.161353, -0.376125 -1.04432, -0.14715, -0.372386 -1.36356, -0.207843, -0.210468 -1.36706, -0.102221, -0.160079 -2.21688, -1.64344, 0.869083 -1.06916, -0.0833787, -0.571932 -1.05452, -0.148479, -0.52581 -1.0394, -0.233998, -0.510296 -1.0412, -0.237907, -0.503406 -3.75186, 3.15942, -0.927578 -1.03604, -0.178968, -0.457102 -1.04151, -0.15579, -0.388347 -1.04123, -0.252775, -0.368593 -1.04117, -0.26998, -0.361737 -1.0358, -0.237741, -0.353612 -3.84244, 2.04588, -2.1832 -1.04007, -0.234593, -0.522876 -1.04576, -0.229904, -0.515822 -1.04903, -0.252421, -0.501271 -1.04191, -0.166596, -0.472048 -1.03762, -0.0917029, -0.462885 -1.02348, -0.230529, -0.438083 -1.04053, -0.179496, -0.435136 -2.28062, 0.869507, -1.48967 -2.15529, 1.28017, -1.09932 -2.2661, 0.797031, -1.01138 -2.14037, 1.22661, -0.585784 -2.14407, 1.2569, -0.550248 -2.16771, 1.42371, -0.532488 -2.14567, 1.18697, -0.515641 -2.14826, 1.34506, -0.365358 -2.14776, 1.38199, -0.339195 -2.11512, 1.08467, 0.0337876 -2.12091, 1.09993, 0.047966 -2.2283, 0.981392, 0.111709 -2.22841, 0.907753, 0.250601 -2.29367, 0.9429, -1.48557 -2.16364, 1.28041, -1.11514 -2.27017, 0.704922, -1.02106 -2.15938, 1.15847, -1.04646 -2.12003, 0.206182, -0.720585 -2.16568, 1.12502, -0.65564 -2.16595, 1.3779, -0.396206 -2.16317, 1.38436, -0.352246 -2.0875, 1.57669, -0.278684 -2.11843, 1.08387, 0.0344846 -2.161, 1.12056, 0.0908172 -2.25459, 0.910255, 0.262438 -2.23924, 0.908003, 0.255492 -2.08531, 1.07036, 0.525438 -2.10928, 1.08021, 0.55586 -2.10351, 0.815863, 0.613358 -1.58439, 0.35928, 0.176925 -2.29132, 0.957852, -1.44958 -2.27041, 0.85495, -1.23636 -2.16164, 1.28057, -1.10607 -2.03075, 1.95747, -0.925335 -1.0422, -0.169864, -0.512426 -2.16289, 0.312911, -0.72961 -2.17535, 0.166411, -0.707717 -2.18402, 0.282302, -0.699209 -2.15733, 0.308456, -0.693399 -2.17901, 0.166175, -0.682276 -2.15866, 1.23571, -0.570414 -2.21052, 1.46247, -0.540423 -2.15502, 1.40849, -0.454275 -2.1426, 1.33041, -0.426838 -1.96754, 1.76728, -0.147053 -2.08526, 1.99655, -0.0889604 -2.09624, 1.07632, 0.549186 -2.07093, 0.808146, 0.626102 -1.03955, -0.149234, -0.512054 -1.03937, -0.268101, -0.488174 -2.12895, 1.28761, -0.383486 -1.04056, -0.252471, -0.367845 -2.17377, 0.792234, 0.577889 -2.20948, 0.896416, -1.39622 -2.08696, 1.12806, -1.02092 -1.02668, -0.19188, -0.538229 -1.03953, -0.149058, -0.533453 -1.03831, -0.165813, -0.531039 -1.03977, -0.160624, -0.526646 -1.03953, -0.166633, -0.52039 -1.04087, -0.161613, -0.507486 -2.17177, 1.20588, -0.504946 -1.03859, -0.165173, -0.422904 -1.03613, -0.210835, -0.40093 -1.04166, -0.161611, -0.374603 -1.04342, -0.131104, -0.348754 -2.09776, 1.08049, 0.525538 -2.2929, 0.62123, -1.29844 -1.04473, -0.182141, -0.546237 -1.04055, -0.167443, -0.518568 -1.03614, -0.222686, -0.48898 -1.04267, -0.18054, -0.472363 -1.04019, -0.166489, -0.471976 -1.04382, -0.180731, -0.45862 -2.29658, 1.4136, -0.426284 -1.04909, -0.172962, -0.435083 -1.04781, -0.0283509, -0.438474 -2.41243, 0.990514, -0.0183294 -1.04014, -0.131828, -0.351773 -1.03398, -0.140552, -0.512702 -1.02374, -0.165247, -0.492736 -1.03764, -0.130164, -0.488477 -1.03815, -0.175936, -0.435444 -1.02636, -0.124648, -0.402614 -1.03801, -0.141369, -0.370279 -2.27787, 0.613916, -1.31455 -2.17118, 1.17388, -1.17675 -2.15182, 1.30763, -1.15086 -2.14249, 1.35051, -1.1045 -2.15551, 1.37074, -1.099 -2.16368, 1.23893, -1.05131 -1.991, 1.9781, -0.859535 -2.01817, 2.00014, -0.860853 -1.90512, 1.87127, -0.829765 -2.26434, 0.30138, -0.714878 -2.14252, 1.17433, -0.501857 -2.15101, 1.34815, -0.50413 -2.13823, 1.15917, -0.371598 -2.1399, 1.19902, -0.347042 -2.14722, 1.32084, -0.306764 -2.02981, 1.40486, -0.294679 -2.36432, 0.938193, -0.0495947 -2.11386, 1.09471, 0.0606752 -2.19636, 0.41814, 0.59879 -2.11895, 0.678749, 0.596131 -2.28981, 0.299616, -1.51614 -2.20115, 1.34698, -1.19035 -2.17591, 1.32443, -1.1626 -2.14434, 1.35165, -1.16006 -2.26323, 0.699155, -1.00648 -2.14724, 1.35301, -1.03963 -2.15838, 1.32898, -0.945975 -2.15755, 1.26073, -0.549776 -1.99256, 1.79673, -0.142212 -2.07659, 1.90596, -0.097456 -2.11509, 1.09163, 0.0487391 -2.28005, 0.933001, 0.277527 -2.29633, 0.189877, 0.33118 -2.29726, 0.276421, -1.38941 -2.29243, 0.943667, -1.48623 -2.28419, 0.291247, -1.36735 -2.29402, 0.629897, -1.26277 -2.28685, 0.638698, -1.24016 -2.28773, 0.920721, -1.24312 -2.14164, 1.25176, -0.993862 -2.16145, 1.2222, -0.995109 -2.25249, 0.226825, -0.748392 -1.86494, 1.82777, -0.815014 -2.34354, 0.350968, -0.725876 -2.14978, 1.20506, -0.583217 -2.15097, 1.23881, -0.579108 -1.80379, 1.05622, -0.480843 -2.30588, 1.56077, -0.452114 -2.06189, 1.33711, -0.340297 -2.06931, 1.86329, -0.241007 -2.16388, 1.33655, -0.241634 -2.29062, 0.10689, 0.328648 -2.23371, 0.757755, 0.594717 -2.19469, 0.424282, 0.560788 -2.28249, 0.935586, -1.47766 -2.28004, 0.702983, -1.41054 -2.26975, 0.626297, -1.23403 -2.24561, 0.901476, 0.238568 -2.29185, 0.317991, -1.50755 -2.28626, 0.372178, -1.45513 -2.30057, 0.619115, -1.30047 -2.30138, 0.14489, -1.24918 -2.2454, 0.703217, -1.22677 -2.02349, 1.85644, -0.102998 -2.12828, 1.10033, 0.0223884 -1.47247, -0.605173, 0.16106 -2.19115, 0.430693, 0.562332 -2.29928, 0.79111, -1.0291 -2.26222, 1.47542, -0.553326 -2.10163, 1.09006, -0.448236 -0.951276, 0.0269742, -0.383027 -0.966301, 0.0108088, -0.36662 -2.29331, 0.907506, -1.28928 -2.11256, 1.09378, -0.666512 -1.98895, 1.10107, -0.57073 -2.28807, 0.366246, -1.48443 -2.30033, 0.384303, -1.29396 -2.29496, 0.0942007, -1.23867 -2.27947, 0.866092, -1.24267 -2.17105, 1.38043, -1.1755 -2.15485, 1.37115, -1.15272 -2.15035, 1.36238, -1.04157 -2.1345, 1.28807, -0.994692 -2.14448, 1.35462, -0.996146 -2.15324, 1.33751, -0.9868 -2.16269, 1.37856, -0.991639 -2.27975, 0.36301, -0.856408 -2.27859, 0.314848, -0.850591 -1.91751, 1.89982, -0.793351 -2.25834, 0.472281, -0.71873 -2.12558, 1.19588, -0.596484 -2.15052, 1.21319, -0.547663 -2.14422, 1.23268, -0.537219 -2.19561, 1.39442, -0.511057 -2.14074, 1.35346, -0.459149 -2.15111, 1.36768, -0.41649 -1.96398, 2.39714, -0.419086 -2.16944, 1.28458, -0.408875 -2.14888, 1.32383, -0.401477 -2.01952, 1.39334, -0.291133 -2.04682, 1.54667, -0.283724 -2.14091, 1.21452, -0.282026 -2.26038, 0.861053, -0.26617 -2.02234, 1.92529, -0.22499 -1.9912, 1.77103, -0.231442 -1.96395, 1.8266, -0.225876 -2.00889, 1.90861, -0.19962 -2.01004, 1.90432, -0.114514 -2.13808, 1.10662, 0.0435929 -2.24166, 0.990569, 0.116939 -2.24379, 0.00693489, 0.333334 -2.2881, 0.00547487, 0.384631 -2.27942, 0.16008, 0.374156 -2.16627, 0.408718, 0.573945 -2.1602, 0.694579, 0.628547 -1.89815, 1.72476, -1.48877 -2.27985, -0.00249987, -1.3802 -2.31216, -0.0367136, -1.3821 -2.30768, 0.931703, -1.43762 -2.29975, -0.0192565, -1.34971 -2.28864, 0.625378, -1.25869 -2.16237, 1.3728, -1.15797 -2.15819, 1.36283, -1.10908 -2.13541, 1.1734, -0.994266 -2.16315, 1.25542, -0.99251 -2.17314, 1.34289, -0.996599 -2.27798, 0.361586, -0.854683 -2.34505, 2.57645, -0.978259 -1.91449, 1.89409, -0.786958 -1.94567, 2.32125, -0.785655 -2.15299, 1.19965, -0.584084 -2.14799, 1.23108, -0.53838 -2.14224, 1.24629, -0.525973 -2.16217, 1.18629, -0.502978 -1.97587, 2.16496, -0.399479 -2.13816, 1.21195, -0.280422 -2.28144, 0.866349, -0.263091 -2.27206, 0.005885, 0.377147 -2.2875, 0.159487, 0.381934 -2.28595, 0.108479, 0.424917 -2.17893, 0.789436, 0.583385 -2.15223, 0.707974, 0.587743 -2.15408, 0.404915, 0.564297 -2.28473, 0.0419107, -1.45115 -2.28275, 0.0425121, -1.42859 -2.28893, 0.924304, -1.4262 -2.29168, 0.199161, -1.34491 -2.30236, 0.094471, -1.24295 -1.98331, 1.94053, -1.09562 -2.14727, 1.254, -0.995335 -2.2983, 0.366304, -0.858032 -2.18893, 0.429272, -0.756838 -1.98126, 2.20135, -0.865114 -2.14999, 1.15372, -0.585666 -2.13708, 1.22015, -0.533666 -2.16752, 1.37461, -0.37145 -2.14425, 1.21453, -0.361013 -2.26733, 0.776338, -0.317807 -2.14291, 1.22489, -0.280453 -2.09317, 2.01197, -0.214892 -2.33681, 0.733575, -0.236888 -2.27728, 0.0305245, 0.32106 -2.28366, 0.16107, 0.380882 -2.28215, 0.138544, 0.417784 -2.32665, 0.190617, 0.445342 -2.10957, 1.08393, 0.496491 -2.1202, -1.51818, 0.801412 -2.24447, 1.06515, -1.19646 -2.19006, -1.4185, -0.607565 -1.95125, 2.09549, -0.633584 -2.1584, 1.40022, -0.55411 -2.03445, 1.53264, -0.08413 -2.28457, 0.0622628, 0.325979 -2.27184, 0.139442, 0.318319 -2.1186, 0.68937, 0.573016 -2.10807, 0.57903, 0.557505 -2.21199, 0.869537, 0.717187 -2.29521, 0.143482, -1.50417 -2.28645, 0.274909, -1.49802 -2.28098, 0.888438, -1.24158 -2.16139, 1.34531, -0.995983 -2.17358, 1.31519, -0.944978 -2.23704, -0.462997, -0.783769 -2.15741, 1.31251, -0.41516 -2.38131, -0.0953649, -0.24477 -2.20123, 1.17552, -0.217631 -1.88642, -0.571149, 0.277388 -2.2783, -0.3894, 0.600068 -2.13929, 0.0748854, -1.2422 -2.47863, 1.64986, -0.583561 -2.14413, 1.278, -0.353389 -2.1193, -0.0385255, -0.286757 -8.30415e+06, -5.63024e+06, 5.8153e+06 -2.18277, 0.805606, 0.57981 -2.11484, 1.08916, -0.67697 -2.15764, 1.22604, -1.1695 -2.14975, 1.24162, -1.16332 -2.15329, 1.19003, -1.11336 -2.1479, 1.21064, -1.10967 -2.15536, 1.3645, -1.11037 -2.14961, 1.18529, -0.99988 -2.14856, 1.27729, -1.0011 -2.144, 1.24245, -0.984209 -2.15596, 1.16566, -0.948138 -2.1546, 1.19091, -0.93513 -2.15577, 1.21175, -0.933435 -2.03997, 2.2497, -0.878348 -1.87223, 1.82887, -0.772918 -2.16189, 1.12309, -0.652912 -2.14222, 1.23698, -0.580978 -2.15051, 1.23582, -0.567228 -2.1365, 1.21889, -0.552733 -2.14362, 1.25281, -0.526588 -1.91491, 1.85949, -0.53458 -1.91154, 1.85356, -0.528813 -2.15164, 1.34943, -0.505705 -2.15295, 1.30867, -0.476465 -2.16034, 1.30888, -0.475622 -2.15169, 1.40882, -0.463417 -2.16136, 1.33969, -0.439937 -2.14802, 1.31211, -0.401487 -1.9579, 2.11043, -0.393884 -2.14774, 1.29114, -0.374168 -1.9524, 2.3131, -0.360809 -2.04878, 2.43935, -0.352552 -2.1485, 1.30474, -0.279763 -1.99291, 1.74817, -0.267065 -2.14469, 1.15143, -0.249323 -1.96177, 1.75423, -0.233572 -1.96517, 1.78446, -0.21544 -1.9617, 1.83991, -0.173789 -2.21407, -0.239566, 0.595992 -2.22117, 0.711378, 0.598669 -2.11754, 0.68751, 0.589001 -2.27142, 0.698416, -1.48206 -2.29046, 0.954525, -1.44724 -2.29982, -0.003198, -1.37162 -2.16272, 1.20363, -1.11594 -2.0134, 2.0558, -0.955699 -2.05491, 2.31902, -0.889167 -2.01123, 2.54665, -0.838896 -2.15743, 1.15435, -0.58615 -2.27214, 0.4534, -0.541237 -2.15181, 1.20805, -0.54841 -2.27649, 0.808031, -0.521448 -2.14988, 1.26951, -0.52627 -2.16316, 1.19609, -0.516366 -1.93979, 2.10075, -0.402497 -2.13595, 1.3545, -0.329986 -2.13237, 1.28252, -0.281366 -2.05645, 1.53925, -0.280346 -2.26338, 0.136264, -0.271072 -2.00247, 1.77233, -0.220093 -2.07526, 1.98949, -0.187795 -2.11733, 2.05116, -0.0859848 -2.13632, 1.10691, 0.066074 -2.17121, 1.12659, 0.0985655 -2.23677, 0.990841, 0.241208 -2.23003, -0.366296, 0.608603 -2.1143, 0.685617, 0.58695 -2.1542, 1.11763, 0.795582 -2.30107, 0.873708, -1.4644 -1.87335, 1.80452, -1.03974 -2.37832, 0.460185, -0.798307 -2.04937, 2.229, -0.897869 -2.29243, 0.456804, -0.542699 -2.15431, 1.21574, -0.549084 -2.14484, 1.33656, -0.509088 -2.26994, 0.289618, -0.402949 -1.89703, 2.05281, -0.399018 -2.27194, 0.334711, -0.355088 -1.98164, 1.84007, -0.218072 -1.96493, 1.8513, -0.189386 -2.31215, 0.211486, 0.315475 -2.29602, -0.0682335, 0.33351 -1.6861, 0.140365, -0.00251075 -2.27882, 0.0936837, 0.381521 -2.29217, -0.809714, 0.64038 -2.20711, -0.351872, 0.590014 -2.18036, 0.791037, 0.580348 -2.15645, 0.709921, 0.587515 -2.15053, 0.57026, 0.581602 -2.11291, 0.701669, 0.575504 -2.16774, 1.13938, 0.833837 -2.2756, 0.611961, -1.41415 -2.28361, 0.0513348, -1.35547 -2.31525, 0.0524435, -1.25735 -1.95534, 1.89129, -1.07219 -2.16075, 1.31703, -0.997863 -2.22333, 2.59715, -1.2148 -1.92892, 1.8013, -0.883864 -2.17656, 1.37428, -0.371869 -2.27281, 0.37271, -0.344744 -2.18941, 1.3376, -0.3407 -2.03452, 1.51523, -0.195194 -2.27684, 0.869754, -0.0744407 -2.15483, 1.11362, 0.041299 -2.21904, -0.354259, 0.597505 -2.19438, 0.828715, 0.586491 -2.15198, 1.12026, 0.797696 -2.2974, 0.623257, -1.42038 -2.2883, 0.056625, -1.37232 -2.32386, 0.173246, -1.3502 -2.28138, 0.263702, -1.23908 -2.41184, -0.45248, -1.32341 -2.1139, 1.33038, -1.03731 -2.19852, 1.44153, -0.68925 -2.1524, 1.33168, -0.46002 -3.42694, 3.89545, 0.176819 -2.05648, 1.56638, -0.0746491 -2.24011, 0.905889, 0.252965 -2.28187, 0.0871203, 0.41199 -2.26665, -0.379758, -1.26185 -2.14064, 1.30876, -0.936364 -2.17788, 1.14201, -0.212364 -1.95189, 1.79294, -0.168603 -2.22276, 0.897834, 0.106179 -2.11598, 0.292003, 0.549266 -2.25085, 0.80014, -1.22823 -2.12637, 1.32232, -0.613535 -2.15288, 1.13449, -0.439138 -2.11646, 1.25246, -0.366231 -2.26205, 0.943354, 0.192192 -2.19627, 0.791442, 0.0361184 -2.032, 1.92036, -0.215881 -1.96465, 1.85001, -0.137526 -2.15313, 1.36025, -0.943743 -1.81852, 2.31658, 0.0746686 -1.96522, 1.97021, -0.851795 -2.15692, 1.19057, -0.557611 -2.14563, 1.3381, -0.408538 -2.14619, 1.32535, -0.326992 -2.3019, 0.0493891, -1.4659 -2.2634, 0.714684, -1.01436 -2.14963, 1.39869, -0.430368 -2.21207, 0.716473, -0.992021 -2.23559, -1.17404, -0.124964 -2.27032, -1.18448, 0.00469787 -2.28832, 1.47385, -0.584789 -2.18773, 1.39597, -0.534866 -2.26802, 1.19354, -0.706548 -2.24383, 1.41063, -0.553159 -2.09859, 1.10969, -0.22925 -2.23692, 1.32171, -0.183578 -2.1482, 1.27435, -1.09536 -2.1513, 1.16345, -1.00163 -2.09088, 0.671546, 0.572114 -2.15284, 1.36423, -1.09622 -2.13881, 1.35019, -0.375419 -2.17259, 2.24717, -0.920547 -2.11286, 1.31541, -0.40405 -2.1281, 1.36693, -0.39262 -1.98769, 1.76987, -0.246166 -2.16546, 1.30766, -1.16425 -2.20884, 1.33745, -1.18882 -1.45346, 1.31691, -0.673033 -2.14454, 1.36315, -0.393215 -2.34663, 1.4076, -0.330246 -2.30516, -0.380756, 0.664147 -2.32225, 0.348083, -1.29364 -2.10801, 1.26793, -0.232206 -2.12749, 1.21, -0.44919 -2.21796, 1.30564, -0.186947 -2.14824, 1.21138, -1.11094 -2.02573, 2.31226, -0.6252 -2.12102, 1.31701, -0.467343 -2.15225, 1.24924, -0.999082 -2.15577, 1.36294, -0.999521 -2.14611, 1.25303, -0.549771 -1.98543, 2.16865, -0.399845 -2.21069, 0.346747, -0.742248 -1.71598, 2.44785, -0.0386018 -1.04969, 0.871157, -0.0246902 -2.17269, 1.30919, -1.16843 -2.12533, 1.38483, -0.452177 -1.68447, 2.20606, -0.247858 -2.29612, -0.111848, -1.28846 -2.17291, 1.26936, -0.412576 -2.14735, 1.32551, -0.329591 -2.13078, 1.34509, -0.608304 -2.14761, 1.14888, -0.226301 -2.04045, 2.53412, -0.812266 -2.01598, 2.64321, -0.809136 -2.0146, 2.67988, -0.580184 -1.62403, 2.38135, -0.494537 -1.81656, 2.32333, -0.480582 -2.13581, 1.39827, -0.464955 -1.84076, 2.65577, -0.47967 -1.85779, 3.0823, -0.477166 -1.92018, 1.80156, -0.178678 -2.25485, 0.993463, 0.121164 -2.17739, 1.31589, -1.16666 -1.68773, 2.16392, -0.381489 -2.07398, 1.54917, -0.280206 -1.63542, 2.86016, 0.00499551 -1.73277, 2.49937, -0.00954968 -1.03881, 0.870969, -0.0163079 -1.81497, 2.51583, -0.444907 -1.55733, 2.15246, -0.177694 -1.70746, 2.42437, -0.0908236 -1.49281, 2.43485, -0.0978387 -1.68819, 2.3852, -0.00784763 -1.05242, 0.909052, -0.0278555 -2.24933, 0.61906, -1.42898 -2.15988, 1.40052, -0.559883 -2.19698, 1.43028, -0.432338 -2.15813, 1.44771, -0.334622 -1.05489, 0.907453, -0.0276069 -2.26114, 0.890897, -1.46999 -2.25491, -0.414248, -1.42843 -2.2568, 1.337, -1.2055 -2.36802, 1.2274, -0.702189 -2.02856, 2.38498, -0.657009 -2.15885, 1.12783, -0.695363 -2.36392, 1.22674, -0.437104 -2.0116, 2.22702, -0.856885 -2.03598, 2.58389, -0.84605 -2.01898, 2.73555, -0.844463 -1.80037, 3.1609, -0.607557 -1.78489, 3.09851, -0.474425 -1.72824, 3.19529, -0.430931 -1.81135, 2.28189, -0.406126 -1.70249, 3.3716, -0.378874 -1.76983, 2.55702, -0.114847 -1.67021, 3.30246, 0.0184218 -1.54081, 2.83833, -0.0481231 -1.79213, 3.04255, 0.0295664 -1.73192, 2.47032, -0.0386407 -1.59194, 2.77709, -0.0159455 -1.01703, 0.901406, -0.0111628 -2.17456, 1.37496, -1.05072 -1.99045, 2.46829, -0.574935 -1.89307, 3.41927, -0.537958 -1.83673, 2.9971, -0.503415 -1.81242, 3.11831, -0.507262 -1.71318, 3.03316, -0.504659 -1.70798, 3.34499, -0.450358 -1.83368, 2.34929, -0.413277 -1.97095, 1.73085, -0.242822 -1.52038, 2.9048, -0.049474 -1.81769, 3.09797, 0.036363 -1.64912, 2.36615, -0.0245871 -1.05559, 0.856786, -0.00921581 -1.01613, 0.899295, -0.0118579 -2.01605, 2.28705, -0.914982 -1.99783, 2.48174, -0.606884 -2.1435, 1.1828, -0.55775 -1.85401, 2.2871, -0.510585 -1.80697, 2.39598, -0.477591 -1.78357, 2.25605, -0.476957 -1.77935, 2.64171, -0.475707 -1.82452, 2.53356, -0.423034 -1.80149, 2.55762, -0.422492 -1.79135, 2.74483, -0.421819 -1.74513, 2.81293, -0.406125 -1.6608, 2.81986, -0.0990578 -1.83296, 3.0253, -0.0211226 -0.834612, 0.942648, -0.197148 -0.872188, 0.925506, -0.155952 -0.868446, 0.91762, -0.147744 -1.04231, 0.902501, -0.037546 -1.01141, 0.893184, -0.0178019 -2.02185, 2.51639, -0.586627 -2.5081, 2.79751, -0.402983 -2.21365, 2.4295, -0.342915 -2.14939, 1.2912, -0.329317 -1.49384, 2.33614, -0.201595 -1.69983, 2.49789, -0.0375272 -1.73581, 2.5169, 0.0225311 -0.851303, 0.940791, -0.199223 -0.871306, 0.920803, -0.161477 -2.07047, 0.875766, -1.32615 -2.16575, 1.17667, -1.16811 -2.13591, 1.539, -0.2407 -1.02217, 0.897439, -0.0362817 -1.04679, 0.863087, -0.00934883 -2.42849, 2.57101, -0.892458 -2.01345, 2.59712, -0.825663 -2.13789, 1.12117, -0.448914 -2.09645, 2.64701, -0.327245 -2.12241, 1.30833, -0.330274 -1.66686, 0.37051, -0.977509 -2.16535, 1.28894, -0.374899 -2.19702, 1.28307, -0.186352 -1.95409, 1.91199, -0.852845 -1.91133, 2.25943, -0.622997 -2.0094, 2.53084, -0.580891 -1.93602, 3.84723, -0.529246 -1.8696, 3.07251, -0.505247 -1.75819, 2.60794, -0.479196 -1.88067, 2.98445, -0.462877 -1.73742, 3.02011, -0.45187 -1.78346, 2.55359, -0.404656 -1.61353, 2.04701, -0.4023 -1.97494, 1.84423, -0.224194 -1.79328, 2.60461, -0.106792 -0.826972, 0.926093, -0.204659 -0.863006, 0.909524, -0.163886 -1.04097, 0.905611, -0.0376837 -1.04351, 0.848389, -0.0202866 -2.1512, 1.19949, -0.583678 -1.78123, 3.11489, -0.604482 -2.12674, 1.27825, -0.475249 -2.04713, 3.29298, -0.489618 -2.1491, 1.34318, -0.46358 -1.89481, 2.64147, -0.47045 -1.70146, 2.88322, -0.465048 -1.84316, 3.67985, -0.410833 -1.80022, 2.2429, -0.407792 -1.75208, 2.79551, -0.40451 -1.52527, 2.77974, -0.0546614 -1.62068, 3.16556, 0.0101367 -1.47982, 2.78784, -0.0475919 -2.23603, 0.903456, 0.255428 -0.860883, 0.903455, -0.155804 -2.27838, 0.709286, -1.02362 -2.10595, 1.22893, -0.984652 -2.16116, 1.33374, -0.994292 -2.01096, 2.1982, -0.868485 -2.00802, 2.50906, -0.582222 -1.85878, 2.33352, -0.496401 -1.77634, 3.0537, -0.504175 -1.86542, 2.85073, -0.453626 -1.86174, 1.74901, -0.184149 -2.24477, 1.35055, -1.20089 -2.10188, 2.05938, -0.871449 -1.70717, 2.45165, -0.35051 -0.801832, 0.926575, -0.17171 -2.84006, 0.34683, -1.55839 -2.16995, 1.30473, -1.16532 -2.09685, 1.36973, -0.431128 -1.54604, 2.76059, -0.0165888 -1.04853, 0.863332, -0.0180002 -1.79753, 2.71234, -0.487973 -1.06992, 0.88209, 0.00934632 -2.29322, 0.89967, -1.29569 -2.32345, 1.22368, -0.181062 -1.99854, 2.22407, -0.969227 -2.01385, 2.02126, -0.87064 -2.05073, 2.03666, -0.870385 -1.92068, 3.33919, -0.731342 -2.01219, 2.64136, -0.618219 -1.95676, 2.08165, -0.52571 -1.78791, 3.23096, -0.503781 -1.74702, 3.04127, -0.474912 -1.80056, 3.2829, -0.468616 -1.82755, 2.81008, -0.428048 -1.81056, 2.38895, -0.423481 -1.92641, 2.0469, -0.376024 -1.9742, 1.85471, -0.168347 -1.71364, 2.43185, -0.0458108 -1.75303, 2.62182, -0.0171624 -1.73946, 2.53522, -0.0064916 -1.72533, 2.42648, 0.0359132 -1.01498, 0.897699, -0.0153007 -2.07598, 2.05114, -1.14671 -2.04591, 2.09537, -0.967109 -2.02682, 2.19043, -0.889035 -1.98647, 1.96677, -0.856053 -2.03256, 2.03849, -0.873146 -1.90551, 3.31748, -0.683148 -1.92456, 2.26665, -0.621446 -2.03094, 2.31849, -0.617757 -1.87039, 3.32136, -0.587187 -1.81067, 3.25199, -0.513076 -2.15116, 1.4043, -0.456081 -1.72672, 2.94326, -0.479596 -1.76549, 3.51648, -0.449995 -1.67933, 3.2965, -0.37836 -1.77829, 2.52994, -0.114686 -1.62132, 2.72336, -0.0755788 -0.825906, 0.92204, -0.205993 -2.03506, 2.5335, -0.608263 -2.06514, 2.69473, -0.61409 -1.79444, 2.52658, -0.492257 -1.03669, -0.201164, -0.463059 -1.87882, 2.36162, -0.427264 -1.69982, 3.35413, -0.378662 -1.97206, 1.89097, -0.150306 -1.76858, 1.64497, -0.177415 -1.81112, 3.0744, -0.0126943 -0.827986, 0.92738, -0.204284 -0.742472, 0.926233, -0.142595 -0.827751, 0.903594, -0.0624395 -0.771462, 0.94194, -0.064607 -2.29659, 0.279853, -1.3746 -1.05309, -0.220642, -0.546446 -1.86393, 3.28671, -0.751129 -1.06099, -0.258182, -0.526188 -1.03975, -0.169, -0.463317 -1.74479, 2.63395, -0.491649 -1.80753, 3.12393, -0.47662 -2.16371, 1.32589, -0.434709 -1.98263, 2.41675, -0.447351 -1.04526, -0.175266, -0.400009 -2.05334, 2.07614, -0.277476 -1.55575, 2.62077, 0.0187986 -1.61397, 2.06673, -0.0277157 -1.88347, 2.1124, -1.02599 -1.03755, -0.166552, -0.499704 -2.01127, 2.51056, -0.588141 -2.03041, 2.28902, -0.553231 -1.72142, 1.88331, -0.495496 -1.02114, -0.150085, -0.4274 -0.769079, 0.925746, -0.0824451 -0.770553, 0.972595, -0.0237849 -2.27206, 2.28936, -0.908425 -1.90772, 2.37561, -0.351512 -1.8416, 2.72329, -0.347916 -2.03776, 1.78567, -0.0839708 -1.49465, 1.95002, -0.037324 -2.28681, 0.644139, -1.28265 -2.02659, 2.00903, -1.0704 -2.0091, 2.58968, -0.821062 -1.59763, 1.8707, -0.438566 -2.06213, 1.88223, -0.174587 -2.58408, 0.16691, 0.432191 -3.28856, 1.00802, 0.3627 -1.16152, 2.14694, -1.81115 -1.48722, 2.7997, 0.00096451 -1.15788, 2.36884, -1.84199 -2.01949, 2.60505, -0.610607 -1.78097, 3.04738, -0.505357 -1.66818, 2.8081, -0.0134829 -0.982153, 0.873164, -0.0382552 -0.781324, 0.929309, -0.0822838 -2.03479, 2.63535, -0.818554 -2.02428, 2.62867, -0.821816 -1.88977, 2.11906, -0.396976 -1.71719, 2.46976, 0.0122168 -1.69928, 2.00506, -0.418458 -1.71559, 2.25367, -0.216045 -1.16691, 2.03346, -1.83735 -1.11889, 2.09374, -1.82318 -1.15963, 2.06471, -1.82885 -1.13763, 2.03561, -1.7808 -1.16079, 2.13438, -1.82345 -1.13526, 2.2239, -1.83938 -1.07192, 2.36105, -1.83156 -1.09476, 2.29284, -1.81002 -1.13664, 2.30278, -1.81682 -1.08502, 2.30621, -1.79266 -1.78843, 2.5598, -0.477466 -1.6696, 2.81586, -0.00951892 -1.55256, 2.77281, -0.0104582 -1.10716, 2.03213, -1.80394 -1.43102, 2.87544, -2.34145 -1.16343, 2.10366, -1.81158 -1.17387, 2.50122, -1.99495 -1.12449, 2.15742, -1.79892 -1.10859, 2.22853, -1.82518 -1.0888, 2.23342, -1.7987 -1.78035, 2.35062, -0.476922 -1.49018, 2.79798, -0.0519167 -1.14534, 2.35736, -1.83043 -1.80671, 2.56596, -0.473554 -2.01303, 2.64962, -0.589894 -2.18063, 1.1431, -0.43888 -2.0115, 2.07714, -0.640395 -2.0383, 2.16174, -0.284761 -1.15753, 2.58121, -2.10371 -1.27438, 2.52558, -2.13265 -1.15464, 2.02877, -1.8022 -1.17245, 2.12387, -1.85119 -1.70879, 3.30431, -2.64544 -1.02298, 2.3415, -1.8173 -1.07281, 2.2184, -1.78294 -1.14037, 2.23935, -1.82307 -1.15143, 2.29431, -1.83545 -1.07808, 2.30097, -1.79303 -1.16481, 2.39271, -1.85326 -1.77467, 2.35682, -0.479095 -0.773113, 0.950334, -0.126146 -1.01852, 0.89697, -0.0144962 -1.39608, 2.44489, -2.18223 -1.12037, 1.97032, -1.79905 -1.64577, 3.33765, -2.6704 -0.991981, 2.22273, -1.81856 -2.12542, 4.49754, -3.47169 -1.58824, 3.27619, -2.63925 -1.1151, 2.07722, -1.81107 -1.02424, 2.21384, -1.81204 -0.988275, 2.25987, -1.81511 -1.09032, 2.16787, -1.82547 -1.14721, 2.13361, -1.83302 -1.07027, 2.04417, -1.75372 -0.988756, 2.28919, -1.8128 -1.1513, 2.07499, -1.80919 -1.07325, 2.23818, -1.82841 -0.995093, 2.196, -1.7691 -1.10796, 2.19439, -1.82529 -0.998739, 2.34944, -1.82671 -1.04921, 2.34444, -1.84924 -1.07602, 2.3256, -1.8357 -1.02909, 2.33816, -1.81833 -1.17389, 2.28273, -1.86196 -1.08982, 2.26642, -1.7982 -1.96099, 2.44397, -0.575404 -1.76071, 3.04028, -0.474843 -1.86122, 3.0813, 0.044271 -1.45725, 2.51374, -2.23915 -1.13652, 2.23417, -1.96168 -1.22515, 2.12172, -1.8901 -1.08889, 2.27249, -1.85331 -1.18791, 2.2252, -1.8474 -1.061, 2.34261, -1.81904 -1.86317, 3.25693, -0.667941 -1.82071, 2.69533, -0.47433 -1.66649, 2.90941, -0.452202 -1.82786, 2.40182, -0.425308 -1.08084, -0.102039, -0.291082 -0.815165, 0.934135, -0.168231 -0.848754, 0.987916, -0.0368526 -0.826491, 0.920867, -0.050332 -0.785656, 0.954986, -0.0560913 -1.87292, 1.1455, -0.361981 -1.10913, 0.835034, -0.562631 -1.83217, 2.59082, -0.47116 -1.97644, 1.1901, -0.404366 -1.97913, 1.83019, -0.19978 -0.861142, 1.00359, -0.0253806 -1.96157, 2.58746, -0.592814 -2.07221, 1.87727, -0.161125 -1.54747, 2.05002, -0.009358 -2.03134, 1.98619, -0.919414 -2.13591, 2.42245, -0.457207 -1.99208, 2.13328, -0.979063 -1.35342, 1.29007, -0.268378 -2.39645, 4.19507, -3.70109 -8.95678, 15.1404, -12.4605 -1.39598, 2.97923, -2.44126 -1.53673, 2.737, -2.4114 -1.66085, 3.32281, -2.70381 -0.910826, 2.26331, -1.81373 -1.09334, 2.10822, -1.83276 -0.926097, 2.32808, -1.83474 -1.47619, 3.07009, -2.43324 -1.16846, 1.99436, -1.82105 -1.07026, 2.18089, -1.83018 -0.956969, 2.31711, -1.82845 -1.0397, 2.33395, -1.83646 -1.07494, 2.32885, -1.8352 -1.0791, 2.37395, -1.84162 -1.77278, 2.63263, -0.474474 -1.41132, 2.87945, -2.4541 -1.60702, 3.16904, -2.63256 -0.91508, 2.21393, -1.81556 -0.943493, 2.30767, -1.85462 -1.46816, 2.89333, -2.37715 -1.05341, 2.13642, -1.8277 -0.931257, 2.35909, -1.83504 -1.05442, 2.13994, -1.80566 -0.998375, 2.17843, -1.78373 -1.97542, 2.51354, -0.83243 -1.88252, 2.31506, -0.468631 -2.0614, 1.81998, -0.2309 -1.38929, 2.49114, -2.22081 -1.31952, 2.93131, -2.36792 -1.07571, 2.12286, -1.81048 -2.46496, 4.83662, -3.67646 -1.16496, 2.02559, -1.81293 -1.12206, 2.00787, -1.7794 -1.23561, 2.27863, -1.92705 -1.03951, 2.22732, -1.79155 -1.05294, 2.39542, -1.85484 -1.72282, 2.28059, -0.236362 -1.6015, 2.82887, -0.0761755 -1.58112, 2.81567, 0.00103268 -1.56943, 3.17234, -2.48582 -2.48399, 4.91281, -3.66606 -2.22133, 1.14386, -0.383377 -1.58271, 2.81324, -0.0787035 -1.44643, 2.77188, -0.0407372 -1.56632, 2.78694, -0.00456171 -2.12214, 1.12159, -0.450308 -2.18295, 2.34087, -0.443249 -1.79342, 2.56516, -0.0747159 -1.8037, 2.48771, 0.0620367 -2.3622, 1.2117, -0.191632 -1.68328, 2.43161, 0.0061753 -2.04923, 2.27211, -0.936908 -4.5107, 8.96465, -6.84684 -2.11778, 3.92004, -3.0607 -1.7655, 3.03818, -0.507373 -0.760528, 0.937831, -0.13326 -1.8189, 2.67553, -0.474112 -1.12019, 0.88634, -0.405732 -1.60131, 2.8291, 0.00709987 -1.73231, 3.15591, -2.58318 -1.87789, 3.27676, -0.669903 -2.02372, 2.51998, -0.605017 -1.81208, 2.67306, -0.472717 -1.77875, 3.08117, -0.453172 -1.55012, 1.03122, -0.367107 -1.61732, 0.975956, -0.287095 -1.64145, 2.35514, -0.123122 -1.71566, 2.4484, 0.0171588 -1.73269, 2.58243, 0.00660647 -1.24856, 2.18635, 0.0364742 -1.79057, 2.08158, -0.372712 -6.55175, 3.02857, 0.281656 -2.29203, 0.659026, -1.4577 -1.98677, 2.05981, -0.624616 -1.07314, -2.14012, -1.85609 -1.12145, -2.16512, -1.85343 -2.10666, 2.58656, -0.819799 -1.47889, 2.83651, 0.0294556 -1.01659, 2.09692, -1.90528 -1.87366, 2.36405, -0.577024 -1.16161, 2.13179, -1.90437 -1.33907, 2.51318, -2.14269 -1.1869, -2.12961, -1.8342 -1.22622, 1.71317, -0.552273 -1.83013, 3.24163, -0.599337 -2.39856, 1.37661, -0.321468 -1.75391, 2.31355, -0.23442 -0.78529, 0.948072, -0.0645556 -2.11148, 2.15471, -0.816281 -1.01871, -0.240342, -0.393371 -1.63015, 2.33689, -0.127495 -1.995, 2.06467, -0.758462 -2.61597, 1.48307, -0.354096 -2.02799, 2.24721, -0.427471 -2.39778, 1.24702, -0.177579 -2.06893, 1.86189, -0.163262 -1.63646, 2.90147, 0.0181231 -1.04903, -0.270957, -0.518968 -1.03436, -0.239258, -0.51888 -1.89491, 2.39096, -0.57733 -1.38112, 2.66068, -0.03656 -1.04738, 2.34295, -0.035858 -1.02823, -0.210685, -0.537695 -1.04163, -0.175642, -0.540683 -1.04188, -0.175753, -0.498819 -1.04583, -0.28764, -0.392075 -1.03675, -0.278139, -0.463602 -2.29026, 0.0744992, -1.47267 -2.26126, 0.0823205, -1.45388 -1.13352, -1.74682, -1.80267 -2.04028, 2.50206, -0.818014 -2.06611, 2.55788, -0.609274 -1.76553, 2.46241, -0.467443 -1.03864, -0.213448, -0.46449 -2.15277, 1.12892, -0.449532 -1.03833, -0.133235, -0.38772 -2.15221, -1.18022, -0.102106 -2.1354, 2.67369, 0.131374 -1.02705, -0.213082, -0.467761 -1.04119, -0.16775, -0.386377 -1.78183, 2.31444, -0.785361 -2.27675, 0.0870316, -1.46423 -2.28029, 1.37494, -0.374726 -1.04299, -0.226873, -0.491884 -1.64562, 2.13096, -0.382819 -2.3588, 0.976182, -0.0678497 -1.03874, -0.230067, -0.491862 -1.03515, -0.176039, -0.356869 -1.12946, -1.73252, -1.76287 -1.18115, -1.82565, -1.80883 -1.10379, -1.82172, -1.7439 -1.18625, -2.07239, -1.81117 -1.98665, 2.48324, -0.581189 -2.0231, 2.50562, -0.602801 -2.17194, 2.73159, -0.594899 -1.67344, 2.79117, -0.00279571 -1.42834, -2.92993, -1.0168 -1.92966, 2.26803, -0.647739 -2.06291, 2.72265, -0.59568 -1.79508, 2.25374, -0.47538 -1.65398, 2.35502, -0.118716 -1.68953, 2.83338, -0.00216691 -1.46355, -2.60128, -0.640448 -1.02326, -0.251466, -0.393371 -1.03394, -0.143358, -0.355828 -1.33699, -2.86011, -1.01473 -1.42498, -2.76913, -0.98181 -1.30809, -2.74693, -0.754524 -2.02832, 2.66464, -0.597017 -2.28842, 1.25169, -0.199622 -2.12075, 1.13996, -0.207957 -0.985279, -1.53296, 0.0408074 -1.09017, -1.72492, -1.72445 -1.12496, -2.23773, -1.81333 -1.27215, -2.45034, -1.97255 -1.16858, -2.23774, -1.82589 -1.19719, -2.30684, -1.85413 -1.20559, -2.25888, -1.83359 -1.19755, -2.26619, -1.82491 -1.9937, 2.53525, -0.832953 -1.05649, -0.257861, -0.519382 -1.99795, 2.62601, -0.578798 -1.74068, 2.9909, -0.463971 -0.682716, -2.11545, -1.11748 -2.28605, 0.0993311, -1.34061 -1.5722, -2.58589, -0.646843 -2.2752, -1.57407, -0.593742 -1.05984, -0.162085, -0.541951 -1.01909, -0.289541, -0.395343 -1.05627, -0.165377, -0.394382 -1.0636, -0.27314, -0.35063 -1.04059, -0.132757, -0.352521 -0.738372, -1.11119, -0.0946502 -0.738459, -1.14311, -0.0736678 -0.65618, -2.08544, -1.06987 -1.70378, 2.23556, -0.224043 -1.66743, 2.37291, -0.115012 -1.96074, 2.2836, -0.659179 -1.06105, -0.229867, -0.511287 -1.93777, 2.7721, -0.464147 -1.02675, -0.174323, -0.359436 -1.27059, 1.74949, -0.0664837 -1.0296, -0.274155, -0.524598 -1.47909, -1.52277, 0.0704247 -2.49283, 0.342922, -1.37318 -0.99915, -1.54394, 0.0558682 -1.1328, -2.15936, -1.84588 -1.16206, -2.09616, -1.79114 -1.2394, -2.26102, -1.89439 -1.15055, -2.10552, -1.78588 -1.17834, -2.16026, -1.81203 -1.16747, -2.16985, -1.80023 -0.937002, -1.12569, -0.888499 -1.9856, 2.58843, -0.808787 -1.04759, -0.261231, -0.523529 -1.87977, 2.20949, -0.627915 -2.0343, 2.53962, -0.612394 -1.03755, -0.255143, -0.479267 -1.56053, 2.74811, -0.519592 -2.1794, 1.31853, -0.476546 -1.63259, 2.02532, -0.477034 -1.74249, 2.47441, -0.475747 -1.91928, 3.18468, -0.476806 -1.76372, 2.51308, -0.413589 -1.77364, 2.28469, -0.401593 -1.76519, 2.27866, -0.390667 -1.71152, 2.44639, -0.118715 -2.35504, 0.990421, -0.0673952 -1.15225, -0.0355083, -0.247054 -1.59201, 2.66679, -0.0188193 -1.4852, 2.82839, -0.0126074 -1.52691, 2.71863, -0.0066377 -0.758838, 0.915942, -0.0716394 -0.822286, 0.936792, -0.0559167 -0.738268, -1.14491, -0.0698618 -1.15453, -2.15985, -1.84993 -0.895969, -1.00768, -1.03708 -0.944932, -1.12081, -0.968322 -2.27323, 0.0974861, -1.5049 -2.25872, 0.286766, -1.50514 -2.1376, 1.20499, -1.04916 -1.6009, -2.72232, -0.66207 -1.87647, 3.21224, -0.732395 -1.8449, 3.19603, -0.665499 -1.97657, 2.55285, -0.605086 -1.0332, -0.249496, -0.505295 -1.82512, 3.25174, -0.528773 -1.83159, 2.54149, -0.469787 -1.66286, 2.54764, -0.456512 -1.76694, 2.41364, -0.421758 -2.13125, 1.23766, -0.403008 -0.968397, 0.840687, -0.405503 -1.03713, -0.256124, -0.369202 -1.77713, 2.35617, -0.228326 -1.83405, 2.85264, -0.0806051 -1.78055, 3.02343, -0.0812527 -1.04444, -0.0515767, -0.263389 -1.81311, 2.57715, -0.0186193 -1.46305, 2.74675, -0.0173653 -1.35951, 2.40688, 0.0927227 -0.682183, -1.09959, -0.241729 -0.751485, -1.11136, -0.225872 -0.759055, 0.950238, -0.0901326 -0.779975, 0.857731, -0.0716179 -0.764889, 0.912589, -0.0689564 -0.908138, -1.11688, -0.159168 -0.718647, -1.12725, -0.142594 -1.06322, -1.10288, -0.0190412 -0.748576, -1.15733, -0.101301 -0.126941, -1.12521, -0.0780631 -1.10941, -1.95223, -1.74107 -0.921059, -0.981455, -1.02534 -1.18315, -1.1799, -1.07517 -0.928839, -1.10814, -0.951282 -2.29285, 0.107835, -1.51434 -2.35325, 0.339593, -1.54426 -1.94316, -0.756721, -1.16655 -0.893543, -1.08829, -0.764379 -1.68814, -1.30426, -0.783646 -1.73613, -3.05662, -0.790804 -1.18086, -2.62221, -0.679358 -1.45968, -2.7275, -0.693331 -2.42148, -1.62785, -0.740136 -2.02063, 2.6094, -0.833192 -0.853824, -1.09275, -0.550808 -1.15542, -1.16593, -0.561402 -1.10025, -1.17869, -0.545098 -2.14508, -1.55667, -0.573652 -1.90937, 2.23666, -0.638314 -1.79014, 3.11854, -0.660821 -1.97707, 2.45976, -0.589471 -1.04758, -0.251612, -0.480294 -1.04999, -0.165986, -0.462114 -2.14329, 1.39228, -0.437751 -1.03112, -0.213823, -0.424499 -1.05781, -0.161731, -0.425339 -1.39285, 2.67735, -0.388329 -1.83154, 3.61301, -0.334892 -1.04527, -0.204614, -0.390154 -2.03989, 1.23194, -0.329366 -2.08243, 1.57856, -0.278701 -1.99884, -2.3123, -0.235521 -1.17959, -1.07583, -0.293944 -1.60029, -1.21171, -0.232072 -1.77172, 3.00357, 0.0224128 -1.44052, 2.72955, -0.0225987 -1.18386, 1.58868, -0.104442 -1.45939, 2.76771, 0.0229512 -1.51496, 2.74034, 0.145341 -0.66893, -1.08326, -0.249848 -0.728603, 0.915638, -0.0957105 -1.03135, -1.14928, -0.121076 -1.18838, -0.295637, -0.0159129 -1.24503, -0.257846, 0.0086495 -0.876724, -1.11533, -0.0576147 -0.763282, -1.15176, -0.0626633 -0.188233, -1.11371, -0.0604048 -0.112673, -1.12246, -0.0904895 -1.38181, -2.36886, -2.02633 -0.691272, -1.06358, -0.847534 -0.901255, -1.10203, -0.933709 -2.27694, 0.0440825, -1.46209 -1.43314, -1.25004, -0.740936 -1.66681, -1.22921, -0.773291 -2.25503, -1.59086, -0.716285 -1.03486, -1.09323, -0.547527 -1.0036, -1.08008, -0.522346 -1.67219, 2.53484, -0.420705 -1.33897, 1.52017, -0.382525 -1.10598, -0.141988, -0.348325 -0.797503, -0.994885, -0.367653 -1.43705, 2.72632, -0.0214407 -1.21442, -1.26891, -0.13211 -0.698558, 0.665421, -0.101277 -0.915561, -1.12347, -0.150063 -1.50426, -1.18683, 0.0361229 -1.19699, -1.10244, -0.044141 -0.743789, -1.16417, -0.126717 -0.731188, -1.14939, -0.058855 -0.960082, -0.989332, -1.04717 -0.990288, -2.04658, -1.0776 -2.33418, -0.715726, -1.47658 -1.9943, -0.491373, -1.34959 -2.07029, 1.92153, -1.61141 -1.43021, -1.19971, -1.03001 -2.1078, 0.783312, -1.22803 -1.06704, -1.12977, -0.813897 -1.42936, -1.23574, -0.69765 -2.08973, -1.53494, -0.700897 -0.97478, -0.332482, -0.594563 -2.15389, -1.50904, -0.601436 -1.92228, -1.4776, -0.551015 -2.0873, 1.31074, -0.615241 -1.87922, 3.05964, -0.431378 -2.09333, 2.90194, -0.39396 -1.28081, 1.33883, -0.385712 -2.09033, 2.66899, -0.344733 -2.2117, 1.35338, -0.220768 -0.799698, -1.00827, -0.347787 -0.597975, -0.997166, -0.348169 -1.44439, 2.0016, -0.146818 -1.26372, -1.05748, -0.177393 -36.6085, -4.81483, 6.1327 -0.62319, -1.06305, -0.254806 -1.16634, 0.033149, -0.0572476 -1.10864, -0.071076, -0.0718106 -0.962898, -1.23594, -0.115806 -1.09181, -0.330126, -0.0374026 -0.762632, -1.0806, -0.114349 -0.853467, -1.00259, -1.05201 -2.27486, -0.718587, -1.26826 -1.31964, -1.23083, -0.687803 -2.25165, -1.45682, -0.732441 -0.848063, -1.10395, -0.543194 -0.140886, -0.90075, -0.47353 -0.495844, -0.938904, -0.390399 -2.14095, 1.13851, -0.218366 -1.80386, -2.37412, -0.153608 -1.20949, -1.04743, -0.177637 -0.980264, -0.152136, -0.115749 -0.710944, -1.0426, -0.0709013 -0.51175, -1.21903, -0.010508 -0.718656, -1.04159, -0.854394 -1.62264, -0.548359, -1.06885 -1.82044, -0.072689, -1.12977 -1.01942, -0.985669, -0.37461 -4.0085, -2.10523, 0.458937 -2.0953, 1.12254, 0.130239 -0.872884, -1.27483, -0.121577 -0.570491, -1.31351, -0.112649 -0.724402, -1.08683, 0.026151 -3.59624e+06, -512610, 163432 -3.60324, 2.35171, 0.430548 -1.59311, 0.318304, -0.00499517 -0.70075, -1.08795, 0.0284549 -0.597931, -1.80773, -1.37048 -1.21955, -1.99729, -1.8455 -3.43774, -5.2961, -4.60889 -1.23466, -2.03167, -1.86212 -1.18545, -2.05305, -1.80892 -1.17068, -2.13512, -1.79974 -1.19926, -2.22104, -1.82755 -1.96908, 2.64267, -0.832882 -1.04791, -0.260497, -0.492435 -1.02796, -0.136588, -0.39765 -2.11272, 1.24096, -0.333803 -1.66597, 2.83598, -0.10254 -2.04466, -1.17646, -0.117077 -0.770173, 0.935959, -0.0592302 -0.832901, -1.09724, -0.180833 -1.19291, -2.15204, -1.82715 -0.918608, -1.12272, -0.885051 -2.34967, -0.00900915, -1.41072 -2.14529, 1.15075, -1.05199 -2.27412, -1.57361, -0.62096 -1.04927, -0.172565, -0.543208 -1.75265, 3.02267, -0.47139 -2.24201, 1.44702, -0.391744 -2.13194, 1.40456, -0.33506 -1.02437, -0.183362, -0.396321 -2.12261, 1.295, -0.332169 -1.07508, -0.133374, -0.382869 -2.07614, 1.56218, -0.279755 -1.05238, -0.138294, -0.367703 -1.03243, -0.182027, -0.352346 -1.79676, 2.57692, -0.10037 -1.83644, 2.64111, -0.0663186 -2.10807, 1.07745, 0.0504705 -0.843049, -1.13903, -0.202882 -0.937232, -1.35478, -0.0249305 -0.732107, -1.15076, -0.117444 -1.02592, -1.10659, -0.922099 -2.30232, -0.0113115, -1.38821 -2.26106, -0.072231, -1.35457 -2.30314, -0.00879277, -1.36766 -1.94821, 3.38837, -0.749822 -2.13589, 1.20507, -0.588312 -1.02015, -0.144508, -0.500459 -2.09123, 4.13351, -0.503066 -1.67194, 3.01923, -0.456294 -1.10115, -0.211523, -0.428643 -1.05405, -0.201023, -0.3873 -1.02831, -0.287198, -0.349357 -1.64189, 2.51827, -0.120491 -1.58478, 2.67067, -0.118063 -1.10986, -1.1072, -0.292252 -0.838348, -1.10547, -0.184585 -0.724062, -1.11729, -0.17132 -1.09821, -2.33719, -1.22516 -2.3031, 0.11492, -1.43584 -0.994969, -1.10125, -0.876398 -2.14315, -0.0788316, -1.32528 -1.04035, -0.281151, -0.539242 -1.74662, 3.10239, -0.733023 -1.02473, -0.287061, -0.462879 -1.42102, 2.32687, -0.145364 -0.845414, -1.10854, -0.0730918 -0.993116, -1.11348, -0.993917 -2.22265, -3.47642, -1.80104 -1.27344, -1.16316, -1.00633 -1.92697, 0.160204, -1.34679 -2.23235, 0.863012, -1.46778 -1.66995, -0.158511, -1.0783 -1.5469, -2.55854, -0.638214 -1.09178, -0.107979, -0.541951 -2.05741, 1.05392, -0.445383 -1.05515, -0.165295, -0.391033 -0.854948, -1.04987, -0.371066 -1.05685, -0.983471, -0.361535 -2.21583, -2.25663, -0.137456 -1.88543, -1.14296, -0.143747 -1.0186, -1.42153, 0.00723361 -0.931315, -2.00002, -1.05443 -2.3545, 0.104862, -1.49359 -2.4877, 0.0367734, -1.44751 -2.54296, -0.406905, -1.39041 -1.83724, -1.25416, -0.792746 -2.08396, 1.08418, -0.678408 -1.76057, -2.3335, -0.162524 -0.95189, -1.15502, -0.170184 -1.07465, -1.09108, -0.661091 -2.81646, -1.72463, -0.595632 -0.87147, -0.327835, -0.483763 -0.757406, -1.07586, -0.119906 -1.70066, -0.568485, -1.11277 -1.10295, -0.124209, -0.424249 -2.34451, 1.32912, -0.103749 -0.907056, -1.27192, -0.0791263 -1.05193, -1.10887, 0.118457 -1.22648, -2.18267, -1.85944 -1.83166, 3.29858, -0.536096 -1.792, 2.55496, -0.476898 -1.96688, 3.15563, -0.458841 -1.75005, 2.51009, -0.113228 -1.0311, -1.1077, -1.03228 -1.0829, -1.13867, -0.782362 -1.02906, -0.272699, -0.521201 -1.87447, 3.2577, -0.669026 -1.04793, -0.219016, -0.498014 -1.04932, -0.171294, -0.471089 -1.94666, 3.39626, -0.491589 -1.06077, -0.204956, -0.398253 -1.03482, -0.25774, -0.368802 -2.30199, 1.30893, -1.0943 -2.14616, 1.21629, -1.05106 -1.06548, -0.140591, -0.531957 -1.04173, -0.204505, -0.459948 -1.77458, 3.07297, -0.479768 -1.70024, 2.22256, -0.383222 -1.04479, -0.239174, -0.395937 -1.0445, -0.134081, -0.400102 -1.04176, -0.150341, -0.384766 -1.60859, 2.55805, -0.160661 -0.765076, 0.98373, -0.0796598 -1.13884, -0.317049, -0.0370799 -2.76885, -0.721766, -1.65272 -0.945309, -1.02563, -0.643461 -1.57941, -2.59281, -0.644779 -0.991003, -0.166239, -0.535155 -2.33063, -1.61732, -0.573345 -1.05153, -0.202924, -0.46105 -1.03861, -0.28537, -0.395379 -1.08301, -0.219982, -0.391209 -1.78744, 2.37456, -0.222597 -1.93306, -2.2622, -0.242473 -0.912097, -1.04526, -0.373304 -1.0539, -0.167097, -0.352591 -0.916066, -1.03323, -0.360082 -1.70848, -2.23513, -0.0385094 -0.709599, -1.1451, -0.123712 -2.13147, 0.259475, -1.42398 -2.23772, 0.342607, -1.44054 -1.11129, -0.242101, -0.546831 -2.09206, 1.07916, -0.441651 -1.70831, 2.50105, -0.356134 -1.04415, -0.243481, -0.388666 -1.16671, -1.00518, -0.358486 -1.05521, -0.163428, -0.354704 -0.979699, -1.39723, -0.00993529 -1.68437, -0.539069, -1.19778 -2.82458, 0.290496, -1.69974 -2.31748, -2.18539, -0.0630096 -2.28375, -2.19924, -1.22217 -2.2419, -1.56266, -0.517295 -1.73383, 2.61802, -0.458462 -1.05155, -0.233785, -0.397939 -1.05346, -0.262717, -0.359822 -1.52683, 2.79704, -0.000143568 -1.69761, 2.28529, 0.0122863 -1.69528, 2.37087, 0.0203662 -0.879016, 0.948229, -0.171884 -2.15079, 1.15597, -1.10835 -1.07829, -0.21957, -0.594589 -1.07715, -0.225432, -0.525234 -1.0441, -0.142973, -0.514842 -1.07156, -0.158886, -0.497564 -1.0435, -0.286411, -0.457835 -1.81827, 3.15383, -0.477041 -1.04331, -0.251423, -0.434176 -1.04941, -0.267668, -0.360888 -1.76362, 2.39918, 0.0287833 -0.984666, -0.966792, -0.481759 -1.71838, 2.73318, -0.431259 -2.13004, 1.30565, -0.331589 -2.17184, 1.33527, -0.23959 -1.91911, 1.76099, -0.207601 -1.63615, 2.20382, -0.00116004 -2.13722, 1.11255, 0.0571601 -0.966848, -1.37479, -0.020089 -1.06729, -1.11552, -0.936874 -1.05114, -0.278911, -0.541512 -1.03611, -0.213966, -0.541096 -1.05116, -0.167085, -0.5431 -1.03433, -0.288932, -0.461418 -1.04901, -0.206276, -0.433683 -1.05795, -0.146746, -0.377054 -1.95998, -2.28573, -0.240863 -0.725524, -1.14952, -0.119447 -2.17035, 1.25908, -0.576214 -2.48488, -1.10878, -0.422766 -2.13212, 1.26919, -0.351866 -1.15462, -0.303158, -0.025466 -0.731304, -1.15455, -0.0404094 -2.28253, 0.670965, -1.45483 -2.19688, -1.18436, -0.156031 -0.69068, -1.11581, -0.0465803 -1.19838, -1.18782, -0.854461 -0.979111, -1.1382, -0.90292 -1.93356, 3.02405, -0.496848 -1.74452, 3.02598, -0.474976 -1.96589, 1.85151, -0.175505 -1.07898, -0.264937, -0.546926 -1.06134, -0.152176, -0.533791 -1.04775, -0.174766, -0.396865 -1.03892, -0.216313, -0.386289 -1.04723, -0.262659, -0.379913 -1.04535, -0.285239, -0.348259 -1.98684, 1.78252, -0.126372 -1.80158, 2.92753, -0.0487411 -0.783753, -1.1175, -0.220137 -1.06382, -1.10628, -0.0165328 -1.05857, -1.0636, -0.0108233 -0.955948, -1.1141, -0.871759 -0.995305, -1.06737, -0.76414 -2.10865, 1.1774, -1.03898 -1.05263, -0.217345, -0.490697 -1.04139, -0.171736, -0.47136 -1.04359, -0.247043, -0.432552 -2.12415, 1.34532, -0.39394 -1.00629, -0.265647, -0.424896 -1.0144, -0.227968, -0.424536 -0.994248, -0.311631, -0.399788 -1.84067, -1.22815, -0.243375 -1.05079, -1.04995, -0.0119106 -0.705149, -1.1203, -0.149145 -0.937367, -1.12415, -0.88462 -2.03345, 1.0341, -0.242194 -1.07523, -1.09801, -0.0249619 -0.742229, -1.13592, -0.0836149 -0.530977, -1.23438, -0.0335732 -1.00298, -0.271654, -0.396677 -1.05903, -0.154837, -0.39204 -1.01714, -0.153809, -0.396095 -1.06928, -0.190215, -0.387719 -1.00541, -0.300307, -0.358337 -1.92677, 1.75014, -0.177489 -1.1139, -0.263653, -0.312731 -0.736231, -1.16165, -0.129554 -1.09608, -1.14292, -0.823691 -1.86337, -1.05472, -0.570807 -0.960315, -1.3421, -0.0295948 -2.07509, -2.20378, -0.156731 -0.969906, -1.12927, 0.0701657 -2.36335, -2.27044, -1.29108 -1.49127, 0.0298201, -0.484026 -0.956498, -1.12904, -0.98496 -1.05426, -0.254603, -0.384145 -1.71861, 2.28358, -0.238097 -1.75039, 2.75113, -0.125111 -0.9561, -1.1108, -0.974924 -1.04463, -0.163646, -0.532507 -1.02854, -0.275725, -0.492003 -1.95436, 2.12569, -0.364645 -1.03443, -0.271015, -0.491459 -1.05873, -0.129884, -0.366434 -2.04989, 1.8821, -0.105936 -1.07858, -1.10306, -0.0233599 -1.04327, -1.09704, -0.0271898 -1.05032, -0.167572, -0.54365 -1.05409, -0.161991, -0.46957 -1.05368, -0.163015, -0.46048 -1.05816, -0.197531, -0.38529 -2.02784, 1.83415, -0.242884 -1.17662, -0.313507, 0.00146083 -1.06634, -1.13463, -0.029111 -0.740587, -1.12199, -0.171686 -0.823733, -1.11069, -0.0325053 -1.01407, -0.958776, -0.840696 -1.96395, 1.76766, -0.121438 -0.824624, -1.11046, -0.0329663 -0.985279, -0.951631, -0.833605 -1.13725, -1.1543, -0.805799 -2.73267, 0.518467, -1.47566 -1.91492, 0.929955, -0.186687 -0.945961, -1.11478, -0.967304 -0.947552, -1.12507, -0.969129 -0.950801, -1.12606, -0.958755 -1.91371, 2.32137, -0.36418 -1.04134, -0.257052, -0.374486 -2.03737, 1.99486, -0.160061 -1.0445, -1.12086, -0.771137 -1.04765, -1.10564, -0.769478 -2.02152, 2.30752, -0.615238 -1.04347, -0.17596, -0.398765 -2.00132, 1.91886, -0.164252 -1.04613, -0.143291, -0.367096 -1.04212, -0.25601, -0.366974 -1.04427, -0.160918, -0.361741 -1.05305, -1.11777, -0.795427 -1.05637, -1.12615, -0.775111 -2.31211, 0.00440096, -1.397 -2.04385, 2.29275, -0.889004 -1.03839, -0.228782, -0.490406 -1.99644, 2.246, -0.418865 -1.04032, -0.241274, -0.434836 -1.96506, 2.40745, -0.354093 -1.04927, -0.171347, -0.398667 -1.04253, -0.239319, -0.398385 -1.03828, -0.163844, -0.3637 -1.03912, -0.135188, -0.351193 -1.04757, -1.12571, -0.0401246 -1.0179, -1.08374, -0.0201858 -1.05518, -1.11017, -0.769018 -2.14333, 2.45054, -0.92188 -1.0403, -0.176852, -0.495724 -1.02703, -0.266948, -0.478414 -2.01868, 2.28528, -0.358485 -2.00652, 1.88367, -0.154502 -1.12468, -0.34282, -0.0266831 -0.815259, -1.0936, -0.189695 -1.07118, -1.10303, -0.0358973 -1.05197, -1.09235, -0.0384504 -0.736822, -1.11715, -0.167659 -1.04285, -0.134237, -0.395523 -1.97935, 1.84731, -0.193655 -1.69408, -2.2179, -0.0516169 -1.08261, -0.344348, -0.0561287 -2.05998, 2.66286, -0.847626 -1.94039, 2.29601, -0.628042 -2.05743, 2.61477, -0.58264 -2.09246, 2.69553, -0.807671 -1.94493, 2.10236, -0.413611 -2.27669, -0.999097, -0.21126 -2.09539, -2.20084, -0.44849 -1.04298, -0.169725, -0.459169 -1.04569, -0.173626, -0.397505 -1.04318, -0.203643, -0.398545 -1.04318, -0.149116, -0.3863 -1.04262, -0.252517, -0.368017 -2.17093, 2.09025, -0.0850907 -2.27733, -1.00732, -0.149115 -0.821124, -1.09839, -0.184713 -0.739025, -1.12006, -0.176194 -2.30566, 0.056819, -1.47531 -2.31143, -0.108728, -1.2979 -1.04393, -0.207831, -0.543757 -1.04577, -0.262886, -0.522045 -1.04854, -0.2492, -0.480196 -1.04228, -0.284882, -0.459059 -1.04491, -0.168593, -0.459637 -1.04618, -0.168549, -0.42353 -1.04595, -0.205067, -0.398535 -2.16902, 2.17514, -0.111984 -2.18944, -1.31732, -0.591005 -1.0349, -0.287742, -0.460364 -2.13668, -2.19739, -0.14836 -1.03956, -0.226086, -0.463341 -1.11728, -0.337826, -0.0294716 -0.825069, -1.0922, -0.182496 -2.28546, 0.299576, -1.49168 -1.87051, -2.27813, -0.324359 -2.27805, -0.674251, -0.316798 -1.99365, 1.92627, -0.128162 -2.28917, 0.277543, -1.38468 -2.32051, -0.724174, -0.261496 -2.27608, -0.608334, -0.262989 -1.0397, -0.182483, -0.349939 -2.28014, -0.858178, -0.118412 -1.05432, -1.04166, -0.0219736 -1.04241, -0.265412, -0.523034 -2.17947, -1.45193, -0.609429 -2.28827, -0.87792, -0.582633 -2.01934, 1.98301, -0.588984 -1.04343, -0.25332, -0.479622 -1.0384, -0.251608, -0.435998 -1.04651, -0.200786, -0.436486 -1.03771, -0.178196, -0.422876 -1.90771, 1.72331, -0.207863 -2.27884, 0.0520121, -0.223564 -2.28181, -0.998845, -0.209154 -2.27527, -0.366132, -0.162309 -2.27134, 0.0569893, 0.317463 -1.21865, -1.4299, -0.0167752 -1.05417, -1.09998, -0.0274087 -1.06313, -1.05922, -0.0198908 -0.814715, -1.08231, -0.186683 -2.29927, -0.742382, -1.31518 -2.297, -0.113451, -1.29189 -2.29776, -0.682408, -1.24265 -2.22064, -1.32626, -0.899865 -1.04834, -0.166333, -0.544456 -1.04873, -0.200992, -0.459344 -1.87455, -2.27722, -0.377206 -2.26383, -0.624611, -0.373754 -1.03976, -0.180295, -0.399201 -1.09594, -0.312075, -0.0455146 -1.04675, -1.09544, -0.0313352 -2.21414, 1.29856, -1.19627 -2.15297, -1.36884, -0.859476 -2.15341, -1.31309, -0.728789 -5.97349, -3.50033, -1.32421 -2.0876, -2.21781, -0.422801 -1.21289, -1.49783, -0.0500276 -1.22279, -1.44563, -0.00906393 -2.28927, -0.748718, -1.54055 -16.0201, -8.61068, -1.28793 -2.63539, 0.264825, -1.43093 -2.31823, -1.37533, -0.548406 -2.27756, -1.22682, -0.106225 -2.28869, -0.0312094, -1.38579 -2.27345, 0.678532, -1.50144 -2.17281, 1.18193, -1.16905 -2.26242, -0.498872, -0.795181 -2.22345, -0.167468, -0.766253 -1.04521, -0.198161, -0.398999 -1.04609, -0.232746, -0.398288 -2.28035, -0.809586, -0.147645 -2.2883, 0.110628, 0.325997 -2.2783, 0.0291782, 0.321129 -2.23629, 0.335697, 0.586894 -2.30646, -0.0289031, -1.37714 -2.29504, 0.382025, -1.28775 -2.2937, 0.268372, -1.24206 -2.31279, 0.625994, -1.3125 -2.23973, 0.678261, -1.00887 -2.26764, -0.176458, -0.745192 -2.29467, -1.13061, -0.719359 -2.28468, -0.716655, -0.301633 -2.29809, 0.0316746, 0.332219 -2.30972, -0.029401, 0.37152 -2.18509, 0.311541, 0.563819 -2.29663, -0.570378, -1.47923 -2.29007, -0.527544, -1.43345 -2.28508, 0.276382, -1.37992 -2.30778, 0.732281, -1.25654 -2.31657, -0.162759, -0.870752 -2.14735, -1.40975, -0.758775 -2.16699, -1.38974, -0.666471 -2.276, 0.298536, -0.714443 -2.26687, 0.365939, -0.655435 -2.27173, -0.857284, -0.119766 -2.12702, 1.10388, 0.0611408 -2.26597, -0.071267, 0.316053 -2.16944, -0.747348, -1.55566 -2.29648, -0.523799, -1.43428 -2.29345, 0.0412912, -1.42788 -2.25665, -1.24513, -1.20353 -2.3121, -0.135179, -1.25917 -2.17184, -1.37297, -0.815296 -2.16126, -1.32086, -0.737044 -2.37684, 0.697038, -0.834838 -3.06298, -0.380125, -0.335317 -2.28307, -0.629092, -0.374427 -2.18626, -0.0811982, -0.300801 -2.2657, 0.0761747, -0.283011 -1.04461, -0.147631, -0.386434 -2.30003, -0.75177, -1.55774 -2.34229, -0.715188, -1.28308 -2.18791, -1.42213, -0.762639 -2.3072, 0.640545, -0.822814 -2.13241, -1.46191, -0.701869 -2.38893, -1.06405, -0.369671 -2.28018, -0.389483, 0.606078 -2.16559, -1.32942, -0.847673 -2.22413, -0.714989, -0.842085 -2.16706, -1.39735, -0.600484 -2.26938, 0.938325, 0.12158 -2.4239, -0.413122, -1.54141 -2.35635, 0.72518, -1.49592 -2.17056, 0.781033, -1.2252 -2.19496, -1.33804, -0.659009 -2.28338, 0.978125, 0.194901 -2.13208, -1.30451, -0.554676 -2.31463, -0.0517474, -1.38305 -2.31742, -0.0476736, -1.36827 -2.28341, 0.272621, -1.38321 -2.14505, 1.14854, -1.05505 -2.61124, 0.57288, -0.771143 -1.04512, -0.242053, -0.525458 -2.07549, -2.16083, -0.425919 -2.2442, -0.714301, -0.307518 -1.04093, -0.15456, -0.38835 -1.04218, -0.263084, -0.387661 -1.03925, -0.280016, -0.387971 -1.03851, -0.257777, -0.382204 -2.24171, -1.21622, -0.0892048 -1.04494, -0.265797, -0.35913 -2.02264, 0.968951, 0.0144313 -2.28348, 0.162985, 0.377088 -2.07778, 0.676469, 0.545964 -2.07683, 0.655545, 0.556682 -2.06617, 0.810888, 0.599251 -2.22542, 0.487314, 0.596557 -2.18966, 0.4752, 0.587338 -2.14013, 0.247879, 0.50982 -2.24125, -0.140592, -1.26239 -2.20225, 0.764472, -1.43797 -2.271, -0.179613, -0.859998 -2.38959, -0.403715, -0.817679 -2.2809, -0.173125, -0.747547 -2.32221, 0.476959, -0.787017 -1.9429, 0.103758, -0.684648 -1.04267, -0.180204, -0.543962 -2.69478, 0.633259, -0.784889 -1.04623, -0.260488, -0.524936 -2.25154, 0.422568, -0.706761 -1.04303, -0.247612, -0.524492 -1.04045, -0.168682, -0.505503 -1.03594, -0.264864, -0.421924 -2.26185, -0.72458, -0.268838 -2.26971, -0.996515, -0.243195 -1.04273, -0.207389, -0.399166 -2.20403, 0.0104264, -0.234399 -1.04677, -0.207003, -0.384013 -1.03895, -0.259704, -0.381482 -1.04117, -0.169155, -0.36217 -2.33998, 0.0833812, 0.353198 -2.2851, 0.00381601, 0.382703 -2.11999, 0.558928, 0.555828 -2.11143, 0.405104, 0.529023 -2.1428, 0.574466, 0.592506 -2.13432, 0.322373, 0.574155 -2.33214, -1.27078, -1.24591 -2.28911, 0.864134, -1.24649 -2.29761, 0.816153, -1.02797 -2.3058, -0.725967, -0.86592 -2.26992, -0.173979, -0.745018 -1.04677, -0.165024, -0.545319 -2.23028, 0.148206, -0.718077 -2.33362, 0.413054, -0.730197 -2.36575, 0.41004, -0.728097 -2.37454, 0.389344, -0.672528 -1.04911, -0.243205, -0.478595 -1.03547, -0.18639, -0.470976 -1.04336, -0.202564, -0.457443 -2.28751, -0.673044, -0.316803 -2.27483, -0.581382, -0.317739 -1.04417, -0.206572, -0.421394 -2.24081, -0.866134, -0.240304 -1.04034, -0.156914, -0.387632 -2.18975, -0.386881, -0.179428 -2.30961, -0.879551, -0.144505 -1.98584, -0.800788, -0.209185 -1.04409, -0.157867, -0.360919 -2.28185, -0.890868, -0.127138 -2.31483, 0.184281, 0.4022 -2.05584, 1.03112, 0.4904 -2.09131, 0.683013, 0.558869 -2.1934, 0.482723, 0.589067 -2.42115, 0.178126, -1.51613 -2.29341, -0.709556, -1.24429 -2.32915, 0.932452, -1.52814 -2.1784, -1.21588, -1.15998 -2.23215, -0.381767, -0.774128 -2.39535, 0.512526, -0.803601 -2.28555, -1.12849, -0.702067 -1.04958, -0.158104, -0.42087 -2.2432, -0.72452, -0.271289 -2.42214, -0.489656, -0.231312 -1.04493, -0.172455, -0.398837 -1.0426, -0.210583, -0.398661 -1.04343, -0.210538, -0.385391 -2.56918, -1.20585, -0.00216129 -2.20294, 0.825964, 0.591811 -2.13947, 0.703706, 0.586806 -2.22004, 0.455914, 0.590216 -2.27763, -0.851548, 0.629662 -2.31823, 0.0741099, -1.48406 -2.29851, -0.0571138, -1.36452 -2.24581, -1.23341, -1.192 -2.28307, 0.74275, -1.24655 -2.02466, 0.639261, -1.13164 -2.5682, 1.03164, -1.13394 -2.37651, 0.885506, -1.05839 -2.18612, -0.499196, -0.776496 -1.03789, -0.290325, -0.541605 -2.29561, -0.883972, -0.125901 -2.23978, -0.0650937, 0.384529 -2.00537, 0.711197, 0.539936 -2.05349, 0.375152, 0.483367 -2.29366, -0.844722, 0.623783 -2.77561, 0.659745, -1.83804 -2.3107, -0.436335, -1.27919 -2.1694, -1.33225, -0.85103 -2.20657, -1.42327, -0.760166 -12.6842, 3.09288, -2.59243 -2.37135, -0.489984, -0.363961 -2.29734, -0.0624362, -0.258268 -2.29086, 0.335563, -1.28457 -2.17826, -1.31097, -0.582506 -2.30107, -1.22101, 0.0308779 -2.15799, -1.49767, -0.903757 -2.215, -1.32352, -0.529469 -2.30319, 0.114105, -1.33884 -2.15818, 1.28125, -1.10317 -2.14915, 1.15707, -1.00202 -2.23031, 0.409913, -0.766173 -2.30045, 0.461268, -0.752998 -2.07915, 1.27035, -0.649184 -1.73557, 0.723616, -0.586764 -1.6715, 0.580528, -0.470898 -6.5377, 8.03553, -0.485915 -2.19764, 1.44502, -0.393504 -2.14636, 1.27205, -0.326846 -2.20826, 0.957852, 0.104423 -2.23831, 0.905437, 0.255638 -2.28172, -0.0408411, 0.356685 -2.30534, 0.0146593, 0.39409 -2.16436, 0.819581, 0.571965 -2.12881, 0.576791, 0.563591 -2.13978, 0.582279, 0.591778 -2.1969, 0.280465, 0.58077 -2.12685, 0.422815, 0.593256 -2.34135, 0.399645, -1.51992 -2.2468, -0.184872, -0.740241 -2.33198, 0.20881, -0.735358 -2.26472, 0.272289, -0.565278 -47.3676, 71.4174, 2.01116 -3.33342, 3.09515, -0.307433 -2.15182, 1.11898, -0.375954 -2.34434, 1.43525, -0.359676 -3.14443, 2.78316, -0.26068 -2.27853, 0.862938, -0.264059 -2.2781, -0.858365, -0.119356 -2.22102, 1.2123, 0.120138 -1.04242, -0.18152, -0.348814 -2.24064, 0.901362, 0.254519 -2.28694, -0.0674431, 0.326031 -2.29617, 0.0102118, 0.364631 -2.13455, 0.711017, 0.603544 -2.08201, 0.383423, 0.505203 -2.29221, -0.517407, -1.47332 -2.27295, 0.926853, -1.25382 -2.08349, 1.07592, -1.07346 -2.24931, -0.268628, -0.83662 -2.14039, -0.192588, -0.74699 -2.17064, -1.32343, -0.739279 -2.28463, -1.12962, -0.71113 -2.30863, 0.327791, -0.756621 -2.2535, 0.424342, -0.745614 -1.04383, -0.241936, -0.499189 -2.55327, 1.95672, -0.70411 -1.04669, -0.259689, -0.488992 -2.28985, 0.292545, -0.566894 -1.91804, 0.882515, -0.565112 -1.04176, -0.255708, -0.477511 -1.91332, 0.856235, -0.496864 -1.12753, -0.208639, -0.46579 -2.28683, 1.50531, -0.475882 -2.26519, -1.03519, -0.389677 -3.35527, 2.94699, -0.26159 -2.13644, 1.25225, -0.335708 -2.44644, 1.7815, -0.295682 -2.28246, -0.716035, -0.299612 -2.53299, 1.632, -0.157306 -2.30157, -1.01335, -0.143607 -2.28999, 0.0111331, 0.361721 -2.27518, -0.0419209, 0.353195 -2.1112, 0.581762, 0.563051 -2.29593, 0.0614642, 0.657977 -2.29885, -0.806844, 0.646256 -2.27866, -0.742176, -1.23812 -2.27994, -0.686468, -1.23696 -2.33417, 0.737734, -0.801868 -2.18577, -1.46044, -0.691792 -1.04405, -0.155577, -0.53216 -2.28142, -1.06267, -0.64746 -2.31722, 0.345654, -0.665366 -2.49078, 1.59478, -0.448604 -1.03007, -0.238834, -0.458053 -2.29704, -0.628351, -0.372496 -1.05625, -0.139512, -0.433473 -1.04086, -0.179279, -0.422502 -1.03915, -0.281367, -0.386379 -1.04806, -0.243615, -0.364325 -1.04663, -0.262662, -0.359074 -1.03269, -0.303974, -0.354087 -2.30051, 0.100012, 0.33487 -2.16293, 0.256207, 0.527033 -2.24953, -0.416182, 0.740408 -2.28786, -0.547758, -1.45878 -2.1346, 1.1258, -1.69589 -2.29337, -0.738956, -1.31679 -2.34333, -0.75022, -1.28277 -2.38782, 0.304924, -1.2977 -2.23638, 0.437851, -0.837039 -2.08071, -1.33785, -0.733011 -1.03494, -0.225709, -0.541089 -2.14761, 1.1305, -0.446855 -2.07144, 1.14622, -0.439893 -2.2831, -1.16743, -0.320484 -2.23383, -0.66795, -0.323642 -2.23434, -0.0651348, -0.289849 -1.04165, -0.281364, -0.398096 -2.46572, -1.19596, -0.0277849 -1.99336, 0.511235, 0.426106 -2.04336, 0.472101, 0.508648 -2.27289, -0.41393, 0.763081 -2.2621, 0.0443736, -1.35831 -2.30925, 0.912181, -1.50999 -2.27803, 0.927747, -0.0843191 -2.2847, 1.01705, 0.266588 -2.3009, -0.0434686, 0.513645 -1.30562, 0.038567, -0.426687 -2.59012, 1.13855, 0.152558 -2.28806, -1.20528, 0.0197423 -9.38547, 1.72873, -1.8032 -2.28581, 0.385466, -1.44273 -2.27299, 0.912698, -1.42033 -2.27123, 0.596473, -1.29249 -2.27998, -0.503811, -0.390947 -2.26339, -1.23236, -0.0844869 -4.43615, -1.8281, 0.498027 -2.26026, 0.151456, 0.364273 -2.15696, 0.40449, 0.568075 -2.2869, -0.002083, -1.36521 -1.0424, -0.179733, -0.544143 -2.14843, -1.30917, -0.734067 -2.29943, 0.295446, -0.567615 -2.25713, -0.872426, -0.154469 -1.03952, -0.279802, -0.376469 -2.26927, -0.0465756, 0.350717 -2.20017, 0.465193, 0.576571 -2.20622, 0.482647, 0.600171 -2.26562, 0.118335, -1.4803 -2.30597, 0.147645, -1.4398 -2.27419, 0.796636, -1.01847 -2.37239, -0.193182, -0.891598 -1.06849, -0.000477764, -0.57259 -2.42636, -0.136068, -0.775912 -2.23739, 0.419211, -0.767926 -2.26165, -1.11816, -0.705229 -1.04198, -0.269151, -0.488568 -2.33022, 0.783436, -0.341453 -1.04003, -0.135853, -0.399933 -2.27999, -0.811461, -0.148446 -1.04401, -0.237291, -0.399229 -1.04223, -0.275247, -0.38741 -1.04428, -0.272482, -0.373825 -1.04133, -0.278242, -0.367086 -2.30358, -0.0931979, 0.315399 -2.29065, -0.0354825, 0.364056 -2.08523, 0.675485, 0.552943 -2.05886, 0.795391, 0.592136 -2.23366, 0.308154, 0.575547 -2.09969, 0.408895, 0.516296 -2.1045, 0.146036, 0.535364 -2.13746, -0.419355, 0.647397 -2.3151, 0.288883, -1.51708 -2.28828, 0.224684, -1.4349 -2.26638, 0.643431, -1.47661 -2.29064, 0.37575, -1.41585 -2.28422, 0.137941, -1.24566 -2.05906, 0.499559, -0.704682 -1.04534, -0.164028, -0.434669 -2.27108, 0.0782191, -0.281719 -2.26589, -0.994784, -0.210375 -2.27259, -0.82001, -0.147662 -2.33601, -1.25553, -0.0720154 -1.04691, -0.270296, -0.371234 -2.29435, 0.0185581, 0.330303 -2.26388, 0.139358, -1.48197 -2.28859, 0.0567041, -1.3549 -2.246, 0.091934, -1.21521 -2.28363, 0.614364, -1.28867 -2.2907, -1.48059, -0.779529 -2.27872, 0.0517422, -0.224411 -2.27725, -0.0385974, -0.207804 -2.34149, -0.828164, -0.133503 -2.18545, -1.04519, -0.106204 -1.0447, -0.260627, -0.385966 -1.0484, -0.162662, -0.345873 -2.34968, 0.757774, -0.77328 -2.29019, -0.0322297, -0.210987 -1.04745, -0.237103, -0.394998 -2.50273, 0.367566, 0.704258 -2.20267, 0.607891, 0.629327 -5.40449, 0.71585, -1.32375 -2.11522, -1.27324, -0.574399 -2.26458, -1.24622, -0.0581115 -2.26567, 0.1636, 0.472 -2.54464, -1.35026, -0.070251 -1.91366, 2.03015, -0.607451 -1.74888, 2.50173, -0.491983 -1.82154, 2.39788, -0.407354 -1.72592, 2.3245, -0.237688 -1.71169, 2.74561, -0.0147375 -2.27377, -0.00847785, -1.33957 -2.24753, -0.152673, -0.770665 -1.80373, 2.66161, -0.493377 -1.61995, 1.98631, -0.391919 -2.11283, 3.55023, -0.46833 -1.78779, 2.65858, 0.0756188 -2.2815, -0.00960693, -1.36935 -2.31054, -0.109412, -1.30072 -1.04336, -0.266771, -0.524757 -1.04337, -0.221566, -0.505346 -1.04163, -0.238777, -0.486972 -1.50904, 1.688, -0.0909742 -1.04359, -0.256277, -0.374307 -1.04086, -0.268452, -0.487696 -1.04252, -0.252389, -0.483824 -1.04137, -0.205401, -0.399696 -2.61935, 0.172919, -1.51826 -1.04189, -0.266791, -0.523456 -1.03938, -0.257978, -0.478643 -2.32328, -1.09076, -0.0698441 -1.94795, 2.40503, -0.815881 -1.97355, 2.53127, -0.63039 -1.97073, 2.40866, -0.602538 -1.91666, 2.01822, -0.521541 -1.8032, 2.77979, -0.497487 -1.67332, 2.20662, 0.000289753 -2.02511, 2.63285, -0.641572 -2.04237, 2.7907, -0.645319 -1.9574, 2.46034, -0.599483 -1.99791, 2.60238, -0.577909 -1.87509, 2.73145, -0.470322 -1.81136, 2.57097, -0.46724 -1.68136, 2.36329, -0.00890862 -1.97607, 2.42131, -0.58036 -1.65484, 1.92348, -0.423628 -1.78661, 0.532564, 0.345779 -1.89204, 2.25573, -0.785153 -1.70837, 2.465, 0.0160281 -1.04476, -0.146504, -0.533388 -1.96507, 2.52472, -0.590668 -1.06312, -0.108515, -0.39224 -1.05336, -0.158251, -0.376456 -1.04281, -0.256609, -0.37407 -1.04031, -0.251635, -0.368308 -1.0425, -0.248539, -0.498806 -1.04688, -0.252373, -0.485542 -2.10551, 1.99658, -0.158728 -1.04295, -0.16347, -0.533348 -1.05528, -0.273285, -0.363054 -1.84378, 2.67655, -0.461773 -1.74424, 2.26516, -0.417833 -1.71302, 2.28617, -0.378913 -2.0418, 2.65645, -0.612322 -1.80228, 2.17356, -0.47506 -1.67631, 2.09502, -0.397976 -1.95705, 2.03206, -0.364866 -2.03379, 1.94632, -0.208838 -1.73389, 2.20973, 0.0393697 -1.99921, 1.94605, -0.597834 -1.94566, 2.55842, -0.631919 -1.82396, 2.18415, -0.365056 -1.73799, 2.347, -0.232551 -2.03532, 1.91795, -0.228628 -1.95429, 2.005, -0.511614 -1.95419, 3.10395, -0.532199 -2.18793, 1.38798, -0.440105 -1.07396, 0.607084, -0.0199633 -1.0346, -0.142814, -0.523887 -2.12864, 1.28724, -0.983599 -1.87458, 2.33044, -0.353875 -1.96307, 1.2927, -0.301811 -2.01419, 1.81196, -0.196292 -2.03547, 1.87838, -0.100924 -1.9954, 1.72125, -0.0831491 -1.97033, 1.89158, -0.872572 -1.9654, 1.99326, -0.642216 -1.87306, 1.87071, -0.319625 -1.03744, -0.165389, -0.531061 -1.05666, 2.21488, -1.75025 -1.11325, 4.91632, -3.03004 -2.10282, 13.0395, -7.13635 -1.1405, 2.54269, -1.82493 -1.71583, 3.33082, 0.0805218 -0.902264, 2.4303, -0.549804 -1.80195, 9.77386, 0.783087 -0.939859, 2.92417, -2.07856 -1.74874, 2.37895, -0.471454 -1.6301, 2.35666, -0.470871 -1.87393, 2.76362, -0.469544 -1.59338, 1.95341, -0.432717 -1.74866, 2.39318, -0.426827 -1.86079, 2.37859, -0.474728 -1.70384, 2.30339, -0.384199 -1.78095, 2.44812, -0.216405 -1.02944, 0.868682, -0.802142 -1.93516, 1.9844, -0.501304 -1.79706, 1.88241, -0.497948 -1.79377, 2.52695, -0.460159 -1.70217, 2.44844, -0.463702 -1.6403, 2.45743, -0.431615 -1.60857, 2.07481, -0.3912 -1.64746, 2.08192, 0.00685542 -1.08425, 1.23389, -0.173793 -2.38002, 1.34756, -0.439298 -1.18705, 1.01227, -0.429527 -2.22736, 3.2675, -0.310308 -1.30659, 1.16283, -0.422905 -2.35251, 2.71589, -1.15208 -2.40828, 2.93666, -0.235387 -1.08969, 3.18251, -1.786 -1.06287, 3.28281, -1.82144 -2.0065, 2.49386, -0.600214 -0.931326, 3.37514, -0.615737 -1.77775, 2.52087, -0.472967 -0.831657, 4.52518, -2.98689 -1.12462, 3.15403, -2.14475 --1.33847, 7.43215, -1.0794 --2.00272, 8.39406, -0.532316 -1.17186, 0.876342, -0.260223 -0.892533, 3.40378, -1.88218 -0.92381, 3.74917, -0.677226 -0.982275, 5.38111, -0.704543 -1.92376, 2.86984, -0.466284 --1.5004, 6.79714, -0.538399 -1.90394, 2.6619, -0.410555 -1.87756, 1.8861, -0.376157 -1.92894, 1.78027, -0.165304 -0.752157, 0.927027, -0.0410297 -0.535457, 2.60814, -1.82618 -2.01679, 2.07077, -0.627179 -1.87514, 2.12268, -0.63187 -1.87579, 2.87124, -0.532023 -1.90945, 1.94219, -0.38629 -2.01165, 1.82645, -0.112428 -1.02989, 0.593296, -0.0750774 -0.875081, 3.65595, -1.97751 -1.87724, 1.92242, -0.428879 -2.01974, 2.53864, -0.433998 -0.309325, 4.69278, 0.146957 -1.66284, 2.17976, -0.46361 -2.18098, 1.31809, -0.218851 --0.176686, 6.43325, -0.31941 -2.18062, 1.32007, -0.213975 -0.954172, 2.11055, -1.83167 -0.784196, 2.16733, -1.68396 -0.405227, 2.15309, -1.64989 -0.755947, 2.76167, -2.006 -0.917274, 2.4478, -1.8172 -1.13099, 2.78841, -1.95734 -0.830345, 0.930153, -0.0505971 -0.826032, 0.938881, -0.0488872 -0.59101, 2.53246, -1.77731 -0.588274, 2.49487, -1.75051 -1.88003, 2.22657, -0.581075 -0.973376, 4.91137, -0.675634 -0.828628, 0.950331, -0.0421438 -0.7457, 2.20082, -1.9485 --0.497536, 6.25547, -1.13123 -1.88501, 2.05952, -0.589176 -1.95247, 2.38112, -0.579586 -0.916072, 3.09983, -0.593709 -1.75212, 2.39703, -0.4289 -0.502989, 6.29483, -0.213836 -1.54531, 3.69768, 0.0251724 -1.18337, 1.58749, -0.212536 -1.22112, 4.00878, -3.12158 -0.867054, 2.58844, -1.87979 -1.90354, 2.10073, -0.589243 -1.93922, 2.00385, -0.397287 -1.08413, 3.49721, 0.341907 -0.741916, 0.919744, -0.132761 -1.96913, 2.59851, -0.467809 -2.25567, 1.39561, -0.207127 --1.03728, 6.50589, -4.80146 -0.230028, 2.96717, -2.09357 -1.19648, 3.72025, 0.0127887 -1.98012, 2.53547, -0.599442 -0.838137, 0.915591, -0.0367846 -0.583926, 0.920076, -0.0452865 -0.888979, 4.07598, -2.18196 --2.04865, 7.44973, -1.11447 -1.17206, 1.90085, -0.0336475 -2.02483, 2.19544, -0.288818 -0.939932, 1.94917, -0.054104 -1.10394, 2.53475, -0.000653219 -1.58337, 2.97254, -2.37275 -1.14246, 2.0934, -1.79599 -0.942675, 5.32959, -3.48092 -0.855967, 2.75639, -1.98676 -0.857454, 3.08364, -1.75929 -1.72212, 2.27963, -0.241254 -1.72923, 2.98622, -0.00327217 --0.749039, 1.99352, -0.877556 -0.293748, 4.91722, 0.190664 --1.70562, 3.99801, -1.05543 -0.748769, 0.95752, -0.0282896 -1.45876, 3.22176, -0.0186265 --6.38901, 17.8577, -13.4798 --2.71951, 18.3009, -13.1949 --2.37523, 9.75073, -7.18888 --0.944949, 7.77551, -5.66619 --2.57627, 14.3216, -10.1098 --4.49822, 14.92, -10.6991 --2.44772, 14.0229, -9.73549 -0.291078, 3.95192, 0.0729048 -1.14518, 1.97267, -1.79118 -0.572992, 2.61639, -1.82527 --0.623377, 1.65541, -0.748895 --1.80719, 4.5337, -1.06024 -1.60672, 2.83313, 0.0068788 -0.753753, 0.925979, -0.0417065 -0.595062, 2.32221, -1.64478 --0.647536, 1.67174, -0.75563 -1.58616, 2.11162, -0.470779 --0.736487, 4.95211, 0.110754 -0.0120021, 0.940099, -0.0298082 -0.751498, 0.948242, -0.0427242 -0.553427, 2.86291, -1.94738 -1.76914, 2.97256, -0.115557 -0.00874523, 0.948425, -0.0243473 -1.14989, 3.85613, 0.398607 -0.830913, 2.92707, -1.68598 -0.579372, 4.18637, -2.02824 -1.58324, 2.37044, -0.159752 -1.32775, 1.65295, -0.166954 -1.24631, 4.62556, 0.551306 --1.24876, 2.84096, 0.0399052 -1.26731, 2.27015, 0.0399754 -0.780379, 1.86088, -1.45893 -1.16197, 2.99998, -2.23255 -1.10195, 2.73133, -2.03223 -1.29084, 3.66864, -2.67879 --7.66854, 25.2578, -17.8345 --0.625948, 1.65024, -0.74554 -1.34582, 3.194, -2.41399 --0.863014, 6.93305, -5.05081 -0.925653, 1.79832, -1.38593 --7.83313, 23.2587, -16.6738 -0.977236, 1.98679, -1.51594 --3.52228, 6.55365, -1.11459 -1.71436, 3.79002, -0.437036 -1.59288, 2.19018, -0.0585316 -1.16055, 2.54995, -1.89361 -0.988962, 0.856689, -0.476346 -0.81718, 0.789743, -0.275921 -0.845102, 0.844804, -0.225566 --3.50461, 6.50872, -1.10185 -1.74468, 2.90571, -0.119675 -0.330927, 4.77652, 0.137194 -1.66273, 2.68389, -0.125097 --1.737, 3.36621, 0.207142 --1.20656, 2.54922, -0.989695 --1.64565, 3.8235, -0.992786 --0.85621, 2.68422, -1.48752 --1.14376, 2.56756, -1.29833 --1.23593, 5.30089, -0.291058 --0.829371, 2.18853, -0.202524 -1.51566, 3.35321, 0.0116968 -1.25432, 5.5692, 0.755327 -0.101035, 1.86892, -1.62917 -0.0812328, 2.33201, -1.74868 -0.125914, 2.3092, -1.68879 --1.02914, 5.88774, -4.22862 -0.0727827, 2.38517, -1.74806 -0.0568395, 2.40017, -1.75391 --1.44624, 3.15471, 0.153033 --0.130579, 3.19196, -2.32324 -1.81533, 2.52847, -0.467553 -1.53333, 2.81909, -0.0447416 --1.91079, 4.84367, -0.573272 -1.7032, 2.8449, 0.00459359 -1.24195, 2.20068, 0.040057 -1.62713, 2.95302, -0.389799 -1.20996, 3.54238, 0.00826518 -1.54426, 1.96984, -0.0310854 --1.05231, 2.45456, -1.23566 --1.44639, 3.50413, -0.925211 -1.91665, 4.11733, -0.181309 -0.0315219, 2.36855, -1.7778 -1.72917, 5.85934, -4.30727 -1.09068, 4.00861, -1.91982 -0.944744, 3.67387, -1.77459 -1.04578, 4.11029, -1.77932 --1.08375, 1.66502, -1.077 -1.77589, 3.2302, -0.464312 -0.328672, 4.99563, 0.0763377 -1.92362, 3.37449, 0.0796182 --1.65112, 3.12866, 0.155618 -0.897589, 3.84694, -2.1288 -1.96022, 2.48084, -0.818158 --0.267397, 5.38479, -1.21502 -1.80104, 2.67437, -0.473062 -0.143354, 5.03797, -0.0496759 -1.71778, 3.31733, 0.0779925 -0.91043, 4.17851, -2.3085 --1.02963, 5.35855, -1.39621 -1.80191, 3.89376, -0.400874 -0.288134, 4.85289, 0.217362 -1.78641, 2.91635, -0.00711439 -1.82661, 3.61346, 0.168426 -1.32902, 2.79773, 0.506399 --1.36033, 4.24368, -1.82329 -1.89895, 3.53913, -0.314688 -1.77694, 2.77875, -0.0865325 -1.45434, 3.20056, -0.0409483 -0.573841, 2.63767, -1.81573 --0.986865, 2.49769, -1.3964 --1.02094, 5.34575, -1.6198 -1.9774, 2.15723, -0.357057 -0.420535, 5.08306, 0.126037 -1.84048, 2.93361, -0.0721527 --0.218946, 2.14033, 0.30816 --0.66097, 2.23626, 0.411723 --0.0893525, 3.52532, -1.75327 --0.955901, 2.8327, -1.33645 -1.97983, 2.27228, -0.598078 -1.79106, 2.88174, -0.426419 -1.85892, 2.2164, -0.42771 --1.87301, 7.48348, -0.202049 --0.411697, 5.51205, -0.0794349 -0.464377, 5.25401, -0.00251928 --0.540818, 4.8542, -0.0119337 --0.955729, 2.23675, -0.220952 -0.286528, 4.78095, 0.144218 -1.70992, 2.44335, -0.121392 --1.70526, 4.258, -1.73042 -1.91386, 2.44916, -0.796856 -1.96737, 2.29403, -0.66003 -1.87802, 2.68407, -0.446701 -1.56167, 3.17042, -0.317616 -1.90731, 2.94593, 0.146856 --1.7125, 4.37997, -1.73606 --1.69471, 3.13554, -1.04045 --2.36411, 4.15045, 0.433038 -0.918787, 3.31418, -1.85459 -0.95008, 3.26522, -1.80615 -1.17421, 4.14013, -1.96622 -1.07826, 3.99797, -1.88046 -0.979496, 6.30728, -2.85748 --1.80237, 3.1692, -1.80302 --1.64848, 2.99065, -1.0573 --1.44937, 1.68145, -0.779806 -1.7883, 3.99436, -0.442852 --0.181163, 5.1029, -0.110017 --0.232216, 5.25705, -0.0535884 --0.0966118, 4.76298, -0.0485818 -0.30574, 4.88007, -0.0245943 --0.722339, 4.96835, 0.0530908 -1.33411, 2.78445, 0.607016 --2.41162, 5.01459, -0.954464 --1.71971, 4.75126, -0.557913 -1.63196, 2.90728, -0.461896 --0.494419, 1.73773, -0.383007 --0.466062, 1.87598, -0.353497 -0.291375, 5.04987, -0.00561111 -0.098479, 5.06768, -0.001778 --0.977452, 5.15661, 0.0125068 -0.261475, 4.21151, 0.110509 --0.399174, 2.19853, 0.391018 -0.23947, 5.05539, -1.55709 --0.478515, 1.77404, -0.658649 --0.595581, 1.66013, -0.654862 --1.37353, 5.46251, -1.05284 --1.67341, 4.36409, -0.549011 --0.244934, 5.38734, -0.218679 --0.301653, 5.26584, -0.0416034 --0.613647, 1.96942, -0.319539 --0.812028, 2.08545, -0.258397 --0.901457, 5.00147, 0.200791 --1.62528, 4.31386, 0.128761 --0.798585, 3.03829, -0.000154844 --0.958664, 3.25627, 0.135182 -0.400748, 5.3625, 0.578129 --0.482057, 2.10935, 0.373745 -1.31381, 2.75737, 0.79363 --0.987803, 5.26413, -1.6029 --0.744618, 5.35639, -1.55693 --1.02953, 2.26058, -0.778994 --1.82993, 4.23336, -0.943598 --2.05918, 4.57553, -0.813945 --0.520848, 1.75545, -0.380372 --0.529962, 1.9349, -0.34839 --0.773438, 2.31484, -0.310644 -0.0796551, 5.03153, -0.0196752 --1.81047, 4.1257, 0.165096 --1.12766, 3.60648, 0.112911 -1.15403, 2.45393, 0.0445832 -1.31621, 2.7487, 0.493476 --0.686968, 2.14444, 0.414421 -0.583612, 2.5081, -1.74792 -0.674824, 4.33536, -1.96487 --0.986545, 2.26889, -0.767474 --2.6154, 4.81676, -0.94889 --0.911809, 2.18281, -0.551329 --0.947325, 2.47306, -0.29469 --0.682956, 5.06671, 0.0232329 -0.309548, 4.87234, 0.174224 -1.244, 4.64461, 0.201701 --0.823428, 2.99518, 0.000319237 --1.39386, 3.26872, 0.0871491 -0.337878, 5.07853, 0.697504 -1.27416, 2.28946, 0.0978221 --0.720354, 5.48679, -1.64482 --1.38104, 1.92145, -0.895582 --2.6552, 4.95621, -0.957521 --2.24205, 4.44807, -0.542713 --2.27746, 4.32559, -0.470275 --0.752642, 4.995, -0.208621 --2.32065, 4.74841, -0.0282766 --0.928979, 2.31715, -0.270061 --1.54912, 3.55616, -0.125158 -1.3779, 2.60144, -0.159847 -1.57967, 3.77362, 0.0240132 -1.32677, 1.85024, -0.149827 -1.19779, 2.31294, 0.0736361 --0.416828, 2.19098, 0.385913 --2.21001, 5.29014, -1.3014 --1.18923, 2.44493, -0.859639 --1.62344, 3.77292, -0.156998 --0.842525, 4.87092, 0.101425 --0.987308, 2.54354, -0.0388139 -0.415682, 4.24799, -1.29719 -1.61397, 2.60309, -0.510692 -1.88911, 3.54514, -0.538654 -1.54715, 2.65528, -0.245535 -1.99527, 4.193, -0.102598 -1.6657, 6.42157, 0.74403 -0.504379, 4.5473, 0.51179 -0.951926, 4.64152, -2.19704 --1.74352, 3.21974, -1.78089 --1.304, 7.0186, -1.11496 --0.507313, 1.74488, -0.381224 --1.30555, 5.32573, -0.022788 --0.357877, 2.37582, 0.426966 -1.16223, 2.7435, 0.726944 --1.13652, 2.9691, -1.69428 --0.934313, 1.61283, -1.0893 --0.950287, 1.66854, -0.774403 --1.20632, 3.91271, -0.911291 --1.5055, 5.79555, -1.09304 --2.1203, 7.6205, -1.17637 -1.83539, 3.24771, -0.605654 -1.74508, 3.48572, -0.487843 --0.618179, 1.89916, -0.286734 -1.58985, 2.69112, -0.112945 -1.78404, 3.03088, 0.0195759 --1.69565, 3.1662, 0.198361 --0.36553, 2.35253, 0.42476 -1.18958, 2.78608, 0.761125 -1.05443, 4.34249, -1.84856 --0.736113, 5.31517, -1.5649 --0.614703, 1.66568, -0.758104 --0.675553, 1.93253, -0.71152 --1.87068, 4.69195, -0.674458 --1.54806, 6.90205, -0.655191 -1.72763, 3.12186, -0.408765 --0.851429, 2.12283, -0.389747 -0.340813, 4.80679, 0.0481693 -2.12705, 5.41881, 0.274175 --1.06184, 3.45665, 0.0719241 --0.897446, 2.98301, 0.275991 --0.838731, 2.4282, 0.513644 --0.375405, 2.15901, 0.357326 -1.18898, 2.80246, 0.75941 -0.54997, 2.50135, -1.765 --0.0495322, 3.63372, -1.7662 --2.12562, 5.19083, -1.03305 --1.92263, 7.08061, -1.11066 --1.88596, 4.32534, -0.468855 -0.192874, 5.06826, 0.028414 -0.333419, 4.89726, 0.0923949 --0.889875, 2.99587, 0.163539 --0.642683, 1.65767, 0.0753931 --0.420254, 2.2156, 0.398365 -1.18607, 2.77952, 0.750898 -1.03115, 2.7581, 0.807272 --1.40123, 2.94981, -1.56879 --0.448115, 1.54536, -0.713222 --7.07862, 10.5218, -0.732039 --1.86975, 5.03254, -0.521745 --3.75462, 5.83377, -0.563458 --0.848799, 2.12764, -0.394238 --0.556086, 5.17599, -0.181631 --0.18588, 4.70726, -0.0953167 -0.510234, 5.41488, 0.115392 -0.261373, 5.26536, 0.141422 -1.64196, 3.53641, -0.0557747 --0.791036, 4.85714, 0.142698 -1.12593, 2.36533, 0.0256789 -1.61399, 3.10782, -0.45862 --1.66774, 3.92415, 0.144074 -1.20399, 3.84448, 0.325348 --0.697238, 1.73757, 0.208154 -1.31188, 2.72628, 0.626687 -0.931656, 4.39755, -2.03221 -1.79035, 2.77443, -0.357042 --1.51607, 4.41447, 0.129709 -1.69188, 2.47824, 0.00328214 -1.30277, 2.69356, 0.508901 -1.20831, 3.43697, 1.30879 --2.57207, 5.73092, -2.16548 -1.9532, 3.22214, -0.673853 -1.78454, 3.74863, -0.226954 -1.74182, 2.76035, -0.0990078 -1.13602, 3.05131, 0.775041 --0.992394, 1.66377, -1.17597 --0.184874, 5.25942, -0.184743 --0.892225, 5.08913, 0.17991 -1.64046, 3.12629, -0.00491623 --1.08351, 2.56918, 0.178698 --1.08174, 3.05242, 0.72899 -1.86563, 3.25352, -0.677449 -1.83809, 3.24701, -0.609601 --1.89872, 4.75067, -0.675971 --0.250643, 5.46631, -0.0418312 --0.570257, 5.33089, -0.0237793 -1.68736, 3.22286, 0.867309 --1.79251, 3.17003, -1.80188 -0.788839, 4.01207, -1.78246 --1.05232, 1.66665, -1.09003 -2.23448, 5.14237, -0.394038 --0.950246, 5.06616, 0.00228453 -0.467931, 5.23402, 0.0868799 --1.78136, 4.06946, 0.139602 -1.72839, 2.5339, -0.00747393 --1.04002, 2.52367, 0.160134 --0.741718, 2.18326, 0.445715 -1.17113, 3.90624, -1.87768 -0.66073, 3.72371, -1.73547 --2.3596, 4.1106, -2.09799 --0.646846, 1.70278, -0.764698 --1.36246, 1.65647, -0.776736 --1.808, 4.6393, -0.849177 -2.04765, 2.67679, -0.600814 -1.7087, 2.7407, -0.14515 -1.73877, 2.85069, -0.0384104 -1.64986, 2.43038, -0.0389099 -1.19285, 3.21447, 0.427116 --0.635235, 2.25866, 0.421376 --1.88333, 3.76269, -1.8228 --1.67148, 3.61842, -1.69676 --1.8831, 4.25984, -0.963664 --1.8393, 4.71923, -0.667031 --2.74221, 5.00398, -0.53088 -1.74306, 2.3066, -0.232498 -0.368806, 5.43337, 0.103207 --0.904462, 2.1817, -0.230022 -1.55182, 5.41154, 0.776861 --0.496959, 2.31772, 0.459047 --1.57106, 2.94752, -1.57624 --1.58205, 3.49326, -1.6404 -0.257738, 5.0805, -1.52972 --0.80432, 1.72671, -0.757134 -1.74294, 2.76085, -0.11028 --1.11493, 2.6119, -0.0438428 -1.71208, 2.22968, 0.0424984 --0.62606, 2.1963, 0.390135 -0.930333, 4.36037, -1.87201 --1.23254, 3.0109, -1.46634 -0.51422, 6.22985, -1.81235 -2.13924, 2.8241, -0.592665 --2.24455, 4.64154, -0.44801 --1.06787, 3.27478, -1.27553 -1.27171, 2.25667, 0.0448705 --1.05716, 1.67563, -1.13473 --1.0886, 1.67586, -1.1175 --0.619575, 1.64781, -0.746406 --0.590753, 1.82468, -0.374932 --0.146357, 5.05402, -0.0246016 -0.138282, 4.86937, -0.0262844 -0.332316, 5.03605, 0.0847609 -0.283393, 3.88877, 0.0558378 --1.49265, 3.3678, 0.137401 --1.06448, 3.56895, 0.182013 --0.567266, 2.21295, 0.433101 --0.969425, 1.63104, -1.10102 --1.4489, 1.65663, -0.610717 -1.77274, 3.07637, -0.477341 --1.7335, 4.84123, -0.510935 -1.65079, 2.12581, -0.391526 --0.5879, 1.82304, -0.375402 --0.576817, 1.91936, -0.324194 --1.48507, 4.45037, 0.185188 -1.27139, 3.92385, 0.12078 --1.5881, 3.12612, 0.12065 --0.990171, 3.41458, 0.148053 --1.11288, 3.14824, 0.768411 --0.441728, 2.26677, 0.431191 --0.541202, 2.19406, 0.43948 -1.1264, 2.61436, 0.658745 --1.03749, 1.64941, -1.10992 --1.79863, 3.32868, -1.8236 --0.645266, 1.64919, -0.751346 --0.570781, 1.7226, -0.633263 -1.70819, 3.73403, -0.436608 -1.81979, 4.06064, -0.411632 -0.258189, 5.04998, -0.00458106 --0.612656, 1.96723, -0.319546 --0.842904, 2.10344, -0.219804 -1.7975, 2.95566, -0.0255624 -1.48604, 2.80058, 0.647177 -1.31824, 2.75638, 0.78789 --1.95333, 4.34606, -1.86202 --0.973899, 1.63494, -0.953337 --1.69951, 4.46593, -0.953537 --1.29503, 4.97125, 0.0319533 -0.432485, 4.69314, 0.057352 -1.20072, 3.83415, 0.351658 -1.32176, 2.75506, 0.668332 --0.638046, 2.15099, 0.428068 -1.16373, 2.94277, 0.858791 --1.70306, 4.33812, -0.513099 --2.61403, 4.82584, -0.54915 --0.990852, 3.34654, 0.15405 --0.903397, 1.74523, 0.183441 --0.428452, 2.20019, 0.390754 --0.639369, 2.13672, 0.419517 --0.0594257, 3.5204, -1.74143 --1.35806, 3.26142, -1.51002 --1.94497, 3.94499, -0.841634 -1.74173, 3.91598, -0.382094 --1.26651, 3.11616, -0.279497 --1.66581, 3.85955, -0.126321 -1.70679, 2.69535, -0.134257 -1.76031, 2.8138, -0.10567 --1.39884, 2.91225, 0.0406543 --0.698626, 2.14577, 0.430027 --0.55905, 2.27884, 0.465877 -1.08511, 3.80726, -1.86193 -0.447852, 5.02621, -1.47416 --0.910643, 1.90295, -0.808276 -2.05792, 3.73381, -0.765657 --1.3075, 7.10816, -1.1057 --0.795721, 2.37983, 0.512462 --1.31075, 4.30386, -1.46238 --1.2973, 3.8112, -1.07338 --1.43614, 3.40809, -0.884062 -2.02556, 4.22689, -0.834293 --0.420488, 4.99676, -0.306435 -1.34587, 2.77228, -0.16192 --1.08091, 1.66303, -1.07465 --1.9826, 3.45082, -1.8476 --0.956889, 2.22334, -0.841457 -1.7637, 2.51449, -0.47726 --1.94721, 4.77036, -0.513801 -1.73397, 3.00939, -0.455571 --0.57147, 1.77846, -0.272476 -0.0262949, 2.4272, -1.76335 --0.424144, 2.21029, 0.39657 --0.828215, 1.80399, -0.56854 --0.862622, 2.12666, -0.209881 --0.805702, 2.38639, 0.502011 --0.517686, 2.16495, 0.408409 --9.77058, 16.06, -2.16466 --3.30248, 5.78484, -1.04799 -1.2915, 3.88555, 0.0155358 --0.920187, 2.21019, -0.85566 --1.84506, 4.68007, -0.66307 -1.54185, 3.33717, -0.0136978 -1.29909, 3.77355, 0.0584044 --0.690474, 2.22332, 0.432307 --1.04659, 2.38304, -0.965381 --0.791813, 1.79724, -0.521916 --1.36304, 5.88569, -0.397327 --3.09522, 5.92577, 0.0314558 --1.1131, 2.53401, -0.119785 --0.875488, 1.84083, -0.576132 --1.37082, 3.61066, 0.137263 --1.01836, 3.3754, 0.155028 --1.14798, 2.51345, 0.0263051 --0.999019, 1.64845, -1.11548 -0.318273, 4.527, 0.146535 --1.09217, 3.63925, 0.199296 --0.569767, 2.08316, -0.373633 --0.904999, 2.18063, -0.205651 --1.4009, 3.66175, 0.149302 --0.632115, 2.25446, 0.419081 --0.548795, 2.18873, 0.421004 --0.895012, 1.86495, -0.537167 --0.936723, 2.23665, -0.129688 --0.69363, 2.23249, 0.403643 --1.13136, 3.65588, 0.213224 -0.659549, 3.51021, -2.03209 --1.27951, 5.28167, -0.156916 --0.585432, 2.09354, -0.335769 --1.27353, 3.45754, 0.0979053 --0.355179, 2.36075, 0.421314 --0.98056, 1.61569, -1.11817 --0.93888, 2.24674, -0.176256 -1.33735, 2.6273, -0.0745811 --1.68791, 4.11615, -1.18971 --0.554329, 1.76855, -0.422726 --1.05343, 1.67297, -1.1148 --0.810022, 1.77293, -0.53901 --1.36353, 3.59146, 0.132112 --1.07531, 2.5591, 0.175465 -1.14209, 2.34601, -1.78596 --1.02085, 1.66552, -1.12927 --1.41114, 3.69078, 0.153925 -0.334688, 5.04356, 0.0770581 --1.46162, 4.3791, 0.171799 --1.01209, 2.93058, 0.668735 --0.455529, 2.25939, 0.412702 --0.547194, 2.18151, 0.414073 --1.10951, 1.66975, -1.05376 --0.611988, 1.64146, -0.745556 -1.83853, 4.2815, 0.098033 --0.645337, 2.27672, 0.434866 --0.477626, 2.3275, 0.467192 --0.678016, 1.76655, -0.77678 -1.75157, 3.02583, -0.473665 --2.99164, 5.2805, -0.541505 --1.31003, 2.97556, 0.0653864 --3.55806, 6.46274, -2.97021 --0.698725, 1.69574, -0.760207 -1.49149, 2.8977, -0.017434 --0.901371, 2.17671, -1.05679 --0.829915, 1.75024, -0.757333 --2.00168, 3.96225, -0.735735 --1.18891, 3.54427, -1.61591 --0.826452, 1.84592, -0.446255 --1.80921, 6.41214, -0.0538085 --0.681447, 2.18265, -0.341534 -1.56028, 4.14544, 0.182316 -1.09417, 2.30874, -1.74768 --1.08191, 1.67324, -1.11293 --0.653956, 1.70153, -0.768157 --0.569575, 1.71931, -0.633159 --1.02124, 3.39752, 0.154138 --1.1253, 3.14831, 0.784399 --0.995573, 1.66858, -1.17729 --0.587754, 1.60717, -0.732937 --0.107058, 5.02615, -0.00740687 --1.40161, 4.23432, 0.147044 --1.38931, 3.06409, 0.124573 --0.984382, 2.8916, 0.645072 --0.464117, 2.27332, 0.416057 -1.16646, 2.38209, -1.81708 --0.568425, 1.71949, -0.633779 --0.831849, 1.80173, -0.542252 --0.639019, 2.00194, -0.316157 --0.995066, 3.34948, 0.137374 --0.966114, 2.82393, 0.612303 --0.63995, 2.26621, 0.427622 --0.555468, 2.19792, 0.425324 --2.79302, 6.19703, -2.50772 --0.566209, 1.71879, -0.634537 --0.816974, 1.78988, -0.532584 --0.794897, 2.02814, -0.399016 -1.21805, 3.76095, 0.010249 --0.874977, 1.8273, 0.179106 --1.5851, 1.77593, -0.808607 -1.63483, 3.12063, 0.0249395 --0.699109, 2.25724, 0.492525 --0.841483, 1.84828, -0.470387 --2.93968, 6.5935, -0.217646 --0.779182, 2.29866, -0.270047 --0.709406, 2.19355, 0.439622 --0.659634, 2.21086, -0.351932 -1.61964, 2.86398, -2.28651 --0.931688, 1.65319, -1.25928 --0.778085, 1.96899, -0.861872 --0.939735, 2.23958, -0.12733 --1.06894, 2.48944, -1.03555 --0.499666, 1.91593, -0.365262 --1.53883, 3.97594, 0.219296 --1.33082, 3.06191, 0.632147 --1.25049, 1.33893, 0.00364466 --1.03647, 1.65552, -1.10983 --0.926131, 2.18582, -1.10945 --0.598735, 1.61739, -0.735968 --1.62981, 4.52682, -0.246531 --0.509816, 1.94883, -0.383003 --1.13703, 2.60191, -0.0357547 --1.44435, 3.15375, 0.136093 --0.881284, 2.11737, -1.02791 --0.78103, 1.74792, -0.565326 --0.665791, 2.42547, -0.351097 -0.704337, 7.81615, 0.682687 --1.38864, 3.13098, 0.0948036 --0.86497, 1.82952, 0.174133 --1.04697, 1.04036, -0.0597393 --0.941916, 1.65701, -1.2512 --1.1561, 2.63882, -0.669995 --1.8146, 4.53531, -0.513338 --1.33374, 2.96911, -0.493378 --1.5624, 4.28026, -0.274083 --0.91787, 2.22554, -0.386826 -1.23895, 3.83327, 0.0307196 -1.2938, 2.74593, 0.76984 --0.459559, 2.24631, 0.422782 --0.89082, 2.13785, -1.11967 --0.580565, 2.04001, 0.303428 --2.18391, 5.16047, -0.27319 --0.44355, 1.78367, -0.369066 --0.536283, 2.25764, 0.460703 --1.40655, 4.0561, -1.83905 --0.763779, 1.96715, -0.889581 --1.76543, 1.96272, -0.867326 -0.834629, 5.19474, -0.410863 --114.788, 258.083, 8.16435 --0.725023, 1.98128, -0.395294 --0.792648, 2.02991, -0.222383 -1.19228, 2.23766, -1.83313 -1.20676, 2.33169, -1.84781 -1.15308, 2.29891, -1.81311 -4.82134, 7.20532, -5.91705 -1.22966, 2.39713, -1.87577 -16.6208, 22.6659, -18.3448 -2.29291, 3.97144, -3.06938 -1.17435, 2.48738, -1.85319 --1.08113, 1.66057, -1.07396 --1.46811, 4.39253, 0.170213 --1.43075, 3.71972, 0.16427 -1.17794, 2.36986, -1.83927 --0.996337, 1.67204, -1.18159 --1.073, 1.65802, -1.11984 --1.30785, 3.43101, 0.104415 --0.997608, 3.35682, 0.137691 --1.8081, 1.22497, -1.12883 --1.08079, 1.27073, -0.0471888 --0.737306, 4.9538, 0.0677591 --0.449337, 2.25483, 0.425233 --0.633542, 2.21096, 0.398153 --0.978731, 2.32354, -0.998495 --0.817489, 1.81076, -0.457443 --0.753334, 1.92275, -0.568258 --0.616942, 2.03531, 0.336921 --0.653404, 1.76811, -0.538968 -1.20102, 2.17551, -1.84402 -1.21045, 2.27965, -1.86276 -1.01936, 2.19611, -1.75507 -1.03042, 2.13626, -1.69867 -1.00361, 2.1772, -1.70502 -2.20446, 3.5769, -2.90042 -4.30325, 6.34109, -5.1009 -5.79084, 7.8008, -6.4026 -1.09592, 2.325, -1.77795 -1.25103, 2.58464, -1.96575 -1.15775, 2.43113, -1.83884 --1.06782, 1.65987, -1.10456 --0.200231, 5.34328, -0.0819822 --0.256391, 5.33462, -0.0528562 -1.05472, 2.24531, -1.74221 --1.02134, 1.64784, -1.10168 --0.567606, 1.72097, -0.633787 --0.176861, 5.14134, -0.141145 --1.02442, 2.93284, 0.673843 --1.14952, 1.28549, -0.040052 --1.32677, 1.56255, -0.601295 --1.32777, 4.94585, -0.279305 --1.11047, 1.29581, -0.400878 --0.554118, 2.00862, -0.320799 --1.10466, 3.16971, 0.800063 --1.41543, 3.81889, 1.17983 --0.639586, 2.26965, 0.429238 -1.17113, 2.40712, -1.84884 --1.47158, 5.82323, -0.167894 -1.29267, 3.95439, 0.0423056 --1.22013, 1.32237, -0.00576224 --0.358943, 2.40689, 0.448168 --0.624368, 2.19326, -0.64643 --0.810911, 1.9756, -0.40352 --1.08208, 4.08179, 0.0288439 -0.286117, 4.23045, 0.100742 --1.22588, 1.31892, -0.00529074 --1.12661, 1.29518, -0.024362 --0.657032, 2.09099, -0.564622 --0.874518, 1.91665, 0.3479 --1.38645, 5.92867, -0.332821 --0.894738, 2.29577, -0.403247 -1.13532, 1.97603, -1.77407 -1.15326, 2.05262, -1.79834 -1.12224, 2.13642, -1.77197 -1.03814, 2.1454, -1.73855 -1.10735, 2.26623, -1.82531 -0.694126, 2.14522, -1.61038 -1.04969, 2.2049, -1.73543 -1.13981, 2.26262, -1.7898 --1.12463, 1.08833, -0.0166849 -5.14668, 7.54843, -6.09123 -0.880972, 2.44889, -1.828 -0.700466, 2.1678, -1.59917 --1.01969, 1.65753, -1.12825 --0.573588, 2.01755, -0.317576 --1.03387, 2.93323, 0.676986 --1.16303, 1.09667, -0.0417318 --1.14108, 1.09309, -0.0166001 --0.812406, 1.79755, -0.569959 --0.937084, 1.96016, -0.435879 --1.24194, 4.96725, -0.242549 --0.557035, 2.05847, -0.313676 -0.434573, 5.35103, 0.0940131 --1.1117, 1.28292, -0.0375891 --1.16156, 1.09458, -0.0417951 --1.38859, 1.61275, -0.609093 --0.842778, 1.87335, -0.57832 --0.825419, 1.81614, -0.534308 --1.31698, 5.53206, -0.212023 --0.471973, 2.32466, 0.440815 --0.96397, 2.31742, -1.08699 --0.892887, 1.84607, -0.768857 --1.26471, 1.50239, -0.59567 --1.38662, 5.21088, -0.315747 -1.72227, 2.95213, 0.0104527 -1.49114, 2.78474, 0.0231428 --1.01457, 1.18272, -0.0395237 -0.656198, 3.22705, -1.76841 -1.59657, 3.05654, 0.0092364 -1.30614, 2.58163, -0.034707 --1.11214, 1.27707, -0.0234502 --0.516906, 2.10403, 0.364085 --0.9208, 1.90903, -0.788651 --0.756764, 1.94984, -0.226003 --0.53178, 2.20894, 0.434534 --0.82354, 1.69826, -0.742017 -0.764522, 5.08002, -0.132232 -1.17165, 2.05057, -1.82193 -1.15717, 2.10941, -1.84582 -1.09193, 2.10401, -1.77228 -0.996224, 2.10065, -1.73337 -0.994589, 2.04722, -1.69249 -1.07607, 2.19434, -1.796 -0.973984, 2.16106, -1.70453 -0.910442, 2.27481, -1.74244 --1.08315, 1.66122, -1.07442 --0.956592, 1.62219, -0.734978 --1.10325, 3.11901, 0.781372 --1.12109, 2.97146, 0.725603 -0.988782, 2.07487, -1.72113 --1.06248, 1.66022, -1.14099 --1.24674, 4.79727, -0.237551 --0.176187, 5.64952, -0.163827 --1.18571, 1.31237, -0.0188647 --1.00387, 1.18391, -0.0464203 --0.988407, 1.0304, -0.046668 --1.02487, 1.65804, -1.12419 --1.9593, 1.2818, -1.20184 --1.10352, 1.29119, -0.402577 --1.18266, 1.30138, -0.0245231 --1.21508, 1.12599, -0.0122902 --1.13427, 1.08818, -0.0220063 --0.985797, 1.65173, -1.16356 --1.40326, 1.62668, -0.611879 --1.30157, 4.98187, -0.219425 --1.08732, 1.31147, -0.399701 --1.13088, 1.31515, -0.398736 -0.397381, 5.20391, 0.175558 -1.54614, 2.76191, -0.0814209 --1.10409, 1.2894, -0.0280027 --1.16163, 1.10412, -0.0397039 --1.09331, 1.71052, -1.1645 --0.712878, 2.12557, 0.40667 -0.758792, 3.2583, -1.75786 --0.893562, 1.85067, -0.76847 --0.924458, 2.27984, -1.1172 --1.13031, 1.29121, -0.0174189 --0.868084, 2.24907, -0.397578 -1.14483, 2.06076, -1.83309 -1.20051, 2.09173, -1.86463 -1.09903, 2.0332, -1.77841 -1.099, 2.1067, -1.81266 -1.21474, 2.17748, -1.87739 -1.1662, 2.10077, -1.8073 -1.07161, 2.22455, -1.82363 -2.22575, 4.4197, -3.42289 -0.89947, 2.25919, -1.78059 -3.82646, 6.54138, -5.05306 -1.00323, 2.33532, -1.84218 -2.4734, 4.29076, -3.365 -1.28682, 2.72764, -2.14145 -2.95449, 4.13588, -3.36005 -2.76974, 4.75332, -3.66856 -0.479232, 2.27417, -1.66936 -2.73055, 4.59705, -3.54059 -0.229044, 4.29348, -0.998254 --1.0949, 1.28851, -0.40133 --1.11725, 2.97062, 0.725278 --1.16656, 1.32415, -0.00914177 --1.10947, 1.3018, -0.021295 -1.10185, 2.04767, -1.80995 -0.441334, 2.62883, -1.96032 -0.439776, 2.25805, -1.6596 --0.599096, 4.15829, -0.922985 --0.519099, 2.0227, -0.632995 --0.899585, 1.87855, -0.436968 --1.03817, 1.22924, -0.409063 --1.05621, 1.26897, -0.403422 --1.07186, 1.29632, -0.361856 --1.16709, 1.2573, -0.0231648 --0.825133, 1.68879, 0.155125 --1.07841, 1.08755, -0.0264104 --1.5003, 1.76116, -0.786263 --1.0252, 2.54526, -1.20919 --1.48127, 1.74107, -0.811319 --0.674731, 2.1008, -0.732997 --0.528661, 2.07257, -0.640012 --1.28681, 1.46096, -0.383687 --0.767299, 3.03897, -0.193815 -1.11131, 2.55599, -0.0421567 --1.19011, 1.29899, -0.0229887 --0.656152, 1.88614, 0.221054 --1.01638, 1.18593, -0.0384537 --1.02049, 1.04924, -0.0723992 --0.591286, 2.02098, -0.596172 -1.95755, 3.68713, -0.542812 -1.0391, 2.10451, -1.83967 -1.07361, 2.07682, -1.82145 -1.14061, 2.10749, -1.8488 -3.66655, 6.02599, -4.68222 -3.13637, 6.06729, -4.49696 -0.32538, 5.00809, -0.0729393 -3.65733, 5.46155, -4.48153 -0.228376, 2.31329, -1.75391 -0.968483, 2.53372, -2.00151 --1.17549, 1.29839, -0.0256001 -0.208239, 2.28966, -1.7558 -0.0724114, 2.67455, -1.95299 --0.572236, 1.76609, -0.642116 --0.876575, 1.82345, -0.439249 --1.32585, 4.94588, -0.271068 --0.975646, 0.912888, -0.350866 --0.957534, 0.911269, -0.350465 -0.55982, 5.34665, 0.772623 --1.18774, 1.30341, -0.0227251 --1.15269, 1.34202, 0.00387784 -1.10802, 2.18894, -1.85407 --0.556928, 1.70418, -0.649769 -0.733723, 3.43084, -1.8806 --0.98178, 1.02667, -0.0531885 --1.12705, 2.98495, -1.5581 -0.537504, 2.8456, -1.5699 --1.53193, 6.55835, -0.306269 --1.08099, 1.22587, -0.0487961 --1.62031, 5.04914, -2.22947 -0.951099, 2.08768, -1.90762 -1.02807, 1.99523, -1.82078 -0.854284, 2.09937, -1.82028 -0.810804, 1.92267, -1.69991 -0.981305, 1.87614, -1.71265 -1.00343, 2.02976, -1.80613 -1.04969, 2.01546, -1.79706 -4.79754, 7.21039, -5.80173 -1.97905, 4.2139, -3.33446 -1.09062, 2.0184, -1.8051 -4.72273, 9.27539, -7.02761 -1.04022, 1.93179, -1.72886 -2.31161, 4.36047, -3.43529 -4.4511, 8.06712, -6.16502 -0.417327, 3.13155, -2.43683 -0.945538, 2.08884, -1.7732 -1.08849, 2.01464, -1.76847 -2.18187, 4.61993, -3.52455 -2.66692, 4.82281, -3.72594 -1.78611, 4.5512, -3.3738 --1.03524, 1.65807, -1.10493 --0.499454, 1.89542, -0.639805 --0.780906, 1.89436, -0.311086 --0.981721, 1.02175, -0.0529495 --0.704182, 0.937168, -0.0386731 -0.937297, 2.04827, -1.8027 -1.24435, 2.26807, -2.00889 -0.932787, 2.07052, -1.81573 -5.18818, 7.67695, -6.19073 --1.15419, 1.25215, -0.030172 --0.968383, 1.01034, -0.0635215 -1.2629, 2.47761, -2.14323 -21.0535, 34.1646, -25.4968 --0.986344, 1.00232, -0.427424 --0.739942, 1.71063, -0.33183 --1.08798, 1.31423, -0.358593 --0.849911, 1.78205, 0.152309 --0.725242, 0.963115, -0.0115135 -0.740006, 1.93537, -1.61184 --0.995111, 1.66898, -1.05554 --0.513396, 3.1736, -0.804895 --1.05589, 2.8009, -0.22173 --0.95728, 0.887085, -0.326211 -2.4341, 4.25185, -3.32673 --0.680958, 5.12407, -1.35564 --0.792081, 1.85698, -0.617661 --0.993481, 0.927209, -0.345363 --0.695042, 1.32607, -0.0439059 --1.50363, 4.50718, -1.97616 --0.485173, 2.48285, -0.715737 --0.854703, 1.75306, 0.144863 --0.595208, 2.2098, -0.615818 -1.31892, 2.47454, -2.2119 -2.66706, 5.96499, -4.55771 -9.20579, 20.1166, -14.5365 -0.91872, 2.09548, -1.84391 -3.03563, 5.21916, -4.13898 -4.6898, 8.19078, -6.25226 -5.26808, 9.82286, -7.36594 -2.93424, 4.95371, -3.93911 -5.02533, 8.34782, -6.36706 -0.977034, 2.04998, -1.80342 --1.80589, 0.849525, -1.20528 --1.82374, 1.0346, -1.24849 --1.05189, 1.63484, -1.12949 --1.40862, 1.6802, -0.773738 -0.464322, 5.59009, -1.19932 --0.963515, 0.919817, -0.350698 -0.952358, 1.99913, -1.81815 -3.10726, 5.58412, -4.33667 -14.3025, 21.7253, -16.1089 --0.869127, 1.805, -0.426939 --1.11802, 1.3078, -0.0151576 -2.37674, 10.7926, -7.51271 --1.38682, 1.6561, -0.768817 --0.519115, 2.60054, -0.739502 --0.883865, 1.74828, -0.569555 --1.07666, 1.28833, -0.399734 --0.708965, 2.63439, -0.243269 -0.409384, 5.19707, 0.0767902 --0.644814, 5.05388, 0.0744609 --0.764736, 5.30302, 0.105888 --0.978284, 0.894313, -0.327869 --0.687174, 0.936761, -0.0395036 --0.657919, 0.928432, -0.0454128 --0.905284, 2.1949, -1.21283 -2.08322, 2.7148, -0.595822 --1.82953, 0.858255, -1.22094 --0.952953, 1.29992, -0.400661 --0.924548, 3.28342, -0.0528908 --0.768822, 1.79761, -0.590769 -0.650616, 3.09884, -1.70428 -1.84289, 12.8181, -2.12792 --0.896976, 1.87771, -0.425064 --1.26742, 1.45888, 0.0720209 --0.528601, 7.88258, -1.27979 -0.0796066, 11.5499, 0.181427 -0.978101, 3.86091, -2.98912 -5.64604e+06, 2.18731e+07, -1.46076e+07 -3.8519, 15.0174, -10.3019 -0.426979, 2.65564, -2.12083 -2.18807, 10.1782, -7.00034 --0.802677, 4.26292, 0.0683505 --1.10757, 1.06381, -0.0453432 --1.80905, 0.851983, -1.21066 --1.27566, 4.62842, -0.379716 --0.600085, 2.70466, -0.298213 --1.11993, 1.32944, -0.356701 --1.09387, 1.0707, -0.0440885 --1.81021, 0.876094, -1.38278 -1.63733, 6.4717, -4.72543 -0.353525, 2.35445, -1.93109 -0.18356, 1.99883, -1.70379 --0.889625, 1.86952, -0.402695 --1.0783, 1.30141, -0.359507 --1.01329, 1.19128, -0.0410153 --0.848452, 1.71173, -0.743802 --0.622221, 1.73938, -0.666752 --0.570746, 3.97554, -0.902551 --0.896798, 3.18333, -0.0714009 --0.853867, 1.95594, -1.08761 --0.967703, 3.10826, 0.00752451 --1.32453, 1.70999, -0.805125 --0.967929, 0.900302, -1.24623 -2.73509, 12.4895, -8.74731 -0.930805, 4.22024, -3.22886 -0.272486, 2.05009, -1.78628 -1.43927, 5.18877, -3.87246 -0.709505, 3.66882, -2.83997 -2.6726, 12.5922, -8.65645 -0.570422, 3.02139, -2.40531 --1.04288, 1.12453, -0.745223 --1.45857, 5.57775, -0.266803 --1.16213, 1.34358, -0.398103 --0.968079, 2.46563, -0.230343 --0.963657, 0.887004, -0.333605 --1.33182, 1.61348, -0.872146 --1.12889, 1.33721, -0.396845 --0.946434, 0.870958, -0.356337 --0.0176518, 1.88125, -1.73885 -1.54796, 6.28196, -4.67965 -1.44828, 5.39235, -4.0324 --1.27397, 1.51269, -0.850732 --1.05223, 1.1378, -0.72209 --1.32762, 1.60425, -0.806833 --0.861334, 9.8782, -1.60268 --0.991276, 0.92646, -0.420989 --0.929698, 0.94121, -0.418074 --0.543333, 1.82249, -0.356907 --0.870773, 2.0629, -0.313263 --0.816831, 3.61018, -0.113689 --0.898961, 1.89347, -0.0240377 --0.961382, 0.959454, -0.114093 --0.980328, 1.02162, -0.0566382 -0.131343, 2.86047, -2.0484 -0.0442059, 3.93163, -2.17873 --1.29788, 3.88827, -1.73388 --0.93892, 2.37679, -0.278376 --0.998691, 2.69511, -0.208198 --0.962428, 3.12992, -0.0694188 -0.135184, 5.38857, -1.17601 --0.518012, 1.77312, -0.62717 --0.977457, 1.34459, -0.357995 --0.933074, 2.27851, -1.21588 -2.05982, 4.29878, -0.0296327 --0.652772, 2.43276, -0.254864 -7.0568, 31.2875, -3.14004 --1.08643, 3.91816, -0.641796 -0.932659, 5.33844, -0.48658 --1.67778, 1.13744, -1.68935 --0.974766, 0.906546, -1.26289 --1.82422, 0.606953, -1.2282 --1.03492, 1.66115, -1.12454 --1.80378, 1.03232, -0.806181 --0.963976, 0.893424, -1.25496 --0.983579, 1.65065, -1.16329 --1.83841, 1.32713, -1.84984 --1.00606, 0.899109, -1.2487 --0.976453, 0.922336, -1.2227 --1.80978, 0.621553, -1.13206 --1.11187, 1.27735, -0.022116 --1.00315, 1.06826, -0.0398402 --0.988251, 0.943404, -1.23927 --1.06328, 1.64911, -1.12316 -0.67194, 5.31725, -1.16238 --1.27257, 3.85546, -1.7514 --0.741225, 1.96162, -0.58719 --0.860527, 2.14739, -0.621497 --0.990023, 1.38319, -0.394937 --0.894028, 2.20473, -0.289373 --1.0369, 5.98356, 0.110091 --0.739834, 6.22916, -1.55512 --0.88988, 4.92424, -0.00813335 --1.06926, 1.66483, -1.05983 --0.618772, 3.3541, -0.750011 -1.62396, 3.95651, 0.0824358 -1.46596, 2.90848, 0.0268463 --1.81832, 0.834949, -1.66769 --1.68937, 1.13856, -1.71079 --1.80337, 1.2951, -1.81286 --1.8211, 1.03117, -1.2423 --0.746204, 2.01729, -0.584379 --1.1691, 1.11422, -0.0424979 --1.15508, 1.07173, -0.0412723 -0.000584466, 0.917648, -0.0457866 --1.70434, 0.37111, -1.26914 --1.79309, 0.465713, -1.19707 --1.79971, 0.617688, -1.13004 --1.82198, 0.703311, -1.09557 --1.02218, 1.77269, -1.08009 --0.51181, 1.84557, -0.614261 --0.966987, 1.01821, -0.412104 --0.968901, 3.13251, -0.200486 --1.0066, 2.8587, -0.175272 --1.09087, 1.10859, -0.0809358 --0.974166, 0.903827, -1.24258 --0.870706, 2.1935, -0.243981 --0.981793, 3.16113, -0.0572911 --1.24809, 1.20136, 0.0276169 --0.986563, 0.940002, -1.22518 --1.7561, 0.452031, -1.17505 --1.28304, 5.14925, -0.300258 --0.96437, 0.877398, -0.419738 --0.989287, 0.964966, -0.342829 --1.00943, 0.974923, -0.312312 --1.1552, 1.13012, -0.0397644 --1.78652, 0.458606, -1.26358 --1.38839, 1.59891, -0.627495 --4.17129, 21.797, 0.239159 --0.9337, 1.4254, -0.400164 --1.5155, 5.1845, 0.0254609 --0.93914, 0.86717, -0.352397 --0.878857, 1.81708, -0.041656 -0.2624, 2.72053, -2.18304 -0.655743, 3.22016, -1.77666 -1.76386, 2.32673, -0.388312 -1.3521, 3.90172, 0.0754304 --0.544285, 5.61969, -1.05221 --1.99526, 16.136, -1.55019 -0.882342, 5.37475, -0.389886 -0.315209, 3.5908, 0.287907 --1.80853, 0.823444, -1.74411 --1.36721, 1.35072, -1.85548 --1.4023, 1.36813, -1.86779 --1.80151, 0.612129, -1.13365 --1.82027, 0.895355, -1.13247 --1.31202, 5.12427, -0.298749 --0.845249, 1.93809, -0.326124 --0.993671, 0.959381, -0.349994 --1.14736, 1.0628, -0.0404301 --1.00735, 0.972996, -0.345844 --0.960686, 0.88766, -0.338485 --1.0404, 1.0218, -0.0620605 -0.0453544, 0.908501, -0.0484341 --1.82676, 0.856986, -1.2197 --0.808101, 1.86305, -0.60187 --1.0795, 1.20393, -0.0463705 --1.80524, 0.604688, -1.19232 --0.973994, 1.35395, -0.356322 --1.04501, 1.02822, -0.0629335 --1.52351, 1.82753, -0.549769 -1.67647, 2.41526, 0.0138733 --1.11173, 1.12188, -0.0598935 --1.0338, 1.03038, -1.3748 --0.11223, 1.64032, -1.56322 --1.03957, 3.58224, -0.337077 --0.630954, 2.10723, -0.258239 --0.871651, 1.34783, -0.531809 --0.8702, 2.72391, -0.44414 -2.21153, 4.56054, -0.0282686 -1.63413, 6.21761, -2.86528 -0.033907, 13.783, 0.324569 --1.46721, 1.35714, -1.86164 --1.83208, 0.415904, -1.34549 --1.03256, 2.39143, -0.433773 --0.516797, 1.91475, -0.37241 --0.953684, 0.862255, -0.339083 --1.11799, 1.01715, -0.0506374 --1.44609, 1.354, -1.86131 --1.80254, 0.47, -1.20379 --0.501987, 1.83473, -0.636882 --0.513039, 1.9422, -0.371597 --0.9805, 0.920247, -0.352211 --0.977689, 0.950411, -0.34631 --1.21031, 5.40847, 0.205036 --1.80683, 0.824415, -1.75199 --1.78019, 0.832241, -1.73421 --1.83269, 0.66613, -1.19482 --1.80798, 0.642889, -1.07573 --0.823048, 1.9523, -0.636982 --0.510541, 1.77853, -0.609551 --0.493865, 1.70723, -0.381374 --0.987159, 3.14361, -0.150839 -0.0697616, 2.79555, -1.89439 --0.822223, 1.11325, -0.415208 --0.982346, 3.16685, -0.14709 --0.97157, 0.897205, -0.31558 -0.0439464, 0.927628, -0.0445581 --0.000478915, 0.935385, -0.0402119 --1.86595, 0.454428, -1.09095 --1.62011, 1.9532, -0.96735 --1.09294, 11.8096, -2.4517 --1.31097, 1.45427, -0.621891 --0.942877, 1.58416, -0.515253 -0.523622, 4.84958, -1.09691 --0.645645, 2.1242, -0.579033 --0.944453, 2.49145, -0.426709 --0.968412, 1.17572, -0.716718 --0.925186, 4.69777, -0.806605 --1.41968, 1.26052, -1.82524 --1.29193, 1.28725, -1.80125 --1.35181, 1.25273, -1.78778 --1.40332, 1.27194, -1.80508 --1.35037, 1.22176, -1.75342 --1.64338, 1.28133, -1.81844 --1.68534, 1.20341, -1.78526 --1.67479, 1.3141, -1.83117 --0.979171, 0.914788, -1.26865 -0.0100724, 2.55911, -1.79705 --0.989198, 0.971745, -0.346884 --1.33776, 1.27093, -1.79558 --1.27485, 1.38135, -0.592878 --1.2517, 1.35703, -0.574879 --0.669729, 1.45998, -0.331824 --1.19403, 1.31374, -0.0154435 --0.998815, 1.04067, -0.0878214 --1.75506, 0.758762, -1.71168 --1.86364, 0.50217, -1.30182 --1.32574, 1.51704, -0.733934 --1.35354, 1.52727, -0.538763 --1.81325, 0.835608, -1.75996 --1.17677, 4.09227, -0.360732 -0.02272, 1.862, -1.71263 -0.298976, 3.93785, -2.70915 --0.998132, 0.965482, -1.23383 --1.95689, 2.56778, -1.10866 --1.01538, 5.44666, -1.41789 --1.79038, 1.14175, -0.809672 --0.710283, 1.6794, -0.560996 --1.39133, 5.7015, -0.295526 --1.86968, 0.497394, -1.31105 -1.23078, 3.61688, 0.0396742 -1.15175, 3.75196, -1.92639 -1.88454, 3.50768, -0.550002 --1.14235, 1.21708, -0.764604 --0.414414, 1.95203, -0.679176 --1.23355, 1.43892, -0.226845 --1.32781, 1.30765, -1.81889 --1.39741, 1.20469, -1.75232 --1.63752, 1.23044, -1.80942 --1.65079, 1.19596, -1.78038 --0.95921, 0.804118, -1.19317 --1.0301, 0.910478, -0.558386 --0.0854423, -0.869721, -0.44901 --0.251408, 5.01203, -0.0789365 --1.10679, 1.01383, -0.0655116 --1.28546, 1.28445, -1.80336 --1.38652, 1.38292, -1.87827 --1.73245, 1.72827, -2.1517 --1.81535, 0.837269, -1.66907 --1.8288, 0.386092, -1.28221 --0.955216, 0.869509, -1.22297 --1.04914, 1.02989, -0.67754 --0.793267, 1.38243, -0.559646 --0.695353, 2.58251, -0.577165 --1.07325, 0.965995, -0.0817858 --1.02384, 0.993676, -0.0698106 --1.36144, 1.21252, -1.7551 --1.21164, 1.57616, -0.390314 --1.09708, 1.0089, -0.0715028 --1.08469, 0.964794, -0.0533125 --1.33516, 1.5955, -0.768311 --0.502579, 1.68526, -0.621064 --1.11404, 4.60338, 0.106833 --0.862341, 2.56098, -0.122323 --2.03343, 1.52916, -2.04234 --1.42771, 2.07437, -0.834678 --1.45436, 1.8014, -0.796316 --1.00033, 1.12784, -0.405779 -0.29527, 3.87724, 0.44779 --2.8792, 4.40897, 0.682203 --1.16022, 1.1308, -0.00500219 -1.59445, 8.32549, -4.96372 --1.05267, 1.70549, -0.749653 --0.460961, 1.17149, -0.606208 -1.85814, 2.30556, -0.443499 -1.74776, 3.8108, -0.379048 -1.49937, 2.6482, 0.000481946 --3.86447, 6.41771, 1.11691 --1.14364, 3.24502, -1.56612 -1.56431, 2.23391, 0.0109996 -0.524107, 2.76965, -1.50229 --1.46116, 1.85319, -1.34401 --1.75001, 6.56783, -2.558 --0.827323, 0.853737, -0.682627 -0.362753, 3.03829, -0.761436 -0.945513, 6.11606, 1.34859 --1.35291, 1.21708, -1.79024 --1.36311, 1.33294, -1.84766 --2.25006, 2.26922, -2.5711 --1.71665, 1.20084, -1.81183 --1.62522, 1.0874, -1.69294 --1.87558, 0.507535, -1.24053 --0.699532, 1.87568, -0.570846 --0.540925, 5.00193, -0.0853022 --0.958113, 0.880479, -0.341691 --1.82157, 0.438717, -1.18152 --1.14455, 1.15783, -0.526772 --0.780757, 1.74186, -0.267288 --1.00277, 0.981198, -1.31055 --1.42153, 1.65689, -0.546069 --1.79476, 0.371797, -1.08287 -1.80508, 4.12547, -0.378105 --0.796603, 1.79514, -0.267144 --1.87028, 0.425421, -1.27997 --0.958727, 1.00644, -0.685923 --0.959358, 1.83725, -0.792614 --0.579345, 1.8965, -0.584187 --0.735955, 1.94171, -0.588141 --0.953391, 1.61687, -0.53072 -1.06335, 3.35125, -1.80297 -1.99256, 1.78979, -0.281572 -1.1719, 3.01711, 0.00344088 -1.63765, 8.46583, -4.93276 --0.984147, 1.08722, -0.697632 --0.490491, 1.6945, -0.648127 -1.37913, 3.93952, 0.0659089 -0.813951, 5.44484, -1.08171 -1.19796, 2.41933, -0.018888 -1.42579, 3.58168, 0.186888 --1.27709, 1.25399, -1.78804 --1.34138, 1.27165, -1.79952 --1.35045, 0.911584, -1.58089 --1.62456, 1.15014, -1.7817 --1.70814, 1.23116, -1.82306 --1.84986, 0.377026, -1.30895 --1.81282, 0.478148, -1.1898 --0.998621, 0.946994, -0.346789 --1.35488, 1.32802, -1.83695 --1.83759, 0.49419, -1.28581 --1.84774, 0.504506, -1.21012 --0.935399, 0.866717, -0.351663 --0.800238, 1.85747, -0.260307 --1.03809, 0.985738, -0.0436045 --0.970216, 1.01687, -0.409033 -0.437076, 3.61779, 0.755368 -0.0449503, 2.06474, -1.81581 --1.89274, 0.437876, -1.35935 -1.41554, 7.65684, -4.75597 -1.94419, 2.54646, -0.583704 -1.81317, 9.80792, -5.82243 -0.0502737, 2.07064, -1.77829 --0.294994, 5.38887, -0.0231319 -1.03439, 4.67248, -3.29623 --1.06065, 1.24514, -0.737772 -1.25377, 3.14298, -0.0177995 -1.37532, 2.47098, -0.0167613 --1.77272, 0.631288, -1.10026 -1.66493, 3.09921, -0.380626 --1.67534, 0.947651, -0.584255 -1.52666, 2.28833, 0.0326498 --1.68377, 0.704169, -1.75742 --1.39972, 1.30403, -1.88233 --1.70704, 1.16504, -1.88753 --1.36928, 1.29878, -1.85234 --1.60331, 1.11874, -1.80747 --1.58802, 1.07967, -1.78347 --1.35835, 1.16869, -1.75766 --1.32023, 1.26545, -1.79454 --1.43221, 1.21608, -1.79581 --1.68144, 1.21522, -1.86676 --1.43931, 1.17608, -1.76442 --1.60406, 1.24794, -1.84957 --5.7056, 6.16587, -5.6596 --1.67332, 1.2432, -1.85702 --1.97376, 1.70789, -2.17786 --0.968488, 0.899323, -1.25855 --1.81414, 0.470053, -1.20935 --1.81829, 0.42754, -1.06405 --0.993643, 1.68096, -1.17732 --1.34437, 1.6597, -0.770417 --0.987047, 0.958528, -0.344132 --1.35229, 1.3536, -1.85765 --1.81274, 0.374373, -1.49606 --1.71352, 1.18099, -1.75347 --1.04036, 0.956742, -1.28065 --1.43351, 1.79057, -0.869515 --0.95396, 0.879338, -0.324888 --1.12327, 4.84588, 0.137413 --1.02455, 0.929177, -1.26588 --0.985139, 1.02302, -0.951333 --1.23926, 1.50272, -0.751757 --0.62304, 1.96093, -0.551118 --0.999183, 0.961436, -0.338451 --1.15263, 1.3024, -0.0161976 --1.40155, 1.73731, -1.16952 -2.06363, 2.42101, -0.65935 --0.640901, 1.81917, -0.556156 -1.07326, 3.05624, -0.0226007 -0.312004, 3.71007, 0.783538 --1.23542, 4.74531, -0.2928 --0.798419, 4.67866, 0.215084 --0.894443, 1.49649, -0.781242 -1.90779, 2.19806, -0.650569 --0.0111234, 2.18377, -1.884 -0.118115, 2.81073, -1.88614 -0.607811, 2.94523, -1.64053 --0.401269, 4.02202, -0.89078 --0.632253, 2.18643, -0.651462 --1.57074, 0.828202, -0.590028 -1.05563, 6.53283, 1.36615 --0.910505, 1.03433, -0.704189 --0.850921, 2.26986, -0.460081 -1.77087, 3.58821, -0.110802 -1.43318, 3.20532, 0.0146704 -1.29418, 2.56397, 0.140479 -0.772751, 4.81535, 0.961754 --3.18889, 3.1324, -3.41447 --3.13901, 3.15385, -3.36709 --1.81348, 0.368369, -1.49801 -0.00275405, 2.52601, -1.79074 --1.74824, 0.581132, -1.11447 --0.878591, 1.98212, -0.393948 --0.979992, 0.916513, -0.345576 --1.78253, 0.795606, -1.74577 --1.08622, 1.03949, -0.0287362 -0.0366501, 2.17177, -1.78472 --1.37548, 1.6704, -0.774846 --0.9952, 2.46077, -0.378949 --0.981278, 0.934389, -0.344761 --1.118, 1.02069, -1.63037 --0.667195, 2.11401, -0.564255 -0.636648, 3.90645, -1.86915 -1.63944, 2.33405, 0.0118641 -1.62162, 3.40364, -0.131033 -1.15843, 3.87646, -1.95466 -1.84622, 3.74751, -0.118186 -2.08504, 7.6998, 0.856519 --1.07573, 1.01509, -0.0516445 --1.81288, 0.849303, -1.75954 --1.81727, 0.367713, -1.49046 --1.83965, 0.482495, -1.22043 --1.82836, 0.609483, -1.22637 --0.979505, 0.868134, -1.48943 --0.98972, 0.926824, -0.319776 -0.323216, 4.91963, 0.0833896 --1.07469, 0.909323, -1.61932 -0.614065, 2.98862, -1.77688 --1.81762, 0.892118, -1.12928 --0.953491, 0.897017, -0.349332 -0.625383, 2.94993, -1.76647 -0.0811721, 2.41483, -1.8958 -1.13593, 7.14743, -4.29461 -1.75228, 1.99707, -0.45458 -0.765168, 5.16566, 0.0284364 --0.659811, 1.70405, -0.454649 -1.39358, 1.90219, -0.0440857 --1.3437, 0.975077, -1.81195 --1.40365, 0.994443, -1.83019 --4.94422, 3.83658, -4.48129 --1.29838, 1.00474, -1.77259 --1.68518, 1.16478, -1.82691 --1.73609, 1.23715, -1.85385 --1.72377, 1.2652, -1.84681 --1.82474, 3.96414, -1.74779 --1.73959, 1.19201, -1.77055 --0.824322, 1.84166, -0.39825 -0.872057, 0.981712, -0.188736 --1.04514, 0.970848, -0.0501342 --1.79477, 0.819604, -1.74461 --1.14129, 1.0235, -1.61188 -0.374697, 3.4365, -2.4253 --1.26816, 3.25613, -0.496569 -0.316683, 4.67083, 0.0628064 --1.25123, 1.34883, -0.004924 --1.00308, 2.37459, -0.880985 --0.823697, 2.06572, -0.235702 --0.854324, 2.0646, -0.851267 --1.12278, 2.74385, -0.397985 --0.927897, 2.98925, -0.321468 -1.13132, 2.56461, -1.86885 -0.628796, 3.72238, -1.85774 -1.7228, 3.77107, -0.392311 --0.985963, 2.42767, -0.924944 -2.05647, 3.97852, -0.565945 --1.25277, 3.1084, -0.411124 -1.67816, 6.74892, 0.722971 --1.16406, 2.90575, -1.03357 --1.44531, 1.83117, -0.853998 --0.548801, 1.7294, -0.629061 --1.34094, 5.66865, -0.309754 -1.46828, 3.08176, 0.132374 --2.26363, 0.920207, -2.46699 --2.27859, 1.27893, -2.40755 --1.64344, 0.771793, -1.79713 -0.885924, 2.51153, -1.79991 -1.02195, 2.44684, -1.77103 --1.50705, 0.796534, -1.86753 --0.925699, 0.864284, -1.51201 -0.172315, 2.09819, -1.78929 -0.047068, 2.10305, -1.77666 -0.068857, 2.08356, -1.75618 --1.31859, 1.55743, -0.764336 --0.945486, 0.879379, -1.52104 -0.904166, 4.77424, -3.14241 -0.986409, 7.27427, -4.40024 --0.959941, 0.852319, -1.22317 --1.51552, 3.90512, -0.507654 -1.75918, 2.75825, -0.0979279 -0.0764393, 1.8452, -1.62581 -1.08467, 3.16647, -1.76065 --1.08585, 0.993707, -0.0466757 -0.740496, 0.917568, -0.0522663 -0.00564792, 2.43085, -1.77772 --0.993472, 1.03929, -0.0774806 --1.63808, 4.40514, -1.90115 --0.997292, 3.05365, -0.312255 -0.829716, 0.847703, -0.089979 -0.806112, 2.93272, -1.85058 --1.33502, 5.68086, -0.982542 -1.11799, 2.2171, -0.0691714 --1.4353, 0.440534, -1.80966 --1.56211, 0.601272, -1.873 --1.3231, 0.648351, -1.77255 --1.55428, 0.77193, -1.89093 --1.34525, 0.827754, -1.81365 --1.37964, 0.801791, -1.79768 --1.62498, 0.653691, -1.78615 --0.962635, 0.779327, -1.55967 -0.0802776, 2.18531, -1.83427 --0.0827707, 5.00361, -3.17294 -0.930889, 2.56256, -1.82294 -0.86886, 2.5044, -1.7996 --0.32676, 6.16266, -3.42815 --1.37319, 0.648716, -1.80962 --1.40186, 0.704636, -1.83126 --1.3129, 0.692393, -1.75608 -0.0759761, 2.23423, -1.859 -0.199967, 2.2754, -1.83348 --1.02676, 0.916785, -1.26192 --0.824521, 1.81591, -0.568575 --1.47347, 3.81218, 0.179713 --1.15676, 1.24785, -0.0307328 --1.4157, 0.541075, -1.872 --1.59989, 0.73399, -1.96727 -0.0173608, 2.14968, -1.79566 --1.63896, 4.14284, -1.00206 -2.11805, 2.70521, -0.596727 --0.815151, 1.78822, -0.566425 --0.994495, 0.933106, -0.343597 --1.1835, 1.26243, -0.0248708 --1.04808, 0.961555, -0.0576456 --1.92575, 1.30182, -1.8835 -1.16211, 2.6054, -1.8563 -0.619747, 2.42614, -1.73179 -1.63477, 1.85417, -0.471167 -2.02633, 2.62066, -0.821236 --0.754435, 1.93295, -0.559788 --0.786475, 1.96824, -0.408856 -0.861021, 0.997358, -0.0303545 -0.0313518, 2.31485, -1.70403 --0.863055, 2.2797, -1.35755 --1.04781, 2.44558, -0.963144 -2.02793, 2.13284, -0.663184 -1.60276, 2.77794, -0.522145 -1.36006, 2.31633, -0.0312038 -2.0172, 2.12607, -0.685299 -1.94883, 2.23911, -0.518556 -0.960679, 0.956449, -0.434821 -0.900588, 0.970304, -0.0626222 --1.48483, 0.542047, -1.84675 --2.06731, 0.929618, -2.24337 --1.33488, 0.79729, -1.79269 -0.903404, 2.55592, -1.92161 --0.977875, 0.906643, -1.26589 -1.3354, 4.62007, 0.124708 --1.39182, 0.504594, -1.78853 --1.48292, 0.703333, -1.90367 --1.32937, 0.906577, -1.82497 --0.978133, 0.753935, -1.59342 --1.68179, 1.16187, -1.82263 -0.144368, 2.26743, -1.87709 --0.97934, 0.916653, -1.27553 -1.79189, 2.15424, -0.584612 --0.976138, 2.30393, -0.0271195 --0.891056, 0.816888, -1.50876 --1.82806, 1.25555, -1.9102 -0.534435, 2.4381, -1.74006 -0.606787, 2.40313, -1.72488 --0.966386, 0.898902, -1.26144 --0.778613, 2.2412, -0.753918 --1.39007, 1.60512, -0.601304 --0.88644, 2.14461, -0.0425933 -1.13468, 1.06292, -0.20673 -1.31244, 1.50093, -0.108131 -1.93769, 2.4732, -0.588 --0.811776, 2.0315, -0.579691 --2.19681, 5.36066, -0.51438 -2.29268, 6.06084, -0.35932 --0.594437, 1.93127, -0.315902 -0.756096, 0.971847, -0.0976208 --1.91499, 1.24402, -1.85539 -0.249186, 2.37326, -1.86725 -0.0430874, 2.22311, -1.70088 -0.518639, 2.62946, -1.82752 -1.34222, 1.15639, -0.453847 -1.6283, 3.5845, -0.38874 --1.00246, 2.37404, -0.400855 -1.0844, 2.15442, -1.71273 --2.42631, 5.65724, -2.09505 --1.5874, 3.8873, -0.964549 -1.751, 2.50504, -0.387514 -1.73072, 3.63662, -0.294684 -1.34958, 1.82161, -0.192615 --1.39142, 1.68936, -0.816604 -1.49018, 2.71051, -0.277796 --0.870415, 2.70748, -1.21273 --0.738176, 2.01298, -0.163588 --0.872498, 2.21265, -0.0384185 --0.766701, 0.305257, -1.41204 --1.40483, 0.685632, -1.86354 --1.45316, 0.615335, -1.85049 --2.94758, 1.65111, -2.95963 -0.275883, 3.86958, -2.66234 -0.224055, 4.23029, -2.78728 -0.993972, 3.38448, -2.3358 -0.825152, 2.44304, -1.77915 -0.9491, 2.42285, -1.7607 -1.07526, 2.33323, -1.72022 -1.06204, 2.34792, -1.72936 --0.996146, 0.889794, -1.23835 --1.00905, 0.995415, -0.0673497 --1.38452, 0.641799, -1.84826 --1.30603, 0.738596, -1.80638 --2.85848, 1.60455, -2.80854 --0.970644, 0.765133, -1.57314 -0.223321, 3.49842, -2.44485 -1.0066, 2.75742, -2.01573 -0.892796, 2.90493, -2.09319 -0.951447, 3.08751, -2.17264 -0.956651, 2.7332, -1.99056 -0.103749, 2.90841, -2.16148 -0.414739, 1.71525, -1.5124 -1.11086, 2.38297, -1.74553 -2.02468, 2.72878, -0.836076 -1.42205, 1.46163, -0.503005 -1.56608, 1.73956, -0.470543 -1.1358, 0.880417, -0.443892 -1.30611, 1.33656, -0.415023 --0.633631, 2.19647, -0.369103 -1.21794, 1.43636, -0.157028 -0.820201, 3.58172, -1.95635 -1.64736, 2.64574, -0.0290322 --1.0827, 2.50531, 0.0205013 -1.37674, 1.48373, -0.109314 --2.6358, 1.76215, -2.39296 --1.0009, 0.894388, -1.2491 -0.62865, 2.74924, -1.68073 --0.95186, 2.23968, -0.915837 -2.19714, 2.73244, -0.683078 --0.254494, -2.3568, -0.466939 -1.22095, 1.6012, -0.386908 --0.716563, 2.30324, -0.344096 -1.9572, 3.46245, 0.24876 -0.641913, 1.95673, -1.50595 -0.706794, 3.26173, -1.89244 --0.793515, 1.99816, -1.11798 --0.960473, 2.26928, -0.910342 --1.39017, 3.36634, -0.969833 -1.88967, 2.31411, -0.701048 --1.13014, 2.62411, -0.574139 -0.719147, 0.722298, -0.119853 --1.85968, 4.46792, -1.78353 --0.907543, 2.24374, -0.52917 -1.43583, 2.7657, -0.415133 -1.00212, 2.21835, -0.0598502 -1.12125, 1.2506, -0.166888 --0.990584, 2.57888, -1.47288 -1.28441, 3.73877, 0.211585 --1.39332, 0.660382, -1.85713 --1.47661, 0.722027, -1.89072 --1.46794, 0.684506, -1.83317 --1.34374, 0.898147, -1.8221 --1.01415, 0.808557, -1.60342 --0.0130414, 2.62016, -2.00605 -0.855146, 3.01041, -2.15124 -0.964791, 2.39019, -1.74849 -1.0126, 2.47335, -1.78349 -1.66385, 3.90642, -0.385361 --1.44131, 0.715921, -1.8715 --1.06967, 0.812906, -1.63918 -0.331962, 2.77454, -2.05788 -0.0564079, 2.31896, -1.83682 -0.149561, 2.85055, -2.09416 --0.953678, 0.757213, -1.55564 --0.183374, 3.21314, -2.30236 -1.44914, 1.48692, -0.549269 --0.885604, 2.15504, -0.59768 -1.89919, 2.67553, -0.205563 --0.251602, 3.55913, -2.48022 --0.791081, 2.02046, -0.488567 --0.964596, 2.27549, -1.22045 -1.13704, 0.871018, -0.569221 -2.21628, 2.66312, -0.622663 -1.14439, 1.07698, -0.424202 -1.12363, 1.18451, -0.421907 -1.71871, 2.81465, -0.0909962 -1.52197, 3.48382, -0.00342685 -1.27979, 1.25559, -0.155261 --0.965704, 2.7526, -0.337712 -0.727659, 2.95763, -1.66679 -0.71512, 3.45659, -1.62497 --1.8654, 4.14813, -0.752356 -1.8087, 2.02552, -0.479495 -1.66785e+06, 1.31262e+07, 3.72313e+06 --0.211564, 1.09949, -0.706144 --1.22058, 1.592, -0.846673 --3.37905, 7.49662, -1.69845 -1.3916, 1.97494, -0.556722 --1.99259, 1.0371, -2.27421 --1.68399, 1.23907, -1.82242 -0.291606, 2.73048, -2.02795 -0.979875, 2.90633, -2.09506 -1.06201, 2.54492, -1.82734 -0.98897, 2.44785, -1.77294 --1.01206, 0.895309, -1.25028 --0.993373, 0.8884, -1.23571 --2.54654, 1.33443, -2.68164 -0.106973, 2.55235, -1.93981 -1.11805, 3.76464, -2.46393 -1.1561, 5.13735, -3.18208 -0.8206, 2.78437, -2.04837 -2.35882, 3.04032, -0.681628 -1.51543, 1.54532, -0.548555 -1.72935, 2.9437, -0.479002 --0.966345, 0.906667, -1.56136 --0.271765, 0.188481, -0.925886 -1.80932, 1.99797, -0.625882 --1.75154, 4.42347, -0.512486 -1.2844, 3.02541, -2.16784 -0.829387, 4.01229, -1.98742 -1.63075, 1.67766, -0.605783 -1.29422, 1.16078, -0.500945 --0.796504, 2.03099, -0.560382 -1.2486, 1.2116, -0.436632 --1.43948, 3.08067, -0.392344 -1.48473, 2.39555, -0.355802 --1.79233, 4.16808, -0.144931 -1.22917, 2.16821, 0.0256955 -0.705148, 5.12428, -3.34291 -0.630904, 3.5071, -1.82599 -1.25665, 3.86615, 0.175837 -0.86134, 1.0002, -0.0412635 -0.793051, 5.41533, -2.57607 --0.932924, 2.4825, -1.48423 --2.26889, 4.71853, -1.06769 --1.80894, 3.97969, -0.233731 --6.76881, 3.06596, -5.41602 --1.37275, 0.858406, -1.81976 --0.973753, 0.76346, -1.59315 --1.39873, 0.908832, -1.83738 --0.828363, 0.710107, -1.44671 -0.855963, 2.70351, -1.91187 -1.16035, 2.88376, -2.00183 --1.74434, 3.39474, 0.112218 -0.480517, 2.54179, 0.634658 -0.384583, 3.91959, 1.19623 --0.836016, 0.706786, -1.47387 -0.380136, 2.0386, -1.84229 --0.177252, 3.43321, -2.75782 --1.4724, 3.19957, 0.158992 -0.309777, 2.30284, -1.83911 --1.3026, 9.98939, -6.23953 -0.831115, 2.70522, -1.9761 -1.76455, 2.85344, -0.0508034 --1.42548, 3.13411, 0.141119 -1.10994, 0.989218, -0.427048 -1.12148, 1.15486, -0.42309 -1.66932, 2.51416, -0.128104 -0.782248, 2.93058, -2.07053 -0.754061, 2.33472, -1.72771 --0.754574, 1.71509, -0.532477 -1.19012, 1.16403, -0.42156 -1.61188, 2.39933, -0.139588 -1.71873, 2.84882, -0.00544025 -1.79525, 1.9654, -0.439949 --0.905959, 2.21494, -0.915666 -1.3419, 1.25302, -0.587137 -1.50862, 2.86676, -0.321354 --0.785491, 2.01055, -0.214658 -2.38694, 5.60441, -0.0250515 -0.922475, 0.830005, -0.348766 -1.69503, 3.89843, 0.366632 -1.14143, 4.13481, 0.421789 --1.43642, 0.871394, -1.83813 --1.08657, 0.833841, -1.62059 --1.45487, 0.954191, -1.84747 --4.39828, 1.65134, -3.33492 -0.997669, 2.05859, -1.83181 -1.0016, 2.04764, -1.81165 --0.084257, 2.69219, -2.2922 --3.65029, 15.7614, -11.1107 -0.25104, 3.55439, -2.45678 -1.04845, 2.58075, -1.88909 -1.64468, 6.95998, -4.26943 -0.750472, 0.941874, -0.0461216 -0.698084, 1.08338, 0.0211176 -0.559355, 0.982054, -0.0200179 -0.507296, 0.981785, -0.0204997 -0.585046, 0.984591, -0.0192866 -0.754868, 0.937703, -0.0358446 --1.3443, 0.853609, -1.80302 -0.873758, 1.14349, -1.21918 -1.24969, 4.26015, -2.78757 -0.541314, 0.983743, -0.019335 -0.533459, 0.994958, -0.0143956 --1.00391, 0.890723, -1.61265 --1.0524, 1.06341, -1.64271 -1.04197, 1.99608, -1.80586 -1.04547, 2.04196, -1.80931 --0.0926966, 2.76199, -2.34003 -0.957498, 2.07001, -1.80442 --0.952996, 0.890124, -1.25325 --0.972084, 0.879058, -1.23291 --1.84962, 0.906512, -1.40094 --1.03858, 1.66627, -1.12696 -0.740657, 1.01518, -0.0949408 --1.03763, 0.781906, -1.63637 -1.13805, 2.02963, -1.82707 --0.676185, 0.459886, -1.18363 --0.903169, 2.18885, -0.217661 --0.968796, 2.29468, -0.0636714 -0.825028, 3.54069, -1.95524 --0.836588, 1.80915, -0.561732 -1.34628, 1.23133, -0.444869 -0.743959, 0.897124, -0.0590094 -1.0054, 1.99601, -1.80298 --1.76398, 1.17691, -1.68989 -1.29772, 12.5775, -5.44908 -0.754516, 1.21783, -0.0520035 --1.69048, 4.45259, -1.97057 --1.74713, 4.52816, -1.68099 -0.899208, 2.66881, 0.470632 --1.39842, 0.577012, -1.78886 --2.10941, 1.06932, -2.35262 --1.114, 0.84392, -1.68616 -1.17662, 2.48333, -2.12718 --1.03524, 0.906399, -1.63792 -1.15764, 2.17492, -1.8949 -1.03221, 2.11537, -1.84117 -0.990708, 2.09651, -1.80531 -0.9612, 2.17074, -1.85405 -0.792726, 4.45233, -3.41058 -1.09198, 2.12914, -1.81388 -0.0175149, 1.99042, -1.78443 -0.870265, 4.9328, -3.25527 -0.174173, 3.35714, -2.32938 --0.915261, 0.87221, -1.23789 -0.798462, 4.77814, -0.275701 --1.21735, 0.864589, -1.77121 --1.58247, 0.940005, -1.98092 --1.09976, 0.945264, -1.68366 -0.00216768, 2.05822, -1.82622 -0.218793, 3.22375, -2.26119 -0.593865, 0.946818, -0.0378355 --0.974237, 0.855489, -1.59282 -1.12192, 2.09248, -1.78908 --1.79817, 0.399084, -1.32716 --0.510656, 1.51252, -0.720514 --0.905017, 2.18357, -0.227873 -0.0842628, 3.24375, 1.12118 -0.0157148, 2.06438, -1.85652 -0.620273, 2.7502, -1.89756 --0.822407, 2.01213, -0.808161 --0.680616, 2.06075, -0.603668 --0.902375, 1.89691, -0.577665 -1.71418, 3.98053, -0.379859 --1.11703, 0.974001, -1.67412 --0.778919, 2.1786, -0.586595 --1.36349, 2.92098, -0.587966 --1.34232, 3.22454, -0.518922 --1.05809, 2.43249, -0.403889 --2.57334, 2.66567, -1.02977 --0.848726, 2.44299, -0.310974 --0.645129, 5.10583, 0.00993536 --0.943535, 2.36903, -1.40585 --1.0234, 1.77917, -0.904645 --1.96724, 4.12664, -1.01837 -1.07022, 2.06583, -1.83406 -1.03213, 2.13876, -1.83375 -0.909725, 1.86169, -1.64075 --1.21128, 0.589583, -1.67532 -0.00197659, 2.25324, -1.95163 --1.08627, 0.936534, -1.62128 -1.19469, 2.29514, -1.98104 --1.07832, 1.06493, -1.66517 --5.86405, 2.7614, -4.83674 --1.05075, 1.18004, -0.0587 --1.04919, 0.952375, -1.6367 -0.044073, 2.26587, -1.89244 --1.14842, 1.09806, -1.71469 -0.544007, 2.6033, -1.82438 --1.81471, 1.23941, -0.814793 --0.762038, 1.72593, -0.528576 --0.739054, 2.15989, -0.607272 --0.166032, 1.24814, -0.524564 --1.42662, 3.0468, 0.140674 --1.19261, 2.9667, -0.777666 --897148, 1.31473e+06, -21655.2 --2.13128, 5.27096, -0.695035 --1.3946, 4.00193, -1.76717 --1.19501, 3.00728, -0.870272 -1.25086, 2.5653, -2.18522 -1.24021, 2.58393, -2.18236 -6.70232, 22.9054, -16.3866 -1.2654, 2.72889, -2.25097 -1.18188, 2.59147, -2.1563 -1.17707, 3.33543, -2.69466 -1.13493, 2.42703, -2.02743 -1.13125, 2.5734, -2.1286 -1.20005, 2.70571, -2.20012 -0.974579, 2.4498, -2.04734 -1.15641, 2.55172, -2.0803 -0.790418, 2.37068, -2.00048 -1.14946, 2.76259, -2.20531 -0.575625, 2.3212, -1.96979 -0.523923, 0.584597, -0.756311 --0.0470188, 2.21821, -1.96476 --0.00952542, 2.14515, -1.87897 --1.0785, 0.858384, -1.64018 --1.09622, 0.985825, -1.65953 --1.93225, 0.401742, -1.95815 --1.29345, 1.25998, -1.81799 -0.403169, 1.5252, -1.26242 --1.52222, 1.29929, -1.92126 --1.86734, 0.724109, -1.91479 --1.91203, 0.752244, -1.9338 --1.848, 0.879109, -1.90318 --1.6195, 0.820667, -1.7564 --1.52599, 1.12351, -1.72471 --1.67022, 0.777897, -1.67726 --0.990312, 1.89649, 0.469728 --0.919644, 1.85951, 0.451789 -1.19088, 2.52698, -2.08104 -0.668847, 1.99041, -1.73847 -1.07443, 2.46903, -2.01127 -1.04205, 2.52175, -2.04851 --0.214429, 0.979894, -0.023411 --1.23313, 0.971753, -1.77188 -0.470741, 2.35178, -1.74537 --1.41177, 0.162009, -1.58089 --0.612872, 1.6435, -0.74753 -0.942763, 5.61435, -3.57294 --0.936002, 0.976696, -1.54243 --1.35452, 1.09351, -1.43378 --1.27308, 1.03768, -0.735073 --1.19516, 1.44865, -0.587333 --1.11464, 0.958177, -0.51692 --0.460239, 1.87373, -0.388213 --0.860057, 2.12858, -0.219962 -0.641566, 3.04174, -2.05063 -0.95277, 6.2818, -2.83605 --1.01047, 0.900691, -0.508635 --3.16831, 6.07736, -0.545769 --1.76753, 0.384642, -1.28255 --1.29388, 1.4934, -0.603178 --1.89343, 0.705246, -0.315022 --1.24832, 0.990645, -0.743178 --0.973845, 1.01091, -0.0825165 --1.3681, 2.06032, -0.906214 --0.938382, 0.821255, -0.706851 --2.30588, 1.25106, -0.629883 -0.120562, 2.34385, -1.94961 --1.20717, 1.22546, -1.74622 --0.952653, 0.882351, -1.2484 --1.14022, 1.28539, -0.569148 --0.815886, 1.79195, -0.562176 --5.03849, 9.2983, -0.616221 --1.71828, 4.24399, -1.73545 --3.86001, 1.02783, -1.65893 --1.23064, -1.08883, -1.73558 --0.98113, -1.12094, -1.30957 --1.11855, 1.12761, -1.63854 --1.09309, -1.1708, -1.66417 --0.840847, -0.961676, -1.46703 --1.88577, 1.27031, -1.86782 --0.993352, 0.930019, -0.343145 --1.0898, 0.941369, -1.65885 --0.943946, 0.656791, -0.049865 --1.70907, 1.14546, -1.71812 --0.946936, 0.662617, -0.0478456 --0.908533, 0.944645, -1.50963 --1.53935, 1.26034, -1.98704 --0.694373, 0.715325, -1.31221 --1.05306, 1.03191, -1.58192 --1.64371, 1.53086, -2.06601 --1.3196, -1.18102, -1.83859 --1.38633, -1.311, -1.8947 --0.553219, -0.675393, -1.22019 --2.28561, -2.19503, -2.65914 --0.822102, 0.791133, -1.31598 --0.400423, -0.527696, -1.0795 --0.766454, -0.928341, -1.402 --0.53291, -0.671852, -1.19434 --1.45026, -1.19158, -1.84952 --0.5475, -0.678815, -1.19731 --1.48666, 1.14088, -1.7565 --0.79911, -0.962616, -1.40694 --1.43122, -1.37915, -1.8555 --1.4299, -1.13259, -1.78009 --1.79723, 1.24909, -1.9135 --1.59321, -1.24375, -1.89951 --0.455748, -0.615117, -1.1037 --1.49828, -1.21736, -1.82992 --0.939499, -0.853206, -1.42915 --1.44539, -1.36102, -1.83897 --2.5653, -2.12429, -2.6367 --1.54108, -1.14126, -1.8165 --1.07045, -0.874701, -1.48253 --0.977356, -1.11793, -1.36512 --1.8062, 0.371345, -1.25938 --1.65591, 0.359989, -1.80122 --0.818506, -0.906192, -1.40554 --3.41087, 1.23692, -2.90379 --3.76895, -3.82364, -3.8695 --0.679731, -0.650538, -1.22761 --0.70754, -0.763966, -1.26346 --0.878663, -1.07657, -1.24737 --1.72437, 0.801021, -1.88252 --1.6446, 0.245686, -1.79594 --0.704131, -1.02787, -1.35361 --2.90594, 1.8691, -2.62742 --1.50094, -1.12179, -1.79021 --0.983584, -1.11809, -1.31407 --0.984728, 0.911927, -1.56449 --1.13939, 1.10886, -1.68282 --1.36398, -2.11089, -1.85324 --1.74625, -1.57161, -1.73482 --1.50481, -1.65903, -1.63756 --1.92676, 1.38225, -0.780355 --1.83306, 1.20414, -1.83817 --1.81709, 0.824738, -1.7625 --0.99932, -1.12108, -1.37828 --1.47334, -2.02436, -1.75159 --0.850575, -1.36442, -0.183626 --0.860595, -1.21887, -0.186356 --0.936177, -1.15922, -0.121092 --1.00672, 0.900214, -1.26363 --1.78433, -1.5885, -1.70404 --1.49868, 1.09346, -0.558229 --1.85555, -0.698931, -1.21464 --1.12836, 1.12895, -1.69254 --1.04398, 0.880588, -1.56452 --1.16804, 1.16275, -1.677 --1.1166, 1.06925, -1.61289 --1.00238, 0.979468, -1.52012 --0.827596, 0.742184, -1.33243 --1.39251, 0.408841, -1.67141 --0.844883, 0.776874, -1.34476 --1.63789, 0.242323, -1.81261 --4.70455, 2.53514, -4.00397 --1.35011, -1.17341, -1.83286 --1.669, 1.0279, -1.87334 --1.66017, 0.851633, -1.83663 --1.70286, 1.07923, -1.88553 --1.68858, 1.07722, -1.87233 --1.49823, -1.22991, -1.88839 --1.59398, 1.14976, -1.80772 --1.61665, 1.19272, -1.82156 --1.38287, -1.23486, -1.79057 --1.52986, -1.38762, -1.89964 --1.68423, 1.19994, -1.82334 --1.57529, -1.22962, -1.86627 --1.31887, -1.868, -1.85156 --1.45471, -1.88496, -1.86167 --1.34309, -1.87605, -1.8033 --1.28022, -2.04883, -1.81682 --1.29749, -2.06101, -1.80404 --0.912544, -1.30942, -0.177556 --1.06587, 0.877436, -1.6026 --1.36681, 1.31334, -1.88387 --1.06089, 0.948319, -1.51207 --1.57642, 1.13612, -1.8735 --0.940385, -0.96261, -1.55153 --1.57553, 0.377427, -1.77467 --2.01584, 0.541013, -2.03379 --1.66552, 0.208524, -1.80329 --1.59475, -1.43637, -2.0203 --0.571818, 0.374375, -1.0805 --1.38182, -1.19085, -1.78593 --1.35931, -1.21447, -1.77245 --1.36466, -2.13386, -1.85061 --0.947014, -1.14223, -1.34472 --1.73692, -2.08644, -1.78373 --0.946554, -1.14571, -0.122488 --1.02965, 0.872576, -1.54987 --1.0639, 0.931533, -1.5294 --2.55252, 0.425799, -2.39315 --1.03429, -1.07269, -1.59895 --1.9103, -1.37741, -1.98531 --0.979776, -1.11521, -1.36615 --1.67679, -2.08955, -1.68259 --1.53852, 1.09925, -0.769557 --0.889732, -1.28148, -0.18136 --1.2217, 1.13707, -1.67164 --1.19133, -1.38632, -1.51591 --1.16836, -1.27031, -1.42755 --1.48555, 1.13513, -0.722057 --0.837527, -1.19232, -0.192245 --1.67308, 0.16767, -1.81339 --0.932416, -1.17532, -1.52667 --0.948383, -1.0758, -1.2873 --0.847601, -1.07169, -1.2294 --1.14332, 0.941718, -0.675736 --1.36836, 1.59154, -0.604602 --1.14583, 1.23855, -0.036551 --1.32365, -2.10609, -1.82041 --1.87314, 0.449706, -1.26265 --1.27051, 0.968628, -0.76213 --0.959179, 0.852846, -1.55184 --0.977416, 0.914383, -1.51556 --0.999057, 0.89671, -1.48238 --1.53502, 0.827258, -1.77108 --2.51226, 1.40829, -2.42972 --1.63891, 0.499799, -1.78585 --1.60402, 1.07533, -1.81618 --1.71712, 0.16077, -1.79658 --0.896245, -0.956875, -1.48789 --1.34421, -2.07029, -1.80792 --1.45032, -2.07953, -1.82364 --0.959455, -1.15639, -1.35649 --1.59555, -2.04082, -1.41971 --0.978, 0.918805, -1.51441 --1.01247, 0.88888, -1.5236 --1.59697, 0.267136, -1.78911 --0.963908, -0.976624, -1.56571 --1.64347, 1.12981, -1.8437 --1.63539, -1.19551, -1.80209 --0.846587, -1.23134, -0.188394 --0.913898, -1.25819, -0.174114 --1.57884, 1.02504, -1.80123 --1.70885, 0.169297, -1.79125 --1.34995, 0.998187, -0.72989 --0.887407, -1.24258, -0.179175 --0.939482, 0.896699, -1.53202 --1.05186, -1.06495, -1.61217 --1.711, 1.17479, -1.75884 --1.90848, 1.2449, -0.820122 --0.948586, 0.587191, -0.0393219 --1.65517, 0.389499, -1.80677 --1.66213, 0.281248, -1.80568 --1.42539, 1.05796, -0.752098 --1.26715, -1.33111, -1.48965 --1.75923, -1.60668, -1.70236 --0.702273, -1.22071, -0.205285 --1.87556, 0.430927, -1.24475 --1.42445, 1.70117, -0.813359 --1.27048, 1.299, -0.00390948 --1.76301, 0.417693, -1.17282 --1.86248, 0.640835, -1.18456 --0.960285, 0.911573, -1.53074 --1.3475, 1.30275, -1.84575 --1.35501, 1.28334, -1.80084 --1.61791, 0.362745, -1.79994 --1.56583, 0.477198, -1.74982 --1.61248, 1.13186, -1.83476 --1.62514, 1.07959, -1.83581 --1.504, 0.996755, -1.74609 --1.62901, 1.11549, -1.83543 --1.33893, -1.17186, -1.83963 --1.39857, -1.19415, -1.858 --1.39251, -1.22738, -1.85325 --1.41672, -1.14781, -1.81626 --1.0044, -1.04884, -1.57299 --1.37771, -1.22114, -1.8125 --1.44389, -1.163, -1.8166 --1.3802, -1.09138, -1.76206 --1.36594, -2.10968, -1.84891 --1.71656, -1.55705, -1.70333 --1.08642, 1.03573, -0.0483519 --1.36109, 1.31171, -1.83631 --1.42918, 1.32982, -1.87853 --1.62108, 0.246822, -1.79769 --1.72881, 0.501326, -1.85598 --1.56046, 1.14265, -1.7413 --1.31991, -1.30407, -1.82562 --1.4834, -1.2031, -1.83444 --1.38373, -2.12084, -1.83926 --0.924183, -1.11552, -1.27812 --2.68118, 0.64586, -2.46876 --1.66983, 0.165275, -1.81393 --1.63945, 1.16993, -1.82064 --1.66633, 1.1616, -1.83447 --0.968466, 0.889478, -0.682461 --1.68781, 0.256274, -1.83226 --1.62983, -1.45142, -2.07076 --0.95801, -1.16367, -1.34722 --1.00296, -1.13051, -1.32593 --1.82443, -2.21938, -1.91401 --1.07642, -1.28706, -1.37017 --0.882584, -0.921787, -1.47194 --1.60066, -1.17938, -1.77424 --1.06555, 1.71233, -1.17918 --0.921019, 0.652797, -0.0552872 --1.25656, -1.83683, -1.64977 --0.873328, -1.228, -0.179953 --1.87956, 0.675895, -1.16176 --1.70963, -0.630967, -1.159 --1.80986, 1.04018, -0.610693 --1.83211, 0.252438, -0.614253 --0.968329, 0.953, -1.45638 --1.59598, 0.183764, -1.78721 --4.22777, 1.87274, -3.56167 --1.68235, 0.262344, -1.80703 --0.971085, -1.08371, -1.57146 --1.70481, -1.21338, -1.8497 --1.11654, -1.26055, -1.44261 --1.73564, 1.17526, -1.81285 --1.09168, -1.099, -1.66711 --0.913255, -1.18384, -0.110657 --0.959124, 0.858948, -1.55713 --0.992157, 0.918584, -1.437 --0.99309, 0.916351, -1.41317 --1.00944, 0.978149, -1.43111 --1.97732, 0.396753, -2.00988 --0.996961, 0.905758, -1.24716 --1.72408, -1.55111, -1.72065 --0.818275, -1.1994, -0.198101 --1.83927, 0.683641, -1.28365 --0.782385, 0.88477, -1.30658 --0.943745, -1.19983, -1.53063 --1.31979, -1.90973, -1.69926 --1.71226, -1.51484, -1.68165 --1.13591, 0.911599, -0.757384 --1.5888, 0.35258, -1.77987 --1.61265, 0.321606, -1.79503 --43.5962, 12.443, -30.0244 --1.61849, 0.235814, -1.79858 --1.37088, -1.20597, -1.86467 --1.74149, -1.68489, -2.16378 --1.31429, -1.30243, -1.81656 --0.98968, -1.19226, -1.36723 --0.91086, -1.19093, -0.111931 --1.12746, 1.07885, -0.0396875 --0.946739, 0.904523, -1.49262 --1.29805, -1.20165, -1.83369 --0.985771, 0.905456, -1.21954 --1.34112, -2.03665, -1.79647 --0.945002, 0.854895, -1.56959 --0.997613, 0.895983, -1.55145 --0.970574, 0.917692, -1.50376 --0.968305, 0.951583, -1.38707 --1.64035, 0.315415, -1.79059 --0.993601, 0.899281, -1.3075 --0.967432, 0.894139, -1.25512 --0.933149, -1.134, -1.42619 --1.31863, -1.60228, -1.57823 --1.80753, 0.399497, -1.23893 --0.935967, 0.587329, -0.0424791 --3.60437, -3.4038, -3.65713 --1.05035, -1.27955, -1.52182 --1.78759, -1.21748, -1.73025 --1.57273, -2.09331, -1.78562 --0.959715, 0.88504, -0.674322 --1.3709, -1.23851, -1.87394 --1.76084, -1.52156, -1.62272 --1.84383, -1.56697, -1.61116 --1.87466, -2.34518, -1.76706 --0.958013, 0.581039, -0.0375031 --1.9718, 0.168588, -1.71941 --1.4859, -2.11134, -1.80814 --1.16374, -1.25676, -1.42075 --1.79187, 0.657572, -0.11765 --1.23289, 1.36647, -0.561738 --1.77831, 0.664086, -0.148924 --1.61105, -1.43019, -1.52825 --1.82978, 0.436533, -1.07037 --0.907456, 0.941885, -1.50826 --0.974249, 0.909473, -1.51201 --0.971734, 0.916429, -1.49536 --1.62256, 0.931148, -1.82251 --1.63046, 0.471184, -1.80565 --1.62459, 0.47834, -1.78808 --1.62675, 0.144069, -1.81567 --1.63265, 0.358651, -1.78653 --1.70794, 0.411479, -1.82378 --0.954024, 0.893438, -1.27587 --3.92507, 0.456584, -3.2889 --1.00636, 0.908687, -1.28068 --0.959179, 0.965802, -1.55062 --2.4749, 0.473738, -2.34219 --1.72926, 0.135643, -1.80866 --1.39927, -1.77028, -1.33169 --1.3359, 1.58227, -0.602222 --1.79824, 0.418469, -1.89454 --0.956259, 0.955704, -1.22721 --0.985009, -1.23317, -0.105414 --1.04576, 0.715075, -0.143426 --1.59839, 0.133439, -1.63046 --0.967717, 0.907633, -1.51069 --1.61622, 0.939579, -1.81981 --1.55955, 0.502692, -1.75802 --1.67591, 0.143543, -1.86542 --1.6709, 0.210071, -1.79719 --1.7396, -1.15664, -1.7927 --1.2837, -1.1495, -1.78949 --0.951503, 0.591519, -0.0386438 --1.29827, -1.13382, -1.80586 --1.32158, -1.53422, -1.52176 --0.623689, 1.64924, -0.747871 --1.18707, 1.0781, -0.0182935 --0.156089, 0.936108, -0.0467936 --1.77756, 0.146329, -1.79365 --0.95896, -1.16834, -1.4509 --1.23246, 0.926892, -0.719578 --1.45075, -2.18741, -1.85032 --1.64735, -1.37902, -1.52975 --0.98175, 0.91384, -0.456847 --0.970114, 0.987691, -0.0645377 --1.10574, 0.670294, 0.00314176 --0.954808, -1.32113, -0.166198 --1.0405, 0.886373, -0.710695 --0.970526, -1.25719, -0.0936774 --1.84044, -0.00706801, -0.481863 --1.66577, 1.32542, -1.89108 --2.96673, 1.48668, -2.79232 --1.33159, -1.21338, -1.86983 --1.33927, -1.21933, -1.84022 --1.74499, -1.2804, -1.878 --1.54566, -2.18601, -1.92909 --1.85319, -1.63414, -1.80163 --0.896106, 0.894357, -1.44933 --1.38833, -1.94341, -1.80333 --1.69997, -1.50863, -1.70135 --1.32824, 1.04257, -1.64551 --1.87583, 1.32395, -1.95511 --1.6293, 1.22079, -1.7817 --1.82666, 1.24385, -0.816628 --1.85717, 1.05543, -0.82312 --1.82313, 1.2204, -0.803933 --0.872327, 1.84917, -0.574111 --1.9206, 1.25667, -0.522651 --1.82815, 1.13083, -1.95232 --1.40981, 1.07825, -1.55982 --1.63954, 1.03717, -0.545184 --1.44449, 1.1197, -0.511422 --1.9258, 1.29706, -1.79849 --1.69613, -1.28952, -1.83683 --1.74599, -1.19023, -1.69464 --1.11992, 1.0012, -0.69775 --1.81225, 0.453578, -0.562504 --1.64934, 1.15873, -1.60228 --1.73488, -1.43586, -1.66297 --1.36994, 0.994986, -0.0631858 --0.985299, 1.07396, -0.0851359 --1.85859, -1.19701, -1.78858 --0.954757, 0.872289, -0.707135 --1.84925, 0.702, -0.335164 --1.70701, -1.21519, -1.82796 --1.61993, -1.91728, -1.70442 --1.14632, 1.25226, -0.0317939 --1.81644, 1.32248, -0.757157 --0.978461, 0.901891, -0.474809 --0.984091, 0.916083, -0.462821 --1.80145, 0.713951, -0.220129 --1.82237, -2.23235, -1.90386 --1.25332, 1.35559, -0.401025 --1.54956, 1.21851, -0.726764 --1.16372, 0.946289, -0.479709 --1.87769, -2.19074, -1.9312 --1.84982, 0.38332, -1.12934 --0.817424, 1.53434, -0.756406 --1.82681, 1.306, -0.583053 --1.19116, 0.93767, -0.511361 --0.579161, 0.747851, -0.393112 --1.19085, 1.29727, -0.0253081 --1.35424, 1.07112, -0.75801 --1.85457, -0.683319, -1.21659 --1.2411, 1.40755, -0.430635 --1.81903, 0.25321, -0.908897 --1.75914, 1.07782, -0.799192 --0.984765, 0.896754, -0.32777 --1.04797, 1.68258, -1.08574 --1.68028, 1.06573, -0.788687 --0.862152, 0.883255, -0.0625925 --1.67837, 0.167743, -1.73108 --0.940652, -1.14673, -1.32924 --1.79589, -1.54293, -1.65503 --1.82406, 1.0793, -0.822792 --1.66506, 1.16281, -0.780673 --1.64056, 1.24305, -0.56963 --1.19751, 1.07441, -0.0183592 --1.72044, -1.3008, -1.84154 --1.50306, -1.7237, -1.66705 --1.79745, -1.54357, -1.68191 --1.6031, 1.14484, -0.771438 --1.34773, 1.58713, -0.618707 --1.16276, 1.06291, -0.0256832 --1.70761, -1.06977, -1.56809 --1.06834, 0.98787, -0.692082 --1.57238, 1.20655, -0.579258 --0.953411, 0.684023, -0.0515283 --1.58653, -1.16394, -1.82475 --1.7431, -1.31983, -1.87809 --1.06563, 1.65581, -1.13838 --1.5968, -1.176, -1.83668 --1.90782, 1.29681, -0.826834 --1.19302, 1.30347, -0.0252859 --1.87007, 0.827582, -1.80277 --1.06646, 1.03592, -0.0487872 --1.13901, 0.932552, -0.690789 --0.976646, 0.667535, -0.0391633 --1.59573, 1.04847, -0.502528 --1.81483, 0.821583, -1.75839 --1.70165, -1.1677, -1.82369 --1.79164, 0.60694, -1.12566 --1.71967, -1.26689, -1.83765 --1.06411, 1.65932, -1.10551 --1.06513, 1.65739, -1.139 --1.72306, -1.44718, -1.69714 --1.71032, -1.99076, -1.7565 --1.63156, 1.04578, -0.779244 --0.981933, 0.91427, -0.463917 --1.83066, -2.26912, -1.91149 --1.71529, 1.27816, -0.745172 --1.42641, 1.155, -0.555517 --1.90122, -1.67128, -1.76013 --1.84706, -1.56735, -1.62084 --1.7074, 0.408466, -1.26446 --1.76361, 0.676617, -0.126655 --1.31908, -1.30614, -1.86675 --2.67306, -3.35925, -3.3251 --1.64471, -1.22484, -1.84583 --1.73972, -1.25483, -1.8611 --0.970794, 0.592572, -0.0378916 --1.33725, -1.17816, -1.83938 --1.35395, -1.38871, -1.90433 --1.0237, 1.64555, -1.09767 --1.81665, 0.93816, -0.973703 --0.958644, 0.889758, -0.331915 --1.18977, 1.08794, -0.025385 --1.32496, -1.23951, -1.87664 --1.76833, -1.30344, -1.89105 --1.80901, -2.31521, -1.90294 --1.80324, 0.39674, -1.18075 --1.16555, 1.28651, -0.0357356 --1.01854, 0.982144, -0.055254 --0.979173, 1.02399, -0.0552788 --0.976018, 0.589772, -0.0346648 --1.23187, 1.32827, -0.00673116 --1.09685, 1.27188, -0.0411403 --1.03887, 1.02626, -0.0605123 --1.71689, -1.27461, -1.83914 --0.991698, 1.1617, -0.0562384 --1.8553, -0.0353421, -0.516441 --0.985207, 0.988079, -0.102105 --1.66986, -1.634, -2.18034 --4.22455, -6.39448, -5.3015 --2.11428, -2.25205, -2.53544 --3.306, -4.01507, -3.83085 --1.80869, 0.609457, -1.14022 --5.42281, -7.12533, -6.26587 --2.20244, -2.23643, -2.56023 --1.80836, 0.052304, -1.28839 --1.38549, -1.02921, -1.72341 --1.39125, 1.61709, -0.609957 --1.14492, 1.0655, -0.0473145 --0.98922, 1.02919, -0.0484614 --1.78128, 0.649895, -1.29676 --0.977849, 1.63488, -1.0342 --1.16613, 1.32234, -0.412742 --1.02562, 1.02188, -0.0647111 --1.04838, 1.06647, -0.0535587 --1.06232, 1.05188, -0.0442809 --1.73188, -1.12257, -1.65402 --1.39473, 1.66237, -0.774358 --1.55792, -1.06364, -1.75482 --1.89366, -1.9318, -2.2753 --1.80006, 0.370379, -1.25564 --1.17412, 1.27401, -0.0153783 --1.08574, 1.05721, -0.0543167 --1.11831, 1.07869, -0.031506 --0.944645, 0.595357, -0.0443631 --1.68583, -1.34777, -1.96291 --1.11227, 1.32039, -0.606946 --1.29176, 1.50908, -0.622807 --1.8333, 1.05518, -0.525686 --1.18587, 1.21, -0.0203173 --0.968941, 0.909014, -0.0403363 --0.93508, 0.90312, -0.0393192 --0.947479, 0.59336, -0.0419493 --1.16853, 1.37942, -0.621584 --0.948924, 0.909446, -0.353804 --1.1046, 1.29256, -0.024025 --1.05643, 1.03937, -0.0428101 --1.77488, 1.19317, -0.794765 --1.22553, 1.32642, -0.574221 --1.78563, 1.21245, -0.51851 --0.97967, 0.902974, -0.472994 --0.950262, 0.900965, -0.464831 --1.78645, 0.790175, -0.272491 --1.06773, 1.07922, -0.0352031 --0.974542, 1.02237, -0.0580998 --0.980539, 1.10883, -0.158843 --1.95017, -1.76528, -2.26042 --1.67471, -1.14011, -1.80985 --1.60134, -1.21958, -1.80281 --1.59932, -1.24821, -1.79959 --1.68668, -1.14712, -1.79899 --1.80864, 0.598512, -1.21894 --1.85104, 0.65905, -1.18273 --1.81026, 0.608429, -1.14341 --1.60171, -1.30086, -1.84363 --1.69561, -1.14641, -1.804 --0.960153, 0.905993, -0.355747 --0.949845, 0.909945, -0.354608 --1.16236, 1.287, -0.0346881 --1.19062, 1.21483, -0.0180304 --1.79599, -1.21059, -1.73528 --1.80342, 0.616628, -1.11818 --1.78109, 0.431297, -1.18975 --1.73418, -1.36096, -1.96126 --1.67082, -1.12341, -1.79773 --1.61468, -1.22465, -1.81519 --1.59007, -1.20945, -1.78009 --1.80321, 0.625693, -1.30254 --1.8002, 0.639267, -1.28968 --1.7917, 0.40253, -1.23073 --1.74786, -1.202, -1.85622 --1.67267, -1.23572, -1.81664 --1.8082, 0.641013, -1.29418 --1.80801, 0.161256, -0.595253 --1.83097, 1.08164, -0.927258 --1.61899, -1.1503, -1.80401 --1.69118, -1.18699, -1.80282 --1.6973, -1.19058, -1.80747 --1.59204, -1.29764, -1.78517 --1.81316, 0.615972, -1.21925 --1.81126, 0.5937, -1.18068 --1.62543, -1.12183, -1.79273 --1.80523, 0.614949, -1.2119 --1.69945, -1.12118, -1.79519 --1.62307, -1.1134, -1.75038 --1.56857, -1.20992, -1.74815 --1.72369, -1.2607, -1.83825 --1.76318, -1.11093, -1.78574 --1.63251, -1.16743, -1.74284 --1.91815, -1.43461, -1.99658 --14.2646, -29.9404, -21.5351 --15.1123, -31.7065, -22.3747 --1.71018, -1.47532, -1.83375 --1.78635, 0.0613903, -1.24553 --1.80575, 0.620343, -1.17053 --1.70219, -1.46726, -1.96305 --1.64657, -1.37074, -1.86709 --1.6216, -1.19863, -1.77048 --1.60857, -1.23484, -1.74522 --1.87755, 0.012053, -1.34682 --1.61108, -1.26478, -1.76249 --2.14571, 1.54618, -0.626808 --1.80904, 0.790146, -0.2639 --1.80726, 0.424671, -1.20663 --1.81421, 0.621647, -1.17475 --1.82137, -1.20642, -1.86466 --1.78814, 0.462634, -1.19627 --1.80856, 0.419073, -1.17501 --1.83519, -1.24204, -1.88592 --1.81009, 0.803051, -0.265315 --1.69125, -1.15299, -1.80596 --1.78908, -1.24261, -1.82259 --1.81045, 0.0737276, -1.30876 --1.80737, 0.0441383, -1.26515 --1.51668, -1.3327, -1.50622 --1.81028, 0.382532, -1.24678 --1.80545, 0.132327, -0.685831 --1.81718, 0.805337, -0.262766 --1.79609, 0.944179, -0.0926027 --1.48958, -1.24609, -1.46301 --1.80288, 1.04003, -0.739678 --1.79682, 0.711491, -0.188885 --1.82609, 1.30862, -0.580753 --1.84392, -0.0522921, -0.539331 --1.81297, 0.783499, -0.271869 --1.81044, 0.625942, -0.162444 --1.71393, -1.16661, -1.78456 --2.24159, -2.25348, -2.56317 --7.6304, -12.5307, -10.5391 --5.03642, -8.81254, -7.08801 --4.98535, -8.71247, -6.85436 --5.34031, -9.69338, -7.4148 --6.05506, -11.5119, -8.49878 --1.80782, 0.0725644, -1.26378 --1.8317, 0.4165, -1.24938 --1.71516, -1.71416, -1.83872 --1.80906, 0.423845, -1.20687 --1.8128, 0.615704, -1.21918 --1.81111, 0.451583, -1.20339 --1.75265, -1.81055, -1.84573 --1.71195, -1.50488, -1.69 --1.7184, -1.51521, -1.6936 --1.80283, 0.612657, -1.17934 --1.81116, 0.59502, -1.17671 --1.80928, 0.621504, -1.17215 --1.69209, -1.76832, -1.73654 --1.76507, -1.363, -1.85358 --1.92683, -1.55233, -2.01219 --1.81106, -1.2181, -1.77008 --1.87427, -1.50341, -1.94124 --1.81563, 0.387085, -1.25345 --1.78528, -1.58259, -1.64145 --1.80546, 0.174815, -0.60079 --1.79788, 0.610964, -1.20719 --1.80072, 1.06788, -0.826064 --1.80504, 0.278487, -0.736716 --1.80524, 0.169427, -0.651655 --1.82554, 0.637834, -0.0823654 --1.80999, 0.594764, -1.19142 --1.80726, 0.170938, -0.654721 --1.80699, 0.119828, -0.569812 --1.81006, 0.754306, -0.214766 --1.81678, 0.629062, -0.190108 --1.80013, 1.07948, -0.764517 --1.80451, 0.169116, -0.652297 --1.81442, 0.788259, -0.247682 --1.80019, -0.729988, -1.20452 --1.80919, 0.162624, -0.648639 --1.79637, 0.619436, -0.249986 --1.80236, 0.907184, -0.0923839 --1.83884, -0.0163545, -0.522126 --2.33376, -2.36464, -2.69831 --1.84891, 0.429201, -1.23741 --1.81646, 1.02543, -1.24541 --1.79157, -1.161, -1.73875 --1.80297, 0.0506958, -1.28228 --1.70105, -1.4776, -1.67508 --1.68385, -1.73101, -1.71962 --1.81392, 0.938423, -0.973769 --1.55407, -0.820989, -0.366842 --1.80685, 0.11897, -0.559736 --1.78094, 0.519695, -1.16087 --1.79736, 0.552759, -1.16807 --1.83113, 1.0109, -0.728628 --1.80262, 1.07807, -0.644183 --1.81852, 0.256204, -0.493857 --1.73778, 0.0142833, -0.51909 --3.95064, -5.44207, -5.07375 --1.88877, -1.80329, -1.96357 --1.58553, -0.873884, -0.362249 --3.61406, -4.63612, -4.34477 --4.28968, -5.92659, -5.30117 --1.63908, -1.54572, -1.65722 --1.80997, -0.532432, -1.1044 --1.89824, -1.7713, -1.79525 --1.80216, 0.0151867, -0.497773 --1.80499, 0.713059, -0.222445 --1.82798, 0.759421, -0.180594 --1.47677, 1.0135, -0.00165324 --1.78581, -0.523688, -0.3784 --1.76708, -0.447455, -0.387411 --1.50837, -0.911572, -0.416119 --1.56308, -0.882638, -0.288851 --1.8087, -1.18475, -1.67068 --2.41901, -2.77875, -2.73406 --1.82013, -1.01853, -0.869482 --1.80653, 0.119966, -0.559241 --1.75121, -1.51821, -1.59996 --1.80961, 0.614332, -1.21491 --1.81127, -0.593545, -0.403079 --1.79622, 0.126015, -0.640748 --1.804, 0.119831, -0.57033 --1.80948, 0.099151, -0.637313 --1.81179, -0.00567992, -0.496877 --1.80528, 0.717564, -0.0701621 --1.81188, -0.637765, -0.938876 --1.80554, 0.108522, -0.642075 --1.84332, 0.667741, -0.311652 --1.79444, 0.363047, -1.24974 --1.81568, 1.07696, -0.927302 --1.65615, -1.44627, -1.62664 --1.8178, -1.73826, -1.85954 --1.79063, -1.56171, -1.74722 --1.82312, 0.464037, -1.20414 --1.80639, 0.010491, -0.568802 --1.73392, -2.00677, -1.79754 --1.79897, 0.131625, -0.684044 --1.90371, -1.76667, -1.75258 --1.79781, 0.121941, -0.644066 --1.46377, -0.863222, -0.463845 --1.82206, -1.28828, -0.636379 --1.47827, -0.889045, -0.461754 --3.70811, -6.09771, -4.78515 --3.57228, -5.91266, -4.62609 --1.63593, -1.93859, -1.69854 --1.60102, -1.79661, -1.61489 --1.64812, -1.9343, -1.6751 --1.82138, 0.693301, -1.19937 --3.94798, -6.02313, -5.17947 --3.73183, -5.98479, -4.85338 --1.66118, -1.89748, -1.73301 --1.64251, -1.86325, -1.68635 --1.67897, -2.01231, -1.757 --1.5874, -1.79185, -1.58466 --1.81057, 0.0403527, -0.669598 --1.81566, -0.580298, -0.394113 --1.69529, -1.44286, -1.53427 --1.65441, -1.89452, -1.68917 --1.81769, 0.486044, -1.31226 --1.81987, 0.646334, -1.30141 --1.81242, 0.591757, -1.19509 --1.81082, 0.884032, -1.12808 --1.70579, -1.01741, -0.38634 --1.81618, 0.633984, -1.14978 --1.43476, -0.807581, -0.076083 --1.80634, -1.37903, -1.25987 --1.86417, -1.33303, -0.641052 --1.68801, -0.58238, -0.189614 --3.06149, -4.50124, -3.73066 --1.68278, -2.07684, -1.77832 --1.86819, 0.65341, -1.24164 --1.33971, -0.961444, -0.370208 --1.26607, -0.812381, -0.278135 --1.221, -0.883697, -0.110977 --2.93695, -4.23901, -3.5353 --1.80526, 0.59235, -1.17609 --1.80789, -0.385341, -0.354385 --1.55806, -0.829392, -0.365771 --1.24904, -0.849603, -0.385945 --1.20099, -0.746979, -0.305539 --1.40052, -0.780269, -0.100357 --1.66389, -2.02023, -1.77623 --1.68034, -1.9735, -1.7446 --1.58671, -1.90128, -1.66328 --1.8169, 0.473951, -1.19939 --1.80881, 0.00965456, -0.549349 --1.29488, -0.76751, -0.43881 --1.16412, -0.748468, -0.400722 --1.80859, -0.343626, -0.315422 --1.22401, -0.767976, -0.295711 --1.39301, 0.175586, -0.961793 --1.80851, 0.164269, -0.592364 --1.40417, -0.817611, -0.466972 --1.46772, -0.857289, -0.437876 --1.48004, -0.937352, -0.434064 --1.27018, -0.815427, -0.196523 --1.19523, -0.847465, -0.13193 --1.30136, -0.789462, -0.463298 --1.51092, -0.96781, -0.434838 --1.54439, -0.849592, -0.369945 --1.25587, -0.693331, -0.403843 --1.92067, 0.424737, -1.26507 --1.23165, -0.705047, -0.448703 --1.8204, -0.407482, -0.112388 --1.72005, -1.17831, -0.607456 --1.8147, 1.41226, -1.90254 --1.67977, -2.10977, -1.79238 --1.78434, -2.30176, -1.85357 --1.79586, -0.906922, -0.949969 --1.2755, -0.827178, -0.276257 --1.29559, -0.8279, -0.184396 --1.26954, -0.482686, -0.0132953 --1.68961, -2.0449, -1.73097 --1.80845, 0.469005, -1.19321 --1.81388, -0.116554, -0.544001 --1.32572, -0.997933, -0.410002 --1.54058, -0.822499, -0.313555 --1.28637, -0.838689, -0.271813 --1.2867, -0.821609, -0.249244 --1.29837, -0.831223, -0.182231 --1.24045, -0.823787, -0.111155 --1.80484, 0.159876, -0.595687 --1.48487, -0.910944, -0.467758 --1.48115, -0.92166, -0.43272 --1.3283, -0.95307, -0.403248 --1.80824, -0.84983, -0.289602 --1.79871, -0.471617, -0.283342 --1.55505, -0.825309, -0.310513 --1.43513, -0.846848, -0.465468 --1.489, -0.976838, -0.46116 --1.53712, -0.93043, -0.447899 --1.45777, -0.866504, -0.335801 --1.24427, -0.894813, -0.0942983 --1.46002, -0.809349, -0.335394 --1.70916, -2.19401, -1.76265 --1.81225, 0.61551, -1.1418 --1.80789, -1.3041, -0.798852 --1.82195, -0.11502, -0.544342 --1.80823, -0.65481, -0.281732 --1.52613, -0.815475, -0.293377 --1.45512, -0.808678, -0.145172 --1.1427, -0.933046, -0.0217765 --1.78943, -2.38502, -1.94448 --1.64173, -2.0279, -1.64091 --2.46488, 2.39466, -2.6839 --1.79545, 0.585217, -1.15242 --1.81413, -0.656634, -0.280645 --1.53772, -0.808334, -0.299447 --1.46803, -1.41721, -0.272076 --1.52259, -0.808625, -0.0891157 --1.22362, -0.775516, -0.103308 --1.13314, -0.925769, -0.0330757 --1.80663, -0.717033, -1.14142 --1.38969, -0.869496, -0.46599 --1.81199, -0.472964, -0.282114 --1.20225, -0.843711, -0.131452 --1.82874, 0.44402, -1.3328 --1.46662, -0.941387, -0.437347 --1.5153, -0.858894, -0.379533 --1.80639, -0.34342, -0.178318 --1.45945, -0.787342, -0.0912226 --1.43807, -0.810703, -0.0720918 --1.48187, -0.946456, -0.443472 --1.72337, -1.91194, -1.73997 --1.71822, 0.793488, -1.72208 --1.6738, -2.03286, -1.70372 --1.70473, 0.751605, -1.6905 --1.49855, 1.27415, -1.80167 --1.53959, 1.31742, -1.83339 --1.59302, 1.23305, -1.80765 --1.92865, -2.51587, -2.01931 --1.58564, 1.29393, -1.81275 --1.6308, 1.13973, -1.75931 --1.64111, 1.29479, -1.82997 --1.60549, 1.26073, -1.79326 --1.77489, 1.33357, -1.90866 --1.69678, 1.22901, -1.80135 --1.70594, 1.13439, -1.71593 --1.7436, 1.15449, -1.74287 --1.78412, -1.36666, -1.23379 --1.31831, -1.12266, -0.265676 --1.25935, -1.05794, -0.277109 --1.53031, -1.83545, -1.51181 --1.66372, 1.41551, -1.91121 --1.93606, 1.77803, -2.17867 --1.75288, 1.25171, -1.82173 --1.81489, -1.24548, -0.563161 --1.41878, -0.863757, -0.430324 --1.80909, -0.414812, -0.367251 --1.55412, -1.34462, -0.248872 --1.75069, -0.142916, -0.538263 --1.56516, -0.84355, -0.346257 --1.53869, -0.808906, -0.308945 --1.29254, -1.07986, -0.276211 --1.52461, -0.937393, -0.131727 --1.19812, -0.949708, -0.19615 --1.1449, -0.918631, -0.175579 --1.2087, -0.863032, -0.122876 --2.50577, 2.56472, -2.7496 --1.11051, -0.784763, -0.426631 --1.56818, -0.855338, -0.365394 --1.80782, -0.678025, -0.310097 --1.56491, -0.84398, -0.34135 --1.22076, -0.802893, -0.372836 --1.53575, -0.841441, -0.331791 --1.27007, -0.826076, -0.255969 --1.44273, -0.803988, -0.173072 --1.46365, -0.815383, -0.144584 --1.25389, -0.797193, -0.20502 --1.11387, -0.910747, -0.0529656 --1.4572, -0.8598, -0.455906 --1.72494, 0.201167, -0.521867 --1.82903, -0.655066, -0.145431 --1.39057, 1.19839, -1.77654 --1.489, 1.21471, -1.81667 --1.46126, 1.22304, -1.76214 --1.74605, 1.67682, -2.11093 --1.83651, 0.966205, -0.988163 --5.63905, 5.40995, -1.1158 --1.36516, -0.956272, -0.131615 --1.47128, 1.28779, -1.81533 --1.56071, 1.24549, -1.81824 --1.81472, 0.827652, -1.67047 --1.51527, -0.862455, -0.469035 --1.45003, -0.804601, -0.147858 --1.24213, -0.853902, -0.119062 --0.957462, -0.112104, -0.0344644 --1.54083, 1.07944, -1.69967 --1.7548, 1.22248, -1.78406 --1.7997, 0.392724, -1.2333 --1.54788, -0.960902, -0.465695 --1.91915, -0.88088, -0.283133 --1.33182, -1.12873, -0.279577 --1.27178, -1.13197, -0.279698 --1.1884, -0.96762, -0.116476 --0.96209, -0.00394696, -0.0283403 --0.957561, -0.0482301, -0.0341536 --0.946252, -1.05966, -0.0368655 --1.9641, 1.68746, -2.12809 --1.43306, -0.992277, -0.460238 --0.958666, -0.0739016, -0.032339 --1.90412, 1.45539, -1.96638 --1.77123, -1.20274, -0.611961 --1.52398, 0.0637726, -0.48831 --1.43271, -0.844829, -0.465741 --1.44915, -0.886266, -0.441242 --1.47085, -0.915907, -0.409829 --1.26008, -1.00456, -0.381985 --1.86999, 0.41079, -1.16091 --1.80919, 0.0391919, -0.575117 --1.44691, -0.903724, -0.4622 --1.59189, -0.877862, -0.373906 --1.51389, -0.792097, -0.0760334 --1.75399, 0.60518, -1.25342 --1.82164, 0.662657, -1.11936 --1.62417, -0.834298, -0.332274 --1.39217, 1.20798, -1.83702 --1.47199, 1.14352, -1.79282 --1.57626, -2.02835, -1.69216 --1.48367, 1.27926, -1.82256 --1.47611, 1.2948, -1.81716 --1.79718, 1.6921, -2.15096 --1.62713, -2.0922, -1.70447 --1.5886, 1.13532, -1.76638 --1.67036, -2.05704, -1.70774 --1.66889, 1.22095, -1.77035 --1.81189, -0.67231, -0.361712 --1.28311, -1.07773, -0.268718 --1.41815, 1.26116, -1.82339 --1.50654, 1.25925, -1.84413 --1.59378, -2.0506, -1.71097 --1.69511, -2.05345, -1.7344 --1.65463, -2.07396, -1.70779 --1.83843, 0.836058, -1.79281 --1.82578, 0.474065, -1.22162 --1.71758, -1.57865, -1.70579 --1.8377, 0.857787, -1.78844 --1.82071, 0.866043, -1.7733 --1.74273, 0.524201, -1.12802 --1.4301, -0.903757, -0.456809 --1.82597, 0.811193, -0.265817 --0.957427, -0.995782, -0.0322111 --0.951322, -0.972874, -0.0380737 --0.860193, -1.12878, -0.0354641 --1.81473, -1.29654, -0.617658 --1.33162, -0.922914, -0.445808 --1.81191, -0.678909, -0.175988 --1.1189, -0.860993, -0.0753331 --0.942744, -1.14496, -0.0297316 --1.90999, 1.48436, -1.96206 --1.39426, -0.798233, -0.173558 --0.916575, -1.19082, -0.109827 --1.45516, -0.878928, -0.450323 --1.65085, -0.873509, -0.321123 --1.81528, -0.664407, -0.149832 --1.50126, -0.814695, -0.19073 --1.27693, -0.911977, -0.182043 --1.64424, -0.819817, -0.00446556 --1.84829, -0.992184, -0.639513 --1.71806, -1.5198, -1.70604 --1.31623, 1.31962, -1.83117 --1.70338, -1.57483, -1.69493 --1.57482, -2.0591, -1.76082 --1.5623, -2.04544, -1.73496 --1.66241, -2.10065, -1.7847 --1.58472, -2.05225, -1.70735 --1.63492, -2.05494, -1.71909 --1.5826, 1.15389, -1.81106 --1.70313, -2.17233, -1.78861 --1.71011, -2.15527, -1.78332 --1.70935, 1.51524, -1.96492 --1.79407, -2.24992, -1.8338 --1.60544, 1.19805, -1.76583 --0.932546, -1.26412, -0.167198 --1.18637, -0.965117, -0.117644 --0.885114, -1.24807, -0.174904 --0.923672, -1.19504, -0.107856 --1.32404, 1.28716, -1.83416 --1.57575, -2.04171, -1.73626 --1.67051, -2.14604, -1.81371 --2.24586, 2.32018, -2.6161 --1.74408, -2.14219, -1.78141 --1.46085, -0.8946, -0.431446 --1.46285, -0.927996, -0.42989 --1.57188, -0.813694, -0.363812 --1.20076, -0.950558, -0.193726 --1.18615, -0.96415, -0.117825 --0.878696, -1.23612, -0.180956 --0.985129, -0.002031, -0.0258487 --0.966877, -0.0702192, -0.0258212 --0.969934, -0.988157, -0.0360559 --0.954731, -0.989009, -0.0355373 --1.36965, 1.31072, -1.85892 --1.53888, -2.04633, -1.74182 --1.41622, 1.30272, -1.87421 --1.72838, 1.28657, -1.8337 --1.67693, 1.13328, -1.72414 --1.81271, 0.596608, -1.18407 --1.42263, -0.884057, -0.433428 --1.34025, -0.99703, -0.375508 --1.2902, -0.797194, -0.211242 --1.53613, -0.823385, -0.141247 --1.18022, -0.962877, -0.119937 --0.917689, -1.23679, -0.175829 --1.22137, -0.774659, -0.10589 --1.1183, -0.857037, -0.077285 --1.12386, -0.867897, -0.0750051 --0.908971, -1.18356, -0.11159 --0.984935, -0.0403819, -0.0254511 --0.961996, -0.0466432, -0.0304495 --0.983082, -0.0685278, -0.0270948 --0.966101, -1.00165, -0.0275101 --4.52379, 6.27356, -5.45376 --1.62675, 1.29117, -1.85328 --1.7616, 0.453348, -1.27694 --1.41715, -0.881211, -0.463733 --1.47749, -0.916757, -0.463514 --1.80964, -0.432724, -0.364933 --1.28322, -1.00916, -0.383636 --1.44416, -0.820028, -0.349492 --1.80484, 0.722816, -0.223533 --1.44997, -0.824109, -0.22167 --1.37841, -0.90032, -0.118883 --1.22316, -0.846422, -0.120524 --0.872775, -1.24439, -0.175349 --0.966399, -1.08971, -0.0400031 --0.937008, -1.06548, -0.0449953 --0.973868, -0.798089, -0.0193748 --1.97976, -0.420356, -1.27416 --1.83974, 0.486991, -1.30314 --1.80716, 0.21033, -1.11209 --1.48563, -0.895891, -0.4419 --1.16542, -0.624709, -0.410263 --1.65717, -0.650574, -0.166045 --1.23666, -0.856028, -0.113132 --0.946733, -1.15638, -0.0223231 --1.91915, 1.35825, -1.81198 --1.79284, -0.576724, -0.312025 --1.83702, -0.893131, -0.0777429 --1.13099, -0.865608, -0.0478626 --1.51661, -0.983798, -0.447035 --1.81637, -0.662773, -0.16872 --1.49353, -0.798289, -0.333877 --1.39156, 1.24962, -1.8399 --1.38664, 1.23272, -1.80732 --1.48131, 1.22459, -1.85676 --1.69267, -1.54798, -1.68458 --1.44267, 1.19932, -1.80408 --1.96345, 2.0361, -2.39722 --1.5937, -2.06095, -1.73443 --1.58656, -2.05024, -1.71204 --1.6325, -2.05905, -1.72415 --1.65314, 1.1519, -1.80747 --1.86254, 0.874632, -1.70587 --1.80702, 0.390783, -1.1839 --1.36339, -0.951007, -0.465982 --1.26218, -1.02051, -0.329132 --1.46975, -0.807661, -0.158442 --0.924515, -1.26679, -0.164981 --0.962939, -0.0427102, -0.0303756 --0.957025, -0.112647, -0.0341993 --0.951039, -0.976739, -0.0374774 --1.41934, 1.16035, -1.82622 --1.37184, 1.25745, -1.82512 --1.42009, 1.19899, -1.8262 --1.59442, -2.0019, -1.75604 --1.62313, -2.06939, -1.78656 --1.7007, -2.09916, -1.77104 --1.67408, -2.07251, -1.71782 --1.79882, 0.579111, -1.15715 --0.92567, -1.23231, -0.175342 --0.927336, -1.24608, -0.170987 --0.878474, -1.23142, -0.182318 --0.720776, -1.11677, -0.0594912 --0.692809, -1.12376, -0.0684891 --1.37063, 1.16813, -1.8092 --1.40895, 1.36635, -1.90755 --1.7019, -1.50567, -1.69042 --1.36217, 1.14233, -1.7397 --1.62601, -2.12093, -1.81901 --1.66561, 1.1138, -1.71326 --1.56815, -1.99532, -1.42467 --1.53102, -0.804619, -0.352867 --1.53183, -0.839253, -0.341937 --1.2292, -0.999521, -0.338779 --1.12471, -0.860792, -0.0739402 --0.860681, -1.13112, -0.0335816 --1.72854, 1.28423, -1.83108 --1.80503, -1.2487, -0.764874 --1.8017, -0.962353, -0.677527 --1.46354, -0.886605, -0.440459 --1.51005, -0.929792, -0.402264 --1.7914, -0.414852, -0.337448 --1.51061, -0.8175, -0.157866 --0.942129, -0.0249277, -0.042377 --0.961252, -0.814799, -0.0532381 --0.962496, -0.971806, -0.0310961 --1.71826, -2.1326, -1.66886 --0.973137, -0.00299455, -0.0259452 --0.96643, -0.865623, -0.0360871 --1.65338, 1.04849, -1.60782 --1.81271, 0.67371, -1.25705 --0.850484, -1.13725, -0.0225665 --1.45489, -0.801019, -0.0155365 --1.38689, 1.27161, -1.80328 --1.70235, -2.11506, -1.80811 --1.66891, -2.11761, -1.77939 --1.76389, 1.3347, -1.86318 --1.80401, -2.56417, -2.18251 --1.29789, 1.26906, -1.81983 --1.65558, -2.09601, -1.83263 --1.48549, 1.35593, -1.8554 --1.67404, -2.10596, -1.809 --1.68047, -2.12, -1.78736 --1.46213, -0.814981, -0.371915 --0.954783, -1.08244, -0.33578 --1.40168, -2.09611, -1.76863 --1.41674, -2.06392, -1.74278 --1.83888, 0.856398, -1.78871 --1.3376, 1.14099, -1.77184 --1.72677, -1.47571, -1.711 --1.82871, 0.673026, -1.33516 --0.916406, -1.21862, -0.183491 --0.955992, -1.16153, -0.127988 --1.82033, 0.391585, -1.13535 --0.989492, -0.793871, -0.0197254 --1.82751, 0.41881, -1.25063 --1.7063, 0.544296, -1.08941 --0.897857, -1.07207, -1.16681 --1.23286, 1.21057, -1.7594 --1.52235, -2.01831, -1.76481 --1.68202, -1.92867, -1.83079 --1.54958, -2.01416, -1.75908 --1.6504, -1.97581, -1.80335 --1.59322, -2.04452, -1.77662 --1.80841, -1.53934, -1.76554 --1.6915, -1.97843, -1.80688 --1.67809, -2.0051, -1.79694 --1.69898, -1.91709, -1.78062 --1.64386, -2.01025, -1.76906 --1.61512, -2.01793, -1.74627 --1.60098, -2.04938, -1.75039 --1.83049, -1.21082, -1.795 --1.78289, -2.10618, -1.96925 --1.58307, -2.07551, -1.81634 --1.62552, -2.08286, -1.82456 --1.67074, -1.87939, -1.78461 --1.58578, -2.01695, -1.43597 --1.80866, 0.0466832, -1.28528 --0.924475, -1.12341, -0.140658 --1.77395, -1.15434, -1.74504 --1.63441, -1.79792, -1.7312 --1.80771, 0.826131, -1.76251 --1.71389, -2.11258, -1.67988 --1.51646, -1.95586, -1.38878 --1.39141, -0.994179, -0.362923 --0.775176, -1.20306, -0.202437 --1.7802, -1.17442, -1.74661 --1.60017, -2.03574, -1.42996 --0.649112, -1.10617, -0.200322 --1.23261, -0.860037, -0.330342 --0.867982, -1.18779, -0.198826 --0.551454, -1.11998, -0.0556783 --0.923675, -0.70821, -0.377374 --1.3125, -0.974558, -0.368551 --1.69015, -1.10504, -1.73919 --1.45349, -0.884573, -0.464213 --1.47353, -1.97387, -1.68231 --0.692957, -1.14566, -0.182544 --1.62607, -1.97687, -1.69346 --1.79389, 0.648899, -1.30906 --1.81813, -0.0129709, -0.511563 --0.932999, -1.26895, -0.17086 --0.847282, -1.55904, -0.146143 --0.81172, -1.28172, -0.171471 --1.77781, 0.433544, -1.20792 --1.79797, 0.248857, -0.70276 --0.984181, -1.11871, -1.43782 --0.989302, -1.1561, -1.44521 --0.997316, -1.12602, -1.43698 --0.885281, -0.998085, -1.32659 --1.35152, -2.07773, -1.8089 --1.15103, 0.286515, -1.47916 --4.08504, 1.66164, -3.5147 --1.39769, -2.08723, -1.81661 --1.44199, -2.00517, -1.80572 --1.45796, -2.1086, -1.836 --1.60704, 0.905581, -1.8615 --1.55217, 0.758946, -1.79021 --1.6266, 0.872852, -1.85785 --1.68477, 0.996636, -1.8988 --1.58719, -2.03021, -1.42485 --1.53393, -0.837931, -0.368201 --0.936133, 0.753791, -1.50385 --1.35452, -2.07218, -1.80836 --2.74984, 1.52928, -2.68424 --1.70024, 0.871144, -1.9068 --1.74177, 0.943759, -1.93806 --1.60345, 0.949516, -1.83105 --1.68573, -2.10926, -1.84234 --1.58288, -2.04479, -1.77546 --1.67936, -2.08767, -1.58771 --1.67802, -2.10822, -1.84778 --1.57632, -2.00642, -1.53351 --1.26129, 1.09176, -1.76632 --1.21504, -0.736756, -0.366415 --2.35408, -1.29557, -0.285829 --0.865093, -1.48774, -0.150445 --0.994116, -1.10562, -1.49657 --0.927883, -1.09208, -1.42491 --1.3339, -2.13273, -1.88729 --1.36513, -1.91181, -1.81642 --1.34415, -2.06343, -1.83975 --1.29444, -1.88712, -1.75602 --2.88836, 1.96804, -2.92453 --1.31977, 1.00339, -1.77925 --1.41629, 0.936001, -1.80982 --3.54174, -4.02693, -3.63124 --1.48707, 0.987072, -1.79603 --1.42238, -2.0775, -1.8062 --1.53482, -2.07938, -1.80644 --1.58624, -2.08922, -1.81587 --1.54335, -1.99697, -1.39264 --1.22861, -1.29116, -1.7813 --1.73533, -1.57618, -1.72291 --1.75838, -1.56441, -1.72322 --0.660465, -1.11855, -0.194266 --0.804851, -1.54663, -0.160312 --0.661752, -1.12141, -0.193738 --1.56778, -2.0012, -1.5273 --0.873048, -1.27211, -0.183762 --0.831761, -1.24928, -0.169286 --1.73966, -2.14069, -1.69335 --1.63993, -2.06468, -1.574 --1.03816, 0.924468, -0.0675591 --1.72626, 0.143418, -1.81802 --1.6566, -0.651399, -0.173054 --1.29367, -2.14205, 0.662441 --2.52006, 0.789119, -1.49246 --1.8841, 0.647747, -1.164 --1.70645, -1.19382, -0.65609 --1.29371, -2.03183, -1.84329 --1.19909, -1.98195, -1.76096 --20.7497, -16.4763, -16.6458 --1.42015, -2.00254, -1.8816 --4.62894, -3.45614, -4.04634 --1.32358, -2.08744, -1.81704 --1.04353, 0.927921, -1.53622 --1.47657, -2.06113, -1.79106 --1.70437, -1.72018, -1.82761 --1.80054, -1.17035, -1.77081 --1.62282, -1.92425, -1.80196 --1.7033, -2.13089, -1.85734 --1.70297, -1.54453, -1.69504 --1.30075, -1.29755, -1.73332 --11.9652, -7.58849, -9.58086 --1.65544, -2.16722, -2.06882 --1.37122, 1.1625, -1.85648 --1.69473, -1.32545, -1.78682 --1.26612, -1.12685, -1.80023 --2.51666, -1.42207, -2.46083 --1.01323, 0.825499, -1.59195 --1.67774, 0.161622, -1.77342 --1.6613, -1.40935, -1.67701 --1.65873, -1.4898, -1.66773 --1.71339, -1.56716, -1.70806 --1.08553, -1.20757, -0.94356 --1.25639, -0.86983, -0.459064 --1.06719, 1.02207, -0.336249 --1.40595, -1.27596, -0.29319 --1.23419, 1.26404, -0.011089 --0.836793, 0.656851, -1.43941 --1.73698, 1.21231, -1.85889 --1.00888, -1.17404, -0.92347 --1.24131, 1.16476, -0.00295222 --1.02529, 0.822585, -1.59164 --1.09731, -1.90067, -1.65203 --1.7879, 0.388339, -1.24466 --1.3431, -1.33563, -1.04547 --1.78757, 1.2509, -1.77271 --1.89698, 0.641375, -1.14772 --1.89908, 0.553601, -1.28379 --1.19361, 0.0391753, -0.532163 --0.804143, -1.18967, -1.44291 --1.29218, 0.936725, -1.82663 --1.39617, 0.462712, -1.77406 --0.670813, -1.07101, -1.28285 --1.29239, -1.1336, -1.67974 --1.35658, 0.950039, -1.82356 --6.56995, 4.69264, -5.73395 --1.37063, 1.06444, -1.84376 --2.80822, 1.8659, -2.84254 --1.54417, 0.301931, -1.78491 --1.25733, 1.05984, -1.76084 --1.62764, 0.175142, -1.8118 --0.923068, 0.25425, -1.37693 --1.56514, -1.1801, -1.8059 --18.6522, -16.7317, -15.6962 --1.86503, -1.99041, -2.1397 --0.745446, -1.09995, -1.27643 --1.32014, -2.06323, -1.85911 --1.72624, -1.18426, -1.82073 --1.73438, -1.22556, -1.82543 --1.71472, -1.14184, -1.79736 --1.78637, -1.18932, -1.83012 --1.01356, -1.03605, -1.36593 --0.984099, -1.11107, -1.35903 --3.71487, -2.09278, -3.09826 --0.901427, -1.06234, -1.29151 --0.958424, -1.10194, -1.31641 --2.40895, -2.17451, -2.28279 --1.6601, -1.80463, -1.79786 --1.61521, -2.08388, -1.80712 --1.76086, -1.57382, -1.72961 --1.1461, -1.07719, -1.6878 --2.4349, -2.88654, -2.77436 --1.41001, -1.54095, -1.81913 --5.10963, 2.01479, -4.24048 --1.44984, -2.13712, -1.9311 --0.980389, -1.00464, -1.35743 --0.990847, -1.11475, -1.33204 --0.770771, -1.1771, -1.41411 --1.18646, -1.14321, -1.67256 --1.66221, 0.140866, -1.79022 --1.23096, -1.91106, -1.74148 --0.916794, -1.1283, -1.27952 --1.2638, -1.0128, -0.335685 --1.13739, -0.834905, -0.34359 --0.986364, 0.899862, -0.0744395 --0.983025, -1.07455, -1.57881 --1.29654, -1.13114, -1.79926 --1.17191, -1.25584, -1.38756 --0.919524, 0.843383, -1.51769 --1.84785, 0.164424, -1.74794 --1.80188, 0.462596, -1.21557 --1.51338, 0.0768235, -1.6079 --0.976414, -1.07836, -1.31871 --1.77689, 0.381844, -1.23239 --1.07009, -1.73801, -0.0982522 --1.74344, 0.407608, -1.28622 --1.77056, 1.22912, -0.583717 --0.931589, -1.08661, -1.54072 --0.912844, -1.07981, -1.51329 --0.835705, -1.19342, -1.45512 --1.55165, 0.192529, -1.83321 --0.949956, 0.778709, -1.52905 --1.50352, 0.244678, -1.77358 --1.54033, 0.218988, -1.79698 --1.53373, 0.205806, -1.78972 --1.4664, 0.650088, -1.80743 --1.49832, 0.507282, -1.80363 --1.31544, 1.0067, -1.77962 --1.51171, -1.15562, -1.81854 --1.56751, 0.516665, -1.82058 --1.62955, 0.207722, -1.8221 --0.851705, -1.15926, -1.40124 --1.62272, 0.241149, -1.81768 --0.963567, -1.10317, -1.45083 --0.941842, -0.999455, -1.41591 --4.2803, -4.15003, -4.16776 --1.52606, -1.17604, -1.80428 --1.52666, -1.18228, -1.80478 --3.1045, -3.04318, -3.18507 --0.935752, -1.13669, -1.43121 --0.958374, -0.991145, -1.41148 --0.928746, -1.06643, -1.40971 --1.31899, -1.58518, -1.76131 --0.896013, -1.1436, -1.40163 --1.18087, -1.52743, -1.65703 --1.29961, -1.99052, -1.84921 --0.956657, -1.07001, -1.40998 --1.28906, -1.88897, -1.80559 --1.80632, 0.26694, -1.87113 --1.69282, 0.153394, -1.78203 --1.33039, -1.89191, -1.80819 --1.26118, -1.97327, -1.78146 --1.75166, -1.22912, -1.83901 --1.03427, -1.03274, -1.39675 --1.77422, -1.16672, -1.81346 --1.34375, -2.03732, -1.77826 --1.02957, -1.13615, -1.39335 --1.64716, 1.14004, -1.79559 --3.92331, -3.03203, -3.28759 --0.977507, -1.11375, -1.30907 --1.7092, -1.98203, -1.82669 --1.43813, -1.15686, -1.80947 --0.979081, -1.02706, -1.42318 --0.957183, -1.15777, -1.42462 --1.27655, -1.80848, -1.76589 --1.7005, 0.235112, -1.80657 --0.66439, -1.12026, -0.191241 --0.928598, -1.0816, -1.50741 --1.57237, 0.199088, -1.80096 --1.53304, 0.239338, -1.76551 --0.940515, -1.08939, -1.43522 --2.93749, -2.90794, -3.06498 --1.03147, -1.05252, -1.41279 --1.69884, 1.16667, -1.83146 --0.954112, 0.88634, -1.24916 --0.659673, -1.11981, -0.19266 --1.00084, -1.18243, -0.111524 --1.05553, 1.04085, -0.0597781 --0.802759, -1.53804, -0.159652 --1.76337, 0.38056, -1.26614 --1.80045, -0.89851, -0.366573 --1.80511, -1.00678, -0.860374 --1.82445, 0.0238583, -0.628306 --6.31746, 3.81621, -5.56405 --5.00315, 3.551, -4.6411 --0.916562, -1.07624, -1.49635 --0.903195, -1.01163, -1.4629 --0.956604, -1.05457, -1.49725 --0.867779, -1.11749, -1.45541 --1.55908, 0.262165, -1.79237 --1.41388, -1.15352, -1.79587 --1.59612, 0.288554, -1.81392 --1.58797, 0.183169, -1.80236 --1.18509, -1.08874, -1.62718 --0.953554, -1.05154, -1.47714 --1.47181, -1.15418, -1.81636 --0.999588, -1.03378, -1.49317 --0.971701, -1.04775, -1.4701 --0.97672, -1.0958, -1.47818 --1.5185, -1.14993, -1.80935 --1.00171, -1.02304, -1.47211 --1.4962, -1.12732, -1.78265 --1.71018, 0.195319, -1.80953 --1.73575, 0.17145, -1.82404 --1.6247, -2.05561, -2.06742 --1.72977, 0.407449, -1.82851 --1.56369, -1.21432, -1.80673 --1.65482, -2.16948, -2.09205 --1.62932, -1.11637, -1.78779 --85.4947, -43.1312, -56.1587 --2.89495, -1.86606, -2.5633 --0.903183, -1.11684, -1.30866 --0.980669, 0.909403, -1.26275 --0.931081, -1.07917, -1.53937 --0.981512, 0.801294, -1.56688 --0.92866, -1.07957, -1.5047 --0.958637, -1.06444, -1.47861 --0.934677, -1.1334, -1.48296 --0.96094, -1.00814, -1.45952 --0.952567, -1.04888, -1.45925 --0.949387, -1.0742, -1.45895 --3.87632, -3.64394, -3.77977 --0.945327, -1.14011, -1.43628 --1.33237, -1.84398, -1.83529 --0.981629, -1.04924, -1.4694 --1.52497, -1.15541, -1.786 --1.42305, -1.38378, -1.66389 --0.908086, -1.12105, -1.32482 --0.622467, -1.11721, -0.210416 --1.52448, -1.14412, -1.8248 --1.67975, -1.12456, -1.80695 --0.958168, -0.78831, -0.0374576 --1.7325, -1.1273, -1.79293 --1.78427, 1.27707, -0.767864 --1.79464, -0.639789, -0.166859 --1.80974, -1.00349, -0.83252 --1.7429, -1.23285, -0.619582 --1.77542, -0.611711, -0.363994 --1.90778, 0.727909, -0.102527 --1.40152, -1.18386, -1.84191 --0.920826, -0.978312, -1.49334 --1.30026, 1.04693, -1.74227 --0.907031, -1.00049, -1.4836 --0.955089, -1.09516, -1.53232 --1.39024, -1.15825, -1.80515 --1.46576, -1.16605, -1.78465 --1.2335, -1.10392, -1.61733 --1.54233, -1.31893, -1.84049 --1.51955, -1.30983, -1.82795 --1.56837, -1.14165, -1.80874 --1.61275, -1.18031, -1.81297 --1.25899, -1.81845, -1.77948 --1.66741, -1.13294, -1.80973 --1.27065, -2.10761, -1.83168 --1.06661, -1.0567, -1.42605 --1.80072, 0.818327, -1.75092 --0.979895, 0.908051, -1.26639 --0.970528, -1.07624, -1.56739 --1.33304, -1.15884, -1.81376 --0.88831, -1.12171, -1.50315 --0.882265, -1.10547, -1.4784 --0.888014, -1.17467, -1.50284 --1.1506, 0.985973, -1.60682 --1.66137, 0.144476, -1.79455 --1.35404, 0.216313, -1.59607 --1.51537, -1.21899, -1.82868 --1.76472, -1.16024, -1.82515 --1.57516, -2.08103, -1.83004 --0.954042, -1.09329, -1.53729 --1.56012, -1.14743, -1.82652 --1.67704, -2.19711, -1.91387 --1.35112, 1.60385, -0.771094 --0.700502, -1.1691, -0.217082 --0.951151, 0.876939, -1.53313 --0.953647, -1.17302, -1.4639 --1.80761, 1.24461, -0.801292 --1.10041, -1.15884, -1.38791 --1.81316, -0.0139481, -0.526738 --2.37439, -1.48711, -0.864475 --1.56458, 1.25145, -1.96284 --0.950013, 0.819169, -1.48424 --1.02133, 0.859677, -1.50717 --1.34734, 0.461955, -1.64848 --2.03231, 0.390404, -2.09603 --1.26416, -1.12525, -1.76292 --0.881272, -0.971317, -1.4803 --0.908897, -0.994643, -1.49387 --1.34921, 0.810176, -1.67613 --1.62311, 0.581863, -1.81327 --9.76239, -6.27153, -8.07783 --1.32882, -1.11999, -1.70634 --0.949715, -1.27231, -1.53523 --2.93483, -2.86202, -3.05542 --1.60683, -1.39943, -1.88777 --1.26439, -1.47037, -1.72694 --1.36982, -1.77406, -1.8691 --1.36653, -1.83098, -1.8266 --0.910091, -1.11666, -1.32415 --0.782545, -0.978868, -1.18619 --0.964106, -1.15921, -1.30369 --1.27751, 1.04339, -1.69942 --1.23306, 0.921315, -1.63786 --0.849724, -0.998018, -1.45203 --0.977395, -1.04781, -1.52593 --0.966943, -1.05349, -1.50095 --1.54063, -1.19429, -1.85461 --1.20084, -1.47284, -1.7375 --0.9682, 0.870561, -1.55807 --0.869944, -1.01522, -1.4871 --0.971502, 0.901857, -1.26191 --0.947747, -1.15092, -1.34516 --0.976517, -1.10835, -1.31209 --0.962297, -1.49013, -1.38326 --1.82493, 0.496854, -1.3087 --0.888709, 0.746508, -1.41787 --0.582679, 0.456267, -1.09534 --0.861214, -1.10579, -1.29274 --1.87717, -1.29839, -1.86345 --1.2025, -1.76782, -1.60309 --1.59991, -1.67533, -1.70464 --1.499, -1.38418, -1.55412 --1.94063, -1.05146, -0.828058 --1.21489, 0.138288, -0.558964 --0.887328, -1.02833, -1.48853 --1.43687, -1.2937, -1.78564 --1.69632, -1.72091, -2.05485 --1.12979, -1.33428, -1.61028 --1.67962, -1.42177, -1.90895 --1.60539, -1.13739, -1.80749 --0.951911, -1.09234, -1.44421 --1.74421, 0.146262, -1.77264 --0.952403, -1.15735, -1.52503 --1.36114, -2.0645, -1.84805 --0.743586, -1.12817, -0.213376 --1.02807, -1.49497, -1.16929 --5.8213, -5.11875, -5.474 --1.78448, -1.19445, -1.78001 --1.12793, -1.67897, -1.54113 --1.87497, 0.194132, -1.75643 --0.722674, -1.41698, -0.182984 --0.562673, -0.773226, -1.05663 --1.31922, -1.7791, -1.36679 --1.56947, 0.143276, -1.8345 --1.50279, 0.130326, -1.77375 -0.0318774, -2.03106, -1.77486 --0.934959, -1.07133, -1.55003 --1.04451, -1.23548, -1.61602 --1.33784, -1.15499, -1.71015 --1.37488, -1.12275, -1.70904 --1.52077, -1.14523, -1.78866 --1.59593, -1.15314, -1.82975 --1.37758, -1.1587, -1.712 --1.43127, -1.08048, -1.70204 --1.32666, -1.1624, -1.67519 --1.61649, -1.14641, -1.81411 --0.668247, -0.74994, -1.19091 --1.47359, -1.19995, -1.73998 --1.33674, -1.18266, -1.65923 --1.30641, -1.78144, -1.8075 --1.58629, -1.41028, -1.77251 --1.59725, -1.96548, -1.9696 --1.53821, -1.49764, -1.71953 --1.29017, -1.2044, -1.84187 --1.43263, -1.17103, -1.77755 --1.35389, -1.05274, -1.66771 --1.39125, -1.21733, -1.74627 --1.36598, -1.24011, -1.7299 --0.887634, -1.18198, -1.42945 --1.40068, -1.90512, -1.70535 --1.91579, 0.202737, -1.92647 -0.0283269, -2.03684, -1.79802 --1.52631, -1.33609, -2.02344 --0.917781, -1.07623, -1.51122 --1.94938, -1.23063, -1.90486 --0.922116, -1.07436, -1.27307 --1.04436, -1.13756, -1.3236 --0.569493, -2.84144, -0.931741 --0.651809, -1.37689, -0.198019 --0.846216, -1.16767, -0.191807 -0.0445938, -2.17571, 0.130225 --1.1231, -1.21598, -1.68414 --0.950349, -1.1604, -1.44794 --0.955831, -1.15934, -1.34647 --1.23828, -1.76653, -1.60751 -0.357668, -2.86072, -0.927169 --1.45472, -1.12511, -1.77827 --0.705584, -0.897044, -1.27566 --0.87928, -1.16485, -1.40578 --1.8041, -1.11774, -1.79236 --0.761664, -1.37957, -1.39449 --1.53056, -1.97616, -1.41867 --0.0223744, -2.70234, -0.893197 -0.241374, -2.08409, -0.0355906 --1.14281, -1.12351, -1.50874 --0.935225, -1.5259, -1.53533 --1.54663, -1.85299, -1.66093 --1.8632, -2.2709, -1.61798 -0.479374, -2.68183, -0.883824 --0.378373, -0.883669, -0.286165 --2.1324, -2.08594, -2.48568 --1.30903, -1.2655, -1.51559 --0.725384, -2.91931, -0.216961 --1.64875, -1.14458, -0.665387 --0.564255, -0.804208, -1.21759 --0.961443, -1.08624, -1.52983 --0.973183, -1.06107, -1.47134 --0.82713, -1.1749, -1.45311 --0.667446, -0.859937, -1.22161 --1.00046, -1.08512, -1.58686 -0.223467, -2.21889, 0.0339316 --1.20175, -1.10414, -1.74783 --1.26269, -1.94443, -1.7702 --1.46375, -1.41224, -1.56596 -0.502679, -2.80027, -0.923178 -0.0363415, -2.05989, -1.78277 --1.42825, -2.09631, -1.9011 -0.910615, -2.85233, -0.747264 -0.0390929, -2.16468, 0.12788 --0.249069, -0.628634, -0.953992 --1.00691, -1.09029, -1.51037 -0.511204, -2.06747, -1.03037 -0.949101, -2.78477, -0.98986 --0.565899, -2.80867, -0.912372 -0.55397, -2.30905, -0.00295994 --0.264791, -0.649697, -0.896938 --0.715203, -1.2323, -0.196176 --0.777583, -1.45913, -0.168761 --0.82969, -1.44095, -0.159331 --1.10221, -1.19192, -1.68323 -1.14133, -2.00268, -1.74782 -0.137111, -2.14858, -1.79992 -0.0782395, -2.23913, -1.86622 -1.05919, -2.13049, -1.7876 -0.260935, -1.99862, -1.69515 --1.16839, -1.22228, -1.43541 -0.667878, -2.13545, -1.09521 --1.31142, -1.38702, -1.7764 -0.105828, -2.08926, -1.75975 --1.93852, -2.40086, -1.98952 -0.459814, -2.75224, -1.03724 -0.573279, -2.20227, -0.0217749 -1.16389, -2.11244, -1.81849 --1.20534, -1.23983, -1.41452 -0.674098, -2.10538, -1.11788 -0.746112, -2.88369, -1.00419 --0.638885, -1.13178, -0.224995 --1.52269, -1.96801, -1.43295 -0.420082, -2.87656, -0.896742 -0.594215, -2.24256, -1.11414 -0.512311, -2.2108, -1.07224 --0.539674, -3.00618, -0.175643 -0.805849, -3.01238, -0.751828 --1.43852, -1.17442, -1.78742 --1.3956, -1.20763, -1.78361 --1.15449, -1.21777, -1.66894 --1.01013, -1.13081, -1.55803 -0.0497062, -1.99074, -1.75619 -1.13612, -2.00368, -1.73151 --1.76551, -1.22564, -1.80585 --1.33972, -1.62107, -1.83101 --1.79317, -1.2946, -1.82524 --0.501291, -0.723515, -1.05995 -0.418116, -2.63946, -0.990357 -0.332915, -2.6966, -0.974603 -0.732208, -2.67715, -0.659929 -1.00512, -2.21762, -0.0732362 -0.232209, -2.44241, 0.0319416 --1.25225, -1.15571, -1.76521 -1.25721, -2.37134, -1.99699 --1.25197, -1.94115, -1.79013 -0.637538, -2.05372, -1.08733 -0.45168, -2.77589, -1.03795 --1.81723, -1.36754, -2.11171 --1.06404, -1.16613, -1.64723 --0.764107, -1.04598, -1.39724 -1.11473, -2.03531, -1.78706 --0.879757, -1.14108, -1.47292 --1.39845, -1.3495, -1.5292 --1.19255, -1.68854, -1.53006 --1.18419, -1.73154, -1.52746 -0.414981, -2.79981, -1.02288 -0.679505, -2.83712, -0.923053 -1.15058, -2.00662, -1.786 --1.17411, -1.48431, -1.73122 -0.67495, -2.13362, -1.09401 --1.23327, -1.24914, -1.48112 --0.7162, -1.01167, -1.17625 -0.242861, -2.78886, -1.04845 --0.144285, -2.08923, 0.102777 -0.533861, -1.82611, -0.925846 -0.723232, -2.77972, -0.952217 -0.153369, -2.19607, 0.0112424 --1.7572, -1.24064, -1.80037 --1.27535, -1.68205, -1.81151 --1.06701, -1.59859, -1.65255 --1.3071, -1.92568, -1.83263 -0.876589, -2.73419, -0.775232 -3.83476, -12.3328, -9.1855 -0.503846, -1.96727, -0.0771739 -0.961916, -2.17216, -1.90055 --0.0220729, -2.27179, -1.96355 --1.16107, -1.23136, -1.49913 --0.937646, -1.14711, -1.36327 --1.39557, -2.17651, -1.89806 -0.622536, -2.09979, -1.06891 --0.626596, -1.11481, -0.226187 --0.891035, -1.12181, -1.33261 --0.876573, -1.10888, -1.26946 -0.22028, -2.22594, 0.0320607 -0.71453, -2.30163, -1.14559 -0.519039, -2.40925, -1.11931 -0.85337, -2.81662, -0.845432 -2.1966, -2.36631, -0.439906 --0.951184, -1.6127, -0.127677 --0.278743, -1.08045, -0.0787388 -0.687124, -2.02558, -0.776911 -0.216396, -1.74324, -0.100105 -0.0508988, -2.00893, -1.8048 --0.88131, -1.21295, -1.52024 --0.896009, -1.25386, -1.53157 --0.871998, -1.23016, -1.50676 --0.898436, -1.065, -1.34296 --0.985077, -1.17725, -1.39679 --0.911606, -1.12147, -1.34078 --1.22859, -1.88373, -1.76859 --1.48778, -1.55287, -1.64121 -0.505722, -2.72919, -0.919916 --1.27534, -1.14858, -1.80919 --1.2785, -1.18144, -1.8043 -2.14593, -4.71485, -3.90941 --1.40641, -1.17432, -1.78902 -1.13887, -2.15582, -1.91122 -1.13316, -1.1855, -0.450812 --1.30407, -1.14628, -1.80604 -1.29097, -2.42887, -2.17759 -1.26948, -2.54758, -2.24831 -1.59972, -3.90993, -3.12896 -1.32723, -2.58808, -2.14355 --1.17951, -1.18792, -1.5092 -1.31363, -1.45105, -0.448366 -0.0370012, -2.20195, 0.13846 --1.18627, -1.10279, -1.72615 --0.723136, -1.03205, -1.38867 --0.888116, -1.27255, -1.51068 --1.02533, -1.05279, -1.40561 --1.04922, -1.1169, -1.43185 --0.889569, -1.24188, -0.178577 --1.253, -1.28403, -1.55867 --0.985898, -1.19531, -1.39139 -0.668635, -1.89143, -1.04794 -0.374725, -2.71996, -1.02944 -0.9379, -3.12619, -0.786 -0.547053, -2.81186, 0.0979574 --1.56038, -1.86173, -1.66934 -0.653041, -1.57849, -0.928443 --0.696631, -1.19935, -0.205779 -0.433379, -2.4571, -1.03112 -0.729615, -1.43698, 0.0485816 -2.07189, -4.03836, -0.954136 -1.15799, -1.80404, -1.81303 --13.1058, -55.312, -39.8441 -1.58008, -4.80941, -3.84728 --0.162865, -2.90492, -2.52588 -1.28073, -2.94333, -2.5483 --0.95523, -1.09597, -1.57491 -1.63772, -5.29534, -4.14463 --0.871121, -1.16231, -1.51326 --1.46027, -1.93078, -1.94967 -0.49901, -2.07139, -1.04078 -0.676949, -2.8711, -0.960588 -2.52963, -7.65427, -5.97671 --0.98929, -1.10374, -1.58184 --0.977085, -1.17965, -1.3859 --0.343912, -3.52744, -2.96567 --0.761934, -1.05043, -1.42361 --1.0507, -1.16209, -1.43131 --1.81344, -1.52689, -1.75306 --1.32302, -1.98253, -1.79777 -0.522582, -2.23558, -1.06648 -0.885544, -3.03292, -0.808022 -0.600398, -2.70426, -0.674666 --1.00994, -1.26752, -1.60356 -0.528, -2.70773, -1.18997 -0.834871, -2.66125, -0.95386 -0.700294, -2.61535, -0.906286 -1.11499, -2.78191, -0.81638 --1.33188, -2.10216, -1.82516 --0.906049, -1.15416, -1.51862 -0.379436, -2.75614, -1.03659 -0.362738, -2.73784, -1.01397 -1.80793, -2.39264, -0.438539 --1.05011, -1.04627, -1.43173 --1.7007, -2.10281, -1.677 -0.948964, -1.09197, -0.583171 -1.79907, -2.32046, -0.438616 -0.943605, -1.09273, -0.58831 -0.948649, -2.79992, -0.975688 -0.201552, -2.48229, -0.0668301 -2.53987e+06, -3.04172e+07, -5.21492e+06 -0.954658, -1.20486, -0.628628 -0.542081, -2.78466, -0.652481 --1.2339, -1.12285, -1.78944 -0.101155, -1.6843, -1.7627 -0.0833504, -1.63115, -1.72608 --0.704679, -0.791246, -1.3614 -1.04214, -1.72334, -1.76693 --1.63302, -1.27923, -1.79146 --1.6036, -1.2959, -1.77441 -1.15906, -2.04643, -1.78344 -1.04468, -2.16653, -0.0767669 -0.888182, -1.65945, -0.133439 --1.26072, -1.19155, -1.77467 -0.0653644, -1.68201, -1.7615 --1.71221, -1.57291, -1.70898 -0.0736862, -1.66147, -1.74604 -0.112061, -1.6682, -1.75071 --1.02181, -1.0484, -1.41384 --1.03155, -1.08943, -1.41754 --1.72576, -1.54709, -1.71599 --0.87544, -1.09068, -1.26592 --1.71896, -2.11133, -1.85148 -0.569386, -2.19369, -0.0238243 -0.0295061, -2.25519, 0.0377324 --0.35691, -1.65652, -1.87295 --1.14246, -1.10941, -1.4861 --0.81316, -0.959616, -1.2799 -0.485824, -2.8241, -1.06105 -1.4643, -2.74282, -0.76356 -0.894973, -2.70408, -0.657895 -0.0588885, -1.98464, -1.73847 --1.13583, -1.22514, -1.48396 --1.04632, -1.15325, -1.37218 --0.869084, -1.22556, -0.18236 --1.93709, -1.66845, -1.90168 --1.16991, -1.43834, -1.4416 -0.589331, -0.914837, -0.606687 -0.184215, -2.83596, -0.831802 -0.749474, -2.80572, -0.701511 -0.779222, -1.65948, -1.74727 --1.65233, -1.21881, -1.80762 --1.60872, -1.22543, -1.77776 -1.14837, -2.05192, -1.76826 --1.37277, -1.74596, -1.76431 -1.49238, -2.81993, -0.783085 -1.90079, -2.2992, -0.341768 -1.0813, -2.32877, -0.0504082 -0.953541, -1.76944, -1.8198 -1.17126, -2.11511, -1.82069 --1.63724, -2.08646, -1.44487 -0.669637, -2.04877, -1.07142 --1.03733, -1.45736, -0.148401 --1.26047, -1.03495, -1.70492 --1.68723, -1.18083, -1.74573 --1.67432, -1.58613, -1.70746 -0.749035, -2.61119, -0.779347 -0.507774, -2.12246, -0.0462762 -0.793329, -1.13959, -0.0782369 -0.970327, -1.82418, -1.85137 --1.13395, -1.71789, -1.63729 --1.53391, -1.82341, -1.66364 -0.707002, -2.20721, -1.1136 --0.406501, -2.88129, -0.741072 -0.0563757, -2.35816, -0.00892591 --0.0791569, -2.26018, 0.056398 --1.61459, -2.25511, -1.88175 -0.851615, -2.64441, -0.58332 --1.42427, -2.24129, -1.89364 --0.409668, -2.8856, -0.777261 -1.05778, -1.41996, -0.654504 -1.41019, -2.88806, -0.759826 --0.569461, -1.21741, -0.0307598 -0.845378, -2.89227, -0.575142 --1.27584, -1.17545, -1.81785 --1.30004, -1.16641, -1.81504 --1.31832, -1.16267, -1.81677 --1.36365, -1.17193, -1.82882 --1.6464, -1.1776, -1.80503 --1.62932, -1.35272, -1.7904 --1.3482, -2.07453, -1.80846 -0.388262, -2.74183, -1.03177 -0.325369, -2.79585, -1.02135 -0.403256, -2.76039, -1.00458 -0.566091, -2.53761, -0.923511 -0.507502, -2.91653, -0.967328 -0.730259, -2.72123, -0.70521 -0.749242, -2.66298, -0.700246 --0.0744772, -2.22295, 0.0512943 --1.23906, -1.12624, -1.75577 -0.533685, -2.22188, -1.10776 -0.498857, -2.81661, -1.03219 -0.453644, -2.41527, 0.308739 -1.60534, -7.55476, -5.58072 -0.494299, -2.24644, -1.10853 -0.519411, -2.30518, -1.09431 --0.506519, -2.87701, -0.641214 -0.596569, -2.4751, -0.433241 -0.25089, -2.58756, -0.000700959 --0.128083, -2.47317, 0.00658013 -0.271763, -2.63802, 0.0330467 -0.301591, -2.52677, 0.0773511 --0.898358, -1.16714, -0.115888 --0.881292, -1.17922, -0.115353 --0.698115, -1.09981, -0.0665223 -0.0947186, -1.21151, -0.0549686 -0.13291, -1.06049, -0.0868003 -0.419956, -0.490961, -0.23963 -0.793547, -1.0324, -0.0591418 --1.33834, -1.18263, -1.84443 --1.60714, -1.41114, -1.67158 --0.936564, -1.05018, -1.33045 --1.22606, -1.38417, -1.45974 --1.48225, -1.88814, -1.37662 -0.518348, -2.29476, -1.08765 --1.16279, -2.19529, -1.1059 -0.972131, -2.74398, -1.12725 -0.585018, -2.53034, -1.05192 -1.57891, -2.85581, -0.762618 -0.580759, -2.3519, -0.428362 --1.0988, -1.55895, -0.129805 -0.538227, -2.02837, -0.0548223 -0.811744, -1.16885, -0.0651426 -1.14557, -2.0801, -1.79174 -0.272851, -3.23814, -1.10759 -0.315091, -2.90173, -0.872484 -0.056604, -2.6366, -0.0177983 -0.11533, -1.0941, -0.0776333 -0.984629, -2.09436, -1.80543 --1.17435, -1.30898, -1.42882 -0.94462, -2.72175, -0.957496 --0.381007, -2.77998, -0.74264 --0.350653, -2.55342, -0.00258643 --0.860583, -1.17362, -0.099647 -2.21585, -2.46409, -0.449652 --0.50956, -2.95309, -0.229007 --0.0829888, -2.70088, 0.00165914 --0.732482, -1.47336, 0.0271142 --0.579263, -7.27414, -2.05532 -0.680259, -2.43105, -0.984795 --0.00716774, -2.82295, -0.979149 -0.0107542, -2.85231, -0.726005 -0.165633, -2.61846, 0.184764 -0.477594, -1.83361, -0.0176013 --1.20145, -1.05424, -1.73322 --1.34822, -1.12597, -1.79098 --1.32076, -1.06863, -1.73641 --3.1438, -2.55837, -3.11023 --3.42006, -2.76417, -3.27878 --1.36321, -1.28619, -1.82893 --1.42349, -1.30729, -1.84717 --1.39889, -1.31998, -1.82554 -1.10888, -1.9083, -1.8138 --1.35566, -2.08238, -1.81059 -0.511621, -2.91849, -1.05068 -0.512779, -2.53687, -0.0638457 --1.34719, -1.18759, -1.85255 -1.07176, -1.06651, -0.451797 -1.06686, -2.44093, -0.0327988 --0.0397067, -1.66735, -2.04967 --1.75278, -1.18489, -1.79303 -0.694441, -1.8308, -1.81774 -1.17833, -2.05531, -1.90802 -0.613999, -2.21303, -1.10921 -0.355275, -2.80215, -1.01354 --1.48335, -1.21878, -1.83893 --1.32182, -1.27874, -1.81912 --1.93205, -1.48914, -2.12169 -0.957244, -2.72496, -0.948195 --0.801752, -1.12515, -0.20416 -0.392573, -3.29605, -1.05975 -0.720594, -2.6713, -0.798816 -1.10685, -2.87851, -0.711307 -0.52491, -2.36923, -0.0162295 -0.159526, -2.6866, 0.112413 -0.374747, -2.54617, -1.11524 -1.19987, -3.10089, -1.12267 --1.24784, -1.20735, -1.80304 --1.26756, -1.23957, -1.80844 --1.55168, -1.14188, -1.79977 --1.40051, -1.25055, -1.79874 --1.35435, -1.3139, -1.82309 --1.37362, -1.33103, -1.80666 --1.35969, -1.13277, -1.79619 --1.24076, -1.2372, -1.79647 --1.44968, -1.13746, -1.78721 -0.38606, -2.74632, -1.00909 -0.680818, -2.11418, -1.0855 --0.13881, -2.70553, -0.902407 -0.680679, -2.8281, -0.822036 -0.656239, -2.44546, -0.912979 -0.667566, -2.68793, -0.933342 --0.0876208, -2.73422, -0.900948 -0.438996, -2.67912, -1.00542 -0.441992, -2.18624, -1.06435 -0.576168, -2.23182, -0.979911 --0.0346151, -1.68836, -2.06422 --1.64809, -1.19623, -1.85004 --1.77114, -1.20707, -1.85778 --1.38697, -1.45722, -1.82024 -1.16438, -2.06677, -1.78729 --0.135021, -2.70656, -0.904782 -0.548185, -2.21786, -0.0206391 -0.857781, -2.79356, -0.966643 --0.849651, -1.16729, -0.0954912 --1.50984, -1.08255, -1.73697 --1.68565, -1.11808, -1.77157 --1.56203, -1.18479, -1.77363 --1.73982, -1.24322, -1.8457 --1.64644, -1.31018, -1.85195 --1.54487, -1.43153, -1.83047 -3.72799, -7.80414, -5.06504 -3.78925, -7.67269, -4.99295 -1.0404, -1.91433, -1.72422 -1.16542, -2.08938, -1.78402 -0.504961, -2.45396, -0.0748158 --1.09324, -1.19909, -1.56637 -1.16156, -2.08826, -1.79283 -0.687105, -2.82396, -0.792639 --1.72009, -1.13374, -1.79398 --1.67807, -1.12542, -1.6654 -1.09669, -2.01036, -1.75818 -0.646624, -2.53752, -0.929281 --1.67526, -1.39554, -1.60649 -1.10642, -2.07017, -1.79224 -0.649495, -2.10694, -1.8081 --1.3283, -1.46419, -1.78162 --1.37303, -1.53873, -1.813 --1.54528, -1.49363, -1.79251 -0.96241, -2.76322, -0.739413 --1.57748, -1.46027, -1.82635 -0.412529, -2.6574, -1.01641 -0.635894, -2.69111, -0.89684 --1.44137, -1.28859, -1.95776 --1.67582, -2.06069, -1.7772 -0.49238, -2.75681, -1.00785 -0.657363, -2.69764, -0.93964 --1.66545, -1.39435, -1.64454 -0.511859, -2.30044, -1.10823 -0.109108, -1.10768, -0.0743721 -0.155856, -1.11941, -0.0621604 -0.484087, -2.44429, -1.12428 --0.844279, -1.22598, -0.202645 -0.31767, -3.87152, 0.723305 -0.375171, -2.44916, -1.10788 --0.945312, -1.91351, -0.0889089 --1.42821, -1.53345, -1.86101 -5.06724, -14.9337, -10.4133 -132.54, -270.303, -153.918 -1.17831, -2.07037, -1.80748 -0.587511, -2.74979, -0.728655 -0.505538, -2.47432, -0.0697561 --1.27553, -1.69099, -1.84248 -0.635643, -2.70506, -0.778864 -0.648025, -2.60723, -0.941679 --1.40828, -2.14717, -1.8302 --1.71881, -2.14879, -1.83991 -0.623153, -2.54902, -0.985021 -0.617563, -2.48349, -0.912917 --0.0765279, -2.18403, 0.0424465 --1.75546, -1.48626, -1.55569 -0.286607, -1.39504, -1.39997 -0.625858, -2.42713, -1.00373 -0.325847, -2.88168, -0.880273 -1.2434, -3.06653, -2.33767 -0.637133, -2.38536, -1.0764 --1.46593, -1.27263, -1.62872 --1.26409, -1.44318, -1.67544 --1.40022, -1.7047, -1.84791 --1.2059, -1.48631, -1.67829 -0.156573, -1.85845, -1.87879 -2.84168, -8.89602, -6.56955 --1.28909, -1.8976, -1.70766 -0.826475, -1.88031, -1.70029 -0.839998, -1.93576, -1.72258 -0.587538, -2.53055, -0.9319 -0.672903, -2.80387, -0.811925 -0.507639, -2.44991, -0.0733367 --0.786536, -0.867885, -0.229798 --0.0742824, -2.22475, 0.0516017 --0.898756, -1.18573, -0.113855 --0.975984, -1.11408, -0.125057 -0.0291447, -2.11854, 0.115498 --1.1097, -1.24015, -1.59297 --1.29071, -1.4931, -1.72375 --1.46497, -1.61159, -1.78497 --4.74939, -7.97098, -6.18853 -0.340192, -1.84042, -1.86485 -0.331902, -1.71891, -1.78067 -0.656072, -1.61654, -1.5451 --1.55788, -0.832708, -0.369796 --0.89241, -1.65082, -0.136284 --0.897028, -1.20577, -0.0998574 -0.132952, -1.11406, -0.0606369 --1.06238, -1.13937, -1.5208 --1.11206, -1.29707, -1.59894 -0.393839, -2.25489, -2.13717 -2.72265, -8.58936, -6.45607 --1.22028, -1.72302, -1.58877 -1.15863, -2.0605, -1.79512 -0.867834, -2.86124, -0.846726 --0.723901, -1.17853, -0.214512 --0.999187, -1.17509, -0.11402 --0.90325, -0.945078, -0.148576 -0.137915, -1.11585, -0.0613143 --0.779505, -0.534603, -1.0791 -0.712064, -2.70587, -0.781884 --0.717238, -1.11174, -0.221516 --0.711007, -1.14006, -0.0556343 --1.74921, -1.50944, -1.62652 --1.83849, -1.56876, -1.62587 --1.25649, -1.76942, -1.61413 -0.483312, -2.80289, -0.993997 -0.488556, -2.78339, -0.978549 -0.741646, -2.5302, -0.940533 --0.735695, -1.11121, -0.207924 -0.24155, -2.51442, 0.0837283 --0.937225, -1.14453, -0.127499 --0.0200734, -1.0893, -0.0720193 -0.982621, -2.90908, -0.943357 --0.972245, -1.12461, -0.317176 --1.03397, -1.39824, -0.0605464 --1.40995, -2.21201, -1.85823 -0.715909, -2.01713, -1.7813 -0.765233, -2.04245, -1.79566 --1.59752, -2.10426, -1.78612 -0.213837, -2.21293, 0.0297057 --0.960609, -1.08649, -0.141929 --1.66138, -1.99218, -1.72539 -0.506176, -2.47285, -0.0707206 --0.828415, -1.22037, -0.176479 -0.23073, -2.07661, -1.97148 --1.31341, -1.93172, -1.86081 --0.966899, -1.29851, -0.16716 --0.926958, -1.19869, -0.163313 -0.0675943, -1.84852, -1.86961 --1.23602, -1.94574, -1.71449 -0.971526, -2.83622, -0.985463 -0.621339, -2.54103, -0.674768 --0.82739, -1.41594, -0.176181 --0.872974, -1.26497, -0.0687413 --1.20224, -1.6368, -1.70657 --1.50208, -1.96813, -1.72631 -0.933698, -2.74065, -0.969177 --0.749962, -1.11622, -0.224818 --0.884433, -1.14222, -0.179484 --1.0019, -1.32175, -0.0770662 --0.997963, -1.17297, -0.316713 --0.872856, -1.33184, -0.179692 --0.959888, -1.3308, -0.159669 -0.169042, -2.26357, 0.0301248 -0.0611106, -1.11796, -0.0613339 -0.662356, -3.09912, -1.25824 --1.02877, -1.06004, -1.44778 --8.26289, -15.7956, -11.9526 -5.58291, -17.7037, -13.1022 -0.610838, -2.11417, -1.8428 -0.654935, -2.72895, -0.98201 -0.721275, -2.72269, -0.985059 --0.965094, -1.11783, -0.335923 --0.787941, -1.61172, -0.148453 -3.47095, -10.3798, -7.65285 -1.37788, -3.52636, -2.98935 -0.507234, -2.45252, -1.0468 --1.00053, -1.20235, -0.329382 --0.976334, -1.15412, -0.321681 --0.856691, -1.27463, -0.168699 --1.02944, -1.05462, -1.4463 --1.0539, -1.14806, -1.48902 --1.4031e+07, -2.7601e+07, -1.98599e+07 -0.960752, -2.25535, -2.1234 --1.17574, -1.90345, -1.69582 -0.66924, -2.81126, -0.794644 --0.963114, -1.10851, -0.349617 --0.977757, -1.12727, -0.127572 --1.00575, -1.18734, -0.0862543 --1.05702, -1.0941, -1.49395 -0.567053, -3.05388, -0.996279 --0.962509, -1.11782, -0.324294 --0.207811, -1.97738, 0.0751855 -0.402497, -1.70085, -1.76317 -1.11189, -2.22615, -2.12381 -0.763405, -2.81748, -0.796728 --0.856326, -0.85125, -0.351325 --1.00946, -1.19939, -0.106197 --0.0401002, -1.81194, 0.0400807 --1.55759, -2.41709, -1.98825 -0.553529, -2.32703, -0.995049 --0.249274, -1.19893, -0.0275643 --2.45304, -2.40796, -2.08647 -1.08795, -2.72322, -0.727456 -0.443024, -2.64914, -1.14651 -0.13505, -1.923, -1.81746 -2.85579, -10.432, -7.70769 -5.47281, -16.6046, -12.2984 -2.27833, -6.3708, -4.94412 -2.33709, -3.60232, -3.18808 -0.0262771, -2.08398, -1.78465 -1.33648, -3.56466, -2.77804 -5.06844, -10.2453, -7.16892 --1.68805, -2.11401, -1.81458 -1.12066, -2.36435, -1.97974 -1.10314, -2.05984, -1.79025 --1.58094, -0.851167, -0.372697 --1.57216, -0.843075, -0.365754 -1.10552, -2.66947, -2.43394 -2.30833, -8.64763, -6.47671 -2.93107, -8.51968, -6.36354 -0.940436, -1.90197, -1.91061 --1.81091, -2.31052, -1.94165 --0.662733, -0.950344, -0.267005 --0.0788516, -2.19395, 0.0438641 --0.924061, -1.2629, -0.146677 -0.0277708, -2.13497, 0.11913 --1.04066, -1.07978, -1.44588 -1.3649, -3.52288, -3.07054 --2.23286, -2.81839, -2.56975 -0.596436, -1.85044, -1.7769 --1.72942, -2.17691, -1.85361 --1.71505, -2.19725, -1.84115 --0.972317, -1.10973, -0.326088 --0.767014, -1.48396, -0.172492 --0.69571, -1.08154, -0.229192 --0.66439, -1.15877, -0.186649 --1.26446, -1.77452, -1.60893 --1.58857, -1.91706, -1.6979 --1.72729, -2.08275, -1.7904 --0.973814, -1.7097, -0.0811009 --1.77282, -2.16717, -1.84982 -0.848703, -2.81265, -0.843463 -0.0970971, -2.26942, 0.165801 --1.40038, -1.80209, -1.61853 -0.657301, -2.8821, -1.11516 --0.720402, -1.07544, -0.222004 -0.635913, -2.4713, -1.06371 -0.390031, -1.32772, -1.45595 -2.30319, -5.82879, -4.65825 -4.02506, -11.4555, -8.50886 -3.31355, -11.5646, -8.42145 -3.05315, -9.55314, -7.02892 -0.918684, -2.05415, -1.78814 -0.720342, -2.81539, -0.997484 -0.695602, -2.80814, -0.995847 -0.32412, -2.80763, -0.972903 -0.625409, -2.68879, -0.765962 --0.163452, -2.57374, 0.0163255 --1.02538, -1.22983, -0.0922194 -3.31984, -11.5468, -8.41158 -0.931701, -2.13163, -2.04602 -3.5418, -11.5379, -8.4043 -3.77156, -11.6053, -8.44142 -0.595096, -2.58649, -0.958055 -0.537739, -2.5422, -0.0602346 -0.0428448, -1.75972, -1.82321 -2.40654, -6.61528, -5.20635 -1.56395, -5.18469, -4.0009 -1.28347, -2.17415, -2.11444 -0.684822, -2.12348, -1.08684 -0.576857, -2.52993, -0.952365 -0.967372, -2.76034, -0.972438 --0.728395, -1.25993, -0.203133 --0.835889, -1.25245, -0.170723 -0.034102, -2.16409, 0.125385 -0.650489, -2.15894, -1.11059 -0.208366, -2.78781, -0.983784 -0.373924, -1.94495, -0.991174 -0.546087, -3.04952, -0.947164 -0.000234109, -2.77248, -0.742133 -1.77116, -2.29718, -0.435836 --0.885473, -1.41625, -0.128801 -1.0498, -2.67583, -0.714292 --0.858361, -1.31391, -0.154846 -0.626605, -2.50601, -0.951024 -0.757123, -2.82433, -0.734106 -0.988633, -1.82101, -1.79959 -7.53107, -13.3197, -10.0039 -1.70708, -3.4673, -2.84383 -7.05972, -11.3492, -8.38392 -0.700185, -2.7286, -0.988705 -0.578989, -2.65062, -0.88614 --0.933784, -1.21803, -0.160588 --0.888834, -1.17698, -0.17119 --0.939496, -1.07907, -1.26703 -1.05653, -2.34504, -2.11821 -1.14039, -1.75808, -1.81173 -12.5441, -19.5545, -14.5712 -0.880087, -2.87099, -0.855993 --0.965052, -1.28893, -0.165009 --0.110009, -1.13443, -0.0644608 -0.713642, -1.46185, -1.55763 -1.10578, -1.83153, -1.84111 -0.451113, -2.69905, -0.928418 -0.728858, -2.74131, -0.92781 --0.833061, -1.33382, -0.167631 -1.07408, -2.88296, -0.797592 --0.748137, -1.31253, -0.203949 -1.08599, -3.10476, -0.923781 -0.145142, -2.75653, -0.83532 -0.857283, -1.85423, -1.81023 -1.01332, -1.80984, -1.79034 -1.13361, -1.78313, -1.79957 -1.04261, -1.83857, -1.78444 -1.16897, -1.78272, -1.79799 -1.18451, -1.90864, -1.90261 -1.14533, -1.8399, -1.84608 --0.720529, -1.20592, -0.2023 --0.914984, -1.21544, -0.183194 --1.6226, -1.97523, -1.70139 -0.268812, -2.77449, -1.01763 -0.205619, -2.76575, -0.99802 -0.54214, -2.51828, -0.630683 --0.780808, -1.5656, -0.157171 --0.704107, -1.14705, -0.22783 --0.912989, -1.2695, -0.172551 --0.167589, -1.10711, -0.0774996 -0.0934939, -2.2649, -1.94885 --1.56573, -1.84435, -1.61472 -1.01367, -1.67004, -1.73039 -1.10398, -1.7989, -1.81241 -0.507095, -2.5132, -0.686511 --0.863472, -1.33528, -0.14654 -1.18618, -2.12324, -1.83251 --0.952972, -1.44819, -0.175606 --0.0152651, -2.01007, 0.0775973 -0.436049, -2.93565, -0.902967 -0.727684, -2.81376, -0.695259 -0.708013, -2.55907, -1.14436 --1.56834, -2.06095, -1.74237 -0.947431, -1.80836, -1.81217 -1.03489, -1.82099, -1.82351 -1.03025, -1.77796, -1.78535 -1.06791, -1.75516, -1.77509 -1.03646, -1.79473, -1.77776 -1.11142, -1.83082, -1.80841 -0.473907, -2.70734, -0.986817 -0.682659, -2.72194, -0.984361 --0.879291, -1.13234, -0.196581 -0.566843, -2.6177, -0.843398 -0.899411, -2.70895, -0.745031 --1.21952, -2.04383, -1.77223 --0.886243, -1.2023, -0.190463 --0.840671, -1.22636, -0.182166 --0.846326, -1.17681, -0.0947647 -1.12666, -1.78486, -1.79917 -0.83408, -1.55763, -1.61338 -1.1466, -1.75747, -1.81115 -0.461758, -2.81216, -0.914794 -0.539881, -2.52835, -0.0580914 -0.87886, -2.81543, -0.976283 --0.784412, -1.53232, -0.151991 --0.746162, -1.11349, -0.220418 --0.794593, -1.46443, -0.114581 --1.21281, -2.05283, -1.75059 --1.21724, -1.95997, -1.65857 -0.0494307, -2.09106, -1.7817 --1.23613, -1.92049, -1.62856 --1.30188, -1.95962, -1.65317 --1.36069, -1.9904, -1.68093 -0.957464, -1.78362, -1.82336 -0.814479, -1.76791, -1.73182 -0.909917, -1.89986, -1.82082 -0.944942, -1.75718, -1.75355 -0.161515, -2.52289, -2.00683 -0.226736, -2.6476, -2.09798 -1.00101, -1.82068, -1.79186 -0.2396, -2.46305, -1.95946 -1.00707, -1.83857, -1.78405 -0.378995, -2.38543, -1.90585 -1.21686, -1.817, -1.8335 -4.34634, -8.76185, -6.92655 -0.83432, -2.05396, -1.8208 -1.20919, -1.87876, -1.85513 -1.11825, -1.94339, -1.84527 -1.16254, -1.87946, -1.82062 -0.834761, -2.09066, -1.81859 -1.18057, -1.9668, -1.86437 -1.17387, -1.92872, -1.83071 -0.960347, -2.02953, -1.79672 -1.11937, -2.05057, -1.80052 -1.14215, -2.05532, -1.80468 -1.16979, -2.07009, -1.79703 --0.114294, -2.80171, -1.0258 -0.792484, -2.85078, -0.967492 -0.660492, -2.79825, -0.867859 -0.955073, -2.79486, -0.750808 -0.149381, -2.43037, 0.0639562 -0.159749, -2.10174, -0.00205407 --0.886151, -1.1975, -0.166082 --1.2526, -2.07386, -1.76329 --1.23816, -2.00339, -1.6951 -0.858403, -1.8828, -1.80779 -0.0612149, -2.31868, -1.86409 -0.187659, -2.43361, -1.93724 -1.01253, -1.85516, -1.79717 -1.04178, -1.77634, -1.77238 -1.11463, -1.90857, -1.84494 -0.710356, -2.71161, -0.930189 -0.696323, -2.86469, -0.824732 --1.26061, -2.00811, -1.70748 -1.00131, -1.76962, -1.76385 -1.15554, -1.73871, -1.79461 -1.28249, -2.18386, -1.88925 -0.606382, -2.54578, -0.941899 -0.669233, -2.82666, -0.871474 -0.681714, -2.82621, -0.79397 -0.902171, -2.70581, -0.750973 -0.219137, -2.43599, 0.0327934 --0.92975, -1.2767, -0.161754 --0.928289, -1.24976, -0.168074 --0.834726, -1.23116, -0.150719 -0.677025, -2.64964, -0.93802 -0.876218, -2.74717, -0.769397 -0.901796, -2.70579, -0.75943 -0.229093, -2.44733, 0.0423172 -0.0426175, -2.2085, 0.139274 -1.17439, -2.10146, -1.81532 -0.520937, -2.44347, -1.05698 -0.684427, -2.85741, -0.845163 --0.767991, -1.47114, -0.178515 --0.542411, -2.80364, -0.904306 --0.802484, -1.30615, -0.15416 --0.970791, -1.20337, -0.106534 -0.230857, -2.35608, 0.066624 --0.7681, -1.29599, -0.172958 -0.0473768, -2.06536, -1.76417 --1.00568, -2.42029, -1.25573 -0.480277, -2.71016, -0.986988 -0.569666, -2.67408, -0.769641 -0.6791, -2.05845, -1.7284 -0.778919, -2.87562, -1.03563 --0.532262, -2.79777, -0.423035 --1.02018, -2.47058, -1.27489 -0.426005, -3.00812, -1.04405 -0.0381411, -2.39139, 0.0378221 -0.709526, -2.59378, -0.940442 --0.248856, -2.3649, 0.00105505 -0.20802, -2.40439, 0.0334907 -0.0017054, -2.20235, 0.0208631 -0.454334, -2.74242, -0.952519 -0.543508, -2.57067, -0.727774 --0.210502, -2.79733, -1.0143 --1.69827, -2.17201, -1.72137 -0.471983, -2.41939, -1.9417 -1.22741, -1.86247, -1.85001 --0.139049, -2.78326, -1.00452 --0.110177, -2.81132, -1.00078 -0.229466, -2.83219, -0.981073 --0.866511, -1.25804, -0.16205 -1.29874, -1.93073, -1.94347 --1.0178, -2.46244, -1.27211 --0.269386, -2.29645, -0.873341 --0.857416, -1.25335, -0.164997 --0.954128, -1.23757, -0.138827 --0.744603, -1.17796, -0.180608 --0.520563, -2.95632, -0.419731 -0.185575, -2.37167, 0.0104228 --0.87617, -1.2595, -0.168002 --0.916867, -1.24444, -0.173036 --0.857435, -1.26745, -0.163225 --0.984386, -1.18912, -0.117837 --0.667561, -1.14582, -0.181571 --0.858306, -1.19819, -0.0836961 --0.0697125, -2.87207, -1.04409 -0.0326684, -2.25818, 0.0461135 --0.863209, -1.25285, -0.165251 -0.0904124, -2.6677, -1.00235 --0.0681433, -2.24066, 0.0580481 --0.756237, -1.30522, -0.147462 -0.470805, -2.59171, -1.03373 --0.980718, -1.51886, -0.156362 --1.70218, -2.19596, -1.80028 --1.69423, -2.19, -1.77771 -0.817812, -2.20569, -1.75258 -0.844439, -2.07134, -1.68728 -0.894557, -2.08499, -1.70052 -0.882736, -2.13834, -1.72 -0.964451, -2.17574, -1.75079 --0.0980216, -2.90224, -1.04764 -0.719466, -2.67768, -0.947628 -0.667627, -2.80291, -0.811059 -0.563764, -2.50242, -0.726988 --0.357958, -6.03915, -0.372695 --0.509385, -2.7434, -0.427559 -0.0496223, -2.22792, 0.373542 -0.871816, -2.06778, -1.71386 -0.88741, -2.05467, -1.70127 -0.968055, -2.18926, -1.76515 -0.298574, -2.80324, -0.973465 -0.640669, -2.56691, -0.905387 -0.652604, -2.7988, -0.792107 -0.12806, -2.48296, 0.0252469 -0.0461107, -2.20513, 0.235999 -0.0435644, -2.22615, 0.370358 -0.657008, -2.57479, -0.910151 -0.776364, -2.63923, -0.701203 --0.510625, -2.67628, -0.426992 --0.159458, -2.58807, 0.021185 -0.141386, -2.50953, 0.0323524 --0.191469, -2.408, 0.00170888 --0.0716897, -2.22849, 0.0531132 -0.598508, -2.55622, -0.945784 -0.559919, -2.6312, -0.766058 -0.9654, -2.79695, -0.977546 -0.538316, -2.62318, -0.792317 --0.212939, -2.37839, -0.00120621 -0.637199, -2.70856, -0.733625 -1.11366, -1.74127, -1.76059 -1.34037, -1.89242, -1.93897 -1.10783, -1.89848, -1.76224 -3.05455, -3.12564, -3.22232 -2.50973, -3.73432, -3.29361 -1.18577, -2.05504, -1.81718 -1.22859, -2.11376, -1.84237 -0.431846, -2.69925, -1.01609 -0.135302, -2.48626, 0.0261229 -1.10764, -1.76269, -1.76501 -1.16261, -1.97713, -1.80902 -0.0539164, -2.81324, -1.02308 -0.848796, -2.91962, -1.02577 -0.651658, -2.66813, -0.902672 -0.737362, -2.66898, -0.954131 -0.608712, -2.55468, -0.700886 -0.424525, -2.34049, -0.102619 --0.166066, -2.64277, 0.00997866 -0.698228, -2.86568, -0.83589 -0.299449, -2.65613, -1.01757 -0.316625, -2.79211, -0.984407 -0.918384, -2.79574, -0.780333 -1.07329, -3.23192, -0.771114 -0.222054, -2.26326, 0.0448299 -0.549134, -2.46421, -0.943071 -0.0345374, -2.21069, 0.14222 -0.518884, -2.57201, -1.04615 -0.943993, -3.13322, -0.821349 -0.64449, -2.71252, -0.704103 -0.215238, -2.31421, 0.053824 -0.0559112, -2.26246, 0.176681 -0.572575, -2.3517, -1.07123 -1.36, -1.94143, -1.95516 -1.30757, -1.97268, -1.91306 -1.11269, -1.92128, -1.76824 -1.29372, -2.03945, -1.9003 -1.20602, -2.00793, -1.83299 -1.22473, -2.05869, -1.8461 -1.01464, -2.13559, -1.70939 --0.110936, -2.76647, -0.975385 -0.651364, -2.78423, -0.852572 -0.10404, -2.34626, 0.0408826 -0.113826, -2.32623, 0.0375896 -1.30264, -1.91756, -1.90915 -1.20786, -1.90674, -1.84219 -1.17983, -2.05669, -1.81108 -1.12822, -2.04263, -1.78038 -1.70719, -2.54841, -2.24241 --0.135993, -2.72612, -0.977105 -0.0449932, -2.71815, -0.888879 -0.538471, -2.51651, -0.726377 -0.934636, -2.72417, -0.771911 -0.499576, -2.44228, -0.0776824 -0.190348, -2.38294, 0.00872239 -0.0168044, -2.2309, 0.0416436 -1.15787, -2.11483, -1.81489 -0.671772, -2.80601, -0.791386 -0.72292, -2.70607, -0.759496 -0.801527, -2.67597, -0.700374 --0.196808, -2.37385, -0.00809458 -0.763983, -2.82017, -0.944397 -0.66936, -2.4191, -0.706914 --0.21843, -2.33767, 0.00694264 -0.683299, -2.84359, -0.830665 -0.990232, -2.82136, -0.79023 --0.0224802, -2.09249, 0.0863806 -0.506886, -2.50129, -0.644037 -0.0727704, -2.42886, 0.0517624 --0.251323, -2.83655, -1.03708 -0.869483, -2.79565, -0.868401 -0.509351, -2.73382, -0.657393 -1.15009, -1.83697, -1.79882 -1.15248, -1.81649, -1.78375 --1.57887, -2.05413, -1.67467 --1.8649, -2.22768, -1.88882 -1.15996, -2.03537, -1.78992 -0.501657, -2.4233, -0.900999 -0.771529, -2.81033, -0.959898 -1.23159, -1.93768, -1.85871 -1.25149, -2.05898, -1.86794 -1.37916, -2.16859, -1.97587 -0.273358, -2.63991, -1.01859 -0.575888, -2.51773, -0.915625 -0.713202, -2.73706, -0.937467 -0.716252, -2.90155, -0.831135 --0.123479, -2.77106, -0.985702 -0.21063, -2.79183, -0.975111 -0.541594, -2.60178, -0.815319 -0.492007, -2.46722, -0.0777181 --0.237245, -2.54032, -0.859936 -0.472167, -2.83954, -0.924775 -0.523124, -2.43317, -0.682662 --0.0115577, -2.76056, -0.910351 -0.58859, -2.68118, -0.827301 -0.625288, -2.71044, -0.778252 -0.58252, -2.22131, -0.0132044 --0.0740499, -2.22225, 0.0507967 -0.0421396, -2.20255, 0.238009 --0.770601, -1.50895, -0.173263 -0.0571027, -2.55999, -0.974116 -0.80104, -2.76277, -1.01094 -1.17984, -2.03129, -1.80662 --1.73238, -2.18207, -1.8196 --1.63191, -2.09694, -1.71262 -1.15266, -2.12817, -1.79985 -0.0320623, -2.85793, -1.3093 -0.335271, -2.8665, -1.05723 -0.393575, -2.70788, -0.994254 -0.186001, -2.76717, -0.975048 --0.0999496, -2.64675, -0.862901 -1.33776, -2.79014, -0.973426 -0.724933, -2.91179, -0.921563 -0.836459, -3.04758, -0.922668 -0.671141, -2.80776, -0.811945 -0.516155, -2.7405, -0.763569 -0.649447, -2.65629, -0.69941 -0.748497, -2.80078, -0.720939 -0.770913, -2.8151, -0.723545 --0.138199, -2.82885, -0.020173 -0.515619, -2.46094, -0.0714721 --0.142528, -2.87793, 0.090628 -0.0943708, -2.39581, 0.482351 --1.59004, -2.06198, -1.70242 -1.17797, -2.16824, -1.81705 -0.0447399, -2.83659, -1.32423 -0.94361, -3.10759, -1.07956 -0.779787, -2.85196, -0.941324 -0.629103, -2.73646, -0.875046 -0.633562, -2.71259, -0.812731 -1.10125, -2.97618, -0.784287 -0.98613, -2.60597, -0.693587 -0.608709, -2.63726, -0.545039 -0.00343914, -2.45218, -0.0418986 -0.102022, -2.34338, 0.0409014 -0.580127, -1.83596, -1.36149 -0.405547, -2.70667, -1.0151 -0.394762, -3.07859, -1.10486 -0.0256733, -2.80917, -0.974295 -0.478746, -2.7297, -0.972375 --0.0872052, -2.68124, -0.865587 -0.614423, -2.71029, -0.8986 --0.145145, -3.48468, -0.927593 -0.533966, -2.52152, -0.778914 -0.604023, -2.61956, -0.783426 -0.655733, -2.69052, -0.774903 -0.546741, -2.53609, -0.729164 --0.160579, -2.54582, 0.0068746 -0.630589, -2.29279, -0.0119935 --0.190545, -2.40575, 0.000842061 -0.0822174, -2.75452, -0.827631 -0.510317, -2.3902, -0.706123 -0.510302, -2.48182, -0.0687628 -0.614269, -2.28336, -0.0155201 -0.9918, -2.81932, -0.970162 --0.15658, -2.55573, -0.736372 --0.166024, -2.52309, -0.00302366 -0.471393, -2.53548, 0.0474228 -0.0217252, -2.24984, 0.0411089 -0.203156, -2.53613, -0.971357 -0.654609, -2.68904, -0.983896 -0.566932, -2.6394, -0.80671 -0.175239, -2.48727, 0.0353346 --0.77572, -1.54531, -0.0391243 -0.591075, -2.28628, 0.00354427 -0.675049, -2.96576, -1.33383 --0.354496, -2.92617, -0.900117 -0.959216, -2.56753, -0.646146 --1.65828, -2.01472, -1.74655 --1.66399, -2.08343, -1.7847 -0.636756, -1.8642, -1.39133 --1.65195, -2.07885, -1.74003 --1.68999, -2.05981, -1.72782 --1.74824, -2.13836, -1.77835 -0.554108, -2.45075, -1.00827 -0.569702, -2.46584, -0.980091 -0.192567, -2.73682, -1.02186 -0.327078, -2.89566, -1.03684 -0.307849, -2.74517, -0.985076 -0.542293, -2.52232, -0.914647 -0.043571, -2.71411, -0.886989 -0.548085, -2.55154, -0.654873 -1.51977, -2.63071, -0.649612 -0.529829, -2.4561, -0.102969 -0.177389, -2.62599, -0.06653 --0.15729, -2.84539, -0.00296827 --0.142691, -2.80885, -0.0101199 -0.193444, -2.63745, -0.0469652 -0.120658, -2.63431, -0.0390812 -0.211685, -2.60811, -0.0444154 -0.229576, -2.59073, -0.0268019 --0.164863, -2.60246, 0.0114767 --0.160337, -2.56387, 0.0126354 --0.0395355, -2.42521, -0.0160092 -0.094919, -2.31775, 0.0314856 --0.622075, -1.27953, -0.0168326 --0.620272, -1.28224, -0.0137861 --1.53842, -2.04017, -1.71271 --1.58622, -2.05785, -1.71433 -1.13975, -2.15165, -1.7758 --1.02511, -2.48572, -1.28289 -0.457781, -2.75859, -0.976083 -0.652599, -2.79742, -0.790276 -0.563283, -2.53241, -0.745469 -0.924265, -2.71157, -0.740454 -0.723467, -2.70539, -0.694387 -1.56956, -2.69179, -0.656403 -0.546751, -2.49856, -0.090825 -0.131822, -2.59488, -0.0395444 -0.227634, -2.59304, -0.0232224 -0.143727, -2.54825, -0.0238405 --0.162109, -2.54949, 0.01018 -0.231429, -2.45891, 0.0312758 --0.0770057, -2.2185, 0.0538787 --0.619744, -1.28076, -0.0156844 --1.87749, -2.16299, -1.88809 -0.562624, -2.45149, -0.970954 -0.665323, -2.65289, -0.972931 -0.580521, -2.58833, -0.937372 -0.0969084, -2.75306, -0.848163 -0.977192, -2.78344, -0.751654 -0.559946, -2.52237, -0.0856987 --0.140213, -2.82156, -0.00877044 -0.466025, -2.40914, -0.0911909 --0.172539, -2.82392, 0.0349807 -0.492604, -2.53404, -0.0158788 -0.149778, -2.57599, -0.0192486 --0.335087, -2.51992, -0.0307987 --0.130583, -2.65074, 0.0100526 --0.21578, -2.34448, 0.0107439 --0.1936, -2.21335, 0.239645 --1.80247, -2.21702, -1.83708 --1.61055, -2.0511, -1.4264 -0.534546, -2.41214, -1.00402 -0.55258, -2.43069, -0.961683 --0.533752, -2.67745, -0.832307 -0.669771, -2.62075, -0.654531 --0.476688, -2.77429, -0.62696 -0.980274, -2.62543, -0.634405 --1.6478, -2.08405, -1.47575 -0.546519, -2.42952, -0.707311 --0.219356, -2.81978, -0.59288 -0.595489, -2.45155, -0.430834 --0.196257, -2.41938, 0.00595142 -0.623878, -2.17176, -0.0279571 --0.188273, -2.21692, 0.366334 --0.0259986, -2.67015, -1.01542 -0.690812, -2.7628, -1.01008 -0.556099, -2.40981, -0.695998 -0.137339, -2.63753, -0.0662156 --0.076011, -2.82662, 0.0348176 --0.871321, -1.6059, -0.120722 --0.178738, -2.19587, 0.132728 --0.81496, -1.58875, -0.00710089 -0.288943, -2.80505, -1.02969 -0.909633, -2.77225, -0.984874 -0.544415, -2.31091, -0.6128 --0.357342, -2.53004, -0.0111008 -0.555308, -2.40477, -0.00505494 -0.695582, -3.1437, -0.685623 -0.376425, -2.00347, -0.0967757 --1.71015, -1.89318, -1.82377 --1.75942, -1.97242, -1.83164 --1.68247, -1.96444, -1.77228 --1.82737, -2.04666, -1.88263 --1.77545, -2.0844, -1.87778 --1.77644, -2.18615, -1.90923 --1.78991, -2.17031, -1.88994 -0.562353, -2.44816, -1.0387 -0.289092, -2.6577, -1.00024 -0.323357, -2.68839, -1.00799 -0.728515, -2.73732, -0.985667 -0.542799, -2.45943, -0.883254 -0.751767, -2.81527, -0.951133 -1.1381, -3.98929, -1.22152 -0.61749, -2.7626, -0.750652 -0.468415, -2.33839, -0.677711 -0.775032, -2.95695, -0.738921 -0.673135, -2.70236, -0.705825 -0.698726, -2.53625, -0.679773 -0.703247, -2.68143, -0.657801 -0.980859, -3.14222, -0.704536 -0.156361, -2.49934, -0.0779488 -0.095688, -2.89846, 0.117007 -0.0873133, -2.48969, 0.0314346 -0.044263, -2.21775, 0.366265 --1.70419, -2.07234, -1.81724 --1.78795, -2.10343, -1.85586 --1.77315, -2.12845, -1.84357 --1.92656, -2.28123, -1.97147 --1.86575, -2.18398, -1.8783 --1.60206, -2.06348, -1.70155 --1.85124, -2.19442, -1.87121 -0.0741349, -2.82844, -1.32554 -0.524857, -2.29895, -1.09831 -0.561036, -2.46737, -1.02656 -0.587183, -2.50781, -0.9905 -0.831465, -3.10659, -1.09707 -0.294508, -2.78248, -0.968131 -0.816759, -3.00142, -1.05649 -0.614833, -2.70148, -0.901389 -0.515814, -2.44286, -0.726491 -0.30114, -1.90305, -0.611869 -0.840118, -2.54609, -0.713996 -0.202871, -2.83145, 0.00966763 -0.15369, -2.77506, 0.00106078 --0.168922, -2.6376, 0.0223579 --1.42234, -1.9167, -1.58174 -0.201627, -2.58865, -0.953694 -0.327825, -2.93157, -1.04881 -0.657032, -2.72556, -0.960692 -0.56127, -2.49091, -0.725238 -0.201536, -2.94026, 0.0617256 -0.185398, -2.39145, -0.0585653 -0.271316, -2.67973, 0.0247693 -0.105929, -2.63782, 0.0289233 -0.721635, -2.24645, -0.0192945 -0.577625, -2.2832, -1.13517 -0.617158, -2.54818, -1.20668 -0.33499, -2.6968, -0.988388 -0.0233861, -2.78157, -0.968259 -2.04921e+06, -5.02356e+06, -1.42568e+06 -4.08095e+06, -1.05469e+07, -2.53059e+06 -0.733867, -2.87743, -0.830665 -0.0206143, -2.4952, -0.0425946 -0.475387, -2.51638, -0.0224232 -0.850921, -2.38623, -1.26196 -0.554437, -2.21651, -1.0975 -1.04755, -2.34052, -1.23568 -0.492077, -2.30471, -0.824047 -0.0554908, -2.41856, -0.814359 -0.406493, -2.78774, -0.875398 -0.676257, -2.81576, -0.794309 --0.472342, -2.75011, -0.636372 -0.593511, -2.50924, 0.0122636 -0.595656, -2.36173, -0.0101512 --0.185444, -2.19483, 0.231938 -0.953947, -2.30396, -1.2151 -1.0558, -2.32489, -1.22381 -0.887275, -3.0995, -1.01946 -1.00946, -2.82825, -0.962057 -0.451352, -2.93332, -0.895056 -0.662523, -3.11297, 0.245739 -0.561005, -2.31129, 0.00903446 -0.216969, -2.28959, 0.052317 -1.49767, -2.95115, -0.780804 -0.586642, -2.29562, 0.0122925 -0.10285, -2.54243, -0.992874 --0.223485, -2.33079, -0.0102769 -0.0105772, -2.244, 0.17581 --1.54482, -2.08446, -1.87202 --1.6065, -1.95721, -1.79829 --1.78269, -2.13949, -1.93454 --1.8323, -2.05147, -1.87517 --1.77044, -2.10881, -1.874 --1.78277, -2.06158, -1.84123 -0.328738, -2.81825, -1.00775 -0.740784, -2.98667, -1.03087 --1.79964, -2.05524, -1.87959 --1.80973, -2.16982, -1.92487 --1.70728, -2.06753, -1.76441 -0.786177, -3.12991, -1.06958 -0.677507, -2.82426, -0.929399 --0.210311, -2.35503, 0.011093 --1.30193, -1.86671, -1.54139 --1.82351, -2.10106, -1.88039 --2.02154, -2.30728, -2.06274 --1.83564, -2.14746, -1.88995 -0.672604, -2.81448, -0.792663 -0.462022, -2.2951, -0.116428 -0.2283, -2.45488, 0.037039 -0.472553, -2.69135, -0.921572 -0.67423, -2.73346, -0.784689 -0.263589, -2.69902, -1.02625 -0.812453, -2.94795, -0.979708 -0.547114, -2.17777, -0.0262522 -2.8635e+06, -8.37384e+06, -2.28634e+06 -1.36716, -2.81012, -0.75698 -1.85775, -2.38, -0.438704 -0.370454, -1.73713, -0.749893 -0.733411, -2.59518, -0.72404 --1.62532, -1.98652, -1.76359 -0.0904673, -2.36121, 0.038504 -0.111077, -2.34496, 0.0435115 --0.196469, -2.23388, 0.3763 -0.459692, -2.79131, -0.983584 -0.729771, -2.90143, -1.01213 -0.687156, -2.86284, -0.928331 -0.494509, -2.49341, -0.0723056 -0.0262665, -2.36043, 0.0352489 -0.633052, -2.19488, -0.0201912 -0.042199, -2.20768, 0.240944 -0.723878, -2.71727, -0.791734 -0.563676, -2.32339, 0.00875615 -0.0360318, -2.33159, 0.0591171 -0.199776, -2.58217, 0.160215 --0.161432, -2.16928, 0.123281 -0.300051, -2.83572, -0.872952 -0.364172, -2.78776, -0.868314 -0.793248, -2.98311, -0.870402 -0.307451, -2.72998, -1.00225 -0.327575, -2.76973, -0.993 -1.00756, -2.83131, -0.759257 -0.926486, -2.81287, -0.744119 --1.043, -2.5101, -1.29466 -0.692589, -2.6796, -0.650362 --1.20953, -2.23575, -1.77228 -0.427325, -2.83768, -1.08079 -0.493916, -2.71812, -0.995358 -0.496265, -2.71508, -0.920756 --0.098623, -2.72614, -0.87458 -0.674405, -2.82504, -0.794333 -1.02745, -2.95064, -0.74321 --0.0740666, -2.60722, -0.845034 -0.512328, -2.49526, -0.063651 -0.0223029, -2.82762, -1.04725 -0.772214, -2.54394, -0.686511 --0.17484, -2.19616, 0.349566 -0.367583, -2.28124, -0.823422 -0.454298, -2.76447, -0.841392 -0.180154, -2.33074, 0.0373423 --1.17193, -1.94201, -1.74711 --1.46312, -2.01812, -1.69822 --1.00109, -2.46134, -1.26556 -0.304968, -2.70905, -1.01457 -0.303264, -2.79768, -0.995865 -0.757483, -2.82848, -0.954185 -0.865287, -2.64919, -0.758017 -0.115375, -2.35138, 0.0468718 -0.30786, -2.70721, -1.01135 -0.384962, -2.73621, -1.00021 --0.190804, -2.40338, 0.00195807 -0.318451, -2.79386, -0.979082 -0.0468718, -2.17877, 0.226474 -0.176097, -2.86243, -1.08635 --0.0401571, -2.68016, -0.722048 -0.630616, -1.89913, -1.00583 -0.289119, -2.77912, -1.00492 -0.843389, -2.73236, -0.951737 -1.00089, -2.88722, -0.984078 -0.479287, -2.5932, -0.842986 -0.537636, -2.3708, 0.0262699 -0.301261, -2.81534, -1.00122 -0.755274, -2.82502, -0.956845 -0.669969, -2.8048, -0.801251 -1.24348, -2.75064, -0.729236 -0.784271, -2.66143, -0.622848 -0.787934, -2.67278, -0.626199 -0.0343274, -2.7421, -1.02033 -0.397406, -2.76686, -1.00252 -0.772716, -2.83612, -0.89084 -0.751294, -2.68085, -0.702159 -1.56443, -2.58179, -0.646787 -0.679413, -2.14326, -1.13397 -1.3755, -2.97533, -1.02466 -0.512413, -2.56828, -0.490852 -0.146146, -2.62077, -0.0334982 -0.190955, -2.61724, -0.0146892 -0.236953, -2.64038, 0.0111381 -0.796203, -2.34988, -0.0530686 -0.544875, -2.28541, -0.0431916 -0.644927, -2.18561, -0.0702447 -0.88303, -2.27273, -1.16724 -1.05643, -2.11042, -1.11163 -1.19248, -2.3127, -1.21942 -0.323071, -2.73828, -0.965962 -0.704887, -2.71117, -0.960561 -0.583003, -2.37344, -0.0325205 -0.616093, -2.26565, -0.020612 -0.999049, -3.2715, -1.68733 -1.70131, -4.47156, -2.26993 -0.965214, -2.74237, -1.382 -1.00781, -2.03066, -1.06948 -2.07942, -3.9721, -2.02581 -0.503115, -2.16289, -1.06497 -0.486934, -3.16533, -0.997279 -0.694161, -2.68977, -0.696415 -0.68083, -2.2926, -0.0668608 -0.967842, -2.08925, -1.10134 -0.512155, -2.07965, -1.02487 -0.678438, -2.51805, -0.639942 -0.793983, -2.85649, -0.678053 --0.0209033, -2.74961, 0.0847969 -0.680117, -2.35685, -0.0494755 -0.691835, -2.46852, -1.25997 -1.24184, -2.93668, -0.824637 -1.18183, -2.1469, -1.82258 -1.53474, -2.61833, -0.6519 -1.89959, -2.31077, -0.313288 -1.73979, -2.05743, -0.438493 -0.571626, -2.20604, -0.0195352 -1.46436, -2.69128, -0.636346 -1.68417, -1.99608, -0.44231 -0.786716, -2.30002, -0.0638977 -0.620207, -2.08563, -0.0561983 -0.679793, -2.13714, -1.09642 -0.476564, -2.66291, -1.01714 -0.493544, -2.71722, -0.892989 -0.752131, -2.68739, -0.694871 -1.702, -2.22516, -0.0403027 -1.21153, -1.42857, -0.0199917 -1.01215, -2.98055, -0.910511 -0.994808, -2.83408, -0.798242 -1.49841, -2.82041, -0.757851 -1.53345, -2.61235, -0.65219 -1.03584, -1.04706, -0.0319613 -0.833696, -2.8476, -0.962602 -1.04285, -1.07413, -0.0388502 -2.1729, -1.43079, -0.862515 -2.18708, -1.37732, -0.692607 -1.44524, -2.80701, -0.757017 -1.97512, -2.06393, -0.445387 -2.13763, -1.07521, -0.110368 -1.99445, -0.985954, -0.155085 -2.18875, -1.31627, -0.583465 -2.3185, -0.840405, 0.630537 -2.28305, -0.00116933, -1.34304 -2.14642, 1.15226, -1.1075 -2.30069, 0.201103, 0.584037 -1.06033, -0.793756, -0.0279378 -2.27152, 0.102779, -1.4994 -2.27792, -0.494478, -0.798088 -1.04146, -0.228721, -0.50476 -1.04402, -0.259069, -0.484196 -2.33486, 1.35548, -0.359509 -2.14517, 1.19643, -0.35027 -1.04284, -0.205978, -0.426532 -2.34037, -0.741256, -0.258063 -2.28498, -1.00316, -0.208506 -1.03944, -0.142431, -0.382431 -2.28297, 0.00455529, 0.382285 -1.07652, -0.791627, -0.0289351 -2.29927, 0.111402, -1.44225 -1.0422, -0.232897, -0.508686 -1.04003, -0.233276, -0.504386 -1.04428, -0.242294, -0.498512 -1.04258, -0.237872, -0.494459 -1.04021, -0.258398, -0.483515 -1.04106, -0.17004, -0.463288 -2.22222, -0.495784, -0.383652 -1.04045, -0.17111, -0.433007 -2.19117, 1.20658, -0.238415 -2.23151, 1.19704, -0.216631 -1.04105, -0.12978, -0.40082 -2.27556, -0.379862, -0.148234 -2.74496, -1.3038, 0.0408773 -3.57566, -1.59584, 0.257556 -1.04068, -0.275191, -0.380534 -1.04477, -0.16704, -0.366147 -1.0431, -0.263017, -0.36874 -2.148, 1.12341, 0.0664891 -1.04345, -1.05902, -0.0209607 -1.07726, -0.789307, -0.0285925 -1.07178, -0.794405, -0.0268862 -1.0405, -0.168287, -0.531973 -1.04072, -0.168243, -0.513754 -1.04205, -0.238264, -0.503531 -1.04016, -0.161618, -0.506325 -1.04087, -0.233371, -0.498585 -1.04112, -0.258205, -0.488863 -1.04687, -0.23724, -0.489171 -1.04066, -0.253426, -0.484009 -1.03545, -0.260499, -0.477385 -1.91652, -2.26441, -0.417779 -2.06496, -2.17017, -0.410994 -1.89014, -2.27275, -0.400413 -1.83862, -2.20407, -0.379207 -1.95717, 1.07195, -0.412386 -1.04027, -0.166667, -0.435556 -1.03907, -0.208693, -0.431322 -1.04308, -0.17971, -0.421486 -2.13682, -0.874567, -0.113845 -1.04, -0.26434, -0.388107 -1.04016, -0.26922, -0.385695 -1.03917, -0.259043, -0.385261 -1.04049, -0.16175, -0.375992 -1.0406, -0.264445, -0.374292 -1.0423, -0.168558, -0.368083 -1.05041, -0.124308, -0.344889 -1.03781, -0.181689, -0.350688 -2.33442, -0.44109, -1.49559 -2.30994, 0.320927, -1.52306 -2.31543, -0.70981, -1.49351 -2.30927, -0.669569, -1.24584 -1.04494, -0.186747, -0.592927 -1.03959, -0.254641, -0.568635 -1.0395, -0.247643, -0.522372 -1.04035, -0.23787, -0.503652 -1.04005, -0.233344, -0.497953 -1.04537, -0.268455, -0.489289 -1.03954, -0.180905, -0.471631 -1.03703, -0.16903, -0.458533 -1.0405, -0.239183, -0.436118 -0.973426, -0.229193, -0.421438 -2.49911, -1.2845, -0.0260691 -1.04241, -0.20302, -0.385955 -1.0323, -0.146874, -0.38482 -1.043, -0.166995, -0.380774 -2.6717, -0.280251, 0.0457762 -1.03756, -0.270025, -0.372454 -1.04229, -0.160708, -0.36208 -2.50976, -0.887549, 0.719506 -2.2939, -0.50395, -1.4636 -1.04192, -0.177385, -0.543112 -2.19057, -1.38772, -0.870785 -2.68606, -0.519574, -0.880657 -2.30506, -1.12174, -0.596498 -1.04394, -0.235063, -0.488934 -1.02623, -0.173678, -0.471431 -1.03705, -0.25338, -0.423369 -2.33532, -2.51259, -0.0995026 -2.25192, -1.07626, -0.193489 -2.30049, -1.23767, -0.0873761 -2.32964, -1.08873, -0.0677469 -1.04012, -0.277144, -0.371176 -1.042, -0.204538, -0.54279 -2.10146, -1.38965, -0.572039 -1.04927, -0.168141, -0.459842 -1.0382, -0.13381, -0.397252 -2.03061, 2.4979, -0.811256 -1.69452, 2.15759, -0.395884 -1.52442, 1.68817, -0.0711181 -2.51062, 1.01644, 0.837998 -1.03849, -0.150591, -0.52619 -1.05371, -0.157273, -0.522662 -1.03961, -0.211847, -0.396133 -1.04157, -0.257971, -0.37517 -2.10264, 0.811926, 0.627094 -2.18568, 0.62925, -1.43128 -1.03889, -0.167124, -0.519721 -1.03825, -0.149721, -0.510326 -1.07664, -0.125594, -0.494175 -1.04157, -0.254646, -0.483219 -1.92954, 1.73701, -0.133213 -2.09754, 1.073, 0.0463838 -1.04069, -0.160936, -0.361941 -1.03427, -0.237634, -0.353298 -1.05902, -0.140959, -0.529141 -1.02196, -0.15995, -0.503432 -1.99819, 2.18547, -0.898714 -1.03796, -0.236676, -0.487881 -1.03967, -0.25541, -0.480056 -1.04351, -0.18029, -0.471919 -1.03888, -0.214935, -0.43485 -1.02971, -0.200966, -0.436497 -1.04485, -0.166914, -0.435059 -1.04107, -0.252363, -0.421782 -1.89428, 1.78334, -0.213375 -1.04183, -0.24087, -0.398989 -1.04182, -0.216498, -0.398978 -1.02721, -0.178096, -0.402969 -1.03973, -0.167616, -0.380516 -1.04263, -0.253452, -0.368304 -2.40658, 1.36506, -0.724908 -2.19582, 1.28034, -0.56863 -1.04605, -0.206738, -0.459498 -2.14572, 1.27415, -0.380321 -2.06726, 1.19665, -0.333752 -1.03809, -0.166888, -0.423307 -1.90078, 1.01568, -0.261483 -1.04033, -0.20368, -0.398058 -1.04013, -0.215835, -0.385883 -1.04151, -0.203907, -0.386728 -1.04998, -0.0502188, -0.305521 diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/import_map_points.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/import_map_points.m deleted file mode 100644 index f36dff249e..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/cvs_computer_vision_system/import_map_points.m +++ /dev/null @@ -1,35 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%import_map_points.m -function map_points = import_map_points(filename) -fid = fopen(filename,'r'); -text = textscan(fid,'%s','Delimiter',','); -text = text{1}; -fclose(fid); - -numbers = single(str2double(text)); -num_points = length(numbers)/3; -map_points = reshape(numbers, [3,num_points])'; - -max_dist = 50; %mapping accidently finds points way too far away, lets remove those -[row, ~] = find(abs(map_points) > max_dist); - -remove_rows = unique(row); -map_points(remove_rows,:) = []; - -end \ No newline at end of file diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_hrn.slx b/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_hrn.slx deleted file mode 100644 index 6ad59d1750..0000000000 Binary files a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_hrn.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_init.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_init.m deleted file mode 100644 index bde845924c..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_init.m +++ /dev/null @@ -1,101 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%% mlp_mid_level_processor_init.m - -mlp_dummy_state_cmd = single(zeros(1,19)); - -%capture command plan -if isunix - [mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT '/sim_model/mlp_mid_level_processor_model/cmc_command_and_mode_control/scenario2.fplan']); -else - [mlp_command_list, mlp_command_times] = gen_cmd_list([ASTROBEE_ROOT '\sim_model\mlp_mid_level_processor_model\cmc_command_and_mode_control\scenario2.fplan']); -end - -%mlp_command_list = single[pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z,quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -%mlp_command_times = uint32[time_sec, time_nsec] - - -% BELOW COMMENTED OUT: is an alternative way to specify a command list -% mlp_command_times = [ -% % time_sec, time_nsec -% ini_time_seconds+0, ini_time_nanoseconds+0; ... -% ini_time_seconds+1, ini_time_nanoseconds+0; ... -% ini_time_seconds+2, ini_time_nanoseconds+14; ... -% ini_time_seconds+49, ini_time_nanoseconds+332179715; ... -% ini_time_seconds+50, ini_time_nanoseconds+332179715; ... -% ini_time_seconds+60, ini_time_nanoseconds+332179715; ... -% ini_time_seconds+61, ini_time_nanoseconds+332179715; ... -% ini_time_seconds+100, ini_time_nanoseconds+332179715; ... -% ini_time_seconds+101, ini_time_nanoseconds+332179715; -% ini_time_seconds+150, ini_time_nanoseconds+332179715]; -% mlp_command_times = uint32(mlp_command_times); -% -% mlp_command_list = [ ... -% % pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z -% 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; ... -% 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, -0.045518, 0.020690, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; ... -% -0.022760, 0.010346, 0.000000, -0.045518, 0.020690, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; ... -% -2.177241, 0.989655, 0.000000, -0.045518, 0.020690, 0.000000, 0.045518, -0.020690, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; ... -% -2.200000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; ... -% -2.200000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.050000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; ... -% -2.200000, 1.025001, 0.000000, 0.000000, 0.050000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; ... -% -2.200000, 2.975001, 0.000000, 0.000000, 0.050000, 0.000000, 0.000000, -0.050000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; ... -% -2.200000, 3.000001, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000; -% -2.200000, 3.000001, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.707000, 0.707000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000]; -% mlp_command_list = single(mlp_command_list); - - -%below is Jesse's hack to put in two extra commands before the rest -mlp_command_times = [mlp_command_times(:,1)+150 mlp_command_times(:,2)]; -mlp_command_times = [ini_time_seconds, ini_time_nanoseconds; ... - ini_time_seconds+30, ini_time_nanoseconds; - mlp_command_times]; - -mlp_command_list = [ ... - 0,0,0, 0,0,0, 0,0,0, 0,0,0,1, 0,0,0, 0,0,0; - 0,0,0, 0,0,0, 0,0,0, 0,0,0,1, 0,0,0, 0,0,0; - mlp_command_list]; - - -%Mode commanding -mlp_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 1000 , 0; ... - 1300 , 0]); -mlp_mode_cmd_times = [mlp_mode_cmd_times(:,1)+ini_time_seconds, mlp_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_mode_cmd_list = uint8([2; 2; 2]); %[] Mode Commands to be sent at specified times. -%2=nominal, 1 = stopped, 0 = idle - -%Speed Gain Commanding -mlp_speed_gain_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 1000 , 0; ... - 1300 , 0]); -mlp_speed_gain_cmd_times = [mlp_speed_gain_cmd_times(:,1)+ini_time_seconds, mlp_speed_gain_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_speed_gain_cmd_list = uint8([2; 2; 2]); %[] Mode Commands to be sent at specified times. - - -%localization mode commanding -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 1000 , 0; ... - 1300 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_loc_mode_cmd_list = uint8([0; 0; 0]); %[] Mode Commands to be sent at specified times. -%bit field: [7:x,x,x,x,3:handrail,2:docking,1:x,0:power] - diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_lib.slx b/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_lib.slx deleted file mode 100644 index 2ee1ed51f2..0000000000 Binary files a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_lib.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_prep.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_prep.m deleted file mode 100644 index 17440ee047..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_prep.m +++ /dev/null @@ -1,25 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%mlp_mid_level_processor_prep.m - -%add the initial sim time to the command_times -mlp_command_times = [mlp_command_times(:,1)+ini_time_seconds, mlp_command_times(:,2)+ini_time_nanoseconds]; - - -mlp_num_commands = single(size(mlp_command_list,1)); -mlp_num_mode_cmds = single(size(mlp_mode_cmd_list,1)); \ No newline at end of file diff --git a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_test.m b/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_test.m deleted file mode 100644 index 729eedd8f9..0000000000 --- a/gnc/matlab/sim_model/mlp_mid_level_processor_model/mlp_mid_level_processor_test.m +++ /dev/null @@ -1,116 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Simple Unit Test for the mid level processor - -%% init -disp('initializing...') -astrobee_init; -mlp_stop_time = 300; -cvs_noise_on = boolean(1); %[flag] 0=no position noise, 1=noise! - -mlp_loc_mode_cmd_times = uint32([ 0 , 0; ... %[sec] Sim time to set the current mode command to the cooresponding value in the mode command list - 100 , 0; ... - 200 , 0]); -mlp_loc_mode_cmd_times = [mlp_loc_mode_cmd_times(:,1)+ini_time_seconds, mlp_loc_mode_cmd_times(:,2)+ini_time_nanoseconds]; - -mlp_loc_mode_cmd_list = uint8([1; 5; 9]); %[] Mode Commands to be sent at specified times. -%1=power, 9=power&handrail, 5 = power&ARtags -astrobee_prep; - -%% Configure Dummy Waypoints -wpts_t_lin = [10 30 60]+60; -wpts_lin = [0 0 0; - 0 0 0; - 0 0 0]; -% wpts_lin = [1 0 0; -% 1 1 0; -% 1 1 1]; - -wpts_t_ang = [1 2 100 200 250]; -wpts_ang = [0 0 0; - 0 0 -0.0157; - 0 0 +0.0157; - 0 0 0; - 0 0 0]; -% wpts_ang = [0.15 0 0; -% 0 0 0; -% -0.15 0 0 -% 0 0 0; -% 0 0.15 0; -% 0 0 0; -% 0 -0.15 0; -% 0 0 0.15 ; -% 0 0 0; -% 0 0 -0.15; -% 0 0 0; -% ]; - -%% -disp('running sim...') -sim('mlp_mid_level_processor_hrn'); - -%% Plot data -disp('plotting...') -close all; - -figure; plot(out_cmc_msg.cmc_mode_cmd) - - -figure; plot(out_cvs_landmark_msg.cvs_valid_flag.Time, squeeze(sum(out_cvs_landmark_msg.cvs_valid_flag.Data,2))); -title('Total landmarks in view'); - -figure; subplot(3,1,1); plot(out_cvs_reg_pulse.cvs_landmark_pulse); -subplot(3,1,2); plot(out_cvs_reg_pulse.cvs_optical_flow_pulse); -subplot(3,1,3); plot(out_cvs_reg_pulse.cvs_handrail_pulse); - -figure; subplot(3,1,1); plot(out_cvs_landmark_msg.cvs_landmarks.Time, squeeze(out_cvs_landmark_msg.cvs_landmarks.Data(:,1,:))') -title('landmarks'); -subplot(3,1,2); plot(out_cvs_landmark_msg.cvs_landmarks.Time, squeeze(out_cvs_landmark_msg.cvs_landmarks.Data(:,2,:))') -subplot(3,1,3); plot(out_cvs_landmark_msg.cvs_landmarks.Time, squeeze(out_cvs_landmark_msg.cvs_landmarks.Data(:,3,:))') - -figure; subplot(2,1,1); plot(out_cvs_landmark_msg.cvs_observations.Time, squeeze(out_cvs_landmark_msg.cvs_observations.Data(:,1,:))'); -title('observations'); -subplot(2,1,2); plot(out_cvs_landmark_msg.cvs_observations.Time, squeeze(out_cvs_landmark_msg.cvs_observations.Data(:,2,:))'); - -figure; plot(out_cvs_opticalflow_msg.cvs_valid_flag.Time, squeeze(sum(squeeze(out_cvs_opticalflow_msg.cvs_valid_flag.Data(:,1,:)),1))); -title('Total OF points in view'); - -figure; subplot(2,1,1); plot(out_cvs_opticalflow_msg.cvs_observations.Time, squeeze(out_cvs_opticalflow_msg.cvs_observations.Data(:,1,1,:))'); -title('observations'); -subplot(2,1,2); plot(out_cvs_opticalflow_msg.cvs_observations.Time, squeeze(out_cvs_opticalflow_msg.cvs_observations.Data(:,2,1,:))'); - -figure; plot(out_cvs_handrail_msg.cvs_valid_flag.Time, squeeze(sum(out_cvs_handrail_msg.cvs_valid_flag.Data,2))); -title('Total handrail points in view'); - -figure; plot(out_cvs_handrail_msg.cvs_handrail_vector); -title('handrail vector'); - -figure; subplot(3,1,1); plot(out_cvs_handrail_msg.cvs_landmarks.Time, squeeze(out_cvs_handrail_msg.cvs_landmarks.Data(:,1,:))') -title('handrail - landmarks'); -subplot(3,1,2); plot(out_cvs_handrail_msg.cvs_landmarks.Time, squeeze(out_cvs_handrail_msg.cvs_landmarks.Data(:,2,:))') -subplot(3,1,3); plot(out_cvs_handrail_msg.cvs_landmarks.Time, squeeze(out_cvs_handrail_msg.cvs_landmarks.Data(:,3,:))') - -figure; subplot(3,1,1); plot(out_cvs_handrail_msg.cvs_observations.Time, squeeze(out_cvs_handrail_msg.cvs_observations.Data(:,1,:))'); -title('handrail - observations'); -subplot(3,1,2); plot(out_cvs_handrail_msg.cvs_observations.Time, squeeze(out_cvs_handrail_msg.cvs_observations.Data(:,2,:))'); -subplot(3,1,3); plot(out_cvs_handrail_msg.cvs_observations.Time, squeeze(out_cvs_handrail_msg.cvs_observations.Data(:,3,:))'); - -%% save data -% save('comp_vis_model_out.mat', 'cvs_truth_fixed_points_iss', 'cvs_truth_flow_points_iss', 'cvs_flow_ids', 'out_cvs_landmark_msg', 'out_cvs_opticalflow_msg', 'out_cvs_reg_pulse') - - diff --git a/gnc/matlab/sim_model/sim_model_lib.slx b/gnc/matlab/sim_model/sim_model_lib.slx deleted file mode 100644 index 1af91208cf..0000000000 Binary files a/gnc/matlab/sim_model/sim_model_lib.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_init.m b/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_init.m deleted file mode 100644 index c75df7d320..0000000000 --- a/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_init.m +++ /dev/null @@ -1,112 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%bpm_blower_propulsion_module_init.m - -%% General -bpm_use_bpm = single(0); %[flag] 1= use BPM, 0 = use VPP - -%tun_bpm_noise_on_flag = single(1); %[flag] 1=noise on, 0 = noise off - -%% impeller motor parameters -%Maxon EC45flat-30W brushless motor with Hall sensors -bpm_imp_ctl_ts = 0.016; %[sec] Time step size for the impeller speed PID controller -bpm_imp_ctl_kp = single(0.2); %[-] P gain for impeller speed PID controller -bpm_imp_ctl_ki = single(0.1); %[-] I gain for impeller speed PID controller -bpm_imp_ctl_kd = single(0.05); %[-] D gain for impeller speed PID controller -bpm_rate_limit = single(200*2*pi/60); % [rad/sec] Rate limit change of impeller speed command -bpm_imp_ctl_filt_n = single(1.0); %[-] Filter Coefficient for impeller speed PID controller -bpm_imp_speed_filt_num = single(1.0); %[-] Transfer function numerator for current impeller speed feedback -bpm_imp_speed_filt_den = single(1.0); %[-] Transfer function denominator for current impeller speed feedback -bpm_imp_cmd_filt_num = single(1.0); %[-] Transfer function numerator for commanded impeller speed -bpm_imp_cmd_filt_den = single(1.0); %[-] Transfer function denominator for commanded impeller speed - -bpm_imp_max_voltage = single(16.6); %[Volt] Maximum voltage for the impeller motor -bpm_imp_nom_voltage = single(12); %[Volt] Nominal voltage for the impeller motor -bpm_imp_nom_torque = single(0.0555); %[Nm] Nominal torque at voltage -bpm_imp_nom_curr = single(2.03); %[A] Nominal current at voltage -bpm_imp_nom_speed = single(2940*2*pi/60); %[rad/s] Nominal speed at voltage -bpm_imp_noload_curr = single(0.144); %[A] No load current -bpm_imp_noload_speed = single(4380*2*pi/60); %[rad/s] No load speed -%imp motor friction coeff in prep.m - -bpm_imp_motor_torque_k = single(0.0255); %[Nm/A] Impeller Torque constant -bpm_imp_motor_speed_k = single(374*2*pi/60); %[(rad/s)/V] Impeller Speed constant -bpm_imp_motor_r = single(1.2); %[ohm] Impeller Motor internal resistance -bpm_imp_motor_l = single(0.0056); %[H] Impeller Motor internal inductance -bpm_imp_motor_inertia = single(0.00000925); %[kg*m^2] Impeller motor intertia -bpm_imp_rotor_inertia = single(0.005); %[kg*m^2] Impeller rotor inertia - -%% servo parameters -%MKS DS92A+ servo -bpm_servo_ctl_ts = 0.016; %[sec] Time step size for the servo PID controller -bpm_servo_ctl_kp = single(5.0); %[-] P gain for servo PID controller -bpm_servo_ctl_ki = single(1.5); %[-] I gain for servo PID controller -bpm_servo_ctl_kd = single(0.8); %[-] D gain for servo PID controller -bpm_servo_ctl_filt_n = single(1.0); %[-] Filter Coefficient for servo PID controller - -bpm_servo_max_voltage = single(6); %[Volt] Saturation voltage for the servo controller output (voltage servo is running at) -bpm_servo_peak_torque = single(0.2893); %[Nm] from Datasheet @6V -bpm_servo_peak_speed = single(0.058/60); %[sec/deg] from Datahsheet @6V -bpm_servo_peak_curr = single(1.5); %[amp] (WAG) Peak current draw of servo -bpm_servo_noload_curr = single(0.01); %[amp] (WAG) Servo no load current draw -bpm_servo_motor_friction_coeff = single(0.0000003); %[Nm*s] (WAG) Due to Viscous Friction - -bpm_servo_motor_gear_ratio = single(1/100); %[-] (WAG) Servo gear ratio -bpm_servo_motor_backlash_deadband = single(1*pi/180); %[rad] (WAG) Servo gearbox backlash deadband -bpm_servo_motor_gear_box_inertia = single(.000000025); %[kg*m^2] (Tuned in analysis_servo_motor_test.m) Servo gearbox inertia. -%servo motor R and K in prep.m - -%% Impeller parameters -bpm_impeller_inertia = single(0.001); %[kg*m^2] Moment of inertia of the blower system -bpm_impeller_inertia_error = single(0); %[kg*m^2] Error in the MOI of the impellers -bpm_impeller_init_speed = single(0); %[rad/s] Initial condition for blower speed -bpm_impeller_eq_density = single(0.001); - -bmp_PM1_impeller_orientation_error = single([0 0 0]); %[vector] PM1 Orientation Error of the impeller rotational pointing unit vector -bmp_PM2_impeller_orientation_error = single([0 0 0]); %[vector] PM2 Orientation Error of the impeller rotational pointing unit vector -bpm_PM1_zero_thrust_area_error = single([0]); %[m^2] PM1 Error associated with the leakage area (zero thrust) -bpm_PM2_zero_thrust_area_error = single([0]); %[m^2] PM2 Error associated with the leakage area (zero thrust) - -%TODO: add the drag parameters (need to add drag model in still) - -%% Nozzle Parameters -bpm_PM1_P_nozzle_B_B_error = zeros(6,3,'single'); %[m] PM1 Position offset error of the nozzles -bpm_PM2_P_nozzle_B_B_error = zeros(6,3,'single'); %[m] PM2 Position offset error of the nozzles -bpm_PM1_nozzle_orientation_az_error = zeros(6,1,'single'); %[rad] PM1 Nozzle Orientation Azimuth Error -bpm_PM2_nozzle_orientation_az_error = zeros(6,1,'single'); %[rad] PM2 Nozzle Orientation Azimuth Error -bpm_PM1_nozzle_orientation_el_error = zeros(6,1,'single'); %[rad] PM1 Nozzle Orientation Elevation Error -bpm_PM2_nozzle_orientation_el_error = zeros(6,1,'single'); %[rad] PM2 Nozzle Orientation Elevation Error -bpm_PM1_nozzle_discharge_coeff_error= zeros(6,1,'single'); %[-] PM1 Error associated with the discharge coefficient -bpm_PM2_nozzle_discharge_coeff_error= zeros(6,1,'single'); %[-] PM2 Error associated with the discharge coefficient -bpm_PM1_nozzle_thrust_noise_1s = zeros(6,1); %[N] PM1 1-sigma value on noise for thrust -bpm_PM2_nozzle_thrust_noise_1s = zeros(6,1); %[N] PM2 1-sigma value on noise for thrust -bpm_PM1_nozzle_noise_feedback_gain = zeros(6,1); %[-] PM1 gain on the feedback on nozzle thrust noise -bpm_PM2_nozzle_noise_feedback_gain = zeros(6,1); %[-] PM2 gain on the feedback on nozzle thrust noise -bpm_PM1_randn_noise_seed = 254; %[-] PM1 Noise seed for the randn block -bpm_PM2_randn_noise_seed = 255; %[-] PM2 Noise seed for the randn block - -%% rotor speed sensor -bpm_sensor_sf = single(1); %[-] Scale factor for the speed sensor -bpm_sensor_sigma_u = .00001; %[-] Sigma U noise for the speed sensor -bpm_sensor_sigma_v = .00001; %[-] Sigma V noise for the speed sensor -bpm_sensor_rand_seed = 1337; %[-] Rand seed for the noise parameters -bpm_sensor_resolution = single(.1); %[-] Resolution per bit for the speed sensor -bpm_sensor_max = single(10000); %[rpm] Maximum output of the speed sensor -bpm_sensor_min = single(0); %[rpm] Minimum output of the speed sensor -bpm_sensor_delay = 1; %[sec] Delay time of the speed sensor - diff --git a/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_lib.slx b/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_lib.slx deleted file mode 100644 index 32144d2545..0000000000 Binary files a/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_lib.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_prep.m b/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_prep.m deleted file mode 100644 index 78a6376664..0000000000 --- a/gnc/matlab/sim_model/veh_vehicle_model/bpm_blower_propulsion_module/bpm_blower_propulsion_module_prep.m +++ /dev/null @@ -1,83 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%bpm_blower_propulsion_module_prep.m - -%% Noise warning -if (tun_bpm_noise_on_flag == 0) - warning('Propulsion Noise Off') -end - -%% motor parameters -bpm_imp_motor_friction_coeff = bpm_imp_noload_curr*bpm_imp_motor_torque_k/bpm_imp_noload_speed ; %[Nm*s] Viscous Friction - -%% servo parameters -bpm_servo_max_theta = abp_nozzle_gear_ratio*(abp_nozzle_max_open_angle - abp_nozzle_min_open_angle); -bpm_servo_min_theta = 0; -bpm_servo_pwm2angle = (bpm_servo_max_theta-bpm_servo_min_theta)/(abp_servo_max_PWM-abp_servo_min_PWM); %[-] Conversion between servo PWM command (0-100) and resulting angle (0-90) -bpm_servo_pwm2angle_bias = bpm_servo_min_theta/bpm_servo_pwm2angle - abp_servo_min_PWM; %[rad] Bias on the PWM->angle conversion -bpm_servo_motor_r = bpm_servo_max_voltage/bpm_servo_peak_curr; %[ohm] Servo internal resistance -bpm_servo_motor_k = bpm_servo_peak_torque*bpm_servo_motor_gear_ratio/(bpm_servo_peak_curr); %[Nm/A] Servo motor torque constant - -%% aerodynamics parameters -%setup input values based on density/max -bpm_impeller_eq_CQ_input = 0:bpm_impeller_eq_density:abp_impeller_eq_max_CQ; -%solve the equation for CP -bpm_impeller_eq_CP_output = polyval(abp_impeller_eq_CQ2CP_poly, bpm_impeller_eq_CQ_input); %[-] CP = f(CQ) -%calculate total area from equation A = (Cq*D*h)/sqrt(2*CP) -bpm_impeller_eq_totalarea_output = (bpm_impeller_eq_CQ_input*abp_impeller_diameter*abp_impeller_height)./sqrt(2*bpm_impeller_eq_CP_output); %[-] - -%change into lookup table parameters -bpm_lookup_Cdp_data = bpm_impeller_eq_CP_output; -bpm_lookup_totalarea_breakpoints = bpm_impeller_eq_totalarea_output; - -%% Nozzle Misalignment -% 1: Define a vector perpendicular to the sensor's sense axis, and randomly rotate it about the sense axis (360 degrees) -% 2: Rotate the sensors sense axis about that random perpendicular vector (by the misalignment angle) -for i=1:length(abp_PM1_nozzle_orientations) - - %Define an arbitrary vector - if abs(abp_PM1_nozzle_orientations(i,1)) ~= 1 - temp_vec1 = [1 0 0]; - else - temp_vec1 = [0 1 0]; - end - if abs(abp_PM2_nozzle_orientations(i,1)) ~= 1 - temp_vec2 = [1 0 0]; - else - temp_vec2 = [0 1 0]; - end - - % Find a perpendicular unit vector - temp_perp_vec1 = cross(abp_PM1_nozzle_orientations(i,:), temp_vec1); - temp_perp_vec2 = cross(abp_PM2_nozzle_orientations(i,:), temp_vec2); - temp_perp_vec1 = temp_perp_vec1/norm(temp_perp_vec1); - temp_perp_vec2 = temp_perp_vec2/norm(temp_perp_vec2); - - %Convert axis-angle to quaternion that rotates the perpendicular vector randomly around the nozzle unit vector (0-360 deg) - temp_rotation_quat1 = [sin(bpm_PM1_nozzle_orientation_az_error(i)/2)*abp_PM1_nozzle_orientations(i,:) cos(bpm_PM1_nozzle_orientation_az_error(i)/2)]; - temp_rotation_quat2 = [sin(bpm_PM2_nozzle_orientation_az_error(i)/2)*abp_PM2_nozzle_orientations(i,:) cos(bpm_PM2_nozzle_orientation_az_error(i)/2)]; - - %Rotate - temp_misalignment_rot_axis1 = quat_rotation_vec(temp_perp_vec1,temp_rotation_quat1); - temp_misalignment_rot_axis2 = quat_rotation_vec(temp_perp_vec2,temp_rotation_quat2); - - % Define the quat. to rotate the nozzle axis about the newly defined vector by the misalignment amount - bpm_PM1_Q_nozzle2misaligned(i,:) = [sin(bpm_PM1_nozzle_orientation_el_error(i)/2)*temp_misalignment_rot_axis1 cos(bpm_PM1_nozzle_orientation_el_error(i)/2)]; - bpm_PM2_Q_nozzle2misaligned(i,:) = [sin(bpm_PM2_nozzle_orientation_el_error(i)/2)*temp_misalignment_rot_axis2 cos(bpm_PM2_nozzle_orientation_el_error(i)/2)]; - -end \ No newline at end of file diff --git a/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_init.m b/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_init.m deleted file mode 100644 index 62c94d7bae..0000000000 --- a/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_init.m +++ /dev/null @@ -1,98 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Epson IMU Model init -% M-G362PDC1 -% Parameters derived from Datasheet - -%% General -%epson_Q_B2sensor_nom = [0 0 0 1]; %[quat] nominal orientation of the sensor -% Replaced with tun_abp_quat_body2imu - -epson_imu_misalignment = [0.01;0.01;0.01]; %[rad] -% epson_P_sensor_B_B_nom Set in tunable file -epson_P_sensor_B_B_error = [0.01,0.01,0.01]; %[m] -epson_output_delay = 0; %[steps] number of descrite steps to delay output - -% Moved to gnc.config -%tun_epson_report_truth = 0; %[flag] report truth gyro/accel measurements with no noise/bias/misalignment - -epson_no_rot_effects = 1; %[flag] remove the rotation effects from the accel reading - -%% Accel Properties -epson_accel_unit = (1/9.80665)*10^3; %[mg/(m/s^2)] output is in milli-g's, model is m/s^2 - -epson_accel_upper_sat = 3000/epson_accel_unit; %[m/s^2] -epson_accel_lower_sat = -3000/epson_accel_unit; %[m/s^2] -epson_accel_resolution = 0.125/epson_accel_unit; %[(m/s^2)/LSB] -% epson_accel_temp_res_coeff = 20; %[ppm/degC] Note: currently unused in model -epson_accel_nonlinearity_coeff = 0.001; %[0.1% of FS] - -epson_accel_orth_misalignment = [0.03;0.03;0.03]*pi/180; %[rad] -epson_accel_bias_ic = 0*[8,8,8]/epson_accel_unit; %[m/s^2] -epson_accel_temp_bias_coeff = [0.02,0.02,0.02]/epson_accel_unit; %[(m/s^2)/degC] - -epson_accel_bias_stab = 0.1/epson_accel_unit; %[m/s^2]; -epson_accel_velocity_rw = 0.04*sqrt(1/3600); %[(m/s)/sqrt(sec)] Note: datasheet discrepency in units (only value in m/s??) -epson_accel_noise_density = 0.1/epson_accel_unit; %[(m/s^2)/sqrt(hz)] -epson_accel_noise_seed = [4 5 6]; %[-] - -epson_accel_3db_bandwidth = 180; %[Hz] -epson_accel_sf_coef = 1; %[-] None specified. - -% Filter is unused since the 3db bandwidth (180Hz) is higher than our operating frequency (100Hz) -epson_accel_filt_num = [1]; -epson_accel_filt_den = [1]; - -% values taken from the imu allan variance analysis -epson_accel_sigma_u = sqrt(3/540)*(0.04/epson_accel_unit); %[(m/s^2)/sec^(3/2)] points taken from datasheet allan variance: 540s, 0.04mG -epson_accel_sigma_v = epson_accel_velocity_rw; %[(m/s)/sqrt(sec)] -epson_accel_sigma_w = 1.5*epson_accel_bias_stab; %[(m/s^2)/sec] -epson_accel_markov_tau = .08; %[sec] - -%% Gyro Properties -epson_gyro_unit = 180/pi; %[rad] output is in radians - -epson_gyro_upper_sat = 150/epson_gyro_unit; %[rad/s] -epson_gyro_lower_sat = -150/epson_gyro_unit; %[rad/s] -epson_gyro_resolution = 0.005/epson_gyro_unit; %[rad/s/LSB] -% epson_gyro_temp_res_coeff = 10; %[ppm/degC] Note: currently unused in model -epson_gyro_nonlinearity_coeff = 0.001; %[0.1% of FS] - -epson_gyro_orth_misalignment = [0.1;0.1;0.1]/epson_gyro_unit; %[rad] -epson_gyro_bias_ic = [0.1,0.1,0.1]/epson_gyro_unit; %[rad/s] -epson_gyro_temp_bias_coeff = [0.001,0.001,0.001]/epson_gyro_unit; %[rad/s/degC] - -epson_gyro_linear_accel_bias_coeff = (0.01/epson_gyro_unit)*(epson_accel_unit*10^-3); %[(rad/s)/(m/s^2)] Note: this value is a maximum. - -epson_gyro_bias_stab = (3/epson_gyro_unit)*(1/3600); %[rad/sec] -epson_gyro_angular_rw = (0.1/epson_gyro_unit)*sqrt(1/3600); %[rad/sqrt(sec)] 0.1 deg/sqrt(hr) -epson_gyro_noise_density = 0.002/epson_gyro_unit; %[rad/s/sqrt(hz)] 1 sigma -epson_gyro_noise_seed = [1 2 3]; %[-] - -epson_gyro_3db_bandwidth = 180; %[Hz] -epson_gyro_sf_coef = 1; %[-] none specified. - -% Filter is unused since the 3db bandwidth (180Hz) is higher than our operating frequency (100Hz) -epson_gyro_filt_num = [1]; -epson_gyro_filt_den = [1]; - -%values taken from the imu allan variance analysis -epson_gyro_sigma_u = 1.25*sqrt(3/500)*(3*(1/3600))/epson_gyro_unit; %[rad/sec^(3/2)] points taken from datasheet allan variance: 500s, 3deg/hour -epson_gyro_sigma_v = epson_gyro_angular_rw; %[rad/sqrt(sec)] -epson_gyro_sigma_w = 2.2*epson_gyro_bias_stab; %[rad/sec] -epson_gyro_markov_tau = .04; %[sec] diff --git a/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_lib.slx b/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_lib.slx deleted file mode 100644 index 8a14c9de58..0000000000 Binary files a/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_lib.slx and /dev/null differ diff --git a/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_prep.m b/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_prep.m deleted file mode 100644 index c3ddf93831..0000000000 --- a/gnc/matlab/sim_model/veh_vehicle_model/epson_imu_model/epson_imu_model_prep.m +++ /dev/null @@ -1,35 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Epson IMU Model prep - -%% misalignment -epson_Q_B2sensor_error = eulers_to_quat(epson_imu_misalignment(1),epson_imu_misalignment(2),epson_imu_misalignment(3)); -epson_Q_sensor2gyro_error = eulers_to_quat(epson_gyro_orth_misalignment(1),epson_gyro_orth_misalignment(2),epson_gyro_orth_misalignment(3)); -epson_Q_sensor2accel_error = eulers_to_quat(epson_accel_orth_misalignment(1),epson_accel_orth_misalignment(2),epson_accel_orth_misalignment(3)); - -epson_Q_B2gyro_error = quatmult(epson_Q_B2sensor_error, epson_Q_sensor2gyro_error); -epson_Q_B2accel_error = quatmult(epson_Q_B2sensor_error, epson_Q_sensor2accel_error); - -%% Moved to lua conig file [ASTROBEE_ROOT '/../../management/astrobee/config/gnc.config'] -%tun_epson_P_sensor_B_B = tun_epson_P_sensor_B_B_nom + tun_epson_imu_pos_error; %[m] nominal positon of the sensor - -%% Debug Warnings -if tun_epson_report_truth - warning('IMU Noise has been disabled'); -end - diff --git a/gnc/matlab/sim_model/veh_vehicle_model/veh_vehicle_model_lib.slx b/gnc/matlab/sim_model/veh_vehicle_model/veh_vehicle_model_lib.slx deleted file mode 100644 index e293224a61..0000000000 Binary files a/gnc/matlab/sim_model/veh_vehicle_model/veh_vehicle_model_lib.slx and /dev/null differ diff --git a/gnc/matlab/tunable_init.m b/gnc/matlab/tunable_init.m deleted file mode 100644 index 8025d4824b..0000000000 --- a/gnc/matlab/tunable_init.m +++ /dev/null @@ -1,51 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Dependencies: -% Requires astrobee_init and/or any prototype _init files to have run -% Run before astrobee_prep - -clear tun_* -lua_config_file_name = which('lua_config'); -if ~isempty(lua_config_file_name) % if the lua config file exists delete it - delete(lua_config_file_name); -end - -filename = [ASTROBEE_ROOT, '/luaconfig/export_config.lua']; -% generate lua_config.m with either lua or luajit -setenv('LUA_PATH', [ASTROBEE_ROOT, '/../../astrobee/config/?.config']) -setenv('ASTROBEE_CONFIG_VERSION', astrobee_version) -setenv('ASTROBEE_CONFIG_ROBOT', [ASTROBEE_ROOT '/../../astrobee/config/robots/sim.config']); -setenv('ASTROBEE_CONFIG_DIR', [ASTROBEE_ROOT '/../../astrobee/config']); -setenv('ASTROBEE_RESOURCE_DIR', [ASTROBEE_ROOT '/../../astrobee/resources']); -setenv('ASTROBEE_ROBOT', 'sim'); -setenv('ASTROBEE_WORLD', 'granite'); - -[status, ~] = system('lua -v'); -if status ~= 0 - luaVer = 'luajit'; -else - luaVer = 'lua'; -end - -[status, result] = system([luaVer,' ', filename]); -if status ~= 0 - disp(result); - assert(false); -end -run('lua_config'); % run generated file - diff --git a/gnc/matlab/utility_blocks/math_utils.slx b/gnc/matlab/utility_blocks/math_utils.slx deleted file mode 100644 index 06c1cfcc1d..0000000000 Binary files a/gnc/matlab/utility_blocks/math_utils.slx and /dev/null differ diff --git a/gnc/matlab/utility_blocks/util_generic_model_lib.slx b/gnc/matlab/utility_blocks/util_generic_model_lib.slx deleted file mode 100644 index a527ef30f7..0000000000 Binary files a/gnc/matlab/utility_blocks/util_generic_model_lib.slx and /dev/null differ diff --git a/gnc/matlab/utility_scripts/axisAngle2quat.m b/gnc/matlab/utility_scripts/axisAngle2quat.m deleted file mode 100644 index 6f39d8bdc5..0000000000 --- a/gnc/matlab/utility_scripts/axisAngle2quat.m +++ /dev/null @@ -1,26 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. -% -% Source: "Fundamentals of Spacecraft Attitude Determination and Control", -% Landis Markley and John Crassidis, eq. 2.124 -function q = axisAngle2quat(axis, angle) - -q(:, 1:3) = axis.*sin(angle/2); -q(:, 4) = cos(angle/2); - -q = quat_norm_and_sign_check(q); - diff --git a/gnc/matlab/utility_scripts/axis_angle_to_quat.m b/gnc/matlab/utility_scripts/axis_angle_to_quat.m deleted file mode 100644 index 51bde60e59..0000000000 --- a/gnc/matlab/utility_scripts/axis_angle_to_quat.m +++ /dev/null @@ -1,25 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function quat = axis_angle_to_quat(axis, angle) -% TODO: Need to add repmats, etc to do this with bulk datasets -% angle in radians -% axis is unit vector of rotation - -axis = axis ./ rssrow(axis); - -quat = [axis.*sin(angle/2) cos(angle/2)]; \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints.m b/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints.m deleted file mode 100644 index deaf49c7ae..0000000000 --- a/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints.m +++ /dev/null @@ -1,41 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [setpoint, cmd_times] = calc_trapazoidal_waypoints(init_pos, cmd_accel, cmd_vel, init_t) -%% [pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -% Currently only outputs dummy attitude -init_quat = [0 0 0 1]; -init_omega = [0 0 0]; -init_alpha = [0 0 0]; - - -% Currently velocity must be vector and accel has to be scalar -init_vel = [0 0 0]; -delta_vel_mag = norm(cmd_vel - init_vel, 'fro'); -boost_end_t = delta_vel_mag / cmd_accel; -vel_unit_vec = (cmd_vel - init_vel) ./ delta_vel_mag; -accel_vec = vel_unit_vec*cmd_accel; -boost_end_pos = init_pos + init_vel*boost_end_t + .5*accel_vec*boost_end_t.^2; - -setpoint(1,:) = [init_pos, init_vel, accel_vec, init_quat, init_omega, init_alpha]; -setpoint(2,:) = [boost_end_pos, cmd_vel, [0 0 0], init_quat, init_omega, init_alpha]; - -% time are seconds and nanoseconds -abs_boost_end_t = boost_end_t+init_t; - -cmd_times = [init_t 0; floor(abs_boost_end_t) mod(abs_boost_end_t,1)*1E9]; - diff --git a/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints_with_att.m b/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints_with_att.m deleted file mode 100644 index 453571b74b..0000000000 --- a/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints_with_att.m +++ /dev/null @@ -1,56 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [setpoint, cmd_times] = calc_trapazoidal_waypoints_with_att(init_pos, cmd_accel, cmd_vel, init_quat, cmd_alpha, cmd_omega, init_t) -%% [pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -% Attitude propagation uses small angle approximation - -% - -% Currently velocity must be vector and accel has to be scalar -init_vel = [0 0 0]; -delta_vel_mag = norm(cmd_vel - init_vel, 'fro'); -if delta_vel_mag == 0 - boost_end_t = 0; - accel_vec = [0 0 0]; - boost_end_pos = init_pos; -else - boost_end_t = delta_vel_mag / cmd_accel; - vel_unit_vec = (cmd_vel - init_vel) ./ delta_vel_mag; - accel_vec = vel_unit_vec*cmd_accel; - boost_end_pos = init_pos + init_vel*boost_end_t + .5*accel_vec*boost_end_t.^2; -end - -init_omega = [0 0 0]; -delta_omega_mag = norm(cmd_omega - init_omega, 'fro'); -att_boost_end_t = delta_omega_mag / cmd_alpha; -omega_unit_vec = (cmd_omega - init_omega) ./ delta_omega_mag; -alpha_vec = omega_unit_vec*cmd_alpha; -delta_angle = init_omega*att_boost_end_t + 0.5*alpha_vec*att_boost_end_t.^2; - -% This kind of a hack, works for single axis and small angles. Need to do integration of q_dot to do it right -delta_quat = axis_angle_to_quat(delta_angle./rssrow(delta_angle), rssrow(delta_angle)); -boost_end_quat = quatmult(init_quat, delta_quat); - -setpoint(1,:) = [init_pos, init_vel, accel_vec, init_quat, init_omega, alpha_vec]; -setpoint(2,:) = [boost_end_pos, cmd_vel, [0 0 0], boost_end_quat, cmd_omega, [0 0 0]]; - -% time are seconds and nanoseconds -abs_boost_end_t = boost_end_t+init_t; - -cmd_times = [init_t 0; floor(abs_boost_end_t) mod(abs_boost_end_t,1)*1E9]; - diff --git a/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints_with_att_end_point.m b/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints_with_att_end_point.m deleted file mode 100644 index 8e47da1d78..0000000000 --- a/gnc/matlab/utility_scripts/calc_trapazoidal_waypoints_with_att_end_point.m +++ /dev/null @@ -1,179 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [setpoint, cmd_times] = calc_trapazoidal_waypoints_with_att_end_point(init_pos, end_pos, cmd_accel, cmd_vel, init_quat,end_quat, cmd_alpha, cmd_omega, init_t) -%% [pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, quat_x, quat_y, quat_z, quat_w, angv_x, angv_y, angv_z, anga_x, anga_y, anga_z] -% Attitude can only be used if there is a zero position change command -% Attitude propagation uses small angle approximation -% Currently only uses times from position - -% Currently velocity must be vector and accel has to be scalar -init_vel = 0; % Only works for initial velocity = 0, right now. -no_cruise_flag = 0; -pos_stationary_flag = 0; -delta_vel_mag = norm(cmd_vel - init_vel, 'fro'); -if delta_vel_mag == 0 || all(init_pos == end_pos) - pos_stationary_flag = 1; - boost_t = 0; - accel_vec = [0 0 0]; - boost_end_pos = init_pos; -else - boost_t = delta_vel_mag / cmd_accel; - pos_delta = end_pos - init_pos; - dir_unit_vec = pos_delta./ norm(pos_delta); - accel_vec = dir_unit_vec*cmd_accel; - boost_pos_delta = init_vel*boost_t + .5*accel_vec*boost_t.^2; - boost_end_pos = init_pos + boost_pos_delta; - - deboost_start_pos = end_pos - boost_pos_delta; - - cruise_vec = deboost_start_pos - boost_end_pos; - cruise_time = norm(cruise_vec) / cmd_vel; - - - deboost_t = boost_t; - - - % Case where you never reach full speed - if any(((deboost_start_pos - boost_end_pos).*sign(dir_unit_vec)) < 0) % Test if you will ever reach full speed - no_cruise_flag = 1; - % If not there will never be a cruise phase - boost_pos_delta = pos_delta / 2; - boost_end_pos = init_pos + boost_pos_delta; - deboost_start_pos = end_pos - boost_pos_delta; - t_roots = roots([.5*cmd_accel init_vel -norm(boost_pos_delta)]); - boost_t = t_roots(find(t_roots > 0,1, 'first')); - deboost_t = boost_t; - cruise_time = 0; - - cmd_vel = boost_t*cmd_accel; % Replace command velocity, with achieved velocity - end - - setpoint(1,:) = [init_pos, [0 0 0], accel_vec, init_quat, [0 0 0], [0 0 0]]; % Boost - setpoint(2,:) = [boost_end_pos, cmd_vel*dir_unit_vec, [0 0 0], init_quat, [0 0 0], [0 0 0]]; % Cruise - setpoint(3,:) = [deboost_start_pos, cmd_vel*dir_unit_vec, -accel_vec, init_quat, [0 0 0], [0 0 0]]; % Deboost - setpoint(4,:) = [end_pos, [0 0 0], [0 0 0], end_quat, [0 0 0], [0 0 0]]; % Hold - - % time are seconds and nanoseconds - abs_boost_end_t = boost_t+init_t; - abs_cruise_end_t = abs_boost_end_t + cruise_time; - abs_deboost_end_t = abs_cruise_end_t + deboost_t; - - cmd_times = [init_t 0; - floor(abs_boost_end_t), mod(abs_boost_end_t,1)*1E9;... - floor(abs_cruise_end_t), mod(abs_cruise_end_t,1)*1E9;... - floor(abs_deboost_end_t), mod(abs_deboost_end_t,1)*1E9;... - ]; - - - if no_cruise_flag - setpoint(2,:) = []; % Delete cruise setpoint - cmd_times(2,:) = []; - - end - - - -end - -%% Attitude Portion -% init_quat = [.7071 .7071 0 0] % 90 deg yaw, followed by 180 deg roll -% end_quat = [0 0 0 1] % Null quat, axes aligned with nav frame -if pos_stationary_flag - % Currently velocity must be vector and accel has to be scalar - init_omega = 0; % Only works for initial velocity = 0, right now. - no_cruise_flag = 0; - delta_omega_mag = norm(cmd_omega - init_omega, 'fro'); - if delta_omega_mag == 0 || all(init_quat == end_quat) - boost_t = 0; - alpha_vec = [0 0 0]; - boost_end_quat = init_quat; - delta_vec = [1 0 0]; - deboost_start_quat = init_quat; - else - boost_t = delta_omega_mag / cmd_alpha; - q_delta = quatmult( quat_inv( init_quat ), end_quat ); - - if ~(q_delta(4) == 0) - q_delta = sign(q_delta(4))*q_delta; % Ensure the shortest route is taken. - end - delta_angle = 2*acos(q_delta(4)); - delta_vec = q_delta(1:3) / sqrt(1 - q_delta(4)*q_delta(4)); - - alpha_vec = delta_vec*cmd_alpha; - boost_angle_delta = init_omega*boost_t + .5*cmd_alpha*boost_t.^2; - body_accel_rate = cmd_alpha*delta_vec; - boost_end_quat = quatmult( init_quat, axis_angle_to_quat(delta_vec./norm(delta_vec), boost_angle_delta) ); % Could have done q_init * delta_quat^-1 - - deboost_start_quat = quatmult( end_quat, axis_angle_to_quat(delta_vec./norm(delta_vec), -boost_angle_delta) ); - deboost_t = boost_t; - - q_delta_cruise = quatmult( quat_inv( boost_end_quat ), deboost_start_quat ); - delta_cruise_angle = 2*acos(q_delta_cruise(4)); - delta_cruise_vec = q_delta_cruise(1:3) / sqrt(1 - q_delta_cruise(4)*q_delta_cruise(4)); - cruise_body_rates = cmd_omega * delta_cruise_vec; - cruise_time = delta_cruise_angle / cmd_omega; - - - % Case where you never reach full speed - % Look to make sure the resultant axis from the end of boost to the - % start of de-boost is aligned with the initial axis of rotation and - % that the angles are of the same sign. This is a good check because I - % calculate deboost_start_q as (end quat - angle change while - % boosting), and we check to make sure that this has not overlapped - % with boost_end_q (i.e. we would need to start slowing down before we - % ever reached full speed). - if ~(norm(delta_vec - delta_cruise_vec) < 1E-4 && sign(delta_cruise_angle) == sign(delta_angle)) - no_cruise_flag = 1; - % If not there will never be a cruise phase - boost_angle_delta = delta_angle / 2; % Assume we accelerate for half the time, then decel for the other half - boost_end_quat = quatmult( init_quat, axis_angle_to_quat(delta_vec./norm(delta_vec), boost_angle_delta) ); % Could have done q_init * delta_quat^-1 - deboost_start_quat = quatmult( end_quat, axis_angle_to_quat(delta_vec./norm(delta_vec), -boost_angle_delta) ); - - t_roots = roots([.5*cmd_alpha init_omega -norm(boost_angle_delta)]); - boost_t = t_roots(find(t_roots > 0,1, 'first')); - deboost_t = boost_t; - cruise_time = 0; - - cmd_omega = boost_t*cmd_alpha; % Replace command velocity, with achieved velocity - end - - end - - setpoint(1,:) = [init_pos, [0 0 0], [0 0 0], init_quat, [0 0 0], alpha_vec]; % Boost - setpoint(2,:) = [init_pos, [0 0 0], [0 0 0], boost_end_quat, delta_vec*cmd_omega, [0 0 0]]; % Cruise - setpoint(3,:) = [init_pos, [0 0 0], [0 0 0], deboost_start_quat, delta_vec*cmd_omega, -alpha_vec]; % Deboost - setpoint(4,:) = [end_pos, [0 0 0], [0 0 0], end_quat, [0 0 0], [0 0 0]]; % Hold - - % time are seconds and nanoseconds - abs_boost_end_t = boost_t+init_t; - abs_cruise_end_t = abs_boost_end_t + cruise_time; - abs_deboost_end_t = abs_cruise_end_t + deboost_t; - - cmd_times = [init_t 0; - floor(abs_boost_end_t), mod(abs_boost_end_t,1)*1E9;... - floor(abs_cruise_end_t), mod(abs_cruise_end_t,1)*1E9;... - floor(abs_deboost_end_t), mod(abs_deboost_end_t,1)*1E9;... - ]; - - - if no_cruise_flag - setpoint(2,:) = []; % Delete cruise setpoint - cmd_times(2,:) = []; - end - -end diff --git a/gnc/matlab/utility_scripts/calibrate_camera_extrinsics.m b/gnc/matlab/utility_scripts/calibrate_camera_extrinsics.m deleted file mode 100644 index beeb2a3bed..0000000000 --- a/gnc/matlab/utility_scripts/calibrate_camera_extrinsics.m +++ /dev/null @@ -1,82 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% How to calibrate: -% Run this on a bag full of data. -function imu_offset = calibrate_camera_extrinsics() - evalin('base', 'tunable_init'); - - directions = [0 0 0 0 1 0; ... - 0 0 0 0 0 1; ... - 0 0 0 1 0 0; ... - 1 0 0 0 0 0; ... - 0 1 0 0 0 0; ... - 0 0 1 0 0 0;]; - multipliers = [1 1 1 0.02 0.02 0.02]'; - windows = [4 2 1 0.5]'; - precisions = [2 1 0.5 0.2]'; - best = [0 0 0 0 0 0]; - for d=1:size(windows, 1) - for i=1:size(directions, 1) - r = multipliers(i) * windows(d) * directions(i, :); - best = minima_search(@test_transform, best - r, best + r); - end - end - t = best(4:6) * pi / 180; - q = eulers_to_quat(t(1), t(2), t(3)); - original_rot = evalin('base', 'tun_abp_q_body2cam'); - q = quatmult(q, original_rot); - fprintf('Best pose: [%10.8g, %10.8g, %10.8g] translation, [%10.8g, %10.8g, %10.8g, %10.8g] quaternion rotation.\n', ... - best(1), best(2), best(3), q(1), q(2), q(3), q(4)); -end - -function best = minima_search(error_function, first, last) - fprintf('Beginning optimization.\n'); - besterr = inf; - best = 0; - for val=0:6 - cur = first + (last - first) * (val / 6); - err = error_function(cur); - fprintf(['Cur: ', mat2str(cur), ' Error: %g\n'], err); - if err < besterr - besterr = err; - best = cur; - end - end - fprintf(['Best: ', mat2str(best), ' Error: %g\n'], besterr); -end - -function err = test_transform(params) - imu_offset = params(1:3); - params(4:end) = params(4:end) .* pi ./ 180; - imu_rotation = eulers_to_quat(params(4), params(5), params(6)); - - assignin('base', 'show_graphs', false); - assignin('base', 'show_results', false); - assignin('base', 'disable_config_overwrite', true); - original_imu = evalin('base', 'tun_abp_p_cam_body_body'); - original_quat = evalin('base', 'tun_abp_q_body2cam'); - assignin('base', 'tun_abp_p_cam_body_body', original_imu + imu_offset); - assignin('base', 'tun_abp_q_body2cam', quatmult(imu_rotation, original_quat)); - evalin('base', 'ase_hw_data_test'); - assignin('base', 'tun_abp_p_cam_body_body', original_imu); - assignin('base', 'tun_abp_q_body2cam', original_quat); - evalin('base', 'clear show_graphs show_results disable_config_overwrite disable_ml disable_of;'); - - err = evalin('base', 'angular_rmse / 100 + pos_rmse'); -end - diff --git a/gnc/matlab/utility_scripts/calibrate_imu.m b/gnc/matlab/utility_scripts/calibrate_imu.m deleted file mode 100644 index 0de9451eb8..0000000000 --- a/gnc/matlab/utility_scripts/calibrate_imu.m +++ /dev/null @@ -1,112 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% How to calibrate: -% 1. Get imu and camera pose estimates from cad files. -% 2. Record bag of spinning in place with no external forces. Set zero_ind -% to indices of a single spin with sinusoidal acceleration. -% Make sure that the imu omega is not saturated. -% 3. Run calibrate_imu to get IMU pose. -function imu_offset = calibrate_imu(zero_ind) - evalin('base', 'tunable_init;'); - - directions = [0 0 0 0 1 0; ... - 0 0 0 0 0 1; ... - 0 0 0 1 0 0; ... - 1 0 0 0 0 0; ... - 0 1 0 0 0 0; ... - 0 0 1 0 0 0;]; - multipliers = [1 1 1 0.02 0.02 0.02]'; - windows = [10 2 0.2 0.02]'; - precisions = [1 0.1 0.01 0.001]'; - best = [0 0 0 0 0 0]; - for d=1:size(windows, 1) - for i=1:size(directions, 1) - r = multipliers(i) * windows(d) * directions(i, :); - best = minima_search(@test_transform, zero_ind, best - r, best + r, multipliers(i) * precisions(d)); - end - end - t = best(4:6) * pi / 180; - q = eulers_to_quat(t(1), t(2), t(3)); - original_rot = evalin('base', 'tun_abp_quat_body2imu'); - q = quatmult(q, original_rot); - fprintf('Best pose: [%10.8g, %10.8g, %10.8g] translation, [%10.8g, %10.8g, %10.8g, %10.8g] quaternion rotation.\n', ... - best(1), best(2), best(3), q(1), q(2), q(3), q(4)); -end - -function best = minima_search(error_function, zero_ind, first, last, precision) - fprintf('Beginning optimization.\n'); - %starterr = error_function(zero_ind, first); - %enderr = error_function(zero_ind, last); - %fprintf(['Left: ', mat2str(first), ' Error: %g\n'], starterr); - %fprintf(['Right: ', mat2str(last), ' Error: %g\n'], enderr); - total = sum(last-first); - while total >= precision - mid = 0.5 * (first + last); - dx = (last-first) ./ sum(last-first); - errs = [0 0 0]; - offsets = [-precision * 0.001 0 precision * 0.001]; - for i=1:3 - errs(i) = error_function(zero_ind, mid + offsets(i) * dx); - end - lefterr = errs(1); - miderr = errs(2); - righterr = errs(3); - fprintf(['Pose: [%8.6g %8.6g %8.6g %8.6g %8.6g %8.6g] Step: %6g, Errors: %6g, %6g, %6g.\n'], ... - mid(1), mid(2), mid(3), mid(4), mid(5), mid(6), precision * 0.001, lefterr, miderr, righterr); - if miderr < righterr && miderr < lefterr - fprintf('Found local minima.\n'); - break; - end - if righterr < lefterr - first = mid; - else - last = mid; - end - total = sum(last-first); - end - best = mid; - fprintf(['Best: ', mat2str(best), ' Error: %g\n'], miderr); -end - -function err = test_transform(zero_ind, params) - imu_offset = params(1:3); - params(4:end) = params(4:end) .* pi ./ 180; - imu_rotation = eulers_to_quat(params(4), params(5), params(6)); - - assignin('base', 'show_graphs', false); - assignin('base', 'show_results', false); - assignin('base', 'disable_config_overwrite', true); - assignin('base', 'disable_ml', true); - assignin('base', 'disable_of', true); - original_imu = evalin('base', 'tun_abp_p_imu_body_body'); - original_quat = evalin('base', 'tun_abp_quat_body2imu'); - assignin('base', 'tun_abp_p_imu_body_body', original_imu + imu_offset); - assignin('base', 'tun_abp_quat_body2imu', quatmult(imu_rotation, original_quat)); - evalin('base', 'ase_hw_data_test'); - assignin('base', 'tun_abp_p_imu_body_body', original_imu); - assignin('base', 'tun_abp_quat_body2imu', original_quat); - evalin('base', 'clear show_graphs, show_results, disable_config_overwrite, disable_ml, disable_of;'); - - omega = evalin('base', 'out_kfl_msg.omega_B_ISS_B.Data'); - errors = var(omega(:, 1:2)); - err = sqrt(sum(errors)); - accel = evalin('base', 'out_kfl_msg.A_B_ISS_ISS'); - accel = accel.Data(zero_ind, :); - err = err + sqrt(sum(var(accel))); -end - diff --git a/gnc/matlab/utility_scripts/close_simulink_libs.m b/gnc/matlab/utility_scripts/close_simulink_libs.m deleted file mode 100644 index eff7f7088b..0000000000 --- a/gnc/matlab/utility_scripts/close_simulink_libs.m +++ /dev/null @@ -1,23 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Clears all the currently loaded simulink libraries out of memory, useful -% if a library changes outside of matlab (for example by a GIT update). - -%% -open_bd = find_system('type', 'block_diagram'); -close_system(open_bd); \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/eulers_to_quat.m b/gnc/matlab/utility_scripts/eulers_to_quat.m deleted file mode 100644 index b39167d594..0000000000 --- a/gnc/matlab/utility_scripts/eulers_to_quat.m +++ /dev/null @@ -1,35 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function quat = eulers_to_quat(phi, theta, psi) -% Convert Euler angles to quaternions. Assumes 3-2-1 rotation sequence for -% euler angles. Corrected typo where first and 3rd rows were flipped. -% From: Fundamentals of Spacecraft Attitude Determination and Control, F. -% Landis Markley and John L. Crassisdis. Table: B.5 -quat = zeros(size(psi,1), 4, 'like', psi); - -c_psi = cos(psi/2); s_psi = sin(psi/2); -c_theta = cos(theta/2); s_theta = sin(theta/2); -c_phi = cos(phi/2); s_phi = sin(phi/2); - -quat(:,1) = s_phi.*c_theta.*c_psi - c_phi.*s_theta.*s_psi; -quat(:,3) = c_phi.*c_theta.*s_psi - s_phi.*s_theta.*c_psi; -quat(:,2) = c_phi.*s_theta.*c_psi + s_phi.*c_theta.*s_psi; -quat(:,4) = c_phi.*c_theta.*c_psi + s_phi.*s_theta.*s_psi; - - - diff --git a/gnc/matlab/utility_scripts/first_ord_butter.m b/gnc/matlab/utility_scripts/first_ord_butter.m deleted file mode 100644 index 9fb378cdcb..0000000000 --- a/gnc/matlab/utility_scripts/first_ord_butter.m +++ /dev/null @@ -1,30 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [num,den] = first_ord_butter(omega_c,Ts) -%Calculates the filter coefficients for a first order butter worth low pass -%filter. -% Calculates the filter coefficients for a first order Butterworth low pass -%filter using the bilinear transformation of the equation: -%omega_c/[s+omega_c] - -filter_const = omega_c*Ts/2; -num = single( [1 1]*filter_const/(1+filter_const) ); -den = single( [1 , (filter_const-1)/(filter_const+1)] ); - -end - diff --git a/gnc/matlab/utility_scripts/genCSVfromTM.m b/gnc/matlab/utility_scripts/genCSVfromTM.m deleted file mode 100644 index f77457d6ad..0000000000 --- a/gnc/matlab/utility_scripts/genCSVfromTM.m +++ /dev/null @@ -1,67 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% % Generate .csv telemData structure -% fileName = 'kfl_output.csv'; -% outTM = out_kfl_msg; -function genCSVfromTM(fileName, outTM) -fileName = [fileName '.csv']; -structFields = fieldnames(outTM); -headerStr = []; -outputMat = []; -for ii = 1:length(structFields) - if ismatrix(outTM.(structFields{ii}).data) % We don't accept stuff with more than 2 dimensions right now - if isa(outTM.(structFields{ii}).data, 'quaternion') - numCol = 4; - else - numCol = size(outTM.(structFields{ii}).data, 2); - end - for kk = 1:numCol - headerStr = [headerStr structFields{ii}, '[' , num2str(kk), '],' ]; - end - outputMat = [outputMat double(outTM.(structFields{ii}).data)]; - else - - sizeTM = size(outTM.(structFields{ii}).data); - if numel(sizeTM) == 3 - for kk = 1:size(outTM.(structFields{ii}).data, 1) - for jj = 1:size(outTM.(structFields{ii}).data, 2) - headerStr = [headerStr structFields{ii}, '[' , num2str(kk), '-', num2str(jj), '],' ]; - end - end - outputMat = [outputMat double(reshape(permute(outTM.(structFields{ii}).data, [3 2 1]),sizeTM(3), sizeTM(1)*sizeTM(2)))]; - else - for kk = 1:size(outTM.(structFields{ii}).data, 1) - for jj = 1:size(outTM.(structFields{ii}).data, 2) - for ll = 1:size(outTM.(structFields{ii}).data, 3) - headerStr = [headerStr structFields{ii}, '[' , num2str(kk), '-', num2str(jj), '-', num2str(ll) '],' ]; - end - end - end - outputMat = [outputMat double(reshape(permute(outTM.(structFields{ii}).data, [4 3 2 1]),sizeTM(4), sizeTM(1)*sizeTM(2)*sizeTM(3)))]; - end - - end -end - -fh = fopen(fileName, 'w'); -fprintf(fh, [headerStr '\n']); -fclose(fh); - -dlmwrite(fileName, outputMat, '-append', 'precision', 16) - -%function [outputMat, headerStr] = genStr(outTM) \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/genCSVfromTM_structs.m b/gnc/matlab/utility_scripts/genCSVfromTM_structs.m deleted file mode 100644 index c2df392919..0000000000 --- a/gnc/matlab/utility_scripts/genCSVfromTM_structs.m +++ /dev/null @@ -1,88 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% % Generate .csv telemData structure -% fileName = 'kfl_output.csv'; -% outTM = out_kfl_msg; -function genCSVfromTM_structs(fileName, outTM) -fileName = [fileName '.csv']; - -headerStr = []; -outputMat = []; -structFields = fieldnames(outTM); -for ii = 1:length(structFields) - if isstruct(outTM.(structFields{ii})) - parentStr = [(structFields{ii}) '.']; - [temp_outputMat, temp_headerStr] = genStr(parentStr, outTM.(structFields{ii})); - headerStr = [headerStr temp_headerStr]; - outputMat = [outputMat temp_outputMat]; - else - parentStr = []; - temp_struct.(structFields{ii}) = outTM.(structFields{ii}); - [temp_outputMat, temp_headerStr] = genStr(parentStr, temp_struct); - headerStr = [headerStr temp_headerStr]; - outputMat = [outputMat temp_outputMat]; - end -end -fh = fopen(fileName, 'w'); -fprintf(fh, [headerStr '\n']); -fclose(fh); - -dlmwrite(fileName, outputMat, '-append', 'precision', 16) - -end - - -function [outputMat, headerStr] = genStr(parentStr, outTM) -headerStr = []; -outputMat = []; -structFields = fieldnames(outTM); -for ii = 1:length(structFields) - if ismatrix(outTM.(structFields{ii}).data) % We don't accept stuff with more than 2 dimensions right now - if isa(outTM.(structFields{ii}).data, 'quaternion') - numCol = 4; - else - numCol = size(outTM.(structFields{ii}).data, 2); - end - for kk = 1:numCol - headerStr = [headerStr parentStr structFields{ii}, '[' , num2str(kk), '],' ]; - end - outputMat = [outputMat double(outTM.(structFields{ii}).data)]; - else - - sizeTM = size(outTM.(structFields{ii}).data); - if numel(sizeTM) == 3 - for kk = 1:size(outTM.(structFields{ii}).data, 1) - for jj = 1:size(outTM.(structFields{ii}).data, 2) - headerStr = [headerStr parentStr structFields{ii}, '[' , num2str(kk), '-', num2str(jj), '],' ]; - end - end - outputMat = [outputMat double(reshape(permute(outTM.(structFields{ii}).data, [3 2 1]),sizeTM(3), sizeTM(1)*sizeTM(2)))]; - else - for kk = 1:size(outTM.(structFields{ii}).data, 1) - for jj = 1:size(outTM.(structFields{ii}).data, 2) - for ll = 1:size(outTM.(structFields{ii}).data, 3) - headerStr = [headerStr parentStr structFields{ii}, '[' , num2str(kk), '-', num2str(jj), '-', num2str(ll) '],' ]; - end - end - end - outputMat = [outputMat double(reshape(permute(outTM.(structFields{ii}).data, [4 3 2 1]),sizeTM(4), sizeTM(1)*sizeTM(2)*sizeTM(3)))]; - end - - end -end -end diff --git a/gnc/matlab/utility_scripts/gen_first_order_lp.m b/gnc/matlab/utility_scripts/gen_first_order_lp.m deleted file mode 100644 index 97d751b064..0000000000 --- a/gnc/matlab/utility_scripts/gen_first_order_lp.m +++ /dev/null @@ -1,33 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [num, den] = gen_first_order_lp(wc, ts) -% Generate a first order low pass filter. Note the controls toolbox is -% needed for this function. We could derive the math for broader audience. -% Inputs: Cutoff frequency -% Sample rate -% Outputs: Filter Numerator -% Filter Denominator - -%% Call the controls tooldbox functions -lp_tf = tf(1, [1/wc 1]) -dt_tf = c2d(lp_tf, ts) -bode(dt_tf) - -%% Pack the outputs -num = dt_tf.num; -den = dt_tf.den; \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/gen_harness_from_bus.m b/gnc/matlab/utility_scripts/gen_harness_from_bus.m deleted file mode 100644 index e3ca730ae0..0000000000 --- a/gnc/matlab/utility_scripts/gen_harness_from_bus.m +++ /dev/null @@ -1,74 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Create a simulink bus creator with zeroes hooked up to it in the base workspace -% from a simulink bus definition. -% Input: 'bus_name' -- Name of a bus object in the workspace -function gen_harness_from_bus(busName) -%busName = 'cmd_msg'; - -busIn = evalin('base', busName); -nInputs = length(busIn.Elements); - -modelName = [busName '_bus_creator']; - -sys_h = new_system(modelName); - -%'Position', When I add blocks to the canvas, I specify the position to provide proper layout. The position parameter provides the top left (x,y) and lower right (x+w,y+h) -% corners of the block. The x and y values are relative to the origin (0,0) in the upper left corner of the canvas; x increases to the right, and y increases down. -const.height = 20; -const.width = 150; -const.x = 0; -% const.y = 110; - -constantCushion = 25; % Space between constants -constantSpacing = const.height + constantCushion; % Top to top spacing -topBottomCushion = 10; % # of pixels above and below first and last constant - -% X-Y coords are top left -creator.x = 320; -creator.y = 55; -creator.width = 5; -creator.height = topBottomCushion*2 + nInputs*const.height + (nInputs-1)*constantCushion; - -busCreator_h = add_block('simulink/Signal Routing/Bus Creator', [modelName '/bus_creator']); -set_param(busCreator_h, 'Inputs', num2str(nInputs)); % Set the number of inputs -set_param(busCreator_h, 'Position', [creator.x creator.y creator.x+creator.width creator.y+creator.height]) % Se the position and dimensions -set_param(busCreator_h, 'OutDataTypeStr', ['Bus: ' busName]); -set_param(busCreator_h, 'NonVirtualBus', 'on'); - - -for ii = 1:nInputs - constY = creator.y+topBottomCushion+(ii-1)*constantSpacing; % Y location of the top left corner of the constant block - constant_h = add_block('simulink/Sources/Constant', [modelName '/constant' num2str(ii)], 'Position', [const.x constY const.x+const.width constY+const.height]); % Create constant - - if length(busIn.Elements(ii).Dimensions) == 1 - set(constant_h, 'Value', [ '[' num2str(zeros(1,busIn.Elements(ii).Dimensions)) ']']); % Initialize the block to zeroes - else - set(constant_h, 'Value', ['zeros([' num2str(busIn.Elements(ii).Dimensions) '])' ]); % Initialize the block to zeroes - end - set(constant_h, 'OutDataTypeStr', busIn.Elements(ii).DataType) - set(constant_h, 'ShowName', 'off') % disable the name from showing - - - line_h = add_line(sys_h, ['constant' num2str(ii) '/1'],['bus_creator/' num2str(ii)] ); % connect the constant to the bus creator - set(line_h, 'Name', busIn.Elements(ii).Name) -end - -open_system(sys_h) - - diff --git a/gnc/matlab/utility_scripts/generate_All_CSV.m b/gnc/matlab/utility_scripts/generate_All_CSV.m deleted file mode 100644 index a4f2b6f6b1..0000000000 --- a/gnc/matlab/utility_scripts/generate_All_CSV.m +++ /dev/null @@ -1,32 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Generate .csv for testing with fsw - -genCSVfromTM('out_act_msg_vpp', out_act_msg_vpp) -genCSVfromTM('out_act_msg', out_act_msg) -genCSVfromTM('out_kfl_msg', out_kfl_msg) -genCSVfromTM('out_cmd_msg', out_cmd_msg) -genCSVfromTM('out_ex_time_msg', out_ex_time_msg) -genCSVfromTM('out_cvs_reg_pulse', out_cvs_reg_pulse) -genCSVfromTM('out_cvs_landmark_msg', out_cvs_landmark_msg) -genCSVfromTM('out_cvs_optflow_msg', out_cvs_optflow_msg) -genCSVfromTM_structs('out_cmc_msg', out_cmc_msg) -genCSVfromTM('out_imu_msg', out_imu_msg) -genCSVfromTM('out_env_msg', out_env_msg) -genCSVfromTM('out_vpp_msg', out_vpp_msg) -genCSVfromTM('out_bpm_msg', out_bpm_msg) diff --git a/gnc/matlab/utility_scripts/generate_ctl_param_reads.m b/gnc/matlab/utility_scripts/generate_ctl_param_reads.m deleted file mode 100644 index be2aa50a64..0000000000 --- a/gnc/matlab/utility_scripts/generate_ctl_param_reads.m +++ /dev/null @@ -1,33 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%% generate_cpp_param_reads.m -% Script that generates the functions for reading tunable parameters - -%% Inputs -source_header = 'ctl_controller0.h'; - -% Boolean flags, will also find any variables tun_*_f + the list below -boolean_exceptions = {'tun_ctl_bypass_cmd_shaper', 'tun_cvs_noise_on', ... - 'tun_env_drag_disturb_on', 'tun_epson_report_truth', ... - 'tun_debug_ctl_use_truth', 'tun_ase_gravity_removal'}; -output_fname = 'ctl_tunable_funcs'; -function_name = 'ctl_ReadParams'; -input_object_type = 'RT_MODEL_ctl_controller0_T*'; -input_object_name = 'ctl'; - -write_tunable_param_functions(source_header, boolean_exceptions, output_fname, function_name, input_object_type, input_object_name); \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/generate_est_param_reads.m b/gnc/matlab/utility_scripts/generate_est_param_reads.m deleted file mode 100644 index 2a614673c4..0000000000 --- a/gnc/matlab/utility_scripts/generate_est_param_reads.m +++ /dev/null @@ -1,33 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%% generate_cpp_param_reads.m -% Script that generates the functions for reading tunable parameters - -%% Inputs -source_header = 'est_estimator.h'; - -% Boolean flags, will also find any variables tun_*_f + the list below -boolean_exceptions = {'tun_ctl_bypass_cmd_shaper', 'tun_cvs_noise_on', ... - 'tun_env_drag_disturb_on', 'tun_epson_report_truth', ... - 'tun_debug_ctl_use_truth', 'tun_ase_enable_of', 'tun_ase_gravity_removal'}; -output_fname = 'est_tunable_funcs'; -function_name = 'est_ReadParams'; -input_object_type = 'RT_MODEL_est_estimator_T*'; -input_object_name = 'est'; - -write_tunable_param_functions(source_header, boolean_exceptions, output_fname, function_name, input_object_type, input_object_name); \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/generate_fam_param_reads.m b/gnc/matlab/utility_scripts/generate_fam_param_reads.m deleted file mode 100644 index bca7ac4846..0000000000 --- a/gnc/matlab/utility_scripts/generate_fam_param_reads.m +++ /dev/null @@ -1,34 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%% generate_cpp_param_reads.m -% Script that generates the functions for reading tunable parameters - -%% Inputs -source_header = 'fam_force_allocation_module.h'; - -% Boolean flags, will also find any variables tun_*_f + the list below -boolean_exceptions = {'tun_ctl_bypass_cmd_shaper', 'tun_cvs_noise_on', ... - 'tun_env_drag_disturb_on', 'tun_epson_report_truth', ... - 'tun_debug_ctl_use_truth', 'tun_ase_gravity_removal'}; -output_fname = 'fam_tunable_funcs'; -function_name = 'fam_ReadParams'; -input_object_type = 'RT_MODEL_fam_force_allocation_T*'; -input_object_name = 'fam'; - -write_tunable_param_functions(source_header, boolean_exceptions, output_fname, function_name, input_object_type, input_object_name); - diff --git a/gnc/matlab/utility_scripts/generate_sim_param_reads.m b/gnc/matlab/utility_scripts/generate_sim_param_reads.m deleted file mode 100644 index c4bb0f141b..0000000000 --- a/gnc/matlab/utility_scripts/generate_sim_param_reads.m +++ /dev/null @@ -1,33 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -%% generate_cpp_param_reads.m -% Script that generates the functions for reading tunable parameters - -%% Inputs -source_header = 'sim_model_lib0.h'; - -% Boolean flags, will also find any variables tun_*_f + the list below -boolean_exceptions = {'tun_ctl_bypass_cmd_shaper', 'tun_cvs_noise_on', ... - 'tun_env_drag_disturb_on', 'tun_epson_report_truth', ... - 'tun_debug_ctl_use_truth', 'tun_bpm_enable', 'tun_ase_gravity_removal'}; -output_fname = 'sim_tunable_funcs'; -function_name = 'sim_ReadParams'; -input_object_type = 'RT_MODEL_sim_model_lib0_T*'; -input_object_name = 'sim'; - -write_tunable_param_functions(source_header, boolean_exceptions, output_fname, function_name, input_object_type, input_object_name); \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/hold_on.m b/gnc/matlab/utility_scripts/hold_on.m deleted file mode 100644 index 06363df06b..0000000000 --- a/gnc/matlab/utility_scripts/hold_on.m +++ /dev/null @@ -1,21 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% hold_on mimics the behavior of the 'hold on' command prior to MATLAB -% 2014b, where the same colors are used for subsequent plots -hold on; -set(gca, 'ColorOrderIndex', 1); \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/quat2axisAngle.m b/gnc/matlab/utility_scripts/quat2axisAngle.m deleted file mode 100644 index 5489d89539..0000000000 --- a/gnc/matlab/utility_scripts/quat2axisAngle.m +++ /dev/null @@ -1,35 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function [axis, angle] = quat2axisAngle(q) -% Derived from Eq. 2.124 in "Fundamentals of Spacecraft Attitude and -% Determination and Control", Markley and Crassidis -angle = 2*acos(q(:,4)); -sin_angle = sin(angle); -axis = q(:,1:3)./sin_angle; - -% Find any place the angle is close to 180, and approximate using the -% quaternion vector postion -axis(angle > 179*pi/180, :) = q(angle > 179*pi/180,1:3); - -% Find any place you have a null quaternion and set the axis to x -min_angle_indx = find(angle < 1E-12); -axis(min_angle_indx, :) = repmat([1 0 0], length(min_angle_indx), 1); - -axis = axis./rssrow(axis); - - diff --git a/gnc/matlab/utility_scripts/quat_error.m b/gnc/matlab/utility_scripts/quat_error.m deleted file mode 100644 index c4e37301ad..0000000000 --- a/gnc/matlab/utility_scripts/quat_error.m +++ /dev/null @@ -1,33 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% quat_error % -% % -% Inputs: two quaternions % -% % -% Outpus: angular error, in millirads -% Source: https://math.stackexchange.com/questions/90081/quaternion-distance -function total_angle_millirad = quat_error( q1, q2 ) - -q1 = quat_norm_and_sign_check(q1); -q2 = quat_norm_and_sign_check(q2); - -total_angle_millirad = acos((2*dot(q1, q2, 2).^2) - 1)*1000; - -% Wrap to pi -% Should be bounded by the stackexchange equation -total_angle_millirad(total_angle_millirad > pi*1000) = total_angle_millirad(total_angle_millirad > pi*1000) - 2000*pi; diff --git a/gnc/matlab/utility_scripts/quat_error_deg.m b/gnc/matlab/utility_scripts/quat_error_deg.m deleted file mode 100644 index 7312b19737..0000000000 --- a/gnc/matlab/utility_scripts/quat_error_deg.m +++ /dev/null @@ -1,26 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% quat_error % -% % -% Inputs: two quaternions % -% % -% Ouptus: angular error, in deg -% Source: https://math.stackexchange.com/questions/90081/quaternion-distance -function [ total_angle_deg ] = quat_error_deg( q1, q2 ) - -total_angle_deg = acosd((2*dot(q1, q2, 2).^2) - 1); \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/quat_inv.m b/gnc/matlab/utility_scripts/quat_inv.m deleted file mode 100644 index 8c6dff8682..0000000000 --- a/gnc/matlab/utility_scripts/quat_inv.m +++ /dev/null @@ -1,21 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function inv_quat_out = quat_inv(quat) - - -inv_quat_out = [-quat(:,1:3), quat(:,4)]; \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/quat_norm_and_sign_check.m b/gnc/matlab/utility_scripts/quat_norm_and_sign_check.m deleted file mode 100644 index f5119ef3f2..0000000000 --- a/gnc/matlab/utility_scripts/quat_norm_and_sign_check.m +++ /dev/null @@ -1,27 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function qOut = quat_norm_and_sign_check(qIn) -% Normalize quaternion and enforce the scalar to be positive - -% If there are negatives we want to multiply by -1 -quatMultiplyer = repmat(sign(qIn(:, 4)), 1, 3); -quatMultiplyer(quatMultiplyer == 0) = 1; % Should multiply by 1 if the scalar is zero - -tempQuat = [qIn(:, 1:3) .* quatMultiplyer abs(qIn(:, 4))]; - -qOut = tempQuat ./ repmat(rssrow(tempQuat), 1, 4); \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/quat_propagate_step.m b/gnc/matlab/utility_scripts/quat_propagate_step.m deleted file mode 100644 index f258feb5b9..0000000000 --- a/gnc/matlab/utility_scripts/quat_propagate_step.m +++ /dev/null @@ -1,44 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function quat_out = quat_propagate_step(quat_in, omega, time_in) -%#codegen - -% omega row vector -% -% From: Indirect Kalman Filter for 3D Attitude Estimation: A tutorial for Quaternion Algebra -% Equation below is from Eq. 122, with Omega matrix and -% identity matrix multiplied together -% -% - -omega_mag = rssrow(omega); -quat_out = zeros(length(omega_mag),4); - -for ii = 1:length(omega_mag) - if omega_mag(ii) == 0 || time_in(ii) <= 0 - quat_out(ii,:) = quat_in(ii,:); - else - c = cos(.5 .* omega_mag(ii) .* time_in(ii)); - sine_mag = sin(.5 .* omega_mag(ii) .* time_in(ii)); - s = sine_mag .* omega(ii,:) ./ omega_mag(ii); - quat_mat = [c, -s(3), s(2), -s(1); s(3), c, -s(1), -s(2); -s(2), s(1), c, -s(3); s(1), s(2), s(3), c]'; % Rollup of trig equations and Omega matrix - quat_out(ii,:) = (quat_mat * quat_in(ii,:)')'; - quat_out(ii,:) = sign(quat_out(ii,4)) * quat_out(ii,:) / rssrow(quat_out(ii,:)); - - end -end \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/quat_rotation_vec.m b/gnc/matlab/utility_scripts/quat_rotation_vec.m deleted file mode 100644 index d961c31d70..0000000000 --- a/gnc/matlab/utility_scripts/quat_rotation_vec.m +++ /dev/null @@ -1,45 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - - -%% -% Q = q_a2b, rotation represents a vector rotating from frame b2a -% q_out = quat_rotation(vector, Q2) -% -% rotates a row-vector of 3-vectors by a row-vector of quaternion Q2 -% -% We accomplish this rotation by using DCMs as an intermediate step -% -function vec_out = quat_rotation_vec(vector, Q) - -if size(vector, 1) == 1 % Check to see if the vector is a single row - vector = repmat(vector, size(Q,1), 1); -end - - - -[nquat, ~] = size(Q); -vec_out = zeros(nquat, 3, 'like', vector); - -for ii = 1:nquat - vec_out(ii, :) = quaternion_to_dcm(Q(ii, :))*vector(ii,:)'; -end - - - - - diff --git a/gnc/matlab/utility_scripts/quat_to_eulers.m b/gnc/matlab/utility_scripts/quat_to_eulers.m deleted file mode 100644 index a2f9a130c1..0000000000 --- a/gnc/matlab/utility_scripts/quat_to_eulers.m +++ /dev/null @@ -1,24 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Converts a quaternion to 3-2-1 Euler angles -function eulers = quat_to_eulers(q) - -eulers(:,1) = atan2(2*(q(:,4).*q(:,1) + q(:,2).*q(:,3)), 1-2.*(q(:,1).^2 + q(:,2).^2)); -eulers(:,2) = asin(2*(q(:,4).*q(:,2) - q(:,3).*q(:,1))); -eulers(:,3) = atan2(2*(q(:,4).*q(:,3) + q(:,1).*q(:,2)), 1-2.*(q(:,2).^2 + q(:,3).^2)); - diff --git a/gnc/matlab/utility_scripts/quaternion_to_dcm.m b/gnc/matlab/utility_scripts/quaternion_to_dcm.m deleted file mode 100644 index 3cc168a2c9..0000000000 --- a/gnc/matlab/utility_scripts/quaternion_to_dcm.m +++ /dev/null @@ -1,28 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Convert quaterion to a DCM. DCM will rotate a vector V by Q_A2B from -% reference frame A to reference frame B. -% -% Indirect Kalman Filter for 3D Attitude Estimation: A Tutorial for Aquaternion Algebra. -% Nikolas Trawny and Stergios I. Roumeliotis -% Equation 78 -function dcm = quaternion_to_dcm(q_in) - -q4 = q_in(4); - -dcm = (2*q4^2 - 1)*eye(3, 'like', q_in) - 2*q4*skew(q_in) + 2*(q_in(1:3)'*q_in(1:3)); diff --git a/gnc/matlab/utility_scripts/quatmult.m b/gnc/matlab/utility_scripts/quatmult.m deleted file mode 100644 index bf7aab7f67..0000000000 --- a/gnc/matlab/utility_scripts/quatmult.m +++ /dev/null @@ -1,33 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Quaternion Multiplication: -% Uses Hamilton's convention where the rotation order is left to right, -% q1*q2 corresponds to the first rotation q1, followed by the second -% rotation q2. -% -% Fundamentals of Spacecraft Attitude Determination and Control, -% F. Landis Markley and John L. Crassidis -% Equation: 2.82b -function qOut = quatmult(p, q) - -qOut = [q(:,4) .* p(:,1:3) + p(:,4).*q(:,1:3) + cross(p(:,1:3), q(:,1:3), 2), ... - p(:,4).*q(:,4) - dot(p(:,1:3), q(:,1:3),2)]; - -qOut = qOut ./ repmat(rssrow(qOut), 1, 4); % Normalize - -end diff --git a/gnc/matlab/utility_scripts/rate_from_quat.m b/gnc/matlab/utility_scripts/rate_from_quat.m deleted file mode 100644 index 1f664cb3d1..0000000000 --- a/gnc/matlab/utility_scripts/rate_from_quat.m +++ /dev/null @@ -1,45 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - - - -function [body_rates, time_out] = rate_from_quat(time, quat) -% Fundamentals of Spacecraft Attitude Determinatinon and Control -% F. Landis Markley and John L. Crassidis -% Eqn 3.21 - -time_out = time(2:end); -body_rates = zeros(length(time)-1, 3); -for ii = 2:length(time) - q = quat(ii,:); - q_dot = (q-quat(ii-1,:))/(time(ii) - time(ii-1)); - body_rates(ii,:) = 2*xi(q)'*q_dot'; - -end - -end - -function mat_out = xi(q) -% Fundamentals of Spacecraft Attitude Determinatinon and Control -% F. Landis Markley and John L. Crassidis -% Eqn A.9b - - q4 = q(4); - q_vec = q(1:3)'; - mat_out = [diag([q4, q4, q4])+skew(q_vec); -q_vec']; - -end diff --git a/gnc/matlab/utility_scripts/rssrow.m b/gnc/matlab/utility_scripts/rssrow.m deleted file mode 100644 index f251bf187b..0000000000 --- a/gnc/matlab/utility_scripts/rssrow.m +++ /dev/null @@ -1,20 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -% Takes the root-sum square of each row of the matrix a -function result=rssrow(a) -result = sqrt(sum(a.^2, 2)); \ No newline at end of file diff --git a/gnc/matlab/utility_scripts/write_tunable_param_functions.m b/gnc/matlab/utility_scripts/write_tunable_param_functions.m deleted file mode 100644 index 135df76f25..0000000000 --- a/gnc/matlab/utility_scripts/write_tunable_param_functions.m +++ /dev/null @@ -1,81 +0,0 @@ -% Copyright (c) 2017, United States Government, as represented by the -% Administrator of the National Aeronautics and Space Administration. -% -% All rights reserved. -% -% The Astrobee platform is licensed under the Apache License, Version 2.0 -% (the "License"); you may not use this file except in compliance with the -% License. You may obtain a copy of the License at -% -% http://www.apache.org/licenses/LICENSE-2.0 -% -% Unless required by applicable law or agreed to in writing, software -% distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -% WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -% License for the specific language governing permissions and limitations -% under the License. - -function write_tunable_param_functions(source_header, boolean_exceptions, output_fname, function_name, input_object_type, input_object_name) -%% Unpack the header file - -% Read in the header file that contains all the tunable parameters -header_text = fileread(source_header); - -expr = '(\w+_T) (tun_\w+)(\[\d+\]|)'; % Reg expression to find all the variables denoted tun_* -tunable_vars = regexp(header_text, expr, 'tokens'); % Execute the reg expression - -% model_name = regexp(header_text, 'RT_MODEL_\w+_T', 'match'); % Find the data type of the controller object -% model_name = model_name{1}; -fid = fopen([output_fname '.cpp'], 'w+'); -fprintf(fid, ['// Autogenerated file, see ' mfilename ' for details\n\n']); -fprintf(fid, ['#include "' output_fname '.h"\n']); -fprintf(fid, ['#include \n' ... - '#include \n#include \n\n']); -fprintf(fid, '// Do config setup and add includes\n'); -fprintf(fid, ['void ' function_name '(config_reader::ConfigReader* config, ' input_object_type ' ' input_object_name ') {\n\n']); -fprintf(fid, [' auto & p = ' input_object_name '->defaultParam;\n\n' ]); -for ii = 1:length(tunable_vars) - % Boolean flags, look for names in the list or with _f suffix - if ~isempty(regexp(tunable_vars{ii}{2}, '_f\>', 'ONCE')) || any(~cellfun(@isempty, strfind(boolean_exceptions, tunable_vars{ii}{2}))) - boolean_var_name = regexp(tunable_vars{ii}{2}, 'tun_(\w+)', 'tokens'); % Extract the variable name with no tun_ - boolean_var_name = boolean_var_name{1}{1}; % extract the string from the regexp return - var_name = tunable_vars{ii}{2}; - expr= [' bool ' boolean_var_name ';\n' ... - ' if (!config->GetBool("' var_name '", &' boolean_var_name '))\n' ... - ' ROS_FATAL("Unspecified ' var_name '.");\n' ... - ' p->' var_name ' = ' boolean_var_name ';']; - - elseif isempty(tunable_vars{ii}{3}) % Real scalar - var_name = tunable_vars{ii}{2}; - expr = [' if (!config->GetReal("' var_name '", &p->' var_name '))\n' ... - ' ROS_FATAL("Unspecified ' var_name '.");']; - % Scalar Flag - elseif ~isempty(tunable_vars{ii}{3}) % Real array - var_name = tunable_vars{ii}{2}; - dims = size(evalin('base', var_name)); % Pull the dimensions from the base workspace - if all(dims > 1) % Check to see if it is a matrix - expr = sprintf([' if (!msg_conversions::config_read_matrix(config, "' var_name '", %d, %d, p->%s))\n' ... - ' ROS_FATAL("Unspecified %s.");'],dims, var_name, var_name); - else - var_size = eval(tunable_vars{ii}{3}); - expr = [' if (!msg_conversions::config_read_array(config, "' var_name '", ' num2str(var_size) ' , p->' var_name '))\n' ... - ' ROS_FATAL("Unspecified ' var_name '.");']; - end - - end - - fprintf(fid, [expr '\n']); - - -end -% Close out the function -fprintf(fid, '}\n'); -fclose(fid); - -%% Write Header File -fid = fopen([output_fname '.h'], 'w+'); -fprintf(fid, '#ifndef READ_PARAMS_CPP\n#define READ_PARAMS_CPP\n\n'); -fprintf(fid, 'namespace config_reader {\n class ConfigReader;\n}\n\n'); -fprintf(fid, ['#include "' source_header '"\n']); -fprintf(fid, ['void ' function_name '(config_reader::ConfigReader* config, ' input_object_type ' ' input_object_name ');\n\n#endif']); -fclose(fid); diff --git a/gnc/pmc/CMakeLists.txt b/gnc/pmc/CMakeLists.txt new file mode 100644 index 0000000000..c47eea724f --- /dev/null +++ b/gnc/pmc/CMakeLists.txt @@ -0,0 +1,79 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +cmake_minimum_required(VERSION 3.0) +project(pmc) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + config_reader +) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + pmc + CATKIN_DEPENDS + roscpp + config_reader +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +# Declare C++ libraries +add_library(pmc + src/pmc_sim.cc + src/fam.cc + src/shared.cc +) +add_dependencies(pmc ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pmc ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/gnc/pmc/include/pmc/fam.h b/gnc/pmc/include/pmc/fam.h new file mode 100644 index 0000000000..bf647bbca6 --- /dev/null +++ b/gnc/pmc/include/pmc/fam.h @@ -0,0 +1,60 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef PMC_FAM_H_ +#define PMC_FAM_H_ + +#include + +namespace pmc { + +struct FamInput { + Eigen::Vector3f body_force_cmd; + Eigen::Vector3f body_torque_cmd; + Eigen::Vector3f center_of_mass; + uint8_t speed_gain_cmd; +}; + +class Fam { + public: + void Step(const FamInput& in, uint8_t* speed_cmd, Eigen::Matrix & servo_pwm_cmd); + void UpdateCOM(const Eigen::Vector3f & com); + protected: + void CalcThrustMatrices(const Eigen::Vector3f & force, const Eigen::Vector3f & torque, + Eigen::Matrix& pmc1_nozzle_thrusts, Eigen::Matrix& pmc2_nozzle_thrusts); + float ComputePlenumDeltaPressure(float impeller_speed, Eigen::Matrix & discharge_coeff, + const Eigen::Matrix & nozzle_thrusts); + void CalcPMServoCmd(bool is_pmc1, uint8_t speed_gain_cmd, const Eigen::Matrix & nozzle_thrusts, + uint8_t & impeller_speed_cmd, Eigen::Matrix & servo_pwm_cmd, + Eigen::Matrix & nozzle_theta_cmd, + Eigen::Matrix & normalized_pressure_density, Eigen::Matrix & command_area_per_nozzle); + + Eigen::Matrix thrust2force_, thrust2torque_; + Eigen::Matrix forcetorque2thrust_; + + static const float IMPELLER_SPEEDS[]; + // this lookup table was computed in fam_force_allocation_module_prep.m. We do not expect the values to + // change so we have just copied them. it is a function of air density and the propeller properties. + static constexpr int THRUST_LOOKUP_SIZE = 316; + static const float THRUST_LOOKUP_BREAKPOINTS[]; + static const float THRUST_LOOKUP_CDP[]; +}; + +} // end namespace pmc + +#endif // PMC_FAM_H_ diff --git a/gnc/pmc/include/pmc/pmc_sim.h b/gnc/pmc/include/pmc/pmc_sim.h new file mode 100644 index 0000000000..92589f716d --- /dev/null +++ b/gnc/pmc/include/pmc/pmc_sim.h @@ -0,0 +1,93 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef PMC_PMC_SIM_H_ +#define PMC_PMC_SIM_H_ + +#include + +namespace pmc { + +class PID { + public: + void Initialize(float Kp, float Ki, float Kd, float out_max, float out_min); + float Run(float x); + + private: + float kp, ki, kd, omax, omin; + float last; + float integral; +}; + +class Blower { + public: + explicit Blower(bool is_pmc1); + void Step(int impeller_cmd, Eigen::Matrix & servo_cmd, Eigen::Vector3f & omega, float voltage); + Eigen::Vector3f Force() {return force_B_;} + Eigen::Vector3f Torque() {return torque_B_;} + float ImpellerCurrent() {return impeller_current_;} + Eigen::Matrix NozzlesTheta() {return nozzles_theta_;} + Eigen::Matrix ServoCurrent() {return servo_current_;} + float MotorSpeed() {return last_speed_;} + + private: + void ServoModel(const Eigen::Matrix & servo_cmd, Eigen::Matrix & nozzle_theta, + Eigen::Matrix & nozzle_area); + void ImpellerModel(int speed_cmd, float voltage, float* motor_current, float* motor_torque); + Eigen::Matrix Aerodynamics(const Eigen::Matrix & nozzle_area); + void BlowerBodyDynamics(const Eigen::Vector3f & omega_body, Eigen::Matrix & nozzle_thrusts, + float motor_torque); + + Eigen::Matrix prev_omega_, servo_thetas_, backlash_theta_, discharge_coeff_; + Eigen::Matrix nozzle_offsets_, nozzle_orientations_; + Eigen::Vector3f center_of_mass_, impeller_orientation_; + PID servo_pids_[6], speed_pid_; + float prev_speed_rate_; + float zero_thrust_area_; + // outputs + Eigen::Vector3f force_B_, torque_B_; + Eigen::Matrix nozzles_theta_, servo_current_; + float impeller_speed_, last_speed_; + float impeller_current_; +}; + +class PMCSim { + public: + PMCSim(); + virtual void Step(); + virtual void SetAngularVelocity(float x, float y, float z); + virtual void SetBatteryVoltage(float voltage); + + void SetImpellerCmd(int blower, unsigned char cmd) {impeller_cmd_[blower] = cmd;} + unsigned char ImpellerCmd(int blower) {return impeller_cmd_[blower];} + void SetServoCmd(int blower, int index, float value) {servo_cmd_[blower][index] = value;} + float MotorSpeed(int blower) {return (blower == 0) ? b1.MotorSpeed() : b2.MotorSpeed();} + Eigen::Vector3f Force() {return b1.Force() + b2.Force();} + Eigen::Vector3f Torque() {return b1.Torque() + b2.Torque();} + + private: + Blower b1, b2; + Eigen::Vector3f omega_; + float voltage_; + unsigned char impeller_cmd_[2]; + Eigen::Matrix servo_cmd_[2]; +}; + +} // end namespace pmc + +#endif // PMC_PMC_SIM_H_ diff --git a/gnc/pmc/include/pmc/shared.h b/gnc/pmc/include/pmc/shared.h new file mode 100644 index 0000000000..e428b54339 --- /dev/null +++ b/gnc/pmc/include/pmc/shared.h @@ -0,0 +1,98 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef PMC_SHARED_H_ +#define PMC_SHARED_H_ + +#include +#include + +namespace pmc { + +// constants +class PMCConstants { + public: + static constexpr float DT = 1.0 / 62.5; + static constexpr float units_inches_to_meters = 0.0254; + static const Eigen::Matrix discharge_coeff1, discharge_coeff2; + // [m] Position vector of the nozzle locations in the body frame + static const Eigen::Matrix nozzle_offsets1, nozzle_offsets2; + // [unit vec] PM1: Pointing Direction of each nozzle + static const Eigen::Matrix nozzle_orientations1, nozzle_orientations2; + // [m^2] If air is leaking from the plenum, the + // '0 thrust' position (all nozzles closed) will have an effective open area + static const Eigen::Vector3f impeller_orientation1, impeller_orientation2; + static const Eigen::Matrix nozzle_widths; // [m] + static constexpr float zero_thrust_area[2] = {0.0044667, 0.0042273}; + // [rad] THEORETICAL max angle that a nozzle can open to + static constexpr float abp_nozzle_max_open_angle = 79.91 * M_PI / 180.0; + // [rad] Min angle that the nozzle can close to + static constexpr float abp_nozzle_min_open_angle = 15.68 * M_PI / 180.0; + static constexpr float abp_nozzle_flap_length = 0.5353 * units_inches_to_meters; // [m] + static constexpr float abp_nozzle_intake_height = 0.5154 * units_inches_to_meters; // [m] + static constexpr float abp_nozzle_gear_ratio = 0.5; + // bpm_servo_peak_torque*bpm_servo_motor_gear_ratio/(bpm_servo_peak_curr); [Nm/A] Servo motor torque constant + static constexpr float servo_motor_k = 0.2893 * 0.01 / 1.5; + // bpm_servo_max_voltage/bpm_servo_peak_curr; %[ohm] Servo internal resistance + static constexpr float servo_motor_r = 6.0 / 1.5; + // bpm_imp_noload_curr*bpm_imp_motor_torque_k/bpm_imp_noload_speed ; %[Nm*s] Viscous Friction + static constexpr float servo_motor_friction = 3e-7; + // %[kg*m^2] (Tuned in analysis_servo_motor_test.m) Servo gearbox inertia. + static constexpr float servo_motor_gearbox_inertia = .000000025; + static constexpr float servo_motor_gear_ratio = 1.0 / 100.0; + // dc_motor_model + static constexpr float imp_motor_speed_k = 374*2*M_PI/60; // [(rad/s)/V] Impeller Speed constant + static constexpr float imp_motor_r = 1.2; // %[ohm] Impeller Motor internal resistance + static constexpr float imp_motor_torque_k = 0.0255; // [Nm/A] Impeller Torque constant + // noload_curr * torque_k / noload_speed [Nm*s] Viscous Friction + static constexpr float imp_motor_friction_coeff = 0.144 * imp_motor_torque_k / (4380*2*M_PI/60.0); + static constexpr float backlash_half_width = 1.0 * M_PI / 180.0 / 2; + static constexpr float impeller_speed2pwm = 0.792095; // [CNT/(rad/sec)] Converts impeller speeds into PWM values + static constexpr float impeller_diameter = 5.5 * units_inches_to_meters; // [m] + static constexpr float air_density = + 1.2; // [kg/m^3] Air density inside of the ISS (some places in code this was 1.225?) + static constexpr float impeller_inertia = 0.001; + // generated in bpm_blower_propulsion_module_prep.m + static const float AREA_LOOKUP_INPUT[]; + static const float CDP_LOOKUP_OUTPUT[]; + static const int AREA_LOOKUP_TABLE_SIZE; +}; + +float Lookup(int table_size, const float lookup[], const float breakpoints[], float value); + +template +Eigen::Matrix pseudoInverse( + const MatT& mat, typename MatT::Scalar tolerance = typename MatT::Scalar{std::numeric_limits::epsilon()}) { + typedef typename MatT::Scalar Scalar; + auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); + const auto& singularValues = svd.singularValues(); + Eigen::Matrix singularValuesInv(mat.cols(), mat.rows()); + singularValuesInv.setZero(); + for (unsigned int i = 0; i < singularValues.size(); ++i) { + if (singularValues(i) > tolerance) { + singularValuesInv(i, i) = Scalar {1} / singularValues(i); + } else { + singularValuesInv(i, i) = Scalar {0}; + } + } + return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint(); +} + +} // end namespace pmc + +#endif // PMC_SHARED_H_ diff --git a/gnc/pmc/package.xml b/gnc/pmc/package.xml new file mode 100644 index 0000000000..f89a147458 --- /dev/null +++ b/gnc/pmc/package.xml @@ -0,0 +1,22 @@ + + + pmc + 0.0.0 + + Simulates the propulsion modules. + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + roscpp + config_reader + roscpp + config_reader + diff --git a/gnc/pmc/readme.md b/gnc/pmc/readme.md new file mode 100644 index 0000000000..f35f7c244c --- /dev/null +++ b/gnc/pmc/readme.md @@ -0,0 +1,8 @@ +\page pmc Propulsion System + +This package includes two components revolving around Astrobee's propulsion system: + +* FAM (Force Allocation Module)--- Converts force and torque commands to nozzle opening angles +* PMC simulator--- simulates the robot's propusion system. It is essentially the reverse of the + FAM--- given impeller commands and servo commands, it returns the force and torque applied to the robot. + diff --git a/gnc/pmc/src/fam.cc b/gnc/pmc/src/fam.cc new file mode 100644 index 0000000000..dc34ee21ac --- /dev/null +++ b/gnc/pmc/src/fam.cc @@ -0,0 +1,222 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include "pmc/fam.h" +#include "pmc/shared.h" + +namespace pmc { + +const float Fam::IMPELLER_SPEEDS[3] = {2000 * 2 * M_PI / 60, 2500 * 2 * M_PI / 60, 2800 * 2 * M_PI / 60}; +const float Fam::THRUST_LOOKUP_CDP[316] = + {0.0825, 0.08262563, 0.08274699, 0.08286414, 0.08297713, 0.08308605, 0.08319096, 0.08329192, 0.083389, 0.08348226, + 0.08357175, 0.08365756, 0.08373973, 0.08381831, 0.08389337, 0.08396497, 0.08403317, 0.084098, 0.08415954, + 0.08421783, 0.08427292, 0.08432487, 0.08437373, 0.08441953, 0.08446234, 0.08450221, 0.08453917, 0.08457327, + 0.08460455, 0.08463307, 0.08465886, 0.08468197, 0.08470242, 0.08472028, 0.08473557, 0.08474834, 0.08475861, + 0.08476643, 0.08477184, 0.08477487, 0.08477554, 0.08477391, 0.08477, 0.08476384, 0.08475546, 0.08474489, + 0.08473217, 0.08471732, 0.08470037, 0.08468134, 0.08466027, 0.08463717, 0.08461209, 0.08458503, 0.08455602, + 0.08452509, 0.08449226, 0.08445755, 0.08442097, 0.08438256, 0.08434232, 0.08430029, 0.08425647, 0.0842109, + 0.08416356, 0.0841145, 0.08406372, 0.08401124, 0.08395708, 0.08390123, 0.08384374, 0.08378459, 0.08372381, + 0.0836614, 0.08359738, 0.08353176, 0.08346455, 0.08339575, 0.08332538, 0.08325344, 0.08317994, 0.08310489, + 0.0830283, 0.08295017, 0.0828705, 0.0827893, 0.08270658, 0.08262234, 0.08253659, 0.08244931, 0.08236053, + 0.08227024, 0.08217844, 0.08208514, 0.08199033, 0.08189403, 0.08179621, 0.08169689, 0.08159607, 0.08149374, + 0.0813899, 0.08128455, 0.0811777, 0.08106932, 0.08095943, 0.08084802, 0.08073508, 0.08062062, 0.08050462, + 0.08038708, 0.080268, 0.08014737, 0.08002519, 0.07990145, 0.07977614, 0.07964926, 0.0795208, 0.07939076, + 0.07925911, 0.07912587, 0.07899102, 0.07885455, 0.07871646, 0.07857673, 0.07843536, 0.07829233, 0.07814765, + 0.07800129, 0.07785325, 0.07770353, 0.0775521, 0.07739896, 0.07724411, 0.07708752, 0.07692919, 0.07676911, + 0.07660726, 0.07644365, 0.07627825, 0.07611104, 0.07594204, 0.07577121, 0.07559855, 0.07542405, 0.0752477, + 0.07506947, 0.07488938, 0.07470739, 0.0745235, 0.0743377, 0.07414997, 0.0739603, 0.07376868, 0.0735751, + 0.07337955, 0.07318202, 0.07298248, 0.07278094, 0.07257736, 0.07237177, 0.07216411, 0.0719544, 0.07174262, + 0.07152876, 0.07131281, 0.07109474, 0.07087456, 0.07065225, 0.0704278, 0.0702012, 0.06997244, 0.0697415, + 0.06950837, 0.06927305, 0.06903552, 0.06879577, 0.06855379, 0.06830958, 0.06806312, 0.06781439, 0.0675634, + 0.06731013, 0.06705457, 0.06679671, 0.06653654, 0.06627406, 0.06600925, 0.06574212, 0.06547263, 0.06520081, + 0.06492662, 0.06465007, 0.06437115, 0.06408985, 0.06380615, 0.06352008, 0.0632316, 0.06294072, 0.06264743, + 0.06235172, 0.0620536, 0.06175305, 0.06145008, 0.06114467, 0.06083682, 0.06052653, 0.0602138, 0.05989863, + 0.05958102, 0.05926095, 0.05893843, 0.05861346, 0.05828604, 0.05795617, 0.05762384, 0.05728908, 0.05695185, + 0.05661217, 0.05627006, 0.05592549, 0.05557849, 0.05522906, 0.05487716, 0.05452287, 0.05416614, 0.05380698, + 0.05344541, 0.05308145, 0.05271507, 0.05234631, 0.05197515, 0.05160162, 0.05122572, 0.05084746, 0.05046686, + 0.0500839, 0.04969862, 0.04931103, 0.04892113, 0.04852895, 0.04813449, 0.04773775, 0.04733878, 0.04693756, + 0.04653414, 0.04612852, 0.0457207, 0.04531071, 0.04489859, 0.04448432, 0.04406797, 0.04364951, 0.043229, + 0.04280642, 0.04238184, 0.04195525, 0.0415267, 0.04109619, 0.04066373, 0.04022939, 0.03979318, 0.03935514, + 0.03891524, 0.03847359, 0.03803019, 0.03758504, 0.03713819, 0.03668968, 0.03623956, 0.03578782, 0.03533453, + 0.0348797, 0.03442341, 0.03396563, 0.03350645, 0.03304591, 0.03258402, 0.03212083, 0.03165638, 0.03119075, + 0.03072393, 0.03025598, 0.02978694, 0.02931687, 0.02884581, 0.02837384, 0.02790096, 0.02742722, 0.02695271, + 0.02647745, 0.02600148, 0.0255249, 0.02504772, 0.02457004, 0.02409187, 0.0236133, 0.02313438, 0.02265514, + 0.02217567, 0.02169603, 0.0212163, 0.02073649, 0.02025672, 0.01977702, 0.01929744, 0.01881814, 0.01833903, + 0.01786034, 0.01738204, 0.01690428, 0.01642704, 0.01595047, 0.0154746, 0.01499952, 0.01452534, 0.01405202}; +const float Fam::THRUST_LOOKUP_BREAKPOINTS[316] = + {-1.726016e-05, -1.722989e-05, -1.719863e-05, -1.716642e-05, -1.713327e-05, -1.70992e-05, + -1.706422e-05, -1.702835e-05, -1.699161e-05, -1.695402e-05, -1.691559e-05, -1.687634e-05, -1.683629e-05, + -1.679544e-05, -1.675383e-05, -1.671145e-05, -1.666833e-05, -1.662449e-05, -1.657993e-05, -1.653467e-05, + -1.648872e-05, -1.64421e-05, -1.639483e-05, -1.63469e-05, -1.629834e-05, -1.624916e-05, -1.619938e-05, + -1.614899e-05, -1.609802e-05, -1.604648e-05, -1.599437e-05, -1.594171e-05, -1.588852e-05, -1.583479e-05, + -1.578054e-05, -1.572579e-05, -1.567054e-05, -1.561479e-05, -1.555857e-05, -1.550188e-05, -1.544472e-05, + -1.538712e-05, -1.532907e-05, -1.527058e-05, -1.521167e-05, -1.515234e-05, -1.50926e-05, -1.503245e-05, + -1.497191e-05, -1.491098e-05, -1.484967e-05, -1.478799e-05, -1.472595e-05, -1.466354e-05, -1.460077e-05, + -1.453767e-05, -1.447422e-05, -1.441043e-05, -1.434632e-05, -1.428188e-05, -1.421712e-05, -1.415206e-05, + -1.408669e-05, -1.402101e-05, -1.395504e-05, -1.388878e-05, -1.382222e-05, -1.375539e-05, -1.368828e-05, + -1.36209e-05, -1.355324e-05, -1.348532e-05, -1.341714e-05, -1.33487e-05, -1.328001e-05, -1.321107e-05, + -1.314188e-05, -1.307245e-05, -1.300278e-05, -1.293287e-05, -1.286272e-05, -1.279235e-05, -1.272175e-05, + -1.265092e-05, -1.257987e-05, -1.250861e-05, -1.243712e-05, -1.236542e-05, -1.22935e-05, -1.222138e-05, + -1.214905e-05, -1.207651e-05, -1.200377e-05, -1.193082e-05, -1.185768e-05, -1.178433e-05, -1.171079e-05, + -1.163706e-05, -1.156313e-05, -1.1489e-05, -1.141469e-05, -1.134019e-05, -1.12655e-05, -1.119062e-05, + -1.111556e-05, -1.104032e-05, -1.096488e-05, -1.088927e-05, -1.081348e-05, -1.073751e-05, -1.066136e-05, + -1.058503e-05, -1.050852e-05, -1.043184e-05, -1.035498e-05, -1.027794e-05, -1.020074e-05, -1.012335e-05, + -1.00458e-05, -9.968076e-06, -9.890177e-06, -9.812112e-06, -9.733873e-06, -9.655467e-06, -9.576891e-06, + -9.498148e-06, -9.419237e-06, -9.340158e-06, -9.260914e-06, -9.181501e-06, -9.101926e-06, -9.022184e-06, + -8.942282e-06, -8.862211e-06, -8.781981e-06, -8.701585e-06, -8.621029e-06, -8.540313e-06, -8.459433e-06, + -8.378394e-06, -8.297197e-06, -8.21584e-06, -8.134323e-06, -8.052652e-06, -7.970822e-06, -7.888837e-06, + -7.806695e-06, -7.724401e-06, -7.641954e-06, -7.559351e-06, -7.476597e-06, -7.393692e-06, -7.310637e-06, + -7.227431e-06, -7.144081e-06, -7.060581e-06, -6.976937e-06, -6.893148e-06, -6.809215e-06, -6.72514e-06, + -6.640923e-06, -6.556566e-06, -6.472074e-06, -6.387444e-06, -6.302679e-06, -6.217779e-06, -6.132747e-06, + -6.047586e-06, -5.962296e-06, -5.876881e-06, -5.791341e-06, -5.705679e-06, -5.619894e-06, -5.533992e-06, + -5.447974e-06, -5.361841e-06, -5.275595e-06, -5.189244e-06, -5.102784e-06, -5.01622e-06, -4.929554e-06, + -4.84279e-06, -4.75593e-06, -4.668978e-06, -4.581933e-06, -4.494807e-06, -4.407594e-06, -4.3203e-06, + -4.23293e-06, -4.145489e-06, -4.057978e-06, -3.970398e-06, -3.882762e-06, -3.795063e-06, -3.707311e-06, + -3.619512e-06, -3.531663e-06, -3.443777e-06, -3.355851e-06, -3.267894e-06, -3.179912e-06, -3.091906e-06, + -3.003885e-06, -2.91585e-06, -2.827807e-06, -2.739765e-06, -2.651725e-06, -2.563695e-06, -2.475683e-06, + -2.387691e-06, -2.299727e-06, -2.211795e-06, -2.123905e-06, -2.036061e-06, -1.94827e-06, -1.860541e-06, + -1.772879e-06, -1.68529e-06, -1.597783e-06, -1.510364e-06, -1.423045e-06, -1.335829e-06, -1.248723e-06, + -1.161742e-06, -1.074888e-06, -9.881705e-07, -9.015994e-07, -8.151846e-07, -7.289327e-07, -6.428545e-07, + -5.569591e-07, -4.712565e-07, -3.85754e-07, -3.004643e-07, -2.153974e-07, -1.305634e-07, -4.597132e-08, + 3.836612e-08, 1.224362e-07, 2.062316e-07, 2.897377e-07, 3.729447e-07, 4.558406e-07, 5.384145e-07, + 6.206483e-07, 7.025319e-07, 7.840572e-07, 8.652069e-07, 9.459663e-07, 1.026325e-06, 1.106265e-06, + 1.185778e-06, 1.264842e-06, 1.343449e-06, 1.421578e-06, 1.49922e-06, 1.576352e-06, 1.652964e-06, + 1.729039e-06, 1.804556e-06, 1.879501e-06, 1.953857e-06, 2.027606e-06, 2.10073e-06, 2.173206e-06, + 2.245024e-06, 2.316159e-06, 2.386592e-06, 2.456303e-06, 2.525276e-06, 2.593483e-06, 2.660908e-06, + 2.727526e-06, 2.793316e-06, 2.858257e-06, 2.922321e-06, 2.985491e-06, 3.047737e-06, 3.109039e-06, + 3.169364e-06, 3.228694e-06, 3.286999e-06, 3.344252e-06, 3.400427e-06, 3.455491e-06, 3.509414e-06, + 3.562171e-06, 3.613729e-06, 3.664055e-06, 3.713118e-06, 3.760882e-06, 3.807314e-06, 3.852379e-06, + 3.896036e-06, 3.938252e-06, 3.978987e-06, 4.018196e-06, 4.055845e-06, 4.091884e-06, 4.126272e-06, + 4.158959e-06, 4.1899e-06, 4.219043e-06, 4.246336e-06, 4.271728e-06, 4.295159e-06, 4.31657e-06, 4.335897e-06, + 4.353081e-06, 4.36805e-06, 4.380732e-06, 4.391055e-06, 4.398935e-06, 4.404289e-06, 4.407031e-06, 4.407059e-06}; + +void Fam::UpdateCOM(const Eigen::Vector3f & com) { + // latch nozzle thrust matrices + Eigen::Matrix fam_P_nozzle_B_B, fam_nozzle_orientations; + fam_P_nozzle_B_B << PMCConstants::nozzle_offsets1, PMCConstants::nozzle_offsets2; + fam_nozzle_orientations << PMCConstants::nozzle_orientations1, PMCConstants::nozzle_orientations2; + + Eigen::Matrix nozzle_moment_arm = fam_P_nozzle_B_B - com.transpose().replicate(12, 1); + for (int i = 0; i < 12; i++) + thrust2torque_.col(i) = -nozzle_moment_arm.row(i).cross(fam_nozzle_orientations.row(i)).transpose(); + + thrust2force_ = -fam_nozzle_orientations.transpose(); + Eigen::Matrix thrust2forcetorque; + thrust2forcetorque << thrust2force_, thrust2torque_; + forcetorque2thrust_ = Eigen::MatrixXf::Identity(12, 6); + forcetorque2thrust_ = pseudoInverse(thrust2forcetorque); +} + +void Fam::CalcThrustMatrices(const Eigen::Vector3f & force, const Eigen::Vector3f & torque, + Eigen::Matrix& pmc1_nozzle_thrusts, Eigen::Matrix& pmc2_nozzle_thrusts) { + Eigen::Matrix forcetorque; + forcetorque << force, torque; + Eigen::Matrix thrust_per_nozzle = forcetorque2thrust_ * forcetorque; + const int fam_nozzle_dirs[][4] = {{1, 2, 7, 8}, {3, 4, 9, 10}, {5, 6, 11, 12}}; + // prevents negative thrust from being commanded by adding the min value below zero to the null vectors + // negative thrust occurs because the inverse matrix distributes thrust accross the + // nozzles aligned in an axis equally, even if they are pointed in opposite directions + for (int i = 0; i < 3; i++) { + float min_thrust = 0.0; + for (int j = 0; j < 4; j++) { + float t = thrust_per_nozzle(fam_nozzle_dirs[i][j] - 1); + if (t < min_thrust) + min_thrust = t; + } + if (min_thrust < 0) { + for (int j = 0; j < 4; j++) + thrust_per_nozzle(fam_nozzle_dirs[i][j] - 1) -= min_thrust; + } + } + pmc1_nozzle_thrusts = thrust_per_nozzle.head<6>(); + pmc2_nozzle_thrusts = thrust_per_nozzle.tail<6>(); +} + +float Fam::ComputePlenumDeltaPressure(float impeller_speed, Eigen::Matrix & discharge_coeff, + const Eigen::Matrix & nozzle_thrusts) { + // lookup_data_tables + float total_thrust = (nozzle_thrusts.array() / discharge_coeff.array()).sum(); + float cdp = Lookup(Fam::THRUST_LOOKUP_SIZE, Fam::THRUST_LOOKUP_CDP, Fam::THRUST_LOOKUP_BREAKPOINTS, + total_thrust / (impeller_speed * impeller_speed)); + return impeller_speed * impeller_speed * PMCConstants::impeller_diameter * PMCConstants::impeller_diameter * + PMCConstants::air_density * cdp; +} + +void Fam::CalcPMServoCmd(bool is_pmc1, uint8_t speed_gain_cmd, const Eigen::Matrix & nozzle_thrusts, + uint8_t & impeller_speed_cmd, Eigen::Matrix & servo_pwm_cmd, + Eigen::Matrix & nozzle_theta_cmd, + Eigen::Matrix & normalized_pressure_density, Eigen::Matrix & command_area_per_nozzle) { + if (speed_gain_cmd > 3) + ROS_FATAL("Invalid speed command %d.", speed_gain_cmd); + + Eigen::Matrix discharge_coeff = + is_pmc1 ? PMCConstants::discharge_coeff1 : PMCConstants::discharge_coeff2; + + float impeller_speed = 0.0f, plenum_delta_pressure = 0.0f; + impeller_speed_cmd = 0; + if (speed_gain_cmd != 0) { + impeller_speed = Fam::IMPELLER_SPEEDS[speed_gain_cmd - 1]; + impeller_speed_cmd = static_cast(PMCConstants::impeller_speed2pwm * impeller_speed + 0.5); + plenum_delta_pressure = ComputePlenumDeltaPressure(impeller_speed, discharge_coeff, nozzle_thrusts); + } + normalized_pressure_density = 2 * (discharge_coeff.array() * discharge_coeff.array()).matrix() * + plenum_delta_pressure; + command_area_per_nozzle = (nozzle_thrusts.array() / normalized_pressure_density.array()).matrix(); + const int nozzle_flap_count = 2; + Eigen::Matrix openings = (PMCConstants::abp_nozzle_intake_height - (1.0 / nozzle_flap_count) * + command_area_per_nozzle.array() / PMCConstants::nozzle_widths.array()).matrix() / + PMCConstants::abp_nozzle_flap_length; + const float min_open_cos = cos(PMCConstants::abp_nozzle_max_open_angle); + const float max_open_cos = cos(PMCConstants::abp_nozzle_min_open_angle); + for (int i = 0; i < 6; i++) { + if (openings(i) < min_open_cos || std::isnan(openings[i])) + openings[i] = min_open_cos; + if (openings(i) > max_open_cos) + openings[i] = max_open_cos; + nozzle_theta_cmd[i] = acos(openings[i]) - PMCConstants::abp_nozzle_min_open_angle; + } + const int servo_max_pwm = 255; + const int servo_min_pwm = 0; + // %[-] Conversion between servo angle (not nozzle angle) command and resulting servo PWM + servo_pwm_cmd = static_cast(servo_max_pwm - servo_min_pwm) / + (PMCConstants::abp_nozzle_max_open_angle - PMCConstants::abp_nozzle_min_open_angle) * + nozzle_theta_cmd; +} + +void Fam::Step(const FamInput& in, uint8_t* speed_cmd, Eigen::Matrix & servo_pwm_cmd) { + Eigen::Matrix thrust2force_B, thrust2torque_B; + Eigen::Matrix pmc1_nozzle_thrusts, pmc2_nozzle_thrusts; + CalcThrustMatrices( + in.body_force_cmd, in.body_torque_cmd, + pmc1_nozzle_thrusts, pmc2_nozzle_thrusts); + Eigen::Matrix nozzle_theta, normalized_pressure_density, command_area_per_nozzle; + Eigen::Matrix servo_pwm; + CalcPMServoCmd(true, in.speed_gain_cmd, pmc1_nozzle_thrusts, speed_cmd[0], servo_pwm, nozzle_theta, + normalized_pressure_density, command_area_per_nozzle); + servo_pwm_cmd.head<6>() = servo_pwm; + CalcPMServoCmd(false, in.speed_gain_cmd, pmc2_nozzle_thrusts, speed_cmd[1], servo_pwm, nozzle_theta, + normalized_pressure_density, command_area_per_nozzle); + servo_pwm_cmd.tail<6>() = servo_pwm; + // computes actual output force and torque, but currently not published + // Eigen::Matrix temp = (command_area_per_nozzle.array() * normalized_pressure_density.array()); + // Eigen::Vector3f predicted_force = thrust2force_B * temp; + // Eigen::Vector3f predicted_torque = thrust2torque_B * temp; +} +} // namespace pmc + diff --git a/gnc/pmc/src/pmc_sim.cc b/gnc/pmc/src/pmc_sim.cc new file mode 100644 index 0000000000..b5221b4b2a --- /dev/null +++ b/gnc/pmc/src/pmc_sim.cc @@ -0,0 +1,186 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include "pmc/pmc_sim.h" +#include "pmc/shared.h" + +namespace pmc { + +void PID::Initialize(float Kp, float Ki, float Kd, float out_max, float out_min) { + kp = Kp; + ki = Ki; + kd = Kd; + omax = out_max; + omin = out_min; + last = integral = 0.0; +} + +float PID::Run(float x) { + float d = kd * x - last; + float out = kp * x + ki * integral + d; + last += d * PMCConstants::DT; // kind of confused here but matches simmulink + integral += x * PMCConstants::DT; + if (out < omin) + out = omin; + if (out > omax) + out = omax; + return out; +} + +Blower::Blower(bool is_pmc1) { + prev_omega_ << 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f; + backlash_theta_ << 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f; + servo_thetas_ << 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f; + prev_speed_rate_ = impeller_speed_ = 0.0f; + for (int i = 0; i < 6; i++) + servo_pids_[i].Initialize(5.0, 1.5, 0.8, 6.0, -6.0); + speed_pid_.Initialize(0.2, 0.1, 0.05, 16.6, -16.6); + if (is_pmc1) { + discharge_coeff_ = PMCConstants::discharge_coeff1; + zero_thrust_area_ = PMCConstants::zero_thrust_area[0]; + nozzle_offsets_ = PMCConstants::nozzle_offsets1; + nozzle_orientations_ = PMCConstants::nozzle_orientations1; + impeller_orientation_ = PMCConstants::impeller_orientation1; + } else { + discharge_coeff_ = PMCConstants::discharge_coeff2; + zero_thrust_area_ = PMCConstants::zero_thrust_area[1]; + nozzle_offsets_ = PMCConstants::nozzle_offsets2; + nozzle_orientations_ = PMCConstants::nozzle_orientations2; + impeller_orientation_ = PMCConstants::impeller_orientation2; + } + + center_of_mass_ << 0.0, 0.0, 0.0; // hmmm.... this seems wrong but was used originally in the simulink +} + +void Blower::ServoModel(const Eigen::Matrix & servo_cmd, Eigen::Matrix & nozzle_theta, + Eigen::Matrix & nozzle_area) { + const float bpm_servo_max_theta = PMCConstants::abp_nozzle_gear_ratio * + (PMCConstants::abp_nozzle_max_open_angle - PMCConstants::abp_nozzle_min_open_angle); + // [-] Conversion between servo PWM command (0-100) and resulting angle (0-90) + const float bpm_servo_pwm2angle = bpm_servo_max_theta / 255.0; + // backlash box + for (int i = 0; i < 6; i++) { + if (servo_thetas_[i] < backlash_theta_[i] - PMCConstants::backlash_half_width) + backlash_theta_[i] = servo_thetas_[i] + PMCConstants::backlash_half_width; + else if (servo_thetas_[i] > backlash_theta_[i] + PMCConstants::backlash_half_width) + backlash_theta_[i] = servo_thetas_[i] - PMCConstants::backlash_half_width; + } + Eigen::Matrix voltage, + err = bpm_servo_pwm2angle * servo_cmd - backlash_theta_ * PMCConstants::servo_motor_gear_ratio; + for (int i = 0; i < 6; i++) + voltage[i] = servo_pids_[i].Run(err[i]); + servo_current_ = (voltage - PMCConstants::servo_motor_k * prev_omega_) * (1.0 / PMCConstants::servo_motor_r); + servo_thetas_ += 1.0 / 62.5 * prev_omega_; + servo_thetas_ = servo_thetas_.cwiseMin(bpm_servo_max_theta / PMCConstants::servo_motor_gear_ratio).cwiseMax(0.0); + // torque - viscous friction + Eigen::Matrix motor_torque = + PMCConstants::servo_motor_k * servo_current_ - PMCConstants::servo_motor_friction * prev_omega_; + prev_omega_ += (1.0 / PMCConstants::servo_motor_gearbox_inertia) / 62.5 * motor_torque; + nozzle_theta = backlash_theta_ * PMCConstants::servo_motor_gear_ratio * 1.0 / PMCConstants::abp_nozzle_gear_ratio; + for (int i = 0; i < 6; i++) { + // * nozzle_flap_count * nozzle_width + nozzle_area[i] = + (PMCConstants::abp_nozzle_intake_height - + cos(PMCConstants::abp_nozzle_min_open_angle + nozzle_theta[i]) * PMCConstants::abp_nozzle_flap_length) * + 2 * PMCConstants::nozzle_widths[i]; + } +} + +void Blower::ImpellerModel(int speed_cmd, float voltage, float* motor_current, float* motor_torque) { + const float max_change = 200*2*M_PI/60.0 * PMCConstants::DT; + float speed = speed_cmd / PMCConstants::impeller_speed2pwm; + if (speed > prev_speed_rate_ + max_change) + speed = prev_speed_rate_ + max_change; + else if (speed < prev_speed_rate_ - max_change) + speed = prev_speed_rate_ - max_change; + prev_speed_rate_ = speed; + + speed = speed - impeller_speed_; + float v = speed_pid_.Run(speed); + v = std::max(-voltage, std::min(voltage, v)); + + *motor_current = (v - impeller_speed_ / PMCConstants::imp_motor_speed_k) / PMCConstants::imp_motor_r; + *motor_torque = + (*motor_current) * PMCConstants::imp_motor_torque_k - PMCConstants::imp_motor_friction_coeff * impeller_speed_; + + last_speed_ = impeller_speed_; + // in simulink, drag torque would be subtracted from motor, but set to 0 + // noise is also set to 0 + const float impeller_inertia = 0.001; + impeller_speed_ += *motor_torque / impeller_inertia / 62.5; +} + +Eigen::Matrix Blower::Aerodynamics(const Eigen::Matrix & nozzle_area) { + float area = zero_thrust_area_ + (discharge_coeff_.array() * nozzle_area.array()).sum(); + float cdp = Lookup(PMCConstants::AREA_LOOKUP_TABLE_SIZE, PMCConstants::CDP_LOOKUP_OUTPUT, + PMCConstants::AREA_LOOKUP_INPUT, area); + float delta_p = impeller_speed_ * impeller_speed_ * cdp * PMCConstants::impeller_diameter * + PMCConstants::impeller_diameter * PMCConstants::air_density; + // 1.25 is thrust_error_scale_factor + return 2 * delta_p * discharge_coeff_.array() * discharge_coeff_.array() * nozzle_area.array() * 1.25; + // note: currently in simulink noise is set to zero +} + +void Blower::Step(int impeller_cmd, Eigen::Matrix & servo_cmd, Eigen::Vector3f & omega, float voltage) { + Eigen::Matrix nozzle_area; + float motor_torque; + ServoModel(servo_cmd, nozzles_theta_, nozzle_area); + ImpellerModel(impeller_cmd, voltage, &impeller_current_, &motor_torque); + Eigen::Matrix nozzle_thrust = Aerodynamics(nozzle_area); + BlowerBodyDynamics(omega, nozzle_thrust, motor_torque); +} + +void Blower::BlowerBodyDynamics(const Eigen::Vector3f & omega_body, Eigen::Matrix & nozzle_thrusts, + float motor_torque) { + Eigen::Matrix thrust2force, thrust2torque; + // latch nozzle thrust matrices + Eigen::Matrix nozzle_moment_arm = nozzle_offsets_ - center_of_mass_.transpose().replicate(6, 1); + for (int i = 0; i < 6; i++) + thrust2torque.col(i) = -nozzle_moment_arm.row(i).cross(nozzle_orientations_.row(i)).transpose(); + + thrust2force = -nozzle_orientations_.transpose(); + + Eigen::Vector3f momentum_B = impeller_orientation_ * PMCConstants::impeller_inertia * impeller_speed_; + + Eigen::Matrix a = nozzle_thrusts; + force_B_ = thrust2force * nozzle_thrusts; + torque_B_ = impeller_orientation_ * -motor_torque + omega_body.cross(momentum_B) + thrust2torque * nozzle_thrusts; +} + +PMCSim::PMCSim(void) : b1(true), b2(false), omega_(0, 0, 0), voltage_(0.0f) { + for (int i = 0; i < 2; i++) { + impeller_cmd_[i] = 0; + servo_cmd_[i] << 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f; + } +} + +void PMCSim::Step(void) { + b1.Step(impeller_cmd_[0], servo_cmd_[0], omega_, voltage_); + b2.Step(impeller_cmd_[1], servo_cmd_[1], omega_, voltage_); +} + +void PMCSim::SetAngularVelocity(float x, float y, float z) { + omega_ << x, y, z; +} + +void PMCSim::SetBatteryVoltage(float voltage) { + voltage_ = voltage; +} + +} // namespace pmc diff --git a/gnc/pmc/src/shared.cc b/gnc/pmc/src/shared.cc new file mode 100644 index 0000000000..ff91080433 --- /dev/null +++ b/gnc/pmc/src/shared.cc @@ -0,0 +1,235 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include "pmc/shared.h" + +namespace pmc { + +const int PMCConstants::AREA_LOOKUP_TABLE_SIZE = 334; +const float PMCConstants::AREA_LOOKUP_INPUT[334] = + { 0.0F, 1.46138646E-5F, 2.92062887E-5F, 4.37784511E-5F, 5.83315159E-5F, + 7.28665837E-5F, 8.7384753E-5F, 0.000101887068F, 0.000116374569F, + 0.000130848246F, 0.000145309066F, 0.000159758F, 0.000174195899F, + 0.000188623759F, 0.000203042364F, 0.000217452631F, 0.000231855331F, + 0.000246251322F, 0.000260641362F, 0.000275026192F, 0.000289406598F, + 0.000303783338F, 0.00031815705F, 0.000332528463F, 0.000346898247F, + 0.000361267099F, 0.00037563566F, 0.000390004541F, 0.000404374383F, + 0.000418745767F, 0.000433119305F, 0.000447495579F, 0.000461875083F, + 0.000476258458F, 0.000490646285F, 0.000505038945F, 0.00051943725F, + 0.000533841376F, 0.000548252079F, 0.000562669651F, 0.00057709479F, + 0.000591527845F, 0.000605969399F, 0.000620419742F, 0.000634879572F, + 0.000649349065F, 0.000663828861F, 0.000678319368F, 0.000692821F, + 0.000707334315F, 0.000721859455F, 0.000736397109F, 0.000750947569F, + 0.0007655113F, 0.000780088594F, 0.000794680149F, 0.000809286F, + 0.000823906856F, 0.000838542939F, 0.000853194739F, 0.000867862604F, + 0.000882546941F, 0.000897248159F, 0.00091196649F, 0.000926702633F, + 0.000941456819F, 0.0009562294F, 0.00097102084F, 0.000985831604F, + 0.00100066199F, 0.00101551227F, 0.00103038305F, 0.0010452749F, + 0.00106018782F, 0.00107512227F, 0.00109007885F, 0.00110505789F, + 0.00112005987F, 0.00113508501F, 0.00115013402F, 0.00116520724F, + 0.0011803048F, 0.00119542738F, 0.00121057557F, 0.00122574961F, 0.00124095F, + 0.00125617709F, 0.00127143157F, 0.00128671364F, 0.001302024F, 0.00131736277F, + 0.00133273099F, 0.00134812866F, 0.00136355648F, 0.00137901504F, + 0.00139450456F, 0.00141002587F, 0.00142557954F, 0.0014411658F, + 0.00145678537F, 0.00147243857F, 0.00148812647F, 0.00150384917F, + 0.00151960761F, 0.00153540191F, 0.00155123312F, 0.0015671018F, + 0.00158300844F, 0.00159895339F, 0.00161493802F, 0.00163096236F, + 0.00164702744F, 0.0016631335F, 0.00167928194F, 0.00169547275F, + 0.00171170721F, 0.00172798557F, 0.00174430886F, 0.0017606779F, + 0.00177709304F, 0.00179355568F, 0.00181006629F, 0.00182662567F, + 0.00184323464F, 0.00185989426F, 0.00187660486F, 0.0018933682F, + 0.00191018439F, 0.00192705484F, 0.00194398011F, 0.00196096138F, + 0.00197799969F, 0.00199509552F, 0.00201225071F, 0.00202946551F, + 0.00204674155F, 0.00206407951F, 0.00208148058F, 0.00209894567F, + 0.00211647665F, 0.00213407329F, 0.00215173839F, 0.00216947193F, + 0.00218727579F, 0.00220515113F, 0.00222309888F, 0.00224112067F, + 0.0022592172F, 0.0022773908F, 0.00229564216F, 0.00231397292F, 0.00233238423F, + 0.00235087774F, 0.00236945506F, 0.00238811737F, 0.00240686629F, + 0.00242570369F, 0.00244463095F, 0.00246364973F, 0.00248276163F, + 0.00250196829F, 0.00252127182F, 0.00254067336F, 0.00256017526F, + 0.00257977913F, 0.00259948708F, 0.00261930074F, 0.00263922219F, + 0.00265925354F, 0.00267939596F, 0.00269965269F, 0.00272002514F, + 0.00274051586F, 0.00276112719F, 0.002781861F, 0.00280271959F, 0.00282370578F, + 0.00284482073F, 0.00286606839F, 0.00288745062F, 0.00290896976F, + 0.00293062883F, 0.00295243063F, 0.00297437701F, 0.00299647171F, + 0.00301871705F, 0.00304111606F, 0.00306367176F, 0.00308638718F, + 0.00310926535F, 0.0031323093F, 0.00315552298F, 0.00317890849F, + 0.00320247072F, 0.00322621223F, 0.00325013627F, 0.00327424728F, + 0.00329854828F, 0.00332304346F, 0.00334773702F, 0.0033726322F, + 0.00339773367F, 0.00342304516F, 0.00344857131F, 0.00347431679F, + 0.00350028556F, 0.00352648227F, 0.00355291134F, 0.00357957813F, + 0.00360648776F, 0.0036336449F, 0.00366105489F, 0.00368872308F, + 0.00371665484F, 0.00374485622F, 0.00377333187F, 0.00380208902F, + 0.00383113371F, 0.00386047084F, 0.00389010808F, 0.00392005173F, + 0.00395030761F, 0.00398088479F, 0.00401178841F, 0.0040430259F, + 0.00407460658F, 0.00410653651F, 0.00413882406F, 0.0041714781F, + 0.00420450559F, 0.00423791679F, 0.00427172F, 0.00430592475F, 0.00434054F, + 0.00437557558F, 0.00441104267F, 0.00444695121F, 0.00448331F, 0.00452013267F, + 0.00455742935F, 0.00459521171F, 0.00463349326F, 0.00467228517F, + 0.00471160142F, 0.00475145364F, 0.00479185674F, 0.00483282562F, + 0.00487437518F, 0.00491652032F, 0.00495927641F, 0.00500265928F, + 0.00504668895F, 0.00509137847F, 0.00513675157F, 0.0051828213F, + 0.00522961235F, 0.00527714146F, 0.00532543147F, 0.00537450658F, + 0.00542438496F, 0.00547509314F, 0.00552665396F, 0.0055790972F, + 0.00563244335F, 0.00568672409F, 0.00574196922F, 0.00579820713F, + 0.00585547043F, 0.00591378892F, 0.00597320078F, 0.00603374047F, + 0.00609544525F, 0.00615835097F, 0.00622250512F, 0.0062879459F, 0.006354718F, + 0.00642287126F, 0.00649245409F, 0.00656351773F, 0.00663611386F, + 0.00671030255F, 0.00678614574F, 0.00686370581F, 0.00694304705F, + 0.00702424115F, 0.00710735889F, 0.00719248503F, 0.0072797006F, + 0.00736908847F, 0.00746074459F, 0.00755477371F, 0.00765126897F, + 0.00775034819F, 0.00785212312F, 0.00795672741F, 0.00806429F, 0.00817495212F, + 0.00828887522F, 0.00840621162F, 0.00852714293F, 0.00865184888F, + 0.00878054276F, 0.00891342945F, 0.00905075204F, 0.00919276662F, + 0.0093397228F, 0.00949195493F, 0.00964974798F, 0.00981347449F, + 0.00998349674F, 0.0101602506F, 0.0103441793F, 0.0105357962F, 0.010735645F, + 0.0109443273F, 0.0111625558F, 0.0113910241F, 0.0116305985F, 0.0118822251F, + 0.0121469265F, 0.0124259172F, 0.0127205F, 0.0130322482F, 0.0133628659F, + 0.0137143685F, 0.0140890917F, 0.0144897308F, 0.0149194878F, 0.01538203F, + 0.0158818662F, 0.0164243504F, 0.0170160942F, 0.0176650118F, 0.0183811728F }; +const float PMCConstants::CDP_LOOKUP_OUTPUT[334] = + { 0.0825F, 0.082625635F, 0.08274699F, 0.0828641355F, 0.082977131F, + 0.083086051F, 0.0831909552F, 0.083291918F, 0.083389F, 0.083482258F, + 0.0835717544F, 0.0836575627F, 0.0837397277F, 0.083818309F, 0.0838933736F, + 0.0839649737F, 0.0840331689F, 0.0840980038F, 0.0841595381F, 0.0842178315F, + 0.0842729211F, 0.084324874F, 0.0843737274F, 0.0844195336F, 0.0844623446F, + 0.0845022127F, 0.0845391676F, 0.0845732689F, 0.0846045539F, 0.0846330673F, + 0.0846588612F, 0.0846819654F, 0.0847024247F, 0.0847202763F, 0.0847355723F, + 0.0847483352F, 0.0847586095F, 0.0847664326F, 0.0847718418F, 0.0847748667F, + 0.0847755447F, 0.084773913F, 0.08477F, 0.0847638398F, 0.0847554579F, + 0.084744893F, 0.0847321674F, 0.0847173184F, 0.0847003683F, 0.0846813396F, + 0.0846602693F, 0.0846371725F, 0.0846120864F, 0.0845850259F, 0.0845560208F, + 0.0845250934F, 0.0844922587F, 0.0844575465F, 0.0844209716F, 0.0843825564F, + 0.0843423232F, 0.0843002871F, 0.0842564702F, 0.084210895F, 0.0841635615F, + 0.0841145F, 0.0840637162F, 0.0840112418F, 0.0839570761F, 0.083901234F, + 0.0838437378F, 0.0837845877F, 0.0837238058F, 0.0836614F, 0.0835973844F, + 0.0835317597F, 0.083464548F, 0.0833957493F, 0.0833253786F, 0.0832534432F, + 0.0831799433F, 0.0831048936F, 0.0830283F, 0.0829501674F, 0.0828705F, + 0.0827893F, 0.0827065781F, 0.0826223418F, 0.0825365856F, 0.0824493095F, + 0.0823605284F, 0.0822702423F, 0.0821784437F, 0.08208514F, 0.0819903314F, + 0.0818940252F, 0.0817962065F, 0.0816968903F, 0.081596069F, 0.0814937353F, + 0.081389904F, 0.0812845528F, 0.0811777F, 0.0810693204F, 0.0809594318F, + 0.0808480158F, 0.0807350799F, 0.0806206167F, 0.0805046186F, 0.0803870782F, + 0.080268F, 0.0801473707F, 0.0800251886F, 0.0799014494F, 0.0797761381F, + 0.0796492621F, 0.0795208F, 0.0793907568F, 0.0792591125F, 0.0791258737F, + 0.0789910182F, 0.0788545534F, 0.0787164569F, 0.0785767287F, 0.0784353614F, + 0.0782923326F, 0.0781476498F, 0.0780012906F, 0.077853255F, 0.0777035281F, + 0.0775521F, 0.0773989633F, 0.0772441104F, 0.0770875216F, 0.0769291893F, + 0.0767691135F, 0.0766072646F, 0.0764436498F, 0.0762782469F, 0.076111041F, + 0.0759420395F, 0.0757712126F, 0.0755985528F, 0.0754240528F, 0.0752477F, + 0.0750694722F, 0.0748893768F, 0.0747073889F, 0.0745235F, 0.0743377F, + 0.0741499662F, 0.0739603F, 0.0737686828F, 0.0735751F, 0.0733795539F, + 0.0731820166F, 0.0729824826F, 0.072780937F, 0.0725773647F, 0.072371766F, + 0.0721641108F, 0.0719544F, 0.071742624F, 0.0715287626F, 0.0713128075F, + 0.0710947439F, 0.0708745569F, 0.0706522539F, 0.0704278F, 0.0702012F, + 0.0699724406F, 0.0697415F, 0.0695083737F, 0.0692730546F, 0.0690355226F, + 0.0687957704F, 0.0685537905F, 0.0683095828F, 0.0680631176F, 0.0678143948F, + 0.0675634F, 0.0673101321F, 0.0670545697F, 0.0667967126F, 0.0665365383F, + 0.0662740618F, 0.0660092533F, 0.0657421201F, 0.0654726326F, 0.0652008057F, + 0.0649266243F, 0.0646500662F, 0.0643711537F, 0.0640898496F, 0.0638061538F, + 0.0635200813F, 0.0632316F, 0.0629407242F, 0.0626474321F, 0.0623517223F, + 0.0620536F, 0.0617530458F, 0.0614500828F, 0.0611446686F, 0.0608368181F, + 0.0605265312F, 0.0602138F, 0.0598986298F, 0.0595810153F, 0.0592609458F, + 0.0589384325F, 0.0586134642F, 0.0582860447F, 0.0579561703F, 0.0576238409F, + 0.0572890751F, 0.0569518507F, 0.0566121712F, 0.0562700555F, 0.0559254885F, + 0.0555784889F, 0.0552290566F, 0.054877162F, 0.0545228682F, 0.0541661382F, + 0.0538069829F, 0.0534454137F, 0.0530814491F, 0.0527150668F, 0.0523463078F, + 0.0519751534F, 0.0516016223F, 0.0512257218F, 0.0508474559F, 0.0504668579F, + 0.0500839F, 0.0496986173F, 0.0493110269F, 0.0489211343F, 0.0485289469F, + 0.0481344871F, 0.0477377549F, 0.04733878F, 0.0469375588F, 0.0465341397F, + 0.0461285152F, 0.0457207039F, 0.0453107134F, 0.0448985882F, 0.0444843248F, + 0.0440679714F, 0.0436495095F, 0.0432290025F, 0.0428064205F, 0.0423818417F, + 0.0419552475F, 0.0415266976F, 0.0410961919F, 0.0406637266F, 0.0402293913F, + 0.0397931822F, 0.0393551365F, 0.038915243F, 0.0384735875F, 0.0380301885F, + 0.0375850387F, 0.0371381938F, 0.0366896801F, 0.0362395607F, 0.0357878208F, + 0.0353345275F, 0.0348797F, 0.0344234072F, 0.0339656323F, 0.033506453F, + 0.033045914F, 0.032584019F, 0.0321208276F, 0.0316563807F, 0.0311907455F, + 0.0307239257F, 0.0302559771F, 0.0297869369F, 0.0293168686F, 0.0288458094F, + 0.0283738412F, 0.0279009566F, 0.0274272151F, 0.0269527063F, 0.0264774524F, + 0.0260014758F, 0.0255248956F, 0.0250477232F, 0.0245700441F, 0.0240918733F, + 0.0236133039F, 0.0231343843F, 0.0226551369F, 0.0221756734F, 0.0216960311F, + 0.0212163F, 0.0207364894F, 0.0202567242F, 0.0197770186F, 0.0192974396F, + 0.0188181363F, 0.0183390342F, 0.0178603418F, 0.0173820443F, 0.0169042759F, + 0.0164270438F, 0.0159504749F, 0.0154745989F, 0.01499952F, 0.0145253353F, + 0.0140520222F, 0.0135798045F, 0.0131086893F, 0.0126387365F, 0.0121700801F, + 0.0117027499F, 0.0112369023F, 0.0107725374F, 0.0103098117F, 0.0098488F, + 0.00938956812F, 0.00893222168F, 0.00847681239F, 0.00802352652F, + 0.00757240178F, 0.007123549F, 0.00667699846F, 0.00623294385F, 0.0057914448F}; + +// determined by test +const Eigen::Matrix PMCConstants::discharge_coeff1 = (Eigen::Matrix() << + 0.914971062, 0.755778254, 0.940762925, 0.792109779, 0.92401881, 0.930319765).finished(); +const Eigen::Matrix PMCConstants::discharge_coeff2 = (Eigen::Matrix() << + 0.947114008, 0.764468916, 1.000000000, 0.90480943, 0.936555627, 0.893794766).finished(); +const Eigen::Matrix PMCConstants::nozzle_widths = + PMCConstants::units_inches_to_meters * (Eigen::Matrix() << 5.0, 5.0, 2.8, 2.8, 2.8, 2.8).finished(); + +const Eigen::Matrix PMCConstants::nozzle_offsets1 = PMCConstants::units_inches_to_meters * + (Eigen::Matrix() << + 6.00, 4.01, -1.56, + -6.00, 4.01, 1.56, + 2.83, 6.00, 2.83, + -2.83, 6.00, -2.83, + -2.66, 4.01, 6.00, + 2.66, 4.01, -6.00).finished(); +const Eigen::Matrix PMCConstants::nozzle_offsets2 = PMCConstants::units_inches_to_meters * + (Eigen::Matrix() << + -6.00, -4.01, -1.56, + 6.00, -4.01, 1.56, + -2.83, -6.00, 2.83, + 2.83, -6.00, -2.83, + 2.66, -4.01, 6.00, + -2.66, -4.01, -6.00).finished(); +const Eigen::Matrix PMCConstants::nozzle_orientations1 = (Eigen::Matrix() << + 1, 0, 0, + -1, 0, 0, + 0, 1, 0, + 0, 1, 0, + 0, 0, 1, + 0, 0, -1).finished(); +const Eigen::Matrix PMCConstants::nozzle_orientations2 = (Eigen::Matrix() << + -1, 0, 0, + 1, 0, 0, + 0, -1, 0, + 0, -1, 0, + 0, 0, 1, + 0, 0, -1).finished(); +const Eigen::Vector3f PMCConstants::impeller_orientation1 = (Eigen::Vector3f() << 0, 1, 0).finished(); +const Eigen::Vector3f PMCConstants::impeller_orientation2 = (Eigen::Vector3f() << 0, -1, 0).finished(); + +float Lookup(int table_size, const float lookup[], const float breakpoints[], float value) { + if (value < breakpoints[0]) + return lookup[0]; + if (value > breakpoints[table_size - 1]) + return lookup[table_size - 1]; + int start = 0, end = table_size; + while (start < end - 1) { + int mid = (start + end) / 2; + if (breakpoints[mid] <= value) + start = mid; + else + end = mid; + } + + float ratio = (value - breakpoints[start]) / (breakpoints[start + 1] - breakpoints[start]); + return (1.0 - ratio) * lookup[start] + ratio * lookup[start + 1]; +} + +} // namespace pmc + diff --git a/gnc/readme.md b/gnc/readme.md index 9fb3258687..1b7b238e22 100644 --- a/gnc/readme.md +++ b/gnc/readme.md @@ -1,42 +1,20 @@ \page gnc Guidance, Navigation & Control -The Guidance, Navigation & Control subsystem is responsible for tracking the -robot's pose and controlling the robot's motion. +The Guidance, Navigation & Control subsystem is responsible for controlling the robot's motion. -# GNC Subsystems +## GNC Subsystems -GNC consists of four main subsystems, that run in a chain: +GNC consists of three main subsystems: -\subpage ekf -1. `EKF`: The extended Kalman filter integrates IMU measurements, as well as -visual measurements from the localization subsystem, to track the robot's pose. -\subpage ctl -2. `CTL`: The control subsystem takes the robot's current state from the EKF, +1. \subpage ctl : The control subsystem takes the robot's current state from the localizer, and determines the forces and torques needed to execute the commands sent from the mobility subsystem. -\subpage fam -3. `FAM`: The force allocation module takes the force and torque commands -from the control subsystem, and determines the PMC speeds and nozzle positions -to best execute these forces and torques. -\subpage -4. `SIM`: The simulator simulates the inputs to the EKF, and simulates the -robot's motion based on the outputs of the FAM. - -# GNC Code Organization -Unlike the rest of flight software, the GNC systems are written mainly in -Simulink. Simulink drag and drop box diagrams are then converted to -automatically generated C code. The structure of the GNC code is: +2. \subpage fam : The force allocation module takes the force and torque commands +from the control subsystem, and determines the PMC speeds and nozzle positions +to best execute these forces and torques. This is a ROS package that calls the +main functionality in the pmc module which does not depend on ROS. -1. [EKF](@ref ekf), [CTL](@ref ctl) and [FAM](@ref fam) are GN&C ROS Wrapper. -[Sim Wrapper](@ref simwrapper) is the model side of the GN&C ROS Wrapper. -These folders contain ROS nodelets that mainly convert inputs and outputs -between ROS messages and Simulink. -\subpage gncautocode -2. [GNC Autocode](@ref gncautocode): This folder contains a thin C++ wrapper -around the auto-generated C functions, which are all compiled in this package -into a library. -\subpage matlab -\subpage simwrapper -3. [Matlab](@ref matlab): This folder contains the Matlab / Simulink code. This -is where all the core functionality is located. +3. \subpage pmc: This module implements the main functionality of the FAM. It also +simulates the PMC, converting PMC speeds and nozzle positions to forces and torques +(the inverse of the force allocation module), used only in simulation. diff --git a/gnc/sim_wrapper/CMakeLists.txt b/gnc/sim_wrapper/CMakeLists.txt deleted file mode 100644 index d4fb7d17ca..0000000000 --- a/gnc/sim_wrapper/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -project(sim_wrapper) - -catkin_package( - LIBRARIES sim_wrapper - CATKIN_DEPENDS roscpp geometry_msgs ff_util nodelet pluginlib -) - -create_library(TARGET sim_wrapper - LIBS ${catkin_LIBRARIES} gnc_autocode sparse_mapping msg_conversions camera ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS ff_hw_msgs -) - -create_tool_targets(DIR tools - LIBS sim_wrapper -) - -install_launch_files() \ No newline at end of file diff --git a/gnc/sim_wrapper/include/sim_wrapper/sim.h b/gnc/sim_wrapper/include/sim_wrapper/sim.h deleted file mode 100644 index faee49b39a..0000000000 --- a/gnc/sim_wrapper/include/sim_wrapper/sim.h +++ /dev/null @@ -1,171 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef SIM_WRAPPER_SIM_H_ -#define SIM_WRAPPER_SIM_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -namespace sim_wrapper { -class Sim { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - explicit Sim(ros::NodeHandle *nh); - ~Sim(); - void Step(); - - protected: - void PmcFamCallBack(ff_hw_msgs::PmcCommand::ConstPtr const& pmc); - - /** - * Publish the ground truth location - */ - void PublishTruth(); - /** - * pull imu message values from gnc sim into ros data type - * @return true if value is updated and should be published - */ - bool PullImuMsg(); - - /** - * pull pmc state message value from gnc sim into ros data type - * @return true if value is updated and should be published - */ - bool PullPmcMsg(); - - /** - * pull the registration pulse, look for a landmark blip - */ - bool PullLandmarkPulseMsg(); - - /** - * pull out the landmarks that have been generated. - */ - bool PullLandmarkMsg(); - - /** - * callback to enable or disable the mapped landmark localization - */ - bool LandmarkEnable(ff_msgs::SetBool::Request &req, ff_msgs::SetBool::Response &res); - - /** - * pull the registration pulse, look for ar tag blip - */ - bool PullTagPulseMsg(); - - /** - * pull out the ar tag landmarks that have been generated. - */ - bool PullTagMsg(); - - /** - * callback to enable or disable the AR tag localization - */ - bool TagEnable(ff_msgs::SetBool::Request &req, ff_msgs::SetBool::Response &res); - - /** - * pull the registration pulse, look for a landmark blip - */ - bool PullOpticalPulseMsg(); - - /** - * pull out the landmarks that have been generated. - */ - bool PullOpticalMsg(); - - /** - * callback to enable or disable the optical flow - */ - bool OpticalEnable(ff_msgs::SetBool::Request &req, ff_msgs::SetBool::Response &res); - - /** - * pull out a depth image. - */ - bool PullDepthMsg(); - - /** - * pull the registration pulse, look for depth blip - */ - bool PullDepthPulseMsg(); - - /** - * callback to enable or disable depth localization - */ - bool DepthEnable(ff_msgs::SetBool::Request &req, ff_msgs::SetBool::Response &res); - - - void ReadParams(void); - - gnc_autocode::GncSimAutocode gnc_; - - config_reader::ConfigReader config_; - ros::Timer config_timer_; - camera::CameraParameters cam_params_; - - // Pre-allocated message structure so we don't hit the allocator. - sensor_msgs::Imu ros_imu_; - ff_hw_msgs::PmcTelemetry ros_pmc_; - ff_msgs::Feature2dArray ros_optical_; - ff_msgs::VisualLandmarks ros_landmark_; - ff_msgs::VisualLandmarks ros_tag_; - ff_msgs::DepthLandmarks ros_depth_; - ff_msgs::CameraRegistration ros_reg_pulse_; - geometry_msgs::PoseStamped ros_truth_pose_; - geometry_msgs::TwistStamped ros_truth_twist_; - geometry_msgs::AccelStamped ros_truth_accel_; - - // variables used to detect change - ros::Time cur_time_, last_landmark_time_, last_tag_time_, last_optical_time_, last_depth_time_; - bool last_landmark_pulse_, last_tag_pulse_, last_optical_pulse_, last_depth_pulse_; - unsigned int ml_camera_count_, ar_camera_count_, of_camera_count_, dl_camera_count_; - bool en_landmark_, en_tag_, en_optical_, en_depth_; - - int pmc_command_id_; - - size_t ml_max_features_, ar_max_features_, hr_max_features_, of_history_size_, of_max_features_; - ros::Publisher pub_imu_, pub_pmc_, pub_clock_, pub_landmark_pulse_, - pub_landmarks_, pub_optical_pulse_, pub_optical_, pub_truth_pose_, - pub_depth_, pub_depth_pulse_, pub_landmark_camera_, pub_ar_tags_camera_, - pub_ar_tags_pulse_, pub_ar_tags_, pub_depth_camera_; - ros::Subscriber sub_fam_, sub_fam_pmc_; - ros::ServiceServer srv_landmark_enable_, srv_optical_enable_, srv_ar_tags_enable_, srv_depth_enable_; -}; -} // end namespace sim_wrapper - -#endif // SIM_WRAPPER_SIM_H_ diff --git a/gnc/sim_wrapper/launch/sim_wrapper.launch b/gnc/sim_wrapper/launch/sim_wrapper.launch deleted file mode 100644 index 1e39418139..0000000000 --- a/gnc/sim_wrapper/launch/sim_wrapper.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/gnc/sim_wrapper/nodelet_plugins.xml b/gnc/sim_wrapper/nodelet_plugins.xml deleted file mode 100644 index f95d40bc8b..0000000000 --- a/gnc/sim_wrapper/nodelet_plugins.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Nodelet for sim_wrapper::SimWrapperNodelet - - \ No newline at end of file diff --git a/gnc/sim_wrapper/package.xml b/gnc/sim_wrapper/package.xml deleted file mode 100644 index f3c0e624ce..0000000000 --- a/gnc/sim_wrapper/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - sim_wrapper - 0.0.0 - - The sim_ros_wrapper package provides a wrapper to the gnc autocode generated SIM module - - - Apache License, Version 2.0 - - - Astrobee Flight Software - - - Astrobee Flight Software - - catkin - nodelet - pluginlib - ff_msgs - ff_util - roscpp - sensor_msgs - geometry_msgs - roscpp - nodelet - pluginlib - ff_msgs - ff_util - sensor_msgs - geometry_msgs - - - - \ No newline at end of file diff --git a/gnc/sim_wrapper/readme.md b/gnc/sim_wrapper/readme.md deleted file mode 100644 index e6fd22d582..0000000000 --- a/gnc/sim_wrapper/readme.md +++ /dev/null @@ -1,26 +0,0 @@ -\page simwrapper GNC Simulation Wrapper - -This package wraps around the GNC simulator functionality. -Currently, the things it simulates are very limited, to -the IMU, sparse mapping localization, optical flow, handrail detection, -and the robot's motion. - -# Required Inputs - -* `/ctl/fam` -* `/pmc_actuator/command` - -# Simulated Outputs - -* `/imu/data` -* `/ctl/vpp_state` -* `/pmc_actuator/telemetry` -* `/clock` -* `/localization/mapped_landmarks/registration` -* `/localization/mapped_landmarks/features` -* `/localization/optical_flow/features` -* `/localization/optical_flow/registration` -* `/localization/handrail/features` -* `/localization/handrail/registration` -* `/ground_truth` - diff --git a/gnc/sim_wrapper/src/sim.cc b/gnc/sim_wrapper/src/sim.cc deleted file mode 100644 index 5901a35502..0000000000 --- a/gnc/sim_wrapper/src/sim.cc +++ /dev/null @@ -1,518 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -#include - -// parameters sim_model_lib0_P are set in -// matlab/code_generation/sim_model_lib0_ert_rtw/sim_model_lib0_data.c - -namespace sim_wrapper { -Sim::Sim(ros::NodeHandle* nh) : - cam_params_(Eigen::Vector2i(0, 0), Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 0)), - cur_time_(0, 0), ml_camera_count_(0), ar_camera_count_(0), of_camera_count_(0), dl_camera_count_(0), - en_landmark_(false), en_tag_(false), en_optical_(false), en_depth_(false), - pmc_command_id_(0) { - config_.AddFile("cameras.config"); - config_.AddFile("geometry.config"); - config_.AddFile("gnc.config"); - - ReadParams(); - gnc_.Initialize(); - - // TODO(bcoltin): load these from gnc autocode - of_history_size_ = 4; - of_max_features_ = 50; - ml_max_features_ = 50; - hr_max_features_ = 50; - ar_max_features_ = 50; - - // initialize ROS - config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { - config_.CheckFilesUpdated(std::bind(&Sim::ReadParams, this)); - }, false, true); - - pub_imu_ = nh->advertise(TOPIC_HARDWARE_IMU, 5); - pub_pmc_ = nh->advertise(TOPIC_HARDWARE_PMC_TELEMETRY, 5); - pub_clock_ = nh->advertise(TOPIC_CLOCK, 1); - // Mapped landmarks - pub_landmark_camera_ = nh->advertise(TOPIC_HARDWARE_NAV_CAM, 1); - srv_landmark_enable_ = nh->advertiseService(SERVICE_LOCALIZATION_ML_ENABLE, &Sim::LandmarkEnable, this); - pub_landmark_pulse_ = nh->advertise(TOPIC_LOCALIZATION_ML_REGISTRATION, 1); - pub_landmarks_ = nh->advertise(TOPIC_LOCALIZATION_ML_FEATURES, 1); - // Optical flow - pub_optical_ = nh->advertise(TOPIC_LOCALIZATION_OF_FEATURES, 1); - srv_optical_enable_ = nh->advertiseService(SERVICE_LOCALIZATION_OF_ENABLE, &Sim::OpticalEnable, this); - pub_optical_pulse_ = nh->advertise(TOPIC_LOCALIZATION_OF_REGISTRATION, 1); - // AR tags - pub_ar_tags_camera_ = nh->advertise(TOPIC_HARDWARE_DOCK_CAM, 1); - srv_ar_tags_enable_ = nh->advertiseService(SERVICE_LOCALIZATION_AR_ENABLE, &Sim::TagEnable, this); - pub_ar_tags_pulse_ = nh->advertise(TOPIC_LOCALIZATION_AR_REGISTRATION, 1); - pub_ar_tags_ = nh->advertise(TOPIC_LOCALIZATION_AR_FEATURES, 1); - // Handrail localization - pub_depth_camera_ = nh->advertise((std::string) TOPIC_HARDWARE_PICOFLEXX_PREFIX - + (std::string) TOPIC_HARDWARE_NAME_PERCH_CAM + (std::string) TOPIC_HARDWARE_PICOFLEXX_SUFFIX, 1); - srv_depth_enable_ = nh->advertiseService(SERVICE_LOCALIZATION_HR_ENABLE, &Sim::DepthEnable, this); - pub_depth_ = nh->advertise(TOPIC_LOCALIZATION_HR_FEATURES, 1); - pub_depth_pulse_ = nh->advertise(TOPIC_LOCALIZATION_HR_REGISTRATION, 1); - // Ground truth - pub_truth_pose_ = nh->advertise(TOPIC_LOCALIZATION_TRUTH, 1); - // FAM callback - sub_fam_pmc_ = nh->subscribe(TOPIC_HARDWARE_PMC_COMMAND, 5, &Sim::PmcFamCallBack, this, - ros::TransportHints().tcpNoDelay()); -} - -Sim::~Sim() {} - -void Sim::ReadParams(void) { - if (!config_.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; - } - - gnc_.ReadParams(&config_); - - cam_params_ = camera::CameraParameters(&config_, "nav_cam"); - - auto& p = gnc_.sim_->defaultParam; - std::string imu_filename; - if (!config_.GetStr("imu_bias_file", &imu_filename)) { - ROS_FATAL("IMU bias file not specified."); - } - std::string bias_file = std::string(ff_common::GetConfigDir()) + std::string("/") + imu_filename; - - // set the biases to what the EKF expects - Eigen::Map accel_bias(p->epson_accel_bias_ic); - Eigen::Map gyro_bias(p->epson_gyro_bias_ic); - std::vector new_bias(3); - FILE* f = fopen(bias_file.c_str(), "r"); - if (f) { - int ret = fscanf(f, "%g %g %g\n", &new_bias[0], &new_bias[1], &new_bias[2]); - if (ret == 3) - gyro_bias = Eigen::Vector3d(new_bias[0], new_bias[1], new_bias[2]); - ret = fscanf(f, "%g %g %g\n", &new_bias[0], &new_bias[1], &new_bias[2]); - if (ret == 3) - accel_bias = Eigen::Vector3f(new_bias[0], new_bias[1], new_bias[2]); - fclose(f); - } else { - ROS_WARN("No bias file found at %s.", bias_file.c_str()); - } -} - -void Sim::PmcFamCallBack(ff_hw_msgs::PmcCommand::ConstPtr const& pmc) { - gnc_.act_msg_.act_timestamp_sec = cur_time_.sec; - gnc_.act_msg_.act_timestamp_nsec = cur_time_.nsec; - - gnc_.act_msg_.act_impeller_speed_cmd[0] = pmc->goals[0].motor_speed; - gnc_.act_msg_.act_impeller_speed_cmd[1] = pmc->goals[1].motor_speed; - for (int i = 0; i < 6; i++) { - gnc_.act_msg_.act_servo_pwm_cmd[i] = pmc->goals[0].nozzle_positions[i]; - gnc_.act_msg_.act_servo_pwm_cmd[6 + i] = pmc->goals[1].nozzle_positions[i]; - } -} - -void Sim::Step() { - gnc_.Step(); - - // Publish the clock msg from the simulator - rosgraph_msgs::Clock clock_msg; - clock_msg.clock.sec = cur_time_.sec; - clock_msg.clock.nsec = cur_time_.nsec; - pub_clock_.publish(clock_msg); - - cur_time_.nsec += 16000000; - if (cur_time_.nsec >= 1000000000) { - cur_time_.nsec -= 1000000000; - cur_time_.sec++; - } - - //////////////////////////////////////// - // Publish all the simulated messages // - //////////////////////////////////////// - // Ground truth - PublishTruth(); - // IMU - if (PullImuMsg()) - pub_imu_.publish(ros_imu_); - // PMC - if (PullPmcMsg()) - pub_pmc_.publish(ros_pmc_); - // 1 - Mapped landmarks - if (PullLandmarkMsg()) - if (en_landmark_) pub_landmarks_.publish(ros_landmark_); - if (PullLandmarkPulseMsg()) { - if (en_landmark_) pub_landmark_pulse_.publish(ros_reg_pulse_); - if (en_landmark_) pub_landmark_camera_.publish(sensor_msgs::Image()); - } - // 2 - AR tags - if (PullTagMsg()) - if (en_tag_) pub_ar_tags_.publish(ros_tag_); - if (PullTagPulseMsg()) { - if (en_tag_) pub_ar_tags_pulse_.publish(ros_reg_pulse_); - if (en_tag_) pub_ar_tags_camera_.publish(sensor_msgs::Image()); - } - // 3 - Optical flow - if (PullOpticalMsg()) - if (en_optical_) pub_optical_.publish(ros_optical_); - if (PullOpticalPulseMsg()) - if (en_optical_) pub_optical_pulse_.publish(ros_reg_pulse_); - // 4 - Handrail - if (PullDepthMsg()) - if (en_depth_) pub_depth_.publish(ros_depth_); - if (PullDepthPulseMsg()) { - if (en_depth_) pub_depth_camera_.publish(sensor_msgs::PointCloud2()); - if (en_depth_) pub_depth_pulse_.publish(ros_reg_pulse_); - } -} - -void Sim::PublishTruth() { - ros_truth_pose_.header.seq++; - ros_truth_pose_.header.stamp.sec = gnc_.ex_time_msg_.timestamp_sec; - ros_truth_pose_.header.stamp.nsec = gnc_.ex_time_msg_.timestamp_nsec; - ros_truth_pose_.header.frame_id = "world"; - ros_truth_pose_.pose.position = msg_conversions::array_to_ros_point(gnc_.env_msg_.P_B_ISS_ISS); - ros_truth_pose_.pose.orientation = msg_conversions::array_to_ros_quat(gnc_.env_msg_.Q_ISS2B); - pub_truth_pose_.publish(ros_truth_pose_); -} - -bool Sim::PullImuMsg() { - // check if value is updated - // publish on every tick - - ros_imu_.header.stamp = ros::Time::now(); - ros_imu_.header.frame_id = "body"; - - // set angular vel, ros message is double cast from float - ros_imu_.angular_velocity.x = static_cast(gnc_.imu_msg_.imu_omega_B_ECI_sensor[0]); - ros_imu_.angular_velocity.y = static_cast(gnc_.imu_msg_.imu_omega_B_ECI_sensor[1]); - ros_imu_.angular_velocity.z = static_cast(gnc_.imu_msg_.imu_omega_B_ECI_sensor[2]); - - // set linear accel - ros_imu_.linear_acceleration.x = static_cast(gnc_.imu_msg_.imu_A_B_ECI_sensor[0]); - ros_imu_.linear_acceleration.y = static_cast(gnc_.imu_msg_.imu_A_B_ECI_sensor[1]); - ros_imu_.linear_acceleration.z = static_cast(gnc_.imu_msg_.imu_A_B_ECI_sensor[2]); - - return true; -} - -bool Sim::PullPmcMsg() { - ros_pmc_.header.stamp.sec = gnc_.act_msg_.act_timestamp_sec; - ros_pmc_.header.stamp.nsec = gnc_.act_msg_.act_timestamp_nsec; - ros_pmc_.header.frame_id = "body"; - ros_pmc_.statuses.clear(); - - for (int i = 0; i < 2; i++) { - ff_hw_msgs::PmcStatus s; - s.motor_speed = gnc_.bpm_msg_.bpm_motor_speed[i]; - s.motor_current = gnc_.bpm_msg_.bpm_motor_curr[i]; - s.pressure = -1; - s.status_1 = 0; - s.status_2 = 0; - s.command_id = pmc_command_id_; - ros_pmc_.statuses.push_back(s); - } - pmc_command_id_++; - - return true; -} - -bool Sim::PullLandmarkPulseMsg() { - if (!(!last_landmark_pulse_ && gnc_.reg_pulse_.cvs_landmark_pulse > 0)) { - // early exit condition, not a rising edge - last_landmark_pulse_ = gnc_.reg_pulse_.cvs_landmark_pulse > 0; - return false; - } - - ros_reg_pulse_.camera_id = ++ml_camera_count_; - - last_landmark_pulse_ = gnc_.reg_pulse_.cvs_landmark_pulse > 0; - return true; -} - -bool Sim::PullLandmarkMsg() { - if (last_landmark_time_.sec == gnc_.landmark_msg_.cvs_timestamp_sec && - last_landmark_time_.nsec == gnc_.landmark_msg_.cvs_timestamp_nsec) { - return false; - } - - // Count how many landmarks there are - size_t landmark_count = 0; - for (auto flag : gnc_.landmark_msg_.cvs_valid_flag) { - if (flag > 0) { - landmark_count++; - } - } - std::vector landmarks, inlier_landmarks; - std::vector observations, inlier_observations; - landmarks.resize(landmark_count); - observations.resize(landmark_count); - size_t output_i = 0; - for (size_t i = 0; i < ml_max_features_; i++) { - if (gnc_.landmark_msg_.cvs_valid_flag[i] > 0) { - landmarks[output_i].x() = gnc_.landmark_msg_.cvs_landmarks[i]; - landmarks[output_i].y() = gnc_.landmark_msg_.cvs_landmarks[ml_max_features_ + i]; - landmarks[output_i].z() = gnc_.landmark_msg_.cvs_landmarks[2 * ml_max_features_ + i]; - observations[output_i].x() = gnc_.landmark_msg_.cvs_observations[i]; - observations[output_i].y() = gnc_.landmark_msg_.cvs_observations[ml_max_features_ + i]; - output_i++; - } - } - camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), cam_params_); - if (sparse_mapping::RansacEstimateCamera(landmarks, observations, 1000, 3, - &camera, &inlier_landmarks, &inlier_observations)) - return false; - Eigen::Affine3d global_pose = camera.GetTransform().inverse(); - Eigen::Quaterniond quat(global_pose.rotation()); - - ros_landmark_.header.seq++; - ros_landmark_.header.stamp.sec = gnc_.landmark_msg_.cvs_timestamp_sec; - ros_landmark_.header.stamp.nsec = gnc_.landmark_msg_.cvs_timestamp_nsec; - ros_landmark_.camera_id = ml_camera_count_; - - ros_landmark_.pose.position = msg_conversions::eigen_to_ros_point(global_pose.translation()); - ros_landmark_.pose.orientation = msg_conversions::eigen_to_ros_quat(quat); - assert(inlier_landmarks.size() == inlier_observations.size()); - ros_landmark_.landmarks.resize(inlier_landmarks.size()); - for (size_t i = 0; i < inlier_landmarks.size(); i++) { - ff_msgs::VisualLandmark l; - l.x = inlier_landmarks[i].x(); - l.y = inlier_landmarks[i].y(); - l.z = inlier_landmarks[i].z(); - l.u = inlier_observations[i].x(); - l.v = inlier_observations[i].y(); - ros_landmark_.landmarks[i] = l; - } - - last_landmark_time_.sec = gnc_.landmark_msg_.cvs_timestamp_sec; - last_landmark_time_.nsec = gnc_.landmark_msg_.cvs_timestamp_nsec; - return true; -} - -bool Sim::LandmarkEnable(ff_msgs::SetBool::Request &req, ff_msgs::SetBool::Response &res) { - en_landmark_ = req.enable; - res.success = true; - return true; -} - -/* TODO(Jesse, Brian) Implement this for AR tag? */ -bool Sim::PullTagPulseMsg() { - if (!(!last_tag_pulse_ && gnc_.reg_pulse_.cvs_ar_tag_pulse > 0)) { - // early exit condition, not a rising edge - last_tag_pulse_ = gnc_.reg_pulse_.cvs_ar_tag_pulse > 0; - return false; - } - ros_reg_pulse_.camera_id = ++ar_camera_count_; - last_tag_pulse_ = gnc_.reg_pulse_.cvs_ar_tag_pulse > 0; - return true; -} - -/* TODO(Jesse, Brian) Implement this for AR tag? */ -bool Sim::PullTagMsg() { - if (last_tag_time_.sec == gnc_.ar_tag_msg_.cvs_timestamp_sec && - last_tag_time_.nsec == gnc_.ar_tag_msg_.cvs_timestamp_nsec) { - return false; - } - - // Count how many landmarks there are - size_t landmark_count = 0; - for (auto flag : gnc_.ar_tag_msg_.cvs_valid_flag) { - if (flag > 0) { - landmark_count++; - } - } - - std::vector landmarks, inlier_landmarks; - std::vector observations, inlier_observations; - landmarks.resize(landmark_count); - observations.resize(landmark_count); - size_t output_i = 0; - for (size_t i = 0; i < ar_max_features_; i++) { - if (gnc_.ar_tag_msg_.cvs_valid_flag[i] > 0) { - landmarks[output_i].x() = gnc_.ar_tag_msg_.cvs_landmarks[i]; - landmarks[output_i].y() = gnc_.ar_tag_msg_.cvs_landmarks[ar_max_features_ + i]; - landmarks[output_i].z() = gnc_.ar_tag_msg_.cvs_landmarks[2 * ar_max_features_ + i]; - observations[output_i].x() = gnc_.ar_tag_msg_.cvs_observations[i]; - observations[output_i].y() = gnc_.ar_tag_msg_.cvs_observations[ar_max_features_ + i]; - output_i++; - } - } - camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), cam_params_); - if (sparse_mapping::RansacEstimateCamera(landmarks, observations, 1000, 3, - &camera, &inlier_landmarks, &inlier_observations)) - return false; - Eigen::Affine3d global_pose = camera.GetTransform().inverse(); - Eigen::Quaterniond quat(global_pose.rotation()); - - ros_tag_.header.seq++; - ros_tag_.header.stamp.sec = gnc_.ar_tag_msg_.cvs_timestamp_sec; - ros_tag_.header.stamp.nsec = gnc_.ar_tag_msg_.cvs_timestamp_nsec; - ros_tag_.camera_id = ar_camera_count_; - - ros_tag_.pose.position = msg_conversions::eigen_to_ros_point(global_pose.translation()); - ros_tag_.pose.orientation = msg_conversions::eigen_to_ros_quat(quat); - assert(inlier_landmarks.size() == inlier_observations.size()); - ros_tag_.landmarks.resize(landmarks.size()); - for (size_t i = 0; i < landmarks.size(); i++) { - ff_msgs::VisualLandmark l; - l.x = landmarks[i].x(); - l.y = landmarks[i].y(); - l.z = landmarks[i].z(); - l.u = observations[i].x(); - l.v = observations[i].y(); - ros_tag_.landmarks[i] = l; - } - - last_tag_time_.sec = gnc_.ar_tag_msg_.cvs_timestamp_sec; - last_tag_time_.nsec = gnc_.ar_tag_msg_.cvs_timestamp_nsec; - return true; -} - -bool Sim::TagEnable(ff_msgs::SetBool::Request &req, ff_msgs::SetBool::Response &res) { - en_tag_ = req.enable; - res.success = true; - return true; -} - -bool Sim::PullOpticalPulseMsg() { - if (!(!last_optical_pulse_ && gnc_.reg_pulse_.cvs_optical_flow_pulse > 0)) { - // early exit condition, not a rising edge - last_optical_pulse_ = gnc_.reg_pulse_.cvs_optical_flow_pulse > 0; - return false; - } - - ros_reg_pulse_.camera_id = ++of_camera_count_; - - last_optical_pulse_ = gnc_.reg_pulse_.cvs_optical_flow_pulse > 0; - return true; -} - -bool Sim::PullOpticalMsg() { - if (last_optical_time_.sec == gnc_.optical_msg_.cvs_timestamp_sec && - last_optical_time_.nsec == gnc_.optical_msg_.cvs_timestamp_nsec) { - return false; - } - - ros_optical_.header.seq++; - ros_optical_.header.stamp.sec = gnc_.optical_msg_.cvs_timestamp_sec; - ros_optical_.header.stamp.nsec = gnc_.optical_msg_.cvs_timestamp_nsec; - ros_optical_.camera_id = of_camera_count_; - - ros_optical_.feature_array.clear(); - - // send message with all new features from this frame - for (size_t i = 0; i < of_max_features_; i++) { - int index = (of_history_size_ - 1) * of_max_features_; - int valid = gnc_.optical_msg_.cvs_valid_flag[index + i]; - if (!valid) - continue; - float x = gnc_.optical_msg_.cvs_observations[index * 2 + i]; - float y = gnc_.optical_msg_.cvs_observations[index * 2 + i + of_max_features_]; - int id = gnc_.optical_msg_.cvs_id_tag[i]; - ff_msgs::Feature2d feature; - feature.id = id; - feature.x = x; - feature.y = y; - ros_optical_.feature_array.push_back(feature); - } - - last_optical_time_.sec = gnc_.optical_msg_.cvs_timestamp_sec; - last_optical_time_.nsec = gnc_.optical_msg_.cvs_timestamp_nsec; - return true; -} - -bool Sim::OpticalEnable(ff_msgs::SetBool::Request &req, ff_msgs::SetBool::Response &res) { - en_optical_ = req.enable; - res.success = true; - return true; -} - -bool Sim::PullDepthPulseMsg() { - if (!(!last_depth_pulse_ && gnc_.reg_pulse_.cvs_handrail_pulse > 0)) { - // early exit condition, not a rising edge - last_depth_pulse_ = gnc_.reg_pulse_.cvs_handrail_pulse > 0; - return false; - } - - ros_reg_pulse_.camera_id = ++dl_camera_count_; - - last_depth_pulse_ = gnc_.reg_pulse_.cvs_handrail_pulse > 0; - return true; -} - -bool Sim::PullDepthMsg() { - if (last_depth_time_.sec == gnc_.hand_msg_.cvs_timestamp_sec && - last_depth_time_.nsec == gnc_.hand_msg_.cvs_timestamp_nsec) { - return false; - } - - ros_depth_.header.seq++; - ros_depth_.header.stamp.sec = gnc_.hand_msg_.cvs_timestamp_sec; - ros_depth_.header.stamp.nsec = gnc_.hand_msg_.cvs_timestamp_nsec; - ros_depth_.camera_id = dl_camera_count_; - - // Count how many landmarks there are - size_t landmark_count = 0; - for (auto flag : gnc_.hand_msg_.cvs_valid_flag) { - if (flag > 0) { - landmark_count++; - } - } - ros_depth_.landmarks.resize(landmark_count); - size_t output_i = 0; - for (size_t i = 0; i < hr_max_features_; i++) { - if (gnc_.hand_msg_.cvs_valid_flag[i] > 0) { - /* - ros_depth_.landmarks[output_i].x = gnc_.hand_msg_.cvs_landmarks[i]; - ros_depth_.landmarks[output_i].y = gnc_.hand_msg_.cvs_landmarks[hr_max_features_ + i]; - ros_depth_.landmarks[output_i].z = gnc_.hand_msg_.cvs_landmarks[2 * hr_max_features_ + i]; - */ - ros_depth_.landmarks[output_i].u = gnc_.hand_msg_.cvs_observations[i]; - ros_depth_.landmarks[output_i].v = gnc_.hand_msg_.cvs_observations[hr_max_features_ + i]; - ros_depth_.landmarks[output_i].w = gnc_.hand_msg_.cvs_observations[2 * hr_max_features_ + i]; - output_i++; - } - } - ros_depth_.end_seen = gnc_.hand_msg_.cvs_3d_knowledge_flag; - - last_depth_time_.sec = gnc_.hand_msg_.cvs_timestamp_sec; - last_depth_time_.nsec = gnc_.hand_msg_.cvs_timestamp_nsec; - return true; -} - -bool Sim::DepthEnable(ff_msgs::SetBool::Request &req, ff_msgs::SetBool::Response &res) { - en_depth_ = req.enable; - res.success = true; - return true; -} - -} // end namespace sim_wrapper diff --git a/gnc/sim_wrapper/src/sim_nodelet.cc b/gnc/sim_wrapper/src/sim_nodelet.cc deleted file mode 100644 index d74f29088e..0000000000 --- a/gnc/sim_wrapper/src/sim_nodelet.cc +++ /dev/null @@ -1,68 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// Standard includes -#include - -// FSW utils -#include - -// For plugin loading -#include -#include - -// Interface -#include - -// C++ -#include - -/** - * \ingroup gnc - */ -namespace sim_wrapper { - -class SimWrapperNodelet : public ff_util::FreeFlyerNodelet { - public: - // Constructor - SimWrapperNodelet() : ff_util::FreeFlyerNodelet(NODE_SIM_WRAPPER, false) {} - - // Destructor - ~SimWrapperNodelet() {} - - protected: - // Called on initialization - void Initialize(ros::NodeHandle *nh) { - sim_ = std::shared_ptr(new sim_wrapper::Sim(nh)); - timer_ = nh->createTimer(ros::Rate(62.5), - &SimWrapperNodelet::TimerCallback, this, false, true); - } - - // Called when the simulaiton needs to be stepped forward - void TimerCallback(ros::TimerEvent const& event) { - sim_->Step(); - } - - protected: - std::shared_ptr sim_; // simulator interface - ros::Timer timer_; // rate loop timer -}; - -PLUGINLIB_EXPORT_CLASS(sim_wrapper::SimWrapperNodelet, nodelet::Nodelet); - -} // namespace sim_wrapper diff --git a/gnc/sim_wrapper/tools/sim_node.cc b/gnc/sim_wrapper/tools/sim_node.cc deleted file mode 100644 index 98de3a9116..0000000000 --- a/gnc/sim_wrapper/tools/sim_node.cc +++ /dev/null @@ -1,57 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -/** - * SIM node - */ -#include - -#include -#include - -#include - -DEFINE_double(rate, 1.0, "Speed up time by this factor."); - -int main(int argc, char** argv) { - ff_common::InitFreeFlyerApplication(&argc, &argv); - ros::init(argc, argv, "sim_wrapper"); - - ros::NodeHandle nh("~"); - - std::shared_ptr sim(new sim_wrapper::Sim(&nh)); - - double rate = FLAGS_rate; - - // Sim node is the source of the clock when it is being ran. Because of this, - // we can not use the regular Rate objects here. We must use their respective - // "Wall" versions because the normal stuff will hang on waiting for "/clock" - // to be published when we are the publisher. - - // TODO(bcoltin): get IMU rate from autogenerated code - ros::WallRate loop_rate(62.5 * rate); - - while (ros::ok()) { - sim->Step(); - - // process call backs in queue - ros::spinOnce(); - - loop_rate.sleep(); - } -} diff --git a/gnc/sim_wrapper/tools/test_sim.cc b/gnc/sim_wrapper/tools/test_sim.cc deleted file mode 100644 index a551d32ac9..0000000000 --- a/gnc/sim_wrapper/tools/test_sim.cc +++ /dev/null @@ -1,137 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include -#include - -#define COMPARE_INT(a, b) if (a != b) {fprintf(stderr, "Comparison failed at line %d. (%d, %d)\n", __LINE__, \ - (a), (b)); return 1;} -#define COMPARE_INT_VECTOR(a, b, len) for (int unused_variable = 0; unused_variable < len; unused_variable++) \ - if ((a)[unused_variable] != (b)[unused_variable]) {\ - fprintf(stderr, "Comparison failed at line %d vector element %d. (%d, %d)\n", __LINE__, \ - unused_variable, ((a)[unused_variable]), ((b)[unused_variable])); return 1;} -#define COMPARE_FLOAT(a, b, tol) if (fabs((a) - (b)) >= tol) {fprintf(stderr, \ - "Comparison failed at line %d. (%g, %g)\n", __LINE__, (a), (b)); return 1;} -#define COMPARE_FLOAT_VECTOR(a, b, len, tol) for (int unused_variable = 0; unused_variable < len; unused_variable++) \ - if (fabs(((a)[unused_variable]) - ((b)[unused_variable])) >= tol) {\ - fprintf(stderr, "Comparison failed at line %d vector element %d. (%g, %g)\n", __LINE__, \ - unused_variable, ((a)[unused_variable]), ((b)[unused_variable])); return 1;} - -int verify_sim_output(const gnc_autocode::GncSimAutocode & sim, const gnc_autocode::GncSimAutocode & csv) { - float tolerance = 1e-4; - - COMPARE_INT(sim.reg_pulse_.cvs_landmark_pulse, csv.reg_pulse_.cvs_landmark_pulse); - COMPARE_INT(sim.reg_pulse_.cvs_optical_flow_pulse, csv.reg_pulse_.cvs_optical_flow_pulse); - - const cvs_landmark_msg & l1 = sim.landmark_msg_; - const cvs_landmark_msg & l2 = csv.landmark_msg_; - COMPARE_INT_VECTOR(l1.cvs_valid_flag, l2.cvs_valid_flag, 50); - // unfortunately precision errors cause these results to differ - // COMPARE_FLOAT_VECTOR(l1.cvs_observations, l2.cvs_observations, 2 * 50, tolerance); - // COMPARE_FLOAT_VECTOR(l1.cvs_landmarks, l2.cvs_landmarks, 3 * 50, 0.005); - - // compare imu_msgs - const imu_msg & i1 = sim.imu_msg_; - const imu_msg & i2 = csv.imu_msg_; - COMPARE_FLOAT_VECTOR(i1.imu_A_B_ECI_sensor, i2.imu_A_B_ECI_sensor, 3, 1e-2); - COMPARE_FLOAT_VECTOR(i1.imu_accel_bias, i2.imu_accel_bias, 3, tolerance); - COMPARE_FLOAT_VECTOR(i1.imu_omega_B_ECI_sensor, i2.imu_omega_B_ECI_sensor, 3, 2e-4); - COMPARE_FLOAT_VECTOR(i1.imu_gyro_bias, i2.imu_gyro_bias, 3, tolerance); - COMPARE_INT(i1.imu_validity_flag, i2.imu_validity_flag); - COMPARE_INT(i1.imu_sat_flag, i2.imu_sat_flag); - - // compare env_msgs - const env_msg & e1 = sim.env_msg_; - const env_msg & e2 = csv.env_msg_; - COMPARE_FLOAT_VECTOR(e1.P_B_ISS_ISS, e2.P_B_ISS_ISS, 3, 0.001); - COMPARE_FLOAT_VECTOR(e1.V_B_ISS_ISS, e2.V_B_ISS_ISS, 3, 0.0001); - COMPARE_FLOAT_VECTOR(e1.A_B_ISS_ISS, e2.A_B_ISS_ISS, 3, 0.0001); - COMPARE_FLOAT_VECTOR(e1.A_B_ISS_B, e2.A_B_ISS_B, 3, tolerance); - COMPARE_FLOAT_VECTOR(e1.A_B_ECI_B, e2.A_B_ECI_B, 3, 0.002); - COMPARE_FLOAT_VECTOR(e1.Q_ISS2B, e2.Q_ISS2B, 4, 0.05); - COMPARE_FLOAT_VECTOR(e1.omega_B_ISS_B, e2.omega_B_ISS_B, 3, 0.001); - COMPARE_FLOAT_VECTOR(e1.alpha_B_ISS_B, e2.alpha_B_ISS_B, 3, 0.0005); - COMPARE_FLOAT_VECTOR(e1.fan_torques_B, e2.fan_torques_B, 3, 0.001); - COMPARE_FLOAT_VECTOR(e1.fan_forces_B, e2.fan_forces_B, 3, 0.001); - return 0; -} - -int verify_ekf_output(const gnc_autocode::GncEkfAutocode & ekf, const gnc_autocode::GncEkfCSV & csv) { - const kfl_msg & k1 = ekf.kfl_; - const kfl_msg & k2 = csv.kfl_; - COMPARE_FLOAT_VECTOR(k1.quat_ISS2B, k2.quat_ISS2B, 4, 0.001); - COMPARE_FLOAT_VECTOR(k1.omega_B_ISS_B, k2.omega_B_ISS_B, 3, 0.001); - COMPARE_FLOAT_VECTOR(k1.gyro_bias, k2.gyro_bias, 3, 0.001); - COMPARE_FLOAT_VECTOR(k1.V_B_ISS_ISS, k2.V_B_ISS_ISS, 3, 0.07); - COMPARE_FLOAT_VECTOR(k1.A_B_ISS_ISS, k2.A_B_ISS_ISS, 3, 0.07); - COMPARE_FLOAT_VECTOR(k1.accel_bias, k2.accel_bias, 3, 0.003); - COMPARE_FLOAT_VECTOR(k1.P_B_ISS_ISS, k2.P_B_ISS_ISS, 3, 0.005); - COMPARE_INT(k1.confidence, k2.confidence); - COMPARE_INT(k1.aug_state_enum, k2.aug_state_enum); - COMPARE_FLOAT_VECTOR(k1.ml_quat_ISS2cam, k2.ml_quat_ISS2cam, 4, 0.001); - COMPARE_FLOAT_VECTOR(k1.ml_P_cam_ISS_ISS, k2.ml_P_cam_ISS_ISS, 3, 0.005); - COMPARE_FLOAT_VECTOR(k1.of_quat_ISS2cam, k2.of_quat_ISS2cam, 20, 0.001); - COMPARE_FLOAT_VECTOR(k1.of_P_cam_ISS_ISS, k2.of_P_cam_ISS_ISS, 15, 0.005); - COMPARE_FLOAT_VECTOR(k1.cov_diag, k2.cov_diag, 51, 0.005); - COMPARE_INT(k1.kfl_status, k2.kfl_status); - return 0; -} - -int main(int argc, char** argv) { - gnc_autocode::GncSimCSV csv_sim; - gnc_autocode::GncSimAutocode sim; - - gnc_autocode::GncEkfCSV csv_ekf; - gnc_autocode::GncEkfAutocode ekf; - - csv_sim.Initialize(std::string(".")); - csv_ekf.Initialize(std::string(".")); - sim.Initialize(); - ekf.Initialize(); - - while (true) { - csv_sim.Step(); - - // set the act message - memcpy(&sim.act_msg_, &csv_sim.act_msg_, sizeof(act_msg)); - sim.Step(); - - if (verify_sim_output(sim, csv_sim)) { - fprintf(stderr, "Sim failed at time %g.\n", - csv_sim.act_msg_.act_timestamp_sec + static_cast(csv_sim.act_msg_.act_timestamp_nsec) / 1e9); - break; - } - - csv_ekf.Step(); - - // now copy the inputs to the EKF - memcpy(&ekf.vis_, &csv_sim.landmark_msg_, sizeof(cvs_landmark_msg)); - memcpy(&ekf.reg_, &csv_sim.reg_pulse_, sizeof(cvs_registration_pulse)); - memcpy(&ekf.of_, &csv_sim.optical_msg_, sizeof(cvs_optical_flow_msg)); - memcpy(&ekf.imu_, &csv_sim.imu_msg_, sizeof(imu_msg)); - ekf.Step(); - - if (verify_ekf_output(ekf, csv_ekf)) { - fprintf(stderr, "EKF failed at time %g.\n", - csv_sim.act_msg_.act_timestamp_sec + static_cast(csv_sim.act_msg_.act_timestamp_nsec) / 1e9); - break; - } - } -} diff --git a/hardware/CMakeLists.txt b/hardware/CMakeLists.txt index b890443d1d..8c98215e38 100644 --- a/hardware/CMakeLists.txt +++ b/hardware/CMakeLists.txt @@ -22,7 +22,6 @@ add_subdirectory(laser) add_subdirectory(gpio) if (USE_ROS) add_subdirectory(epson_imu) - #add_subdirectory(flycapture) add_subdirectory(is_camera) add_subdirectory(eps_driver) add_subdirectory(pmc_actuator) diff --git a/hardware/eps_driver/CMakeLists.txt b/hardware/eps_driver/CMakeLists.txt index 74baa86642..ad23c586b0 100644 --- a/hardware/eps_driver/CMakeLists.txt +++ b/hardware/eps_driver/CMakeLists.txt @@ -15,9 +15,24 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(eps_driver) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + pluginlib + ff_util + ff_hw_msgs + i2c +) + catkin_package( + INCLUDE_DIRS include LIBRARIES eps_driver CATKIN_DEPENDS @@ -26,41 +41,90 @@ catkin_package( pluginlib ff_util ff_hw_msgs + i2c ) -# ROS agnostic proxy library and tool +########### +## Build ## +########### -create_library( - DIR src/eps_driver - TARGET eps_driver - LIBS i2c +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_tool_targets( - DIR tools/eps_driver_tool - LIBS eps_driver ff_common +# Declare C++ libraries +add_library(eps_driver + src/eps_driver/eps_driver.cc ) +add_dependencies(eps_driver ${catkin_EXPORTED_TARGETS}) +target_link_libraries(eps_driver ${catkin_LIBRARIES}) -# ROS node -create_library( - DIR src/eps_driver_node - TARGET eps_driver_node - LIBS ${catkin_LIBRARIES} eps_driver config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS ff_hw_msgs +add_library(eps_driver_node + src/eps_driver_node/eps_driver_node.cc ) +add_dependencies(eps_driver_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(eps_driver_node eps_driver ${catkin_LIBRARIES}) + +## Declare a C++ executable: eps_driver_tool +add_executable(eps_driver_tool tools/eps_driver_tool/eps_driver_tool.cc) +add_dependencies(eps_driver_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(eps_driver_tool + eps_driver gflags ${catkin_LIBRARIES}) -# Simulator +## Declare a C++ executable: eps_simulator +add_executable(eps_simulator tools/eps_simulator/eps_simulator.cc) +add_dependencies(eps_simulator ${catkin_EXPORTED_TARGETS}) +target_link_libraries(eps_simulator + eps_driver gflags ${catkin_LIBRARIES}) -create_tool_targets( - DIR tools/eps_simulator - LIBS ${catkin_LIBRARIES} eps_driver ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS ff_hw_msgs +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS ${PROJECT_NAME}_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) -install_launch_files() +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Install C++ executables +install(TARGETS eps_driver_tool DESTINATION bin) +install(TARGETS eps_simulator DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/eps_driver_tool share/${PROJECT_NAME} + COMMAND ln -s ../../bin/eps_simulator share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + install(FILES tools/eps_aux.py DESTINATION share/${PROJECT_NAME}) diff --git a/hardware/eps_driver/package.xml b/hardware/eps_driver/package.xml index f1c49b11d1..1a809c5486 100644 --- a/hardware/eps_driver/package.xml +++ b/hardware/eps_driver/package.xml @@ -18,11 +18,13 @@ pluginlib ff_util ff_hw_msgs + i2c roscpp nodelet pluginlib ff_util ff_hw_msgs + i2c diff --git a/hardware/eps_driver/tools/eps_aux.py b/hardware/eps_driver/tools/eps_aux.py index bd1552373b..eeee8d9913 100755 --- a/hardware/eps_driver/tools/eps_aux.py +++ b/hardware/eps_driver/tools/eps_aux.py @@ -1,22 +1,24 @@ #!/usr/bin/python import rospy - from ff_hw_msgs.msg import EpsHousekeeping + def callback(data): - aux = float('nan') - system = float('nan') + aux = float("nan") + system = float("nan") for c in data.values: - if c.name.find("AUX_PWR_I") >= 0 : + if c.name.find("AUX_PWR_I") >= 0: aux = c.value - if c.name.find("SYSTEM_I") >= 0 : + if c.name.find("SYSTEM_I") >= 0: system = c.value - print("system= %.3fA | aux= %.3fA " % (system, aux)) - + print(("system= %.3fA | aux= %.3fA " % (system, aux))) + + def listener(): - rospy.init_node('pmc_consumption', anonymous=True) + rospy.init_node("pmc_consumption", anonymous=True) rospy.Subscriber("/hw/eps/housekeeping", EpsHousekeeping, callback) rospy.spin() - -if __name__ == '__main__': + + +if __name__ == "__main__": listener() diff --git a/hardware/eps_driver/tools/eps_simulator/eps_simulator.cc b/hardware/eps_driver/tools/eps_simulator/eps_simulator.cc index 9ab30b81c3..48ad0de14c 100644 --- a/hardware/eps_driver/tools/eps_simulator/eps_simulator.cc +++ b/hardware/eps_driver/tools/eps_simulator/eps_simulator.cc @@ -20,7 +20,7 @@ #include // FSW standard naming -#include +#include // Standard messages #include diff --git a/hardware/epson_imu/CMakeLists.txt b/hardware/epson_imu/CMakeLists.txt index 65c492fb90..635c1137e4 100644 --- a/hardware/epson_imu/CMakeLists.txt +++ b/hardware/epson_imu/CMakeLists.txt @@ -15,19 +15,72 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(epson_imu) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + rospy + nodelet + sensor_msgs + ff_util +) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused-result") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-result") catkin_package( LIBRARIES epson_imu - CATKIN_DEPENDS roscpp rospy sensor_msgs + CATKIN_DEPENDS roscpp rospy nodelet sensor_msgs ff_util +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(epson_imu + src/epson_imu_nodelet.cc + src/G362P.cc + src/GPIO.cc +) +add_dependencies(epson_imu ${catkin_EXPORTED_TARGETS}) +target_link_libraries(epson_imu ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -create_library(TARGET epson_imu - LIBS ${catkin_LIBRARIES} ff_nodelet config_reader - INC ${catkin_INCLUDE_DIRS} +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE ) -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/hardware/epson_imu/package.xml b/hardware/epson_imu/package.xml index 2c6eab0869..9e9928e77e 100644 --- a/hardware/epson_imu/package.xml +++ b/hardware/epson_imu/package.xml @@ -15,12 +15,14 @@ catkin roscpp rospy - sensor_msgs nodelet + sensor_msgs + ff_util roscpp rospy - sensor_msgs nodelet + sensor_msgs + ff_util diff --git a/hardware/fam_cmd_i2c/CMakeLists.txt b/hardware/fam_cmd_i2c/CMakeLists.txt index 60884dc8ad..0cbdf2941c 100644 --- a/hardware/fam_cmd_i2c/CMakeLists.txt +++ b/hardware/fam_cmd_i2c/CMakeLists.txt @@ -15,16 +15,64 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(fam_cmd_i2c) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + rospy + ff_msgs + std_msgs + i2c +) + catkin_package( - CATKIN_DEPENDS roscpp rospy ff_msgs std_msgs + CATKIN_DEPENDS roscpp rospy ff_msgs std_msgs i2c ) -create_tool_targets(DIR tools - LIBS ${catkin_LIBRARIES} i2c - INC ${catkin_INCLUDE_DIRS} - DEPS ff_msgs std_msgs +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -install_launch_files() +## Declare a C++ executable: inspection_tool +add_executable(fam_cmd_gen tools/fam_cmd_gen.cc) +add_dependencies(fam_cmd_gen ${catkin_EXPORTED_TARGETS}) +target_link_libraries(fam_cmd_gen + ${catkin_LIBRARIES}) + +## Declare a C++ executable: inspection_tool +add_executable(fam_cmd_i2c_node tools/fam_cmd_i2c_node.cc) +add_dependencies(fam_cmd_i2c_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(fam_cmd_i2c_node + ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Install C++ executables +install(TARGETS fam_cmd_gen DESTINATION bin) +install(TARGETS fam_cmd_i2c_node DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/fam_cmd_gen share/${PROJECT_NAME} + COMMAND ln -s ../../bin/fam_cmd_i2c_node share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + diff --git a/hardware/fam_cmd_i2c/package.xml b/hardware/fam_cmd_i2c/package.xml index c9c89e41e3..ac3ebc363a 100644 --- a/hardware/fam_cmd_i2c/package.xml +++ b/hardware/fam_cmd_i2c/package.xml @@ -18,9 +18,11 @@ rospy ff_msgs std_msgs + i2c roscpp rospy ff_msgs std_msgs + i2c diff --git a/hardware/ff_serial/CMakeLists.txt b/hardware/ff_serial/CMakeLists.txt index 58d30a1be4..87355d7a07 100644 --- a/hardware/ff_serial/CMakeLists.txt +++ b/hardware/ff_serial/CMakeLists.txt @@ -15,10 +15,58 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(ff_serial) -create_library( - TARGET ff_serial - DIR src - LIBS ${Boost_THREAD_LIBRARY} ${Boost_SYSTEM_LIBRARY} +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS +) + +catkin_package( + LIBRARIES ff_serial + INCLUDE_DIRS include +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(ff_serial + src/serial.cc +) +add_dependencies(ff_serial ${catkin_EXPORTED_TARGETS}) +target_link_libraries(ff_serial ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE ) diff --git a/hardware/flashlight/CMakeLists.txt b/hardware/flashlight/CMakeLists.txt index 2b70e3bedf..868a45dcd2 100644 --- a/hardware/flashlight/CMakeLists.txt +++ b/hardware/flashlight/CMakeLists.txt @@ -15,34 +15,104 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(flashlight) -# Core library -create_library(TARGET flashlight - INC ${GLOG_INCLUDE_DIRS} - LIBS ${GLOG_LIBRARIES} i2c +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + roslib + nodelet + ff_hw_msgs + i2c + ff_util + config_reader +) + +catkin_package( + LIBRARIES + flashlight + CATKIN_DEPENDS + roscpp + roslib + nodelet + ff_hw_msgs + i2c + ff_util + config_reader ) -# Standalone test programs -create_tool_targets(DIR tools - INC ${GFLAGS_INCLUDE_DIRS} - LIBS ${GFLAGS_LIBRARIES} flashlight ff_common +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -# Create ROS nodes if we are using ROS -if(USE_ROS) - catkin_package( - LIBRARIES flashlight - CATKIN_DEPENDS roscpp roslib nodelet - DEPENDS ff_hw_msgs + # Declare C++ librarie - Core library + add_library(flashlight + src/flashlight.cc ) + add_dependencies(flashlight ${catkin_EXPORTED_TARGETS}) + target_link_libraries(flashlight glog ${catkin_LIBRARIES}) - create_library(TARGET flashlight_nodelet - DIR src/ros - LIBS ${roscpp_LIBRARIES} ${nodelet_LIBRARIES} flashlight config_reader - ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS ff_hw_msgs + # Declare C++ librarie + add_library(flashlight_nodelet + src/ros/flashlight_nodelet.cc ) - install_launch_files() -endif(USE_ROS) + add_dependencies(flashlight_nodelet ${catkin_EXPORTED_TARGETS}) + target_link_libraries(flashlight_nodelet flashlight ${catkin_LIBRARIES}) + +## Declare a C++ executable: flashlight_test +add_executable(flashlight_test tools/flashlight_test.cc) +add_dependencies(flashlight_test ${catkin_EXPORTED_TARGETS}) +target_link_libraries(flashlight_test + flashlight gflags ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS ${PROJECT_NAME}_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Install C++ executables +install(TARGETS flashlight_test DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/flashlight_test share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/hardware/flashlight/include/flashlight/ros/flashlight_nodelet.h b/hardware/flashlight/include/flashlight/ros/flashlight_nodelet.h index 398aef61c8..98f401cdf8 100644 --- a/hardware/flashlight/include/flashlight/ros/flashlight_nodelet.h +++ b/hardware/flashlight/include/flashlight/ros/flashlight_nodelet.h @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include diff --git a/hardware/flashlight/package.xml b/hardware/flashlight/package.xml index 035c062fcc..d2756af322 100644 --- a/hardware/flashlight/package.xml +++ b/hardware/flashlight/package.xml @@ -18,11 +18,16 @@ roslib nodelet ff_hw_msgs + i2c + ff_util + config_reader roscpp + roslib nodelet ff_hw_msgs - roslib - message_runtime + i2c + ff_util + config_reader diff --git a/hardware/flashlight/test_flashlight.md b/hardware/flashlight/test_flashlight.md index 2dc58a89fc..bb9fe52b49 100644 --- a/hardware/flashlight/test_flashlight.md +++ b/hardware/flashlight/test_flashlight.md @@ -21,23 +21,23 @@ If running on P4C, please change the dev flag from /dev/i2c-1 to /dev/i2c-0 There are two ways to set the flashlight brightness in the fsw. The fsw is set up such that setting brightness to 0 turns the flashlight off and setting the brightness anywhere from 1 to 200 turns the flashlight on. The first way is by calling the set flashlight service from the command line. To turn the front flashlight on, run: - rosservice call /hw/light_front/control 100 + rosservice call /hw/flashlight_front/control 100 To turn the front flashlight off, run: - rosservice call /hw/light_front/control 0 + rosservice call /hw/flashlight_front/control 0 To turn the back flashlight on, run: - rosservice call /hw/light_aft/control 100 + rosservice call /hw/flashlight_aft/control 100 To turn the back flashlight off, run: - rosservice call /hw/light_aft/control 0 + rosservice call /hw/flashlight_aft/control 0 The second way is to send a set flashlight brightness command to the executive. You can either do this by using the teleop command tab in GDS or publishing the command to the /command topic from the command line. To turn the front flashlight on, run: - rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "setFlashlightBrightness", subsys_name: "Astrobee", args: [{data_type: 5, s: Front}, {data_type: 2, f: 100}]}' + rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "setFlashlightBrightness", subsys_name: "Astrobee", args: [{data_type: 5, s: Front}, {data_type: 2, f: 1}]}' To turn the front flashlight off, run: @@ -45,7 +45,7 @@ To turn the front flashlight off, run: To turn the back flashlight on, run: - rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "setFlashlightBrightness", subsys_name: "Astrobee", args: [{data_type: 5, s: Back}, {data_type: 2, f: 100}]}' + rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "setFlashlightBrightness", subsys_name: "Astrobee", args: [{data_type: 5, s: Back}, {data_type: 2, f: 1}]}' To turn the back flashlight off, run: diff --git a/hardware/flycapture/CMakeLists.txt b/hardware/flycapture/CMakeLists.txt deleted file mode 100644 index 71b7762520..0000000000 --- a/hardware/flycapture/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -project(flycapture) - -# Need to think of a better way to get flycapture -find_path(FLYCAPTURE_INCLUDE_DIR flycapture/FlyCapture2Defs.h - HINTS ${FLYCAPTURE_ROOT_DIR}/include /usr) -find_library(FLYCAPTURE_LIBRARY NAMES flycapture libflycapture - HINTS /usr/lib ${FLYCAPTURE_ROOT_DIR}/lib) -find_package_handle_standard_args(FLYCAPTURE DEFAULT_MSG - FLYCAPTURE_INCLUDE_DIR FLYCAPTURE_LIBRARY) - -catkin_package( - LIBRARIES flycapture - CATKIN_DEPENDS roscpp cv_bridge image_transport -) - - -if (FLYCAPTURE_FOUND) - create_tool_targets(DIR tools - LIBS ${FLYCAPTURE_LIBRARY} ${catkin_LIBRARIES} ff_nodelet - INC ${FLYCAPTURE_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} - ) -else() - message(STATUS "Not building fly_node") -endif() diff --git a/hardware/flycapture/package.xml b/hardware/flycapture/package.xml deleted file mode 100644 index 6711657350..0000000000 --- a/hardware/flycapture/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - flycapture - 0.0.0 - - The flycapture package - - - Apache License, Version 2.0 - - - Astrobee Flight Software - - - Astrobee Flight Software - - catkin - cv_bridge - image_transport - roscpp - cv_bridge - image_transport - roscpp - - diff --git a/hardware/flycapture/tools/fly_node.cc b/hardware/flycapture/tools/fly_node.cc deleted file mode 100644 index 390aab9500..0000000000 --- a/hardware/flycapture/tools/fly_node.cc +++ /dev/null @@ -1,177 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -std::string DecoderRing(int code) { - switch (code) { - case FlyCapture2::VIDEOMODE_160x120YUV444: - return "VIDEOMODE_160x120YUV444"; - case FlyCapture2::VIDEOMODE_320x240YUV422: - return "VIDEOMODE_320x240YUV422"; - case FlyCapture2::VIDEOMODE_640x480YUV411: - return "VIDEOMODE_640x480YUV411"; - case FlyCapture2::VIDEOMODE_640x480YUV422: - return "VIDEOMODE_640x480YUV422"; - case FlyCapture2::VIDEOMODE_640x480RGB: - return "VIDEOMODE_640x480RGB"; - case FlyCapture2::VIDEOMODE_640x480Y8: - return "VIDEOMODE_640x480Y8"; - case FlyCapture2::VIDEOMODE_640x480Y16: - return "VIDEOMODE_640x480Y16"; - case FlyCapture2::VIDEOMODE_800x600YUV422: - return "VIDEOMODE_800x600YUV422"; - case FlyCapture2::VIDEOMODE_800x600RGB: - return "VIDEOMODE_800x600RGB"; - case FlyCapture2::VIDEOMODE_800x600Y8: - return "VIDEOMODE_800x600Y8"; - case FlyCapture2::VIDEOMODE_800x600Y16: - return "VIDEOMODE_800x600Y16"; - case FlyCapture2::VIDEOMODE_1024x768YUV422: - return "VIDEOMODE_1024x768YUV422"; - case FlyCapture2::VIDEOMODE_1024x768RGB: - return "VIDEOMODE_1024x768RGB"; - case FlyCapture2::VIDEOMODE_1024x768Y8: - return "VIDEOMODE_1024x768Y8"; - case FlyCapture2::VIDEOMODE_1024x768Y16: - return "VIDEOMODE_1024x768Y16"; - case FlyCapture2::VIDEOMODE_1280x960YUV422: - return "VIDEOMODE_1280x960YUV422"; - case FlyCapture2::VIDEOMODE_1280x960RGB: - return "VIDEOMODE_1280x960RGB"; - case FlyCapture2::VIDEOMODE_1280x960Y8: - return "VIDEOMODE_1280x960Y8"; - case FlyCapture2::VIDEOMODE_1280x960Y16: - return "VIDEOMODE_1280x960Y16"; - case FlyCapture2::VIDEOMODE_1600x1200YUV422: - return "VIDEOMODE_1600x1200YUV422"; - case FlyCapture2::VIDEOMODE_1600x1200RGB: - return "VIDEOMODE_1600x1200RGB"; - case FlyCapture2::VIDEOMODE_1600x1200Y8: - return "VIDEOMODE_1600x1200Y8"; - case FlyCapture2::VIDEOMODE_1600x1200Y16: - return "VIDEOMODE_1600x1200Y16"; - case FlyCapture2::VIDEOMODE_FORMAT7: - return "VIDEOMODE_FORMAT7"; - case FlyCapture2::NUM_VIDEOMODES: - return "NUM_VIDEOMODES"; - case FlyCapture2::VIDEOMODE_FORCE_32BITS: - return "VIDEOMODE_FORCE_32BITS"; - default: - return "UNKNOWN"; - } -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "fly_node"); - ros::NodeHandle node_handle; - - // Let's find the point gray camera - FlyCapture2::BusManager bus; - if (bus.RescanBus() != FlyCapture2::PGRERROR_OK) { - ROS_FATAL("Unable to scan flycapture bus."); - ros::shutdown(); - } - - unsigned int number_of_cameras = 0; - bus.GetNumOfCameras(&number_of_cameras); - - // Let's get the point gray camera - FlyCapture2::PGRGuid guid; - bus.GetCameraFromIndex(0, &guid); - FlyCapture2::Camera* camera = new FlyCapture2::Camera(); - if (camera->Connect(&guid) != FlyCapture2::PGRERROR_OK) { - ROS_FATAL("Unable to connect to overhead camera."); - ros::shutdown(); - } - - // Set Resolution - FlyCapture2::Format7Info info; - bool supported; - if (camera->GetFormat7Info(&info, &supported) != - FlyCapture2::PGRERROR_OK) { - ROS_FATAL("Unable to FlyCapture2::GetFormat7Info"); - ros::shutdown(); - } - camera->StopCapture(); - FlyCapture2::Format7ImageSettings settings; - settings.width = 1280; - settings.height = 960; - settings.mode = FlyCapture2::MODE_0; - settings.offsetX = (info.maxWidth - 1280)/2; - settings.offsetY = (info.maxHeight - 960)/2; - settings.pixelFormat = FlyCapture2::PIXEL_FORMAT_MONO8; - bool valid; - FlyCapture2::Format7PacketInfo pinfo; - if (camera->ValidateFormat7Settings(&settings, &valid, &pinfo) - != FlyCapture2::PGRERROR_OK) { - ROS_FATAL("Unable to ValidateFormat7Settings"); - ros::shutdown(); - } - if (!valid) { - ROS_FATAL("Invalid requested camera image format"); - ros::shutdown(); - } - unsigned int packet_size = pinfo.maxBytesPerPacket; - if (camera->SetFormat7Configuration(&settings, packet_size) != - FlyCapture2::PGRERROR_OK) { - ROS_FATAL("Unable to SetFormat7Configuration"); - ros::shutdown(); - } - - // Create a return frame - IplImage* frame = cvCreateImage(cvSize(1280, 960), IPL_DEPTH_8U, 1); - camera->StartCapture(); - - // Let's create our image transport - image_transport::ImageTransport it(node_handle); - image_transport::Publisher pub = it.advertise(TOPIC_LOCALIZATION_OVERHEAD_IMAGE, 1); - - // Lets capture data - FlyCapture2::Image image; - ros::Rate loop_rate(30); - uint32_t seq = 0; - while (node_handle.ok()) { - camera->RetrieveBuffer(&image); - int64 length = 1280 * 960; - memcpy(frame->imageData, image.GetData(), length); - - // I have a frame! - sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), - "mono8", cv::cvarrToMat(frame)).toImageMsg(); - msg->header.stamp = ros::Time::now(); - msg->header.seq = seq++; - pub.publish(msg); - - // Check thyself before thou wrecks thyself. - ros::spinOnce(); - loop_rate.sleep(); - } - delete camera; - - return 0; -} diff --git a/hardware/gpio/CMakeLists.txt b/hardware/gpio/CMakeLists.txt index 7fb5761e9c..78cc53f436 100644 --- a/hardware/gpio/CMakeLists.txt +++ b/hardware/gpio/CMakeLists.txt @@ -15,4 +15,44 @@ # License for the specific language governing permissions and limitations # under the License. -create_library(TARGET gpio) +cmake_minimum_required(VERSION 3.0) +project(gpio) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS +) + +catkin_package( + LIBRARIES + gpio +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(gpio + src/gpio.cc +) +add_dependencies(gpio ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gpio ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) \ No newline at end of file diff --git a/hardware/i2c/CMakeLists.txt b/hardware/i2c/CMakeLists.txt index 3f73f4b024..1ba70cf931 100644 --- a/hardware/i2c/CMakeLists.txt +++ b/hardware/i2c/CMakeLists.txt @@ -15,8 +15,52 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(i2c) -create_library(TARGET i2c - LIBS ${GLOG_LIBRARIES} - INC ${GLOG_INCLUDE_DIRS}) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS +) + +catkin_package( + LIBRARIES i2c + INCLUDE_DIRS include +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(i2c + src/i2c.cc + src/i2c_new.cc +) +add_dependencies(i2c ${catkin_EXPORTED_TARGETS}) +target_link_libraries(i2c glog ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/hardware/i2c/package.xml b/hardware/i2c/package.xml index 0dcc2828d7..00136cafd4 100644 --- a/hardware/i2c/package.xml +++ b/hardware/i2c/package.xml @@ -15,7 +15,5 @@ Astrobee Flight Software catkin - roscpp - roscpp diff --git a/hardware/is_camera/CMakeLists.txt b/hardware/is_camera/CMakeLists.txt index 32b6d3c1c9..d12782f93d 100644 --- a/hardware/is_camera/CMakeLists.txt +++ b/hardware/is_camera/CMakeLists.txt @@ -15,8 +15,22 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(is_camera) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + image_transport + nodelet + ff_util + config_reader + camera +) + # Need to find libv4l2 find_path(V4L2_INCLUDE_DIR libv4l2.h linux/videodev2.h HINTS /usr ${V4L2_ROOT_DIR}) @@ -25,22 +39,70 @@ find_library(V4L2_LIBRARY NAMES libv4l2 v4l2 find_package_handle_standard_args(V4L2 DEFAULT_MSG V4L2_INCLUDE_DIR V4L2_LIBRARY) + +# Find OpenCV +find_package(OpenCV 4.0 REQUIRED) + catkin_package( LIBRARIES is_camera - CATKIN_DEPENDS roscpp image_transport nodelet + CATKIN_DEPENDS + roscpp + image_transport + nodelet + ff_util + config_reader + camera ) -create_library(TARGET is_camera - LIBS ${catkin_LIBRARIES} ${V4L2_LIBRARY} config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} ${V4L2_INCLUDE_DIR} - DEPS ff_msgs) +########### +## Build ## +########### -create_tool_targets(DIR tools - LIBS is_camera - ) +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(is_camera + src/camera.cc +) +add_dependencies(is_camera ${catkin_EXPORTED_TARGETS}) +target_link_libraries(is_camera ${V4L2_LIBRARY} ${catkin_LIBRARIES}) + +# Declare C++ libraries +add_library(debayer + src/debayer.cc +) +add_dependencies(debayer ${catkin_EXPORTED_TARGETS}) +target_link_libraries(debayer ${V4L2_LIBRARY} ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark libraries for installation +install(TARGETS debayer + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) -# I'm not sure who installed package.xml for us, but we should do the same for -# nodelet_plugins.xml -install(FILES nodelet_plugins.xml DESTINATION share/is_camera) +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/hardware/is_camera/include/is_camera/camera.h b/hardware/is_camera/include/is_camera/camera.h index e61331d10e..b65d4b589f 100644 --- a/hardware/is_camera/include/is_camera/camera.h +++ b/hardware/is_camera/include/is_camera/camera.h @@ -24,7 +24,9 @@ #include #include #include +#include #include +#include #include #include @@ -46,18 +48,22 @@ struct V4LStruct; // Nodelet class class CameraNodelet : public ff_util::FreeFlyerNodelet { public: - static constexpr size_t kImageMsgBuffer = 15; // 17.6 Mb of buffer space. This - // number is so large because - // this sets the life time for - // each image message sent - // out. Eventually we'll write - // over the pointer we've - // handed out. 15 frames means - // .. an image will stay valid - // for 1.0 seconds. - // Localization can process at - // 2 Hz, so it only needs an - // image for 0.5 seconds. + // The size of the image message ring buffer for publishing + // grayscale images. 30 images = 35.2 MB of buffer space. This + // number is so large because it sets the lifetime for each image + // message sent out. Eventually we'll write over the pointer we've + // handed out. At 15 Hz, 30 frames means an image will stay valid + // for 2.0 seconds. Localization can process at 1 Hz, so it only + // needs an image for 1.0 second. + static constexpr size_t kImageMsgBuffer = 30; + + // The size of the image message ring buffer for publishing raw + // Bayer format images. The same comments apply about message + // lifetime, but in this case we're publishing at a lower rate and + // expecting the subscriber callback to at most de-Bayer the image + // before passing it on, so the buffer doesn't need to be so long. + static constexpr size_t kBayerImageMsgBufferLength = 5; + static constexpr size_t kImageWidth = 1280; static constexpr size_t kImageHeight = 960; @@ -70,23 +76,52 @@ class CameraNodelet : public ff_util::FreeFlyerNodelet { private: void PublishLoop(); - bool EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res); // NOLINT + void LoadCameraInfo(); + void EnableBayer(bool enable); + void AutoExposure(); + size_t getNumBayerSubscribers(); + bool SetExposure(ff_msgs::SetExposure::Request &req, ff_msgs::SetExposure::Response &res); + + sensor_msgs::CameraInfo info_msg_; + ros::NodeHandle *nh_; sensor_msgs::ImagePtr img_msg_buffer_[kImageMsgBuffer]; + sensor_msgs::ImagePtr bayer_img_msg_buffer_[kBayerImageMsgBufferLength]; size_t img_msg_buffer_idx_; + size_t bayer_img_msg_buffer_idx_; std::thread thread_; std::atomic thread_running_; ros::Publisher pub_; + ros::Publisher info_pub_; + ros::Publisher bayer_pub_; + ros::Publisher pub_exposure_; + ros::ServiceServer srv_exposure_; std::shared_ptr v4l_; config_reader::ConfigReader config_; ros::Timer config_timer_; - std::string output_topic_; + ros::Timer auto_exposure_timer_; std::string camera_device_; std::string camera_topic_; + std::string bayer_camera_topic_; std::string config_name_; - int camera_gain_, camera_exposure_; + int camera_gain_, camera_exposure_, camera_auto_exposure_; bool calibration_mode_; + + // bayer_enable: Set to true to enable publishing raw Bayer image + // (can be converted to RGB). May incur significant I/O overhead. + bool bayer_enable_; + + // bayer_throttle_ratio: Set to n to publish 1 raw Bayer image for + // every n images grabbed. With n = 1, every image is + // published. Larger n reduces I/O overhead. + unsigned int bayer_throttle_ratio_; + size_t bayer_throttle_ratio_counter_; + + // Auto exposure parameters + bool auto_exposure_; + int hist_size_; + double desired_msv_, k_p_, k_i_, max_i_, err_p_, err_i_, tolerance_; }; } // end namespace is_camera diff --git a/hardware/is_camera/nodelet_plugins.xml b/hardware/is_camera/nodelet_plugins.xml index e47347fb18..755139e4ec 100644 --- a/hardware/is_camera/nodelet_plugins.xml +++ b/hardware/is_camera/nodelet_plugins.xml @@ -1,9 +1,16 @@ - - - - This nodelet opens the camera that is /dev/video? and publishes a - grayscale unrectified image. - - + + + + + This nodelet opens the camera that is /dev/video? and publishes a + grayscale unrectified image. + + + + + Nodelet for debayer::DebayerNodelet + + + diff --git a/hardware/is_camera/package.xml b/hardware/is_camera/package.xml index ee9bbd1cf2..4e03a2b541 100644 --- a/hardware/is_camera/package.xml +++ b/hardware/is_camera/package.xml @@ -15,12 +15,18 @@ Astrobee Flight Software catkin + image_transport roscpp nodelet - image_transport + ff_util + config_reader + camera roscpp - nodelet image_transport + nodelet + ff_util + config_reader + camera diff --git a/hardware/is_camera/src/camera.cc b/hardware/is_camera/src/camera.cc index 1882467938..46f4071cad 100644 --- a/hardware/is_camera/src/camera.cc +++ b/hardware/is_camera/src/camera.cc @@ -20,6 +20,7 @@ // SUBSYSTEM=="video4linux", ATTRS{serial}=="02510265",SYMLINK+="nav_cam" #include +#include #include #include @@ -162,7 +163,17 @@ namespace is_camera { }; CameraNodelet::CameraNodelet() : ff_util::FreeFlyerNodelet(), - img_msg_buffer_idx_(0), thread_running_(false), camera_topic_(""), calibration_mode_(false) + img_msg_buffer_idx_(0), + bayer_img_msg_buffer_idx_(0), + thread_running_(false), + camera_topic_(""), + bayer_camera_topic_(""), + calibration_mode_(false), + bayer_throttle_ratio_counter_(0), + bayer_enable_(false), + auto_exposure_(false), + err_p_(0), + err_i_(0) {} CameraNodelet::~CameraNodelet() { @@ -172,6 +183,8 @@ namespace is_camera { } void CameraNodelet::Initialize(ros::NodeHandle* nh) { + // Store the node handle for future use + nh_ = nh; calibration_mode_ = false; config_name_ = GetName(); if (GetName() == "nav_cam") { @@ -194,8 +207,28 @@ namespace is_camera { ReadParams(); config_timer_ = GetPrivateHandle()->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { config_.CheckFilesUpdated(std::bind(&CameraNodelet::ReadParams, this));}, false, true); + auto_exposure_timer_ = GetPrivateHandle()->createTimer( + ros::Duration(1), + [this](ros::TimerEvent e) { + // Publish exposure data + std_msgs::Int32MultiArray msg; + msg.data.push_back(camera_gain_); + if (auto_exposure_) { + AutoExposure(); + msg.data.push_back(camera_auto_exposure_); + } else { + msg.data.push_back(camera_exposure_); + } + pub_exposure_.publish(msg); + } + , + false, true); pub_ = nh->advertise(camera_topic_, 1); + info_pub_ = nh->advertise(camera_topic_ + TOPIC_HARDWARE_CAM_INFO, 1); + pub_exposure_ = nh->advertise(camera_topic_ + "_ctrl", 1); + + srv_exposure_ = nh->advertiseService(camera_topic_ + SERVICE_SET_EXPOSURE, &CameraNodelet::SetExposure, this); // Allocate space for our output msg buffer for (size_t i = 0; i < kImageMsgBuffer; i++) { @@ -212,6 +245,34 @@ namespace is_camera { thread_ = std::thread(&CameraNodelet::PublishLoop, this); } + // Set the exposure + bool CameraNodelet::SetExposure(ff_msgs::SetExposure::Request &req, + ff_msgs::SetExposure::Response &res) { + if (thread_running_ && !auto_exposure_) { + // Set exposure + camera_exposure_ = req.exposure; + v4l_->SetParameters(camera_gain_, camera_exposure_); + // Success! + res.success = true; + res.status_message = "Success"; + } else { + // Failed + res.success = false; + res.status_message = "Failed! Either thread not running (" + std::to_string(thread_running_) + + ") or auto-exposure on (" + std::to_string(auto_exposure_) + ")"; + } + + return true; + } + + size_t CameraNodelet::getNumBayerSubscribers(void) { + if (bayer_enable_) { + return bayer_pub_.getNumSubscribers(); + } else { + return 0; + } + } + void CameraNodelet::ReadParams(void) { if (!config_.ReadFiles()) { ROS_ERROR("Failed to read config files."); @@ -234,7 +295,7 @@ namespace is_camera { } if (!camera.GetInt("exposure", &camera_exposure_)) { - FF_FATAL("Gain not specified."); + FF_FATAL("Exposure not specified."); exit(EXIT_FAILURE); } @@ -251,17 +312,168 @@ namespace is_camera { } } - if (thread_running_) { + bool bayer_enable; + if (!camera.GetBool("bayer_enable", &bayer_enable)) { + FF_FATAL("Bayer enable not specified."); + exit(EXIT_FAILURE); + } + // Check if bayer configuration changed + EnableBayer(bayer_enable); + + LoadCameraInfo(); + + // Auto Exposure Parameters + if (!camera.GetBool("auto_exposure", &auto_exposure_)) { + FF_FATAL("Auto Exposure enable not specified."); + exit(EXIT_FAILURE); + } + + if (!config_.GetReal("desired_msv", &desired_msv_)) { + FF_FATAL("Auto-Exposure: Desired MSV not specified."); + exit(EXIT_FAILURE); + } + if (!config_.GetReal("k_p", &k_p_)) { + FF_FATAL("Auto-Exposure: Kp not specified."); + exit(EXIT_FAILURE); + } + if (!config_.GetReal("k_i", &k_i_)) { + FF_FATAL("Auto-Exposure: Ki not specified."); + exit(EXIT_FAILURE); + } + if (!config_.GetReal("max_i", &max_i_)) { + FF_FATAL("Auto-Exposure: Maximum I not specified."); + exit(EXIT_FAILURE); + } + if (!config_.GetReal("tolerance", &tolerance_)) { + FF_FATAL("Auto-Exposure: Error tolerance not specified."); + exit(EXIT_FAILURE); + } + + if (!camera.GetUInt("bayer_throttle_ratio", &bayer_throttle_ratio_)) { + FF_FATAL("Bayer throttle ratio not specified."); + exit(EXIT_FAILURE); + } + + if (thread_running_ && !auto_exposure_) { v4l_->SetParameters(camera_gain_, camera_exposure_); } } + void CameraNodelet::LoadCameraInfo() { + camera::CameraParameters cam_params(&config_, config_name_.c_str()); + + // Load parameters + Eigen::Matrix3d K = cam_params.GetIntrinsicMatrix(); + Eigen::VectorXd D = cam_params.GetDistortion(); + Eigen::Vector2i size = cam_params.GetDistortedSize(); + + // Populate message + info_msg_.width = size[0]; + info_msg_.height = size[1]; + info_msg_.K = {K(0, 0), K(0, 1), K(0, 2), + K(1, 0), K(1, 1), K(1, 2), + K(2, 0), K(2, 1), K(2, 2)}; + if (D.size() == 1) { + info_msg_.distortion_model = "fov"; + info_msg_.D = {D[0]}; + } else if (D.size() == 4 || D.size() == 5) { + info_msg_.distortion_model = "plumb_bob"; + info_msg_.D = {D[0], D[1], D[2], D[3], 0}; + if (D.size() > 4) { + info_msg_.D[4] = D[4]; + } + } + + // Don't use projection or rotation, but populate + info_msg_.P = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0}; + info_msg_.R = {1, 0, 0, + 0, 1, 0, + 0, 0, 1}; + } + + void CameraNodelet::EnableBayer(bool enable) { + if (enable && !bayer_enable_) { + bayer_camera_topic_ = camera_topic_ + TOPIC_HARDWARE_CAM_SUFFIX_BAYER_RAW; + bayer_pub_ = nh_->advertise(bayer_camera_topic_, 1); + // Allocate space for our Bayer output msg buffer + for (size_t i = 0; i < kBayerImageMsgBufferLength; i++) { + // Ignore if already initialized once + if (bayer_img_msg_buffer_[i] != NULL) continue; + bayer_img_msg_buffer_[i].reset(new sensor_msgs::Image()); + bayer_img_msg_buffer_[i]->width = kImageWidth; + bayer_img_msg_buffer_[i]->height = kImageHeight; + + // This was tested in the lab using a color test picture + // Images in https://github.com/nasa/astrobee/issues/434 + bayer_img_msg_buffer_[i]->encoding = "bayer_grbg8"; + bayer_img_msg_buffer_[i]->step = kImageWidth; + bayer_img_msg_buffer_[i]->data.resize(kImageWidth * kImageHeight); + } + } else if (!enable && bayer_enable_) { + bayer_pub_.shutdown(); + } else { + return; + } + bayer_enable_ = enable; + } + + // Timer that periodically changes camera exposure based on the captured image's histogram + void CameraNodelet::AutoExposure() { + // If images not being captured, do not adjust exposure + if (!thread_running_ || ((pub_.getNumSubscribers() == 0) && (getNumBayerSubscribers() == 0))) return; + + // Get last generated image from buffer (index incremented after image completed) + sensor_msgs::ImagePtr& grey_image = img_msg_buffer_[(img_msg_buffer_idx_ + kImageMsgBuffer - 1) % kImageMsgBuffer]; + + cv::Mat input(kImageHeight, kImageWidth, + cv::DataType::type, + &(grey_image->data[0]), + kImageWidth); + + // Calculate Histogram + cv::Mat hist; + int hist_size = 5; + float range[] = {0, 256}; // the upper boundary is exclusive + const float* hist_range[] = { range }; + bool uniform = true, accumulate = false; + cv::calcHist(&input, 1, 0, cv::Mat(), hist, 1, &hist_size, hist_range, uniform, accumulate); + + // Calculate mean sample value + double mean_sample_value = 0; + + for (int i = 0; i < hist_size; ++i) { + mean_sample_value += hist.at(i) * (i+1); + } + mean_sample_value /= (kImageHeight * kImageWidth); + + // Control Auto exposure with a PI controller + err_p_ = desired_msv_ - mean_sample_value; + + // Don't change exposure if we're close enough. Changing too often slows + // down the data rate of the camera. + if (std::abs(err_p_) > tolerance_) { + // Calculate PI coefficients + err_i_ += err_p_; + if (std::abs(err_i_) > max_i_) { + err_i_ = (err_i_ > 0) ? max_i_ : - max_i_; + } + // Update camera exposure parameter + camera_auto_exposure_ = std::round(camera_exposure_ + k_p_ * err_p_ + k_i_ * err_i_); + if (camera_auto_exposure_ < 50.0) camera_auto_exposure_ = 50.0; + else if (camera_auto_exposure_ > 250.0) camera_auto_exposure_ = 250.0; + + v4l_->SetParameters(camera_gain_, camera_auto_exposure_); + } + } + void CameraNodelet::PublishLoop() { bool camera_running = true; while (thread_running_) { if (!camera_running) { - while ((pub_.getNumSubscribers() == 0) && thread_running_) + while ((pub_.getNumSubscribers() + getNumBayerSubscribers() == 0) && thread_running_) usleep(100000); if (!thread_running_) break; @@ -299,34 +511,81 @@ namespace is_camera { int last_buf = v4l_->buf.index; v4l_->buf.index = (last_buf + 1) % v4l_->req.count; - if (pub_.getNumSubscribers() != 0) + if (pub_.getNumSubscribers() + getNumBayerSubscribers() > 0) xioctl(v4l_->fd, VIDIOC_QBUF, &v4l_->buf); else camera_running = false; - ros::Time timestamp = ros::Time::now(); - - // Select our output msg buffer - img_msg_buffer_idx_ = (img_msg_buffer_idx_ + 1) % kImageMsgBuffer; if (!failed) { - // Wrap the buffer with cv::Mat so we can manipulate it. - cv::Mat wrapped(v4l_->fmt.fmt.pix.height, - v4l_->fmt.fmt.pix.width, - cv::DataType::type, - v4l_->buffers[last_buf].start, - v4l_->fmt.fmt.pix.width); // does not copy - cv::Mat owrapped(kImageHeight, kImageWidth, - cv::DataType::type, - &(img_msg_buffer_[img_msg_buffer_idx_]->data[0]), - kImageWidth); - cv::cvtColor(wrapped, owrapped, - CV_BayerGR2GRAY); - - // Attach the time - img_msg_buffer_[img_msg_buffer_idx_]->header = std_msgs::Header(); - img_msg_buffer_[img_msg_buffer_idx_]->header.stamp = timestamp; - - pub_.publish(img_msg_buffer_[img_msg_buffer_idx_]); + // our camera driver gives monotonic time, convert to wall time + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + ros::Time now = ros::Time::now(); + ros::Time now_monotonic(ts.tv_sec, ts.tv_nsec); + ros::Time timestamp(v4l_->buf.timestamp.tv_sec, 1000 * v4l_->buf.timestamp.tv_usec); + timestamp = now + (timestamp - now_monotonic); + + if (pub_.getNumSubscribers() > 0) { + sensor_msgs::ImagePtr& out_image = img_msg_buffer_[img_msg_buffer_idx_]; + + // Use OpenCV to convert raw Bayer format to grayscale (writing the result + // into the pre-allocated slot in the image message ring buffer). + cv::Mat wrapped(v4l_->fmt.fmt.pix.height, + v4l_->fmt.fmt.pix.width, + cv::DataType::type, + v4l_->buffers[last_buf].start, + v4l_->fmt.fmt.pix.width); // does not copy + cv::Mat owrapped(kImageHeight, kImageWidth, + cv::DataType::type, + &(out_image->data[0]), + kImageWidth); + // This encoding is incorrect, however given that the maps were built + // with it it will not be changed. It's supposed to be BayerGB2GRAY [1], given + // that the bayer encoding is BAYER_GRBG8, as tested in the lab. + // [1] https://github.com/ros-perception/image_pipeline/blob/71d537a50bf4e7769af513f4a3d3df0d188cfa05/image_proc/src/nodelets/debayer.cpp#L226 + cv::cvtColor(wrapped, owrapped, + CV_BayerGR2GRAY); + + // Attach the time + out_image->header = std_msgs::Header(); + out_image->header.stamp = timestamp; + + pub_.publish(out_image); + + // Increment index in ring buffer + img_msg_buffer_idx_ = (img_msg_buffer_idx_ + 1) % kImageMsgBuffer; + } + + if (getNumBayerSubscribers() > 0) { + bayer_throttle_ratio_counter_ = (bayer_throttle_ratio_counter_ + 1) + % bayer_throttle_ratio_; + + if (bayer_throttle_ratio_counter_ == 0) { + sensor_msgs::ImagePtr& out_image = bayer_img_msg_buffer_[bayer_img_msg_buffer_idx_]; + + // Copy the raw Bayer format data into the pre-allocated + // slot in the image message ring buffer. + memcpy(&(out_image->data[0]), + v4l_->buffers[last_buf].start, + kImageWidth * kImageHeight); + + // Attach the time + out_image->header = std_msgs::Header(); + out_image->header.stamp = timestamp; + + bayer_pub_.publish(out_image); + + // Increment index in ring buffer + bayer_img_msg_buffer_idx_ = (bayer_img_msg_buffer_idx_ + 1) % kBayerImageMsgBufferLength; + } + } + + // Publish camera info either way + if (getNumBayerSubscribers() + pub_.getNumSubscribers() > 0) { + info_msg_.header = std_msgs::Header(); + info_msg_.header.stamp = timestamp; + info_pub_.publish(info_msg_); + } } ros::spinOnce(); diff --git a/hardware/is_camera/src/debayer.cc b/hardware/is_camera/src/debayer.cc new file mode 100644 index 0000000000..b75fbfe018 --- /dev/null +++ b/hardware/is_camera/src/debayer.cc @@ -0,0 +1,130 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Core ROS +#include +#include +#include + +// Shared libraries +#include +#include + +#include +#include +#include + +// Messages +#include + +#include +#include + +/** + * \ingroup hardware + */ +namespace debayer { + +namespace enc = sensor_msgs::image_encodings; + +class DebayerNodelet : public ff_util::FreeFlyerNodelet { + public: + DebayerNodelet() : ff_util::FreeFlyerNodelet(false) {} + ~DebayerNodelet() {} + + protected: + void Initialize(ros::NodeHandle *nh) { + std::string topic, topic_bayer_color, topic_bayer_raw; + config_name_ = GetName(); + if (GetName() == "nav_cam_debayer") { + topic = TOPIC_HARDWARE_NAV_CAM; + } else if (GetName() == "dock_cam_debayer") { + topic = TOPIC_HARDWARE_DOCK_CAM; + } else { + FF_FATAL("Unknown camera driver name."); + } + + // Advertise color image + topic_bayer_color = topic + TOPIC_HARDWARE_CAM_SUFFIX_BAYER_COLOR; + pub_color_ = nh->advertise(topic_bayer_color, 1); + + // Subscribe to the raw bayer topic + topic_bayer_raw = topic + TOPIC_HARDWARE_CAM_SUFFIX_BAYER_RAW; + sub_raw_ = nh->subscribe(topic_bayer_raw, 1, &DebayerNodelet::BayerImageCallback, this); + } + + // Called when depth image data arrives + void BayerImageCallback(const sensor_msgs::ImageConstPtr& raw_msg) { + // Next, publish to color + if (!pub_color_.getNumSubscribers()) return; + + int bit_depth = enc::bitDepth(raw_msg->encoding); + + if (enc::isBayer(raw_msg->encoding)) { + int type = bit_depth == 8 ? CV_8U : CV_16U; + const cv::Mat bayer(raw_msg->height, raw_msg->width, CV_MAKETYPE(type, 1), + const_cast(&raw_msg->data[0]), raw_msg->step); + + sensor_msgs::ImagePtr color_msg = boost::make_shared(); + color_msg->header = raw_msg->header; + color_msg->height = raw_msg->height; + color_msg->width = raw_msg->width; + color_msg->encoding = bit_depth == 8? enc::BGR8 : enc::BGR16; + color_msg->step = color_msg->width * 3 * (bit_depth / 8); + color_msg->data.resize(color_msg->height * color_msg->step); + + cv::Mat color(color_msg->height, color_msg->width, CV_MAKETYPE(type, 3), + &color_msg->data[0], color_msg->step); + + int code = -1; + if (raw_msg->encoding == enc::BAYER_RGGB8 || + raw_msg->encoding == enc::BAYER_RGGB16) + code = cv::COLOR_BayerBG2BGR; + else if (raw_msg->encoding == enc::BAYER_BGGR8 || + raw_msg->encoding == enc::BAYER_BGGR16) + code = cv::COLOR_BayerRG2BGR; + else if (raw_msg->encoding == enc::BAYER_GBRG8 || + raw_msg->encoding == enc::BAYER_GBRG16) + code = cv::COLOR_BayerGR2BGR; + else if (raw_msg->encoding == enc::BAYER_GRBG8 || + raw_msg->encoding == enc::BAYER_GRBG16) + code = cv::COLOR_BayerGB2BGR; + + try { + cv::cvtColor(bayer, color, code); + } catch (cv::Exception& e) { + NODELET_WARN_THROTTLE(30, "cvtColor error: '%s', bayer code: %d, width %d, height %d", + e.what(), code, bayer.cols, bayer.rows); + return; + } + + pub_color_.publish(color_msg); + } + } + + private: + // ROS + ros::Subscriber sub_raw_; + ros::Publisher pub_color_; + + std::string config_name_; +}; + +PLUGINLIB_EXPORT_CLASS(debayer::DebayerNodelet, nodelet::Nodelet); + +} // namespace debayer diff --git a/hardware/laser/CMakeLists.txt b/hardware/laser/CMakeLists.txt index b064c43921..e8534aee9c 100644 --- a/hardware/laser/CMakeLists.txt +++ b/hardware/laser/CMakeLists.txt @@ -15,33 +15,102 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(laser) -# Core library -create_library(TARGET laser - INC ${GLOG_INCLUDE_DIRS} - LIBS ${GLOG_LIBRARIES} i2c - DEPS i2c) - -# Standalone test programs -create_tool_targets(DIR tools - INC ${GFLAGS_INCLUDE_DIRS} - LIBS ${GFLAGS_LIBRARIES} laser ff_common - DEPS laser) - -# Create ROS nodes if we are using ROS -if(USE_ROS) - catkin_package( - LIBRARIES laser - CATKIN_DEPENDS roscpp roslib nodelet - DEPENDS ff_hw_msgs - ) - - create_library(TARGET laser_nodelet - DIR src/ros - LIBS ${catkin_LIBRARIES} laser config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS ff_hw_msgs - ) - install_launch_files() -endif(USE_ROS) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + roslib + nodelet + cv_bridge + ff_util + ff_common + ff_hw_msgs + i2c + config_reader +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES laser + CATKIN_DEPENDS + roscpp + roslib + nodelet + cv_bridge + ff_util + ff_common + ff_hw_msgs + i2c + config_reader +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +# Declare C++ libraries +add_library(laser + src/laser.cc +) +add_dependencies(laser ${catkin_EXPORTED_TARGETS}) +target_link_libraries(laser ${catkin_LIBRARIES} ${EIGEN_LIBRARIES}) + +# Declare C++ libraries +add_library(laser_nodelet + src/ros/laser_nodelet.cc +) +add_dependencies(laser_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(laser_nodelet laser ${catkin_LIBRARIES} ${EIGEN_LIBRARIES}) + +## Declare a C++ executable: bag_to_csv +add_executable(laser_test tools/laser_test.cc) +add_dependencies(laser_test ${catkin_EXPORTED_TARGETS}) +target_link_libraries(laser_test + laser gflags glog ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS ${PROJECT_NAME}_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS laser_test DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/laser_test share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/hardware/laser/include/laser/ros/laser_nodelet.h b/hardware/laser/include/laser/ros/laser_nodelet.h index 1dd6aafc93..cc8ed26869 100644 --- a/hardware/laser/include/laser/ros/laser_nodelet.h +++ b/hardware/laser/include/laser/ros/laser_nodelet.h @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include diff --git a/hardware/laser/package.xml b/hardware/laser/package.xml index 653cf7196c..63332e9b2b 100644 --- a/hardware/laser/package.xml +++ b/hardware/laser/package.xml @@ -15,12 +15,21 @@ roscpp roslib nodelet + cv_bridge + ff_util + ff_common ff_hw_msgs + i2c + config_reader roscpp + roslib nodelet + cv_bridge + ff_util + ff_common ff_hw_msgs - roslib - message_runtime + i2c + config_reader diff --git a/hardware/perching_arm/CMakeLists.txt b/hardware/perching_arm/CMakeLists.txt index 2784bdf506..6f1e67d3e5 100644 --- a/hardware/perching_arm/CMakeLists.txt +++ b/hardware/perching_arm/CMakeLists.txt @@ -15,8 +15,22 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(perching_arm) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + pluginlib + ff_util + ff_serial + ff_hw_msgs +) + catkin_package( LIBRARIES perching_arm @@ -25,21 +39,61 @@ catkin_package( nodelet pluginlib ff_util + ff_serial ff_hw_msgs - sensor_msgs ) -create_library( - TARGET perching_arm - LIBS ${catkin_LIBRARIES} ff_serial config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS perching_arm_gencpp +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} ) -create_tool_targets(DIR tools - LIBS ${catkin_LIBRARIES} perching_arm ff_common - INC ${catkin_INCLUDE_DIRS} - DEPS ff_common +# Declare C++ libraries +add_library(perching_arm + src/perching_arm.cc + src/perching_arm_node.cc ) +add_dependencies(perching_arm ${catkin_EXPORTED_TARGETS}) +target_link_libraries(perching_arm ${catkin_LIBRARIES} ${EIGEN_LIBRARIES}) + +## Declare a C++ executable: bag_to_csv +add_executable(perching_arm_tool tools/perching_arm_tool.cc) +add_dependencies(perching_arm_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(perching_arm_tool + perching_arm gflags glog ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS perching_arm_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/perching_arm_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/hardware/perching_arm/package.xml b/hardware/perching_arm/package.xml index 60601af3e8..921d8ba8e2 100644 --- a/hardware/perching_arm/package.xml +++ b/hardware/perching_arm/package.xml @@ -17,14 +17,14 @@ roscpp nodelet pluginlib - sensor_msgs ff_util + ff_serial ff_hw_msgs roscpp nodelet pluginlib - sensor_msgs ff_util + ff_serial ff_hw_msgs diff --git a/hardware/perching_arm/readme.md b/hardware/perching_arm/readme.md index 5de2733c33..62bddc5d18 100644 --- a/hardware/perching_arm/readme.md +++ b/hardware/perching_arm/readme.md @@ -1,3 +1,8 @@ \page perching_arm Perching arm -To be written... \ No newline at end of file +## Re-initialize service + +In case we forget to power on the arm bay before starting fsw, it is +possible to re-initialize the perching arm, trying to re-connect with: + + rosservice call /hw/arm/enable_arm "enabled: true" \ No newline at end of file diff --git a/hardware/perching_arm/src/perching_arm_node.cc b/hardware/perching_arm/src/perching_arm_node.cc index 97f3a3dedd..ddc4140408 100644 --- a/hardware/perching_arm/src/perching_arm_node.cc +++ b/hardware/perching_arm/src/perching_arm_node.cc @@ -50,6 +50,7 @@ class PerchingArmNode : public ff_util::FreeFlyerNodelet { // Called on flight software stack initialization - every NODELET_FATAIL // call below should be converted to an initialization fault... void Initialize(ros::NodeHandle *nh) { + nh_ = *nh; // Read the configuration config_reader::ConfigReader config_params; config_params.AddFile("hw/perching_arm.config"); @@ -60,6 +61,15 @@ class PerchingArmNode : public ff_util::FreeFlyerNodelet { if (!config_params.GetTable("perching_arm", &devices)) NODELET_FATAL("Could get perching_arm item in config file"); + // Reconnect to the arm service + if (!initialized_) { + srv_a_ = + nh->advertiseService(SERVICE_HARDWARE_PERCHING_ARM_ENABLE, + &PerchingArmNode::EnableArmCallback, + this); + initialized_ = true; + } + // Iterate over all devices for (int i = 0; i < devices.GetSize(); i++) { config_reader::ConfigReader::Table device_info; @@ -99,6 +109,7 @@ class PerchingArmNode : public ff_util::FreeFlyerNodelet { NODELET_WARN("Could not initialize the arm. It is attached?"); return; } + arm_connected_ = true; // Grab config parameters for the matched device config_reader::ConfigReader::Table config_list; @@ -347,6 +358,23 @@ class PerchingArmNode : public ff_util::FreeFlyerNodelet { } } + // This service re-initializes the perching arm if the arm was + // not powered on during startup + bool EnableArmCallback(ff_hw_msgs::SetEnabled::Request &req, + ff_hw_msgs::SetEnabled::Response &res) { + if (req.enabled && !arm_connected_) { + Initialize(&nh_); + res.success = arm_connected_; + } else if (!req.enabled && arm_connected_) { + res.success = false; + NODELET_WARN("It is not possible to disable the arm"); + } else { + res.success = true; + NODELET_WARN("Already satisfies request"); + } + return true; + } + // Set the distal velocity bool SetDistVelCallback(ff_hw_msgs::SetJointMaxVelocity::Request &req, ff_hw_msgs::SetJointMaxVelocity::Response &res) { @@ -415,11 +443,15 @@ class PerchingArmNode : public ff_util::FreeFlyerNodelet { } private: + ros::NodeHandle nh_; + bool initialized_ = false; + bool arm_connected_ = false; PerchingArm arm_; // Arm interface library ros::Subscriber sub_; // Joint state subscriber ros::Publisher pub_; // Joint state publisher + ros::ServiceServer srv_a_; // Enable the arm ros::ServiceServer srv_p_; // Set max pan velocity - ros::ServiceServer srv_t_; // Set max tilt velcoity + ros::ServiceServer srv_t_; // Set max tilt velocity ros::ServiceServer srv_ps_; // Enable/Disable the proximal joint servo ros::ServiceServer srv_ds_; // Enable/Disable the distal joint servo ros::ServiceServer srv_gs_; // Enable/Disable the gripper joint servo diff --git a/hardware/pico_driver/CMakeLists.txt b/hardware/pico_driver/CMakeLists.txt index 5cd842e83c..2d1a7215f3 100644 --- a/hardware/pico_driver/CMakeLists.txt +++ b/hardware/pico_driver/CMakeLists.txt @@ -15,25 +15,112 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(pico_driver) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +if (ENABLE_PICOFLEXX) +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + ff_msgs + ff_util + cv_bridge +) + +# System dependencies are found with CMake's conventions +find_package(royale REQUIRED) + catkin_package( LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp nodelet ff_msgs - DEPENDS ${PROJECT_NAME} + CATKIN_DEPENDS roscpp nodelet ff_msgs ff_util cv_bridge ) -link_directories(${royale_LIB_DIR}) +########### +## Build ## +########### -create_library(TARGET ${PROJECT_NAME} - LIBS ${catkin_LIBRARIES} ${royale_LIBRARIES} config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} ${royale_INCLUDE_DIRS} +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${royale_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) -create_tool_targets(DIR tools - LIBS ${catkin_LIBRARIES} ${royale_LIBRARIES} uvc - INC ${catkin_INCLUDE_DIRS} ${royale_INCLUDE_DIRS} - DEPS ff_msgs opencv +# Declare C++ libraries +add_library(pico_driver + src/pico_driver.cc ) +add_dependencies(pico_driver ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pico_driver ${royale_LIBRARIES} ${EIGEN_LIBRARIES} ${catkin_LIBRARIES}) + +add_library(pico_proxy + src/pico_proxy.cc +) +add_dependencies(pico_proxy ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pico_proxy + pico_driver gflags glog ${catkin_LIBRARIES}) + +## Declare a C++ executable: bag_to_csv +add_executable(pico_tool tools/pico_tool.cc) +add_dependencies(pico_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pico_tool + pico_driver gflags glog ${royale_LIBRARIES} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + # Pico flexx initialization fault tester + add_rostest_gtest(test_init_pico_flexx + test/test_init_pico_flexx.test + test/test_init_pico_flexx.cc + ) + + target_link_libraries(test_init_pico_flexx + glog ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS pico_proxy + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS pico_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/pico_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) -install_launch_files() +else (ENABLE_PICOFLEXX) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (ENABLE_PICOFLEXX) diff --git a/hardware/pico_driver/launch/pico_proxy.launch b/hardware/pico_driver/launch/pico_proxy.launch index e3fef382bf..45f7766df0 100644 --- a/hardware/pico_driver/launch/pico_proxy.launch +++ b/hardware/pico_driver/launch/pico_proxy.launch @@ -15,12 +15,13 @@ - - - - - - - - + + + + + + + + + diff --git a/hardware/pico_driver/nodelet_plugins.xml b/hardware/pico_driver/nodelet_plugins.xml index 2ac4fa2928..9c65bb3490 100644 --- a/hardware/pico_driver/nodelet_plugins.xml +++ b/hardware/pico_driver/nodelet_plugins.xml @@ -1,5 +1,12 @@ - - - Nodelet for pico_driver::PicoDriverNodelet - - \ No newline at end of file + + + + Nodelet for pico_driver::PicoDriverNodelet + + + + + Nodelet for pico_proxy::PicoProxyNodelet + + + \ No newline at end of file diff --git a/hardware/pico_driver/package.xml b/hardware/pico_driver/package.xml index 00b2e7634a..d789d1c0cb 100644 --- a/hardware/pico_driver/package.xml +++ b/hardware/pico_driver/package.xml @@ -17,11 +17,15 @@ catkin roscpp nodelet - ff_msgs + ff_msgs + ff_util + cv_bridge roscpp nodelet tf2_ros ff_msgs + ff_util + cv_bridge diff --git a/hardware/pico_driver/readme.md b/hardware/pico_driver/readme.md index d37f984aa0..5a3982a39a 100644 --- a/hardware/pico_driver/readme.md +++ b/hardware/pico_driver/readme.md @@ -1,7 +1,11 @@ -\page picoflexx PicoFlexx ROS Driver +\page picoflexx PicoFlexx The PMD PicoFlexx camera is a time of flight sensor that returns depth images, rather than RGB images. The sensor also comes factory calibrated with camera intrinsics and distortion parameters burned into its ROM. Using this information it is possible to project the depth measurements into a point cloud, which is represented in the optical axis. In order to project this into another frame (ISS, body) camera extrinsics are required. +# Utility scripts +\subpage picoflexx_python + + The objective of this page is to document an experiment to test the amount of computational resources (CPU load) required to run the PicoFlexx on our target architecture. For the sake of repeatability, this page provides description of the hardware and software setup, and includes attachments that may be of use to the reader. # Experimental setup @@ -10,7 +14,7 @@ The objective of this page is to document an experiment to test the amount of co The hardware setup is pictured below. It is essentially an Inforce IFC6501 system on module (SOM) plugged into a development board. This board is connected by a USB 2.0 hub to two PMD PicoFlexx sensors, and through Ethernet to a host PC (Ubuntu 16.04 64bit VM). The host PC acts as a DHCP server, NAT router, DNS forwarder and debugger. -![alt text](../images/hw/pico-setup.jpg "The experimental setup") +![alt text](/doc/images/hw/pico-setup.jpg "The experimental setup") The SOM is fitted with a cooling solution (heatsink and fan) that roughly mimics what is on the actual platform. Thermal testing has shown that by default all cores are run at maximum speed and under high load the CPU frequency switches three of the four cores offline when a thermal barrier is reached. For this reason the following script is run on boot to lower the maximum operating frequency of all four cores to 1.96GHz (they may be throttled lower than this in response to demand). @@ -116,7 +120,7 @@ There are a couple of interesting results from this experiment: It is also important to note that, although it is not shown in the results, the output of the pico_flexx_driver showed the point cloud callback rate to be within 1% of the target rate. For this reason, frame dropping cannot be used as a justification for the plateau in load we see from 15Hz onwards. -![alt text](../images/hw/pico-results.jpg "Performance results") +![alt text](/doc/images/hw/pico-results.jpg "Performance results") The raw data for this experiment, as well as the octave code used to generate the plots, is included in [matlab-results.tar.gz](https://babelfish.arc.nasa.gov/trac/freeflyer/attachment/wiki/pmd_picoflexx/matlab-results.tar.gz). @@ -126,7 +130,7 @@ The PicoFlexx SDK provides a C++ interface for querying point clouds from the ca Here we compare the performance of the pico_flexx_driver to that of a light-weight driver written for the FreeFlyer flight software stack. For both drivers we used the same SDK (our custom build) and the ROS Kinetic test environment described in the previous section. We limit our analysis to just the 5Hz mode, because this is the only feasible configuration -- all other moved are too resource-intensive for our application. The results are shown below. -![alt text](../images/hw/pico-drivers.jpg "Comparison to third party ROS drivers") +![alt text](/doc/images/hw/pico-drivers.jpg "Comparison to third party ROS drivers") Our results show that we are likely to gain a 1% improvement in resource usage by running our pico_driver rather than the pico_flexx_driver. However, the pico_driver does need to be updated to distinguish between multiple cameras; it currently just picks the first one that it finds on the bus. @@ -134,7 +138,7 @@ Our results show that we are likely to gain a 1% improvement in resource usage b The current version of pico_driver performed an iterative copy of all points from the royale SDK to a point cloud, and then a message conversion from the PointCloud to a PointCloud2 data type. I added the improvements mentioned in the section below, and optimized the code to use a memcpy rather than an iterative copy, by exploiting the fact that the PointCloud2 data type accommodates complex data structures. This yielded a small improvement in speed for low rates, but a massive improvement for higher rates. The results are shown in the graph below: -![alt text](../images/hw/pico-optimization.jpg "Optimizing the ROS drivers") +![alt text](/doc/images/hw/pico-optimization.jpg "Optimizing the ROS drivers") # Setup on a laptop diff --git a/hardware/pico_driver/scripts/README.md b/hardware/pico_driver/scripts/README.md new file mode 100644 index 0000000000..01e767c91b --- /dev/null +++ b/hardware/pico_driver/scripts/README.md @@ -0,0 +1,109 @@ +\page picoflexx_python Pico Flexx Python utilities + +These Python utilities enable us to process logged +`ff_msgs/PicoflexxIntermediateData` messages on the +`/hw/depth_haz/extended` topic and split the data to regenerate both +point cloud messages normally logged on the `/hw/depth_haz/points` topic +and amplitude image messages logged on the +`/hw/depth_haz/extended/amplitude_int` topic. (Those are the +HazCam-specific topics; to use PerchCam, specify `--cam=perch`.) + +As a result, once these tools are fully validated, we will no longer +need to log the latter two topics, providing a major savings in data +volume. To quantify the impact, we studied the longest `delayed` bag +from the SoundSee-Data-3 Astrobee ISS activity, running an ISAAC-style +survey. Out of a total bag size of 20.9 GB, the top four messages by +data volume were NavCam images (8.6 GB), HazCam point clouds (6.3 GB), +HazCam extended messages (5.0 GB), and HazCam amplitude images (0.6 +GB). By no longer logging point clouds and amplitude images, we could +save up to 6.9 GB (33%). These savings impact not just storage space on +the Astrobee MLP but also CPU load required to log the data, which +experience shows can be significant. + +## System requirements and installation + +So far, these tools were tested only under Ubuntu 18.04 and Python 2.7 +on the host astrobeast.ndc.nasa.gov. This host already had the necessary +dependencies pre-installed by the admins in the ARC-TI Systems Group, +so no installation steps were needed, other than checking out the scripts +from the repository. + +Some minor changes would probably be needed to support Python 3.x. If +the tools will be used on other hosts that aren't pre-configured, this +section should be expanded to document how to install the dependencies. + +## Usage + +Using these tools is a two-step process: + +1. Prep once for each Pico Flexx serial number: Using a prior bag that + contains point clouds, extract and save xyz coefficients that are + required to recover point clouds from extended messages. Optionally, + if the prior bag contains all three Pico message types, you can also + use it to do a dry run of the split operation and check the quality + of the reconstructed data. + +2. Production: A single command performs the split operation, writing a + new bag with the recovered point clouds and amplitude images. + +This usage example shows how to do the prep: + + scripts=$HOME/astrobee/src/hardware/pico_driver/scripts + # example bag archived at hivemind.ndc.nasa.gov:/home/data-processing/freeflyer/2022-01-03_100/robot_data/SN003/bags + inbag=20220103_1429_soundsee_isaac_isaac_survey15mins.bag + $scripts/pico_write_xyz_coeff.py $inbag bumble_haz_xyz_coeff.npy + # optional: dry run and check data quality + $scripts/pico_split_extended.py $inbag bumble_haz_xyz_coeff.npy haz_split.bag + $scripts/pico_check_split_extended.py $inbag haz_split.bag + +And this example excerpts the one step needed in production: + + $scripts/pico_split_extended.py $inbag bumble_xyz_coeff.npy haz_split.bag + +## Getting valid xyz coefficients + +A file of xyz coefficients is required to recover point clouds from +extended messages. + +Using the pico_write_xyz_coeff.py script, the xyz coefficients are +effectively derived from the camera intrinsic parameters stored in the +camera firmware. These parameters are unique to each Pico Flexx serial +number and calibrated at the factory. By convention, when saving the +coefficients, the output file name should document which robot and which +camera (haz or perch) the coefficients are for. During reconstruction, +be careful to use the correct coefficients file for your robot and +camera. + +The method we are using to recover the coefficients requires a bag of +sample data recorded from the sensor, and it can only produce +coefficients for a given sensor pixel if at least one frame of the +input bag has valid depth data for that pixel. This means that: + +- The bag needs to be recorded with a scene that is suitable for the + Pico Flexx (objects within the right distance range, objects not too + dark or specular, etc.). + +- If the bag is recorded during a normal Astrobee activity in the + complicated ISS environment, you will effectively never see a static + scene that has all valid pixels. Therefore, it's better to process a + bag that contains lots of Pico Flexx frames with varying scenery, in + order to have the best chance of filling in the occasional dropout + pixels. + +- We've seen in practice that even if the guidelines above are followed, + there may be many pixels (~3%) near the corners of the image that + never get any valid depth returns. This doesn't cause anything to + crash and isn't a big deal *except* that if we use the resulting + coefficients, then even if future Pico Flexx sensor frames have valid + depth returns for those pixels, we won't be able to reconstruct the + corresponding xyz points due to the missing coefficients. The relevant + entries in the reconstructed xyz output will be set to the no-data + value, as if there was not a valid depth return. Note that for dense + 3D mapping purposes, the pixels near the corners were already being + ignored anyway, because they were considered suspect due to + limitations of the camera calibration and other issues. + +- In a controlled lab setting, the ideal bag to collect to maximize + valid depth returns for calibrating the coefficients would probably + have the camera staring at a blank white surface just a bit farther + away than the minimum depth range of the camera (e.g., 20 cm). diff --git a/hardware/pico_driver/scripts/debug_pico_utils.py b/hardware/pico_driver/scripts/debug_pico_utils.py new file mode 100755 index 0000000000..59c5da902b --- /dev/null +++ b/hardware/pico_driver/scripts/debug_pico_utils.py @@ -0,0 +1,312 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Generate various plots that may help with debugging pico_utils issues. +""" + +from __future__ import print_function + +import matplotlib + +matplotlib.use("Agg") + +import argparse +import itertools +import logging + +import numpy as np +import rosbag +from matplotlib import collections as mc +from matplotlib import pyplot as plt + +import pico_utils as pico + + +def plot_xy_grid_many(inbag_path, verbose=False, fast=False, cam=pico.DEFAULT_CAM): + """ + Verify that over multiple point cloud frames, the xy values for z = + 1 remain consistent. + + Different frames will be plotted in different colors; you should + generally see only the last plotted color, except for a few + scattered points where one frame or another is missing data. And all + points should fall on a "warped grid" pattern based on the camera + intrinsics. + """ + print() + print("=== plot_xy_grid_many ===") + + fig = plt.figure() + fig, ax = plt.subplots() + ax.axis("equal") + + for pts in pico.get_msgs(inbag_path, pico.get_pts_topic(cam), fast): + xyz = pico.xyz_from_points(pts) + x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2] + xp = x / z + yp = y / z + plt.plot(xp, yp, ".") + + plt.tight_layout() + fig_path = "xy_grid_many.png" + fig.set_size_inches((40, 40)) + plt.savefig(fig_path) + logging.info("wrote to %s", fig_path) + plt.close() + + +def iterate_pixels(img): + """ + Given @img an N-channel image with dimensions (H, W, N), returns a + generator that iterates through the pixels, where the value of each + pixel is a length-N array. + """ + w, h, _ = img.shape + for ix in range(w): + for iy in range(h): + yield img[ix, iy, :] + + +def debug_plot_grid_lines(igrid): + """ + Given @igrid a 2-channel "grid" image with dimensions (H, W, 2) + where channels 0 and 1 are interpreted as x and y coordinates, plot + the edges that connect Manhattan neighbor (x, y) points in order to + visually verify that the grid is sensible. + """ + lines = [] + + # logging.debug("grid shapes %s %s", g.igrid[:, :-1, :].shape, g.igrid[:, 1:, :].shape) + grid_pairs = ( + # horizontal neighbor pairs + (igrid[:, :-1, :], igrid[:, 1:, :]), + # vertical neighbor pairs + (igrid[:-1, :, :], igrid[1:, :, :]), + ) + for from_grid, to_grid in grid_pairs: + for from_pt, to_pt in itertools.izip( + iterate_pixels(from_grid), iterate_pixels(to_grid) + ): + # logging.debug("from_pt %s to_pt %s", from_pt, to_pt) + if not (np.isnan(from_pt[0]) or np.isnan(to_pt[0])): + lines.append((from_pt, to_pt)) + # logging.debug("lines d/d0 %f", np.linalg.norm(to_pt - from_pt) / 0.005) + + lc = mc.LineCollection(lines, linewidths=[0.1] * len(lines)) + fig, ax = plt.subplots() + ax.add_collection(lc) + ax.autoscale() + + +def debug_plot_grid_points(igrid): + """ + Given @igrid a 2-channel "grid" image with dimensions (H, W, 2) + where channels 0 and 1 are interpreted as x and y coordinates, plot + the (x, y) points. + """ + plt.plot(igrid[:, :, 0], igrid[:, :, 1], "r.", markersize=0.1) + + +def xy_from_xyz(xyz): + x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2] + xp = x / z + yp = y / z + xy = np.column_stack([xp, yp]) + return xy + + +def igrid_from_xyz(xyz): + xy = xy_from_xyz(xyz) + return xy.reshape((pico.SENSOR_HEIGHT_PIXELS, pico.SENSOR_WIDTH_PIXELS, 2)) + + +def get_igrid(inbag_path, fast=False, cam=pico.DEFAULT_CAM): + xyz = pico.merge_point_clouds( + pico.get_msgs(inbag_path, pico.get_pts_topic(cam), fast=fast) + ) + return igrid_from_xyz(xyz) + + +def plot_xy_grid_merge(inbag_path, verbose=False, fast=False, cam=pico.DEFAULT_CAM): + """ + Verify the sanity of the x/y grid after merging all xyz frames. + It should follow a "warped grid" pattern. + """ + print() + print("=== plot_xy_grid_merge ===") + igrid = get_igrid(inbag_path, fast=fast, cam=cam) + + fig = plt.figure() + fig, ax = plt.subplots() + ax.axis("equal") + + debug_plot_grid_lines(igrid) + debug_plot_grid_points(igrid) + + plt.tight_layout() + fig_path = "xy_grid_merge.pdf" + fig.set_size_inches((40, 40)) + plt.savefig(fig_path) + logging.info("wrote to %s", fig_path) + plt.close() + + +def plot_msg_sync(inbag_path, verbose=False, cam=pico.DEFAULT_CAM): + """ + Figure out how to match extended messages to points messages from + the same frame. + + From the resulting plots, it looks like generally when a new frame + arrives from the sensor, its "points" message will be published + first and its "extended" message will be published < 100 ms + later. (Going by the msg.header.stamp publish timestamp.) + """ + print() + print("=== plot_msg_sync ===") + + with rosbag.Bag(inbag_path, "r") as inbag: + topics = [pico.get_ext_topic(cam), pico.get_pts_topic(cam)] + topic_times = {} + for topic, msg, t in inbag.read_messages(topics): + times = topic_times.setdefault(topic, []) + times.append(msg.header.stamp) + if len(times) >= 1000: + break + + t0 = topic_times[pico.get_pts_topic(cam)][0].to_sec() + for topic in topics: + topic_times[topic] = np.array([t.to_sec() for t in topic_times[topic]]) - t0 + + topic_data = {} + WIN_RADIUS = 10 + DT_MIN = -2 + DT_MAX = 2 + + pts_times = topic_times[pico.get_pts_topic(cam)] + for topic in topics: + data = [] + times = topic_times[topic] + for i, pts_t in enumerate(pts_times): + dt = times[(i - WIN_RADIUS) : (i + WIN_RADIUS)] - pts_t + in_bounds = (dt >= DT_MIN) & (dt <= DT_MAX) + dt = dt[in_bounds] + for dt_val in dt: + data.append((pts_t, dt_val)) + topic_data[topic] = np.array(data) + + fig = plt.figure() + for topic in topics: + data = topic_data[topic] + plt.plot(data[:, 0], data[:, 1], ",", markersize=0.2) + plt.legend(topics) + plt.xlabel("elapsed time msg.header.stamp (s)") + plt.ylabel("dt (s)") + plt.yticks(np.arange(DT_MIN, DT_MAX + 0.1, 0.1)) + plt.grid() + + fig_path = "message_sync_all.pdf" + fig.set_size_inches((50, 10)) + plt.savefig(fig_path) + logging.info("wrote to %s", fig_path) + plt.close() + + +def plot_first_distance_image(inbag_path, verbose=False, cam=pico.DEFAULT_CAM): + """ + Save the distance image extracted from the first extended message in + @inbag_path as an imshow() plot. + """ + print() + print("=== plot_first_distance_image ===") + ext_msgs = pico.get_msgs(inbag_path, pico.get_ext_topic(cam)) + distance, amplitude, intensity, noise = pico.split_extended(next(ext_msgs)) + + plt.figure() + plt.imshow(distance) + plt.tight_layout() + fig_path = "distance_image.png" + plt.savefig(fig_path) + logging.info("wrote to %s", fig_path) + plt.close() + + +def debug_xyz_agreement(inbag_path, verbose=False, fast=False, cam=pico.DEFAULT_CAM): + print() + print("=== debug_xyz_agreement ===") + xyz_coeff = pico.get_xyz_coeff(inbag_path, fast) + pair_stream = pico.get_ext_pts_pairs(inbag_path, fast=fast, cam=cam) + + err_max_log = [] + err_rms_log = [] + for i, (ext_msg, pts_msg) in enumerate(pair_stream): + distance, amplitude, intensity, noise = pico.split_extended(ext_msg) + distance = distance.reshape( + (pico.SENSOR_HEIGHT_PIXELS * pico.SENSOR_WIDTH_PIXELS,) + ) + xyz_ext = xyz_coeff * distance[:, np.newaxis] + xyz_pts = pico.xyz_from_points(pts_msg) + success, err_max, err_rms = pico.check_xyz_error(xyz_pts, xyz_ext, i) + err_max_log.append(err_max) + err_rms_log.append(err_rms) + + logging.info("Summary over all messages:") + logging.info(" Error max (um): %.3f", np.max(err_max_log) * 1e6) + logging.info(" Mean error RMS (um): %.3f", np.mean(err_rms_log) * 1e6) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "-v", + "--verbose", + help="print more verbose debug info", + default=False, + action="store_true", + ) + parser.add_argument( + "-f", + "--fast", + help="speed up testing by only processing part of the bag", + default=False, + action="store_true", + ) + parser.add_argument( + "-c", + "--cam", + help="specify camera for rosbag topic filtering [%(default)s]", + nargs="?", + choices=pico.CAM_CHOICES, + default=pico.DEFAULT_CAM, + ) + parser.add_argument("inbag", help="input bag") + + args = parser.parse_args() + level = logging.DEBUG if args.verbose else logging.INFO + logging.basicConfig(level=level, format="%(message)s") + + plot_xy_grid_many(args.inbag, verbose=args.verbose, fast=args.fast, cam=args.cam) + plot_xy_grid_merge(args.inbag, verbose=args.verbose, fast=args.fast, cam=args.cam) + plot_msg_sync(args.inbag, verbose=args.verbose, cam=args.cam) + plot_first_distance_image(args.inbag, verbose=args.verbose, cam=args.cam) + debug_xyz_agreement(args.inbag, verbose=args.verbose, fast=args.fast, cam=args.cam) + + # suppress confusing ROS message at exit + logging.getLogger().setLevel(logging.WARN) diff --git a/hardware/pico_driver/scripts/pico_check_split_extended.py b/hardware/pico_driver/scripts/pico_check_split_extended.py new file mode 100755 index 0000000000..82ef553d54 --- /dev/null +++ b/hardware/pico_driver/scripts/pico_check_split_extended.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Given two bags containing the message topics output by +pico_split_extended.py, aligns matching messages by publish timestamp +and checks their contents are the same. For "points" messages, the check +includes a (very strict) error tolerance to accommodate round-off +error. For "amplitude" messages, the check is for exact integer +equality. +""" + +import argparse +import logging + +import numpy as np + +import pico_utils as pico + + +def check_points_msgs( + orig_bag_path, test_bag_path, fast=False, verbose=False, cam=pico.DEFAULT_CAM +): + pts_topic = pico.get_pts_topic(cam) + orig_msgs = pico.get_msg_tuples(orig_bag_path, pts_topic) + test_msgs = pico.get_msg_tuples(test_bag_path, pts_topic) + + # In raw robot bags studied so far, the extended message is reliably + # published <100 ms after the points message. The points messages in + # orig_bag are timestamped like points messages. The points messages in + # test_bag will have timestamps the same as the extended messages they are + # derived from. We set dt_limit to reflect this. + pair_stream = pico.get_matched_msg_pairs( + orig_msgs, + test_msgs, + dt_limit=pico.EXT_MINUS_PTS_DT_LIMIT, + ) + if fast: + pair_stream = pico.fast_filter(pair_stream) + + success = True + err_max_log = [] + err_rms_log = [] + + k_logger = pico.PrintEveryK(100) + for i, (orig_msg, test_msg) in enumerate(pair_stream): + msg_success, err_max, err_rms = pico.check_points_msg_pair( + orig_msg, test_msg, i, verbose + ) + success &= msg_success + err_max_log.append(err_max) + err_rms_log.append(err_rms) + + k_logger.info("checked %5d points messages", k_logger.count + 1) + + logging.info("Summary over all %d points messages:", k_logger.count) + logging.info(" Error max (um): %.3f", np.max(err_max_log) * 1e6) + logging.info(" Mean error RMS (um): %.3f", np.mean(err_rms_log) * 1e6) + + return success + + +def check_amp_msgs( + orig_bag_path, test_bag_path, fast=False, verbose=False, cam=pico.DEFAULT_CAM +): + amp_topic = pico.get_amp_topic(cam) + orig_msgs = pico.get_msg_tuples(orig_bag_path, amp_topic) + test_msgs = pico.get_msg_tuples(test_bag_path, amp_topic) + + # In the original bag, we should be guaranteed that the amp message comes + # after the extended message, because it is derived from the extended + # message by the pico_proxy nodelet. The amp messages in orig_bag should be + # timestamped like amp messages, but the timestamps in test_bag inherit + # their timestamps from the extended messages. Therefore dt should be negative. + pair_stream = pico.get_matched_msg_pairs( + orig_msgs, + test_msgs, + dt_limit=(-0.2, 0), + ) + if fast: + pair_stream = pico.fast_filter(pair_stream) + + success = True + + k_logger = pico.PrintEveryK(100) + for i, (orig_msg, test_msg) in enumerate(pair_stream): + success &= pico.check_amp_msg_pair(orig_msg, test_msg, i, verbose) + + k_logger.info("checked %5d amplitude_int messages", k_logger.count + 1) + + return success + + +def pico_check_split_extended( + orig_bag_path, test_bag_path, fast=False, verbose=False, cam=pico.DEFAULT_CAM +): + success = True + success &= check_points_msgs( + orig_bag_path, test_bag_path, fast=fast, verbose=verbose, cam=cam + ) + success &= check_amp_msgs( + orig_bag_path, test_bag_path, fast=fast, verbose=verbose, cam=cam + ) + + if success: + print("OK - All checks passed.") + else: + print("FAILED - Some checks failed. See previous messages.") + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "-v", + "--verbose", + help="Print debug info", + default=False, + action="store_true", + ) + parser.add_argument( + "-f", + "--fast", + help="speed testing up by only processing part of the bags", + default=False, + action="store_true", + ) + parser.add_argument( + "-c", + "--cam", + help="specify camera for rosbag topic filtering [%(default)s]", + nargs="?", + choices=pico.CAM_CHOICES, + default=pico.DEFAULT_CAM, + ) + parser.add_argument( + "orig_bag", help="input bag containing original points messages" + ) + parser.add_argument( + "test_bag", + help="input bag containing points messages to check against original messages", + ) + + args = parser.parse_args() + level = logging.DEBUG if args.verbose else logging.INFO + logging.basicConfig(level=level, format="%(message)s") + + pico_check_split_extended( + args.orig_bag, + args.test_bag, + fast=args.fast, + verbose=args.verbose, + cam=args.cam, + ) + + # suppress confusing ROS message at exit + logging.getLogger().setLevel(logging.WARN) + + +if __name__ == "__main__": + main() diff --git a/hardware/pico_driver/scripts/pico_split_extended.py b/hardware/pico_driver/scripts/pico_split_extended.py new file mode 100755 index 0000000000..39fd0f380e --- /dev/null +++ b/hardware/pico_driver/scripts/pico_split_extended.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Given a bag containing /hw/depth_*/extended messages and an xyz coefficients +file output by pico_write_xyz_coeff.py, output a bag of /hw/depth_*/points +messages. +""" + +import argparse +import logging +import os +import shutil + +import numpy as np +import rosbag + +import pico_utils as pico + + +class PrintEveryK: + def __init__(self, k): + self.k = k + self.count = 0 + + def debug(self, *args, **kwargs): + self.count += 1 + if (self.count % self.k) == 0: + logging.debug(*args, **kwargs) + + +def pico_xyz_from_extended( + in_bag_path, + in_npy_path, + out_bag_path, + fast=False, + verbose=False, + cam=pico.DEFAULT_CAM, + save_all_topics=False, +): + with open(in_npy_path, "rb") as in_npy: + xyz_coeff = np.load(in_npy) + + k_logger = pico.PrintEveryK(100) + ext_topic = pico.get_ext_topic(cam) + pts_topic = pico.get_pts_topic(cam) + amp_topic = pico.get_amp_topic(cam) + topics_bag = [] if save_all_topics else ext_topic + + with rosbag.Bag(in_bag_path, "r") as bag, rosbag.Bag(out_bag_path, "w") as out_bag: + # for ext_msg in pico.get_msgs(in_bag_path, ext_topic, fast): + for topic, msg, t in bag.read_messages(topics_bag): + if topic == ext_topic: + distance, amplitude, intensity, noise = pico.split_extended(msg) + + xyz = pico.xyz_from_distance(distance, xyz_coeff) + pts_msg = pico.make_points_msg(msg.header, xyz) + out_bag.write(pts_topic, pts_msg, pts_msg.header.stamp) + + amp_msg = pico.make_amp_msg(msg.header, amplitude) + out_bag.write(amp_topic, amp_msg, amp_msg.header.stamp) + + k_logger.info("split %5d extended messages", k_logger.count + 1) + + if save_all_topics: + out_bag.write(topic, msg, t) + logging.info("wrote split messages to %s", out_bag_path) + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "inbag", nargs="+", help="Input bag containing extended messages" + ) + parser.add_argument("--in_npy", help="Input xyz coefficients file", default="") + parser.add_argument( + "-o", + "--output", + help="path for output bag", + default="{inbag}.split_extended.bag", + ) + parser.add_argument( + "-v", + "--verbose", + help="print more verbose debug info", + default=False, + action="store_true", + ) + parser.add_argument( + "-f", + "--fast", + help="speed testing up by only processing part of the bag", + default=False, + action="store_true", + ) + parser.add_argument( + "-c", + "--cam", + help="specify camera for rosbag topic filtering [%(default)s]", + nargs="?", + choices=pico.CAM_CHOICES, + default=pico.DEFAULT_CAM, + ) + parser.add_argument( + "-s", + "--save-all-topics", + dest="save_all_topics", + action="store_true", + help="Save all topics from input bagfile to output bagfile.", + ) + parser.add_argument( + "-n", + dest="do_nothing", + action="store_true", + help="Option to not debayer anything and write output", + ) + + args = parser.parse_args() + + level = logging.DEBUG if args.verbose else logging.INFO + logging.basicConfig(level=level, format="%(message)s") + + for inbag_path in args.inbag: + # Check if input bag exists + if not os.path.isfile(inbag_path): + print(("Bag file " + inbag_path + " does not exist.")) + sys.exit() + output_bag_name = args.output.format(inbag=inbag_path) + + # Check if output bag already exists + if os.path.exists(output_bag_name): + parser.error("not replacing existing file %s" % output_bag_name) + + if not args.do_nothing: + # Split extended message + pico_xyz_from_extended( + inbag_path, + args.in_npy, + output_bag_name, + args.fast, + args.verbose, + args.cam, + args.save_all_topics, + ) + else: + os.rename(inbag_path, output_bag_name) + + # suppress confusing ROS message at exit + logging.getLogger().setLevel(logging.WARN) + + +if __name__ == "__main__": + main() diff --git a/hardware/pico_driver/scripts/pico_utils.py b/hardware/pico_driver/scripts/pico_utils.py new file mode 100644 index 0000000000..32931d24d7 --- /dev/null +++ b/hardware/pico_driver/scripts/pico_utils.py @@ -0,0 +1,477 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Utilities for working with Pico Flexx data in Python. +""" + +from __future__ import print_function + +import collections +import io +import itertools +import logging +import struct + +import cv2 +import numpy as np +import rosbag +import sensor_msgs.point_cloud2 as pc2 +from cv_bridge import CvBridge +from sensor_msgs.msg import Image, PointCloud2, PointField + +PTS_TOPIC_TMPL = "/hw/depth_{cam}/points" +EXT_TOPIC_TMPL = "/hw/depth_{cam}/extended" +AMP_TOPIC_TMPL = "/hw/depth_{cam}/extended/amplitude_int" +CAM_CHOICES = ("haz", "perch") +DEFAULT_CAM = "haz" +SENSOR_WIDTH_PIXELS = 224 +SENSOR_HEIGHT_PIXELS = 171 + +# We've observed that in the raw bags from Astrobee, the depth "extended" +# message is reliably published < 100 ms after the "points" message, when +# comparing the msg.header.stamp values. We can use this to robustly identify +# matched message pairs, even if they are occasionally out of order in the +# rosbag. +EXT_MINUS_PTS_DT_LIMIT = (0, 0.1) +PTS_MINUS_EXT_DT_LIMIT = (-EXT_MINUS_PTS_DT_LIMIT[1], -EXT_MINUS_PTS_DT_LIMIT[0]) + +# This tolerance is much stricter than what we really need from the application +# side, and about double the maximum error we've seen so far with +# reconstructing points messages from extended messages. If we start to see +# tolerance violations at this level, we could consider increasing it. +ERROR_TOLERANCE_METERS = 250e-6 + +# The amplitude portion of the extended message is encoded as a floating point +# value that needs to be scaled before converting to an integer type for Kalibr +# compatibility. The scaling factor is somewhat arbitrary, and Oleg verified +# this value works empirically, although it may not be optimal. +AMPLITUDE_SCALE = 100 + + +def get_ext_topic(cam): + return EXT_TOPIC_TMPL.format(cam=cam) + + +def get_pts_topic(cam): + return PTS_TOPIC_TMPL.format(cam=cam) + + +def get_amp_topic(cam): + return AMP_TOPIC_TMPL.format(cam=cam) + + +def rms(v): + return np.sqrt(np.mean(v * v)) + + +class PrintEveryK: + """ + Acts a bit like a logger, but prints the message only one out of + every k times you call it. Convenient to use with large k for + occasionally indicating progress. + """ + + def __init__(self, k): + self.k = k + self.count = 0 + + def debug(self, *args, **kwargs): + self.count += 1 + if (self.count % self.k) == 0: + logging.debug(*args, **kwargs) + + def info(self, *args, **kwargs): + self.count += 1 + if (self.count % self.k) == 0: + logging.info(*args, **kwargs) + + +def fast_filter(msgs): + """ + Grabs an arbitrary subset of 100 messages from the stream. + Convenient for fast testing. + """ + return itertools.islice(msgs, 100, 1100, 10) + + +def get_msg_tuples_internal(inbag_path, topic): + with rosbag.Bag(inbag_path, "r") as inbag: + for msg_tuple in inbag.read_messages([topic]): + yield msg_tuple + + +def get_msg_tuples(inbag_path, topic, fast=False): + """ + A generator that iterates through message tuples (topic, msg, t) + in the ROS bag @inbag_path on topic @topic. If @fast is True, + only returns a subset of the bag for fast testing. + """ + msg_tuples = get_msg_tuples_internal(inbag_path, topic) + if fast: + return fast_filter(msg_tuples) + return msg_tuples + + +def get_msgs(inbag_path, topic, fast=False): + """ + A generator that iterates through messages in the ROS bag + @inbag_path on topic @topic. If @fast is True, only returns a subset + of the bag for fast testing. + """ + return (msg_tuple[1] for msg_tuple in get_msg_tuples(inbag_path, topic, fast)) + + +def dt64(t): + """ + Convert rospy.Time to np.datetime64. + """ + return np.datetime64(t.to_nsec(), "ns") + + +def peek(iterable): + """ + Python generators don't provide a peek() method to examine the first + item in the sequence without actually popping it off. This function + provides similar functionality if you call it like this: + + first, it = peek(it) + + After the call, @first will still be the first item in @it. + """ + try: + first = next(iterable) + except StopIteration: + return None, None + return first, itertools.chain([first], iterable) + + +def split_msg_stream_by_topic(in_stream, topics): + """ + Given @in_stream a message tuple generator and @topics a sequence of + topics [topic_1, .., topic_n], returns a sequence of message tuple + generators [stream_1, .., stream_n] such that stream_i generates + the message tuples from @in_stream that match topic_i. + """ + it = iter(in_stream) + deques = [collections.deque() for i in range(len(topics))] + deque_by_topic = dict(zip(topics, deques)) + + def gen(deq): + while 1: + while not deq: + try: + newval = next(it) + except StopIteration: + return + msg_topic, msg, t = newval + # logging.debug("got message topic %s", msg_topic) + deque_by_topic[msg_topic].append(newval) + # logging.debug("propagated message topic %s", topic) + yield deq.popleft() + + return tuple(gen(deq) for deq in deques) + + +def get_matched_msg_pairs(stream1, stream2, dt_limit=None): + """ + Given @stream1 and @stream2 message tuple generators, returns + matched pairs of messages from the two streams. The behavior is very + similar to itertools.izip() followed by selecting just the message + part of the (topic, message, t) tuple. + + However, if @dt_limit is specified, instead of simply matching + message pairs that have the same index in their respective + sequences, it also calculates their publish timestamp differential: + + dt = msg2.header.stamp - msg1.header.stamp + + Two messages are potential matches if dt falls within the specified + interval @dt_limit = (dt_min, dt_max). Subject to that constraint, + the pairing is greedy, and each message can be used at most once. + If a particular message has no valid match based on dt, it will be + dropped with a debug message. + + Using @dt_limit can make the matching more robust to occasional out + of order rosbag logging. + """ + while 1: + val1, stream1 = peek(stream1) + val2, stream2 = peek(stream2) + if val1 is None or val2 is None: + logging.debug( + "get_matched_msg_pairs: ran out of pairs, stream1 %s, stream2 %s", + "done" if val1 is None else "not done", + "done" if val2 is None else "not done", + ) + return + topic1, msg1, t1 = val1 + topic2, msg2, t2 = val2 + if dt_limit is not None: + dt_min, dt_max = dt_limit + dt = (msg2.header.stamp - msg1.header.stamp).to_sec() + if not (dt_min <= dt): + # need a newer t2 to make dt large enough: drop t2 + logging.debug( + "get_matched_msg_pairs: dropping unmatched message from stream1, dt = %.3f, at %s", + dt, + dt64(t1), + ) + next(stream2) + continue + if not (dt <= dt_max): + # need a newer t1 to make dt small enough: drop t1 + logging.debug( + "get_matched_msg_pairs: dropping unmatched message from stream2, dt = %.3f, at %s", + dt, + dt64(t2), + ) + next(stream1) + continue + yield msg1, msg2 + next(stream1) + next(stream2) + + +def get_ext_pts_pairs_internal(inbag_path, cam): + topics = [get_ext_topic(cam), get_pts_topic(cam)] + with rosbag.Bag(inbag_path, "r") as inbag: + msg_stream = inbag.read_messages(topics) + ext_stream, pts_stream = split_msg_stream_by_topic(msg_stream, topics) + # note: we can't just return the generator here because returning from this + # function would close the bag and render the generator invalid. + for pair in get_matched_msg_pairs( + ext_stream, pts_stream, dt_limit=PTS_MINUS_EXT_DT_LIMIT + ): + yield pair + + +def get_ext_pts_pairs(inbag_path, fast=False, cam=DEFAULT_CAM): + """ + Given @inbag_path a path to an input bag, returns a generator that + will produce matched message pairs (ext_msg, pts_msg) from the bag + on the "extended" and "points" topics, respectively. + + The message matching attempts to guarantee the matched messages are + from the same sensor frame by comparing their publish timestamps, + which is empirically more robust than relying on the sequence + ordering in the bag. + """ + pairs = get_ext_pts_pairs_internal(inbag_path, cam=cam) + if fast: + return fast_filter(pairs) + return pairs + + +def xyz_from_points(pts): + """ + Extract xyz points from a "points" message. Replaces zero entries + with np.nan to explicitly mark them as invalid. + """ + pc = pc2.read_points(pts, field_names=("x", "y", "z")) + xyz = np.array(list(pc)) + + # mark rows that have z == 0 as nan + xyz[xyz[:, 2] == 0, :] = np.nan + + return xyz + + +def merge_point_clouds(pts_stream): + """ + Given @pts_stream a stream of points messages, merges the xyz points + from all messages into a single xyz frame. + + All points messages have the same length of xyz array, and each + index within the array consistently corresponds to the same pixel of + the sensor. + + The merge semantics produces an output xyz array where the value at + each pixel is the mean value over all of the messages that have a + valid value for that pixel (not nan). + """ + xyz_sum = np.zeros( + (SENSOR_HEIGHT_PIXELS * SENSOR_WIDTH_PIXELS, 3), dtype=np.float64 + ) + xyz_count = np.zeros((SENSOR_HEIGHT_PIXELS * SENSOR_WIDTH_PIXELS,), dtype=np.uint32) + for i, pts in enumerate(pts_stream): + xyz = xyz_from_points(pts) + valid = ~np.isnan(xyz[:, 0]) + xyz_sum[valid] += xyz[valid] + xyz_count[valid] += 1 + + merged = np.empty((SENSOR_HEIGHT_PIXELS * SENSOR_WIDTH_PIXELS, 3), dtype=np.float64) + merged.fill(np.nan) + valid = xyz_count != 0 + merged[valid] = xyz_sum[valid] / xyz_count[valid, np.newaxis] + + logging.info("merge_point_clouds: processed %s point clouds", i + 1) + num_invalid_pixels = np.count_nonzero(xyz_count == 0) + logging.info( + "merge_point_clouds: invalid pixels: %d (%.1f%%)", + num_invalid_pixels, + float(num_invalid_pixels) / (SENSOR_HEIGHT_PIXELS * SENSOR_WIDTH_PIXELS) * 100, + ) + return merged + + +def split_extended(ext_msg): + bridge = CvBridge() + cv_image = bridge.imgmsg_to_cv2(ext_msg.raw, desired_encoding="32FC4") + distance, amplitude, intensity, noise = cv2.split(cv_image) + + # explicitly mark no-data value of 0 as nan + distance[distance == 0] = [np.nan] + + return distance, amplitude, intensity, noise + + +def get_xyz_coeff(inbag_path, fast=False, cam=DEFAULT_CAM): + xyz = merge_point_clouds(get_msgs(inbag_path, get_pts_topic(cam), fast=fast)) + xyz = xyz / np.linalg.norm(xyz, axis=1)[:, np.newaxis] + return xyz + + +def xyz_from_distance(distance, xyz_coeff): + return ( + xyz_coeff + * distance.reshape((SENSOR_HEIGHT_PIXELS * SENSOR_WIDTH_PIXELS,))[:, np.newaxis] + ) + + +def xyz_from_extended(ext_msg, xyz_coeff): + distance, amplitude, intensity, noise = split_extended(ext_msg) + return xyz_from_distance(distance, xyz_coeff) + + +def make_amp_msg(header, amplitude): + msg = Image() + msg.header = header + msg.height = SENSOR_HEIGHT_PIXELS + msg.width = SENSOR_WIDTH_PIXELS + msg.encoding = "mono16" + msg.is_bigendian = False + msg.step = SENSOR_WIDTH_PIXELS * 2 + + # simulate OpenCV convertTo() operation used in pico_proxy + max16 = (1 << 16) - 1 + amp16 = np.clip(np.rint(amplitude * AMPLITUDE_SCALE), 0, max16).astype(np.uint16) + + msg.data = amp16.tobytes() + + return msg + + +def make_points_msg(header, xyz): + pts = PointCloud2() + pts.header = header + pts.height = SENSOR_HEIGHT_PIXELS + pts.width = SENSOR_WIDTH_PIXELS + pts.fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + ] + pts.is_bigendian = False + pts.point_step = 20 + pts.row_step = 4480 + + xyz32 = xyz.astype(np.float32) + xyz32[np.isnan(xyz32)] = 0.0 + buf = io.BytesIO() + for row in xyz32: + # write xyz values (12 bytes) + buf.write(struct.pack(" 100: + logging.info("FAILED at index %d: %s values differ", i, field_name) + else: + logging.info("FAILED at index %d: %s %s != %s", i, field_name, a, b) + return False + + +def check_xyz_error(xyz1, xyz2, i): + success = True + + # the same entries should be valid in both point clouds + valid_diff = np.isnan(xyz1) ^ np.isnan(xyz2) + if np.count_nonzero(valid_diff): + logging.info("FAILED at index %d: messages have different valid pixels", i) + success = False + + # check accuracy in valid areas + valid = ~np.isnan(xyz1) + err = xyz2[valid] - xyz1[valid] + err_max = np.max(np.abs(err)) + err_rms = rms(err) + + logging.debug("%5d Error max (um): %.3f", i, err_max * 1e6) + logging.debug("%5d Error RMS (um): %.3f", i, err_rms * 1e6) + if err_max > ERROR_TOLERANCE_METERS: + logging.info("FAILED at index %d: Error max (um) = %.3f", i, err_max * 1e6) + success = False + + return success, err_max, err_rms + + +def check_points_msg_pair(orig_msg, test_msg, i, verbose=False): + success = True + + success &= check_equal(orig_msg.height, test_msg.height, i, "height") + success &= check_equal(orig_msg.width, test_msg.width, i, "width") + success &= check_equal(orig_msg.fields, test_msg.fields, i, "fields") + success &= check_equal( + orig_msg.is_bigendian, test_msg.is_bigendian, i, "is_bigendian" + ) + success &= check_equal(orig_msg.point_step, test_msg.point_step, i, "point_step") + success &= check_equal(orig_msg.row_step, test_msg.row_step, i, "row_step") + success &= check_equal(orig_msg.is_dense, test_msg.is_dense, i, "is_dense") + + orig_xyz = xyz_from_points(orig_msg) + test_xyz = xyz_from_points(test_msg) + + xyz_success, err_max, err_rms = check_xyz_error(orig_xyz, test_xyz, i) + success &= xyz_success + + return success, err_max, err_rms + + +def check_amp_msg_pair(orig_msg, test_msg, i, verbose=False): + success = True + + success &= check_equal(orig_msg.height, test_msg.height, i, "height") + success &= check_equal(orig_msg.width, test_msg.width, i, "width") + success &= check_equal( + orig_msg.is_bigendian, test_msg.is_bigendian, i, "is_bigendian" + ) + success &= check_equal(orig_msg.step, test_msg.step, i, "step") + success &= check_equal(orig_msg.data, test_msg.data, i, "data") + + return success diff --git a/hardware/pico_driver/scripts/pico_write_xyz_coeff.py b/hardware/pico_driver/scripts/pico_write_xyz_coeff.py new file mode 100755 index 0000000000..c7416fc580 --- /dev/null +++ b/hardware/pico_driver/scripts/pico_write_xyz_coeff.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Given a bag containing Pico Flexx points messages, output a *.npy file +containing array xyz coefficients that can subsequently be used to +reconstruct point clouds from Pico Flexx extended messages. +""" + +import argparse +import logging + +import numpy as np + +import pico_utils as pico + + +def pico_write_xyz_coeff( + in_bag_path, out_npy_path, fast=False, verbose=False, cam=pico.DEFAULT_CAM +): + xyz_coeff = pico.get_xyz_coeff(in_bag_path, fast=fast, cam=cam) + with open(out_npy_path, "wb") as out_npy: + np.save(out_npy, xyz_coeff, allow_pickle=False) + logging.info("wrote pico xyz coefficients to %s", out_npy_path) + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "-v", + "--verbose", + help="print more verbose debug info", + default=False, + action="store_true", + ) + parser.add_argument( + "-f", + "--fast", + help="speed up testing by only processing part of the bag", + default=False, + action="store_true", + ) + parser.add_argument( + "-c", + "--cam", + help="specify camera for rosbag topic filtering [%(default)s]", + nargs="?", + choices=pico.CAM_CHOICES, + default=pico.DEFAULT_CAM, + ) + parser.add_argument("in_bag", help="input bag containing points messages") + parser.add_argument("out_npy", help="output npy file of xyz coefficients") + + args = parser.parse_args() + level = logging.DEBUG if args.verbose else logging.INFO + logging.basicConfig(level=level, format="%(message)s") + + pico_write_xyz_coeff( + args.in_bag, args.out_npy, fast=args.fast, verbose=args.verbose, cam=args.cam + ) + + # suppress confusing ROS message at exit + logging.getLogger().setLevel(logging.WARN) + + +if __name__ == "__main__": + main() diff --git a/hardware/pico_driver/src/pico_driver.cc b/hardware/pico_driver/src/pico_driver.cc index f2319d3aef..a0cc01f505 100644 --- a/hardware/pico_driver/src/pico_driver.cc +++ b/hardware/pico_driver/src/pico_driver.cc @@ -22,7 +22,7 @@ #include // Shared libraries -#include +#include #include #include @@ -231,23 +231,46 @@ class PicoDriverL1 : public PicoDriver, public royale::IDepthDataListener, publi cloud_.point_step = sizeof(struct royale::DepthPoint); cloud_.row_step = cloud_.width * cloud_.point_step; cloud_.data.resize(cloud_.row_step * cloud_.height); - // X, Y and Z + + // Fill metadata about available fields in point cloud sensor_msgs::PointField field; + field.name = "x"; field.offset = offsetof(struct royale::DepthPoint, x); field.datatype = sensor_msgs::PointField::FLOAT32; field.count = 1; // Number of ELEMENTS, not bytes! cloud_.fields.push_back(field); + field.name = "y"; field.offset = offsetof(struct royale::DepthPoint, y); field.datatype = sensor_msgs::PointField::FLOAT32; field.count = 1; // Number of ELEMENTS, not bytes! cloud_.fields.push_back(field); + field.name = "z"; field.offset = offsetof(struct royale::DepthPoint, z); field.datatype = sensor_msgs::PointField::FLOAT32; field.count = 1; // Number of ELEMENTS, not bytes! cloud_.fields.push_back(field); + + field.name = "noise"; + field.offset = offsetof(struct royale::DepthPoint, noise); + field.datatype = sensor_msgs::PointField::FLOAT32; + field.count = 1; // Number of ELEMENTS, not bytes! + cloud_.fields.push_back(field); + + field.name = "grayValue"; + field.offset = offsetof(struct royale::DepthPoint, grayValue); + field.datatype = sensor_msgs::PointField::UINT16; + field.count = 1; // Number of ELEMENTS, not bytes! + cloud_.fields.push_back(field); + + field.name = "depthConfidence"; + field.offset = offsetof(struct royale::DepthPoint, depthConfidence); + field.datatype = sensor_msgs::PointField::UINT8; + field.count = 1; // Number of ELEMENTS, not bytes! + cloud_.fields.push_back(field); + // Generate a nice readable name for the camera std::string topic_name_c = (std::string) TOPIC_HARDWARE_PICOFLEXX_PREFIX + (std::string) topic @@ -306,7 +329,7 @@ class PicoDriverL1 : public PicoDriver, public royale::IDepthDataListener, publi } // If we have depth data, use the same mechanism as L1 to push it if (pub_cloud_.getNumSubscribers() > 0) { - cloud_.header.stamp = ros::Time::now(); + cloud_.header.stamp.fromNSec(std::chrono::duration_cast(data->timeStamp).count()); std::copy( reinterpret_cast(data->points.data()), reinterpret_cast(data->points.data()) + cloud_.row_step * cloud_.height, @@ -328,7 +351,9 @@ class PicoDriverL1 : public PicoDriver, public royale::IDepthDataListener, publi } // If we have depth data, use the same mechanism as L1 to push it if (pub_depth_image_.getNumSubscribers() > 0) { - depth_image_.header.stamp = ros::Time::now(); + // units not documented in DepthImage.hpp, maybe usecs like DepthData? + uint64_t stampUsecs = data->timestamp; + depth_image_.header.stamp.fromNSec(stampUsecs * 1000); std::copy( reinterpret_cast(data->data.data()), reinterpret_cast(data->data.data()) + depth_image_.height * depth_image_.step, @@ -430,10 +455,21 @@ class PicoDriverL2 : public PicoDriver, public royale::IExtendedDataListener { ROS_WARN("data pointer = nullptr"); return; } + + ros::Time commonStamp(0, 0); + if (data->hasDepthData() && data->getDepthData() != nullptr) { + commonStamp.fromNSec( + std::chrono::duration_cast(data->getDepthData()->timeStamp).count()); + } else if (data->hasIntermediateData() + && data->getIntermediateData() != nullptr) { + commonStamp.fromNSec( + std::chrono::duration_cast(data->getIntermediateData()->timeStamp).count()); + } + // If we have depth data, use the same mechanism as L1 to push it if (data->hasDepthData() && pub_cloud_.getNumSubscribers() > 0 && data->getDepthData() != nullptr) { - cloud_.header.stamp = ros::Time::now(); + cloud_.header.stamp = commonStamp; std::copy( reinterpret_cast(data->getDepthData()->points.data()), reinterpret_cast(data->getDepthData()->points.data()) + cloud_.row_step * cloud_.height, @@ -443,7 +479,7 @@ class PicoDriverL2 : public PicoDriver, public royale::IExtendedDataListener { // If we have a listener and the extended data contains intermediate data, publish it if (data->hasIntermediateData() && pub_extended_.getNumSubscribers() > 0 && data->getIntermediateData() != nullptr) { - extended_.header.stamp = ros::Time::now(); + extended_.header.stamp = commonStamp; // Populate the modulation frequencies and exposures used to produce this data extended_.frequency.resize(data->getIntermediateData()->modulationFrequencies.size()); for (size_t i = 0; i < data->getIntermediateData()->modulationFrequencies.size(); i++) diff --git a/hardware/pico_driver/src/pico_proxy.cc b/hardware/pico_driver/src/pico_proxy.cc new file mode 100644 index 0000000000..a5fa76f64e --- /dev/null +++ b/hardware/pico_driver/src/pico_proxy.cc @@ -0,0 +1,244 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Core ROS +#include +#include +#include + +// Shared libraries +#include +#include +#include + +#include +#include +#include +#include +#include + +// Messages +#include +#include + +#include +#include + +/** + * \ingroup hardware + */ +namespace pico_proxy { + +class PicoProxyNodelet : public ff_util::FreeFlyerNodelet { + public: + PicoProxyNodelet() : ff_util::FreeFlyerNodelet(false) {} + ~PicoProxyNodelet() {} + + protected: + void Initialize(ros::NodeHandle *nh) { + std::string topic, topic_type; + topic = TOPIC_HARDWARE_PICOFLEXX_PREFIX; + config_name_ = GetName(); + // Read the camera name + if (config_name_.find("haz") != std::string::npos) { + topic += TOPIC_HARDWARE_NAME_HAZ_CAM; + } else if (config_name_.find("perch") != std::string::npos) { + topic += TOPIC_HARDWARE_NAME_PERCH_CAM; + } else { + NODELET_FATAL("You need to pass a camera name to the pico proxy nodelet name"); + } + + // Read the topic type + if (config_name_.find("extended") != std::string::npos) { + topic += TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED; + + // Read config file for amplitude factor + config_.AddFile("cameras.config"); + if (!ReadParams()) + return; + + pub_d_ = nh->advertise(topic + "/distance/", 1); + pub_a_ = nh->advertise(topic + "/amplitude/", 1); + pub_i_ = nh->advertise(topic + "/intensity/", 1); + pub_c_ = nh->advertise(topic + "/noise/", 1); + pub_a_int_ = nh->advertise(topic + "/amplitude_int/", 1); + sub_ = nh->subscribe(topic, 1, &PicoProxyNodelet::ExtendedCallback, this); + + } else if (config_name_.find("depth_image") != std::string::npos) { + topic += TOPIC_HARDWARE_PICOFLEXX_SUFFIX_DEPTH_IMAGE; + + pub_d_ = nh->advertise(topic + "/distance", 1); + pub_c_ = nh->advertise(topic + "/confidence", 1); + sub_ = nh->subscribe(topic, 1, &PicoProxyNodelet::DepthImageCallback, this); + + } else { + NODELET_FATAL("The pico proxy nodelet name must have extended or depth_image"); + } + } + + bool ReadParams(void) { + // Read the config file + if (!config_.ReadFiles()) { + NODELET_FATAL("Failed to read config files."); + return false; + } + // Try and open the config file + config_reader::ConfigReader::Table pconfig; + if (!config_.GetTable("picoflexx", &pconfig)) { + NODELET_FATAL("Picoflexx config not found in LUA."); + return false; + } + // Read the device information from the config table + config_reader::ConfigReader::Table devices; + if (!pconfig.GetTable("devices", &devices)) { + NODELET_FATAL("Lua:Could get devices item in config file"); + } + + for (int i = 0; i < devices.GetSize(); i++) { + // Get the device info + config_reader::ConfigReader::Table device_info; + if (!devices.GetTable(i + 1, &device_info)) { + NODELET_FATAL("Lua:Could get row in table table"); + continue; + } + + // Get the parameters + std::string name; + if (!device_info.GetStr("name", &name)) { + NODELET_FATAL("Lua:Could not find row 'name' in table"); + continue; + } + + if (config_name_.find(name) == std::string::npos) + continue; + + // Query the amplitude factor + if (device_info.GetReal("amplitude_factor", &litude_factor_)) + return true; + } + NODELET_FATAL("You need to pass the amplitude factor to camera parameters"); + return false; + } + + struct null_deleter { + void operator()(void const *) const {} + }; + + // Called when extended data arrives + void ExtendedCallback(const ff_msgs::PicoflexxIntermediateData::ConstPtr& msg) { + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare( + sensor_msgs::ImageConstPtr(&msg->raw, null_deleter()), + sensor_msgs::image_encodings::TYPE_32FC4); + cv::split(cv_ptr->image, layers_); + + // Keep the same timestamp as the original data + std_msgs::Header header; + header.stamp = msg->header.stamp; + header.frame_id = msg->header.frame_id; + + cv_bridge::CvImage d(header, sensor_msgs::image_encodings::TYPE_32FC1, layers_[0]); + cv_bridge::CvImage a(header, sensor_msgs::image_encodings::TYPE_32FC1, layers_[1]); + cv_bridge::CvImage i(header, sensor_msgs::image_encodings::TYPE_32FC1, layers_[2]); + cv_bridge::CvImage n(header, sensor_msgs::image_encodings::TYPE_32FC1, layers_[3]); + + // Kalibr cannot handle float images. Hence, we need to create an + // integer version of the amplitude topic (while keeping the + // original, as it is used in other contexts). + + // TODO(oalexan1): I found out that kalibr cannot handle a float + // amplitude. Hence, here it is multiplied by amplitude_factor and + // cast to uint16. The best experimental value for amplitude_factor + // turned out to be 100, but this may need further + // experimentation. Casting to uint8 was not enough. This will need + // a deeper study. If the amplitude image looks too saturated or too + // dark, a different amplitude factor can be set when launching this. + layers_[1].convertTo(img_int_, CV_16UC1, amplitude_factor_, 0); + cv_bridge::CvImage a_int(header, sensor_msgs::image_encodings::MONO16, img_int_); + + // Publish individual images + pub_d_.publish(d.toImageMsg()); + pub_a_.publish(a.toImageMsg()); + pub_i_.publish(i.toImageMsg()); + pub_c_.publish(n.toImageMsg()); + + pub_a_int_.publish(a_int.toImageMsg()); + } + + // Called when depth image data arrives + void DepthImageCallback(const sensor_msgs::ImageConstPtr& msg) { + // Prepare distance image + distance_.header.stamp = msg->header.stamp; + distance_.header.frame_id = msg->header.frame_id; + distance_.height = msg->height; + distance_.width = msg->width; + distance_.is_bigendian = msg->is_bigendian; + distance_.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + distance_.step = distance_.width * sizeof(uint16_t); + distance_.data.resize(distance_.height * distance_.step); + // Prepare confidence image + confidence_.header.stamp = msg->header.stamp; + confidence_.header.frame_id = msg->header.frame_id; + confidence_.height = msg->height; + confidence_.width = msg->width; + confidence_.is_bigendian = msg->is_bigendian; + confidence_.encoding = sensor_msgs::image_encodings::TYPE_8UC1; + confidence_.step = confidence_.width * sizeof(uint8_t); + confidence_.data.resize(confidence_.height * confidence_.step); + // Set pointers to the data fields of the corresponding ROS images + uint16_t *msg_ptr = const_cast( + reinterpret_cast(&msg->data[0])); + uint16_t *dist_ptr = reinterpret_cast(&distance_.data[0]); + uint8_t *conf_ptr = reinterpret_cast(&confidence_.data[0]); + // Iterate through the image found in msg + for (size_t i = 0; i < (distance_.width * distance_.height); + ++i, ++msg_ptr, ++dist_ptr, ++conf_ptr) { + *dist_ptr = *msg_ptr & 0x1fff; // Grab the distance + *conf_ptr = (uint8_t)((*msg_ptr & 0xE000) >> 13); // Grab the confidence + } + pub_d_.publish(distance_); + pub_c_.publish(confidence_); + } + + private: + // Configs + config_reader::ConfigReader config_; + std::string config_name_; + + // Publishers + ros::Publisher pub_d_; + ros::Publisher pub_a_; + ros::Publisher pub_i_; + ros::Publisher pub_c_; + // Subscriber + ros::Subscriber sub_; + + ros::Publisher pub_a_int_; + + // Layer extraction + std::vector layers_ = std::vector(4); + cv::Mat img_int_; + + // Depth and confidence matrices + sensor_msgs::Image distance_, confidence_; + + double amplitude_factor_; +}; + +PLUGINLIB_EXPORT_CLASS(pico_proxy::PicoProxyNodelet, nodelet::Nodelet); + +} // namespace pico_proxy diff --git a/management/sys_monitor/test/test_init_pico_flexx.cc b/hardware/pico_driver/test/test_init_pico_flexx.cc similarity index 89% rename from management/sys_monitor/test/test_init_pico_flexx.cc rename to hardware/pico_driver/test/test_init_pico_flexx.cc index c8ccd32b6e..9a166987fc 100644 --- a/management/sys_monitor/test/test_init_pico_flexx.cc +++ b/hardware/pico_driver/test/test_init_pico_flexx.cc @@ -20,7 +20,7 @@ // Required for the test cases #include -#include +#include // Required for the test framework #include @@ -28,6 +28,15 @@ #include +// Test initialization pico flexx +// These initialization tests make sure the node is spawned. To do so, +// it spawns the node and the system manager. The node uses the wrong folder +// for the config files on purpose, which it will make it unable to read the +// config files. Because the node can't read the config files it triggers the +// initialization anomaly to the system monitor. Is this anomaly is issued +// within 2 seconds, then the test passes. + + bool test_done = false; ff_msgs::FaultStateConstPtr fault_state; int init_fault_id = -1; diff --git a/management/sys_monitor/test/test_init_pico_flexx.test b/hardware/pico_driver/test/test_init_pico_flexx.test similarity index 96% rename from management/sys_monitor/test/test_init_pico_flexx.test rename to hardware/pico_driver/test/test_init_pico_flexx.test index 75f19e6058..ed444de386 100644 --- a/management/sys_monitor/test/test_init_pico_flexx.test +++ b/hardware/pico_driver/test/test_init_pico_flexx.test @@ -28,5 +28,5 @@ - + diff --git a/hardware/pico_driver/tools/pico_proxy.cc b/hardware/pico_driver/tools/pico_proxy.cc deleted file mode 100644 index 9fa1c0a6c1..0000000000 --- a/hardware/pico_driver/tools/pico_proxy.cc +++ /dev/null @@ -1,163 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -// Publishers -ros::Publisher pub_d_; -ros::Publisher pub_a_; -ros::Publisher pub_i_; -ros::Publisher pub_c_; - -ros::Publisher pub_a_int_; - -// Layer extraction -std::vector layers(4); -cv::Mat img_int; - -// Depth and confidence matrices -sensor_msgs::Image distance_, confidence_; - -double amplitude_factor; - -struct null_deleter { - void operator()(void const *) const {} -}; - -// Called when extended data arrives -void ExtendedCallback(const ff_msgs::PicoflexxIntermediateData::ConstPtr& msg) { - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare( - sensor_msgs::ImageConstPtr(&msg->raw, null_deleter()), - sensor_msgs::image_encodings::TYPE_32FC4); - cv::split(cv_ptr->image, layers); - - // Keep the same timestamp as the original data - std_msgs::Header header; - header.stamp = msg->header.stamp; - header.frame_id = msg->header.frame_id; - - cv_bridge::CvImage d(header, sensor_msgs::image_encodings::TYPE_32FC1, layers[0]); - cv_bridge::CvImage a(header, sensor_msgs::image_encodings::TYPE_32FC1, layers[1]); - cv_bridge::CvImage i(header, sensor_msgs::image_encodings::TYPE_32FC1, layers[2]); - cv_bridge::CvImage n(header, sensor_msgs::image_encodings::TYPE_32FC1, layers[3]); - - // Kalibr cannot handle float images. Hence, we need to create an - // integer version of the amplitude topic (while keeping the - // original, as it is used in other contexts). - - // TODO(oalexan1): I found out that kalibr cannot handle a float - // amplitude. Hence, here it is multiplied by amplitude_factor and - // cast to uint16. The best experimental value for amplitude_factor - // turned out to be 100, but this may need further - // experimentation. Casting to uint8 was not enough. This will need - // a deeper study. If the amplitude image looks too saturated or too - // dark, a different amplitude factor can be set when launching this. - layers[1].convertTo(img_int, CV_16UC1, amplitude_factor, 0); - cv_bridge::CvImage a_int(header, sensor_msgs::image_encodings::MONO16, img_int); - - // Publish individual images - pub_d_.publish(d.toImageMsg()); - pub_a_.publish(a.toImageMsg()); - pub_i_.publish(i.toImageMsg()); - pub_c_.publish(n.toImageMsg()); - - pub_a_int_.publish(a_int.toImageMsg()); -} - -// Called when depth image data arrives -void DepthImageCallback(const sensor_msgs::ImageConstPtr& msg) { - // Prepare distance image - // TODO(oalexan1): keep same timestamp as the input? - distance_.header.stamp = ros::Time::now(); - distance_.header.frame_id = msg->header.frame_id; - distance_.height = msg->height; - distance_.width = msg->width; - distance_.is_bigendian = msg->is_bigendian; - distance_.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - distance_.step = distance_.width * sizeof(uint16_t); - distance_.data.resize(distance_.height * distance_.step); - // Prepare confidence image - // TODO(oalexan1): keep same timestamp as the input? - confidence_.header.stamp = ros::Time::now(); - confidence_.header.frame_id = msg->header.frame_id; - confidence_.height = msg->height; - confidence_.width = msg->width; - confidence_.is_bigendian = msg->is_bigendian; - confidence_.encoding = sensor_msgs::image_encodings::TYPE_8UC1; - confidence_.step = confidence_.width * sizeof(uint8_t); - confidence_.data.resize(confidence_.height * confidence_.step); - // Set pointers to the data fields of the corresponding ROS images - uint16_t *msg_ptr = const_cast( - reinterpret_cast(&msg->data[0])); - uint16_t *dist_ptr = reinterpret_cast(&distance_.data[0]); - uint8_t *conf_ptr = reinterpret_cast(&confidence_.data[0]); - // Iterate through the image found in msg - for (size_t i = 0; i < (distance_.width * distance_.height); - ++i, ++msg_ptr, ++dist_ptr, ++conf_ptr) { - *dist_ptr = *msg_ptr & 0x1fff; // Grab the distance - *conf_ptr = (uint8_t)((*msg_ptr & 0xE000) >> 13); // Grab the confidence - } - pub_d_.publish(distance_); - pub_c_.publish(confidence_); -} - -// Main entry point of application -int main(int argc, char **argv) { - ros::init(argc, argv, "listener"); - ros::NodeHandle n("~"); - if (!n.hasParam("topic")) - ROS_FATAL("You need to pass a topic to the pico proxy"); - if (!n.hasParam("topic_type")) - ROS_FATAL("You need to pass a topic type to the pico proxy"); - if (!n.hasParam("amplitude_factor")) - ROS_FATAL("You need to pass the amplitude factor to the pico proxy"); - std::string topic, topic_type; - n.getParam("topic", topic); - n.getParam("topic_type", topic_type); - n.getParam("amplitude_factor", amplitude_factor); - ROS_INFO_STREAM("Listening on topic " << topic); - ROS_INFO_STREAM("Using amplitude factor " << amplitude_factor); - ros::Subscriber sub; - if (topic_type == "extended") { - pub_d_ = n.advertise(topic + "/distance/", 1); - pub_a_ = n.advertise(topic + "/amplitude/", 1); - pub_i_ = n.advertise(topic + "/intensity/", 1); - pub_c_ = n.advertise(topic + "/noise/", 1); - pub_a_int_ = n.advertise(topic + "/amplitude_int/", 1); - sub = n.subscribe(topic, 1, ExtendedCallback); - } else if (topic_type == "depth_image") { - pub_d_ = n.advertise(topic + "/distance", 1); - pub_c_ = n.advertise(topic + "/confidence", 1); - sub = n.subscribe(topic, 1, DepthImageCallback); - } else { - ROS_FATAL("Unsupported type (must be \"extended\" or \"depth_image\")"); - } - ros::spin(); - return 0; -} diff --git a/hardware/pmc_actuator/CMakeLists.txt b/hardware/pmc_actuator/CMakeLists.txt index 823883697b..438e4e1ed4 100644 --- a/hardware/pmc_actuator/CMakeLists.txt +++ b/hardware/pmc_actuator/CMakeLists.txt @@ -15,8 +15,24 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(pmc_actuator) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + pluginlib + ff_util + std_msgs + ff_msgs + ff_hw_msgs + i2c +) + catkin_package( LIBRARIES pmc_actuator @@ -28,36 +44,103 @@ catkin_package( std_msgs ff_msgs ff_hw_msgs + i2c ) -# ROS agnostic driver and tool +########### +## Build ## +########### -create_library( - DIR src/pmc_actuator - TARGET pmc_actuator - LIBS i2c +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} ) -create_tool_targets(DIR tools/pmc_actuator_tool - LIBS pmc_actuator +# ROS agnostic driver and tool + +# Declare C++ libraries +add_library(pmc_actuator + src/pmc_actuator/pmc_actuator.cc ) +add_dependencies(pmc_actuator ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pmc_actuator ${catkin_LIBRARIES} ${EIGEN_LIBRARIES}) + +## Declare a C++ executable: bag_to_csv +add_executable(pmc_actuator_tool tools/pmc_actuator_tool/pmc_actuator_tool.cc) +add_dependencies(pmc_actuator_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pmc_actuator_tool + pmc_actuator gflags glog ${catkin_LIBRARIES}) # ROS nodelet and tools -create_library( - DIR src/pmc_actuator_nodelet - TARGET pmc_actuator_nodelet - LIBS ${catkin_LIBRARIES} pmc_actuator config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS ff_hw_msgs +# Declare C++ libraries +add_library(pmc_actuator_nodelet + src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc +) +add_dependencies(pmc_actuator_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pmc_actuator_nodelet pmc_actuator ${catkin_LIBRARIES} ${EIGEN_LIBRARIES}) + +## Declare a C++ executable: pmc_actuator_cmd_test +add_executable(pmc_actuator_cmd_test tools/pmc_actuator_cmd_test.cc) +add_dependencies(pmc_actuator_cmd_test ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pmc_actuator_cmd_test + pmc_actuator gflags glog ${catkin_LIBRARIES}) + +## Declare a C++ executable: pmc_actuator_feeder +add_executable(pmc_actuator_feeder tools/pmc_actuator_feeder.cc) +add_dependencies(pmc_actuator_feeder ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pmc_actuator_feeder + pmc_actuator gflags glog ${catkin_LIBRARIES}) + +## Declare a C++ executable: pmc_autotrim_node +add_executable(pmc_autotrim_node tools/pmc_autotrim_node.cc) +add_dependencies(pmc_autotrim_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pmc_autotrim_node + pmc_actuator gflags glog ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS ${PROJECT_NAME}_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) -create_tool_targets(DIR tools - LIBS ${catkin_LIBRARIES} pmc_actuator ff_nodelet - INC ${catkin_INCLUDE_DIRS} +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -install_launch_files() +# Install C++ executables +install(TARGETS pmc_actuator_tool DESTINATION bin) +install(TARGETS pmc_actuator_cmd_test DESTINATION bin) +install(TARGETS pmc_actuator_feeder DESTINATION bin) +install(TARGETS pmc_autotrim_node DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/pmc_actuator_tool share/${PROJECT_NAME} + COMMAND ln -s ../../bin/pmc_actuator_cmd_test share/${PROJECT_NAME} + COMMAND ln -s ../../bin/pmc_actuator_feeder share/${PROJECT_NAME} + COMMAND ln -s ../../bin/pmc_autotrim_node share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + install(FILES tools/pmc_currents.py tools/pmc_status.py diff --git a/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc b/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc index 2fb25e5648..aa3691684a 100644 --- a/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc +++ b/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc @@ -37,6 +37,7 @@ // Services #include +#include #include #include @@ -100,12 +101,16 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { // State publisher as a latched topic pub_state_ = nh->advertise( - TOPIC_HARDWARE_PMC_STATE, pub_queue_size_, true); + TOPIC_HARDWARE_PMC_STATE, pub_queue_size_); // PMC enable/disable service srv_ = nh->advertiseService( SERVICE_HARDWARE_PMC_ENABLE, &PmcActuatorNodelet::EnableService, this); + // Update PMC watchdog timer timeout + update_timeout_srv_ = nh->advertiseService( + SERVICE_HARDWARE_PMC_TIMEOUT, &PmcActuatorNodelet::IdlingTimeoutService, this); + // Watchdog timer timer_ = nh->createTimer( watchdog_period_, &PmcActuatorNodelet::TimerCallback, this, false, true); @@ -189,6 +194,12 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { return false; } + // get minimum allowed control rate + if (!config_params.GetPosReal("max_timeout", &max_timeout_)) { + ROS_FATAL("PMC Actuator: minimum control rate not specified!"); + return false; + } + // get initial fan speed if (!config_params.GetInt("null_speed", &null_fan_speed_)) { ROS_FATAL("PMC Actuator: null fan speed not specified!"); @@ -468,15 +479,33 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { return true; } + // Update Minimum Control Frequency (and cutoff time) + bool IdlingTimeoutService(ff_msgs::SetFloat::Request &req, + ff_msgs::SetFloat::Response &res) { // NOLINT + double new_timeout = req.data; + // Check if the new rate is within the safe and default limits + if (new_timeout <= max_timeout_ && new_timeout >= (20.0/control_rate_hz_)) { + watchdog_period_ = ros::Duration(new_timeout); + timer_.setPeriod(watchdog_period_); + ROS_INFO("PMC idling timeout updated."); + res.success = true; + } else { + ROS_INFO("Selected timeout is not within the safe timeout bounds."); + res.success = false; + } + return true; + } + private: config_reader::ConfigReader config_params; // LUA configuration reader ros::Subscriber sub_command_; // Command flight mode sub ros::Publisher pub_telemetry_, pub_state_; // Telemetry publisher ros::ServiceServer srv_; // Enable / disable service + ros::ServiceServer update_timeout_srv_; // Update minimum control rate service ros::Timer timer_; // Watchdog timer ros::Duration watchdog_period_; // Watchdog period ff_hw_msgs::PmcTelemetry telemetry_vector_; // Telemetry message - uint32_t num_pmcs_; // Number of PMCs to control + uint32_t num_pmcs_; // Number of PMCs to control int sub_queue_size_; // Subscriber queue size int pub_queue_size_; // Publisher queue size std::string frame_id_; // Frame ID @@ -484,6 +513,7 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { std::vector i2c_addrs_; // 7-bit I2C addresses int i2c_retries_; // Number of I2C bus retries double control_rate_hz_; // Control rate in Hz. + double max_timeout_; // Maximum idling timeout allowed int null_fan_speed_; // Initial fan speed. std::vector null_nozzle_positions_; // Initial nozzle positions uint8_t trims_[NUM_PMC][NUM_TRIM][NUM_NOZZLE]; // Trims for each nozzle diff --git a/hardware/pmc_actuator/tools/pmc_actuator_cmd_test.cc b/hardware/pmc_actuator/tools/pmc_actuator_cmd_test.cc index 666d96cfb8..5227bbc112 100644 --- a/hardware/pmc_actuator/tools/pmc_actuator_cmd_test.cc +++ b/hardware/pmc_actuator/tools/pmc_actuator_cmd_test.cc @@ -18,7 +18,7 @@ #include -#include +#include #include #include diff --git a/hardware/pmc_actuator/tools/pmc_actuator_feeder.cc b/hardware/pmc_actuator/tools/pmc_actuator_feeder.cc index d632be6360..19b8b88438 100644 --- a/hardware/pmc_actuator/tools/pmc_actuator_feeder.cc +++ b/hardware/pmc_actuator/tools/pmc_actuator_feeder.cc @@ -19,7 +19,7 @@ #include #include -#include +#include #include diff --git a/hardware/pmc_actuator/tools/pmc_actuator_tool/pmc_actuator_tool.cc b/hardware/pmc_actuator/tools/pmc_actuator_tool/pmc_actuator_tool.cc index 96576ddf0d..eb86a9dc9c 100644 --- a/hardware/pmc_actuator/tools/pmc_actuator_tool/pmc_actuator_tool.cc +++ b/hardware/pmc_actuator/tools/pmc_actuator_tool/pmc_actuator_tool.cc @@ -24,7 +24,7 @@ #include #include -// C++11 STL includes +// C++ STL includes #include #include #include diff --git a/hardware/pmc_actuator/tools/pmc_autotrim_node.cc b/hardware/pmc_actuator/tools/pmc_autotrim_node.cc index 5f59d9496f..f1f3a30bbf 100644 --- a/hardware/pmc_actuator/tools/pmc_autotrim_node.cc +++ b/hardware/pmc_actuator/tools/pmc_autotrim_node.cc @@ -31,7 +31,7 @@ #include // Flight software names -#include +#include // STL includes #include diff --git a/hardware/pmc_actuator/tools/pmc_currents.py b/hardware/pmc_actuator/tools/pmc_currents.py index ea44339774..6f364b411a 100755 --- a/hardware/pmc_actuator/tools/pmc_currents.py +++ b/hardware/pmc_actuator/tools/pmc_currents.py @@ -1,25 +1,27 @@ #!/usr/bin/python import rospy - from ff_hw_msgs.msg import EpsHousekeeping + def callback(data): - pmc_1 = float('nan') - pmc_2 = float('nan') - system = float('nan') + pmc_1 = float("nan") + pmc_2 = float("nan") + system = float("nan") for c in data.values: - if c.name.find("MOTOR1_I") >= 0 : + if c.name.find("MOTOR1_I") >= 0: pmc_1 = c.value - if c.name.find("MOTOR2_I") >= 0 : + if c.name.find("MOTOR2_I") >= 0: pmc_2 = c.value - if c.name.find("SYSTEM_I") >= 0 : + if c.name.find("SYSTEM_I") >= 0: system = c.value - print("system= %.3fA | pmc_1= %.3fA | pmc_2=%.3fA" % (system, pmc_1, pmc_2)) - + print(("system= %.3fA | pmc_1= %.3fA | pmc_2=%.3fA" % (system, pmc_1, pmc_2))) + + def listener(): - rospy.init_node('pmc_consumption', anonymous=True) + rospy.init_node("pmc_consumption", anonymous=True) rospy.Subscriber("/hw/eps/housekeeping", EpsHousekeeping, callback) rospy.spin() - -if __name__ == '__main__': + + +if __name__ == "__main__": listener() diff --git a/hardware/pmc_actuator/tools/pmc_status.py b/hardware/pmc_actuator/tools/pmc_status.py index e898852d3b..e7d05bac9c 100755 --- a/hardware/pmc_actuator/tools/pmc_status.py +++ b/hardware/pmc_actuator/tools/pmc_status.py @@ -1,8 +1,8 @@ #!/usr/bin/python import rospy - from ff_hw_msgs.msg import PmcTelemetry + def callback(data): # print("got new data!") index = 0 @@ -11,17 +11,32 @@ def callback(data): state = p.status_1 >> 2 & 0b11 bad_crc = p.status_1 >> 6 & 0b1 m_disabled = p.status_1 >> 5 & 0b1 - + speedcam_line = p.status_2 >> 1 & 0b1 - print("PMC[%d]: cmd_id=%03d m_speed=%03d m_cur=%02d mode=%d, state=%d bad_crc=%d m_dis=%d sc_line=%d" % - (index, p.command_id, p.motor_speed, p.motor_current, - mode, state, bad_crc, m_disabled, speedcam_line)) + print( + ( + "PMC[%d]: cmd_id=%03d m_speed=%03d m_cur=%02d mode=%d, state=%d bad_crc=%d m_dis=%d sc_line=%d" + % ( + index, + p.command_id, + p.motor_speed, + p.motor_current, + mode, + state, + bad_crc, + m_disabled, + speedcam_line, + ) + ) + ) index = index + 1 - + + def listener(): - rospy.init_node('pmc_status', anonymous=True) + rospy.init_node("pmc_status", anonymous=True) rospy.Subscriber("/hw/pmc/telemetry", PmcTelemetry, callback) rospy.spin() - -if __name__ == '__main__': + + +if __name__ == "__main__": listener() diff --git a/hardware/readme.md b/hardware/readme.md index c191b745db..5e7cc3021b 100644 --- a/hardware/readme.md +++ b/hardware/readme.md @@ -1,13 +1,23 @@ \page hw Hardware \subpage eps_driver + \subpage flashlight + \subpage laser + \subpage perching_arm + \subpage picoflexx + \subpage pmc_actuator + \subpage signal_lights + \subpage smart_dock + \subpage speed_cam + \subpage temp_monitor + \subpage vive diff --git a/hardware/signal_lights/CMakeLists.txt b/hardware/signal_lights/CMakeLists.txt index 7a6ba0ae3a..ce53bd52e6 100644 --- a/hardware/signal_lights/CMakeLists.txt +++ b/hardware/signal_lights/CMakeLists.txt @@ -15,8 +15,23 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(signal_lights) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + pluginlib + ff_util + std_msgs + ff_msgs + ff_hw_msgs +) + catkin_package( LIBRARIES signal_lights @@ -30,32 +45,80 @@ catkin_package( ff_hw_msgs ) -# ROS agnostic driver and tool -create_library( - DIR src/custom_i2c - TARGET custom_i2c +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(custom_i2c + src/custom_i2c/i2c.cc ) +add_dependencies(custom_i2c ${catkin_EXPORTED_TARGETS}) +target_link_libraries(custom_i2c ${catkin_LIBRARIES}) -create_library( - DIR src/signal_lights - TARGET signal_lights - LIBS custom_i2c +# Declare C++ libraries +add_library(signal_lights + src/signal_lights/signal_lights.cc ) +add_dependencies(signal_lights ${catkin_EXPORTED_TARGETS}) +target_link_libraries(signal_lights custom_i2c ${catkin_LIBRARIES}) -create_tool_targets( - DIR tools - LIBS signal_lights ff_common custom_i2c +# Declare C++ libraries +add_library(signal_lights_nodelet + src/signal_lights_node/signal_lights_nodelet.cc ) +add_dependencies(signal_lights_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(signal_lights_nodelet signal_lights ${catkin_LIBRARIES}) -# ROS nodelet and tools +## Declare a C++ executable: inspection_tool +add_executable(signal_lights_tool tools/signal_lights_tool.cc) +add_dependencies(signal_lights_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(signal_lights_tool + signal_lights gflags ${catkin_LIBRARIES}) -create_library( - DIR src/signal_lights_node - TARGET signal_lights_nodelet - LIBS ${catkin_LIBRARIES} signal_lights config_reader ff_nodelet custom_i2c - INC ${catkin_INCLUDE_DIRS} - DEPS ff_hw_msgs +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS custom_i2c + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS signal_lights + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS signal_lights_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) -install_launch_files() +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS signal_lights_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/signal_lights_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/hardware/signal_lights/src/signal_lights_node/signal_lights_nodelet.cc b/hardware/signal_lights/src/signal_lights_node/signal_lights_nodelet.cc index b9de936001..ece78ec1d2 100644 --- a/hardware/signal_lights/src/signal_lights_node/signal_lights_nodelet.cc +++ b/hardware/signal_lights/src/signal_lights_node/signal_lights_nodelet.cc @@ -25,7 +25,7 @@ #include // FSW nodelet -#include +#include #include // Proxy library diff --git a/hardware/smart_dock/CMakeLists.txt b/hardware/smart_dock/CMakeLists.txt index c1044eba9d..9a3429897d 100644 --- a/hardware/smart_dock/CMakeLists.txt +++ b/hardware/smart_dock/CMakeLists.txt @@ -15,56 +15,128 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(smart_dock) -set(DEPS - rapidExtAstrobee -) - -set(LIBS - Qt4::QtXml - rapidIo - rapidExtAstrobee - ff_common) - -set(INCLUDES - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${SORACORE_INCLUDE_DIRS} -) - -set(RTI_QOS_FILES - ${CMAKE_SOURCE_DIR}/submodules/common/communications/dds/NDDS_DISCOVERY_PEERS - ${CMAKE_SOURCE_DIR}/submodules/common/communications/dds/RAPID_QOS_PROFILES.xml -) - -set(CONFIG_DIR ${CMAKE_SOURCE_DIR}/astrobee/config/communications/dds) - -create_library(TARGET smart_dock - LIBS ${LIBS} i2c eps_driver - INC ${INCLUDES} - DEPS ${DEPS} -) - -create_tool_targets(DIR tools - LIBS ${LIBS} smart_dock config_reader - INC ${INCLUDES} - DEPS ${DEPS} -) - -# Determine our module name -get_filename_component(MODULE_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME) - -install(CODE "execute_process( - COMMAND mkdir -p share/${MODULE_NAME} - COMMAND ln -s ../../bin/dds_ros_bridge share/${MODULE_NAME} - WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} - OUTPUT_QUIET - ERROR_QUIET - )") - -# Copy these files outside of an install to make available -# with rosrun/roslaunch -file(MAKE_DIRECTORY ${CONFIG_DIR}) -file(COPY ${RTI_QOS_FILES} - DESTINATION ${CONFIG_DIR}) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) +if (USE_DDS) + ## Find catkin macros and libraries + find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + i2c + eps_driver + dds_msgs + ) + + # System dependencies are found with CMake's conventions + find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options timer) + + list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") + + if (USE_CTC) + set(SORACORE_ROOT_DIR ${ARM_CHROOT_DIR}/usr) + else (USE_CTC) + set(SORACORE_ROOT_DIR /usr) + endif (USE_CTC) + + set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) + + SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) + SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + + # find Qt version according to OS + find_program(LSB_RELEASE_EXEC lsb_release) + execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + + if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) + else () + find_package(Qt4 4.6.0 REQUIRED QtXml) + endif () + + find_package(Miro REQUIRED) + find_package(RtiDds REQUIRED) + find_package(Soracore REQUIRED) + catkin_package( + LIBRARIES + signal_lights + CATKIN_DEPENDS + roscpp + nodelet + i2c + eps_driver + dds_msgs + ) + + ########### + ## Build ## + ########### + + # Specify additional locations of header files + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${RTIDDS_INCLUDE_DIR} + ${SORACORE_INCLUDE_DIRS} + ${MIRO_INCLUDE_DIR} + ${QT_INCLUDE_DIR} + ${QT_INCLUDE_DIR}/Qt + ) + + + # Declare C++ libraries + add_library(smart_dock + src/smart_dock.cc + src/smart_dock_node.cc + ) + target_compile_definitions(smart_dock PUBLIC ${RTIDDS_DEFINE_FLAGS}) + add_dependencies(smart_dock ${catkin_EXPORTED_TARGETS}) + if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + target_link_libraries(smart_dock rapidIo Qt5::Xml ${catkin_LIBRARIES}) + else () + target_link_libraries(smart_dock rapidIo ${catkin_LIBRARIES}) + endif () + ## Declare a C++ executable: smart_dock_service + add_executable(smart_dock_service tools/smart_dock_service.cc) + add_dependencies(smart_dock_service ${catkin_EXPORTED_TARGETS}) + target_link_libraries(smart_dock_service + smart_dock gflags ${catkin_LIBRARIES}) + + ## Declare a C++ executable: smart_dock_tool + add_executable(smart_dock_tool tools/smart_dock_tool.cc) + add_dependencies(smart_dock_tool ${catkin_EXPORTED_TARGETS}) + target_link_libraries(smart_dock_tool + smart_dock gflags ${catkin_LIBRARIES}) + + # Determine our module name + get_filename_component(MODULE_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME) + + ############# + ## Install ## + ############# + + # Mark libraries for installation + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + # Install C++ executables + install(TARGETS smart_dock_service DESTINATION bin) + install(TARGETS smart_dock_tool DESTINATION bin) + install(CODE "execute_process( + COMMAND ln -s ../../bin/smart_dock_service share/${PROJECT_NAME} + COMMAND ln -s ../../bin/smart_dock_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +else (USE_DDS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_DDS) diff --git a/hardware/smart_dock/include/smart_dock/smart_dock.h b/hardware/smart_dock/include/smart_dock/smart_dock.h index 114becf1a9..d2a8713899 100644 --- a/hardware/smart_dock/include/smart_dock/smart_dock.h +++ b/hardware/smart_dock/include/smart_dock/smart_dock.h @@ -155,6 +155,10 @@ class SmartDock { FAULT_OT_CHARGER, FAULT_OT_ACTUATOR_1, FAULT_OT_ACTUATOR_2, + FAULT_WDT1_EPS_REBOOT, + FAULT_WDT2_DOCKCTL_REBOOT, + FAULT_WDT3_DOCKPC_REBOOT, + FAULT_WDT4_DOCKCTL_REBOOT, NUM_FAULTS }; diff --git a/hardware/smart_dock/include/smart_dock/smart_dock_node.h b/hardware/smart_dock/include/smart_dock/smart_dock_node.h index 4bdc4362c2..a04e0c31cc 100644 --- a/hardware/smart_dock/include/smart_dock/smart_dock_node.h +++ b/hardware/smart_dock/include/smart_dock/smart_dock_node.h @@ -28,11 +28,11 @@ #include #include -#include "AstrobeeConstants.h" +#include "dds_msgs/AstrobeeConstants.h" #include "ff_common/init.h" -#include "DockStateSupport.h" +#include "dds_msgs/DockStateSupport.h" #include "knDds/DdsEntitiesFactory.h" #include "knDds/DdsEntitiesFactorySvc.h" diff --git a/hardware/smart_dock/package.xml b/hardware/smart_dock/package.xml new file mode 100644 index 0000000000..91f95b596c --- /dev/null +++ b/hardware/smart_dock/package.xml @@ -0,0 +1,33 @@ + + smart_dock + 0.0.1 + + Package to control the signal lights + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + roscpp + nodelet + i2c + eps_driver + ff_util + ff_msgs + ff_hw_msgs + dds_msgs + roscpp + nodelet + i2c + eps_driver + ff_util + ff_msgs + ff_hw_msgs + dds_msgs + diff --git a/hardware/smart_dock/tools/smart_dock_tool.cc b/hardware/smart_dock/tools/smart_dock_tool.cc index b9b544c25d..f45c77a3b6 100644 --- a/hardware/smart_dock/tools/smart_dock_tool.cc +++ b/hardware/smart_dock/tools/smart_dock_tool.cc @@ -814,6 +814,10 @@ int main(int argc, char** argv) { idxs[SD::FAULT_OT_CHARGER] = Value("Charger o/temp", {"t_chg"}); idxs[SD::FAULT_OT_ACTUATOR_1] = Value("Actuator 1 o/temp", {"t_act1"}); idxs[SD::FAULT_OT_ACTUATOR_2] = Value("Actuator 2 o/temp", {"t_act2"}); + idxs[SD::FAULT_WDT1_EPS_REBOOT] = Value("WatchDog Timer 1 has rebooted EPS", {"wdt1_eps"}); + idxs[SD::FAULT_WDT2_DOCKCTL_REBOOT] = Value("WatchDog Timer 2 has rebooted Dock Controller", {"wdt2_dctl"}); + idxs[SD::FAULT_WDT3_DOCKPC_REBOOT] = Value("WatchDog Timer 3 has rebooted Dock Processor", {"wdt3_dpc"}); + idxs[SD::FAULT_WDT4_DOCKCTL_REBOOT] = Value("WatchDog Timer 4 has rebooted Dock Controller", {"wdt4_dctl"}); if (FLAGS_list) return Print("Indexes", idxs, true); if (FLAGS_clear) { if (!sd.ClearFaults()) return Error("Could not clear the faults."); diff --git a/hardware/speed_cam/CMakeLists.txt b/hardware/speed_cam/CMakeLists.txt index 0398cc787e..1a0a5aa3f2 100644 --- a/hardware/speed_cam/CMakeLists.txt +++ b/hardware/speed_cam/CMakeLists.txt @@ -15,32 +15,99 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(speed_cam) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + ff_util + ff_hw_msgs + ff_serial + mavlink +) + catkin_package( LIBRARIES speed_cam CATKIN_DEPENDS roscpp nodelet - pluginlib - sensor_msgs - geometry_msgs ff_util ff_hw_msgs - mavlink + ff_serial ) -create_library( - TARGET speed_cam - LIBS ${catkin_LIBRARIES} ff_serial config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS mavlink speed_cam_gencpp +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_tool_targets(DIR tools - LIBS ${catkin_LIBRARIES} speed_cam - INC ${catkin_INCLUDE_DIRS} +# Declare C++ libraries +# Declare C++ libraries +add_library(speed_cam + src/speed_cam.cc + src/speed_cam_node.cc ) +add_dependencies(speed_cam ${catkin_EXPORTED_TARGETS}) +target_link_libraries(speed_cam ${catkin_LIBRARIES}) + +## Declare a C++ executable: speed_cam_tool +add_executable(speed_cam_tool tools/speed_cam_tool.cc) +add_dependencies(speed_cam_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(speed_cam_tool + speed_cam gflags ${catkin_LIBRARIES}) + + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + # Speed cam initialization fault tester + add_rostest_gtest(test_init_speed_cam + test/test_init_speed_cam.test + test/test_init_speed_cam.cc + ) + + target_link_libraries(test_init_speed_cam + ${catkin_LIBRARIES} glog + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS speed_cam_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/speed_cam_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) -install_launch_files() diff --git a/hardware/speed_cam/package.xml b/hardware/speed_cam/package.xml index 5a47339720..985cc8b6d5 100644 --- a/hardware/speed_cam/package.xml +++ b/hardware/speed_cam/package.xml @@ -16,19 +16,15 @@ catkin roscpp nodelet - pluginlib - sensor_msgs - geometry_msgs ff_util ff_hw_msgs + ff_serial mavlink roscpp nodelet - pluginlib - sensor_msgs - geometry_msgs ff_util ff_hw_msgs + ff_serial mavlink diff --git a/management/sys_monitor/test/test_init_speed_cam.cc b/hardware/speed_cam/test/test_init_speed_cam.cc similarity index 89% rename from management/sys_monitor/test/test_init_speed_cam.cc rename to hardware/speed_cam/test/test_init_speed_cam.cc index 4afda2db2f..76cf77054c 100644 --- a/management/sys_monitor/test/test_init_speed_cam.cc +++ b/hardware/speed_cam/test/test_init_speed_cam.cc @@ -16,11 +16,19 @@ * under the License. */ +// Test initialization speed cam +// These initialization tests make sure the node is spawned. To do so, +// it spawns the node and the system manager. The node uses the wrong folder +// for the config files on purpose, which it will make it unable to read the +// config files. Because the node can't read the config files it triggers the +// initialization anomaly to the system monitor. Is this anomaly is issued +// within 2 seconds, then the test passes. + #include // Required for the test cases #include -#include +#include // Required for the test framework #include diff --git a/management/sys_monitor/test/test_init_speed_cam.test b/hardware/speed_cam/test/test_init_speed_cam.test similarity index 95% rename from management/sys_monitor/test/test_init_speed_cam.test rename to hardware/speed_cam/test/test_init_speed_cam.test index 0d7ceb0f23..bbc486eb93 100644 --- a/management/sys_monitor/test/test_init_speed_cam.test +++ b/hardware/speed_cam/test/test_init_speed_cam.test @@ -28,5 +28,5 @@ - + diff --git a/hardware/temp_monitor/CMakeLists.txt b/hardware/temp_monitor/CMakeLists.txt index ac4e787f8f..e74a1eff61 100644 --- a/hardware/temp_monitor/CMakeLists.txt +++ b/hardware/temp_monitor/CMakeLists.txt @@ -15,30 +15,86 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(temp_monitor) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + i2c + config_reader + ff_hw_msgs + ff_msgs + ff_util +) + catkin_package( LIBRARIES temp_monitor CATKIN_DEPENDS roscpp nodelet - pluginlib + i2c + config_reader ff_util ff_hw_msgs - sensor_msgs ) -create_library( - TARGET temp_monitor - LIBS ${catkin_LIBRARIES} i2c config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS config_reader i2c +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_tool_targets( - DIR tools - LIBS temp_monitor +# Declare C++ libraries +add_library(temp_monitor + src/ADT7410.cc + src/TCN75A.cc + src/temp_monitor.cc + src/temp_monitor_node.cc ) +add_dependencies(temp_monitor ${catkin_EXPORTED_TARGETS}) +target_link_libraries(temp_monitor ${catkin_LIBRARIES}) + +## Declare a C++ executable: temp_monitor_tool +add_executable(temp_monitor_tool tools/temp_monitor_tool.cc) +add_dependencies(temp_monitor_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(temp_monitor_tool + temp_monitor gflags ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install C++ executables +install(TARGETS temp_monitor_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/temp_monitor_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") -install_launch_files() \ No newline at end of file +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/hardware/temp_monitor/package.xml b/hardware/temp_monitor/package.xml index 5be91546e6..00b8076e65 100644 --- a/hardware/temp_monitor/package.xml +++ b/hardware/temp_monitor/package.xml @@ -17,16 +17,16 @@ catkin roscpp nodelet - pluginlib + i2c ff_util ff_hw_msgs - sensor_msgs + config_reader roscpp nodelet - pluginlib + i2c ff_util ff_hw_msgs - sensor_msgs + config_reader diff --git a/hardware/vive/CMakeLists.txt b/hardware/vive/CMakeLists.txt index 128153502a..9a21e664d4 100644 --- a/hardware/vive/CMakeLists.txt +++ b/hardware/vive/CMakeLists.txt @@ -15,11 +15,23 @@ # License for the specific language governing permissions and limitations # under the License. -# Declare the project +cmake_minimum_required(VERSION 3.0) project(vive) -# We need standard math -find_library(CMATH_LIBRARIES NAMES m) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + ff_util + ff_hw_msgs + config_reader +) + +# System dependencies are found with CMake's conventions +find_package(cmake_modules REQUIRED) # declare the catkin package catkin_package( @@ -29,36 +41,86 @@ catkin_package( CATKIN_DEPENDS roscpp nodelet - pluginlib ff_util ff_hw_msgs + config_reader ) -# ROS agnostic proxy library and tool +########### +## Build ## +########### -create_library( - DIR src/vive_driver - TARGET vive_driver - INC ${USB_INCLUDE_DIRS} ${JSONC_INCLUDE_DIRS} ${ZLIB_INCLUDE_DIRS} - LIBS ${USB_LIBRARIES} ${JSONC_LIBRARIES} ${ZLIB_LIBRARIES} ${CMATH_LIBRARIES} +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_tool_targets( - DIR tools/vive_tool - INT ${ARGTABLE2_INCLUDE_DIRS} - LIBS vive_driver ${ARGTABLE2_LIBRARIES} +# Declare C++ libraries +add_library(vive_driver + src/vive_driver/vive.c + src/vive_driver/vive_data_button.c + src/vive_driver/vive_data_imu.c + src/vive_driver/vive_data_light.c + src/vive_driver/vive_data_ootx.c + src/vive_driver/vive_dev_tracker.c + src/vive_driver/vive_dev_watchman.c + src/vive_driver/vive_usb.c ) +add_dependencies(vive_driver ${catkin_EXPORTED_TARGETS}) +target_link_libraries(vive_driver json-c usb-1.0 z ${catkin_LIBRARIES}) + +# Declare C++ libraries +add_library(vive_nodelet + src/vive_nodelet/vive_nodelet.cc +) +add_dependencies(vive_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(vive_nodelet vive_driver ${catkin_LIBRARIES}) + +## Declare a C++ executable: vive_tool +add_executable(vive_tool tools/vive_tool/vive_tool.c) +add_dependencies(vive_tool ${catkin_EXPORTED_TARGETS}) +target_link_libraries(vive_tool + vive_driver argtable2 ${catkin_LIBRARIES}) -# ROS nodelet +############# +## Install ## +############# -create_library( - DIR src/vive_nodelet - TARGET vive_nodelet - LIBS ${catkin_LIBRARIES} vive_driver config_reader ff_nodelet - INC ${catkin_INCLUDE_DIRS} - DEPS ff_hw_msgs +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME}_driver + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +install(TARGETS ${PROJECT_NAME}_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -# Launch files +# Install C++ executables +install(TARGETS vive_tool DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/vive_tool share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/hardware/vive/package.xml b/hardware/vive/package.xml index 03448f9c7a..6fcbe6babb 100644 --- a/hardware/vive/package.xml +++ b/hardware/vive/package.xml @@ -15,14 +15,14 @@ catkin roscpp nodelet - pluginlib ff_util ff_hw_msgs + config_reader roscpp nodelet - pluginlib ff_util ff_hw_msgs + config_reader diff --git a/licenses.csv b/licenses.csv index e5df1bddb9..34ce8ecf56 100644 --- a/licenses.csv +++ b/licenses.csv @@ -34,7 +34,6 @@ Library (external),MAVLink: Micro Air Vehicle Message Marshalling Library, https Library (external),libsurvive: An open source tool for working with Lighthouse data,https://github.com/cnlohr/libsurvive, MIT, YES, See hardware/vive Library (optional),RTI DDS SDK,http://www.rti.com/,Source license,NO,None Library (optional),PicoFlexx SDK,http://pmdtec.com/picoflexx/software,Closed source,YES,None -Library (optional),Flycapture SDK,https://www.ptgrey.com/flycapture-sdk ,Closed source,NO,None ROS package,ros_core,https://github.com/ros/metapackages,BSD-3-Clause,NO,None ROS package,common_msgs,https://github.com/ros/common_msgs ,BSD-3-Clause,NO,None ROS package,nodelet,https://github.com/ros/nodelet_core,BSD-3-Clause,NO,None diff --git a/localization/CMakeLists.txt b/localization/CMakeLists.txt index 27a2c24a75..329b9d92c1 100644 --- a/localization/CMakeLists.txt +++ b/localization/CMakeLists.txt @@ -16,16 +16,25 @@ # under the License. add_subdirectory(camera) +add_subdirectory(graph_optimizer) +add_subdirectory(imu_integration) add_subdirectory(interest_point) +add_subdirectory(localization_common) +add_subdirectory(localization_measurements) +add_subdirectory(optimization_common) add_subdirectory(sparse_mapping) if (USE_ROS) + add_subdirectory(depth_odometry) + add_subdirectory(graph_localizer) + add_subdirectory(ground_truth_localizer) + add_subdirectory(imu_augmentor) add_subdirectory(marker_tracking) add_subdirectory(lk_optical_flow) add_subdirectory(handrail_detect) add_subdirectory(localization_manager) add_subdirectory(localization_node) - if (ENABLE_VIVE_SOLVER) + if (ENABLE_VIVE_SOLVER AND ENABLE_VIVE) add_subdirectory(vive_localization) endif() endif (USE_ROS) diff --git a/localization/camera/CMakeLists.txt b/localization/camera/CMakeLists.txt index b1e1357786..32eaab90a2 100644 --- a/localization/camera/CMakeLists.txt +++ b/localization/camera/CMakeLists.txt @@ -15,47 +15,106 @@ # License for the specific language governing permissions and limitations # under the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0) project(camera) -set(LIBS - config_reader - ${GLOG_LIBRARIES} - ${GFLAGS_LIBRARIES} - ${OpenCV_LIBRARIES} - ${EIGEN_LIBRARIES} - ${Boost_SYSTEM_LIBRARY} - ${Boost_FILESYSTEM_LIBRARY} - ) -set(INCLUDES +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + ff_common config_reader - ${GFLAGS_INCLUDE_DIRS} - ${GLOG_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} ) +# System dependencies are found with CMake's conventions +find_package(cmake_modules REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) +find_package(Eigen3 REQUIRED) + +# Find OpenCV +find_package(OpenCV 4.0 REQUIRED) + + + catkin_package( INCLUDE_DIRS include LIBRARIES camera CATKIN_DEPENDS -# DEPENDS system_lib + ff_common + config_reader ) -create_library(TARGET camera - LIBS ${LIBS} - INC ${INCLUDES} +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${OpenCV_INCLUDE_DIRS} ) -create_tool_targets(DIR tools - LIBS camera config_reader - INC ${INCLUDES} +# Declare C++ libraries +add_library(camera + src/camera_model.cc + src/camera_params.cc ) +add_dependencies(camera ${catkin_EXPORTED_TARGETS}) +target_link_libraries(camera ${catkin_LIBRARIES} ${EIGEN_LIBRARIES} ${OpenCV_LIBRARIES}) + +## Declare a C++ executable: undistort_image +add_executable(undistort_image tools/undistort_image.cc) +add_dependencies(undistort_image ${catkin_EXPORTED_TARGETS}) +target_link_libraries(undistort_image + camera gflags glog ${catkin_LIBRARIES}) -create_test_targets(DIR test - LIBS camera ${CERES_LIBRARIES} - INC ${INCLUDES} ${CERES_INCLUDE_DIRS} +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_rostest_gtest(test_camera_model + test/test_camera_model.test + test/test_camera_model.cc + ) + target_link_libraries(test_camera_model + camera + ) + + add_rostest_gtest(test_camera_params + test/test_camera_params.test + test/test_camera_params.cc + ) + target_link_libraries(test_camera_params + camera + ) + +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) -set(CAMERA_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include PARENT_SCOPE) +# Install C++ executables +install(TARGETS undistort_image DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/undistort_image share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/camera/include/camera/camera_params.h b/localization/camera/include/camera/camera_params.h index a7c8a58ec1..8de3723def 100644 --- a/localization/camera/include/camera/camera_params.h +++ b/localization/camera/include/camera/camera_params.h @@ -71,7 +71,7 @@ namespace camera { // Used to create a remap table. This table is the same size as the // UNDISTORTED image, where every pixel's value is the cooresponding pixel // location in the DISTORTED image. - void GenerateRemapMaps(cv::Mat* remap_map); + void GenerateRemapMaps(cv::Mat* remap_map, double scale = 1.0); // Conversion utilities template @@ -104,9 +104,11 @@ namespace camera { void SetOpticalOffset(Eigen::Vector2d const& offset); const Eigen::Vector2d& GetOpticalOffset() const; - // Helper function that approximates the focal length as a single value. - // Will produce wrong results if pixels are not square. Should be exactly - // correct if image is UNDISTORTED. + // Helper functions for focal length. Unless pixels are square, + // the focal length in x may differ than the one in y. Note that + // they are equal for undistorted images. Note that + // GetFocalLength() returns the mean of the two focal lengths + // while GetFocalVector() returns the vector. void SetFocalLength(Eigen::Vector2d const&); double GetFocalLength() const; const Eigen::Vector2d& GetFocalVector() const; diff --git a/localization/camera/package.xml b/localization/camera/package.xml index 412f360bd7..d489a27330 100644 --- a/localization/camera/package.xml +++ b/localization/camera/package.xml @@ -2,7 +2,7 @@ camera 1.0.0 - This package localizes the Astrobee on a map. + This package provides camera utilities. Apache License, Version 2.0 @@ -14,24 +14,8 @@ Astrobee Flight Software catkin - roscpp - message_generation - std_msgs - sensor_msgs - cv_bridge - image_transport - rosbag - ff_msgs - nodelet - tf2 - roscpp - message_runtime - std_msgs - sensor_msgs - cv_bridge - image_transport - rosbag - ff_msgs - nodelet - tf2 + ff_common + config_reader + ff_common + config_reader diff --git a/localization/camera/readme.md b/localization/camera/readme.md index 2c06d16d4c..7e64a02e75 100644 --- a/localization/camera/readme.md +++ b/localization/camera/readme.md @@ -1,11 +1,11 @@ -\page camera Camera Library +\page camera Camera library This library handles camera calibration parameters and transformations, providing functions to handle between undistorted and distorted coordinate frames, and other helper functions to deal with reading and applying camera transformations. -A tool named undistort_image is provided that can undistort (and +A tool named `undistort_image` is provided that can undistort (and optionally crop) a given set of images. The undistorted image dimensions, as well as the focal length and @@ -13,14 +13,75 @@ undistorted optical center will be displayed on the screen. Usage: - export ASTROBEE_RESOURCE_DIR=$SOURCE_PATH/astrobee/resources - export ASTROBEE_CONFIG_DIR=$SOURCE_PATH/astrobee/config - export ASTROBEE_WORLD=granite - export ASTROBEE_ROBOT=p4d + export ASTROBEE_RESOURCE_DIR=$SOURCE_PATH/astrobee/resources + export ASTROBEE_CONFIG_DIR=$SOURCE_PATH/astrobee/config + export ASTROBEE_WORLD=granite + export ASTROBEE_ROBOT=p4d - undistort_image input_dir/*[0-9].jpg --output_directory output_dir \ - --robot_camera nav_cam + undistort_image input_dir/*[0-9].jpg --output_directory output_dir \ + --robot_camera nav_cam -Other options, useful with the dense mapper, are --save_bgr, ---undistorted_crop_win, and --image_list. See undistort_image.cc for -more information. +Options: + + --image_list: A file having the list of images to undistort, one per + line. If not specified it is assumed they are passed in directly on + the command line. + + --output_directory: Output directory. If not specified, undistorted + images will saved in the same directory as the inputs. + + --scale: Undistort images at different resolution, with their width + being a multiple of this scale compared to the camera model. The + default is 1. + + --undistorted_crop_win: After undistorting, apply a crop window of + these dimensions centered at the undistorted image center. The + adjusted dimensions and optical center will be printed on screen. + Specify as: 'crop_x crop_y'. Or specify 'loose' to use the smallest + crop window that contains all of the undistorted source pixels and + keeps the optical center centered. + + --save_bgr: Save the undistorted images as BGR instead of + grayscale. (Some tools expect BGR.) + + --histogram_equalization: If true, do histogram equalization. + + --robot_camera: Which of bot's cameras to use. Tested with nav_cam + and sci_cam. The default is nav_cam. + + --alpha: Add an alpha channel and make areas outside the remapped + input transparent. Forces PNG output format. Not compatible with + save_bgr. + + --cubic: Use more expensive cubic interpolation for best image + quality. + +Notes +----- + +For stitching Astrobee SciCam panoramas, the Hugin tools by default +expect input image frames to have a "rectilinear" camera model. We can +satisfy this by preprocessing input frames using `undistort_image`. This +lets us lean on our own quality-controlled SciCam calibration rather +than adding extra lens parameters to complicate Hugin's already huge and +not-always-stable stitching optimization problem. + +The following flags are recommended for this use case: + + --undistorted_crop_win loose: The undistorted image is not in general + rectangular. This argument tells the tool to use the tightest crop + box that saves all the useful remapped pixels and keeps the optical + center centered. The resulting images are much smaller than when + uncropped and help to avoid Hugin errors that can come from poorly + conditioned math with an overly-wide field of view. + + --alpha: Since the undistorted image is not rectangular, there will be + some junk where there is no real data around the borders. By + default, it is black, but this argument adds an alpha channel so it + can be transparent. Luckily, Hugin will respect an alpha channel in + the input images if it is provided, so we won't see random black + border bits in the stitched output. + + --cubic: We want to avoid losing image quality wherever possible, + since that is what the panorama users care about! Mild quality + improvement, takes a bit longer. diff --git a/localization/camera/src/camera_model.cc b/localization/camera/src/camera_model.cc index 2fb5a01eb9..a2c998b321 100644 --- a/localization/camera/src/camera_model.cc +++ b/localization/camera/src/camera_model.cc @@ -70,12 +70,12 @@ void CameraModel::SetTransform(const Eigen::Affine3d & cam_t_global) { double CameraModel::GetFovX(void) const { // This is an approximation since it doesn't take in account lens distortion - return atan(1.0 / (params_.GetFocalVector()[0] * params_.GetDistortedHalfSize()[0])) * 2; + return atan(params_.GetDistortedHalfSize()[0] / params_.GetFocalVector()[0] ) * 2; } double CameraModel::GetFovY(void) const { // This is an approximation since it doesn't take in account lens distortion - return atan(1.0 / (params_.GetFocalVector()[1] * params_.GetDistortedHalfSize()[1])) * 2; + return atan(params_.GetDistortedHalfSize()[1] / params_.GetFocalVector()[1] ) * 2; } const camera::CameraParameters& CameraModel::GetParameters() const { @@ -92,8 +92,10 @@ Eigen::Vector2d CameraModel::ImageCoordinates(const Eigen::Vector3d & p) const { } Eigen::Vector3d CameraModel::Ray(int x, int y) const { - return cam_t_global_.rotation().inverse() * Eigen::Vector3d(x / params_.GetFocalVector()[0], - y / params_.GetFocalVector()[1], 1.0).normalized(); + Eigen::Vector2d undistorted; + params_.Convert(Eigen::Vector2d(x, y), &undistorted); + return cam_t_global_.rotation().inverse() * Eigen::Vector3d(undistorted[0] / params_.GetFocalVector()[0], + undistorted[1] / params_.GetFocalVector()[1], 1.0).normalized(); } Eigen::Vector3d CameraModel::CameraCoordinates(double x, double y, double z) const { @@ -104,18 +106,18 @@ Eigen::Vector3d CameraModel::CameraCoordinates(const Eigen::Vector3d & p) const return cam_t_global_ * p; } -// TODO(oalexan1): This looks buggy. Because ImageCoordinates() -// returns an undistorted pixel, it must compare to GetUndistortedHalfSize(). bool CameraModel::IsInFov(const Eigen::Vector3d & p) const { Eigen::Vector3d t = cam_t_global_ * p; if (t.z() <= 0.0) return false; - Eigen::Vector2d camera = ImageCoordinates(p); - if (camera.x() < -params_.GetDistortedHalfSize()[0] || - camera.x() >= params_.GetDistortedHalfSize()[0]) + Eigen::Vector2d undistorted, distorted; + undistorted = ImageCoordinates(p); + params_.Convert(undistorted, &distorted); + if (distorted.x() < -params_.GetDistortedHalfSize()[0] || + distorted.x() >= params_.GetDistortedHalfSize()[0]) return false; - if (camera.y() < -params_.GetDistortedHalfSize()[1] || - camera.y() >= params_.GetDistortedHalfSize()[1]) + if (distorted.y() < -params_.GetDistortedHalfSize()[1] || + distorted.y() >= params_.GetDistortedHalfSize()[1]) return false; return true; } diff --git a/localization/camera/src/camera_params.cc b/localization/camera/src/camera_params.cc index 9119fd12c2..4b3f880cd4 100644 --- a/localization/camera/src/camera_params.cc +++ b/localization/camera/src/camera_params.cc @@ -122,16 +122,21 @@ camera::CameraParameters::CameraParameters(config_reader::ConfigReader* config, optical_offset_ << cam_mat.at(0, 2), cam_mat.at(1, 2); focal_length_ << cam_mat.at(0, 0), cam_mat.at(1, 1); - // Read in the distortion coefficients (if not a real number, may be a list) + // Read in the distortion coefficients. It can be a number or a list. Eigen::VectorXd buffer(1); - if (!camera.GetReal("distortion_coeff", &buffer[0])) { - config_reader::ConfigReader::Table dist(&camera, "distortion_coeff"); + std::string dist_name = "distortion_coeff"; + if (camera.IsNumber(dist_name.c_str())) { + if (!camera.GetReal(dist_name.c_str(), &buffer[0])) + fprintf(stderr, "Could not read camera distortion_coeff."); + } else { + config_reader::ConfigReader::Table dist(&camera, dist_name.c_str()); buffer.resize(dist.GetSize()); for (int i = 0; i < dist.GetSize(); i++) { if (!dist.GetReal(i + 1, &buffer[i])) fprintf(stderr, "Could not read camera distortion_coeff."); } } + SetDistortion(buffer); if (!camera.GetInt("undistorted_width", &size[0])) @@ -141,6 +146,7 @@ camera::CameraParameters::CameraParameters(config_reader::ConfigReader* config, SetUndistortedSize(size); // right now all our crops are zero + crop_offset_.setZero(); // ff_common::ConfigReader::Table crop_table(&camera, "crop"); // // Read in crop offset // if (!crop_table.GetInt("x", &size[0])) @@ -331,14 +337,18 @@ void camera::CameraParameters::UndistortCentered(Eigen::Vector2d const& distorte } } -void camera::CameraParameters::GenerateRemapMaps(cv::Mat* remap_map) { - remap_map->create(undistorted_image_size_[1], undistorted_image_size_[0], CV_32FC2); +// The 'scale' variable is useful when we have the distortion model for a given +// image, and want to apply it to a version of that image at a different resolution, +// with 'scale' being the ratio of the width of the image at different resolution +// and the one at the resolution at which the distortion model is computed. +void camera::CameraParameters::GenerateRemapMaps(cv::Mat* remap_map, double scale) { + remap_map->create(scale*undistorted_image_size_[1], scale*undistorted_image_size_[0], CV_32FC2); Eigen::Vector2d undistorted, distorted; - for (undistorted[1] = 0; undistorted[1] < undistorted_image_size_[1]; undistorted[1]++) { - for (undistorted[0] = 0; undistorted[0] < undistorted_image_size_[0]; undistorted[0]++) { - Convert(undistorted, &distorted); - remap_map->at(undistorted[1], undistorted[0])[0] = distorted[0]; - remap_map->at(undistorted[1], undistorted[0])[1] = distorted[1]; + for (undistorted[1] = 0; undistorted[1] < scale*undistorted_image_size_[1]; undistorted[1]++) { + for (undistorted[0] = 0; undistorted[0] < scale*undistorted_image_size_[0]; undistorted[0]++) { + Convert(undistorted/scale, &distorted); + remap_map->at(undistorted[1], undistorted[0])[0] = scale*distorted[0]; + remap_map->at(undistorted[1], undistorted[0])[1] = scale*distorted[1]; } } } diff --git a/localization/camera/test/test_camera_model.cc b/localization/camera/test/test_camera_model.cc new file mode 100644 index 0000000000..2ae5888e6e --- /dev/null +++ b/localization/camera/test/test_camera_model.cc @@ -0,0 +1,184 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +#include +#include +#include + +#include +#include + +#define EXPECT_VECTOR3D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); \ + EXPECT_NEAR(p1[2], p2[2], t); + +#define EXPECT_VECTOR2D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); + +TEST(camera_model, angleaxis_transforms) { + // Make an arbitrary rotation + Eigen::Matrix3d R1 = + Eigen::AngleAxisd(-.156 * M_PI, + Eigen::Vector3d(.14, -.56, .4).normalized()).matrix(); + Eigen::Matrix3d R2; + Eigen::Vector3d rodrigues; + + camera::RotationToRodrigues(R1, &rodrigues); + camera::RodriguesToRotation(rodrigues, &R2); + + for (int j = 0; j < 3; j++) { + for (int i = 0; i < 3; i++) { + EXPECT_NEAR(R1(j, i), R2(j, i), 1e-6); + } + } + + Eigen::Vector3d input(4, 5, 3); + Eigen::Vector3d output2 = R1 * input; + Eigen::Vector3d output1; + ceres::AngleAxisRotatePoint(&rodrigues[0], + &input[0], &output1[0]); + for (int i = 0; i < 3; i++) + EXPECT_NEAR(output1[i], output2[i], 1e-6); +} + +TEST(camera_model, camera) { + std::string data_dir = std::string(std::getenv("DATA_DIR")); + camera::CameraParameters params(data_dir + "iss_tango_camera.xml"); // Ideally we would use a + // different file which + // represents an undistorted + // image. + // params.undistorted = + // true; + params.SetDistortedSize(Eigen::Vector2i(640, 480)); + params.SetUndistortedSize(Eigen::Vector2i(640, 480)); + + // Over-ride focal length for the tests below to pass + params.SetFocalLength(Eigen::Vector2d::Constant(1.0 / (tan(M_PI_2 / 2) / 320))); + + Eigen::Vector3d pos(10, -10, 5); + Eigen::Matrix3d rotation; + rotation = Eigen::AngleAxisd(1.0 * M_PI, Eigen::Vector3d::UnitX()) + * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitZ()); + camera::CameraModel camera(pos, rotation, M_PI_2, 640, 480); + camera::CameraModel camera2(pos, rotation, params); + + // check that two constructors give same camera + EXPECT_EQ(camera.GetPosition(), camera2.GetPosition()); + EXPECT_EQ(camera.GetRotation(), camera2.GetRotation()); + EXPECT_EQ(camera.GetTransform().matrix(), camera2.GetTransform().matrix()); + EXPECT_VECTOR2D_NEAR(camera.GetParameters().GetUndistortedSize(), + camera2.GetParameters().GetUndistortedSize(), 0); + EXPECT_DOUBLE_EQ(camera.GetParameters().GetFocalLength(), camera2.GetParameters().GetFocalLength()); + + // check that camera is as specified + EXPECT_VECTOR3D_NEAR(camera.GetPosition(), pos, 1e-5); + EXPECT_VECTOR2D_NEAR(camera.GetParameters().GetUndistortedSize(), params.GetUndistortedSize(), 0); + EXPECT_DOUBLE_EQ(camera.GetParameters().GetFocalLength(), params.GetFocalLength()); + + // check that the center of rotation is origin of camera coordinates + Eigen::Vector3d test_point(10, -10, 5); + EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(test_point), Eigen::Vector3d(0, 0, 0), 1e-5); + EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(test_point.x(), test_point.y(), test_point.z()), + Eigen::Vector3d(0, 0, 0), 1e-5); + + // check that a point at (0, 0, 1) in camera coordinates is in the center of the image + EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(10, -11, 5), Eigen::Vector3d(0, 0, 1), 1e-5); + EXPECT_VECTOR2D_NEAR(camera.ImageCoordinates(Eigen::Vector3d(10, -11, 5)), Eigen::Vector2d(0, 0), 1e-5); + EXPECT_VECTOR2D_NEAR(camera.ImageCoordinates(10, -11, 5), Eigen::Vector2d(0, 0), 1e-5); + // and in FOV + EXPECT_TRUE(camera.IsInFov(10, -11, 5)); + // point behind camera isn't in FOV + EXPECT_FALSE(camera.IsInFov(Eigen::Vector3d(10, -9, 5))); + + // check points on border and right beyond border of FOV + Eigen::Vector3d p; + Eigen::Vector2d t; + + p << 10, -15, 10-1e-5; + t = camera.ImageCoordinates(p); + EXPECT_NEAR(t.x(), -320, 1); + EXPECT_NEAR(t.y(), 0, 1); + EXPECT_TRUE(camera.IsInFov(p)); + EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(0, 0, 1e-1))); + + p << 10, -15, 0+1e-2; // due to rounding, 1e-2 + t = camera.ImageCoordinates(p); + EXPECT_NEAR(t.x(), 320, 1); + EXPECT_NEAR(t.y(), 0, 1); + EXPECT_TRUE(camera.IsInFov(p)); + EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(0, 0, -1e-1))); + + p << 6.25+1e-5, -15, 5; + t = camera.ImageCoordinates(p); + EXPECT_NEAR(t.x(), 0, 1); + EXPECT_NEAR(t.y(), -240, 1); + EXPECT_TRUE(camera.IsInFov(p)); + EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(-1e-1, 0, 0))); + + p << 13.75-1e-2, -15, 5; // due to rounding, 1e-2 + t = camera.ImageCoordinates(p); + EXPECT_NEAR(t.x(), 0, 1); + EXPECT_NEAR(t.y(), 240, 1); + EXPECT_TRUE(camera.IsInFov(p)); + EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(1e-1, 0, 1e-2))); + + // check SetTransform + Eigen::Affine3d new_transform; + new_transform.setIdentity(); + camera.SetTransform(new_transform); + EXPECT_EQ(camera.CameraCoordinates(1, 2, 3), Eigen::Vector3d(1, 2, 3)); +} + +TEST(camera_model, DistortionCoeffs) { + std::string data_dir = std::string(std::getenv("DATA_DIR")); + + // Test with our three camera models + std::vector files {"p2_camera.xml", "iss_tango_camera.xml", + "earth_tango_camera.xml", "iss_tango_undistorted.xml", "earth_tango_undistorted.xml"}; + for (std::string const& file : files) { + camera::CameraParameters camera_params(data_dir + file); + + for (float x = -320; x < 320; x += 30) { + for (float y = -240; y < 240; y += 30) { + Eigen::Vector2d distort_location, loopback_location; + camera_params.Convert + (Eigen::Vector2d(x, y), &distort_location); + camera_params.Convert + (distort_location, &loopback_location); + EXPECT_NEAR(x, loopback_location[0], 1e-4) << file; + EXPECT_NEAR(y, loopback_location[1], 1e-4) << file; + } + } + } + + // Verify that the optical center is not at the center of the image + // in undistort location + camera::CameraParameters camera_params(data_dir + "iss_tango_camera.xml"); + Eigen::Vector2d optical_center; + camera_params.Convert(Eigen::Vector2d::Zero(), &optical_center); + EXPECT_NEAR(3.92, optical_center[0], 1e-3); + EXPECT_NEAR(0.827, optical_center[1], 1e-3); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/camera/test/test_camera_model.cxx b/localization/camera/test/test_camera_model.cxx deleted file mode 100644 index f676f32990..0000000000 --- a/localization/camera/test/test_camera_model.cxx +++ /dev/null @@ -1,178 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include - -#include -#include -#include - -#include -#include - -#define EXPECT_VECTOR3D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); \ - EXPECT_NEAR(p1[2], p2[2], t); - -#define EXPECT_VECTOR2D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); - -TEST(camera_model, angleaxis_transforms) { - // Make an arbitrary rotation - Eigen::Matrix3d R1 = - Eigen::AngleAxisd(-.156 * M_PI, - Eigen::Vector3d(.14, -.56, .4).normalized()).matrix(); - Eigen::Matrix3d R2; - Eigen::Vector3d rodrigues; - - camera::RotationToRodrigues(R1, &rodrigues); - camera::RodriguesToRotation(rodrigues, &R2); - - for (int j = 0; j < 3; j++) { - for (int i = 0; i < 3; i++) { - EXPECT_NEAR(R1(j, i), R2(j, i), 1e-6); - } - } - - Eigen::Vector3d input(4, 5, 3); - Eigen::Vector3d output2 = R1 * input; - Eigen::Vector3d output1; - ceres::AngleAxisRotatePoint(&rodrigues[0], - &input[0], &output1[0]); - for (int i = 0; i < 3; i++) - EXPECT_NEAR(output1[i], output2[i], 1e-6); -} - -TEST(camera_model, camera) { - std::string data_dir = std::string(TEST_DIR) + "/../../sparse_mapping/test/data/"; - camera::CameraParameters params(data_dir + "iss_tango_camera.xml"); // Ideally we would use a - // different file which - // represents an undistorted - // image. - // params.undistorted = - // true; - params.SetDistortedSize(Eigen::Vector2i(640, 480)); - params.SetUndistortedSize(Eigen::Vector2i(640, 480)); - - // Over-ride focal length for the tests below to pass - params.SetFocalLength(Eigen::Vector2d::Constant(1.0 / (tan(M_PI_2 / 2) / 320))); - - Eigen::Vector3d pos(10, -10, 5); - Eigen::Matrix3d rotation; - rotation = Eigen::AngleAxisd(1.0 * M_PI, Eigen::Vector3d::UnitX()) - * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitZ()); - camera::CameraModel camera(pos, rotation, M_PI_2, 640, 480); - camera::CameraModel camera2(pos, rotation, params); - - // check that two constructors give same camera - EXPECT_EQ(camera.GetPosition(), camera2.GetPosition()); - EXPECT_EQ(camera.GetRotation(), camera2.GetRotation()); - EXPECT_EQ(camera.GetTransform().matrix(), camera2.GetTransform().matrix()); - EXPECT_VECTOR2D_NEAR(camera.GetParameters().GetUndistortedSize(), - camera2.GetParameters().GetUndistortedSize(), 0); - EXPECT_DOUBLE_EQ(camera.GetParameters().GetFocalLength(), camera2.GetParameters().GetFocalLength()); - - // check that camera is as specified - EXPECT_VECTOR3D_NEAR(camera.GetPosition(), pos, 1e-5); - EXPECT_VECTOR2D_NEAR(camera.GetParameters().GetUndistortedSize(), params.GetUndistortedSize(), 0); - EXPECT_DOUBLE_EQ(camera.GetParameters().GetFocalLength(), params.GetFocalLength()); - - // check that the center of rotation is origin of camera coordinates - Eigen::Vector3d test_point(10, -10, 5); - EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(test_point), Eigen::Vector3d(0, 0, 0), 1e-5); - EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(test_point.x(), test_point.y(), test_point.z()), - Eigen::Vector3d(0, 0, 0), 1e-5); - - // check that a point at (0, 0, 1) in camera coordinates is in the center of the image - EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(10, -11, 5), Eigen::Vector3d(0, 0, 1), 1e-5); - EXPECT_VECTOR2D_NEAR(camera.ImageCoordinates(Eigen::Vector3d(10, -11, 5)), Eigen::Vector2d(0, 0), 1e-5); - EXPECT_VECTOR2D_NEAR(camera.ImageCoordinates(10, -11, 5), Eigen::Vector2d(0, 0), 1e-5); - // and in FOV - EXPECT_TRUE(camera.IsInFov(10, -11, 5)); - // point behind camera isn't in FOV - EXPECT_FALSE(camera.IsInFov(Eigen::Vector3d(10, -9, 5))); - - // check points on border and right beyond border of FOV - Eigen::Vector3d p; - Eigen::Vector2d t; - - p << 10, -15, 10-1e-5; - t = camera.ImageCoordinates(p); - EXPECT_NEAR(t.x(), -320, 1); - EXPECT_NEAR(t.y(), 0, 1); - EXPECT_TRUE(camera.IsInFov(p)); - EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(0, 0, 1e-1))); - - p << 10, -15, 0+1e-2; // due to rounding, 1e-2 - t = camera.ImageCoordinates(p); - EXPECT_NEAR(t.x(), 320, 1); - EXPECT_NEAR(t.y(), 0, 1); - EXPECT_TRUE(camera.IsInFov(p)); - EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(0, 0, -1e-1))); - - p << 6.25+1e-5, -15, 5; - t = camera.ImageCoordinates(p); - EXPECT_NEAR(t.x(), 0, 1); - EXPECT_NEAR(t.y(), -240, 1); - EXPECT_TRUE(camera.IsInFov(p)); - EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(-1e-1, 0, 0))); - - p << 13.75-1e-2, -15, 5; // due to rounding, 1e-2 - t = camera.ImageCoordinates(p); - EXPECT_NEAR(t.x(), 0, 1); - EXPECT_NEAR(t.y(), 240, 1); - EXPECT_TRUE(camera.IsInFov(p)); - EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(1e-1, 0, 1e-2))); - - // check SetTransform - Eigen::Affine3d new_transform; - new_transform.setIdentity(); - camera.SetTransform(new_transform); - EXPECT_EQ(camera.CameraCoordinates(1, 2, 3), Eigen::Vector3d(1, 2, 3)); -} - -TEST(camera_model, DistortionCoeffs) { - std::string data_dir = std::string(TEST_DIR) + "/../../sparse_mapping/test/data/"; - - // Test with our three camera models - std::vector files {"p2_camera.xml", "iss_tango_camera.xml", - "earth_tango_camera.xml", "iss_tango_undistorted.xml", "earth_tango_undistorted.xml"}; - for (std::string const& file : files) { - camera::CameraParameters camera_params(data_dir + file); - - for (float x = -320; x < 320; x += 30) { - for (float y = -240; y < 240; y += 30) { - Eigen::Vector2d distort_location, loopback_location; - camera_params.Convert - (Eigen::Vector2d(x, y), &distort_location); - camera_params.Convert - (distort_location, &loopback_location); - EXPECT_NEAR(x, loopback_location[0], 1e-4) << file; - EXPECT_NEAR(y, loopback_location[1], 1e-4) << file; - } - } - } - - // Verify that the optical center is not at the center of the image - // in undistort location - camera::CameraParameters camera_params(data_dir + "iss_tango_camera.xml"); - Eigen::Vector2d optical_center; - camera_params.Convert(Eigen::Vector2d::Zero(), &optical_center); - EXPECT_NEAR(3.92, optical_center[0], 1e-3); - EXPECT_NEAR(0.827, optical_center[1], 1e-3); -} diff --git a/localization/camera/test/test_camera_model.test b/localization/camera/test/test_camera_model.test new file mode 100644 index 0000000000..cd6f29fa7d --- /dev/null +++ b/localization/camera/test/test_camera_model.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/camera/test/test_camera_params.cc b/localization/camera/test/test_camera_params.cc new file mode 100644 index 0000000000..6f417afd29 --- /dev/null +++ b/localization/camera/test/test_camera_params.cc @@ -0,0 +1,203 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#define EXPECT_VECTOR3D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); \ + EXPECT_NEAR(p1[2], p2[2], t); + +#define EXPECT_VECTOR2D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); + +void Function(camera::CameraParameters const& params) { + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params.GetDistortedSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params.GetDistortedHalfSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params.GetUndistortedSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params.GetUndistortedHalfSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params.GetFocalVector(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params.GetOpticalOffset(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params.GetCropOffset(), 1e-3); +} + +TEST(camera_params, creation) { + camera::CameraParameters params( + Eigen::Vector2i(1200, 600), + Eigen::Vector2d(300, 200), + Eigen::Vector2d(660.7, 500.3)); + EXPECT_VECTOR2D_NEAR(params.GetDistortedHalfSize(), Eigen::Vector2i(600, 300), 1e-3); + EXPECT_VECTOR2D_NEAR(params.GetUndistortedHalfSize(), Eigen::Vector2i(600, 300), 1e-3); + EXPECT_EQ(0, params.GetDistortion().size()); + EXPECT_NEAR(250, params.GetFocalLength(), 1e-3); + + // Make RAW and DISTORT be different frames + params.SetCropOffset(Eigen::Vector2i(50, 100)); + EXPECT_VECTOR2D_NEAR(params.GetCropOffset(), Eigen::Vector2i(50, 100), 1e-3); + + // Verify we get the correct intrinsic matrix + Eigen::Matrix3d k = params.GetIntrinsicMatrix(); + EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); + EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(710.7, 600.3, 1), 1e-3); + + k = params.GetIntrinsicMatrix(); + EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); + EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(660.7, 500.3, 1), 1e-3); + + k = params.GetIntrinsicMatrix(); + EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); + EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(60.7, 200.3, 1), 1e-3); + + k = params.GetIntrinsicMatrix(); + EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); + EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(600, 300, 1), 1e-3); + + k = params.GetIntrinsicMatrix(); + EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); + EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(0, 0, 1), 1e-3); + + // See what happens when we do a copy + camera::CameraParameters params2 = params; + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params2.GetDistortedSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params2.GetDistortedHalfSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params2.GetUndistortedSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params2.GetUndistortedHalfSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params2.GetFocalVector(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params2.GetOpticalOffset(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params2.GetCropOffset(), 1e-3); + + camera::CameraParameters params3(params); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params3.GetDistortedSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params3.GetDistortedHalfSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params3.GetUndistortedSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params3.GetUndistortedHalfSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params3.GetFocalVector(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params3.GetOpticalOffset(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params3.GetCropOffset(), 1e-3); + + params3.SetOpticalOffset(Eigen::Vector2d(99, -100)); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(99, -100), params3.GetOpticalOffset(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params.GetOpticalOffset(), 1e-3); + + // Verify that we don't have memory alignment issues with shared ptrs + std::shared_ptr params4; + params4.reset(new camera::CameraParameters(params)); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params4->GetDistortedSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params4->GetDistortedHalfSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params4->GetUndistortedSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params4->GetUndistortedHalfSize(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params4->GetFocalVector(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params4->GetOpticalOffset(), 1e-3); + EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params4->GetCropOffset(), 1e-3); + + // Maybe alignment problems happen when passing to a function + Function(params); + + auto f_bind = std::bind(Function, params); + f_bind(); + + std::function f_bind2 = Function; + f_bind2(params); + + // Maybe the alignment problem is an STL container? + std::list > list; + list.emplace_back(params); + list.emplace_back(params); +} + + +TEST(camera_params, conversion) { + // raw d d_c u u_c + // 1 ---> + // 2 <--- + // 3 <------ + // 4 ------> + // 5 ---> + // 6 <--- + // 7 ------> + // 8 <------ + // 9 ---------> + // 10 <--------- + + Eigen::VectorXd distortion(1); + distortion[0] = 0.7; // FOV model + camera::CameraParameters params( + Eigen::Vector2i(1200, 600), + Eigen::Vector2d(300, 300), + Eigen::Vector2d(660.5, 500.3), distortion); + params.SetCropOffset(Eigen::Vector2i(25, 50)); + + // RAW to DISTORTED conversions + Eigen::Vector2d raw = Eigen::Vector2d::Zero(), output = Eigen::Vector2d::Zero(); + params.Convert(raw, &output); + EXPECT_NEAR(-25, output[0], 1e-6); + EXPECT_NEAR(-50, output[1], 1e-6); + params.Convert(output, &output); + EXPECT_NEAR(0, output[0], 1e-6); + EXPECT_NEAR(0, output[1], 1e-6); + + // forward backward via 3, 4 + Eigen::Vector2d input(-40, 77.3), output2; + params.Convert(input, &output); + params.Convert(output, &output2); + EXPECT_NEAR(input[0], output2[0], 1e-6); + EXPECT_NEAR(input[1], output2[1], 1e-6); + + // forward backward via 7, 8 + input << 233.4, 368.9; + params.Convert(input, &output); + params.Convert(output, &output2); + EXPECT_NEAR(input[0], output2[0], 1e-6); + EXPECT_NEAR(input[1], output2[1], 1e-6); + + // forward backward via 9, 10 + input << 542.4, 182.5; + params.Convert(input, &output); + params.Convert(output, &output2); + EXPECT_NEAR(input[0], output2[0], 1e-6); + EXPECT_NEAR(input[1], output2[1], 1e-6); + + // forward through 5, 10, 7 + input << 43.6, -213.9; + params.Convert(input, &output2); + params.Convert(output2, &output); + params.Convert(output, &output2); + EXPECT_NEAR(input[0], output2[0], 1e-6); + EXPECT_NEAR(input[1], output2[1], 1e-6); + + // forward through 8, 9, 6 + input << 542.4, 182.5; + params.Convert(input, &output2); + params.Convert(output2, &output); + params.Convert(output, &output2); + EXPECT_NEAR(input[0], output2[0], 1e-6); + EXPECT_NEAR(input[1], output2[1], 1e-6); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/camera/test/test_camera_params.cxx b/localization/camera/test/test_camera_params.cxx deleted file mode 100644 index 2021953974..0000000000 --- a/localization/camera/test/test_camera_params.cxx +++ /dev/null @@ -1,197 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#define EXPECT_VECTOR3D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); \ - EXPECT_NEAR(p1[2], p2[2], t); - -#define EXPECT_VECTOR2D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); - -void Function(camera::CameraParameters const& params) { - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params.GetDistortedSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params.GetDistortedHalfSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params.GetUndistortedSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params.GetUndistortedHalfSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params.GetFocalVector(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params.GetOpticalOffset(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params.GetCropOffset(), 1e-3); -} - -TEST(camera_params, creation) { - camera::CameraParameters params( - Eigen::Vector2i(1200, 600), - Eigen::Vector2d(300, 200), - Eigen::Vector2d(660.7, 500.3)); - EXPECT_VECTOR2D_NEAR(params.GetDistortedHalfSize(), Eigen::Vector2i(600, 300), 1e-3); - EXPECT_VECTOR2D_NEAR(params.GetUndistortedHalfSize(), Eigen::Vector2i(600, 300), 1e-3); - EXPECT_EQ(0, params.GetDistortion().size()); - EXPECT_NEAR(250, params.GetFocalLength(), 1e-3); - - // Make RAW and DISTORT be different frames - params.SetCropOffset(Eigen::Vector2i(50, 100)); - EXPECT_VECTOR2D_NEAR(params.GetCropOffset(), Eigen::Vector2i(50, 100), 1e-3); - - // Verify we get the correct intrinsic matrix - Eigen::Matrix3d k = params.GetIntrinsicMatrix(); - EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); - EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(710.7, 600.3, 1), 1e-3); - - k = params.GetIntrinsicMatrix(); - EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); - EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(660.7, 500.3, 1), 1e-3); - - k = params.GetIntrinsicMatrix(); - EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); - EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(60.7, 200.3, 1), 1e-3); - - k = params.GetIntrinsicMatrix(); - EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); - EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(600, 300, 1), 1e-3); - - k = params.GetIntrinsicMatrix(); - EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3); - EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(0, 0, 1), 1e-3); - - // See what happens when we do a copy - camera::CameraParameters params2 = params; - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params2.GetDistortedSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params2.GetDistortedHalfSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params2.GetUndistortedSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params2.GetUndistortedHalfSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params2.GetFocalVector(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params2.GetOpticalOffset(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params2.GetCropOffset(), 1e-3); - - camera::CameraParameters params3(params); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params3.GetDistortedSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params3.GetDistortedHalfSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params3.GetUndistortedSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params3.GetUndistortedHalfSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params3.GetFocalVector(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params3.GetOpticalOffset(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params3.GetCropOffset(), 1e-3); - - params3.SetOpticalOffset(Eigen::Vector2d(99, -100)); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(99, -100), params3.GetOpticalOffset(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params.GetOpticalOffset(), 1e-3); - - // Verify that we don't have memory alignment issues with shared ptrs - std::shared_ptr params4; - params4.reset(new camera::CameraParameters(params)); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params4->GetDistortedSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params4->GetDistortedHalfSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params4->GetUndistortedSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params4->GetUndistortedHalfSize(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params4->GetFocalVector(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params4->GetOpticalOffset(), 1e-3); - EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params4->GetCropOffset(), 1e-3); - - // Maybe alignment problems happen when passing to a function - Function(params); - - auto f_bind = std::bind(Function, params); - f_bind(); - - std::function f_bind2 = Function; - f_bind2(params); - - // Maybe the alignment problem is an STL container? - std::list > list; - list.emplace_back(params); - list.emplace_back(params); -} - - -TEST(camera_params, conversion) { - // raw d d_c u u_c - // 1 ---> - // 2 <--- - // 3 <------ - // 4 ------> - // 5 ---> - // 6 <--- - // 7 ------> - // 8 <------ - // 9 ---------> - // 10 <--------- - - Eigen::VectorXd distortion(1); - distortion[0] = 0.7; // FOV model - camera::CameraParameters params( - Eigen::Vector2i(1200, 600), - Eigen::Vector2d(300, 300), - Eigen::Vector2d(660.5, 500.3), distortion); - params.SetCropOffset(Eigen::Vector2i(25, 50)); - - // RAW to DISTORTED conversions - Eigen::Vector2d raw = Eigen::Vector2d::Zero(), output = Eigen::Vector2d::Zero(); - params.Convert(raw, &output); - EXPECT_NEAR(-25, output[0], 1e-6); - EXPECT_NEAR(-50, output[1], 1e-6); - params.Convert(output, &output); - EXPECT_NEAR(0, output[0], 1e-6); - EXPECT_NEAR(0, output[1], 1e-6); - - // forward backward via 3, 4 - Eigen::Vector2d input(-40, 77.3), output2; - params.Convert(input, &output); - params.Convert(output, &output2); - EXPECT_NEAR(input[0], output2[0], 1e-6); - EXPECT_NEAR(input[1], output2[1], 1e-6); - - // forward backward via 7, 8 - input << 233.4, 368.9; - params.Convert(input, &output); - params.Convert(output, &output2); - EXPECT_NEAR(input[0], output2[0], 1e-6); - EXPECT_NEAR(input[1], output2[1], 1e-6); - - // forward backward via 9, 10 - input << 542.4, 182.5; - params.Convert(input, &output); - params.Convert(output, &output2); - EXPECT_NEAR(input[0], output2[0], 1e-6); - EXPECT_NEAR(input[1], output2[1], 1e-6); - - // forward through 5, 10, 7 - input << 43.6, -213.9; - params.Convert(input, &output2); - params.Convert(output2, &output); - params.Convert(output, &output2); - EXPECT_NEAR(input[0], output2[0], 1e-6); - EXPECT_NEAR(input[1], output2[1], 1e-6); - - // forward through 8, 9, 6 - input << 542.4, 182.5; - params.Convert(input, &output2); - params.Convert(output2, &output); - params.Convert(output, &output2); - EXPECT_NEAR(input[0], output2[0], 1e-6); - EXPECT_NEAR(input[1], output2[1], 1e-6); -} diff --git a/localization/camera/test/test_camera_params.test b/localization/camera/test/test_camera_params.test new file mode 100644 index 0000000000..4d1f116c43 --- /dev/null +++ b/localization/camera/test/test_camera_params.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/camera/tools/undistort_image.cc b/localization/camera/tools/undistort_image.cc index f796a3b6f1..70c992542e 100644 --- a/localization/camera/tools/undistort_image.cc +++ b/localization/camera/tools/undistort_image.cc @@ -45,16 +45,30 @@ separate directory is specified. */ -DEFINE_string(image_list, "", "The list of images to undistort, one per line. If not specified, " - "it is assumed they are passed in directly on the command line."); +DEFINE_string(image_list, "", "A file having the list of images to undistort, one per line. " + "If not specified, it is assumed they are passed in directly on the command line."); -DEFINE_string(output_directory, "", "If not specified, undistorted images will " +DEFINE_string(output_directory, "", "Output directory. If not specified, undistorted images will " "saved in the same directory as the inputs."); +DEFINE_string(output_list, "", "Save the undistorted images with names given in this list, " + "instead of using the output directory."); + +DEFINE_string(undistorted_intrinsics, "", "Save to this file the undistorted camera intrinsics."); + +DEFINE_string(scale, "auto", + "Undistort images at different resolution, with their width " + "being a multiple of this scale compared to the camera model. Or set to " + "'auto' to select the integer scale factor based on the image size."); + DEFINE_string(undistorted_crop_win, "", - "After undistorting, crop this many pixels from all 4 sides. The adjusted " - "dimensions and optical center will be displayed below. " - "Specify as: 'crop_x crop_y'."); + "After undistorting, apply a crop window of these dimensions " + "centered at the undistorted image center. The adjusted " + "dimensions and optical center will be printed on screen. " + "Specify as: 'crop_x crop_y'. Or specify 'loose' to use the " + "smallest crop window that contains all of the " + "undistorted source pixels and keeps the optical center " + "centered."); DEFINE_bool(save_bgr, false, "Save the undistorted images as BGR instead of grayscale. (Some tools expect BGR.)"); @@ -63,7 +77,14 @@ DEFINE_bool(histogram_equalization, false, "If true, do histogram equalization."); DEFINE_string(robot_camera, "nav_cam", - "Which of bot's cameras to use. Anything except nav_cam is experimental."); + "Which of bot's cameras to use. Tested with nav_cam and sci_cam."); + +DEFINE_bool(alpha, false, + "Add an alpha channel and make areas outside the remapped input transparent. Forces PNG output format. Not " + "compatible with save_bgr."); + +DEFINE_bool(cubic, false, + "Use more expensive cubic interpolation for best image quality."); int main(int argc, char ** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); @@ -83,6 +104,18 @@ int main(int argc, char ** argv) { if (images.empty()) LOG(FATAL) << "Expecting at least one input image."; + std::vector undist_images; + if (FLAGS_output_list != "") { + std::ifstream ifs(FLAGS_output_list); + std::string image; + while (ifs >> image) + undist_images.push_back(image); + + if (undist_images.size() != images.size()) + LOG(FATAL) << "There must be as many output undistorted " + << "images as input distorted images.\n"; + } + // Read the camera config_reader::ConfigReader config; config.AddFile("cameras.config"); @@ -103,21 +136,74 @@ int main(int argc, char ** argv) { } } + double scale; + if (FLAGS_scale == "auto") { + std::string test_image_path(images[0]); + cv::Mat test_image = cv::imread(test_image_path, cv::IMREAD_UNCHANGED); + int ref_cols = cam_params.GetDistortedSize()[0]; + int ref_rows = cam_params.GetDistortedSize()[1]; + if ((test_image.cols % ref_cols == 0) + && (test_image.rows % ref_rows == 0) + && ((test_image.cols / ref_cols) == (test_image.rows / ref_rows))) { + scale = test_image.cols / ref_cols; + } else { + LOG(FATAL) << "Input image dimensions " << test_image.cols << "x" << test_image.rows + << " must be an integer multiple of camera model image dimensions " << ref_cols << "x" << ref_rows + << "."; + } + } else { + scale = std::stod(FLAGS_scale); + } + // Create the undistortion map cv::Mat floating_remap, fixed_map, interp_map; - cam_params.GenerateRemapMaps(&floating_remap); + cam_params.GenerateRemapMaps(&floating_remap, scale); + + // We have to conform to the OpenCV API, which says: + // undist_image(x, y) = dist_image(floating_remap(x, y)). + + // If floating_remap(x, y) is out of dist_image bounds, the above + // should return a black pixel, yet a straightforward application of + // this formula will result in a segfault. + + // The solution is to grow dist_image by padding it with black pixels + // so that the above API succeeds. + + // This can have the following problem though. floating_remap(x, y) + // can be huge for unreasonable distortion. So tame it. We only + // want to know that if this falls outside the image bounds, a black + // pixel is assigned to undist_image(x, y), so if it falls too + // much outside the image just assign it to a closer pixel outside + // the image. + float max_extra = 100.0f; // the furthest floating_remap(x, y) can deviate - // Deal with the fact that this map may request pixels from the - // image that are out of bounds. Hence we will need to grow the - // image before interpolating into it. + // The image dimensions + Eigen::Vector2i dims(round(scale * cam_params.GetDistortedSize()[0]), + round(scale * cam_params.GetDistortedSize()[1])); + int img_cols = dims[0], img_rows = dims[1]; - // Find the expanded image bounds cv::Vec2f start = floating_remap.at(0, 0); - double min_x = start[0], max_x = start[0]; - double min_y = start[1], max_y = start[1]; + double min_x = 0.0, max_x = 0.0, min_y = 0.0, max_y = 0.0; // will change very soon for (int col = 0; col < floating_remap.cols; col++) { for (int row = 0; row < floating_remap.rows; row++) { cv::Vec2f pix = floating_remap.at(row, col); + + // Tame floating_remap + pix[0] = std::max(pix[0], -max_extra); + pix[0] = std::min(pix[0], img_cols + max_extra); + pix[1] = std::max(pix[1], -max_extra); + pix[1] = std::min(pix[1], img_rows + max_extra); + floating_remap.at(row, col) = pix; + + if (col == 0 && row == 0) { + // initialize with the potentially adjusted value of pix. + min_x = pix[0]; + max_x = pix[0]; + min_y = pix[1]; + max_y = pix[1]; + } + + // Find the expanded (but tamed) image bounds if (pix[0] < min_x) min_x = pix[0]; if (pix[0] > max_x) @@ -128,13 +214,11 @@ int main(int argc, char ** argv) { max_y = pix[1]; } } + + // Convert the bounds to int min_x = floor(min_x); max_x = ceil(max_x); min_y = floor(min_y); max_y = ceil(max_y); - // The image dimensions - Eigen::Vector2i dims = cam_params.GetDistortedSize(); - int img_cols = dims[0], img_rows = dims[1]; - // Ensure that the expanded image is not smaller than the old one, // to make the logic simpler if (min_x > 0) @@ -164,21 +248,55 @@ int main(int argc, char ** argv) { // Convert the map for speed cv::convertMaps(floating_remap, cv::Mat(), fixed_map, interp_map, CV_16SC2); - Eigen::Vector2i dist_size = cam_params.GetDistortedSize(); - Eigen::Vector2i undist_size = cam_params.GetUndistortedSize(); - double focal_length = cam_params.GetFocalLength(); - Eigen::Vector2d optical_center = cam_params.GetUndistortedHalfSize(); + Eigen::Vector2i dist_size(round(scale * cam_params.GetDistortedSize()[0]), + round(scale * cam_params.GetDistortedSize()[1])); + Eigen::Vector2i undist_size(round(scale * cam_params.GetUndistortedSize()[0]), + round(scale * cam_params.GetUndistortedSize()[1])); + double focal_length = scale * cam_params.GetFocalLength(); + Eigen::Vector2d optical_center = scale * cam_params.GetUndistortedHalfSize(); // Handle the cropping cv::Rect cropROI; if (!FLAGS_undistorted_crop_win.empty()) { - std::vector vals; - ff_common::parseStr(FLAGS_undistorted_crop_win, vals); - if (vals.size() < 2) - LOG(FATAL) << "Could not parse --undistorted_crop_win."; + int widx, widy; + + if (FLAGS_undistorted_crop_win == "loose") { + // Figure out bounding box of undistorted pixels + + // Create a white test image to undistort + cv::Mat white(dist_size[1], dist_size[0], CV_8UC1, 255); + + // Expand the test image with a black border before interpolating into it + cv::Mat expanded_white; + cv::copyMakeBorder(white, expanded_white, border_top, border_bottom, + border_left, border_right, + cv::BORDER_CONSTANT, 0); + + // Undistort the test image + cv::Mat undist_white; + cv::remap(expanded_white, undist_white, fixed_map, interp_map, cv::INTER_LINEAR); + + // Get the bounding rectangle of the white pixels in the undistorted + // test image + cv::Rect rect = cv::boundingRect(undist_white); + + // The resulting bounding box may not be centered on the optical + // center of the undistorted image. Grow the size of the rectangle + // as needed to keep the optical center in the middle of the final + // output. + int cx = optical_center[0]; + int cy = optical_center[1]; + widx = 2 * std::max(cx - rect.x, (rect.x + rect.width) - cx); + widy = 2 * std::max(cy - rect.y, (rect.y + rect.height) - cy); + } else { + std::vector vals; + ff_common::parseStr(FLAGS_undistorted_crop_win, vals); + if (vals.size() < 2) + LOG(FATAL) << "Could not parse --undistorted_crop_win."; - int widx = vals[0]; - int widy = vals[1]; + widx = vals[0]; + widy = vals[1]; + } // A couple of sanity checks if (widx % 2 != 0 || widy % 2 != 0 ) @@ -186,19 +304,23 @@ int main(int argc, char ** argv) { if (undist_size[0] % 2 != 0 || undist_size[1] % 2 != 0 ) LOG(FATAL) << "The undistorted image dimensions must be even."; - int startx = (undist_size[0] - widx)/2; - int starty = (undist_size[1] - widy)/2; - - cropROI = cv::Rect(startx, starty, widx, widy); - if (cropROI.empty()) - LOG(FATAL) << "Empty crop region."; - std::cout << "Crop region: " << cropROI << std::endl; + // Ensure that the crop window is within the image bounds + int startx = std::max((undist_size[0] - widx)/2, 0); + int starty = std::max((undist_size[1] - widy)/2, 0); + widx = std::min(widx, undist_size[0] - startx); + widy = std::min(widy, undist_size[1] - starty); // Update these quantities undist_size[0] = widx; undist_size[1] = widy; optical_center[0] -= startx; optical_center[1] -= starty; + + cropROI = cv::Rect(startx, starty, widx, widy); + if (cropROI.empty()) + LOG(FATAL) << "Empty crop region."; + + std::cout << "Undistorted crop region: " << cropROI << std::endl; } for (size_t i = 0; i < images.size(); i++) { @@ -216,6 +338,17 @@ int main(int argc, char ** argv) { if (image.rows != img_rows || image.cols != img_cols) LOG(FATAL) << "The input images have wrong dimensions."; + // Add alpha channel if user requested it + if (FLAGS_alpha) { + cv::Mat alpha(image.rows, image.cols, CV_8UC1, 255); + std::vector channels; + cv::split(image, channels); + channels.push_back(alpha); + cv::Mat tmp_image; + cv::merge(channels, tmp_image); + image = tmp_image; + } + // Expand the image before interpolating into it cv::Scalar paddingColor = 0; cv::Mat expanded_image; @@ -225,7 +358,8 @@ int main(int argc, char ** argv) { // Undistort it cv::Mat undist_image; - cv::remap(expanded_image, undist_image, fixed_map, interp_map, cv::INTER_LINEAR); + cv::InterpolationFlags interp_mode = FLAGS_cubic ? cv::INTER_CUBIC : cv::INTER_LINEAR; + cv::remap(expanded_image, undist_image, fixed_map, interp_map, interp_mode); // Crop, if desired if (!cropROI.empty()) { @@ -236,12 +370,23 @@ int main(int argc, char ** argv) { // The output file name std::string undist_file; - if (!FLAGS_output_directory.empty()) { - // A separate output directory was specified - undist_file = FLAGS_output_directory + "/" + ff_common::basename(filename); + if (!undist_images.empty()) { + // Was specified via a list + undist_file = undist_images[i]; } else { - // Save in same directory with new name - undist_file = filename.substr(0, filename.size() - 4) + "_undistort.jpg"; + if (!FLAGS_output_directory.empty()) { + // A separate output directory was specified + undist_file = FLAGS_output_directory + "/" + ff_common::basename(filename); + } else { + // Save in same directory with new name + undist_file = filename.substr(0, filename.size() - 4) + "_undistort.jpg"; + } + if (FLAGS_alpha) { + // Typical input format is JPEG that can't represent alpha + // channel, so let's switch default output format to PNG. User + // can override using undist_images if they need to. + undist_file = undist_file.substr(0, undist_file.size() - 4) + ".png"; + } } // Save to disk the undistorted image @@ -249,7 +394,11 @@ int main(int argc, char ** argv) { cv::Mat bgr_image; if (FLAGS_save_bgr && undist_image.channels() == 1) { // Convert from grayscale to color if needed - cvtColor(undist_image, bgr_image, CV_GRAY2BGR); + #if (CV_VERSION_MAJOR >= 4) + cvtColor(undist_image, bgr_image, cv::COLOR_GRAY2BGR); + #else + cvtColor(undist_image, bgr_image, CV_GRAY2BGR); + #endif undist_image = bgr_image; } cv::imwrite(undist_file, undist_image); @@ -261,15 +410,18 @@ int main(int argc, char ** argv) { std::cout << "Focal length: " << focal_length << "\n"; std::cout << "Undistorted optical center: " << optical_center.transpose() << "\n"; - if (!FLAGS_output_directory.empty()) { - std::string intrinsics_file = FLAGS_output_directory + "/undistorted_intrinsics.txt"; + std::string intrinsics_file = FLAGS_undistorted_intrinsics; + if (intrinsics_file.empty() && !FLAGS_output_directory.empty()) + intrinsics_file = FLAGS_output_directory + "/undistorted_intrinsics.txt"; + + if (!intrinsics_file.empty()) { + std::cout << "Writing: " << intrinsics_file << std::endl; std::ofstream ofs(intrinsics_file.c_str()); ofs.precision(17); - ofs << "# Unidistored width and height, focal length, undistorted optical center\n"; + ofs << "# Undistorted width and height, focal length, undistorted optical center\n"; ofs << undist_size.transpose() << " " << focal_length << " " << optical_center.transpose() << "\n"; ofs.close(); - std::cout << "Wrote: " << intrinsics_file << std::endl; } return 0; diff --git a/localization/depth_odometry/CMakeLists.txt b/localization/depth_odometry/CMakeLists.txt new file mode 100644 index 0000000000..531977b2a6 --- /dev/null +++ b/localization/depth_odometry/CMakeLists.txt @@ -0,0 +1,140 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(depth_odometry) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + cv_bridge + ff_util + image_transport + localization_common + localization_measurements + msg_conversions + nodelet + optimization_common + point_cloud_common + vision_common +) + +# Find OpenCV +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV4WithXFeatures REQUIRED) + +find_package(Eigen3 REQUIRED) + +find_package(Ceres REQUIRED) + +find_package(PCL REQUIRED COMPONENTS common search) + +find_package(pcl_conversions REQUIRED) + +catkin_package( + INCLUDE_DIRS include ${CERES_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS} + LIBRARIES ${PROJECT_NAME} ${CERES_LIBRARIES} ${PCL_LIBRARIES} ${GTSAM_LIBRARIES} + CATKIN_DEPENDS roscpp cv_bridge ff_msgs ff_util image_transport localization_common localization_measurements msg_conversions nodelet optimization_common point_cloud_common vision_common + DEPENDS gtsam pcl +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/depth_odometry_nodelet.cc + src/depth_odometry_wrapper.cc + src/image_features_with_known_correspondences_aligner_depth_odometry.cc + src/parameter_reader.cc + src/point_to_plane_icp_depth_odometry.cc + src/utilities.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} gtsam) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_point_to_plane_icp_depth_odometry + test/test_point_to_plane_icp_depth_odometry.test + test/test_point_to_plane_icp_depth_odometry.cc + test/test_utilities.cc + ) + target_link_libraries(test_point_to_plane_icp_depth_odometry + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + + add_rostest_gtest(test_image_features_with_known_correspondences_aligner_depth_odometry + test/test_image_features_with_known_correspondences_aligner_depth_odometry.test + test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc + test/test_utilities.cc + ) + target_link_libraries(test_image_features_with_known_correspondences_aligner_depth_odometry + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + + add_rostest_gtest(test_depth_odometry_wrapper + test/test_depth_odometry_wrapper.test + test/test_depth_odometry_wrapper.cc + test/test_utilities.cc + ) + target_link_libraries(test_depth_odometry_wrapper + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/localization/depth_odometry/include/depth_odometry/depth_image_features_and_points.h b/localization/depth_odometry/include/depth_odometry/depth_image_features_and_points.h new file mode 100644 index 0000000000..d8996fe5f2 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_image_features_and_points.h @@ -0,0 +1,72 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#ifndef DEPTH_ODOMETRY_DEPTH_IMAGE_FEATURES_AND_POINTS_H_ +#define DEPTH_ODOMETRY_DEPTH_IMAGE_FEATURES_AND_POINTS_H_ + +namespace depth_odometry { +class DepthImageFeaturesAndPoints { + public: + // TODO(rsoussan): Pass clahe as boost optional ref! add default as boost none! + DepthImageFeaturesAndPoints(const localization_measurements::DepthImage& depth_image, cv::Feature2D& feature_detector, + const cv::Ptr clahe, const bool normals_required = false) + : depth_image_(depth_image) { + if (clahe) { + cv::Mat clahe_image; + clahe->apply(depth_image_.image(), clahe_image); + feature_image_.reset(new vision_common::FeatureImage(clahe_image, feature_detector)); + } else { + feature_image_.reset(new vision_common::FeatureImage(depth_image_.image(), feature_detector)); + } + + if (normals_required) { + kdtree_.reset(new pcl::search::KdTree()); + filtered_point_cloud_ = + point_cloud_common::FilteredPointCloud(depth_image_.unfiltered_point_cloud()); + kdtree_->setInputCloud(filtered_point_cloud_); + } + } + + boost::optional Normal(const Eigen::Vector3d& point_3d, const double search_radius) const { + return point_cloud_common::GetNormal(point_3d, *filtered_point_cloud_, *kdtree_, search_radius); + } + + const vision_common::FeatureImage& feature_image() const { return *feature_image_; } + + const localization_measurements::DepthImage& depth_image() const { return depth_image_; } + + private: + localization_measurements::DepthImage depth_image_; + std::unique_ptr feature_image_; + pcl::search::KdTree::Ptr kdtree_; + pcl::PointCloud::Ptr filtered_point_cloud_; +}; +} // namespace depth_odometry +#endif // DEPTH_ODOMETRY_DEPTH_IMAGE_FEATURES_AND_POINTS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry.h b/localization/depth_odometry/include/depth_odometry/depth_odometry.h new file mode 100644 index 0000000000..11ac64b7b6 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry.h @@ -0,0 +1,36 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_H_ + +#include +#include +#include +#include + +#include + +namespace depth_odometry { +class DepthOdometry { + public: + virtual boost::optional DepthImageCallback( + const localization_measurements::DepthImageMeasurement& depth_image) = 0; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_nodelet.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_nodelet.h new file mode 100644 index 0000000000..02a7e5f1d9 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_nodelet.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_NODELET_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_NODELET_H_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace depth_odometry { +class DepthOdometryNodelet : public ff_util::FreeFlyerNodelet { + public: + DepthOdometryNodelet(); + + private: + void Initialize(ros::NodeHandle* nh) final; + void SubscribeAndAdvertise(ros::NodeHandle* nh); + void PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg); + void ImageCallback(const sensor_msgs::ImageConstPtr& image_msg); + bool EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res); + + DepthOdometryWrapper depth_odometry_wrapper_; + image_transport::Subscriber image_sub_; + ros::Subscriber point_cloud_sub_; + ros::Publisher depth_odometry_pub_; + ros::ServiceServer enable_srv_; + bool enabled_; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_NODELET_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_params.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_params.h new file mode 100644 index 0000000000..6565d91094 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_params.h @@ -0,0 +1,29 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_PARAMS_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_PARAMS_H_ + +namespace depth_odometry { +struct DepthOdometryParams { + double max_time_diff; + double position_covariance_threshold; + double orientation_covariance_threshold; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_PARAMS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h new file mode 100644 index 0000000000..8190654f6b --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h @@ -0,0 +1,57 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include + +namespace depth_odometry { +class DepthOdometryWrapper { + public: + explicit DepthOdometryWrapper(const std::string& path_prefix = "localization/"); + explicit DepthOdometryWrapper(const DepthOdometryWrapperParams& params); + std::vector PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg); + std::vector ImageCallback(const sensor_msgs::ImageConstPtr& image_msg); + + private: + void Initialize(const DepthOdometryWrapperParams& params); + std::vector ProcessDepthImageIfAvailable(); + std::unique_ptr depth_odometry_; + DepthOdometryWrapperParams params_; + localization_common::TimestampedSet point_cloud_buffer_; + localization_common::TimestampedSet image_buffer_; + localization_common::Timer timer_ = localization_common::Timer("Depth Odometry"); +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h new file mode 100644 index 0000000000..16d8c2d30a --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h @@ -0,0 +1,44 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_PARAMS_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_PARAMS_H_ + +#include +#include + +#include + +#include + +namespace depth_odometry { +struct DepthOdometryWrapperParams { + double max_image_and_point_cloud_time_diff; + // icp or image_feature + std::string method; + Eigen::Isometry3d body_T_haz_cam; + Eigen::Affine3d haz_cam_A_haz_depth; + PointToPlaneICPDepthOdometryParams icp; + ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams image_features; + // Used for image and point cloud buffer + double max_buffer_size; + // Max number of depth images to generate per cycle + double max_depth_images; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_PARAMS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry.h b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry.h new file mode 100644 index 0000000000..f4e497ab5b --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_H_ +#define DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace depth_odometry { +class ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry : public DepthOdometry { + public: + explicit ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry( + const ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params); + boost::optional DepthImageCallback( + const localization_measurements::DepthImageMeasurement& depth_image) final; + const ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params() const { return params_; } + + private: + bool ValidImagePoint(const Eigen::Vector2d& image_point) const; + bool Valid3dPoint(const boost::optional& point) const; + + ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams params_; + point_cloud_common::PointCloudWithKnownCorrespondencesAligner aligner_; + std::shared_ptr previous_depth_image_features_and_points_; + std::shared_ptr latest_depth_image_features_and_points_; + localization_common::Time previous_timestamp_; + localization_common::Time latest_timestamp_; + std::unique_ptr feature_detector_and_matcher_; + cv::Ptr clahe_; + bool normals_required_; + boost::optional point_to_plane_icp_depth_odometry_; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_H_ diff --git a/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h new file mode 100644 index 0000000000..4d47dcaf09 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h @@ -0,0 +1,58 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_ +#define DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace depth_odometry { +struct ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams : public DepthOdometryParams { + point_cloud_common::PointCloudWithKnownCorrespondencesAlignerParams aligner; + vision_common::BriskFeatureDetectorAndMatcherParams brisk_feature_detector_and_matcher; + vision_common::LKOpticalFlowFeatureDetectorAndMatcherParams lk_optical_flow_feature_detector_and_matcher; + vision_common::SurfFeatureDetectorAndMatcherParams surf_feature_detector_and_matcher; + std::string detector; + // CLAHE params + bool use_clahe; + int clahe_grid_length; + double clahe_clip_limit; + // Other + double min_x_distance_to_border; + double min_y_distance_to_border; + int min_num_correspondences; + bool only_correspondences; + bool refine_estimate; + PointToPlaneICPDepthOdometryParams point_to_plane_icp; + Eigen::Matrix3d cam_intrinsics; + std::shared_ptr cam_params; + bool filter_outliers; + int filter_method; + double inlier_threshold; + double inlier_probability; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/parameter_reader.h b/localization/depth_odometry/include/depth_odometry/parameter_reader.h new file mode 100644 index 0000000000..966cd0b29e --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/parameter_reader.h @@ -0,0 +1,35 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_PARAMETER_READER_H_ +#define DEPTH_ODOMETRY_PARAMETER_READER_H_ + +#include +#include +#include +#include +#include + +namespace depth_odometry { +void LoadDepthOdometryParams(config_reader::ConfigReader& config, DepthOdometryParams& params); +void LoadDepthOdometryWrapperParams(config_reader::ConfigReader& config, DepthOdometryWrapperParams& params); +void LoadPointToPlaneICPDepthOdometryParams(config_reader::ConfigReader& config, + PointToPlaneICPDepthOdometryParams& params); +void LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams( + config_reader::ConfigReader& config, ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params); +} // namespace depth_odometry +#endif // DEPTH_ODOMETRY_PARAMETER_READER_H_ diff --git a/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry.h b/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry.h new file mode 100644 index 0000000000..a85d623826 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_H_ +#define DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_H_ + +#include +#include +#include +#include + +namespace depth_odometry { +class PointToPlaneICPDepthOdometry : public DepthOdometry { + public: + explicit PointToPlaneICPDepthOdometry(const PointToPlaneICPDepthOdometryParams& params); + boost::optional DepthImageCallback( + const localization_measurements::DepthImageMeasurement& depth_image_measurement) final; + boost::optional DepthImageCallbackWithEstimate( + const localization_measurements::DepthImageMeasurement& depth_image_measurement, + const boost::optional target_T_source_initial_estimate = boost::none); + const PointToPlaneICPDepthOdometryParams& params() const { return params_; } + + private: + pcl::PointCloud::Ptr DownsampleAndFilterCloud( + const pcl::PointCloud::Ptr& cloud) const; + + PointToPlaneICPDepthOdometryParams params_; + point_cloud_common::PointToPlaneICP icp_; + pcl::PointCloud::Ptr previous_point_cloud_with_normals_; + pcl::PointCloud::Ptr latest_point_cloud_with_normals_; + localization_common::Time previous_timestamp_; + localization_common::Time latest_timestamp_; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_H_ diff --git a/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry_params.h b/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry_params.h new file mode 100644 index 0000000000..6864813306 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry_params.h @@ -0,0 +1,44 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_PARAMS_H_ +#define DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_PARAMS_H_ + +#include +#include + +#include + +namespace depth_odometry { +struct PointToPlaneICPDepthOdometryParams : public DepthOdometryParams { + point_cloud_common::PointToPlaneICPParams icp; + bool downsample; + double downsample_leaf_size; + // Organized normal estimation + bool use_organized_normal_estimation; + pcl::IntegralImageNormalEstimation::NormalEstimationMethod normal_estimation_method; + bool use_depth_dependent_smoothing; + double max_depth_change_factor; + double normal_smoothing_size; + // Normal space sampling + bool use_normal_space_sampling; + int bins_per_axis; + int num_samples; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_PARAMS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/pose_with_covariance_and_correspondences.h b/localization/depth_odometry/include/depth_odometry/pose_with_covariance_and_correspondences.h new file mode 100644 index 0000000000..72d0056d28 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/pose_with_covariance_and_correspondences.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_POSE_WITH_COVARIANCE_AND_CORRESPONDENCES_H_ +#define DEPTH_ODOMETRY_POSE_WITH_COVARIANCE_AND_CORRESPONDENCES_H_ + +#include +#include +#include +#include + +namespace depth_odometry { +struct PoseWithCovarianceAndCorrespondences { + PoseWithCovarianceAndCorrespondences(const localization_common::PoseWithCovariance& pose_with_covariance, + const point_cloud_common::ICPCorrespondences& correspondences, + const localization_common::Time source_time, + const localization_common::Time target_time) + : pose_with_covariance(pose_with_covariance), + depth_correspondences(correspondences.source_points, correspondences.target_points), + source_time(source_time), + target_time(target_time) {} + + PoseWithCovarianceAndCorrespondences(const localization_common::PoseWithCovariance& pose_with_covariance, + const localization_measurements::DepthCorrespondences& correspondences, + const localization_common::Time source_time, + const localization_common::Time target_time) + : pose_with_covariance(pose_with_covariance), + depth_correspondences(correspondences), + source_time(source_time), + target_time(target_time) {} + + localization_common::PoseWithCovariance pose_with_covariance; + localization_measurements::DepthCorrespondences depth_correspondences; + localization_common::Time source_time; + localization_common::Time target_time; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_POSE_WITH_COVARIANCE_AND_CORRESPONDENCES_H_ diff --git a/localization/depth_odometry/include/depth_odometry/utilities.h b/localization/depth_odometry/include/depth_odometry/utilities.h new file mode 100644 index 0000000000..564d5448eb --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/utilities.h @@ -0,0 +1,40 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_UTILITIES_H_ +#define DEPTH_ODOMETRY_UTILITIES_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace depth_odometry { +ff_msgs::DepthOdometry DepthOdometryMsg(const PoseWithCovarianceAndCorrespondences& sensor_F_source_T_target, + const localization_common::PoseWithCovariance& body_F_source_T_target, + const double runtime); +ff_msgs::Odometry OdometryMsg(const localization_common::PoseWithCovariance& sensor_F_source_T_target, + const localization_common::PoseWithCovariance& body_F_source_T_target); +std::vector CorrespondencesMsg( + const localization_measurements::DepthCorrespondences& depth_correspondences); +} // namespace depth_odometry +#endif // DEPTH_ODOMETRY_UTILITIES_H_ diff --git a/localization/depth_odometry/launch/depth_odometry.launch b/localization/depth_odometry/launch/depth_odometry.launch new file mode 100644 index 0000000000..57f35c7183 --- /dev/null +++ b/localization/depth_odometry/launch/depth_odometry.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/depth_odometry/nodelet_plugins.xml b/localization/depth_odometry/nodelet_plugins.xml new file mode 100644 index 0000000000..04eccafc8d --- /dev/null +++ b/localization/depth_odometry/nodelet_plugins.xml @@ -0,0 +1,11 @@ + + + + This nodelet wraps depth_odometry + + + + + diff --git a/localization/depth_odometry/package.xml b/localization/depth_odometry/package.xml new file mode 100644 index 0000000000..ca00e05794 --- /dev/null +++ b/localization/depth_odometry/package.xml @@ -0,0 +1,48 @@ + + depth_odometry + 1.0.0 + + The depth odometry package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + cv_bridge + ff_msgs + ff_util + image_transport + localization_common + localization_measurements + msg_conversions + nodelet + optimization_common + pcl_conversions + pcl + point_cloud_common + vision_common + roscpp + cv_bridge + ff_msgs + ff_util + image_transport + localization_common + localization_measurements + msg_conversions + nodelet + optimization_common + pcl_conversions + pcl + point_cloud_common + vision_common + roscpp + + + + diff --git a/localization/depth_odometry/readme.md b/localization/depth_odometry/readme.md new file mode 100644 index 0000000000..a3ff0a1ae3 --- /dev/null +++ b/localization/depth_odometry/readme.md @@ -0,0 +1,25 @@ +\page depthodometry Depth Odometry + +# Package Overview + +### DepthOdometryNodelet +The DepthOdometryNodlet subscribes to ROS messages for online use and publishes DepthOdometry messages. + +### DepthOdometryWrapper +The DepthOdometryWrapper joins input point clouds and intensity images with the same timestamp and passes the resulting depth image measurement to the chosen DepthOdometry class. + +### DepthOdometry +Base class for performing depth odometry using input depth image measurements. Child classes include ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry and PointToPlaneICPDepthOdometry. + +### ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry +Tracks image features between successive depth image measurements using only the intensity images of the measurements and performs alignement with the correspondending 3d points. Supports brisk, surf, and lucas kanade optical flow tracking and optimization-based point to point, point to plane, and symmetric point to plane 3d point aligment. See the point\_cloud\_common package for more details on aligment options and the vision\_common package for more details on image feature tracking options. + +### PointToPlaneICPDepthOdometry +Utilizes a chosen variant of point to plane ICP (nonsymmetric cost, symmetric cost, coarse to fine, etc.) to align successive depth image measurements. See the point\_cloud\_common package for more details on the point to plane ICP options. + +# Inputs +* `/hw/depth\_haz/extended/amplitude\_int` +* `/hw/depth\_haz/points` + +# Outputs +* `/loc/depth/odometry` diff --git a/localization/depth_odometry/src/depth_odometry_nodelet.cc b/localization/depth_odometry/src/depth_odometry_nodelet.cc new file mode 100644 index 0000000000..c759b5ee99 --- /dev/null +++ b/localization/depth_odometry/src/depth_odometry_nodelet.cc @@ -0,0 +1,73 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace mc = msg_conversions; + +DepthOdometryNodelet::DepthOdometryNodelet() : ff_util::FreeFlyerNodelet(NODE_DEPTH_ODOM, true), enabled_(false) {} + +void DepthOdometryNodelet::Initialize(ros::NodeHandle* nh) { SubscribeAndAdvertise(nh); } + +void DepthOdometryNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { + const std::string point_cloud_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX); + point_cloud_sub_ = nh->subscribe( + point_cloud_topic, 1, &DepthOdometryNodelet::PointCloudCallback, this, ros::TransportHints().tcpNoDelay()); + + image_transport::ImageTransport image_transport(*nh); + const std::string image_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE); + image_sub_ = image_transport.subscribe(image_topic, 1, &DepthOdometryNodelet::ImageCallback, this); + depth_odometry_pub_ = nh->advertise(TOPIC_LOCALIZATION_DEPTH_ODOM, 10); + enable_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_DO_ENABLE, &DepthOdometryNodelet::EnableService, this); +} + +void DepthOdometryNodelet::PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg) { + if (!enabled_) return; + const auto depth_odometry_msgs = depth_odometry_wrapper_.PointCloudCallback(point_cloud_msg); + for (const auto& depth_odometry_msg : depth_odometry_msgs) { + depth_odometry_pub_.publish(depth_odometry_msg); + } +} + +void DepthOdometryNodelet::ImageCallback(const sensor_msgs::ImageConstPtr& image_msg) { + if (!enabled_) return; + const auto depth_odometry_msgs = depth_odometry_wrapper_.ImageCallback(image_msg); + for (const auto& depth_odometry_msg : depth_odometry_msgs) { + depth_odometry_pub_.publish(depth_odometry_msg); + } +} + +bool DepthOdometryNodelet::EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res) { + enabled_ = req.enable; + res.success = true; + return true; +} +} // namespace depth_odometry + +PLUGINLIB_EXPORT_CLASS(depth_odometry::DepthOdometryNodelet, nodelet::Nodelet); diff --git a/localization/depth_odometry/src/depth_odometry_wrapper.cc b/localization/depth_odometry/src/depth_odometry_wrapper.cc new file mode 100644 index 0000000000..36452090c7 --- /dev/null +++ b/localization/depth_odometry/src/depth_odometry_wrapper.cc @@ -0,0 +1,130 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +DepthOdometryWrapper::DepthOdometryWrapper(const std::string& config_prefix) { + config_reader::ConfigReader config; + config.AddFile("cameras.config"); + config.AddFile("transforms.config"); + config.AddFile("geometry.config"); + config.AddFile((config_prefix + "depth_odometry.config").c_str()); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + DepthOdometryWrapperParams params; + LoadDepthOdometryWrapperParams(config, params); + Initialize(params); +} + +DepthOdometryWrapper::DepthOdometryWrapper(const DepthOdometryWrapperParams& params) { Initialize(params); } + +void DepthOdometryWrapper::Initialize(const DepthOdometryWrapperParams& params) { + params_ = params; + if (params_.method == "icp") { + depth_odometry_.reset(new PointToPlaneICPDepthOdometry(params.icp)); + } else if (params_.method == "image_feature") { + depth_odometry_.reset(new ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry(params.image_features)); + } else { + LogFatal("DepthOdometryWrapper: Invalid depth odometry method selected."); + } + + point_cloud_buffer_ = localization_common::TimestampedSet(params.max_buffer_size); + image_buffer_ = localization_common::TimestampedSet(params.max_buffer_size); +} + +std::vector DepthOdometryWrapper::PointCloudCallback( + const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg) { + point_cloud_buffer_.Add(lc::TimeFromHeader(point_cloud_msg->header), point_cloud_msg); + return ProcessDepthImageIfAvailable(); +} + +std::vector DepthOdometryWrapper::ImageCallback(const sensor_msgs::ImageConstPtr& image_msg) { + image_buffer_.Add(lc::TimeFromHeader(image_msg->header), image_msg); + return ProcessDepthImageIfAvailable(); +} + +std::vector DepthOdometryWrapper::ProcessDepthImageIfAvailable() { + std::vector depth_image_measurements; + boost::optional latest_added_point_cloud_msg_time; + boost::optional latest_added_image_msg_time; + int added_depth_images = 0; + // Point clouds and depth images for the same measurement arrive on different topics. + // Correlate pairs of these if possible. + // Only add up to max_depth_images per cycle. + for (auto image_msg_it = image_buffer_.set().rbegin(); + image_msg_it != image_buffer_.set().rend() && added_depth_images < params_.max_depth_images; ++image_msg_it) { + const auto image_msg_timestamp = image_msg_it->first; + const auto point_cloud_msg = point_cloud_buffer_.Closest(image_msg_timestamp); + if (point_cloud_msg && + std::abs(point_cloud_msg->timestamp - image_msg_timestamp) <= params_.max_image_and_point_cloud_time_diff) { + const auto depth_image_measurement = + lm::MakeDepthImageMeasurement(point_cloud_msg->value, image_msg_it->second, params_.haz_cam_A_haz_depth); + if (!depth_image_measurement) { + LogError("ProcessDepthImageIfAvailable: Failed to create depth image measurement."); + continue; + } + depth_image_measurements.emplace_back(*depth_image_measurement); + ++added_depth_images; + if (!latest_added_point_cloud_msg_time) latest_added_point_cloud_msg_time = point_cloud_msg->timestamp; + if (!latest_added_image_msg_time) latest_added_image_msg_time = image_msg_timestamp; + } + } + + // Remove any measuremets older than measurements used for depth image creation + // Also remove measurements used for depth image creation + if (latest_added_point_cloud_msg_time) { + point_cloud_buffer_.RemoveOldValues(*latest_added_point_cloud_msg_time); + point_cloud_buffer_.Remove(*latest_added_point_cloud_msg_time); + } + if (latest_added_image_msg_time) { + image_buffer_.RemoveOldValues(*latest_added_image_msg_time); + image_buffer_.Remove(*latest_added_image_msg_time); + } + + std::vector depth_odometry_msgs; + for (const auto& depth_image_measurement : depth_image_measurements) { + timer_.Start(); + auto sensor_F_source_T_target = depth_odometry_->DepthImageCallback(depth_image_measurement); + timer_.Stop(); + if (sensor_F_source_T_target) { + const lc::PoseWithCovariance body_F_source_T_target = lc::FrameChangeRelativePoseWithCovariance( + sensor_F_source_T_target->pose_with_covariance, params_.body_T_haz_cam); + ff_msgs::DepthOdometry depth_odometry_msg = + DepthOdometryMsg(*sensor_F_source_T_target, body_F_source_T_target, timer_.last_value()); + depth_odometry_msgs.emplace_back(depth_odometry_msg); + } + } + return depth_odometry_msgs; +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc b/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc new file mode 100644 index 0000000000..faf0e79b5f --- /dev/null +++ b/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc @@ -0,0 +1,238 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace pc = point_cloud_common; +namespace vc = vision_common; + +ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry( + const ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params) + : params_(params), aligner_(params.aligner) { + if (params_.detector == "brisk") { + feature_detector_and_matcher_.reset( + new vc::BriskFeatureDetectorAndMatcher(params_.brisk_feature_detector_and_matcher)); + } else if (params_.detector == "lk_optical_flow") { + feature_detector_and_matcher_.reset( + new vc::LKOpticalFlowFeatureDetectorAndMatcher(params_.lk_optical_flow_feature_detector_and_matcher)); + } else if (params_.detector == "surf") { + feature_detector_and_matcher_.reset( + new vc::SurfFeatureDetectorAndMatcher(params_.surf_feature_detector_and_matcher)); + } else { + LogFatal("ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry: Invalid feature detector and matcher."); + } + + if (params_.use_clahe) + clahe_ = cv::createCLAHE(params_.clahe_clip_limit, cv::Size(params_.clahe_grid_length, params_.clahe_grid_length)); + + normals_required_ = params_.aligner.use_point_to_plane_cost || params_.aligner.use_symmetric_point_to_plane_cost; + + if (params_.refine_estimate) { + point_to_plane_icp_depth_odometry_ = PointToPlaneICPDepthOdometry(params_.point_to_plane_icp); + } +} + +boost::optional +ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback( + const lm::DepthImageMeasurement& depth_image_measurement) { + if (!previous_depth_image_features_and_points_ && !latest_depth_image_features_and_points_) { + latest_depth_image_features_and_points_.reset(new DepthImageFeaturesAndPoints( + depth_image_measurement.depth_image, *(feature_detector_and_matcher_->detector()), clahe_, normals_required_)); + latest_timestamp_ = depth_image_measurement.timestamp; + if (!params_.only_correspondences && params_.refine_estimate) + point_to_plane_icp_depth_odometry_->DepthImageCallback(depth_image_measurement); + return boost::none; + } + const lc::Time timestamp = depth_image_measurement.timestamp; + if (timestamp < latest_timestamp_) { + LogError("DepthImageCallback: Out of order measurement received."); + return boost::none; + } + + previous_depth_image_features_and_points_ = latest_depth_image_features_and_points_; + previous_timestamp_ = latest_timestamp_; + latest_depth_image_features_and_points_.reset(new DepthImageFeaturesAndPoints( + depth_image_measurement.depth_image, *(feature_detector_and_matcher_->detector()), clahe_, normals_required_)); + latest_timestamp_ = timestamp; + + const double time_diff = latest_timestamp_ - previous_timestamp_; + if (time_diff > params_.max_time_diff) { + LogError("DepthImageCallback: Time difference too large, time diff: " << time_diff); + return boost::none; + } + + const auto& matches = feature_detector_and_matcher_->Match(previous_depth_image_features_and_points_->feature_image(), + latest_depth_image_features_and_points_->feature_image()); + + // Get 3d points and required normals for matches + // Continue if any of these, including image points, are invalid + std::vector source_image_points; + std::vector target_image_points; + std::vector source_landmarks; + std::vector target_landmarks; + std::vector source_normals; + std::vector target_normals; + for (int i = 0; i < static_cast(matches.size()); ++i) { + const auto& match = matches[i]; + const auto& source_image_point = match.source_point; + const auto& target_image_point = match.target_point; + if (!ValidImagePoint(source_image_point) || !ValidImagePoint(target_image_point)) continue; + const auto source_point_3d = previous_depth_image_features_and_points_->depth_image().InterpolatePoint3D( + source_image_point.x(), source_image_point.y()); + const auto target_point_3d = latest_depth_image_features_and_points_->depth_image().InterpolatePoint3D( + target_image_point.x(), target_image_point.y()); + if (!Valid3dPoint(source_point_3d) || !Valid3dPoint(target_point_3d)) continue; + const Eigen::Vector3d source_landmark = pc::Vector3d(*source_point_3d); + const Eigen::Vector3d target_landmark = pc::Vector3d(*target_point_3d); + if (normals_required_ && !params_.only_correspondences) { + const auto target_normal = + latest_depth_image_features_and_points_->Normal(target_landmark, params_.aligner.normal_search_radius); + if (!target_normal) continue; + if (params_.aligner.use_symmetric_point_to_plane_cost) { + const auto source_normal = + previous_depth_image_features_and_points_->Normal(source_landmark, params_.aligner.normal_search_radius); + if (!source_normal) continue; + source_normals.emplace_back(*source_normal); + } + target_normals.emplace_back(*target_normal); + } + + source_image_points.emplace_back(source_image_point); + target_image_points.emplace_back(target_image_point); + source_landmarks.emplace_back(source_landmark); + target_landmarks.emplace_back(target_landmark); + } + + if (target_landmarks.size() < params_.min_num_correspondences) { + LogDebug("DepthImageCallback: Too few points provided, need " << params_.min_num_correspondences << " but given " + << target_landmarks.size() << "."); + return boost::none; + } + + if (params_.filter_outliers) { + std::vector inliers; + std::vector source_points, target_points; + // Undistort and store image points as CV points + for (int i = 0; i < source_image_points.size(); ++i) { + Eigen::Vector2d undistorted_source, undistorted_target; + params_.cam_params->Convert(source_image_points[i], + &undistorted_source); + params_.cam_params->Convert(target_image_points[i], + &undistorted_target); + source_points.emplace_back(cv::Point2d(undistorted_source.x(), undistorted_source.y())); + target_points.emplace_back(cv::Point2d(undistorted_target.x(), undistorted_target.y())); + } + cv::Mat intrinsics; + cv::eigen2cv(params_.cam_intrinsics, intrinsics); + // TODO(rsoussan): Pass iterations limit if OpenCV version upgraded + cv::findEssentialMat(source_points, target_points, intrinsics, params_.filter_method, params_.inlier_probability, + params_.inlier_threshold, inliers); + // Filter inliers in source and target point sets + source_image_points.erase(std::remove_if(source_image_points.begin(), source_image_points.end(), + [&source_image_points, &inliers](const Eigen::Vector2d& p) { + return static_cast(inliers[(&p - &*source_image_points.begin())]) == + 0; + }), + source_image_points.end()); + target_image_points.erase(std::remove_if(target_image_points.begin(), target_image_points.end(), + [&target_image_points, &inliers](const Eigen::Vector2d& p) { + return static_cast(inliers[(&p - &*target_image_points.begin())]) == + 0; + }), + target_image_points.end()); + source_landmarks.erase(std::remove_if(source_landmarks.begin(), source_landmarks.end(), + [&source_landmarks, &inliers](const Eigen::Vector3d& p) { + return static_cast(inliers[(&p - &*source_landmarks.begin())]) == 0; + }), + source_landmarks.end()); + target_landmarks.erase(std::remove_if(target_landmarks.begin(), target_landmarks.end(), + [&target_landmarks, &inliers](const Eigen::Vector3d& p) { + return static_cast(inliers[(&p - &*target_landmarks.begin())]) == 0; + }), + target_landmarks.end()); + } + + if (params_.only_correspondences) { + return PoseWithCovarianceAndCorrespondences( + lc::PoseWithCovariance(Eigen::Isometry3d::Identity(), lc::PoseCovariance()), + lm::DepthCorrespondences(source_image_points, target_image_points, source_landmarks, target_landmarks), + previous_timestamp_, latest_timestamp_); + } + + // TODO(rsoussan): This isn't required with std:optional, remove when upgrade to c++17 and change normals + // containers to be boost::optional types + boost::optional&> source_normals_ref = + normals_required_ && params_.aligner.use_symmetric_point_to_plane_cost + ? boost::optional&>(source_normals) + : boost::none; + boost::optional&> target_normals_ref = + normals_required_ ? boost::optional&>(target_normals) : boost::none; + + auto target_T_source = + aligner_.ComputeRelativeTransform(source_landmarks, target_landmarks, source_normals_ref, target_normals_ref); + if (!target_T_source) { + LogError("DepthImageCallback: Failed to get relative transform."); + return boost::none; + } + + if (params_.refine_estimate) { + return point_to_plane_icp_depth_odometry_->DepthImageCallbackWithEstimate(depth_image_measurement, + target_T_source->pose); + } + + const auto source_T_target = lc::InvertPoseWithCovariance(*target_T_source); + + if (!lc::PoseCovarianceSane(source_T_target.covariance, params_.position_covariance_threshold, + params_.orientation_covariance_threshold)) { + LogError("DepthImageCallback: Sanity check failed - invalid covariance."); + return boost::none; + } + + return PoseWithCovarianceAndCorrespondences( + source_T_target, + lm::DepthCorrespondences(source_image_points, target_image_points, source_landmarks, target_landmarks), + previous_timestamp_, latest_timestamp_); +} + +bool ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::ValidImagePoint( + const Eigen::Vector2d& image_point) const { + const int cols = latest_depth_image_features_and_points_->feature_image().cols(); + const int rows = latest_depth_image_features_and_points_->feature_image().rows(); + const double x_distance_to_border = std::min(image_point.x(), cols - image_point.x()); + const double y_distance_to_border = std::min(image_point.y(), rows - image_point.y()); + return (x_distance_to_border >= params_.min_x_distance_to_border && + y_distance_to_border >= params_.min_y_distance_to_border); +} + +bool ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::Valid3dPoint( + const boost::optional& point) const { + return point && pc::ValidPoint(*point) && point->z >= 0; +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/src/parameter_reader.cc b/localization/depth_odometry/src/parameter_reader.cc new file mode 100644 index 0000000000..c26ce440e9 --- /dev/null +++ b/localization/depth_odometry/src/parameter_reader.cc @@ -0,0 +1,112 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include +#include + +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace mc = msg_conversions; +namespace pc = point_cloud_common; +namespace vc = vision_common; + +void LoadDepthOdometryWrapperParams(config_reader::ConfigReader& config, DepthOdometryWrapperParams& params) { + params.max_image_and_point_cloud_time_diff = mc::LoadDouble(config, "max_image_and_point_cloud_time_diff"); + params.method = mc::LoadString(config, "depth_odometry_method"); + params.body_T_haz_cam = msg_conversions::LoadEigenTransform(config, "haz_cam_transform"); + params.haz_cam_A_haz_depth = Eigen::Affine3d::Identity(); + LoadPointToPlaneICPDepthOdometryParams(config, params.icp); + LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(config, params.image_features); + params.max_buffer_size = mc::LoadDouble(config, "max_buffer_size"); + params.max_depth_images = mc::LoadDouble(config, "max_depth_images"); +} + +void LoadDepthOdometryParams(config_reader::ConfigReader& config, DepthOdometryParams& params) { + params.max_time_diff = mc::LoadDouble(config, "max_time_diff"); + params.position_covariance_threshold = mc::LoadDouble(config, "position_covariance_threshold"); + params.orientation_covariance_threshold = mc::LoadDouble(config, "orientation_covariance_threshold"); +} + +void LoadPointToPlaneICPDepthOdometryParams(config_reader::ConfigReader& config, + PointToPlaneICPDepthOdometryParams& params) { + pc::LoadPointToPlaneICPParams(config, params.icp); + params.downsample = mc::LoadBool(config, "downsample"); + params.downsample_leaf_size = mc::LoadDouble(config, "downsample_leaf_size"); + params.use_organized_normal_estimation = mc::LoadBool(config, "use_organized_normal_estimation"); + const std::string normal_estimation_method_name = mc::LoadString(config, "organized_normal_method"); + if (normal_estimation_method_name == "avg_3d_gradient") { + params.normal_estimation_method = + pcl::IntegralImageNormalEstimation::NormalEstimationMethod::AVERAGE_3D_GRADIENT; + } else if (normal_estimation_method_name == "covariance") { + params.normal_estimation_method = + pcl::IntegralImageNormalEstimation::NormalEstimationMethod::COVARIANCE_MATRIX; + } else if (normal_estimation_method_name == "avg_depth_change") { + params.normal_estimation_method = + pcl::IntegralImageNormalEstimation::NormalEstimationMethod::AVERAGE_DEPTH_CHANGE; + } else { + LogFatal("LoadPointToPlaneICPDepthOdometryParams: Invalid normal estimation method provided."); + } + params.normal_smoothing_size = mc::LoadDouble(config, "normal_smoothing_size"); + params.use_depth_dependent_smoothing = mc::LoadBool(config, "use_depth_dependent_smoothing"); + params.max_depth_change_factor = mc::LoadDouble(config, "max_depth_change_factor"); + params.normal_smoothing_size = mc::LoadDouble(config, "normal_smoothing_size"); + params.use_normal_space_sampling = mc::LoadBool(config, "use_normal_space_sampling"); + params.bins_per_axis = mc::LoadInt(config, "bins_per_axis"); + params.num_samples = mc::LoadInt(config, "num_samples"); + LoadDepthOdometryParams(config, params); +} + +void LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams( + config_reader::ConfigReader& config, ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params) { + pc::LoadPointCloudWithKnownCorrespondencesAlignerParams(config, params.aligner); + vc::LoadBriskFeatureDetectorAndMatcherParams(config, params.brisk_feature_detector_and_matcher); + vc::LoadLKOpticalFlowFeatureDetectorAndMatcherParams(config, params.lk_optical_flow_feature_detector_and_matcher); + vc::LoadSurfFeatureDetectorAndMatcherParams(config, params.surf_feature_detector_and_matcher); + params.detector = mc::LoadString(config, "detector"); + params.use_clahe = mc::LoadBool(config, "use_clahe"); + params.clahe_grid_length = mc::LoadInt(config, "clahe_grid_length"); + params.clahe_clip_limit = mc::LoadDouble(config, "clahe_clip_limit"); + params.min_x_distance_to_border = mc::LoadDouble(config, "min_x_distance_to_border"); + params.min_y_distance_to_border = mc::LoadDouble(config, "min_y_distance_to_border"); + params.min_num_correspondences = mc::LoadInt(config, "min_num_correspondences"); + params.only_correspondences = mc::LoadBool(config, "only_correspondences"); + params.refine_estimate = mc::LoadBool(config, "refine_estimate"); + params.cam_intrinsics = lc::LoadCameraIntrinsicsMatrix(config, "haz_cam"); + params.cam_params.reset(new camera::CameraParameters(&config, "haz_cam")); + params.filter_outliers = mc::LoadBool(config, "filter_outliers"); + const std::string method = mc::LoadString(config, "filter_method"); + if (method == "ransac") + params.filter_method = cv::RANSAC; + else if (method == "lmeds") + params.filter_method = cv::LMEDS; + else if (method == "rho") + params.filter_method = cv::RHO; + else + LogFatal("Invalid outlier filter method used"); + params.inlier_threshold = mc::LoadDouble(config, "inlier_threshold"); + params.inlier_probability = mc::LoadDouble(config, "inlier_probability"); + if (params.refine_estimate) LoadPointToPlaneICPDepthOdometryParams(config, params.point_to_plane_icp); + LoadDepthOdometryParams(config, params); +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/src/point_to_plane_icp_depth_odometry.cc b/localization/depth_odometry/src/point_to_plane_icp_depth_odometry.cc new file mode 100644 index 0000000000..5a361c8b74 --- /dev/null +++ b/localization/depth_odometry/src/point_to_plane_icp_depth_odometry.cc @@ -0,0 +1,130 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace pc = point_cloud_common; + +PointToPlaneICPDepthOdometry::PointToPlaneICPDepthOdometry(const PointToPlaneICPDepthOdometryParams& params) + : params_(params), icp_(params.icp) {} + +pcl::PointCloud::Ptr PointToPlaneICPDepthOdometry::DownsampleAndFilterCloud( + const pcl::PointCloud::Ptr& cloud) const { + pcl::PointCloud::Ptr filtered_cloud_with_normals(new pcl::PointCloud()); + if (params_.use_organized_normal_estimation) { + pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud(*cloud)); + pc::ReplaceZerosWithNans(*filtered_cloud); + pc::EstimateOrganizedNormals( + filtered_cloud, params_.normal_estimation_method, params_.use_depth_dependent_smoothing, + params_.max_depth_change_factor, params_.normal_smoothing_size, *filtered_cloud_with_normals); + pc::RemoveInvalidAndZeroPoints(*filtered_cloud_with_normals); + // Downsample after estimating organized normals since organized normal estimation applies some smoothing and is + // fast enough to use before downsampling (and more accurate before downsampling) + if (params_.downsample) { + filtered_cloud_with_normals = + pc::DownsamplePointCloud(filtered_cloud_with_normals, params_.downsample_leaf_size); + } + } else { + if (params_.downsample) { + const auto downsampled_cloud = pc::DownsamplePointCloud(cloud, params_.downsample_leaf_size); + filtered_cloud_with_normals = pc::FilteredPointCloudWithNormals( + downsampled_cloud, params_.icp.search_radius); + } else { + filtered_cloud_with_normals = + pc::FilteredPointCloudWithNormals(cloud, params_.icp.search_radius); + } + } + if (params_.use_normal_space_sampling) { + pc::NormalSpaceSubsampling(filtered_cloud_with_normals, params_.bins_per_axis, + params_.num_samples); + } + return filtered_cloud_with_normals; +} + +boost::optional PointToPlaneICPDepthOdometry::DepthImageCallback( + const lm::DepthImageMeasurement& depth_image_measurement) { + return DepthImageCallbackWithEstimate(depth_image_measurement); +} + +boost::optional PointToPlaneICPDepthOdometry::DepthImageCallbackWithEstimate( + const localization_measurements::DepthImageMeasurement& depth_image_measurement, + const boost::optional target_T_source_initial_estimate) { + if (!previous_point_cloud_with_normals_ && !latest_point_cloud_with_normals_) { + latest_point_cloud_with_normals_ = + DownsampleAndFilterCloud(depth_image_measurement.depth_image.unfiltered_point_cloud()); + latest_timestamp_ = depth_image_measurement.timestamp; + return boost::none; + } + const lc::Time timestamp = depth_image_measurement.timestamp; + if (timestamp < latest_timestamp_) { + LogWarning("DepthImageCallback: Out of order measurement received."); + return boost::none; + } + + previous_point_cloud_with_normals_ = latest_point_cloud_with_normals_; + previous_timestamp_ = latest_timestamp_; + latest_point_cloud_with_normals_ = + DownsampleAndFilterCloud(depth_image_measurement.depth_image.unfiltered_point_cloud()); + latest_timestamp_ = timestamp; + + const double time_diff = latest_timestamp_ - previous_timestamp_; + if (time_diff > params_.max_time_diff) { + LogWarning("DepthImageCallback: Time difference too large, time diff: " << time_diff); + return boost::none; + } + + if (target_T_source_initial_estimate) { + pcl::transformPointCloudWithNormals(*previous_point_cloud_with_normals_, *previous_point_cloud_with_normals_, + target_T_source_initial_estimate->matrix()); + } + auto target_T_source = + icp_.ComputeRelativeTransform(previous_point_cloud_with_normals_, latest_point_cloud_with_normals_); + if (!target_T_source) { + LogWarning("DepthImageCallback: Failed to get relative transform."); + return boost::none; + } + + if (target_T_source_initial_estimate) { + target_T_source->pose = target_T_source->pose * *target_T_source_initial_estimate; + // TODO(rsoussan): Frame change covariance! + } + + const auto source_T_target = lc::InvertPoseWithCovariance(*target_T_source); + + if (!lc::PoseCovarianceSane(source_T_target.covariance, params_.position_covariance_threshold, + params_.orientation_covariance_threshold)) { + LogWarning("DepthImageCallback: Sanity check failed - invalid covariance."); + return boost::none; + } + + const auto correspondences = icp_.correspondences(); + if (!correspondences) { + LogWarning("DepthImageCallback: Failed to get correspondences."); + return boost::none; + } + + return PoseWithCovarianceAndCorrespondences(source_T_target, *correspondences, previous_timestamp_, + latest_timestamp_); +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/src/utilities.cc b/localization/depth_odometry/src/utilities.cc new file mode 100644 index 0000000000..5e8f059fdd --- /dev/null +++ b/localization/depth_odometry/src/utilities.cc @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +ff_msgs::DepthOdometry DepthOdometryMsg(const PoseWithCovarianceAndCorrespondences& sensor_F_source_T_target, + const lc::PoseWithCovariance& body_F_source_T_target, const double runtime) { + ff_msgs::DepthOdometry depth_odometry_msg; + depth_odometry_msg.odometry = OdometryMsg(sensor_F_source_T_target.pose_with_covariance, body_F_source_T_target); + depth_odometry_msg.correspondences = CorrespondencesMsg(sensor_F_source_T_target.depth_correspondences); + depth_odometry_msg.valid_image_points = sensor_F_source_T_target.depth_correspondences.valid_image_points; + depth_odometry_msg.valid_points_3d = sensor_F_source_T_target.depth_correspondences.valid_3d_points; + lc::TimeToHeader(sensor_F_source_T_target.source_time, depth_odometry_msg.header); + lc::TimeToMsg(sensor_F_source_T_target.source_time, depth_odometry_msg.odometry.source_time); + lc::TimeToMsg(sensor_F_source_T_target.target_time, depth_odometry_msg.odometry.target_time); + depth_odometry_msg.runtime = runtime; + depth_odometry_msg.header.frame_id = "haz_cam"; + return depth_odometry_msg; +} + +ff_msgs::Odometry OdometryMsg(const lc::PoseWithCovariance& sensor_F_source_T_target, + const lc::PoseWithCovariance& body_F_source_T_target) { + ff_msgs::Odometry odometry_msg; + mc::EigenPoseCovarianceToMsg(sensor_F_source_T_target.pose, sensor_F_source_T_target.covariance, + odometry_msg.sensor_F_source_T_target); + mc::EigenPoseCovarianceToMsg(body_F_source_T_target.pose, body_F_source_T_target.covariance, + odometry_msg.body_F_source_T_target); + return odometry_msg; +} + +std::vector CorrespondencesMsg(const lm::DepthCorrespondences& depth_correspondences) { + std::vector correspondences_msg; + int num_points = 0; + if (depth_correspondences.valid_3d_points) + num_points = depth_correspondences.source_3d_points.size(); + else if (depth_correspondences.valid_image_points) + num_points = depth_correspondences.source_image_points.size(); + for (int i = 0; i < num_points; ++i) { + ff_msgs::DepthCorrespondence correspondence; + if (depth_correspondences.valid_3d_points) { + mc::VectorToMsg(depth_correspondences.source_3d_points[i], correspondence.source_3d_point); + mc::VectorToMsg(depth_correspondences.target_3d_points[i], correspondence.target_3d_point); + } + if (depth_correspondences.valid_image_points) { + mc::Vector2dToMsg(depth_correspondences.source_image_points[i], correspondence.source_image_point); + mc::Vector2dToMsg(depth_correspondences.target_image_points[i], correspondence.target_image_point); + } + correspondences_msg.emplace_back(correspondence); + } + return correspondences_msg; +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/test/test_depth_odometry_wrapper.cc b/localization/depth_odometry/test/test_depth_odometry_wrapper.cc new file mode 100644 index 0000000000..c571ba98a9 --- /dev/null +++ b/localization/depth_odometry/test/test_depth_odometry_wrapper.cc @@ -0,0 +1,575 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include +#include + +#include + +namespace dd = depth_odometry; +namespace lc = localization_common; + +TEST(DepthOdometryWrapperTester, PointToPlaneICP) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.icp.icp.search_radius = 1.0; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::CubicPointsMsg(source_timestamp); + const auto source_image_msg = dd::ImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = dd::TransformPointsMsg(target_timestamp, source_points_msg, target_T_source); + const auto target_image_msg = dd::ImageMsg(target_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_source_T_target, target_T_source.inverse(), 1e-4); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_source_T_target, true_body_F_source_T_target, 1e-4); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = dd::TransformPointsMsg(target_2_timestamp, target_points_msg, target_2_T_target); + const auto target_2_image_msg = dd::ImageMsg(target_2_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_target_T_target_2, target_2_T_target.inverse(), 1e-4); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_target_T_target_2, true_body_F_target_T_target_2, 1e-4); + } +} + +TEST(DepthOdometryWrapperTester, SymmetricPointToPlaneICP) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.icp.icp.search_radius = 1.0; + params.icp.icp.symmetric_objective = true; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::CubicPointsMsg(source_timestamp); + const auto source_image_msg = dd::ImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = dd::TransformPointsMsg(target_timestamp, source_points_msg, target_T_source); + const auto target_image_msg = dd::ImageMsg(target_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_source_T_target, target_T_source.inverse(), 1e-4); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_source_T_target, true_body_F_source_T_target, 1e-4); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = dd::TransformPointsMsg(target_2_timestamp, target_points_msg, target_2_T_target); + const auto target_2_image_msg = dd::ImageMsg(target_2_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_target_T_target_2, target_2_T_target.inverse(), 1e-4); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_target_T_target_2, true_body_F_target_T_target_2, 1e-4); + } +} + +TEST(DepthOdometryWrapperTester, CorrespondenceRejectorSymmetricPointToPlaneICP) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.icp.icp.search_radius = 1.0; + params.icp.icp.symmetric_objective = true; + params.icp.icp.correspondence_rejector_surface_normal = true; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::CubicPointsMsg(source_timestamp); + const auto source_image_msg = dd::ImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = dd::TransformPointsMsg(target_timestamp, source_points_msg, target_T_source); + const auto target_image_msg = dd::ImageMsg(target_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_source_T_target, target_T_source.inverse(), 1e-4); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_source_T_target, true_body_F_source_T_target, 1e-4); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = dd::TransformPointsMsg(target_2_timestamp, target_points_msg, target_2_T_target); + const auto target_2_image_msg = dd::ImageMsg(target_2_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_target_T_target_2, target_2_T_target.inverse(), 1e-4); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_target_T_target_2, true_body_F_target_T_target_2, 1e-4); + } +} + +TEST(DepthOdometryWrapperTester, ImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_source_T_target, target_T_source.inverse(), 1e-2); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_source_T_target, true_body_F_source_T_target, 1e-2); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_target_T_target_2, target_2_T_target.inverse(), 1e-2); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_target_T_target_2, true_body_F_target_T_target_2, 1e-2); + } +} + +TEST(DepthOdometryWrapperTester, PointToPlaneImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + params.image_features.aligner.use_point_to_plane_cost = true; + params.image_features.aligner.normal_search_radius = 3.0; + params.image_features.position_covariance_threshold = 10; + params.image_features.orientation_covariance_threshold = 10; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_source_T_target, target_T_source.inverse(), 1e-2); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_source_T_target, true_body_F_source_T_target, 1e-2); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_target_T_target_2, target_2_T_target.inverse(), 1e-2); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_target_T_target_2, true_body_F_target_T_target_2, 1e-2); + } +} + +TEST(DepthOdometryWrapperTester, SymmetricPointToPlaneImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + params.image_features.aligner.use_symmetric_point_to_plane_cost = true; + params.image_features.aligner.normal_search_radius = 3.0; + params.image_features.position_covariance_threshold = 10; + params.image_features.orientation_covariance_threshold = 10; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_source_T_target, target_T_source.inverse(), 1e-2); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_source_T_target, true_body_F_source_T_target, 1e-2); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_target_T_target_2, target_2_T_target.inverse(), 1e-2); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_target_T_target_2, true_body_F_target_T_target_2, 1e-2); + } +} + +TEST(DepthOdometryWrapperTester, SingleIterationUmeyamaImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + params.image_features.aligner.use_single_iteration_umeyama = true; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_source_T_target, target_T_source.inverse(), 1e-2); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_source_T_target, true_body_F_source_T_target, 1e-2); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_target_T_target_2, target_2_T_target.inverse(), 1e-2); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_target_T_target_2, true_body_F_target_T_target_2, 1e-2); + } +} + +TEST(DepthOdometryWrapperTester, UmeyamaInitialGuessImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + params.image_features.aligner.use_umeyama_initial_guess = true; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_source_T_target, target_T_source.inverse(), 1e-2); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_source_T_target, true_body_F_source_T_target, 1e-2); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_MATRIX_NEAR(sensor_F_target_T_target_2, target_2_T_target.inverse(), 1e-2); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_MATRIX_NEAR(body_F_target_T_target_2, true_body_F_target_T_target_2, 1e-2); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/depth_odometry/test/test_depth_odometry_wrapper.test b/localization/depth_odometry/test/test_depth_odometry_wrapper.test new file mode 100644 index 0000000000..c9cf302f9d --- /dev/null +++ b/localization/depth_odometry/test/test_depth_odometry_wrapper.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc b/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc new file mode 100644 index 0000000000..73fba934d3 --- /dev/null +++ b/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc @@ -0,0 +1,179 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include + +#include + +namespace dd = depth_odometry; +namespace lc = localization_common; + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, RampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_MATRIX_NEAR(correspondences.source_image_points[i], + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2); + EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i], + 1e-2); + } +} + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, PointToPlaneRampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.aligner.use_point_to_plane_cost = true; + params.aligner.normal_search_radius = 3.0; + params.position_covariance_threshold = 10; + params.orientation_covariance_threshold = 10; + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_MATRIX_NEAR(correspondences.source_image_points[i], + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2); + EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i], + 1e-2); + } +} + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, SymmetricPointToPlaneRampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.aligner.use_symmetric_point_to_plane_cost = true; + params.aligner.normal_search_radius = 3.0; + params.position_covariance_threshold = 10; + params.orientation_covariance_threshold = 10; + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_MATRIX_NEAR(correspondences.source_image_points[i], + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2); + EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i], + 1e-2); + } +} + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, UmeyamaRampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.aligner.use_single_iteration_umeyama = true; + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_MATRIX_NEAR(correspondences.source_image_points[i], + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2); + EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i], + 1e-2); + } +} + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, UmeyamaInitialGeussRampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.aligner.use_umeyama_initial_guess = true; + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_MATRIX_NEAR(correspondences.source_image_points[i], + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2); + EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i], + 1e-2); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.test b/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.test new file mode 100644 index 0000000000..169da15a51 --- /dev/null +++ b/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.cc b/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.cc new file mode 100644 index 0000000000..07f8636bf1 --- /dev/null +++ b/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.cc @@ -0,0 +1,157 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include + +#include + +namespace dd = depth_odometry; +namespace lc = localization_common; + +TEST(PointToPlaneICPDepthOdometryTester, PointToPlaneCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i], + 1e-2); + } +} + +TEST(PointToPlaneICPDepthOdometryTester, PointToPlaneDownsampledCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + params.downsample = true; + params.downsample_leaf_size = 0.03; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4); +} + +TEST(PointToPlaneICPDepthOdometryTester, SymmetricPointToPlaneCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + params.icp.symmetric_objective = true; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4); +} + +TEST(PointToPlaneICPDepthOdometryTester, CorrespondenceRejectorPointToPlaneCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + params.icp.correspondence_rejector_surface_normal = true; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4); +} + +TEST(PointToPlaneICPDepthOdometryTester, PointToPlane3MeasurementsCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4); + + // Add third measurement + const auto target2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target2_depth_image_measurement = + dd::TransformDepthImageMeasurement(target_depth_image_measurement, 0.2, target2_T_target); + const auto pose_with_covariance2 = icp_depth_odometry.DepthImageCallback(target2_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance2 != boost::none); + EXPECT_MATRIX_NEAR(pose_with_covariance2->pose_with_covariance.pose, target2_T_target.inverse(), 1e-4); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.test b/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.test new file mode 100644 index 0000000000..d71dbe2966 --- /dev/null +++ b/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/depth_odometry/test/test_utilities.cc b/localization/depth_odometry/test/test_utilities.cc new file mode 100644 index 0000000000..225c597898 --- /dev/null +++ b/localization/depth_odometry/test/test_utilities.cc @@ -0,0 +1,263 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace pc = point_cloud_common; +namespace vc = vision_common; + +lm::DepthImageMeasurement DefaultDepthImageMeasurement(const lc::Time timestamp) { + const auto cubic_points = pc::CubicPoints(); + const auto point_cloud = pc::PointCloud(cubic_points.first); + return lm::DepthImageMeasurement(cv::Mat(), point_cloud, timestamp); +} + +std::vector RampedPoints(int cols, int rows) { + std::vector points; + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + points.emplace_back(Eigen::Vector3d(col, row, col)); + } + } + return points; +} + +lm::DepthImageMeasurement ImageFeatureDepthImageMeasurement(const lc::Time timestamp, const cv::Point2i& offset) { + int num_markers_added; + const auto image = vc::MarkerImage(33, 33, num_markers_added, offset); + const int num_points = image.cols * image.rows; + const auto points = RampedPoints(image.cols, image.rows); + const auto point_cloud = pc::PointCloud(points); + return lm::DepthImageMeasurement(image, point_cloud, timestamp); +} + +lm::DepthImageMeasurement TransformDepthImageMeasurement(const lm::DepthImageMeasurement& depth_image_measurement, + const lc::Time timestamp, + const Eigen::Isometry3d& target_T_source) { + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); + pcl::transformPointCloud(*(depth_image_measurement.depth_image.unfiltered_point_cloud()), *transformed_cloud, + Eigen::Affine3d(target_T_source.matrix())); + return lm::DepthImageMeasurement(depth_image_measurement.depth_image.image(), transformed_cloud, timestamp); +} + +lm::DepthImageMeasurement OffsetImageFeatureDepthImageMeasurement( + const lc::Time timestamp, const lm::DepthImageMeasurement& depth_image_measurement, const cv::Point2i& offset, + const Eigen::Isometry3d& target_T_source) { + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); + pcl::transformPointCloud(*(depth_image_measurement.depth_image.unfiltered_point_cloud()), *transformed_cloud, + Eigen::Affine3d(target_T_source.matrix())); + int num_markers_added; + const auto offset_image = vc::MarkerImage(33, 33, num_markers_added, offset); + const int rows = offset_image.rows; + const int cols = offset_image.cols; + pcl::PointCloud::Ptr offset_and_transformed_cloud(new pcl::PointCloud()); + const int num_points = transformed_cloud->points.size(); + // Points are in row order + for (int i = 0; i < num_points; ++i) { + const int row = i / cols; + const int col = i - row * cols; + const int new_row = row - offset.y; + const int new_col = col - offset.x; + if (new_row >= rows || new_col >= cols || new_row < 0 || new_col < 0) { + pcl::PointXYZI zero_point; + zero_point.x = 0; + zero_point.y = 0; + zero_point.z = 0; + zero_point.intensity = 0; + offset_and_transformed_cloud->points.emplace_back(zero_point); + } else { + const int new_point_index = new_row * cols + new_col; + offset_and_transformed_cloud->points.emplace_back(transformed_cloud->points[new_point_index]); + } + } + return lm::DepthImageMeasurement(offset_image, offset_and_transformed_cloud, timestamp); +} + +sensor_msgs::PointCloud2ConstPtr CubicPointsMsg(const lc::Time timestamp) { + const auto cubic_points = pc::CubicPoints(); + auto point_cloud = pc::PointCloud(cubic_points.first); + // DepthImageMeasurement expects a point cloud with a width and height to correlate with an intenisty image + // TODO(rsoussan): Replace with resize when pcl version is updated + // point_cloud->resize(20, 15); + pcl::PointCloud resized_point_cloud(20, 15); + int i = 0; + for (const auto& point : point_cloud->points) { + resized_point_cloud.points[i++] = point; + } + sensor_msgs::PointCloud2 msg; + lc::TimeToHeader(timestamp, msg.header); + pcl::toROSMsg(resized_point_cloud, msg); + return sensor_msgs::PointCloud2ConstPtr(new sensor_msgs::PointCloud2(msg)); +} + +sensor_msgs::PointCloud2ConstPtr RampedPointsMsg(const lc::Time timestamp) { + const int cols = 640; + const int rows = 480; + const auto ramped_points = RampedPoints(cols, rows); + auto point_cloud = pc::PointCloud(ramped_points); + // DepthImageMeasurement expects a point cloud with a width and height to correlate with an intenisty image + // TODO(rsoussan): Replace with resize when pcl version is updated + // point_cloud->resize(cols, rows); + pcl::PointCloud resized_point_cloud(cols, rows); + int i = 0; + for (const auto& point : point_cloud->points) { + resized_point_cloud.points[i++] = point; + } + sensor_msgs::PointCloud2 msg; + lc::TimeToHeader(timestamp, msg.header); + pcl::toROSMsg(resized_point_cloud, msg); + return sensor_msgs::PointCloud2ConstPtr(new sensor_msgs::PointCloud2(msg)); +} + +sensor_msgs::PointCloud2ConstPtr TransformPointsMsg(const lc::Time timestamp, + const sensor_msgs::PointCloud2ConstPtr old_msg, + const Eigen::Isometry3d& new_T_old) { + pcl::PointCloud point_cloud; + pcl::fromROSMsg(*old_msg, point_cloud); + pcl::PointCloud transformed_cloud; + pcl::transformPointCloud(point_cloud, transformed_cloud, Eigen::Affine3d(new_T_old.matrix())); + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(transformed_cloud, msg); + lc::TimeToHeader(timestamp, msg.header); + return sensor_msgs::PointCloud2ConstPtr(new sensor_msgs::PointCloud2(msg)); +} + +sensor_msgs::PointCloud2ConstPtr OffsetAndTransformPointsMsg(const lc::Time timestamp, + const sensor_msgs::PointCloud2ConstPtr old_msg, + const cv::Point2i& offset, + const Eigen::Isometry3d& target_T_source) { + pcl::PointCloud old_point_cloud; + pcl::fromROSMsg(*old_msg, old_point_cloud); + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); + pcl::transformPointCloud(old_point_cloud, *transformed_cloud, Eigen::Affine3d(target_T_source.matrix())); + int num_markers_added; + const auto offset_image = vc::MarkerImage(33, 33, num_markers_added, offset); + const int rows = offset_image.rows; + const int cols = offset_image.cols; + pcl::PointCloud::Ptr offset_and_transformed_cloud(new pcl::PointCloud(cols, rows)); + const int num_points = transformed_cloud->points.size(); + // Points are in row order + for (int i = 0; i < num_points; ++i) { + const int row = i / cols; + const int col = i - row * cols; + const int new_row = row - offset.y; + const int new_col = col - offset.x; + if (new_row >= rows || new_col >= cols || new_row < 0 || new_col < 0) { + pcl::PointXYZI zero_point; + zero_point.x = 0; + zero_point.y = 0; + zero_point.z = 0; + zero_point.intensity = 0; + offset_and_transformed_cloud->points[i] = zero_point; + } else { + const int new_point_index = new_row * cols + new_col; + offset_and_transformed_cloud->points[i] = transformed_cloud->points[new_point_index]; + } + } + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(*offset_and_transformed_cloud, msg); + lc::TimeToHeader(timestamp, msg.header); + return sensor_msgs::PointCloud2ConstPtr(new sensor_msgs::PointCloud2(msg)); +} + +sensor_msgs::ImageConstPtr ImageMsg(const lc::Time timestamp) { + cv_bridge::CvImage msg_bridge; + msg_bridge.encoding = sensor_msgs::image_encodings::MONO8; + msg_bridge.image = cv::Mat(15, 20, CV_8UC1); + auto msg = msg_bridge.toImageMsg(); + lc::TimeToHeader(timestamp, msg->header); + return sensor_msgs::ImageConstPtr(msg); +} + +sensor_msgs::ImageConstPtr MarkerImageMsg(const lc::Time timestamp, const cv::Point2i& offset) { + cv_bridge::CvImage msg_bridge; + msg_bridge.encoding = sensor_msgs::image_encodings::MONO8; + int num_markers_added; + msg_bridge.image = vc::MarkerImage(33, 33, num_markers_added, offset); + auto msg = msg_bridge.toImageMsg(); + lc::TimeToHeader(timestamp, msg->header); + return sensor_msgs::ImageConstPtr(msg); +} + +PointToPlaneICPDepthOdometryParams DefaultPointToPlaneICPDepthOdometryParams() { + PointToPlaneICPDepthOdometryParams params; + params.icp = pc::DefaultPointToPlaneICPParams(); + params.downsample = false; + params.downsample_leaf_size = 0.02; + params.use_organized_normal_estimation = false; + params.max_depth_change_factor = 0.02; + params.normal_smoothing_size = 10.0; + params.use_normal_space_sampling = false; + params.bins_per_axis = 15; + params.num_samples = 3300; + DefaultDepthOdometryParams(params); + return params; +} + +ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams +DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams() { + ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams params; + params.aligner = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.lk_optical_flow_feature_detector_and_matcher = vc::DefaultLKOpticalFlowFeatureDetectorAndMatcherParams(); + params.detector = "lk_optical_flow"; + params.use_clahe = false; + params.clahe_grid_length = 8; + params.clahe_clip_limit = 40; + params.min_x_distance_to_border = 0; + params.min_y_distance_to_border = 0; + params.min_num_correspondences = 0; + params.only_correspondences = false; + params.refine_estimate = false; + params.filter_outliers = false; + DefaultDepthOdometryParams(params); + return params; +} + +void DefaultDepthOdometryParams(DepthOdometryParams& params) { + params.max_time_diff = 1.0; + params.position_covariance_threshold = 1.0; + params.orientation_covariance_threshold = 1.0; +} + +DepthOdometryWrapperParams DefaultDepthOdometryWrapperParams() { + DepthOdometryWrapperParams params; + params.max_image_and_point_cloud_time_diff = 0.01; + params.method = "icp"; + params.body_T_haz_cam = Eigen::Isometry3d::Identity(); + params.haz_cam_A_haz_depth = Eigen::Affine3d::Identity(); + params.icp = DefaultPointToPlaneICPDepthOdometryParams(); + params.image_features = DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.max_buffer_size = 10; + params.max_depth_images = 10; + return params; +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/test/test_utilities.h b/localization/depth_odometry/test/test_utilities.h new file mode 100644 index 0000000000..b1be5cb7fe --- /dev/null +++ b/localization/depth_odometry/test/test_utilities.h @@ -0,0 +1,80 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef DEPTH_ODOMETRY_TEST_UTILITIES_H_ // NOLINT +#define DEPTH_ODOMETRY_TEST_UTILITIES_H_ // NOLINT + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +namespace depth_odometry { +localization_measurements::DepthImageMeasurement DefaultDepthImageMeasurement( + const localization_common::Time timestamp); + +localization_measurements::DepthImageMeasurement ImageFeatureDepthImageMeasurement( + const localization_common::Time timestamp, const cv::Point2i& offset = cv::Point2i(0, 0)); + +localization_measurements::DepthImageMeasurement TransformDepthImageMeasurement( + const localization_measurements::DepthImageMeasurement& depth_image_measurement, + const localization_common::Time timestamp, const Eigen::Isometry3d& target_T_source); + +localization_measurements::DepthImageMeasurement OffsetImageFeatureDepthImageMeasurement( + const localization_common::Time timestamp, + const localization_measurements::DepthImageMeasurement& depth_image_measurement, const cv::Point2i& offset, + const Eigen::Isometry3d& target_T_source); + +std::vector RampedPoints(int cols, int rows); + +sensor_msgs::PointCloud2ConstPtr RampedPointsMsg(const localization_common::Time timestamp); + +sensor_msgs::PointCloud2ConstPtr CubicPointsMsg(const localization_common::Time timestamp); + +sensor_msgs::ImageConstPtr ImageMsg(const localization_common::Time timestamp); + +sensor_msgs::ImageConstPtr MarkerImageMsg(const localization_common::Time timestamp, + const cv::Point2i& offset = cv::Point2i(0, 0)); + +sensor_msgs::PointCloud2ConstPtr TransformPointsMsg(const localization_common::Time timestamp, + const sensor_msgs::PointCloud2ConstPtr old_msg, + const Eigen::Isometry3d& transform); + +sensor_msgs::PointCloud2ConstPtr OffsetAndTransformPointsMsg(const localization_common::Time timestamp, + const sensor_msgs::PointCloud2ConstPtr old_msg, + const cv::Point2i& offset, + const Eigen::Isometry3d& target_T_source); + +PointToPlaneICPDepthOdometryParams DefaultPointToPlaneICPDepthOdometryParams(); + +ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams +DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + +void DefaultDepthOdometryParams(DepthOdometryParams& params); + +DepthOdometryWrapperParams DefaultDepthOdometryWrapperParams(); +} // namespace depth_odometry +#endif // DEPTH_ODOMETRY_TEST_UTILITIES_H_ // NOLINT diff --git a/localization/doc/astroloc_library_quickstart.pdf b/localization/doc/astroloc_library_quickstart.pdf new file mode 100644 index 0000000000..d4393b939e Binary files /dev/null and b/localization/doc/astroloc_library_quickstart.pdf differ diff --git a/localization/doc/images/graph_localizer.png b/localization/doc/images/graph_localizer.png new file mode 100644 index 0000000000..362abb331f Binary files /dev/null and b/localization/doc/images/graph_localizer.png differ diff --git a/localization/doc/images/graph_optimizer.png b/localization/doc/images/graph_optimizer.png new file mode 100644 index 0000000000..804ab5e3fd Binary files /dev/null and b/localization/doc/images/graph_optimizer.png differ diff --git a/localization/doc/images/graph_vio.png b/localization/doc/images/graph_vio.png new file mode 100644 index 0000000000..334528651b Binary files /dev/null and b/localization/doc/images/graph_vio.png differ diff --git a/localization/doc/images/sliding_window_graph_optimizer.png b/localization/doc/images/sliding_window_graph_optimizer.png new file mode 100644 index 0000000000..484355719f Binary files /dev/null and b/localization/doc/images/sliding_window_graph_optimizer.png differ diff --git a/localization/doc/src/absolute_pose_factor_adder.tex b/localization/doc/src/absolute_pose_factor_adder.tex new file mode 100644 index 0000000000..10c6523a29 --- /dev/null +++ b/localization/doc/src/absolute_pose_factor_adder.tex @@ -0,0 +1,45 @@ +\begin{minted}[]{c++} +#include +#include +#include +#include + +class AbsolutePoseFactorAdder + : public SingleMeasurementBasedFactorAdder< + localization_measurements:: + TimestampedPoseWithCovariance> { + public: + AbsolutePoseFactorAdder( + const AbsolutePoseFactorAdderParams& params, + const std::shared_ptr node_adder) + : SingleMeasurementBasedFactorAdder< + localization_measurements:: + TimestampedPoseWithCovariance>(params), + params_(params), + node_adder_(node_adder) {} + + private: + int AddFactorsForSingleMeasurement( + const localization_measurements:: + TimestampedPoseWithCovariance& measurement, + gtsam::NonlinearFactorGraph& factors) final { + node_adder_->AddNode(measurement.timestamp, factors); + const auto keys = node_adder_->Keys(measurement.timestamp); + // First key is pose key + const auto& pose_key = keys[0]; + const auto pose_noise = + gtsam::Prior::shared_ptr pose_prior_factor( + new gtsam::Prior( + pose_key, measurement.pose.pose, pose_noise)); + factors.push_back(pose_prior_factor); + } + + bool CanAddFactor( + const localization_common::Time time) const final { + return node_adder_->CanAddNode(time); + } + + std::shared_ptr node_adder_; + AbsolutePoseFactorAdderParams params_; +}; +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/bib.bib b/localization/doc/src/bib.bib new file mode 100644 index 0000000000..e9e19e6870 --- /dev/null +++ b/localization/doc/src/bib.bib @@ -0,0 +1,10 @@ +@article{kaess2012isam2, + title={iSAM2: Incremental smoothing and mapping using the Bayes tree}, + author={Kaess, Michael and Johannsson, Hordur and Roberts, Richard and Ila, Viorela and Leonard, John J and Dellaert, Frank}, + journal={The International Journal of Robotics Research}, + volume={31}, + number={2}, + pages={216--235}, + year={2012}, + publisher={Sage Publications Sage UK: London, England} +} diff --git a/localization/doc/src/commands.tex b/localization/doc/src/commands.tex new file mode 100644 index 0000000000..b8623c2814 --- /dev/null +++ b/localization/doc/src/commands.tex @@ -0,0 +1,63 @@ +\newcommand{\pluseq}{\mathrel{+}=} +\newcommand{\timeseq}{\mathrel{*}=} +\newcommand{\vect}[1]{\boldsymbol{#1}} +\newcommand{\matr}[1]{\boldsymbol{#1}} +\newcommand{\tra}[1]{{\textrm{Tr}}({#1})} +\newcommand{\trafo}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\textbf{T}} +\newcommand{\rot}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\textbf{R}} +\newcommand{\trans}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\textbf{t}} +\newcommand{\rotlie}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\boldsymbol{\phi}} +\newcommand{\rotliepert}[0]{\rotlie{b}{\bar{b}}_{i}} +\newcommand{\translie}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\boldsymbol{\rho}} +\newcommand{\transliepert}[0]{\translie{b}{\bar{b}}_{i}} +\newcommand{\trafolie}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\boldsymbol{\varphi}} +\newcommand{\trafoliepert}[0]{\trafolie{b}{\bar{b}}_{i}} +\newcommand{\trafolietrafo}[0]{\trafo{b}{\bar{b}}} + +%\newcommand{\trafo}[2]{\substack{\scriptscriptstyle{#1} \\ \scriptscriptstyle{#2}}T} +%\newcommand{\rot}[2]{\substack{\scriptscriptstyle{#1} \\ \scriptscriptstyle{#2}}R} +\newcommand{\expmap}[1]{\exp(#1)} +%\newcommand{\Expmap}[1]{Exp(#1)} +\newcommand{\logmap}[1]{\log(#1)} +\newcommand{\Frame}[1]{\textbf{#1}} +\newcommand{\NavState}[0]{S} +\newcommand{\liealg}[2]{\substack{\scriptscriptstyle{\textbf{#1}} \\ \scriptscriptstyle{\textbf{#2}}}\vect{\phi}} +\newcommand{\soIII}{so(3)} +\newcommand{\SOIII}{SO(3)} +\newcommand{\seIII}{se(3)} +\newcommand{\SEIII}{SE(3)} +\newcommand{\unitquat}[2]{\substack{\scriptscriptstyle{\textbf{#1}} \\ \scriptscriptstyle{\textbf{#2}}}\Phi} +\newcommand{\Vee}[1]{{\left[#1\right]}^{\textrm{v}}} +\newcommand{\skewhat}[1]{{{\left[#1\right]}^{\wedge}}} +\newcommand{\Rn}[1]{\mathbb{R}^{#1}} +\newcommand{\ith}{\textrm{i}^{\textrm{th}}} +\newcommand{\inv}[1]{#1^{-1}} +\newcommand{\ithindex}[1]{\begin{bmatrix}#1 \end{bmatrix}_{i}} +\newcommand{\fourblock}[4]{\renewcommand\arraystretch{1.3} +\mleft[ +\begin{array}{c|c} + #1 & #2 \\ \hline + #3 & #4 +\end{array}\mright]} +\newcommand{\trafoblock}[2]{\fourblock{#1}{#2}{0}{1}} +\newcommand{\vectortwodblock}[2]{\renewcommand\arraystretch{1.3} +\mleft[ +\begin{array}{c} + #1 \\ \hline + #2 +\end{array}\mright]} +\newcommand{\rowvectortwodblock}[2]{ +\mleft[ +\begin{array}{c|c} + #1 +\end{array}\mright]} +\newcommand{\blocklog}[1]{\log\left[#1\right]} +\newcommand{\adjoint}[1]{\textrm{Ad}\left[#1\right]} +\newcommand{\homonorm}[1]{h\left(\vect{#1}\right)} +\newcommand{\projection}[1]{\pi\left({#1}\right)} +\newcommand{\camintrinsics}{\matr{C}} + + + +%\newcommand{\boxplus}{\mathbin{\mathop\amssymbboxplus}} +%\newcommand{\boxminus}{\mathbin{\mathop\amssymbboxminus}} \ No newline at end of file diff --git a/localization/doc/src/factor_adder.pdf b/localization/doc/src/factor_adder.pdf new file mode 100644 index 0000000000..d944d8ce4c Binary files /dev/null and b/localization/doc/src/factor_adder.pdf differ diff --git a/localization/doc/src/graph_localizer_h.tex b/localization/doc/src/graph_localizer_h.tex new file mode 100644 index 0000000000..22dae2da01 --- /dev/null +++ b/localization/doc/src/graph_localizer_h.tex @@ -0,0 +1,42 @@ +\begin{minted}[]{c++} +#include +#include + +#include "absolute_pose_factor_adder.h" +#include "localizer_params.h" +#include "relative_pose_node_adder.h" + +class Localizer : public sliding_window_graph_optimizer:: + SlidingWindowGraphOptimizer { + public: + explicit Localizer(const LocalizerParams& params) + : params_(params) { + // Register factor and node adders + AddFactorAdder(factor_adder_); + AddSlidingWindowNodeAdder(node_adder_); + } + + void AddRelativePoseMeasurement( + const localization_measurements:: + TimestampedPoseWithCovariance& measurement) { + node_adder_->AddMeasurement(measurement); + } + + void AddAbsolutePoseMeasurement( + const localization_measurements:: + TimestampedPoseWithCovariance& measurement) { + factor_adder_->AddMeasurement(measurement); + } + + const TimestampedNodes& timestamped_nodes() + const { + return node_adder_->nodes(); + } + + private: + LocalizerParams params_; + + std::shared_ptr factor_adder_; + std::shared_ptr node_adder_; +}; +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/graph_opt.pdf b/localization/doc/src/graph_opt.pdf new file mode 100644 index 0000000000..ea93fd069f Binary files /dev/null and b/localization/doc/src/graph_opt.pdf differ diff --git a/localization/doc/src/interfacing_cc.tex b/localization/doc/src/interfacing_cc.tex new file mode 100644 index 0000000000..56da479b12 --- /dev/null +++ b/localization/doc/src/interfacing_cc.tex @@ -0,0 +1,27 @@ +\begin{minted}[]{c++} +#include "localizer.h" + +int main() { + Localizer localizer(LocalizerParams()); + // Add relative and absolute pose measurements at successive + // timestamps + for (int i = 0; i < 10; ++i) { + localizer.AddRelativePoseMeasurement( + RandomPoseMeasurement(i)); + localizer.AddAbsolutePoseMeasurement( + RandomPoseMeasurement(i)); + } + + localizer.Update(); + + // Access optimized timestamped nodes + const auto& timestamped_nodes = localizer.timestamped_nodes(); + // Access optimized GTSAM values + const auto& values = localizer.values(); + // Access GTSAM factors + const auto& factors = localizer.factors(); + // Compute covariance for a node at timestamp 1 + const auto keys = timestamped_nodes.Keys(1); + const auto covariance = localizer.Covariance(keys[0]); +} +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/loc_graph.pdf b/localization/doc/src/loc_graph.pdf new file mode 100644 index 0000000000..6305b793a2 Binary files /dev/null and b/localization/doc/src/loc_graph.pdf differ diff --git a/localization/doc/src/node_adder.pdf b/localization/doc/src/node_adder.pdf new file mode 100644 index 0000000000..38fbf4917b Binary files /dev/null and b/localization/doc/src/node_adder.pdf differ diff --git a/localization/doc/src/overview.tex b/localization/doc/src/overview.tex new file mode 100644 index 0000000000..9afce9a2ff --- /dev/null +++ b/localization/doc/src/overview.tex @@ -0,0 +1,59 @@ +\section{AstroLoc Library Overview} +The AstroLoc library offers useful tools for localization, odometry, calibration, mapping, and more using GTSAM graph-based optimization. +The library consists of graph optimizer and sliding window graph optimizer classes that use node adders and factor adders for node and factor creation, where nodes are state parameters optimized in the graph. +\begin{figure}[ht] + \centering +\includegraphics[width=0.9\textwidth]{graph_opt.pdf} + \caption{A graph optimizer contains node adders that create nodes and relative factors connecting them and factor adders that create measurement-based factors for certain nodes in the graph.} + \label{img:graph_opt} +\end{figure} +The library contains packages for each of these objects and has useful factor and node adders already implemented to help perform localization, odometry, calibration, and mapping. +\subsection{Optimizers}\label{sec:overview} +Nonlinear and ISAM2 optimizers can be used for graph optimization. +ISAM2 \cite{kaess2012isam2} is particularly helpful for mapping when keeping a large history of nodes to optimize, whereas often nonlinear optimization is sufficient for performing sliding window optimization. +\subsection{Graph Optimizer and Sliding Window Graph Optimizer} +\subsubsection{Graph Optimizer} +The graph optimizer shown in Figure ~\ref{img:graph_opt} contains a GTSAM factor graph and factor and node adders that process input measurements and output the nodes and factors used for optimization. +It contains functions to perform optimization and access covariances for its nodes. +\subsubsection{Sliding Window Graph Optimizer} +The sliding window graph optimizer in Figure ~\ref{img:sliding_window_graph_opt} extends the graph optimizer for sliding window optimization and uses sliding window node adders that remove old nodes and factors that fall outside of the window. +It uses an Update() function call that adds factors from factor adders up to the latest measurements, optimizes the graph, and slides the window as required. +The sliding window size is defined in both a maximum duration and number of nodes and is configured in the sliding window parameters. +\begin{figure}[ht] + \centering +\includegraphics[width=0.9\textwidth]{sliding_window_graph_opt.pdf} + \caption{The sliding window graph optimizer uses sliding window node adders that remove old nodes and factors as required.} + \label{img:sliding_window_graph_opt} +\end{figure} +\subsection{Node Adders} +The node adders displayed in Figure ~\ref{img:node_adder} define the node type to be optimized and are in charge of adding relative factors and nodes to the graph for those types. +A node adder is templated on measurement, node, timestamped nodes, and node adder model types. +\begin{figure}[H] + \centering +\includegraphics[width=0.9\textwidth]{node_adder.pdf} + \caption{A node adder taking measurements and using a node adder model to generate timestamped nodes and relative factors.} + \label{img:node_adder} +\end{figure} +\subsubsection{Measurement Type} +The measurement is passed to the node adder model and contains the information necessary to create future nodes and relative factors. +\subsubsection{Node and TimestampedNodes Types} +The node type specifies the state parameter optimized in the graph optimizer. +The timestamped nodes type specifies the container for interfacing with the nodes in the optimizer. +For simple, single-typed timestamped values, such as a pose or point, the TimestampedNodes class should be used. +For nodes containing multiple types (i.e. a pose, velocity, and bias for VIO), use the TimestampedCombinedNodes class and make sure to override the required functions for adding and accessing nodes. +See CombinedNavStateNodes in the nodes package for an example. +Multiple node adders can exist in a single graph optimizer that handle different node types, and all these are optimized together in the same graph. +\subsubsection{Node Adder Models} +The node adder model handles adding nodes and relative factors and stores measurements used for creating these. +Prefer using the BetweenFactorMeasurementBasedTimestampedNodeAdderModel class which adds relative factors using gtsam::BetweenFactors for a single-valued node type. +For more complicated relative factors, use MeasurementBasedTimestampedNodeAdderModel and customize the functions for adding relative nodes and factors as desired. +See the CombinedNavStateNodeAdder in the node adders package for an example. +\subsection{Factor Adders} +The factor adders illustrated in Figure ~\ref{img:factor_adder} add measurement-based factors that can depend on single or multiple node values. +For factor creation that depends on a single measurement at a time, use SingleMeasurementBasedFactorAdder, otherwise use MeasurementBasedFactorAdder. +\begin{figure}[h] + \centering +\includegraphics[width=0.9\textwidth]{factor_adder.pdf} + \caption{A factor adder taking measurements and generating factors. Nodes are created for the graph at the measurement timestamp using a shared node adder.} + \label{img:factor_adder} +\end{figure} \ No newline at end of file diff --git a/localization/doc/src/quickstart.tex b/localization/doc/src/quickstart.tex new file mode 100644 index 0000000000..c867faeea3 --- /dev/null +++ b/localization/doc/src/quickstart.tex @@ -0,0 +1,8 @@ +Here we provide an overview of the AstroLoc library and present several examples to help explain the code structure and abilities of the library. +For an overview of the code base and library objects, see ~\ref{sec:overview}. +For a simple localization example, see ~\ref{sec:simple_localizer}. +For more examples, see the various sections after ~\ref{sec:simple_localizer}. + +\input{overview} +\input{simple_localizer} + diff --git a/localization/doc/src/relative_pose_node_adder_h.tex b/localization/doc/src/relative_pose_node_adder_h.tex new file mode 100644 index 0000000000..e876f481e9 --- /dev/null +++ b/localization/doc/src/relative_pose_node_adder_h.tex @@ -0,0 +1,15 @@ +\begin{minted}[]{c++} +#include +#include +#include + +#include + +"relative_pose_node_adder_model.h" + + using RelativePoseNodeAdder = + MeasurementBasedTimestampedNodeAdder< + localization_measurements::TimestampedPoseWithCovariance, + gtsam::Pose3, nodes::TimestampedNodes, + RelativePoseNodeAdderModel>; +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/relative_pose_node_adder_model_h.tex b/localization/doc/src/relative_pose_node_adder_model_h.tex new file mode 100644 index 0000000000..1d205eeac3 --- /dev/null +++ b/localization/doc/src/relative_pose_node_adder_model_h.tex @@ -0,0 +1,68 @@ +\begin{minted}[]{c++} +#include +#include +#include +#include + +#include +#include + +#include + +class RelativePoseNodeAdderModel + : public BetweenFactorMeasurementBasedTimestampedNodeAdderModel< + localization_measurements::TimestampedPoseWithCovariance, + gtsam::Pose3> { + public: + using NodesType = nodes::TimestampedNodes; + using Params = TimestampedNodeAdderModelParams; + + explicit PoseNodeAdderModel(const Params& params) + : Base(params) {} + + gtsam::KeyVector AddNode( + const localization_common::Time timestamp, + NodesType& nodes) const final { + const auto pose = interpolator_.Interpolate(timestamp); + return nodes.Add(timestamp, pose->pose); + } + + boost::optional< + std::pair> + RelativeNodeAndNoise( + const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b) const final { + const auto relative_pose = + interpolator_.Relative(timestamp_a, timestamp_b); + const auto relative_pose_noise = + gtsam::noiseModel::Gaussian::Covariance( + relative_pose->covariance); + return std::pair( + relative_pose->pose, relative_pose_noise); + } + + void AddMeasurement( + const localization_measurements:: + TimestampedPoseWithCovariance& measurement) { + interpolator_.Add(measurement.time, + measurement.pose_with_covariance); + } + + void RemoveMeasurements( + const localization_common::Time oldest_allowed_time) { + // Keep lower bound so future measurements can be + // interpolated using it. + interpolator_.RemoveBelowLowerBoundValues( + oldest_allowed_time); + } + bool CanAddNode( + const localization_common::Time timestamp) const final { + return interpolator_.WithinBounds(timestamp); + ; + } + + private: + localization_common::PoseWithCovarianceInterpolater + interpolator_; +}; +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/root.tex b/localization/doc/src/root.tex new file mode 100644 index 0000000000..aa0ac1bb5d --- /dev/null +++ b/localization/doc/src/root.tex @@ -0,0 +1,34 @@ +\documentclass{article} +\usepackage[utf8]{inputenc} + +\usepackage[noend]{algorithmic} +\usepackage{amsmath} +\usepackage{amsfonts} +\usepackage{amssymb} +\usepackage{graphicx} +\usepackage{mathtools} +\usepackage{hyperref} +\usepackage{physics} +\usepackage[thinc]{esdiff} +\usepackage{listings} +\usepackage{mleftright} +\usepackage{mathbbol} +\usepackage{xcolor} +\usepackage{minted} +\input{commands} +\setcounter{tocdepth}{4} +\setcounter{secnumdepth}{4} + +\title{AstroLoc Library Quickstart} +\author{Ryan Soussan} +\date{April 2023} + +\begin{document} + +\maketitle +\input{quickstart} + +\bibliographystyle{IEEEtran} +\bibliography{bib} + +\end{document} diff --git a/localization/doc/src/simple_localizer.pdf b/localization/doc/src/simple_localizer.pdf new file mode 100644 index 0000000000..a4752cf844 Binary files /dev/null and b/localization/doc/src/simple_localizer.pdf differ diff --git a/localization/doc/src/simple_localizer.tex b/localization/doc/src/simple_localizer.tex new file mode 100644 index 0000000000..41ead5a1b1 --- /dev/null +++ b/localization/doc/src/simple_localizer.tex @@ -0,0 +1,53 @@ +\section{Simple Localizer}\label{sec:simple_localizer} +Here we will create a simple localizer taking relative odometry pose measurements and absolute map-based pose measurements as input. +The structure of the localizer is shown in Figure ~\ref{img:simple_localizer}. + +\begin{figure}[ht] + \centering +\includegraphics[width=0.9\textwidth]{simple_localizer.pdf} + \caption{Localizer structure.} + \label{img:simple_localizer} +\end{figure} +The localizer takes relative and absolute pose measurements and passes these to the node adder and factor adder respectively. +These are described in more detail below. \par + +The graph structure is shown in Figure ~\ref{img:loc_graph}. + +\begin{figure}[ht] + \centering +\includegraphics[width=0.9\textwidth]{loc_graph.pdf} + \caption{Localization graph structure. Optimized pose nodes are connected by pose between factors. An initial prior constrains the first node and absolute measurement priors are added to future nodes.} + \label{img:loc_graph} +\end{figure} + +Pose nodes are optimized at various timestamps. +An initial pose prior factor is added to the first pose node, while pose between factors connect successive pose nodes. +Absolute pose priors are added using incoming measurements that trigger the creation of new pose nodes and relative factors at the same timestamp. + +\subsection{Localizer Sliding Window Graph Optimizer} +We will make the localizer a sliding-window graph optimizer so that old nodes and factors are removed. +\input{graph_localizer_h.tex} +The graph localizer includes both the relative pose node adder and absolute pose factor adder. + +\subsection{Relative Pose Node Adder} +The pose node adder uses timestamped relative pose measurements to create gtsam::Pose3 nodes. +It relies on the pose node adder model for node and relative factor creation. +\input{relative_pose_node_adder_h.tex} +\subsubsection{Pose Measurement} +A timestamped pose with covariance is used to generate values for future node and relative factors. +\subsubsection{Pose Node and TimestampedNodes} +The node type is a gtsam::Pose3 and the timestamped nodes type is a TimestampedNodes object since the node type is a single value. +\subsubsection{Relative Pose Node Adder Model} +\input{relative_pose_node_adder_model_h.tex} +The pose node adder model uses a pose interpolator to generate required relative poses and covariances using input relative pose measurements. +Since the model is a between factor adder model, it inserts gtsam::Pose3 between factors as relative factors. +\subsection{Absolute Pose Factor Adder} +The absolute pose factor adder creates a GTSAM pose prior factor for each absolute pose measurement. +Since each created factor uses a single measurement, the factor adder is a SingleMeasurementBasedFactorAdder. +When adding the prior factor, it uses the relative pose node adder to add a pose node at the same timestamp as the prior factor. +\input{absolute_pose_factor_adder.tex} +\subsection{Interfacing with Localizer} +Measurements are passed to the node and factor adders at various timestamps. +Optimization, factor and node creation, and sliding the window occur during the Update() call. +Various helper functions exist for accessing optimized values and covariances from the localizer. +\input{interfacing_cc} \ No newline at end of file diff --git a/localization/doc/src/sliding_window_graph_opt.pdf b/localization/doc/src/sliding_window_graph_opt.pdf new file mode 100644 index 0000000000..99fde3e653 Binary files /dev/null and b/localization/doc/src/sliding_window_graph_opt.pdf differ diff --git a/localization/factor_adders/CMakeLists.txt b/localization/factor_adders/CMakeLists.txt new file mode 100644 index 0000000000..e9298be4ee --- /dev/null +++ b/localization/factor_adders/CMakeLists.txt @@ -0,0 +1,101 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(factor_adders) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + graph_factors + localization_common + localization_measurements + node_adders + vision_common +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS graph_factors localization_common localization_measurements node_adders vision_common +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + # Loc factor adder test + add_rostest_gtest(test_loc_factor_adder + test/test_loc_factor_adder.test + test/test_loc_factor_adder.cc + ) + target_link_libraries(test_loc_factor_adder + ${catkin_LIBRARIES} + ) + # Single measurement based factor adder test + add_rostest_gtest(test_single_measurement_based_factor_adder + test/test_single_measurement_based_factor_adder.test + test/test_single_measurement_based_factor_adder.cc + ) + target_link_libraries(test_single_measurement_based_factor_adder + ${catkin_LIBRARIES} + ) + # Standstill factor adder test + add_rostest_gtest(test_standstill_factor_adder + test/test_standstill_factor_adder.test + test/test_standstill_factor_adder.cc + ) + target_link_libraries(test_standstill_factor_adder + ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_vo_smart_projection_factor_adder + test/test_vo_smart_projection_factor_adder.test + test/test_vo_smart_projection_factor_adder.cc + ) + target_link_libraries(test_vo_smart_projection_factor_adder + ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder.h b/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder.h new file mode 100644 index 0000000000..9075bc883f --- /dev/null +++ b/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder.h @@ -0,0 +1,136 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace factor_adders { +// Adds GTSAM Pose Between factors for depth odometry relative pose measurements +// point to point between factors for depth odometry correspondence measurements. +// Adds pose nodes using PoseNodeAdder at the same timestamps as the measurements. +template +class DepthOdometryFactorAdder + : public SingleMeasurementBasedFactorAdder { + public: + DepthOdometryFactorAdder(const DepthOdometryFactorAdderParams& params, + const std::shared_ptr node_adder); + + private: + // Creates a pose between factor or point to point between factors and pose nodes for the given measurement. + int AddFactorsForSingleMeasurement(const localization_measurements::DepthOdometryMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) final; + + bool CanAddFactor(const localization_measurements::DepthOdometryMeasurement& measurement) const final; + + std::shared_ptr node_adder_; + DepthOdometryFactorAdderParams params_; +}; + +template +DepthOdometryFactorAdder::DepthOdometryFactorAdder( + const DepthOdometryFactorAdderParams& params, const std::shared_ptr node_adder) + : SingleMeasurementBasedFactorAdder(params), + params_(params), + node_adder_(node_adder) {} + +template +int DepthOdometryFactorAdder::AddFactorsForSingleMeasurement( + const localization_measurements::DepthOdometryMeasurement& measurement, gtsam::NonlinearFactorGraph& factors) { + const double translation_norm = measurement.odometry.sensor_F_source_T_target.pose.translation().norm(); + if (params_.reject_large_translation_norm && translation_norm > params_.pose_translation_norm_threshold) { + LogDebug("AddFactors: Ignoring pose with large translation norm. Norm: " + << translation_norm << ", threshold: " << params_.pose_translation_norm_threshold); + return 0; + } + + if (!node_adder_->AddNode(measurement.odometry.source_time, factors) || + !node_adder_->AddNode(measurement.odometry.target_time, factors)) { + LogError("AddFactorsForSingleMeasurement: Failed to add nodes at source and target time."); + return 0; + } + const auto keys_a = node_adder_->Keys(measurement.odometry.source_time); + // First key is pose key + const auto& pose_key_a = keys_a[0]; + const auto keys_b = node_adder_->Keys(measurement.odometry.target_time); + const auto& pose_key_b = keys_b[0]; + + if (params_.use_points_between_factor) { + int num_between_factors = 0; + for (int i = 0; i < measurement.correspondences.source_3d_points.size() && + num_between_factors < params_.max_num_points_between_factors; + ++i) { + const Eigen::Vector3d& sensor_t_point_source = measurement.correspondences.source_3d_points[i]; + const Eigen::Vector3d& sensor_t_point_target = measurement.correspondences.target_3d_points[i]; + + double noise_sigma = 1.0; + if (params_.scale_point_between_factors_with_inverse_distance) { + noise_sigma = sensor_t_point_source.norm(); + } else if (params_.scale_point_between_factors_with_estimate_error) { + const Eigen::Vector3d estimate_error = + sensor_t_point_source - measurement.odometry.sensor_F_source_T_target.pose * sensor_t_point_target; + noise_sigma = estimate_error.norm(); + } + if (params_.reject_large_point_to_point_error) { + const Eigen::Vector3d estimate_error = + sensor_t_point_source - measurement.odometry.sensor_F_source_T_target.pose * sensor_t_point_target; + if (estimate_error.norm() > params_.point_to_point_error_threshold) { + continue; + } + } + + const auto points_between_factor_noise = localization_common::Robust( + gtsam::noiseModel::Isotropic::Sigma(3, noise_sigma * params_.point_noise_scale), params_.huber_k); + gtsam::PointToPointBetweenFactor::shared_ptr points_between_factor( + new gtsam::PointToPointBetweenFactor(sensor_t_point_source, sensor_t_point_target, params_.body_T_sensor, + points_between_factor_noise, pose_key_a, pose_key_b)); + factors.push_back(points_between_factor); + ++num_between_factors; + } + LogDebug("AddFactors: Added " << num_between_factors << " points between factors."); + return num_between_factors; + } else { + const auto relative_pose_noise = localization_common::Robust( + gtsam::noiseModel::Gaussian::Covariance(params_.pose_covariance_scale * + measurement.odometry.body_F_source_T_target.covariance), + params_.huber_k); + const gtsam::BetweenFactor::shared_ptr pose_between_factor(new gtsam::BetweenFactor( + pose_key_a, pose_key_b, localization_common::GtPose(measurement.odometry.body_F_source_T_target.pose), + relative_pose_noise)); + factors.push_back(pose_between_factor); + return 1; + } +} + +template +bool DepthOdometryFactorAdder::CanAddFactor( + const localization_measurements::DepthOdometryMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.odometry.source_time) && + node_adder_->CanAddNode(measurement.odometry.target_time); +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder_params.h b/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder_params.h new file mode 100644 index 0000000000..f16f7e3b16 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder_params.h @@ -0,0 +1,42 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ + +#include + +#include + +namespace factor_adders { +struct DepthOdometryFactorAdderParams : public FactorAdderParams { + double pose_covariance_scale; + double point_noise_scale; + bool use_points_between_factor; + bool scale_point_between_factors_with_inverse_distance; + bool scale_point_between_factors_with_estimate_error; + bool reject_large_translation_norm; + double pose_translation_norm_threshold; + bool reject_large_point_to_point_error; + double point_to_point_error_threshold; + int max_num_points_between_factors; + gtsam::Pose3 body_T_sensor; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/factor_adder.h b/localization/factor_adders/include/factor_adders/factor_adder.h new file mode 100644 index 0000000000..d15e2c762a --- /dev/null +++ b/localization/factor_adders/include/factor_adders/factor_adder.h @@ -0,0 +1,60 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_FACTOR_ADDER_H_ + +#include +#include + +#include + +#include + +#include + +namespace factor_adders { +// Adds factors to a graph. Base class for measurement-based factor adder. +class FactorAdder { + public: + explicit FactorAdder(const FactorAdderParams& params) : params_(params) {} + + // Default constructor for serialization only + FactorAdder() = default; + + virtual ~FactorAdder() = default; + + // Add factors in valid time range to existing factor graph. + // Returns number of added factors. + virtual int AddFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, gtsam::NonlinearFactorGraph& factors) = 0; + + protected: + FactorAdderParams params_; + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(params_); + } +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/factor_adder_params.h b/localization/factor_adders/include/factor_adders/factor_adder_params.h new file mode 100644 index 0000000000..fa54195e4e --- /dev/null +++ b/localization/factor_adders/include/factor_adders/factor_adder_params.h @@ -0,0 +1,29 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_FACTOR_ADDER_PARAMS_H_ + +namespace factor_adders { +struct FactorAdderParams { + bool enabled; + double huber_k; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/loc_factor_adder.h b/localization/factor_adders/include/factor_adders/loc_factor_adder.h new file mode 100644 index 0000000000..d6473937ed --- /dev/null +++ b/localization/factor_adders/include/factor_adders/loc_factor_adder.h @@ -0,0 +1,243 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_LOC_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_LOC_FACTOR_ADDER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace factor_adders { +// SingleMeasurementBasedFactorAdder that adds either loc projection factors and loc pose priors for given matched +// projection measurements. +template +class LocFactorAdder + : public SingleMeasurementBasedFactorAdder { + using Base = SingleMeasurementBasedFactorAdder; + + public: + LocFactorAdder(const LocFactorAdderParams& params, const std::shared_ptr node_adder); + + // Default constructor for serialization only. + LocFactorAdder() = default; + + private: + // Adds loc projection factor or loc pose factor depending on params. + // If the loc projection factor is selected but fails due to a reprojection error, adds a loc pose factor + // as a fallback. + int AddFactorsForSingleMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors) final; + + // Returns whether the node adder can add a node at the provided time. + bool CanAddFactor(const localization_measurements::MatchedProjectionsMeasurement& measurement) const final; + + // Adds a loc pose factor (pose prior) + int AddLocPoseFactor(const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors); + + // Adds a loc projection factor. If a reprojection error occurs, adds a loc pose factor. + int AddLocProjectionFactor( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors); + + // Helper function to add a pose node if needed and return the node's key. + boost::optional AddPoseNode(const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors); + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(node_adder_); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(num_landmarks_averager_); + ar& BOOST_SERIALIZATION_NVP(pose_prior_noise_sigmas_); + } + + std::shared_ptr node_adder_; + LocFactorAdderParams params_; + localization_common::Averager num_landmarks_averager_ = localization_common::Averager("Num Landmarks"); + gtsam::Vector6 pose_prior_noise_sigmas_; +}; + +// Implementation +template +LocFactorAdder::LocFactorAdder(const LocFactorAdderParams& params, + std::shared_ptr node_adder) + : Base(params), params_(params), node_adder_(node_adder) { + pose_prior_noise_sigmas_ = (gtsam::Vector(6) << params_.prior_translation_stddev, params_.prior_translation_stddev, + params_.prior_translation_stddev, params_.prior_quaternion_stddev, + params_.prior_quaternion_stddev, params_.prior_quaternion_stddev) + .finished(); +} + +template +int LocFactorAdder::AddFactorsForSingleMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors) { + if (matched_projections_measurement.matched_projections.empty()) { + LogDebug("AddFactorsForSingleMeasurement: Empty measurement."); + return 0; + } + + if (static_cast(matched_projections_measurement.matched_projections.size()) < + params_.min_num_matches_per_measurement) { + LogDebug("AddFactorsForSingleMeasurement: Not enough matches in projection measurement."); + return 0; + } + + const int num_landmarks = matched_projections_measurement.matched_projections.size(); + num_landmarks_averager_.Update(num_landmarks); + int num_loc_projection_factors = 0; + int num_loc_pose_factors = 0; + if (params_.add_projection_factors) { + num_loc_projection_factors = AddLocProjectionFactor(matched_projections_measurement, factors); + // Add loc pose factors as a fallback if all projection factors failed and fallback + // enabled + if (num_loc_projection_factors == 0 && params_.add_prior_if_projection_factors_fail) { + num_loc_pose_factors = AddLocPoseFactor(matched_projections_measurement, factors); + } + } + // Add loc pose factors if enabled and fallback hasn't already been triggered + if (params_.add_pose_priors && num_loc_pose_factors == 0) { + num_loc_pose_factors = AddLocPoseFactor(matched_projections_measurement, factors); + } + return num_loc_pose_factors + num_loc_projection_factors; +} + +template +bool LocFactorAdder::CanAddFactor( + const localization_measurements::MatchedProjectionsMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.timestamp); +} + +template +int LocFactorAdder::AddLocProjectionFactor( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors) { + double noise_scale = params_.projection_noise_scale; + if (params_.scale_projection_noise_with_num_landmarks) { + const int num_landmarks = matched_projections_measurement.matched_projections.size(); + noise_scale *= std::pow((num_landmarks_averager_.average() / static_cast(num_landmarks)), 2); + } + const auto pose_key = AddPoseNode(matched_projections_measurement.timestamp, factors); + if (!pose_key) { + LogError("AddLocProjectionFactors: Failed to get pose key."); + return 0; + } + const auto world_T_body = node_adder_->nodes().template Value(*pose_key); + if (!world_T_body) { + LogError("AddLocProjectionFactors: Failed to get world_T_body at timestamp " + << matched_projections_measurement.timestamp << "."); + return 0; + } + + int num_loc_projection_factors = 0; + for (const auto& matched_projection : matched_projections_measurement.matched_projections) { + gtsam::SharedNoiseModel noise; + // Use the landmark distance from the camera to inversely scale the noise if desired. + if (params_.scale_projection_noise_with_landmark_distance) { + const Eigen::Vector3d& world_t_landmark = matched_projection.map_point; + const Eigen::Isometry3d nav_cam_T_world = + localization_common::EigenPose(*world_T_body * params_.body_T_cam).inverse(); + const gtsam::Point3 nav_cam_t_landmark = nav_cam_T_world * world_t_landmark; + // Don't use robust cost here to more effectively correct a drift occurance + noise = gtsam::SharedIsotropic( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + } else { + noise = localization_common::Robust( + gtsam::SharedIsotropic(gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params_.cam_noise->sigma())), + params_.huber_k); + } + gtsam::LocProjectionFactor<>::shared_ptr loc_projection_factor( + new gtsam::LocProjectionFactor<>(matched_projection.image_point, matched_projection.map_point, noise, *pose_key, + params_.cam_intrinsics, params_.body_T_cam)); + // Check for errors, discard factor if too large of projection error occurs + // or a cheirality error occurs + const auto error = (loc_projection_factor->evaluateError(*world_T_body)).norm(); + if (error > params_.max_valid_projection_error) continue; + const auto cheirality_error = loc_projection_factor->cheiralityError(*world_T_body); + if (cheirality_error) continue; + factors.push_back(loc_projection_factor); + ++num_loc_projection_factors; + if (num_loc_projection_factors >= params_.max_num_projection_factors) break; + } + + LogDebug("AddFactorsForSingleMeasurement: Added " << num_loc_projection_factors + << " loc projection factors at timestamp " + << matched_projections_measurement.timestamp << "."); + return num_loc_projection_factors; +} + +template +int LocFactorAdder::AddLocPoseFactor( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors) { + double noise_scale = params_.pose_noise_scale; + if (params_.scale_pose_noise_with_num_landmarks) { + const int num_landmarks = matched_projections_measurement.matched_projections.size(); + noise_scale *= std::pow((num_landmarks_averager_.average() / static_cast(num_landmarks)), 2); + } + const auto pose_key = AddPoseNode(matched_projections_measurement.timestamp, factors); + if (!pose_key) { + LogError("AddLocProjectionFactors: Failed to get pose key."); + return 0; + } + + const auto pose_noise = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(noise_scale * pose_prior_noise_sigmas_)), + params_.huber_k); + + gtsam::LocPoseFactor::shared_ptr pose_prior_factor(new gtsam::LocPoseFactor( + *pose_key, matched_projections_measurement.global_T_cam * params_.body_T_cam.inverse(), pose_noise)); + factors.push_back(pose_prior_factor); + LogDebug("AddLocPoseFactor: Added loc pose prior factor at timestamp " << matched_projections_measurement.timestamp + << "."); + return 1; +} + +template +boost::optional LocFactorAdder::AddPoseNode(const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors) { + if (!node_adder_->AddNode(timestamp, factors)) { + LogError("AddLocPoseFactor: Failed to add node for timestamp " << timestamp << "."); + return boost::none; + } + const auto keys = node_adder_->Keys(timestamp); + if (keys.empty()) { + LogError("AddLocPoseFactor: Failed to get keys for timestamp " << timestamp << "."); + return boost::none; + } + + // Assumes first key is pose + const auto& pose_key = keys[0]; + return pose_key; +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_LOC_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/loc_factor_adder_params.h b/localization/factor_adders/include/factor_adders/loc_factor_adder_params.h new file mode 100644 index 0000000000..dfae8cc094 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/loc_factor_adder_params.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_LOC_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_LOC_FACTOR_ADDER_PARAMS_H_ + +#include + +#include +#include +#include + +namespace factor_adders { +struct LocFactorAdderParams : public FactorAdderParams { + // Add pose prior factors using estimated pose from matched + // projections measurement. + bool add_pose_priors; + // Add projections factor using matched projections. + bool add_projection_factors; + // Add pose prior factors if all projection factors fail. + bool add_prior_if_projection_factors_fail; + // Translation stddev noise for prior factors. + double prior_translation_stddev; + // Quaternion stddev noise for prior factors. + double prior_quaternion_stddev; + // Inversely scale noise with the number of matched projections when creating a pose factor. + bool scale_pose_noise_with_num_landmarks; + // Scale projection noise with the square of the ratio of the average number of matches per measurement + // to the current number of matches when creating a projections factor. + // Yields lower noise for measurements with more than average matches. + bool scale_projection_noise_with_num_landmarks; + // Scale projection factor noise using the inverse of the distance to a landmark. + bool scale_projection_noise_with_landmark_distance; + // Relative pose noise scale. + double pose_noise_scale; + // Relative projections noise scale. + double projection_noise_scale; + // Max num projection factors to add for a matched projections measurement. + int max_num_projection_factors; + // Min number of matched projections to create a factor (prior or projection). + int min_num_matches_per_measurement; + // Max projection error norm for a projection factor to be valid. + double max_valid_projection_error; + // Camera extrinsics. + gtsam::Pose3 body_T_cam; + // Camera intrinsics. + boost::shared_ptr cam_intrinsics; + // Camera noise used for projection factor noise. + gtsam::SharedIsotropic cam_noise; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_LOC_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/measurement_based_factor_adder.h b/localization/factor_adders/include/factor_adders/measurement_based_factor_adder.h new file mode 100644 index 0000000000..165da4dbf2 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/measurement_based_factor_adder.h @@ -0,0 +1,135 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_MEASUREMENT_BASED_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_MEASUREMENT_BASED_FACTOR_ADDER_H_ + +#include +#include + +namespace factor_adders { +template +// FactorAdder that stores measurements in a measurement buffer +// and uses these to create factors given a time range. +class MeasurementBasedFactorAdder : public FactorAdder { + public: + explicit MeasurementBasedFactorAdder(const FactorAdderParams& params); + virtual ~MeasurementBasedFactorAdder() = default; + + // Adds factors then removes old measurements. + // Requires implementation of AddMeasurementBasedFactors. + int AddFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, gtsam::NonlinearFactorGraph& factors) final; + + // Add measurement to measurement buffer. + void AddMeasurement(const MeasurementType& measurement); + + // Remove old measurements from measurement buffer. + void RemoveOldMeasurements(const localization_common::Time oldest_allowed_time); + + protected: + // Helper function to process measurements in valid time range and remove processed ones + // from the measurement buffer. + // Removes the measurement if the process_measurement_function returns true. + void ProcessMeasurements( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + const std::function& process_measurement_function, + gtsam::NonlinearFactorGraph& factors); + + // Wrapper for ProcessMeasurements that doesn't use factors. + void ProcessMeasurements(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + const std::function& process_measurement_function); + + localization_common::TimestampedSet measurements_; + + private: + // Adds factors based on measurements. + // Removes used measurements from measurement buffer. + virtual int AddMeasurementBasedFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) = 0; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(FactorAdder); + ar& BOOST_SERIALIZATION_NVP(measurements_); + } +}; + +// Implementation +template +MeasurementBasedFactorAdder::MeasurementBasedFactorAdder(const FactorAdderParams& params) + : FactorAdder(params) {} + +template +int MeasurementBasedFactorAdder::AddFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) { + const int num_added_factors = AddMeasurementBasedFactors(oldest_allowed_time, newest_allowed_time, factors); + RemoveOldMeasurements(oldest_allowed_time); + return num_added_factors; +} + +template +void MeasurementBasedFactorAdder::AddMeasurement(const MeasurementType& measurement) { + measurements_.Add(measurement.timestamp, measurement); +} + +template +void MeasurementBasedFactorAdder::RemoveOldMeasurements( + const localization_common::Time oldest_allowed_timestamp) { + measurements_.RemoveOldValues(oldest_allowed_timestamp); +} + +template +void MeasurementBasedFactorAdder::ProcessMeasurements( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + const std::function& process_measurement_function, + gtsam::NonlinearFactorGraph& factors) { + auto& measurements = measurements_.set(); + for (auto it = measurements.begin(); it != measurements.end();) { + const auto& measurement = it->second; + const auto timestamp = it->first; + if (timestamp >= oldest_allowed_time && timestamp <= newest_allowed_time && + process_measurement_function(measurement, factors)) { + // Remove used measurements. + it = measurements.erase(it); + } else { + ++it; + } + } +} + +template +void MeasurementBasedFactorAdder::ProcessMeasurements( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + const std::function& process_measurement_function) { + gtsam::NonlinearFactorGraph dummy_factors; + ProcessMeasurements( + oldest_allowed_time, newest_allowed_time, + [&process_measurement_function](const MeasurementType& measurement, gtsam::NonlinearFactorGraph& factors) { + return process_measurement_function(measurement); + }, + dummy_factors); +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_MEASUREMENT_BASED_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/pose_factor_adder.h b/localization/factor_adders/include/factor_adders/pose_factor_adder.h new file mode 100644 index 0000000000..64e4de173c --- /dev/null +++ b/localization/factor_adders/include/factor_adders/pose_factor_adder.h @@ -0,0 +1,80 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef FACTOR_ADDERS_POSE_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_POSE_FACTOR_ADDER_H_ + +#include +#include +#include +#include + +#include +#include + +namespace factor_adders { +using PoseFactorAdderParams = FactorAdderParams; + +// Adds GTSAM Pose Prior factors for absolute pose measurements. +// Adds pose nodes using PoseNodeAdder at the same timestamps as the measurements. +template +class PoseFactorAdder + : public SingleMeasurementBasedFactorAdder { + public: + PoseFactorAdder(const PoseFactorAdderParams& params, const std::shared_ptr node_adder); + + private: + // Creates a pose factor and pose node for the given measurement. + int AddFactorsForSingleMeasurement(const localization_measurements::PoseWithCovarianceMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) final; + + // Able to add a factor if the node adder can create a node for the provided measurement. + bool CanAddFactor(const localization_measurements::PoseWithCovarianceMeasurement& measurement) const final; + + std::shared_ptr node_adder_; + PoseFactorAdderParams params_; +}; + +template +PoseFactorAdder::PoseFactorAdder(const PoseFactorAdderParams& params, + const std::shared_ptr node_adder) + : SingleMeasurementBasedFactorAdder(params), + params_(params), + node_adder_(node_adder) {} + +template +int PoseFactorAdder::AddFactorsForSingleMeasurement( + const localization_measurements::PoseWithCovarianceMeasurement& measurement, gtsam::NonlinearFactorGraph& factors) { + node_adder_->AddNode(measurement.timestamp, factors); + const auto keys = node_adder_->Keys(measurement.timestamp); + // First key is pose key + const auto& pose_key = keys[0]; + const auto pose_noise = gtsam::noiseModel::Gaussian::Covariance(measurement.covariance); + const gtsam::PriorFactor::shared_ptr pose_prior_factor( + new gtsam::PriorFactor(pose_key, measurement.pose, pose_noise)); + factors.push_back(pose_prior_factor); + return 1; +} + +template +bool PoseFactorAdder::CanAddFactor( + const localization_measurements::PoseWithCovarianceMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.timestamp); +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_POSE_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/relative_pose_factor_adder.h b/localization/factor_adders/include/factor_adders/relative_pose_factor_adder.h new file mode 100644 index 0000000000..e851100d99 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/relative_pose_factor_adder.h @@ -0,0 +1,88 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_H_ + +#include +#include +#include +#include + +#include +#include + +namespace factor_adders { +// Adds GTSAM Pose Between factors for relative pose measurements. +// Adds pose nodes using PoseNodeAdder at the same timestamps as the measurements. +template +class RelativePoseFactorAdder + : public SingleMeasurementBasedFactorAdder { + public: + RelativePoseFactorAdder(const RelativePoseFactorAdderParams& params, + const std::shared_ptr node_adder); + + private: + // Creates a pose between factor and pose nodes for the given measurement. + int AddFactorsForSingleMeasurement( + const localization_measurements::RelativePoseWithCovarianceMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) final; + + bool CanAddFactor(const localization_measurements::RelativePoseWithCovarianceMeasurement& measurement) const final; + + std::shared_ptr node_adder_; + RelativePoseFactorAdderParams params_; +}; + +template +RelativePoseFactorAdder::RelativePoseFactorAdder(const RelativePoseFactorAdderParams& params, + const std::shared_ptr node_adder) + : SingleMeasurementBasedFactorAdder(params), + params_(params), + node_adder_(node_adder) {} + +template +int RelativePoseFactorAdder::AddFactorsForSingleMeasurement( + const localization_measurements::RelativePoseWithCovarianceMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) { + if (!node_adder_->AddNode(measurement.timestamp_a, factors) || + !node_adder_->AddNode(measurement.timestamp_b, factors)) { + LogError("AddFactorsForSingleMeasurement: Failed to add nodes at respective times."); + return 0; + } + + const auto keys_a = node_adder_->Keys(measurement.timestamp_a); + // First key is pose key + const auto& pose_key_a = keys_a[0]; + const auto keys_b = node_adder_->Keys(measurement.timestamp_b); + const auto& pose_key_b = keys_b[0]; + const auto relative_pose_noise = + gtsam::noiseModel::Gaussian::Covariance(params_.covariance_scale * measurement.covariance); + const gtsam::BetweenFactor::shared_ptr pose_between_factor( + new gtsam::BetweenFactor(pose_key_a, pose_key_b, measurement.relative_pose, relative_pose_noise)); + factors.push_back(pose_between_factor); + return 1; +} + +template +bool RelativePoseFactorAdder::CanAddFactor( + const localization_measurements::RelativePoseWithCovarianceMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.timestamp_a) && node_adder_->CanAddNode(measurement.timestamp_b); +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/relative_pose_factor_adder_params.h b/localization/factor_adders/include/factor_adders/relative_pose_factor_adder_params.h new file mode 100644 index 0000000000..719c7037b6 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/relative_pose_factor_adder_params.h @@ -0,0 +1,30 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_PARAMS_H_ + +#include + +namespace factor_adders { +struct RelativePoseFactorAdderParams : public FactorAdderParams { + double covariance_scale; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/single_measurement_based_factor_adder.h b/localization/factor_adders/include/factor_adders/single_measurement_based_factor_adder.h new file mode 100644 index 0000000000..47adf9a7af --- /dev/null +++ b/localization/factor_adders/include/factor_adders/single_measurement_based_factor_adder.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_SINGLE_MEASUREMENT_BASED_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_SINGLE_MEASUREMENT_BASED_FACTOR_ADDER_H_ + +#include + +namespace factor_adders { +template +// MeasurementBasedFactorAdder that adds factors using single measurements at a time +// for measurements in a provided time range. +class SingleMeasurementBasedFactorAdder : public MeasurementBasedFactorAdder { + public: + explicit SingleMeasurementBasedFactorAdder(const FactorAdderParams& params); + virtual ~SingleMeasurementBasedFactorAdder() = default; + + private: + // Add factors for all measurements in valid time range to existing factor graph. + // Calls AddFactors(measurement) for each measurement in range. + // Subsequently removes measurements that are addable (for example, if a corresponding + // node adder can create a node at it's timestamp) as determined by CanAddFactor(). + // Returns number of added factors. + int AddMeasurementBasedFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) final; + + // Add factors given a single measurement. + // Returns number of added factors. + virtual int AddFactorsForSingleMeasurement(const MeasurementType& measurement, + gtsam::NonlinearFactorGraph& factors) = 0; + + // Whether a factor can be added for the given measurement. + virtual bool CanAddFactor(const MeasurementType& measurement) const = 0; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(MeasurementBasedFactorAdder); + } +}; + +// Implementation +template +SingleMeasurementBasedFactorAdder::SingleMeasurementBasedFactorAdder(const FactorAdderParams& params) + : MeasurementBasedFactorAdder(params) {} + +template +int SingleMeasurementBasedFactorAdder::AddMeasurementBasedFactors( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) { + int num_added_factors = 0; + this->ProcessMeasurements( + oldest_allowed_time, newest_allowed_time, + [this, &num_added_factors](const MeasurementType& measurement, gtsam::NonlinearFactorGraph& factors) { + if (!CanAddFactor(measurement)) return false; + num_added_factors += AddFactorsForSingleMeasurement(measurement, factors); + return true; + }, + factors); + return num_added_factors; +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_SINGLE_MEASUREMENT_BASED_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/standstill_factor_adder.h b/localization/factor_adders/include/factor_adders/standstill_factor_adder.h new file mode 100644 index 0000000000..f58d93bb07 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/standstill_factor_adder.h @@ -0,0 +1,173 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_H_ + +#include +#include +#include +#include + +#include + +#include + +namespace factor_adders { +// SingleMeasurementBasedFactorAdder that generates standstill factors (zero velocity prior and zero relative pose +// between factors) based on provided params. +template +class StandstillFactorAdder + : public SingleMeasurementBasedFactorAdder { + using Base = SingleMeasurementBasedFactorAdder; + + public: + StandstillFactorAdder(const StandstillFactorAdderParams& params, + const std::shared_ptr node_adder); + + // Default constructor for serialization only + StandstillFactorAdder() = default; + + private: + // Adds zero velocity and/or zero relative pose factors depending on params. + int AddFactorsForSingleMeasurement(const localization_measurements::StandstillMeasurement& standstill_measurement, + gtsam::NonlinearFactorGraph& factors) final; + + // Returns whether the node adder can add a node at the provided time. + bool CanAddFactor(const localization_measurements::StandstillMeasurement& measurement) const final; + + // Adds a velocity prior at the provided timestamp with zero velocity. + bool AddZeroVelocityPrior(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors); + // Adds a relative pose between factor with zero relative movement between nodes at timestamp a and b. + bool AddZeroRelativePoseFactor(const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b, gtsam::NonlinearFactorGraph& factors); + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(node_adder_); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(zero_velocity_noise_); + ar& BOOST_SERIALIZATION_NVP(zero_relative_pose_noise_); + } + + std::shared_ptr node_adder_; + StandstillFactorAdderParams params_; + gtsam::SharedNoiseModel zero_velocity_noise_; + gtsam::SharedNoiseModel zero_relative_pose_noise_; +}; + +// Implementation +template +StandstillFactorAdder::StandstillFactorAdder( + const StandstillFactorAdderParams& params, const std::shared_ptr node_adder) + : Base(params), params_(params), node_adder_(node_adder) { + // Create zero velocity noise + const gtsam::Vector3 velocity_prior_noise_sigmas( + (gtsam::Vector(3) << params_.prior_velocity_stddev, params_.prior_velocity_stddev, params_.prior_velocity_stddev) + .finished()); + zero_velocity_noise_ = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(velocity_prior_noise_sigmas)), + params_.huber_k); + + // Create zero relative pose noise + const gtsam::Vector6 pose_between_noise_sigmas( + (gtsam::Vector(6) << params_.pose_between_factor_rotation_stddev, params_.pose_between_factor_rotation_stddev, + params_.pose_between_factor_rotation_stddev, params_.pose_between_factor_translation_stddev, + params_.pose_between_factor_translation_stddev, params_.pose_between_factor_translation_stddev) + .finished()); + zero_relative_pose_noise_ = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(pose_between_noise_sigmas)), params_.huber_k); +} + +template +int StandstillFactorAdder::AddFactorsForSingleMeasurement( + const localization_measurements::StandstillMeasurement& standstill_measurement, + gtsam::NonlinearFactorGraph& factors) { + int num_factors_added = 0; + if (params_.add_pose_between_factor) { + if (AddZeroRelativePoseFactor(standstill_measurement.previous_timestamp, standstill_measurement.timestamp, factors)) + ++num_factors_added; + } + if (params_.add_velocity_prior) { + if (AddZeroVelocityPrior(standstill_measurement.timestamp, factors)) ++num_factors_added; + } + return num_factors_added; +} + +template +bool StandstillFactorAdder::CanAddFactor( + const localization_measurements::StandstillMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.timestamp); +} + +template +bool StandstillFactorAdder::AddZeroVelocityPrior(const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors) { + if (!node_adder_->AddNode(timestamp, factors)) { + return false; + } + const auto keys = node_adder_->Keys(timestamp); + if (keys.empty()) { + LogError("AddZeroVelocityPrior: Failed to get keys for timestamp."); + return false; + } + // Assumes second key is velocity + const auto& velocity_key = keys[1]; + + // Create factor + gtsam::PriorFactor::shared_ptr velocity_prior_factor( + new gtsam::PriorFactor(velocity_key, gtsam::Velocity3::Zero(), zero_velocity_noise_)); + factors.push_back(velocity_prior_factor); + LogDebug("AddFactorsForSingleMeasurement: Added standstill velocity prior factor."); + return true; +} + +template +bool StandstillFactorAdder::AddZeroRelativePoseFactor( + const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, + gtsam::NonlinearFactorGraph& factors) { + if (!(node_adder_->AddNode(timestamp_a, factors) && node_adder_->AddNode(timestamp_b, factors))) { + return false; + } + + const auto keys_a = node_adder_->Keys(timestamp_a); + if (keys_a.empty()) { + LogError("AddZeroVelocityPrior: Failed to get keys for timestamp_a."); + return false; + } + const auto keys_b = node_adder_->Keys(timestamp_b); + if (keys_b.empty()) { + LogError("AddZeroVelocityPrior: Failed to get keys for timestamp_b."); + return false; + } + // Assumes first key is pose + const auto& pose_key_a = keys_a[0]; + const auto& pose_key_b = keys_b[0]; + + gtsam::BetweenFactor::shared_ptr pose_between_factor(new gtsam::BetweenFactor( + pose_key_a, pose_key_b, gtsam::Pose3::identity(), zero_relative_pose_noise_)); + factors.push_back(pose_between_factor); + LogDebug("AddFactorsForSingleMeasurement: Added standstill pose between factor."); + return true; +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/standstill_factor_adder_params.h b/localization/factor_adders/include/factor_adders/standstill_factor_adder_params.h new file mode 100644 index 0000000000..d0b20ab263 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/standstill_factor_adder_params.h @@ -0,0 +1,34 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_PARAMS_H_ + +#include + +namespace factor_adders { +struct StandstillFactorAdderParams : public FactorAdderParams { + bool add_velocity_prior; + bool add_pose_between_factor; + double prior_velocity_stddev; + double pose_between_factor_translation_stddev; + double pose_between_factor_rotation_stddev; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h new file mode 100644 index 0000000000..ab88e97a10 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h @@ -0,0 +1,344 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace factor_adders { +using SmartFactorCalibration = gtsam::Cal3_S2; +using SmartFactorCamera = gtsam::PinholePose; +using RobustSmartFactor = gtsam::RobustSmartProjectionPoseFactor; +using SharedRobustSmartFactor = boost::shared_ptr; +template +// Adds visual-odometry (VO) smart factors for feature tracks using GTSAM smart factors. +// Smart factors avoid bundle-adjustment by marginalizing out triangulated feature track +// landmark points before adding for optimization, making them much more efficient at the expense +// of accuracy. Since the landmark position for the point is not included during optimization, +// GTSAM performs retriangulation of the landmark during optimization if poses in the smart +// factor have changed by more than a set threshold. Retriangulation also triggers relinearization of the smart factor. +// Measurements are downsampled in the feature tracker, avoiding issues of downsampling +// from the latest measurement which may introduce many new timestamped pose nodes to +// the graph in addition to the latest measurement's timestamp. +class VoSmartProjectionFactorAdder + : public MeasurementBasedFactorAdder { + using Base = MeasurementBasedFactorAdder; + + public: + VoSmartProjectionFactorAdder(const VoSmartProjectionFactorAdderParams& params, + std::shared_ptr node_adder); + + // Default constructor for serialization only. + VoSmartProjectionFactorAdder() = default; + + // Const accesor to feature tracker + const vision_common::SpacedFeatureTracker& feature_tracker() const; + + private: + // Add factors using either set measurement spacing or max spacing. + int AddMeasurementBasedFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) final; + + // Add factors using downsampled measurements, determined by the measurement_spacing param. + // Measurements are in affect downsampled before being used for feature tracks. + int AddFactorsUsingDownsampledMeasurements(gtsam::NonlinearFactorGraph& factors) const; + + // Helper function to add a smart factor given a set of feature track points. + // Assumes points are ordered from oldest to latest, so oldest points are + // added first and prioritized over later points given a max number of points to add. + bool AddSmartFactor(const vision_common::FeaturePoints& feature_track_points, + gtsam::NonlinearFactorGraph& factors) const; + + // Checks the average distance from the mean and the number of points in the track. + bool ValidTrack(const vision_common::FeaturePoints& feature_track) const; + + // Fixes invalid smart factors (that suffer cheirality errors) by removing individual measurements and if that fails + // by removing measurement sequences. + // If both fail, the smart factor is left in the graph in case it becomes + // valid over the course of optimization. + void FixSmartFactors(const gtsam::Values& values, gtsam::NonlinearFactorGraph& factors) const; + + // Attempts to fix smart factors by removing individual measurements. + boost::optional FixSmartFactorByRemovingIndividualMeasurements( + const gtsam::Values& values, const RobustSmartFactor& smart_factor) const; + + // Attempts to fix smart factors by removing measurement sequences. + boost::optional FixSmartFactorByRemovingMeasurementSequence( + const gtsam::Values& values, const RobustSmartFactor& smart_factor) const; + + // Helper function for creating and testing a fixed smart factor + boost::optional FixedSmartFactor( + const gtsam::Values& values, gtsam::PinholePose::MeasurementVector& measurements, + gtsam::KeyVector& keys) const; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(node_adder_); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(feature_tracker_); + } + + std::shared_ptr node_adder_; + VoSmartProjectionFactorAdderParams params_; + std::shared_ptr feature_tracker_; +}; + +// Implementation +template +VoSmartProjectionFactorAdder::VoSmartProjectionFactorAdder( + const VoSmartProjectionFactorAdderParams& params, std::shared_ptr node_adder) + : Base(params), params_(params), node_adder_(node_adder) { + feature_tracker_.reset(new vision_common::SpacedFeatureTracker(params.spaced_feature_tracker)); +} + +template +const vision_common::SpacedFeatureTracker& VoSmartProjectionFactorAdder::feature_tracker() const { + return *feature_tracker_; +} + +template +int VoSmartProjectionFactorAdder::AddMeasurementBasedFactors( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) { + // Update feature tracker with new measurements and remove these from the measurement buffer if nodes can be created + // for them. + ProcessMeasurements(oldest_allowed_time, newest_allowed_time, + [this](const localization_measurements::FeaturePointsMeasurement& measurement) { + // Don't add measurements until pose nodes can be created at their timestamp + if (!node_adder_->CanAddNode(measurement.timestamp)) return false; + feature_tracker_->Update(measurement.feature_points); + return true; + }); + // Remove old feature track points before adding factors + feature_tracker_->RemoveOldPoints(oldest_allowed_time); + // Remove old smart factors before adding new ones, since otherwise there would be repeat factors + // for already existing feature tracks + localization_common::DeleteFactors(factors); + + // Create smart factors based on feature tracks + int num_added_factors = 0; + num_added_factors = AddFactorsUsingDownsampledMeasurements(factors); + // Attempt to fix broken factors if enabled + if (params_.fix_invalid_factors) FixSmartFactors(node_adder_->nodes().gtsam_values(), factors); + return num_added_factors; +} + +template +int VoSmartProjectionFactorAdder::AddFactorsUsingDownsampledMeasurements( + gtsam::NonlinearFactorGraph& factors) const { + int num_added_factors = 0; + const auto spaced_feature_tracks = feature_tracker_->SpacedFeatureTracks(); + for (const auto& feature_track : spaced_feature_tracks) { + if (num_added_factors >= params_.max_num_factors) break; + if (ValidTrack(feature_track)) { + if (AddSmartFactor(feature_track, factors)) ++num_added_factors; + } + } + return num_added_factors; +} + +template +bool VoSmartProjectionFactorAdder::AddSmartFactor( + const vision_common::FeaturePoints& feature_track_points, gtsam::NonlinearFactorGraph& factors) const { + SharedRobustSmartFactor smart_factor; + const int num_feature_track_points = feature_track_points.size(); + // Optionally scale the noise with the number of points, as a longer track the relies on a + // longer history of poses tends to have higher error + const double noise_scale = + params_.scale_noise_with_num_points ? params_.noise_scale * num_feature_track_points : params_.noise_scale; + const auto noise = gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params_.cam_noise->sigma()); + smart_factor = + boost::make_shared(noise, params_.cam_intrinsics, params_.body_T_cam, params_.smart_factor, + params_.rotation_only_fallback, params_.robust, params_.huber_k); + + for (int i = 0; i < static_cast(feature_track_points.size()); ++i) { + // Start in reverse so most recent measurements are added first + const auto& feature_point = feature_track_points[feature_track_points.size() - 1 - i]; + if (i >= params_.max_num_points_per_factor) break; + const auto& timestamp = feature_point.timestamp; + // Add or get pose key + if (!node_adder_->AddNode(timestamp, factors)) { + LogError("AddSmartFactor: Failed to add node for timestamp " << std::setprecision(15) << timestamp << "."); + return false; + } + const auto keys = node_adder_->Keys(timestamp); + if (keys.empty()) { + LogError("AddSmartFactor: Failed to get keys for timestamp " << std::setprecision(15) << timestamp << "."); + return false; + } + // Assumes first key is pose + const auto& pose_key = keys[0]; + + smart_factor->add(SmartFactorCamera::Measurement(feature_point.image_point), pose_key); + } + factors.push_back(smart_factor); + return true; +} + +template +bool VoSmartProjectionFactorAdder::ValidTrack( + const vision_common::FeaturePoints& feature_track) const { + const double average_distance_from_mean = vision_common::AverageDistanceFromMean(feature_track); + if (static_cast(feature_track.size()) < params_.min_num_points_per_factor) return false; + return (average_distance_from_mean >= params_.min_avg_distance_from_mean); +} + +template +void VoSmartProjectionFactorAdder::FixSmartFactors(const gtsam::Values& values, + gtsam::NonlinearFactorGraph& factors) const { + for (auto& factor : factors) { + auto smart_factor = dynamic_cast(factor.get()); + if (!smart_factor) continue; + // Can't remove measurements if there are only 2 or fewer + if (smart_factor->measured().size() <= 2) continue; + const auto point = smart_factor->triangulateSafe(smart_factor->cameras(values)); + if (point.valid()) continue; + // Invalid factor, first attempt to fix by removing individiual measurements. + { + const auto fixed_smart_factor = FixSmartFactorByRemovingIndividualMeasurements(values, *smart_factor); + if (fixed_smart_factor) { + factor = *fixed_smart_factor; + continue; + } + } + // If removing individiual measurements fails, attempt to fix by removing measurement + // sequences. + { + const auto fixed_smart_factor = FixSmartFactorByRemovingMeasurementSequence(values, *smart_factor); + if (fixed_smart_factor) { + factor = *fixed_smart_factor; + continue; + } + } + // Don't remove factor in case it becomes valid over the course of optimization. + LogDebug("SplitSmartFactorsIfNeeded: Failed to fix smart factor"); + } +} + +template +boost::optional +VoSmartProjectionFactorAdder::FixSmartFactorByRemovingIndividualMeasurements( + const gtsam::Values& values, const RobustSmartFactor& smart_factor) const { + // TODO(rsoussan): Make this more efficient by enabled removal of measurements and keys in smart factor + const auto original_measurements = smart_factor.measured(); + const auto original_keys = smart_factor.keys(); + int measurement_index_to_remove; + // Start with latest measurement + for (measurement_index_to_remove = original_measurements.size() - 1; measurement_index_to_remove >= 0; + --measurement_index_to_remove) { + gtsam::PinholePose::MeasurementVector measurements_to_add; + gtsam::KeyVector keys_to_add; + for (int i = 0; i < static_cast(original_measurements.size()); ++i) { + if (i == measurement_index_to_remove) continue; + measurements_to_add.emplace_back(original_measurements[i]); + keys_to_add.emplace_back(original_keys[i]); + } + const auto fixed_smart_factor = FixedSmartFactor(values, measurements_to_add, keys_to_add); + if (fixed_smart_factor) { + LogDebug("FixSmartFactorByRemovingIndividualMeasurements: Fixed by removing measurement " + << measurement_index_to_remove << ", num original measurements: " << original_measurements.size()); + return fixed_smart_factor; + } + } + return boost::none; +} + +template +boost::optional +VoSmartProjectionFactorAdder::FixSmartFactorByRemovingMeasurementSequence( + const gtsam::Values& values, const RobustSmartFactor& smart_factor) const { + constexpr int min_num_measurements = 2; + // TODO(rsoussan): Make this more efficient by enabled removal of measurements and keys in smart factor + const auto original_measurements = smart_factor.measured(); + const auto original_keys = smart_factor.keys(); + int num_measurements_to_add = original_measurements.size() - 1; + // Try to remove min number of most recent measurements + while (num_measurements_to_add >= min_num_measurements) { + gtsam::PinholePose::MeasurementVector measurements_to_add; + gtsam::KeyVector keys_to_add; + for (int i = 0; i < num_measurements_to_add; ++i) { + measurements_to_add.emplace_back(original_measurements[i]); + keys_to_add.emplace_back(original_keys[i]); + } + const auto fixed_smart_factor = FixedSmartFactor(values, measurements_to_add, keys_to_add); + if (fixed_smart_factor) { + LogDebug( + "FixSmartFactorByRemovingMeasurementSequence: Fixed smart factor by removing most recent " + "measurements. Original " + "measurement size: " + << original_measurements.size() << ", new size: " << num_measurements_to_add); + return fixed_smart_factor; + } else { + --num_measurements_to_add; + } + } + num_measurements_to_add = original_measurements.size() - 1; + // Try to remove min number of oldest measurements + while (num_measurements_to_add >= min_num_measurements) { + gtsam::PinholePose::MeasurementVector measurements_to_add; + gtsam::KeyVector keys_to_add; + for (int i = original_measurements.size() - num_measurements_to_add; + i < static_cast(original_measurements.size()); ++i) { + measurements_to_add.emplace_back(original_measurements[i]); + keys_to_add.emplace_back(original_keys[i]); + } + const auto fixed_smart_factor = FixedSmartFactor(values, measurements_to_add, keys_to_add); + if (fixed_smart_factor) { + LogDebug( + "FixSmartFactorByRemovingMeasurementSequence: Fixed smart factor by removing oldest measurements. " + "Original " + "measurement size: " + << original_measurements.size() << ", new size: " << num_measurements_to_add); + return fixed_smart_factor; + } else { + --num_measurements_to_add; + } + } + // Failed to fix smart factor + return boost::none; + // TODO(rsoussan): delete factor if fail to find acceptable new one? + // TODO(rsoussan): attempt to make a second factor with remaining measuremnts!!! +} + +template +boost::optional VoSmartProjectionFactorAdder::FixedSmartFactor( + const gtsam::Values& values, gtsam::PinholePose::MeasurementVector& measurements, + gtsam::KeyVector& keys) const { + auto new_smart_factor = boost::make_shared( + params_.cam_noise, params_.cam_intrinsics, params_.body_T_cam, params_.smart_factor, params_.rotation_only_fallback, + params_.robust, params_.huber_k); + new_smart_factor->add(measurements, keys); + const auto new_point = new_smart_factor->triangulateSafe(new_smart_factor->cameras(values)); + if (!new_point.valid()) return boost::none; + return new_smart_factor; +} +} // namespace factor_adders +#endif // FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder_params.h b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder_params.h new file mode 100644 index 0000000000..7624b75501 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder_params.h @@ -0,0 +1,78 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ + +#include +#include + +#include +#include +#include +#include + +namespace factor_adders { +struct VoSmartProjectionFactorAdderParams : public FactorAdderParams { + vision_common::SpacedFeatureTrackerParams spaced_feature_tracker; + // Maximum number of smart factors to include in a graph at a time. + int max_num_factors; + // Minimum number of points for a feature track to be used for a smart factor. + int min_num_points_per_factor; + // Maximum number of points in a feature track to include in a smart factor. + int max_num_points_per_factor; + // Minimum average deviation for points in a feature track to be used. + // A higher deviation provides a larger baseline between points and is less likely + // to yield numerical errors during triangulation and optimization. + double min_avg_distance_from_mean; + // Use a robust loss for the factor. + bool robust; + // Minimum distance in image space between the latest measurement in a feature track + // and already included latest measurements in other feature tracks for the feature track + // to be used as a smart factor. + /* double feature_track_min_separation;*/ + // If triangulation fails, use a rotation-only version of the smart factor. + // Otherwise, the smart factor is disabled. + bool rotation_only_fallback; + // Attempt to fix smart factors that are invalid by removing individual measurements + // and if this fails measurement sequences. + bool fix_invalid_factors; + // Scale the noise with the number of points, as a longer track the relies on a + // longer history of poses tends to have higher error + bool scale_noise_with_num_points; + // Relative noise scale for all smart factors. + double noise_scale; + // Camera extrinsics. + gtsam::Pose3 body_T_cam; + // Camera intrinsics. + boost::shared_ptr cam_intrinsics; + // Camera noise. + gtsam::SharedIsotropic cam_noise; + // GTSAM Smart factor params, see GTSAM documentation and below for more details. + // enableEPI: Refine triangulation using Levenberg-Marquardt Optimization. + // verboseCheirality: Print error on cheirality errors + // landmarkDistanceThreshold: Maximum valid distance for a triangulated point. + // dynamicOutlierRejectionThreshold: Maximum valid reprojection error for a valid triangulated point. + // retriangulationThreshold: Equality threshold for poses in a smart factor to trigger retriangulation + // of a landmark point. If any of the poses vary by more than this threshold + // compared to their previous value, retriangulation is triggered. + gtsam::SmartProjectionParams smart_factor; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/package.xml b/localization/factor_adders/package.xml new file mode 100644 index 0000000000..7269c605eb --- /dev/null +++ b/localization/factor_adders/package.xml @@ -0,0 +1,27 @@ + + factor_adders + 1.0.0 + + The factor adders package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + graph_factors + localization_common + localization_measurements + node_adders + vision_common + graph_factors + localization_common + localization_measurements + node_adders + vision_common + diff --git a/localization/factor_adders/readme.md b/localization/factor_adders/readme.md new file mode 100644 index 0000000000..096ca049dc --- /dev/null +++ b/localization/factor_adders/readme.md @@ -0,0 +1,22 @@ +\page factoradders Factor Adders + +# Package Overview +The factor adders package provides a set of factor adders that add factors to a graph optimizer given a range of timestamps. + +## FactorAdder +Base class for adding factors given a timestamp range. + +## MeasurementBasedFactorAdder +Extends the FactorAdder and stores measurements in a buffer before using these for factor creation. + +## SingleMeasurementBasedFactorAdder +MeasurementBasedFactorAdder that adds factors using single measurements at a time for measurements in a provided time range. + +## LocFactorAdder +SingleMeasurementBasedFactorAdder that takes map-based image feature measurements and generates either LocProjectionFactors or LocPoseFactors. Optionally adds loc pose factors as a fallback for projection factors that suffer cheirality errors. Uses a pose node adder for required node creation. + +### VOSmartProjectionFactorAdder +Generates visual odometry smart factors using image feature tracks. Downsamples measurements as desired for smart factors. See gtsam::SmartProjectionPoseFactor for more information on smart factors. Uses a pose node adder for required node creation. + +### StandstillFactorAdder +Creates a zero velocity prior and a zero tranform between factor for successive nodes when standstill is detected. Uses a pose and velocity node adder for required node creation. diff --git a/localization/factor_adders/test/test_loc_factor_adder.cc b/localization/factor_adders/test/test_loc_factor_adder.cc new file mode 100644 index 0000000000..a73d3026b7 --- /dev/null +++ b/localization/factor_adders/test/test_loc_factor_adder.cc @@ -0,0 +1,520 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; + +// Test node adder that just returns keys that should be used. +// Key values are calculated using the integer timestamps passed. +class SimplePoseNodeAdder : public na::NodeAdder { + public: + SimplePoseNodeAdder() : values_(std::make_shared()), nodes_(values_) {} + + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& graph) final{}; + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { + if (!CanAddNode(timestamp)) return false; + if (!values_) std::cout << "no values!" << std::endl; + values().Add(gtsam::Pose3::identity()); + return true; + } + + bool CanAddNode(const localization_common::Time timestamp) const final { + return timestamp <= latest_measurement_time_; + } + + // Assumes integer timestamps that perfectly cast to ints. + // First key is pose key. + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { + gtsam::KeyVector keys; + // Offset by 1 since node keys start at 1 + keys.emplace_back(gtsam::Key(static_cast(timestamp + 1))); + return keys; + } + + const no::Values& values() const { return *values_; } + + no::Values& values() { return *values_; } + + no::TimestampedNodes& nodes() { return nodes_; } + + // Simulate measurement delay for node adder and control end of measurements time. + int latest_measurement_time_ = 10; + + private: + std::shared_ptr values_; + no::TimestampedNodes nodes_; +}; + +class LocFactorAdderTest : public ::testing::Test { + public: + LocFactorAdderTest() { node_adder_.reset(new SimplePoseNodeAdder()); } + + void SetUp() final { AddMeasurements(); } + + void AddMeasurements() { + for (int time = 0; time < num_times_; ++time) { + lm::MatchedProjectionsMeasurement measurement; + measurement.global_T_cam = lc::RandomPose(); + measurement.timestamp = time; + for (int i = 0; i < num_projections_per_measurement_; ++i) { + const lm::ImagePoint image_point(i, i + 1); + const lm::MapPoint map_point(i, i + 1, i + 2); + const lm::MatchedProjection matched_projection(image_point, map_point, time); + measurement.matched_projections.emplace_back(matched_projection); + } + measurements_.emplace_back(measurement); + } + } + + void Initialize(const fa::LocFactorAdderParams& params) { + factor_adder_.reset(new fa::LocFactorAdder(params, node_adder_)); + params_ = params; + pose_prior_noise_sigmas_ = (gtsam::Vector(6) << params_.prior_translation_stddev, params_.prior_translation_stddev, + params_.prior_translation_stddev, params_.prior_quaternion_stddev, + params_.prior_quaternion_stddev, params_.prior_quaternion_stddev) + .finished(); + } + + fa::LocFactorAdderParams DefaultParams() { + fa::LocFactorAdderParams params; + params.add_pose_priors = false; + params.add_projection_factors = false; + params.add_prior_if_projection_factors_fail = false; + params.prior_translation_stddev = 0.1; + params.prior_quaternion_stddev = 0.2; + params.scale_pose_noise_with_num_landmarks = false; + params.scale_projection_noise_with_num_landmarks = false; + params.scale_projection_noise_with_landmark_distance = false; + params.pose_noise_scale = 2; + params.projection_noise_scale = 2; + params.max_num_projection_factors = 3; + params.min_num_matches_per_measurement = 1; + params.max_valid_projection_error = 1e6; + params.body_T_cam = gtsam::Pose3::identity(); + params.cam_intrinsics = boost::make_shared(); + params.cam_noise = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); + params.huber_k = 1.345; + params.enabled = true; + return params; + } + + void EXPECT_SAME_POSE_FACTOR(const int factor_index, const int measurement_index) { + const auto pose_factor = dynamic_cast(factors_[factor_index].get()); + ASSERT_TRUE(pose_factor); + const gtsam::Pose3 factor_pose = pose_factor->prior(); + const gtsam::Pose3 measurement_pose = measurements_[measurement_index].global_T_cam * params_.body_T_cam.inverse(); + EXPECT_MATRIX_NEAR(factor_pose, measurement_pose, 1e-6); + } + + void EXPECT_SAME_PROJECTION_FACTOR(const int factor_index, const int measurement_time, const int measurement_index) { + const auto projection_factor = dynamic_cast*>(factors_[factor_index].get()); + ASSERT_TRUE(projection_factor); + const auto& measurement = measurements_[measurement_index].matched_projections[measurement_index]; + EXPECT_MATRIX_NEAR(projection_factor->measured(), measurement.image_point, 1e-6); + EXPECT_MATRIX_NEAR(projection_factor->landmark_point(), measurement.map_point, 1e-6); + EXPECT_MATRIX_NEAR(projection_factor->body_P_sensor()->matrix(), params_.body_T_cam, 1e-6); + EXPECT_MATRIX_NEAR(projection_factor->calibration()->matrix(), params_.cam_intrinsics->matrix(), 1e-6); + } + + void EXPECT_SAME_NOISE(const int factor_index, const gtsam::SharedNoiseModel& noise) { + const auto projection_factor = dynamic_cast*>(factors_[factor_index].get()); + ASSERT_TRUE(projection_factor); + EXPECT_MATRIX_NEAR(na::Covariance(projection_factor->noiseModel()), na::Covariance(noise), 1e-6); + } + + void EXPECT_SAME_NOISE_NON_ROBUST(const int factor_index, const gtsam::SharedNoiseModel& noise) { + const auto projection_factor = dynamic_cast*>(factors_[factor_index].get()); + ASSERT_TRUE(projection_factor); + const auto factor_noise = + dynamic_cast(projection_factor->noiseModel().get())->covariance(); + const auto expected_noise = dynamic_cast(noise.get())->covariance(); + EXPECT_MATRIX_NEAR(factor_noise, expected_noise, 1e-6); + } + + void EXPECT_SAME_POSE_NOISE(const int factor_index, const gtsam::SharedNoiseModel& noise) { + const auto pose_factor = dynamic_cast(factors_[factor_index].get()); + ASSERT_TRUE(pose_factor); + EXPECT_MATRIX_NEAR(na::Covariance(pose_factor->noiseModel()), na::Covariance(noise), 1e-6); + } + + std::unique_ptr> factor_adder_; + std::shared_ptr node_adder_; + gtsam::NonlinearFactorGraph factors_; + const int num_times_ = 10; + const int num_projections_per_measurement_ = 3; + std::vector measurements_; + fa::LocFactorAdderParams params_; + gtsam::Vector6 pose_prior_noise_sigmas_; +}; + +TEST_F(LocFactorAdderTest, ProjectionFactors) { + auto params = DefaultParams(); + params.add_projection_factors = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), num_projections_per_measurement_); + EXPECT_EQ(factors_.size(), 3); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); + EXPECT_SAME_PROJECTION_FACTOR(2, 0, 2); + + // Add second factors + factor_adder_->AddMeasurement(measurements_[1]); + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), num_projections_per_measurement_); + EXPECT_EQ(factors_.size(), 6); + // t0 factors + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); + EXPECT_SAME_PROJECTION_FACTOR(2, 0, 2); + // t1 factors + EXPECT_SAME_PROJECTION_FACTOR(3, 1, 0); + EXPECT_SAME_PROJECTION_FACTOR(4, 1, 1); + EXPECT_SAME_PROJECTION_FACTOR(5, 1, 2); + + // Repeat add factors with no new measurements, nothing should change + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 0); + EXPECT_EQ(factors_.size(), 6); +} + +TEST_F(LocFactorAdderTest, MaxProjectionFactors) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.max_num_projection_factors = 2; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors, only max_num_projection_factors should be added + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), params.max_num_projection_factors); + EXPECT_EQ(factors_.size(), params.max_num_projection_factors); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); +} + +TEST_F(LocFactorAdderTest, MinNumMeasurementsPerFactor) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.add_pose_priors = true; + params.min_num_matches_per_measurement = 10; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors, none should be added + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 0); + EXPECT_EQ(factors_.size(), 0); +} + +TEST_F(LocFactorAdderTest, ProjectionNoise) { + auto params = DefaultParams(); + params.add_projection_factors = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), params.max_num_projection_factors); + const auto expected_noise = + localization_common::Robust(gtsam::SharedIsotropic(gtsam::noiseModel::Isotropic::Sigma( + 2, params_.projection_noise_scale * params_.cam_noise->sigma())), + params_.huber_k); + EXPECT_SAME_NOISE(0, expected_noise); +} + +TEST_F(LocFactorAdderTest, ScaleProjectionNoiseWithNumLandmarks) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.scale_projection_noise_with_num_landmarks = true; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 1); + { + // Ratio is 1 for the first measurement + const double num_landmarks_ratio = 1; + const double noise_scale = params_.projection_noise_scale * std::pow(num_landmarks_ratio, 2); + const auto expected_noise = localization_common::Robust( + gtsam::SharedIsotropic(gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params_.cam_noise->sigma())), + params_.huber_k); + EXPECT_SAME_NOISE(0, expected_noise); + } + // Add second measurement with all matches + factor_adder_->AddMeasurement(measurements_[1]); + // Add second factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 3); + EXPECT_EQ(factors_.size(), 4); + { + // New average is (1+3)/2 = 2, so ratio is 2/3 for the second measurement + const double num_landmarks_ratio = 2.0 / 3.0; + const double noise_scale = params_.projection_noise_scale * std::pow(num_landmarks_ratio, 2); + const auto expected_noise = localization_common::Robust( + gtsam::SharedIsotropic(gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params_.cam_noise->sigma())), + params_.huber_k); + EXPECT_SAME_NOISE(1, expected_noise); + EXPECT_SAME_NOISE(2, expected_noise); + EXPECT_SAME_NOISE(3, expected_noise); + } +} + +TEST_F(LocFactorAdderTest, ScaleProjectionNoiseWithLandmarkDistance) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.scale_projection_noise_with_landmark_distance = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), params.max_num_projection_factors); + const auto world_T_body = node_adder_->values().Value(factors_[0]->keys()[0]); + ASSERT_TRUE(world_T_body != boost::none); + // Check first factor noise + { + const Eigen::Vector3d& world_t_landmark = measurements_[0].matched_projections[0].map_point; + const Eigen::Isometry3d nav_cam_T_world = + localization_common::EigenPose(*world_T_body * params_.body_T_cam).inverse(); + const gtsam::Point3 nav_cam_t_landmark = nav_cam_T_world * world_t_landmark; + // Don't use robust cost here to more effectively correct a drift occurance + const auto expected_noise = gtsam::SharedIsotropic( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + EXPECT_SAME_NOISE_NON_ROBUST(0, expected_noise); + } + // Check second factor noise + { + const Eigen::Vector3d& world_t_landmark = measurements_[0].matched_projections[1].map_point; + const Eigen::Isometry3d nav_cam_T_world = + localization_common::EigenPose(*world_T_body * params_.body_T_cam).inverse(); + const gtsam::Point3 nav_cam_t_landmark = nav_cam_T_world * world_t_landmark; + // Don't use robust cost here to more effectively correct a drift occurance + const auto expected_noise = gtsam::SharedIsotropic( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + EXPECT_SAME_NOISE_NON_ROBUST(1, expected_noise); + } + // Check third factor noise + { + const Eigen::Vector3d& world_t_landmark = measurements_[0].matched_projections[2].map_point; + const Eigen::Isometry3d nav_cam_T_world = + localization_common::EigenPose(*world_T_body * params_.body_T_cam).inverse(); + const gtsam::Point3 nav_cam_t_landmark = nav_cam_T_world * world_t_landmark; + // Don't use robust cost here to more effectively correct a drift occurance + const auto expected_noise = gtsam::SharedIsotropic( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + EXPECT_SAME_NOISE_NON_ROBUST(2, expected_noise); + } +} + +TEST_F(LocFactorAdderTest, MinReprojectionError) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.max_valid_projection_error = 1e6; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + const auto world_T_body = node_adder_->values().Value(factors_[0]->keys()[0]); + ASSERT_TRUE(world_T_body != boost::none); + double projection_error; + // Check projection error + { + const auto projection_factor = dynamic_cast*>(factors_[0].get()); + ASSERT_TRUE(projection_factor); + projection_error = projection_factor->evaluateError(*world_T_body).norm(); + } + + const double epsilon = 1e-3; + // Redo adding factors, but set projection error threshold slightly below previous error. + // Factor shouldn't be added. + { + factors_ = gtsam::NonlinearFactorGraph(); + auto params = DefaultParams(); + params.add_projection_factors = true; + params.max_valid_projection_error = projection_error - epsilon; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Factor should fail due to reprojection error + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 0); + } + // Redo adding factors, but set projection error threshold slightly above previous error. + // Factor should be added. + { + factors_ = gtsam::NonlinearFactorGraph(); + auto params = DefaultParams(); + params.add_projection_factors = true; + params.max_valid_projection_error = projection_error + epsilon; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Factor should succeed + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + } +} + +TEST_F(LocFactorAdderTest, PoseFactors) { + auto params = DefaultParams(); + params.add_pose_priors = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factor + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_POSE_FACTOR(0, 0); + const auto pose_noise = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas( + Eigen::Ref(params_.pose_noise_scale * pose_prior_noise_sigmas_)), + params_.huber_k); + EXPECT_SAME_POSE_NOISE(0, pose_noise); + + factor_adder_->AddMeasurement(measurements_[1]); + // Add second factor + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_POSE_FACTOR(0, 0); + EXPECT_SAME_POSE_NOISE(0, pose_noise); + EXPECT_SAME_POSE_FACTOR(1, 1); + EXPECT_SAME_POSE_NOISE(1, pose_noise); +} + +TEST_F(LocFactorAdderTest, PoseFactorsTooSoon) { + auto params = DefaultParams(); + params.add_pose_priors = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factor + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_POSE_FACTOR(0, 0); + // Add factor too soon, set node adder latest measurement before factor newest_allowed_time + // so factor can't be created. + node_adder_->latest_measurement_time_ = 0.5; + factor_adder_->AddMeasurement(measurements_[1]); + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 0); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_POSE_FACTOR(0, 0); + // Change node adder latest measurement time back, make sure measurement for factor + // adder hasn't been removed and factor can still be created. + node_adder_->latest_measurement_time_ = 2; + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_POSE_FACTOR(0, 0); + EXPECT_SAME_POSE_FACTOR(1, 1); +} + +TEST_F(LocFactorAdderTest, ScalePoseNoiseWithNumLandmarks) { + auto params = DefaultParams(); + params.add_pose_priors = true; + params.scale_pose_noise_with_num_landmarks = true; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 1); + { + // Ratio is 1 for the first measurement + const double num_landmarks_ratio = 1; + const double noise_scale = params_.pose_noise_scale * std::pow(num_landmarks_ratio, 2); + const auto expected_noise = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(noise_scale * pose_prior_noise_sigmas_)), + params_.huber_k); + EXPECT_SAME_POSE_NOISE(0, expected_noise); + } + // Add second measurement with all matches + factor_adder_->AddMeasurement(measurements_[1]); + // Add second factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 2); + { + // New average is (1+3)/2 = 2, so ratio is 2/3 for the second measurement + const double num_landmarks_ratio = 2.0 / 3.0; + const double noise_scale = params_.pose_noise_scale * std::pow(num_landmarks_ratio, 2); + const auto expected_noise = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(noise_scale * pose_prior_noise_sigmas_)), + params_.huber_k); + EXPECT_SAME_POSE_NOISE(1, expected_noise); + } +} + +TEST_F(LocFactorAdderTest, PoseFallback) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.add_pose_priors = false; + params.add_prior_if_projection_factors_fail = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), num_projections_per_measurement_); + EXPECT_EQ(factors_.size(), 3); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); + EXPECT_SAME_PROJECTION_FACTOR(2, 0, 2); + + // Force cheirality error for projections, add second factor, expect pose prior to be added + auto measurement_1 = measurements_[1]; + for (int i = 0; i < num_projections_per_measurement_; ++i) { + measurement_1.matched_projections[i].map_point = lm::MapPoint(-100 + i, -99 + i, -98 + i); + } + factor_adder_->AddMeasurement(measurement_1); + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); + EXPECT_SAME_PROJECTION_FACTOR(2, 0, 2); + EXPECT_SAME_POSE_FACTOR(3, 1); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/factor_adders/test/test_loc_factor_adder.test b/localization/factor_adders/test/test_loc_factor_adder.test new file mode 100644 index 0000000000..ee6e741f07 --- /dev/null +++ b/localization/factor_adders/test/test_loc_factor_adder.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/factor_adders/test/test_single_measurement_based_factor_adder.cc b/localization/factor_adders/test/test_single_measurement_based_factor_adder.cc new file mode 100644 index 0000000000..16fb321d60 --- /dev/null +++ b/localization/factor_adders/test/test_single_measurement_based_factor_adder.cc @@ -0,0 +1,209 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include + +#include + +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; + +struct SimpleAdderParams : public fa::FactorAdderParams { + SimpleAdderParams() { + huber_k = 1.345; + enabled = true; + } +}; + +class SimpleAdder : public fa::SingleMeasurementBasedFactorAdder { + public: + explicit SimpleAdder(const SimpleAdderParams& params = SimpleAdderParams()) + : fa::SingleMeasurementBasedFactorAdder(params), params_(params) { + // Create pose noise + const double translation_stddev = 0.1; + const double rotation_stddev = 0.2; + const gtsam::Vector6 pose_noise_sigmas((gtsam::Vector(6) << rotation_stddev, rotation_stddev, rotation_stddev, + translation_stddev, translation_stddev, translation_stddev) + .finished()); + pose_noise_ = lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(pose_noise_sigmas)), + params_.huber_k); + } + + int end_time_ = 10; + + private: + int AddFactorsForSingleMeasurement(const lm::PoseMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) final { + gtsam::PriorFactor::shared_ptr pose_prior_factor( + new gtsam::PriorFactor(gtsam::Key(key_value_++), measurement.pose, pose_noise_)); + factors.push_back(pose_prior_factor); + return 1; + } + + // Simulate some node adder delay and don't add factors if past end time. + bool CanAddFactor(const lm::PoseMeasurement& measurement) const final { return measurement.timestamp <= end_time_; } + + int key_value_ = 0; + gtsam::SharedNoiseModel pose_noise_; + SimpleAdderParams params_; +}; + +class SingleMeasurementBasedFactorAdderTest : public ::testing::Test { + public: + SingleMeasurementBasedFactorAdderTest() {} + + void SetUp() final { + constexpr int kNumMeasurements = 10; + for (int i = 0; i < kNumMeasurements; ++i) { + const lm::PoseMeasurement measurement(lc::RandomPose(), lc::Time(i)); + measurements_.emplace_back(measurement); + } + } + + void AddMeasurements() { + for (const auto& measurement : measurements_) factor_adder_.AddMeasurement(measurement); + } + + // TODO(rsoussan): Unify this with pose_node_adder test! + void EXPECT_SAME_PRIOR_FACTOR(const int index, const Eigen::Isometry3d& pose) { + const auto pose_prior_factor = dynamic_cast*>(factors_[index].get()); + ASSERT_TRUE(pose_prior_factor); + EXPECT_MATRIX_NEAR(pose_prior_factor->prior(), pose, 1e-6); + } + + void EXPECT_SAME_PRIOR_FACTOR(const int index, const gtsam::Pose3& pose) { + EXPECT_SAME_PRIOR_FACTOR(index, lc::EigenPose(pose)); + } + + void EXPECT_SAME_PRIOR_FACTOR(const int index) { EXPECT_SAME_PRIOR_FACTOR(index, pose(index)); } + + void EXPECT_SAME_PRIOR_FACTOR(const int factor_index, const int pose_index) { + EXPECT_SAME_PRIOR_FACTOR(factor_index, pose(pose_index)); + } + + lc::Time time(int index) { return measurements_[index].timestamp; } + + const gtsam::Pose3& pose(const int index) { return measurements_[index].pose; } + + SimpleAdder factor_adder_; + std::vector measurements_; + gtsam::NonlinearFactorGraph factors_; + + private: +}; + +TEST_F(SingleMeasurementBasedFactorAdderTest, AddMeasurements) { + AddMeasurements(); + EXPECT_EQ(factor_adder_.AddFactors(-100, -10, factors_), 0); + EXPECT_EQ(factors_.size(), 0); + // Add factor 0 + EXPECT_EQ(factor_adder_.AddFactors(-100.1, time(0), factors_), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_PRIOR_FACTOR(0); + // Add factors 1, 2, 3 + EXPECT_EQ(factor_adder_.AddFactors(time(1) - 0.1, time(3) + 0.1, factors_), 3); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + // Add factors 1, 2, 3 again, should have no affect + EXPECT_EQ(factor_adder_.AddFactors(time(1) - 0.1, time(3) + 0.1, factors_), 0); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + // Add factors 4, 5 + EXPECT_EQ(factor_adder_.AddFactors(time(4) - 0.1, time(5), factors_), 2); + EXPECT_EQ(factors_.size(), 6); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + EXPECT_SAME_PRIOR_FACTOR(4); + EXPECT_SAME_PRIOR_FACTOR(5); + // Old and used measurements should be removed up, expect adding factors 0,1,2 to fail + EXPECT_EQ(factor_adder_.AddFactors(time(0) - 0.1, time(2), factors_), 0); + EXPECT_EQ(factors_.size(), 6); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + EXPECT_SAME_PRIOR_FACTOR(4); + EXPECT_SAME_PRIOR_FACTOR(5); + // Add factors 8, 9 + EXPECT_EQ(factor_adder_.AddFactors(time(8) - 0.1, time(9), factors_), 2); + EXPECT_EQ(factors_.size(), 8); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + EXPECT_SAME_PRIOR_FACTOR(4); + EXPECT_SAME_PRIOR_FACTOR(5); + EXPECT_SAME_PRIOR_FACTOR(6, 8); + EXPECT_SAME_PRIOR_FACTOR(7, 9); + // Add factors 6,7 - should fail since already used an oldest allowed time (8) newer + // than each of these and therefore measurements were removed + EXPECT_EQ(factor_adder_.AddFactors(time(6) - 0.1, time(7), factors_), 0); + EXPECT_EQ(factors_.size(), 8); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + EXPECT_SAME_PRIOR_FACTOR(4); + EXPECT_SAME_PRIOR_FACTOR(5); + EXPECT_SAME_PRIOR_FACTOR(6, 8); + EXPECT_SAME_PRIOR_FACTOR(7, 9); + // Can't add factors newer than measurements + EXPECT_EQ(factor_adder_.AddFactors(time(9) + 0.1, time(9) + 1000.1, factors_), 0); + EXPECT_EQ(factors_.size(), 8); +} + +TEST_F(SingleMeasurementBasedFactorAdderTest, AddFactorsTooSoon) { + AddMeasurements(); + // Add factor 0 + EXPECT_EQ(factor_adder_.AddFactors(-100.1, time(0), factors_), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_PRIOR_FACTOR(0); + // Make factors unaddable, make sure none are added + factor_adder_.end_time_ = 0.5; + EXPECT_EQ(factor_adder_.AddFactors(time(1), time(3), factors_), 0); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_PRIOR_FACTOR(0); + // Increase end time, measurements shouldn't have been removed and + // now factors should be added. + factor_adder_.end_time_ = 10; + EXPECT_EQ(factor_adder_.AddFactors(time(1), time(3), factors_), 3); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/factor_adders/test/test_single_measurement_based_factor_adder.test b/localization/factor_adders/test/test_single_measurement_based_factor_adder.test new file mode 100644 index 0000000000..55c3f342bd --- /dev/null +++ b/localization/factor_adders/test/test_single_measurement_based_factor_adder.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/factor_adders/test/test_standstill_factor_adder.cc b/localization/factor_adders/test/test_standstill_factor_adder.cc new file mode 100644 index 0000000000..9183c2c51f --- /dev/null +++ b/localization/factor_adders/test/test_standstill_factor_adder.cc @@ -0,0 +1,249 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; + +// Test node adder that just returns keys that should be used. +// Key values are calculated using the integer timestamps passed, where the +// first key is a pose key and second is a velocity key. +// Pose keys are 2*timestamp and velocity keys are 2*timestamp + 1. +class SimplePoseVelocityNodeAdder : public na::NodeAdder { + public: + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& graph) final{}; + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { return true; } + + bool CanAddNode(const localization_common::Time timestamp) const final { return true; } + + // Assumes integer timestamps that perfectly cast to ints. + // First key is pose key, second is velocity key + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { + gtsam::KeyVector keys; + keys.emplace_back(gtsam::Key(static_cast(timestamp) * 2)); + keys.emplace_back(gtsam::Key(static_cast(timestamp) * 2 + 1)); + return keys; + } +}; + +class StandstillFactorAdderTest : public ::testing::Test { + public: + StandstillFactorAdderTest() { node_adder_.reset(new SimplePoseVelocityNodeAdder()); } + + void SetUp() final {} + + void AddMeasurements() { + constexpr int kNumMeasurements = 10; + for (int i = 0; i < kNumMeasurements; ++i) { + const lm::StandstillMeasurement measurement(i + 1, i); + measurements_.emplace_back(measurement); + factor_adder_->AddMeasurement(measurement); + } + } + + void Initialize(const fa::StandstillFactorAdderParams& params) { + factor_adder_.reset(new fa::StandstillFactorAdder(params, node_adder_)); + // Create zero velocity noise + const gtsam::Vector3 velocity_prior_noise_sigmas( + (gtsam::Vector(3) << params.prior_velocity_stddev, params.prior_velocity_stddev, params.prior_velocity_stddev) + .finished()); + zero_velocity_noise_ = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(velocity_prior_noise_sigmas)), + params.huber_k); + + // Create zero relative pose noise + const gtsam::Vector6 pose_between_noise_sigmas( + (gtsam::Vector(6) << params.pose_between_factor_rotation_stddev, params.pose_between_factor_rotation_stddev, + params.pose_between_factor_rotation_stddev, params.pose_between_factor_translation_stddev, + params.pose_between_factor_translation_stddev, params.pose_between_factor_translation_stddev) + .finished()); + zero_relative_pose_noise_ = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(pose_between_noise_sigmas)), + params.huber_k); + } + + fa::StandstillFactorAdderParams DefaultParams() { + fa::StandstillFactorAdderParams params; + params.enabled = true; + params.huber_k = 1.345; + params.add_velocity_prior = true; + params.add_pose_between_factor = true; + params.prior_velocity_stddev = 0.1; + params.pose_between_factor_translation_stddev = 0.2; + params.pose_between_factor_rotation_stddev = 0.3; + return params; + } + + // TODO(rsoussan): Get from common location, share with pose_node_adder test + template + void EXPECT_SAME_NOISE(const FactorPtrType factor, const gtsam::Matrix& covariance) { + EXPECT_MATRIX_NEAR(na::Covariance(factor->noiseModel()), covariance, 1e-6); + } + + template + void EXPECT_SAME_NOISE(const FactorPtrType factor, const gtsam::SharedNoiseModel noise) { + EXPECT_SAME_NOISE(factor, na::Covariance(noise)); + } + + void EXPECT_SAME_VELOCITY_PRIOR_FACTOR(const int factor_index, const int key_index) { + const auto velocity_prior_factor = + dynamic_cast*>(factors_[factor_index].get()); + ASSERT_TRUE(velocity_prior_factor); + EXPECT_MATRIX_NEAR(velocity_prior_factor->prior(), Eigen::Vector3d::Zero(), 1e-6); + EXPECT_EQ(velocity_prior_factor->key(), gtsam::Key(key_index)); + EXPECT_SAME_NOISE(velocity_prior_factor, zero_velocity_noise_); + } + + void EXPECT_SAME_POSE_BETWEEN_FACTOR(const int factor_index, const int key_index) { + const auto pose_between_factor = dynamic_cast*>(factors_[factor_index].get()); + ASSERT_TRUE(pose_between_factor); + EXPECT_MATRIX_NEAR(pose_between_factor->measured(), Eigen::Isometry3d::Identity(), 1e-6); + EXPECT_EQ(pose_between_factor->key1(), gtsam::Key(key_index)); + EXPECT_EQ(pose_between_factor->key2(), gtsam::Key((key_index + 2))); + EXPECT_SAME_NOISE(pose_between_factor, zero_relative_pose_noise_); + } + + lc::Time time(int index) { return measurements_[index].timestamp; } + + std::unique_ptr> factor_adder_; + std::shared_ptr node_adder_; + gtsam::NonlinearFactorGraph factors_; + gtsam::SharedNoiseModel zero_velocity_noise_; + gtsam::SharedNoiseModel zero_relative_pose_noise_; + + private: + std::vector measurements_; +}; + +TEST_F(StandstillFactorAdderTest, PoseAndVelocityFactors) { + auto params = DefaultParams(); + Initialize(params); + AddMeasurements(); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(time(0), time(0), factors_), 2); + EXPECT_EQ(factors_.size(), 2); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // Factors and their indices: + // pose_between: 0, velocity_prior: 1 + EXPECT_SAME_POSE_BETWEEN_FACTOR(0, 0); + // Use velocity_1 key since velocity prior is added to most recent timestamp + // in standstill measurement + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(1, 3); + // Add 2nd and 3rd factors + EXPECT_EQ(factor_adder_->AddFactors((time(0) + time(1)) / 2.0, (time(2) + time(3)) / 2.0, factors_), 4); + EXPECT_EQ(factors_.size(), 6); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // pose_2: 4, velocity_1: 5 + // pose_3: 6, velocity_1: 7 + // Factors and their indices: + // pose_between: 0, velocity_prior: 1 + // pose_between: 2, velocity_prior: 3 + // pose_between: 4, velocity_prior: 5 + EXPECT_SAME_POSE_BETWEEN_FACTOR(0, 0); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(1, 3); + EXPECT_SAME_POSE_BETWEEN_FACTOR(2, 2); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(3, 5); + EXPECT_SAME_POSE_BETWEEN_FACTOR(4, 4); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(5, 7); +} + +TEST_F(StandstillFactorAdderTest, PoseOnlyFactors) { + auto params = DefaultParams(); + params.add_velocity_prior = false; + Initialize(params); + AddMeasurements(); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(time(0), time(0), factors_), 1); + EXPECT_EQ(factors_.size(), 1); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // Factors and their indices: + // pose_between: 0 + EXPECT_SAME_POSE_BETWEEN_FACTOR(0, 0); + // Add 2nd and 3rd factors + EXPECT_EQ(factor_adder_->AddFactors((time(0) + time(1)) / 2.0, (time(2) + time(3)) / 2.0, factors_), 2); + EXPECT_EQ(factors_.size(), 3); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // pose_2: 4, velocity_1: 5 + // pose_3: 6, velocity_1: 7 + // Factors and their indices: + // pose_between: 0 + // pose_between: 1 + // pose_between: 2 + EXPECT_SAME_POSE_BETWEEN_FACTOR(0, 0); + EXPECT_SAME_POSE_BETWEEN_FACTOR(1, 2); + EXPECT_SAME_POSE_BETWEEN_FACTOR(2, 4); +} + +TEST_F(StandstillFactorAdderTest, VelocityOnlyFactors) { + auto params = DefaultParams(); + params.add_pose_between_factor = false; + Initialize(params); + AddMeasurements(); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(time(0), time(0), factors_), 1); + EXPECT_EQ(factors_.size(), 1); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // Factors and their indices: + // velocity_prior: 0 + // Use velocity_1 key since velocity prior is added to most recent timestamp + // in standstill measurement + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(0, 3); + // Add 2nd and 3rd factors + EXPECT_EQ(factor_adder_->AddFactors((time(0) + time(1)) / 2.0, (time(2) + time(3)) / 2.0, factors_), 2); + EXPECT_EQ(factors_.size(), 3); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // pose_2: 4, velocity_1: 5 + // pose_3: 6, velocity_1: 7 + // Factors and their indices: + // velocity_prior: 0 + // velocity_prior: 1 + // velocity_prior: 2 + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(0, 3); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(1, 5); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(2, 7); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/factor_adders/test/test_standstill_factor_adder.test b/localization/factor_adders/test/test_standstill_factor_adder.test new file mode 100644 index 0000000000..1f4d38543b --- /dev/null +++ b/localization/factor_adders/test/test_standstill_factor_adder.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/factor_adders/test/test_vo_smart_projection_factor_adder.cc b/localization/factor_adders/test/test_vo_smart_projection_factor_adder.cc new file mode 100644 index 0000000000..3d91bd1a1c --- /dev/null +++ b/localization/factor_adders/test/test_vo_smart_projection_factor_adder.cc @@ -0,0 +1,592 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; +namespace vc = vision_common; + +// Test node adder that just returns keys that should be used. +// Key values are calculated using the integer timestamps passed. +class SimplePoseNodeAdder : public na::NodeAdder { + public: + SimplePoseNodeAdder() : values_(std::make_shared()), nodes_(values_) {} + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& graph) final{}; + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { return true; } + + bool CanAddNode(const localization_common::Time timestamp) const final { + return timestamp <= latest_measurement_time_; + } + + // Assumes integer timestamps that perfectly cast to ints. + // First key is pose key. + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { + if (!CanAddNode(timestamp)) return gtsam::KeyVector(); + gtsam::KeyVector keys; + keys.emplace_back(gtsam::Key(static_cast(timestamp))); + return keys; + } + + const no::Values& values() const { return *values_; } + + no::Values& values() { return *values_; } + + no::TimestampedNodes& nodes() { return nodes_; } + + // Simulate measurement delay for node adder and control end of measurements time. + double latest_measurement_time_ = 10; + + private: + std::shared_ptr values_; + no::TimestampedNodes nodes_; +}; + +class VoSmartProjectionFactorAdderTest : public ::testing::Test { + public: + VoSmartProjectionFactorAdderTest() { node_adder_.reset(new SimplePoseNodeAdder()); } + + void SetUp() final { CreateMeasurements(); } + + // Add measurements for each track at multiple timestamps + void CreateMeasurements() { + for (int time = 0; time < num_measurements_; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + for (int track_id = 0; track_id < num_tracks_; ++track_id) { + // Make image points different for different measurements + const vc::FeaturePoint p(track_id * 10 + time, track_id * 10 + time + 1, time + 1, track_id, time); + measurement.feature_points.emplace_back(p); + } + measurements_.emplace_back(measurement); + } + } + + void EXPECT_SAME_FACTOR(const int factor_index, const std::vector& timestamps, + const std::vector& measurements, + const std::vector& measurement_indices) { + const auto factor = factors_[factor_index]; + const auto smart_factor = dynamic_cast(factor.get()); + ASSERT_TRUE(smart_factor); + const auto& keys = factor->keys(); + EXPECT_EQ(keys.size(), timestamps.size()); + const auto& factor_measurements = smart_factor->measured(); + EXPECT_EQ(factor_measurements.size(), timestamps.size()); + for (int i = 0; i < keys.size(); ++i) { + // Factors store measurements in reverse order + int measurement_index = keys.size() - 1 - i; + EXPECT_EQ(keys[measurement_index], gtsam::Key(timestamps[i])); + EXPECT_MATRIX_NEAR(factor_measurements[measurement_index], + measurements[measurement_indices[i]].feature_points[factor_index].image_point, 1e-6); + } + + // Check factor noise + // Uses inv sigma scaling, applies optional robust loss to whitened noise after + const double noise_scale = + params_.scale_noise_with_num_points ? params_.noise_scale * measurements.size() : params_.noise_scale; + const double expected_sigma = noise_scale * params_.cam_noise->sigma(); + EXPECT_NEAR(smart_factor->noise_inv_sigma(), 1.0 / expected_sigma, 1e-6); + EXPECT_EQ(smart_factor->robust(), params_.robust); + } + + void EXPECT_SAME_FACTOR(const int factor_index, const std::vector& timestamps) { + std::vector measurement_indices; + for (const auto& time : timestamps) { + measurement_indices.emplace_back(time); + } + EXPECT_SAME_FACTOR(factor_index, timestamps, measurements_, measurement_indices); + } + + lc::Time timestamp(const int measurement_index) { return measurements_[measurement_index].timestamp; } + + void Initialize(const fa::VoSmartProjectionFactorAdderParams& params) { + factor_adder_.reset(new fa::VoSmartProjectionFactorAdder(params, node_adder_)); + params_ = params; + } + + fa::VoSmartProjectionFactorAdderParams DefaultParams() { + fa::VoSmartProjectionFactorAdderParams params; + // Feature Tracker Params + params.spaced_feature_tracker.measurement_spacing = 0; + params.spaced_feature_tracker.remove_undetected_feature_tracks = true; + params.max_num_factors = 2; + params.min_num_points_per_factor = 2; + params.max_num_points_per_factor = 3; + params.min_avg_distance_from_mean = 1e-9; + params.robust = true; + params.rotation_only_fallback = false; + params.fix_invalid_factors = false; + params.scale_noise_with_num_points = false; + params.noise_scale = 1; + params.body_T_cam = gtsam::Pose3::identity(); + params.cam_intrinsics = boost::make_shared(); + params.cam_noise = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); + // Smart Factor Params + params.smart_factor.verboseCheirality = false; + params.smart_factor.setRankTolerance(1e-9); + params.smart_factor.setLandmarkDistanceThreshold(3); + params.smart_factor.setDynamicOutlierRejectionThreshold(1); + params.smart_factor.setRetriangulationThreshold(0.1); + params.smart_factor.setDegeneracyMode(gtsam::DegeneracyMode::HANDLE_INFINITY); + params.smart_factor.setEnableEPI(false); + params.enabled = true; + params.huber_k = 1.345; + return params; + } + + std::unique_ptr> factor_adder_; + std::shared_ptr node_adder_; + gtsam::NonlinearFactorGraph factors_; + std::vector measurements_; + fa::VoSmartProjectionFactorAdderParams params_; + int num_measurements_ = 10; + int num_tracks_ = 3; +}; + +TEST_F(VoSmartProjectionFactorAdderTest, AddFactors) { + auto params = DefaultParams(); + Initialize(params); + const int max_factors = std::min(params.max_num_factors, num_tracks_); + // Add first measurement + // No factors should be added since there are too few measurements for each factor + factor_adder_->AddMeasurement(measurements_[0]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(0), factors_), 0); + // Track: Measurement Timestamps + // 0: 0 + // 1: 0 + // 2: 0 + EXPECT_EQ(factors_.size(), 0); + // Add second measurement + factor_adder_->AddMeasurement(measurements_[1]); + // No factors added if don't include second measurement timestamp + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), (timestamp(0) + timestamp(1)) / 2.0, factors_), 0); + // Track: Measurement Timestamps + // 0: 0 + // 1: 0 + // 2: 0 + EXPECT_EQ(factors_.size(), 0); + // All factors added if include second measurement timestamp + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(1), factors_), max_factors); + // Track: Measurement Timestamps + // 0: 0, 1 + // 1: 0, 1 + // 2: 0, 1 + EXPECT_SAME_FACTOR(0, {0, 1}); + EXPECT_SAME_FACTOR(1, {0, 1}); + EXPECT_EQ(factors_.size(), max_factors); + // Try to add factors from t: 1->2, but no measurement added yet for t2, so not enough + // measurements to add factors + EXPECT_EQ(factor_adder_->AddFactors(timestamp(1), timestamp(2), factors_), 0); + // Track: Measurement Timestamps + // 0: 1 + // 1: 1 + // 2: 1 + EXPECT_EQ(factors_.size(), 0); + // Add 3rd measurement + factor_adder_->AddMeasurement(measurements_[2]); + // Add factors from t: 1->2 + EXPECT_EQ(factor_adder_->AddFactors(timestamp(1), timestamp(2), factors_), 2); + // Track: Measurement Timestamps + // 0: 1, 2 + // 1: 1, 2 + // 2: 1, 2 + EXPECT_SAME_FACTOR(0, {1, 2}); + EXPECT_SAME_FACTOR(1, {1, 2}); + EXPECT_EQ(factors_.size(), 2); + // Add 4th and 5th Measurement + factor_adder_->AddMeasurement(measurements_[3]); + factor_adder_->AddMeasurement(measurements_[4]); + // Add factors from t: 5->6 + // Should erase all measurements and add no factors + EXPECT_EQ(factor_adder_->AddFactors(timestamp(5), timestamp(6), factors_), 0); + // Track: Measurement Timestamps + // 0: + // 1: + // 2: + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(5), factors_), 0); + factor_adder_->AddMeasurement(measurements_[5]); + factor_adder_->AddMeasurement(measurements_[6]); + factor_adder_->AddMeasurement(measurements_[7]); + factor_adder_->AddMeasurement(measurements_[8]); + factor_adder_->AddMeasurement(measurements_[9]); + // Add factors from t: 5->8 + EXPECT_EQ(factor_adder_->AddFactors(timestamp(5), timestamp(8), factors_), 2); + // Track: Measurement Timestamps + // 0: 5, 6, 7, 8 + // 1: 5, 6, 7, 8 + // 2: 5, 6, 7, 8 + EXPECT_EQ(factors_.size(), 2); + // Since max points per factor is 3, expect only last three measurements to be used. + EXPECT_SAME_FACTOR(0, {6, 7, 8}); + EXPECT_SAME_FACTOR(1, {6, 7, 8}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, AddSpacedFactors) { + auto params = DefaultParams(); + params.spaced_feature_tracker.measurement_spacing = 1; + Initialize(params); + const int max_factors = std::min(params.max_num_factors, num_tracks_); + // Add first measurement + // No factors should be added since there are too few measurements for each factor + factor_adder_->AddMeasurement(measurements_[0]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(0), factors_), 0); + // Track: Measurement Timestamps + // 0: 0 + // 1: 0 + // 2: 0 + EXPECT_EQ(factors_.size(), 0); + // Add second measurement + // No factors added since skip second measurement due to measurement spacing + factor_adder_->AddMeasurement(measurements_[1]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(1), factors_), 0); + // Track: Measurement Timestamps (skipped measurements in parenthesis) + // 0: 0, (1) + // 1: 0, (1) + // 2: 0, (1) + EXPECT_EQ(factors_.size(), 0); + // Add 3rd measurement + // Add factors from t: 0->3 + factor_adder_->AddMeasurement(measurements_[2]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); + // Track: Measurement Timestamps + // 0: 0, (1), 2 + // 1: 0, (1), 2 + // 2: 0, (1), 2 + EXPECT_SAME_FACTOR(0, {0, 2}); + EXPECT_SAME_FACTOR(1, {0, 2}); + // Adding repeat factors shouldn't change graph + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); + // Track: Measurement Timestamps + // 0: 0, (1), 2 + // 1: 0, (1), 2 + // 2: 0, (1), 2 + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_FACTOR(0, {0, 2}); + EXPECT_SAME_FACTOR(1, {0, 2}); + // Add measurements from 3->7 + factor_adder_->AddMeasurement(measurements_[3]); + factor_adder_->AddMeasurement(measurements_[4]); + factor_adder_->AddMeasurement(measurements_[5]); + factor_adder_->AddMeasurement(measurements_[6]); + factor_adder_->AddMeasurement(measurements_[7]); + // Add factors from 3 -> 7 + // Only timestamps 4 and 6 should be used + EXPECT_EQ(factor_adder_->AddFactors(timestamp(3), timestamp(7), factors_), 2); + // Track: Measurement Timestamps + // 0: (3), 4, (5), 6, (7) + // 1: (3), 4, (5), 6, (7) + // 2: (3), 4, (5), 6, (7) + EXPECT_SAME_FACTOR(0, {4, 6}); + EXPECT_SAME_FACTOR(1, {4, 6}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, AvgDistFromMean) { + auto params = DefaultParams(); + // Each point pair is separated by (1, 1), so for 3 pairs: + // (10, 11), (11, 12), (12, 13) -> mean: (11, 12) + // avg dist from mean: 2*|((1,1)|/3 = 2sqrt(2)/3 ~= 0.9428 + // Use a slightly larger min so all tracks are invalid + const double expected_avg_distance_from_mean = 2.0 * std::sqrt(2) / 3.0; + const double epsilon = 1e-6; + params.min_avg_distance_from_mean = expected_avg_distance_from_mean + epsilon; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + factor_adder_->AddMeasurement(measurements_[1]); + factor_adder_->AddMeasurement(measurements_[2]); + // A factors should fail to be added due to avg distance from mean failure. + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 0); + + // Repeat with slightly smaller distance, all factors should be added + params.min_avg_distance_from_mean = expected_avg_distance_from_mean - epsilon; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + factor_adder_->AddMeasurement(measurements_[1]); + factor_adder_->AddMeasurement(measurements_[2]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); +} + +TEST_F(VoSmartProjectionFactorAdderTest, ValidFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + Initialize(params); + const int track_id = 0; + const int times = 3; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + const gtsam::Pose3 pose(gtsam::Rot3::identity(), gtsam::Point3(time, time, 0)); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + // All measurements should be valid + EXPECT_EQ(factor_adder_->AddFactors(1, 3, factors_), 1); + // Keys 1,2,3, should match to measurements 0,1,2 + EXPECT_SAME_FACTOR(0, {1, 2, 3}, measurements, {0, 1, 2}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, InvalidLastMeasurementFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + params.max_num_points_per_factor = 4; + Initialize(params); + const int track_id = 0; + const int times = 4; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + gtsam::Rot3 rotation = gtsam::Rot3::identity(); + gtsam::Point3 translation(time, time, 0); + // Make last measurement invalid. + // Turn the camera in the opposite direction so point can't be triangulated + if (time == times) { + rotation = gtsam::Rot3::Ypr(M_PI / 2, 0., -M_PI / 2); + } + const gtsam::Pose3 pose(rotation, translation); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + factor_adder_->AddMeasurement(measurements[3]); + // Valid factor since with removed measurement should stil have two points. + EXPECT_EQ(factor_adder_->AddFactors(0, 5, factors_), 1); + // Middle measurement should be removed. + // Keys 1,2,3 should match to measurements 0,1,2 + EXPECT_SAME_FACTOR(0, {1, 2, 3}, measurements, {0, 1, 2}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, InvalidMiddleMeasurementFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + params.max_num_points_per_factor = 4; + Initialize(params); + const int track_id = 0; + const int times = 4; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + gtsam::Rot3 rotation = gtsam::Rot3::identity(); + gtsam::Point3 translation(time, time, 0); + // Make middle measurement invalid. + // Turn the camera in the opposite direction so point can't be triangulated + if (time == 3) { + rotation = gtsam::Rot3::Ypr(M_PI / 2, 0., -M_PI / 2); + } + const gtsam::Pose3 pose(rotation, translation); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + factor_adder_->AddMeasurement(measurements[3]); + // Valid factor since with removed measurement should stil have two points. + EXPECT_EQ(factor_adder_->AddFactors(1, 4, factors_), 1); + // Middle measurement should be removed. + // Keys 1,2,4 should match to measurements 0,1,3 + EXPECT_SAME_FACTOR(0, {1, 2, 4}, measurements, {0, 1, 3}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, InvalidLastTwoMeasurementsFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + params.max_num_points_per_factor = 5; + Initialize(params); + const int track_id = 0; + const int times = 5; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + gtsam::Rot3 rotation = gtsam::Rot3::identity(); + gtsam::Point3 translation(time, time, 0); + // Make last two measurements invalid. + // Turn the camera in the opposite direction so point can't be triangulated + if (time >= 4) { + rotation = gtsam::Rot3::Ypr(M_PI / 2, 0., -M_PI / 2); + } + const gtsam::Pose3 pose(rotation, translation); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + factor_adder_->AddMeasurement(measurements[3]); + factor_adder_->AddMeasurement(measurements[4]); + // Valid factor since with removed measurement should still have two points. + EXPECT_EQ(factor_adder_->AddFactors(1, 5, factors_), 1); + // Middle measurement should be removed. + // Keys 1,2,3 should match to measurements 0,1,2 + EXPECT_SAME_FACTOR(0, {1, 2, 3}, measurements, {0, 1, 2}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, InvalidFirstTwoMeasurementsFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + params.max_num_points_per_factor = 5; + Initialize(params); + const int track_id = 0; + const int times = 5; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + gtsam::Rot3 rotation = gtsam::Rot3::identity(); + gtsam::Point3 translation(time, time, 0); + // Make first two measurements invalid. + // Turn the camera in the opposite direction so point can't be triangulated + if (time < 3) { + rotation = gtsam::Rot3::Ypr(M_PI / 2, 0., -M_PI / 2); + } + const gtsam::Pose3 pose(rotation, translation); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + factor_adder_->AddMeasurement(measurements[3]); + factor_adder_->AddMeasurement(measurements[4]); + // Valid factor since with removed measurement should still have two points. + EXPECT_EQ(factor_adder_->AddFactors(1, 5, factors_), 1); + // First two measurements should be removed. + // Keys 3,4,5 should match to measurements 2,3,4 + EXPECT_SAME_FACTOR(0, {3, 4, 5}, measurements, {2, 3, 4}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, TooSoonFactors) { + auto params = DefaultParams(); + Initialize(params); + const int max_factors = std::min(params.max_num_factors, num_tracks_); + // Add first measurement + // No factors should be added since there are too few measurements for each factor + factor_adder_->AddMeasurement(measurements_[0]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(0), factors_), 0); + // Track: Measurement Timestamps + // 0: 0 + // 1: 0 + // 2: 0 + EXPECT_EQ(factors_.size(), 0); + // Add second measurement + factor_adder_->AddMeasurement(measurements_[1]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(1), factors_), max_factors); + // Track: Measurement Timestamps + // 0: 0, 1 + // 1: 0, 1 + // 2: 0, 1 + EXPECT_SAME_FACTOR(0, {0, 1}); + EXPECT_SAME_FACTOR(1, {0, 1}); + EXPECT_EQ(factors_.size(), max_factors); + // Add factor too soon, set node adder latest measurement before factor newest_allowed_time + // so factor can't be created. + factor_adder_->AddMeasurement(measurements_[2]); + node_adder_->latest_measurement_time_ = 1.5; + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); + // Track: Measurement Timestamps + // 0: 0, 1, 2 + // 1: 0, 1, 2 + // 2: 0, 1, 2 + EXPECT_SAME_FACTOR(0, {0, 1}); + EXPECT_SAME_FACTOR(0, {0, 1}); + EXPECT_EQ(factors_.size(), max_factors); + // Change node adder latest measurement time back, make sure measurement for factor + // adder hasn't been removed and latest measurements are still added to factor. + node_adder_->latest_measurement_time_ = 2; + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); + EXPECT_SAME_FACTOR(0, {0, 1, 2}); + EXPECT_SAME_FACTOR(1, {0, 1, 2}); + EXPECT_EQ(factors_.size(), max_factors); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/factor_adders/test/test_vo_smart_projection_factor_adder.test b/localization/factor_adders/test/test_vo_smart_projection_factor_adder.test new file mode 100644 index 0000000000..2a4ef70dc0 --- /dev/null +++ b/localization/factor_adders/test/test_vo_smart_projection_factor_adder.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_factors/CMakeLists.txt b/localization/graph_factors/CMakeLists.txt new file mode 100644 index 0000000000..41c2761245 --- /dev/null +++ b/localization/graph_factors/CMakeLists.txt @@ -0,0 +1,137 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(graph_factors) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + localization_common + localization_measurements + vision_common +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS localization_common localization_measurements vision_common +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_inverse_depth_projection_factor + test/test_inverse_depth_projection_factor.test + test/test_inverse_depth_projection_factor.cc + ) + target_link_libraries(test_inverse_depth_projection_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_line_factor + test/test_point_to_line_factor.test + test/test_point_to_line_factor.cc + ) + target_link_libraries(test_point_to_line_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_line_segment_factor + test/test_point_to_line_segment_factor.test + test/test_point_to_line_segment_factor.cc + ) + target_link_libraries(test_point_to_line_segment_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_plane_factor + test/test_point_to_plane_factor.test + test/test_point_to_plane_factor.cc + ) + target_link_libraries(test_point_to_plane_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_point_between_factor + test/test_point_to_point_between_factor.test + test/test_point_to_point_between_factor.cc + ) + target_link_libraries(test_point_to_point_between_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_handrail_endpoint_factor + test/test_point_to_handrail_endpoint_factor.test + test/test_point_to_handrail_endpoint_factor.cc + ) + target_link_libraries(test_point_to_handrail_endpoint_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_silu + test/test_silu.test + test/test_silu.cc + ) + target_link_libraries(test_silu + graph_factors ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/graph_factors/include/graph_factors/cumulative_factor.h b/localization/graph_factors/include/graph_factors/cumulative_factor.h new file mode 100644 index 0000000000..6f3c2192dd --- /dev/null +++ b/localization/graph_factors/include/graph_factors/cumulative_factor.h @@ -0,0 +1,40 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_CUMULATIVE_FACTOR_H_ +#define GRAPH_FACTORS_CUMULATIVE_FACTOR_H_ + +#include + +#include + +namespace gtsam { +// Base case for cumulative factors which have a variable-sized set of measurements and keys accumulated +// over time. When using with a sliding window graph, old measurements and keys should +// be removed instead of removing the entire factor. +class CumulativeFactor { + public: + virtual ~CumulativeFactor() = default; + + // Returns a copy of the factor with keys in keys_to_remove removed along with + // any associated measurements. + virtual boost::shared_ptr PrunedCopy(const std::unordered_set& keys_to_remove) const = 0; +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_CUMULATIVE_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/inverse_depth_projection_factor.h b/localization/graph_factors/include/graph_factors/inverse_depth_projection_factor.h new file mode 100644 index 0000000000..b698c7eae1 --- /dev/null +++ b/localization/graph_factors/include/graph_factors/inverse_depth_projection_factor.h @@ -0,0 +1,148 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_INVERSE_DEPTH_PROJECTION_FACTOR_H_ +#define GRAPH_FACTORS_INVERSE_DEPTH_PROJECTION_FACTOR_H_ + +#include + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Non-linear factor for a constraint derived from a 2D measurement. The + * calibration is known and the landmark point is represented using the inverse depth parameterization. + */ +class InverseDepthProjectionFactor : public NoiseModelFactor3 { + protected: + Point2 measured_; + bool verboseCheirality_; + + public: + typedef NoiseModelFactor3 Base; + typedef boost::shared_ptr shared_ptr; + typedef InverseDepthProjectionFactor This; + + InverseDepthProjectionFactor() : measured_(0, 0), verboseCheirality_(false) {} + + /** + * Constructor + * @param measured is the 2 dimensional location of point in image (the + * measurement) + * @param model is the standard deviation + * @param sourcePoseKey is the index of the source camera + * @param inverseDepthMeasurementKey is the index of the inverse depth measurement + * @param targetPoseKey is the index of the target camera + */ + InverseDepthProjectionFactor(const Point2& measured, const SharedNoiseModel& model, Key sourcePoseKey, + Key inverseDepthMeasurementKey, Key targetPoseKey) + : Base(model, sourcePoseKey, inverseDepthMeasurementKey, targetPoseKey), + measured_(measured), + verboseCheirality_(false) {} + + /** + * Constructor with exception-handling flags + * @param measured is the 2 dimensional location of point in image (the + * measurement) + * @param model is the standard deviation + * @param sourcePoseKey is the index of the source camera + * @param inverseDepthMeasurementKey is the index of the inverse depth measurement + * @param targetPoseKey is the index of the target camera + * @param verboseCheirality determines whether exceptions are printed for + * Cheirality + */ + InverseDepthProjectionFactor(const Point2& measured, const SharedNoiseModel& model, Key sourcePoseKey, + Key inverseDepthCameraPoseKey, Key targetPoseKey, bool verboseCheirality) + : Base(model, sourcePoseKey, inverseDepthCameraPoseKey, targetPoseKey), + measured_(measured), + verboseCheirality_(verboseCheirality) {} + + virtual ~InverseDepthProjectionFactor() {} + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast(NonlinearFactor::shared_ptr(new This(*this))); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "InverseDepthProjectionFactor, z = "; + traits::Print(measured_); + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& world_T_source, + const vision_common::InverseDepthMeasurement& inverseDepthMeasurement, + const Pose3& world_T_target, + boost::optional d_projected_point_d_world_T_source = boost::none, + boost::optional d_projected_point_d_inverse_depth = boost::none, + boost::optional d_projected_point_d_world_T_target = boost::none) const override { + const auto projected_measurement = + inverseDepthMeasurement.Project(world_T_source, world_T_target, d_projected_point_d_world_T_source, + d_projected_point_d_world_T_target, d_projected_point_d_inverse_depth); + if (!projected_measurement) { + if (d_projected_point_d_world_T_source) *d_projected_point_d_world_T_source = Matrix::Zero(2, 6); + if (d_projected_point_d_world_T_target) *d_projected_point_d_world_T_target = Matrix::Zero(2, 6); + if (d_projected_point_d_inverse_depth) *d_projected_point_d_inverse_depth = Matrix::Zero(2, 1); + if (verboseCheirality_) + std::cout << "Landmark moved behind camera " << DefaultKeyFormatter(this->key2()) << std::endl; + return Vector2::Constant(0.0); + } + return *projected_measurement - measured_; + } + + const Point2& measured() const { return measured_; } + + inline bool verboseCheirality() const { return verboseCheirality_; } + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(measured_); + ar& BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam + +#endif // GRAPH_FACTORS_INVERSE_DEPTH_PROJECTION_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/loc_pose_factor.h b/localization/graph_factors/include/graph_factors/loc_pose_factor.h new file mode 100644 index 0000000000..c3a578a011 --- /dev/null +++ b/localization/graph_factors/include/graph_factors/loc_pose_factor.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_LOC_POSE_FACTOR_H_ +#define GRAPH_FACTORS_LOC_POSE_FACTOR_H_ + +#include +#include + +namespace gtsam { +// Allows for differentiation between loc pose factors and other pose priors, perhaps +// added as starting priors or marginalization factors +class LocPoseFactor : public PriorFactor { + private: + typedef PriorFactor Base; + + public: + typedef typename boost::shared_ptr shared_ptr; + + LocPoseFactor() {} + + virtual ~LocPoseFactor() {} + + LocPoseFactor(Key key, const Pose3& prior, const SharedNoiseModel& model = nullptr) : Base(key, prior, model) {} + + LocPoseFactor(Key key, const Pose3& prior, const Matrix& covariance) : Base(key, prior, covariance) {} + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_LOC_POSE_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/loc_projection_factor.h b/localization/graph_factors/include/graph_factors/loc_projection_factor.h new file mode 100644 index 0000000000..1f39bee668 --- /dev/null +++ b/localization/graph_factors/include/graph_factors/loc_projection_factor.h @@ -0,0 +1,231 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_LOC_PROJECTION_FACTOR_H_ +#define GRAPH_FACTORS_LOC_PROJECTION_FACTOR_H_ + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Non-linear factor for a constraint derived from a 2D measurement. The + * calibration and landmark point are known here. Adapted from gtsam + * GenericProjectionFactor. + */ +template +class LocProjectionFactor : public NoiseModelFactor1 { + protected: + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + LANDMARK landmark_point_; ///< Landmark point + boost::shared_ptr K_; ///< shared pointer to calibration object + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: + ///< false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions + ///< (default: false) + + public: + /// shorthand for base class type + typedef NoiseModelFactor1 Base; + + /// shorthand for this class + typedef LocProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + LocProjectionFactor() + : measured_(0, 0), landmark_point_(0, 0, 0), throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor + * @param measured is the 2 dimensional location of point in image (the + * measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) + */ + LocProjectionFactor(const Point2& measured, const LANDMARK& landmark_point, const SharedNoiseModel& model, + Key poseKey, const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) + : Base(model, poseKey), + measured_(measured), + landmark_point_(landmark_point), + K_(K), + body_P_sensor_(body_P_sensor), + throwCheirality_(false), + verboseCheirality_(false) {} + + /** + * Constructor with exception-handling flags + * @param measured is the 2 dimensional location of point in image (the + * measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are + * rethrown + * @param verboseCheirality determines whether exceptions are printed for + * Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) + */ + LocProjectionFactor(const Point2& measured, const LANDMARK& landmark_point, const SharedNoiseModel& model, + Key poseKey, const boost::shared_ptr& K, bool throwCheirality, + bool verboseCheirality, boost::optional body_P_sensor = boost::none) + : Base(model, poseKey), + measured_(measured), + landmark_point_(landmark_point), + K_(K), + body_P_sensor_(body_P_sensor), + throwCheirality_(throwCheirality), + verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~LocProjectionFactor() {} + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast(NonlinearFactor::shared_ptr(new This(*this))); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "LocProjectionFactor, z = "; + traits::Print(measured_); + std::cout << " landmark_point = "; + traits::Print(landmark_point_); + if (this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol) && + traits::Equals(this->landmark_point_, e->landmark_point_, tol) && this->K_->equals(*e->K_, tol) && + ((!body_P_sensor_ && !e->body_P_sensor_) || + (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /// Evaluate error h(x)-z and optionally derivatives + // TODO(rsoussan): Replace PinholeCamera with PinholePose? + Vector evaluateError(const Pose3& pose, boost::optional H1 = boost::none) const { + try { + if (body_P_sensor_) { + if (H1) { + Matrix H0; + PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); + Point2 reprojectionError(camera.project(landmark_point_, H1, boost::none, boost::none) - measured_); + *H1 = *H1 * H0; + return reprojectionError; + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(landmark_point_, H1, boost::none, boost::none) - measured_; + } + } else { + PinholeCamera camera(pose, *K_); + return camera.project(landmark_point_, H1, boost::none, boost::none) - measured_; + } + } catch (CheiralityException& e) { + if (H1) *H1 = Matrix::Zero(2, 6); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark moved behind camera " << DefaultKeyFormatter(this->key()) << std::endl; + if (throwCheirality_) throw CheiralityException(this->key()); + } + return Vector2::Constant(0.0); + } + + bool cheiralityError(const Pose3& pose) const { + try { + if (body_P_sensor_) { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + camera.project(landmark_point_) - measured_; + } else { + PinholeCamera camera(pose, *K_); + camera.project(landmark_point_) - measured_; + } + } catch (CheiralityException& e) { + return true; + } + return false; + } + + /** return the measurement */ + const Point2& measured() const { return measured_; } + + const LANDMARK& landmark_point() const { return landmark_point_; } + + LANDMARK& landmark_point() { return landmark_point_; } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { return K_; } + + /** return verbosity */ + inline bool verboseCheirality() const { return verboseCheirality_; } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { return throwCheirality_; } + + boost::optional body_P_sensor() const { return body_P_sensor_; } + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(measured_); + ar& BOOST_SERIALIZATION_NVP(landmark_point_); + ar& BOOST_SERIALIZATION_NVP(K_); + ar& BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar& BOOST_SERIALIZATION_NVP(throwCheirality_); + ar& BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// traits +template +struct traits> + : public Testable> {}; + +} // namespace gtsam + +#endif // GRAPH_FACTORS_LOC_PROJECTION_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/point_to_handrail_endpoint_factor.h b/localization/graph_factors/include/graph_factors/point_to_handrail_endpoint_factor.h new file mode 100644 index 0000000000..c31b0b912b --- /dev/null +++ b/localization/graph_factors/include/graph_factors/point_to_handrail_endpoint_factor.h @@ -0,0 +1,126 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ + +#include +#include +#include + +#include + +namespace gtsam { +class PointToHandrailEndpointFactor : public NoiseModelFactor1 { + typedef NoiseModelFactor1 Base; + typedef PointToHandrailEndpointFactor This; + + public: + PointToHandrailEndpointFactor() {} + + PointToHandrailEndpointFactor(const Point3& sensor_t_point, const Point3& world_t_endpoint_a, + const Point3& world_t_endpoint_b, const Pose3& body_T_sensor, + const SharedNoiseModel& model, Key pose_key) + : Base(model, pose_key), + sensor_t_point_(sensor_t_point), + world_t_endpoint_a_(world_t_endpoint_a), + world_t_endpoint_b_(world_t_endpoint_b), + body_T_sensor_(body_T_sensor) {} + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PointToHandrailEndpointFactor, z = "; + traits::Print(sensor_t_point_); + traits::Print(world_t_endpoint_a_); + traits::Print(world_t_endpoint_b_); + traits::Print(body_T_sensor_); + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && traits::Equals(this->sensor_t_point(), e->sensor_t_point(), tol) && + traits::Equals(this->world_t_endpoint_a(), e->world_t_endpoint_a(), tol) && + traits::Equals(this->world_t_endpoint_b(), e->world_t_endpoint_b(), tol) && + traits::Equals(this->body_T_sensor(), e->body_T_sensor(), tol); + } + + Vector evaluateError(const Pose3& world_T_body, boost::optional H = boost::none) const override { + if (H) { + Matrix66 d_world_T_sensor_d_world_T_body; + Matrix36 d_world_t_point_d_world_T_sensor; + Matrix33 d_local_d_world_t_point; + const auto error = getError(world_T_body, d_world_T_sensor_d_world_T_body, d_world_t_point_d_world_T_sensor, + d_local_d_world_t_point); + *H = d_local_d_world_t_point * d_world_t_point_d_world_T_sensor * d_world_T_sensor_d_world_T_body; + return error; + } + return getError(world_T_body); + } + + Vector getError(const Pose3& world_T_body, OptionalJacobian<6, 6> d_world_T_sensor_d_world_T_body = boost::none, + OptionalJacobian<3, 6> d_world_t_point_d_world_T_sensor = boost::none, + OptionalJacobian<3, 3> d_local_d_world_t_point = boost::none) const { + const Pose3 world_T_sensor = world_T_body.transformPoseFrom(body_T_sensor_, d_world_T_sensor_d_world_T_body); + const Point3 world_t_point = world_T_sensor.transformFrom(sensor_t_point_, d_world_t_point_d_world_T_sensor); + const double distance_to_endpoint_a = (world_t_point - world_t_endpoint_a_).norm(); + const double distance_to_endpoint_b = (world_t_point - world_t_endpoint_b_).norm(); + // TODO(rsoussan): Find closest endpoint before creating factor and stick to it? Add option to redo correspondence? + const Point3& world_t_closest_endpoint = + distance_to_endpoint_a < distance_to_endpoint_b ? world_t_endpoint_a_ : world_t_endpoint_b_; + const Point3 world_F_point_t_closest_endpoint = + local(world_t_point, world_t_closest_endpoint, d_local_d_world_t_point); + return world_F_point_t_closest_endpoint; + } + + // In gtsam terminology, local is the difference in the tangent space between two Lie group elements. + // In this case this is simply the difference between the two Point3 elements. + static Point3 local(const Point3& point1, const Point3& point2, + OptionalJacobian<3, 3> d_local_d_point1 = boost::none) { + if (d_local_d_point1) { + *d_local_d_point1 = I_3x3; + } + return point1 - point2; + } + + const Point3& sensor_t_point() const { return sensor_t_point_; } + const Point3& world_t_endpoint_a() const { return world_t_endpoint_a_; } + const Point3& world_t_endpoint_b() const { return world_t_endpoint_b_; } + const Pose3& body_T_sensor() const { return body_T_sensor_; } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(sensor_t_point_); + ar& BOOST_SERIALIZATION_NVP(world_t_endpoint_a_); + ar& BOOST_SERIALIZATION_NVP(world_t_endpoint_b_); + ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); + } + + Point3 sensor_t_point_; + Point3 world_t_endpoint_a_; + Point3 world_t_endpoint_b_; + Pose3 body_T_sensor_; + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/point_to_line_factor.h b/localization/graph_factors/include/graph_factors/point_to_line_factor.h new file mode 100644 index 0000000000..38fd276dbd --- /dev/null +++ b/localization/graph_factors/include/graph_factors/point_to_line_factor.h @@ -0,0 +1,66 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_POINT_TO_LINE_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_LINE_FACTOR_H_ + +#include + +#include +#include + +#include + +namespace gtsam { +class PointToLineFactor : public PointToLineFactorBase { + typedef PointToLineFactorBase Base; + typedef PointToLineFactor This; + + public: + PointToLineFactor() {} + + PointToLineFactor(const Point3& sensor_t_point, const Pose3& world_T_line, const Pose3& body_T_sensor, + const SharedNoiseModel& model, Key pose_key, const bool use_silu = false) + : Base(sensor_t_point, world_T_line, body_T_sensor, model, pose_key) {} + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PointToLineFactor, z = "; + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { return Base::equals(p, tol); } + + Vector evaluateError(const Pose3& world_T_body, boost::optional H = boost::none) const override { + const auto line_t_point = Base::error(world_T_body, H); + if (H) { + // Remove last row as error does not account for z value in line_t_point + *H = H->topRows(2).eval(); + } + return line_t_point.head<2>(); + } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_POINT_TO_LINE_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/point_to_line_factor_base.h b/localization/graph_factors/include/graph_factors/point_to_line_factor_base.h new file mode 100644 index 0000000000..5e7a929417 --- /dev/null +++ b/localization/graph_factors/include/graph_factors/point_to_line_factor_base.h @@ -0,0 +1,115 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_POINT_TO_LINE_FACTOR_BASE_H_ +#define GRAPH_FACTORS_POINT_TO_LINE_FACTOR_BASE_H_ + +#include +#include +#include + +#include + +namespace gtsam { +class PointToLineFactorBase : public NoiseModelFactor1 { + typedef NoiseModelFactor1 Base; + typedef PointToLineFactorBase This; + + public: + PointToLineFactorBase() {} + + // The line in world_T_line is oriented such that the z axis is along the line and is initialized with an arbitrary + // orientation about the z axis. body_T_sensor is the sensor extrinsics and assumed to be static. + PointToLineFactorBase(const Point3& sensor_t_point, const Pose3& world_T_line, const Pose3& body_T_sensor, + const SharedNoiseModel& model, Key pose_key) + : Base(model, pose_key), + sensor_t_point_(sensor_t_point), + world_T_line_(world_T_line), + body_T_sensor_(body_T_sensor), + sensor_R_body_(body_T_sensor.inverse().rotation().matrix()), + body_t_sensor_(body_T_sensor_.translation()) {} + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PointToLineFactorBase, z = "; + traits::Print(sensor_t_point_); + traits::Print(world_T_line_); + traits::Print(body_T_sensor_); + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && traits::Equals(this->sensor_t_point(), e->sensor_t_point(), tol) && + traits::Equals(this->world_T_line(), e->world_T_line(), tol) && + traits::Equals(this->body_T_sensor(), e->body_T_sensor(), tol); + } + + Vector error(const Pose3& world_T_body, boost::optional H = boost::none) const { + const Pose3 world_T_sensor = world_T_body * body_T_sensor_; + const Pose3 line_T_sensor = world_T_line_.inverse() * world_T_sensor; + if (H) { + // GTSAM equivalent: + /* gtsam::Matrix H_a; + const auto world_T_sensor = world_T_body.transformPoseFrom(body_T_sensor_, H_a); + gtsam::Matrix H_b; + const auto line_T_sensor = world_T_line_.inverse().transformPoseFrom(world_T_sensor, boost::none, H_b); + gtsam::Matrix H_c; + const auto line_t_point = line_T_sensor.transformFrom(sensor_t_point_, H_c); + H = H_c *H_b* H_a; + */ + const Matrix3 line_R_sensor = line_T_sensor.rotation().matrix(); + const Matrix A = -1.0 * line_R_sensor * + (skewSymmetric(sensor_t_point_) + skewSymmetric(sensor_R_body_ * body_t_sensor_)) * + sensor_R_body_; + const Matrix B = line_R_sensor * sensor_R_body_; + // TODO(rsoussan): avoid zero initialization + *H = Eigen::Matrix::Zero(); + *H << A, B; + } + return line_T_sensor * sensor_t_point_; + } + + const Point3& sensor_t_point() const { return sensor_t_point_; } + const Pose3& world_T_line() const { return world_T_line_; } + const Pose3& body_T_sensor() const { return body_T_sensor_; } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(sensor_t_point_); + ar& BOOST_SERIALIZATION_NVP(world_T_line_); + ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); + ar& BOOST_SERIALIZATION_NVP(sensor_R_body_); + ar& BOOST_SERIALIZATION_NVP(body_t_sensor_); + } + + Point3 sensor_t_point_; + Pose3 world_T_line_; + Pose3 body_T_sensor_; + // Cached for faster Jacobian calculations + Matrix sensor_R_body_; + Point3 body_t_sensor_; + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_POINT_TO_LINE_FACTOR_BASE_H_ diff --git a/localization/graph_factors/include/graph_factors/point_to_line_segment_factor.h b/localization/graph_factors/include/graph_factors/point_to_line_segment_factor.h new file mode 100644 index 0000000000..5e701805c0 --- /dev/null +++ b/localization/graph_factors/include/graph_factors/point_to_line_segment_factor.h @@ -0,0 +1,106 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_POINT_TO_LINE_SEGMENT_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_LINE_SEGMENT_FACTOR_H_ + +#include +#include + +#include +#include + +#include + +namespace gtsam { +class PointToLineSegmentFactor : public PointToLineFactorBase { + typedef PointToLineFactorBase Base; + typedef PointToLineSegmentFactor This; + + public: + PointToLineSegmentFactor() {} + + PointToLineSegmentFactor(const Point3& sensor_t_point, const Pose3& world_T_line, const Pose3& body_T_sensor, + const double line_length, const SharedNoiseModel& model, Key pose_key, + const bool use_silu = false) + : Base(sensor_t_point, world_T_line, body_T_sensor, model, pose_key), + line_length_(line_length), + use_silu_(use_silu) {} + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast(gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PointToLineSegmentFactor, z = "; + std::cout << "line length: " << line_length_; + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return Base::equals(p, tol) && traits::Equals(this->line_length(), e->line_length(), tol); + } + + Vector evaluateError(const Pose3& world_T_body, boost::optional H = boost::none) const override { + const auto line_t_point = Base::error(world_T_body, H); + const auto line_z_point = line_t_point.z(); + Point3 error = line_t_point; + // Check if z value is in between segment endpoints, zero z error component and Jacobian row if so. + // Otherwise add or subtract half line length to the z error to make it reflect the closest distance to + // one of the segment endpoints. + if (!use_silu_) { + if (line_z_point >= -1.0 * line_length_ / 2.0 && line_z_point <= line_length_ / 2.0) { + error.z() = 0; + if (H) { + H->block<1, 6>(2, 0) = Eigen::Matrix::Zero(); + } + } else if (line_z_point < -1.0 * line_length_ / 2.0) { + error.z() = error.z() + line_length_ / 2.0; + } else { + error.z() = error.z() - line_length_ / 2.0; + } + } else { + if (H) { + Matrix11 d_silu_d_z; + error.z() = graph_factors::SiluWithOffsetTwoWay(error.z(), line_length_ / 2.0, d_silu_d_z); + H->block<1, 6>(2, 0) = d_silu_d_z * H->block<1, 6>(2, 0); + } + error.z() = graph_factors::SiluWithOffsetTwoWay(error.z(), line_length_ / 2.0); + } + return error; + } + + double line_length() const { return line_length_; } + bool use_silu() const { return use_silu_; } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(line_length_); + ar& BOOST_SERIALIZATION_NVP(use_silu_); + } + + double line_length_; + bool use_silu_; +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_POINT_TO_LINE_SEGMENT_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/point_to_plane_factor.h b/localization/graph_factors/include/graph_factors/point_to_plane_factor.h new file mode 100644 index 0000000000..b54e181743 --- /dev/null +++ b/localization/graph_factors/include/graph_factors/point_to_plane_factor.h @@ -0,0 +1,107 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_POINT_TO_PLANE_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_PLANE_FACTOR_H_ + +#include + +#include +#include +#include + +#include + +namespace gtsam { +class PointToPlaneFactor : public NoiseModelFactor1 { + typedef NoiseModelFactor1 Base; + typedef PointToPlaneFactor This; + + public: + PointToPlaneFactor() {} + + PointToPlaneFactor(const Point3& sensor_t_point, const localization_measurements::Plane& world_T_plane, + const Pose3& body_T_sensor, const SharedNoiseModel& model, Key pose_key) + : Base(model, pose_key), + sensor_t_point_(sensor_t_point), + world_T_plane_(world_T_plane), + body_T_sensor_(body_T_sensor) {} + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PointToPlaneFactor, z = "; + traits::Print(sensor_t_point_); + traits::Print(world_T_plane_); + traits::Print(body_T_sensor_); + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && traits::Equals(this->sensor_t_point(), e->sensor_t_point(), tol) && + traits::Equals(this->world_T_plane(), e->world_T_plane(), tol) && + traits::Equals(this->body_T_sensor(), e->body_T_sensor(), tol); + } + + Vector evaluateError(const Pose3& world_T_body, boost::optional H = boost::none) const override { + if (H) { + Matrix66 d_world_T_sensor_d_world_T_body; + Matrix36 d_world_t_point_d_world_T_sensor; + Matrix13 d_distance_d_world_t_point; + const auto error = getError(world_T_body, d_world_T_sensor_d_world_T_body, d_world_t_point_d_world_T_sensor, + d_distance_d_world_t_point); + *H = d_distance_d_world_t_point * d_world_t_point_d_world_T_sensor * d_world_T_sensor_d_world_T_body; + return error; + } + return getError(world_T_body); + } + + Vector getError(const Pose3& world_T_body, OptionalJacobian<6, 6> d_world_T_sensor_d_world_T_body = boost::none, + OptionalJacobian<3, 6> d_world_t_point_d_world_T_sensor = boost::none, + OptionalJacobian<1, 3> d_distance_d_world_t_point = boost::none) const { + const Pose3 world_T_sensor = world_T_body.transformPoseFrom(body_T_sensor_, d_world_T_sensor_d_world_T_body); + const Point3 world_t_point = world_T_sensor.transformFrom(sensor_t_point_, d_world_t_point_d_world_T_sensor); + const double distance = world_T_plane_.Distance(world_t_point, d_distance_d_world_t_point); + Vector error(1); + error << distance; + return error; + } + + const Point3& sensor_t_point() const { return sensor_t_point_; } + const localization_measurements::Plane& world_T_plane() const { return world_T_plane_; } + const Pose3& body_T_sensor() const { return body_T_sensor_; } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(sensor_t_point_); + ar& BOOST_SERIALIZATION_NVP(world_T_plane_); + ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); + } + + Point3 sensor_t_point_; + localization_measurements::Plane world_T_plane_; + Pose3 body_T_sensor_; + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_POINT_TO_PLANE_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/point_to_point_between_factor.h b/localization/graph_factors/include/graph_factors/point_to_point_between_factor.h new file mode 100644 index 0000000000..13385c969d --- /dev/null +++ b/localization/graph_factors/include/graph_factors/point_to_point_between_factor.h @@ -0,0 +1,92 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_POINT_TO_POINT_BETWEEN_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_POINT_BETWEEN_FACTOR_H_ + +#include +#include +#include + +#include + +namespace gtsam { +class PointToPointBetweenFactor : public NoiseModelFactor2 { + typedef NoiseModelFactor2 Base; + typedef PointToPointBetweenFactor This; + + public: + PointToPointBetweenFactor() {} + + PointToPointBetweenFactor(const Point3& sensor_t_point_source, const Point3& sensor_t_point_target, + const Pose3& body_T_sensor, const SharedNoiseModel& model, Key source_pose_key, + Key target_pose_key) + : Base(model, source_pose_key, target_pose_key), + sensor_t_point_source_(sensor_t_point_source), + sensor_t_point_target_(sensor_t_point_target), + body_T_sensor_(body_T_sensor) {} + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PointToPointBetweenFactor, z = "; + traits::Print(sensor_t_point_source_); + traits::Print(sensor_t_point_target_); + traits::Print(body_T_sensor_); + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && + traits::Equals(this->sensor_t_point_source(), e->sensor_t_point_source(), tol) && + traits::Equals(this->sensor_t_point_target(), e->sensor_t_point_target(), tol) && + traits::Equals(this->body_T_sensor(), e->body_T_sensor(), tol); + } + + Vector evaluateError(const Pose3& world_T_body_source, const Pose3& world_T_body_target, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + const auto world_t_point_source = world_T_body_source.transformFrom(body_T_sensor_ * sensor_t_point_source_, H1); + const auto world_t_point_target = world_T_body_target.transformFrom(body_T_sensor_ * sensor_t_point_target_, H2); + if (H2) *H2 = -1.0 * (*H2); + return world_t_point_source - world_t_point_target; + } + + const Point3& sensor_t_point_source() const { return sensor_t_point_source_; } + const Point3& sensor_t_point_target() const { return sensor_t_point_target_; } + const Pose3& body_T_sensor() const { return body_T_sensor_; } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(sensor_t_point_source_); + ar& BOOST_SERIALIZATION_NVP(sensor_t_point_target_); + ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); + } + + Point3 sensor_t_point_source_; + Point3 sensor_t_point_target_; + Pose3 body_T_sensor_; + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_POINT_TO_POINT_BETWEEN_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/pose_rotation_factor.h b/localization/graph_factors/include/graph_factors/pose_rotation_factor.h new file mode 100644 index 0000000000..9ef0c9251d --- /dev/null +++ b/localization/graph_factors/include/graph_factors/pose_rotation_factor.h @@ -0,0 +1,98 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_POSE_ROTATION_FACTOR_H_ +#define GRAPH_FACTORS_POSE_ROTATION_FACTOR_H_ + +#include +#include +#include + +#include + +namespace gtsam { +class PoseRotationFactor : public NoiseModelFactor2 { + typedef NoiseModelFactor2 Base; + typedef PoseRotationFactor This; + + public: + PoseRotationFactor() {} + + PoseRotationFactor(const Rot3& rotation, const SharedNoiseModel& model, Key pose_key_1, Key pose_key_2) + : Base(model, pose_key_1, pose_key_2), rotation_(rotation) {} + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast(gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PoseRotationFactor, z = "; + traits::Print(rotation_); + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && traits::Equals(this->rotation_, e->rotation(), tol); + } + + Vector evaluateError(const Pose3& pose1, const Pose3& pose2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + const auto& rot1 = pose1.rotation(H1); + const auto& rot2 = pose2.rotation(H2); + // TODO(rsoussan): How to use ref to block of H1 and H2 instead of making new matrices and copying? + Matrix H1_rot_matrix; + Matrix H2_rot_matrix; + boost::optional H1_rot = H1_rot_matrix; + boost::optional H2_rot = H2_rot_matrix; + if (!H1) H1_rot = boost::none; + if (!H2) H2_rot = boost::none; + // Adapted from BetweenFactor.h + const auto relative_rotation = traits::Between(rot1, rot2, H1_rot, H2_rot); + // manifold equivalent of h(x)-z -> log(z,h(x)) + traits::ChartJacobian::Jacobian Hlocal; + Vector error = traits::Local(rotation_, relative_rotation, boost::none, (H1_rot || H2_rot) ? &Hlocal : 0); + if (H1_rot) { + *H1_rot = Hlocal * (*H1_rot); + H1->block<3, 3>(0, 0) = H1_rot_matrix; + } + if (H2_rot) { + *H2_rot = Hlocal * (*H2_rot); + H2->block<3, 3>(0, 0) = H2_rot_matrix; + } + return error; + } + + const Rot3& rotation() const { return rotation_; } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(rotation_); + } + + Rot3 rotation_; + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_POSE_ROTATION_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/robust_smart_projection_pose_factor.h b/localization/graph_factors/include/graph_factors/robust_smart_projection_pose_factor.h new file mode 100644 index 0000000000..69ed2b2ed2 --- /dev/null +++ b/localization/graph_factors/include/graph_factors/robust_smart_projection_pose_factor.h @@ -0,0 +1,259 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ +#define GRAPH_FACTORS_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ + +#include + +#include + +#include +#include + +namespace gtsam { +template +class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactor, public CumulativeFactor { + typedef PinholePose Camera; + typedef SmartFactorBase Base; + typedef typename Camera::Measurement Z; + static const int Dim = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + public: + // For serialization + RobustSmartProjectionPoseFactor() {} + + /** + * Constructor + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param K (fixed) calibration, assumed to be the same for all cameras + * @param params parameters for the smart projection factors + */ + RobustSmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, + const boost::optional body_P_sensor, + const SmartProjectionParams& params = SmartProjectionParams(), + const bool rotation_only_fallback = false, const bool robust = true, + const double huber_k = 1.0) + : SmartProjectionPoseFactor(sharedNoiseModel, K, body_P_sensor, params), + sharedNoiseModel_(sharedNoiseModel), + params_(params), + rotation_only_fallback_(rotation_only_fallback), + robust_(robust), + huber_k_(huber_k) { + // From SmartFactorBase + if (!sharedNoiseModel) throw std::runtime_error("RobustSmartProjectionPoseFactor: sharedNoiseModel is required"); + SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast(sharedNoiseModel); + if (!sharedIsotropic) throw std::runtime_error("RobustSmartProjectionPoseFactor: needs isotropic"); + noise_inv_sigma_ = 1.0 / sharedIsotropic->sigma(); + triangulation_params_ = params.triangulation; + } + + // Helper function to return copy of factor with only measurements for allowed keys. + boost::shared_ptr PrunedCopy(const std::unordered_set& keys_to_remove) const final { + boost::shared_ptr pruned_factor( + new RobustSmartProjectionPoseFactor(sharedNoiseModel_, this->calibration(), this->body_P_sensor(), params_, + rotation_only_fallback_, robust_, huber_k_)); + for (int i = 0; i < this->keys().size(); ++i) { + if (keys_to_remove.count(this->keys()[i]) == 0) pruned_factor->add(this->measured()[i], this->keys()[i]); + } + // If too few measurements, factor is invalid + if (pruned_factor->measured().size() < 2) return nullptr; + return pruned_factor; + } + + boost::shared_ptr linearize(const Values& values) const override { + typename Base::Cameras cameras = this->cameras(values); + // if (!this->triangulateForLinearize(cameras)) return boost::make_shared>(this->keys()); + const auto result = this->triangulateSafe(cameras); + // Adapted from SmartFactorBase::CreateJacobianSVDFactor + size_t m = this->keys().size(); + typename Base::FBlocks F; + Vector b; + const size_t M = ZDim * m; + Matrix E0(M, M - 3); + + // Handle behind camera result with rotation only factors (see paper) + // Degenerate result tends to lead to solve failures, so return empty factor in this case + if (result.valid()) { + this->computeJacobiansSVD(F, E0, b, cameras, *(this->point())); + } else if (useForRotationOnly(result)) { // Rotation only factor + Unit3 backProjected = cameras[0].backprojectPointAtInfinity(this->measured().at(0)); + // Cheirality error can still occur with backprojection + try { + this->computeJacobiansSVD(F, E0, b, cameras, backProjected); + } catch (...) { + return boost::make_shared>(this->keys()); + } + } else { // Empty factor // NOLINT + return boost::make_shared>(this->keys()); + } + return createRegularJacobianFactorSVD(this->keys(), F, E0, b); + } + + bool useForRotationOnly(const gtsam::TriangulationResult& result) const { + // Use rotation only for all failure cases + return true; + } + + double error(const Values& values) const override { + if (this->active(values)) { + try { + const double total_reprojection_loss = this->totalReprojectionError(this->cameras(values)); + const auto result = this->point(); + if (!result.valid() && !useForRotationOnly(result)) return 0.0; + // Multiply by 2 since totalReporjectionError divides mahal distance by 2, and robust_model_->loss + // expects mahal distance + const double loss = robust_ ? robustLoss(2.0 * total_reprojection_loss) : total_reprojection_loss; + return loss; + } catch (...) { + // Catch cheirality and other errors, zero on errors + return 0.0; + } + } else { // Inactive + return 0.0; + } + } + + // These "serialized" functions are only needed due to an error in gtsam for serializing result_. + // Call these instead of error() and point() for a smart factor that has been serialized. + // TODO(rsoussan): Remove these when gtsam bug fixed + double serialized_error(const Values& values) const { + if (this->active(values)) { + try { + const auto point = serialized_point(values); + const double total_reprojection_loss = this->totalReprojectionError(this->cameras(values), point); + if (!point.valid() && !useForRotationOnly(point)) return 0.0; + // Multiply by 2 since totalReporjectionError divides mahal distance by 2, and robust_model_->loss + // expects mahal distance + const double loss = robust_ ? robustLoss(2.0 * total_reprojection_loss) : total_reprojection_loss; + return loss; + } catch (...) { + // Catch cheirality and other errors, zero on errors + return 0.0; + } + } else { // Inactive + return 0.0; + } + } + + TriangulationResult serialized_point(const Values& values) const { + return gtsam::triangulateSafe(this->cameras(values), this->measured(), triangulation_params_); + } + + bool robust() const { return robust_; } + + double noise_inv_sigma() const { return noise_inv_sigma_; } + + // More efficient implementation of robust loss (also avoids inheritance calls) + double robustLoss(const double mahal_distance) const { + const double sqrt_mahal_distance = std::sqrt(mahal_distance); + const double absError = std::abs(sqrt_mahal_distance); + if (absError <= huber_k_) { // |x| <= k + return mahal_distance / 2; + } else { // |x| > k + return huber_k_ * (absError - (huber_k_ / 2)); + } + } + + bool valid(const Values& values) const { + typename Base::Cameras cameras = this->cameras(values); + const auto point = this->triangulateSafe(cameras); + if (point.valid()) { + return true; + } else if (useForRotationOnly(point)) { + Unit3 backProjected = cameras[0].backprojectPointAtInfinity(this->measured().at(0)); + try { + cameras.reprojectionError(backProjected, this->measured()); + } catch (...) { + return false; + } + return true; + } else { + return false; + } + // Shouldn't get here + return false; + } + + private: + template + boost::shared_ptr> createRegularJacobianFactorSVD( + const KeyVector& keys, + const std::vector, Eigen::aligned_allocator>>& + Fblocks, + const Matrix& Enull, const Vector& b) const { + typedef std::pair KeyMatrix; + + Vector reduced_error = Enull.transpose() * b; + // Apply noise whitening and robust weighting manually to more efficiently robustify + // error vector and jacobians. Equivalent to calling whitenSystem with a robust noise model. + // Assumes noise is diagonal (required for this factor anyway). + reduced_error *= noise_inv_sigma_; + double reduced_matrix_weight = noise_inv_sigma_; + if (robust_) { + const double robust_weight = robustWeight(reduced_error.norm()); + reduced_error *= robust_weight; + reduced_matrix_weight *= robust_weight; + } + + size_t numKeys = Enull.rows() / ZDim; + size_t m2 = Enull.cols(); + std::vector reduced_matrices; + reduced_matrices.reserve(numKeys); + for (size_t k = 0; k < Fblocks.size(); ++k) { + Key key = keys[k]; + reduced_matrices.emplace_back( + KeyMatrix(key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k] * reduced_matrix_weight)); + } + + return boost::make_shared>(reduced_matrices, reduced_error); + } + + // TODO(rsoussan): profile these calls vs. gtsam better, is there a significant improvement? + // More efficient implementation of robust weight (also avoids inheritance calls) + double robustWeight(const double error_norm) const { + const double squared_weight = (error_norm <= huber_k_) ? (1.0) : (huber_k_ / error_norm); + return std::sqrt(squared_weight); + } + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SmartProjectionPoseFactor); + ar& BOOST_SERIALIZATION_NVP(noise_inv_sigma_); + ar& BOOST_SERIALIZATION_NVP(huber_k_); + ar& BOOST_SERIALIZATION_NVP(robust_); + ar& BOOST_SERIALIZATION_NVP(rotation_only_fallback_); + ar& BOOST_SERIALIZATION_NVP(triangulation_params_); + } + + bool rotation_only_fallback_; + double robust_; + double huber_k_; + double noise_inv_sigma_; + // TODO(rsoussan): Remove once result_ serialization bug in gtsam fixed + TriangulationParameters triangulation_params_; + // Copies stored here to access for empty measurement copy creation + SharedNoiseModel sharedNoiseModel_; + SmartProjectionParams params_; +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ diff --git a/localization/graph_factors/include/graph_factors/silu.h b/localization/graph_factors/include/graph_factors/silu.h new file mode 100644 index 0000000000..01fe7cf987 --- /dev/null +++ b/localization/graph_factors/include/graph_factors/silu.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_SILU_H_ +#define GRAPH_FACTORS_SILU_H_ + +#include + +namespace graph_factors { +// Sigmoid linear unit function +double Silu(const double x, gtsam::OptionalJacobian<1, 1> d_silu_d_x = boost::none); + +double SiluWithOffset(const double x, const double offset, gtsam::OptionalJacobian<1, 1> d_silu_d_x = boost::none); + +// Create silu that creates a two way ramp: __/ +// / +// centered at x = 0 rather than a single ramp _/ Note that this is discontinous at x = 0. +double SiluWithOffsetTwoWay(const double x, const double offset, + gtsam::OptionalJacobian<1, 1> d_silu_d_x = boost::none); +} // namespace graph_factors + +#endif // GRAPH_FACTORS_SILU_H_ diff --git a/localization/graph_factors/package.xml b/localization/graph_factors/package.xml new file mode 100644 index 0000000000..ac4252ebe6 --- /dev/null +++ b/localization/graph_factors/package.xml @@ -0,0 +1,23 @@ + + graph_factors + 1.0.0 + + The graph factors package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + localization_common + localization_measurements + vision_common + localization_common + localization_measurements + vision_common + diff --git a/localization/graph_factors/readme.md b/localization/graph_factors/readme.md new file mode 100644 index 0000000000..b4627100ca --- /dev/null +++ b/localization/graph_factors/readme.md @@ -0,0 +1,30 @@ +\page graphfactors Graph Factors + +# Package Overview +The graph factors package provides a set of graph factors for use with a GTSAM graph optimizer. See the GraphOptimizer package for more information on their usage. + +## Factors + +### LocPoseFactor +The LocPoseFactor is simply a gtsam::PriorFactor\ that enables the differention of a pose prior from a localization map-based image feature factor. + +### LocProjectionFactor +The LocProjectionFactor is almost a direct copy of the gtsam::ProjectionFactor except it does not optimize for the 3D feature point location. + +### PoseRotationFactor +The PoseRotationFactor constrains two gtsam::Pose3 nodes using their relative rotation. + +### PointToHandrailEndpointFactor.h +The PointToHandrailEndpointFactor constrains a gtsam::Pose3 using a handrail endpoint detection in the sensor frame compared with the closest handrail endpoint from a know handrail. + +### PointToLineFactor.h +The PointToLineFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a line in the world frame. + +### PointToLineSegmentFactor.h +The PointToLineSegmentFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a line segment in the world frame. This factor contains a discontinuity in the Jacobian as there is zero error along the line segment axis if the point is between line segment endpoints and non-zero error otherwise. An option to use a SILU (Sigmoid Linear Unit) approximation is provided for this case. + +###PointToPlaneFactor.h +The PointToPlaneFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a plane in the world frame. + +### RobustSmartProjectionFactor +The RobustSmartProjectionFactor adds to the gtsam::SmartProjectionFactor by providing a robust huber kernel. Additionally, it fixes some issues in the SmartProjectionFactor allowing for a rotation-only fallback when using the JacobianSVD option and allows for proper serialization of the factor. diff --git a/localization/graph_factors/src/silu.cc b/localization/graph_factors/src/silu.cc new file mode 100644 index 0000000000..bb8f03b378 --- /dev/null +++ b/localization/graph_factors/src/silu.cc @@ -0,0 +1,43 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +namespace graph_factors { +double Silu(const double x, gtsam::OptionalJacobian<1, 1> d_silu_d_x) { + const double silu_x = x / (1.0 + std::exp(-1.0 * x)); + if (d_silu_d_x) { + *d_silu_d_x << (1.0 + std::exp(-1.0 * x) + x * std::exp(-1.0 * x)) / std::pow(1 + std::exp(-1.0 * x), 2); + } + return silu_x; +} + +double SiluWithOffset(const double x, const double offset, gtsam::OptionalJacobian<1, 1> d_silu_d_x) { + const double offset_x = x - offset; + return Silu(offset_x, d_silu_d_x); +} + +double SiluWithOffsetTwoWay(const double x, const double offset, gtsam::OptionalJacobian<1, 1> d_silu_d_x) { + if (x >= 0) return SiluWithOffset(x, offset, d_silu_d_x); + const double inverted_x = -1.0 * x; + const double silu_inverted_x = SiluWithOffset(inverted_x, offset, d_silu_d_x); + return -1.0 * silu_inverted_x; +} +} // namespace graph_factors diff --git a/localization/graph_factors/test/test_inverse_depth_projection_factor.cc b/localization/graph_factors/test/test_inverse_depth_projection_factor.cc new file mode 100644 index 0000000000..9ada1f7a2c --- /dev/null +++ b/localization/graph_factors/test/test_inverse_depth_projection_factor.cc @@ -0,0 +1,211 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace vc = vision_common; + +TEST(InverseDepthProjectionFactorTester, EvaluateError) { + constexpr double translation_stddev = 0.05; + constexpr double rotation_stddev = 1; + for (int i = 0; i < 50; ++i) { + const gtsam::Point3 source_cam_t_measurement = lc::RandomFrontFacingPoint(); + const gtsam::Pose3 body_T_cam = lc::RandomPose(); + const Eigen::Matrix3d intrinsics = lc::RandomIntrinsics(); + const auto source_measurement = vc::Project(source_cam_t_measurement, intrinsics); + vc::InverseDepthMeasurement inverse_depth_measurement(1.0 / source_cam_t_measurement.z(), source_measurement, + intrinsics, body_T_cam); + // Make sure point is still likely to project into target camera frame + const gtsam::Pose3 source_cam_T_target_cam = + lc::GtPose(lc::RandomIdentityCenteredIsometry3d(translation_stddev, rotation_stddev)); + const gtsam::Pose3 world_T_source_body = lc::RandomPose(); + const gtsam::Pose3 world_T_target_body = + world_T_source_body * body_T_cam * source_cam_T_target_cam * body_T_cam.inverse(); + const gtsam::Point3 target_cam_t_measurement = source_cam_T_target_cam.inverse() * source_cam_t_measurement; + const auto target_measurement = vc::Project(target_cam_t_measurement, intrinsics); + const auto new_target_measurement = lc::RandomPoint2d(); + const auto noise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector2(1, 1)); + const gtsam::InverseDepthProjectionFactor factor(new_target_measurement, noise, 1, 2, 3); + const auto error = factor.evaluateError(world_T_source_body, inverse_depth_measurement, world_T_target_body); + const Eigen::Vector2d expected_error = target_measurement - new_target_measurement; + EXPECT_MATRIX_NEAR(error, expected_error, 1e-6); + } +} + +TEST(InverseDepthProjectionFactorTester, InvalidError) { + const gtsam::Point3 source_cam_t_measurement = lc::RandomFrontFacingPoint(); + const gtsam::Pose3 body_T_cam = lc::RandomPose(); + const Eigen::Matrix3d intrinsics = lc::RandomIntrinsics(); + const auto source_measurement = vc::Project(source_cam_t_measurement, intrinsics); + vc::InverseDepthMeasurement inverse_depth_measurement(1.0 / source_cam_t_measurement.z(), source_measurement, + intrinsics, body_T_cam); + // Move target camera frame in front of measurement point, ensures that point is behind target frame and projection + // should fail + const gtsam::Pose3 source_cam_T_target_cam = + lc::GtPose(lc::Isometry3d(Eigen::Vector3d(0, 0, source_cam_t_measurement.z() + 1), Eigen::Matrix3d::Identity())); + const gtsam::Pose3 world_T_source_body = lc::RandomPose(); + const gtsam::Pose3 world_T_target_body = + world_T_source_body * body_T_cam * source_cam_T_target_cam * body_T_cam.inverse(); + const auto new_target_measurement = lc::RandomPoint2d(); + const auto noise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector2(1, 1)); + const gtsam::InverseDepthProjectionFactor factor(new_target_measurement, noise, 1, 2, 3); + gtsam::Matrix d_error_d_inverse_depth; + gtsam::Matrix d_error_d_world_T_source_body; + gtsam::Matrix d_error_d_world_T_target_body; + const auto error = + factor.evaluateError(world_T_source_body, inverse_depth_measurement, world_T_target_body, + d_error_d_world_T_source_body, d_error_d_inverse_depth, d_error_d_world_T_target_body); + EXPECT_MATRIX_NEAR(error, Eigen::Vector2d::Zero(), 1e-6); + EXPECT_MATRIX_NEAR(d_error_d_inverse_depth, (Eigen::Matrix::Zero()), 1e-6); + EXPECT_MATRIX_NEAR(d_error_d_world_T_source_body, (Eigen::Matrix::Zero()), 1e-6); + EXPECT_MATRIX_NEAR(d_error_d_world_T_target_body, (Eigen::Matrix::Zero()), 1e-6); +} + +TEST(InverseDepthProjectionFactorTester, Jacobians) { + constexpr double translation_stddev = 0.05; + constexpr double rotation_stddev = 1; + for (int i = 0; i < 5000; ++i) { + const gtsam::Point3 source_cam_t_measurement = lc::RandomFrontFacingPoint(); + const gtsam::Pose3 body_T_cam = lc::RandomPose(); + const Eigen::Matrix3d intrinsics = lc::RandomIntrinsics(); + const auto source_measurement = vc::Project(source_cam_t_measurement, intrinsics); + const double inverse_depth = 1.0 / source_cam_t_measurement.z(); + vc::InverseDepthMeasurement inverse_depth_measurement(inverse_depth, source_measurement, intrinsics, body_T_cam); + // Make sure point is still likely to project into target camera frame + const gtsam::Pose3 source_cam_T_target_cam = + lc::GtPose(lc::RandomIdentityCenteredIsometry3d(translation_stddev, rotation_stddev)); + const gtsam::Pose3 world_T_source_body = lc::RandomPose(); + const gtsam::Pose3 world_T_target_body = + world_T_source_body * body_T_cam * source_cam_T_target_cam * body_T_cam.inverse(); + const auto new_target_measurement = lc::RandomPoint2d(); + const auto noise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector2(1, 1)); + const gtsam::InverseDepthProjectionFactor factor(new_target_measurement, noise, 1, 2, 3); + gtsam::Matrix d_error_d_inverse_depth; + gtsam::Matrix d_error_d_world_T_source_body; + gtsam::Matrix d_error_d_world_T_target_body; + const auto error = + factor.evaluateError(world_T_source_body, inverse_depth_measurement, world_T_target_body, + d_error_d_world_T_source_body, d_error_d_inverse_depth, d_error_d_world_T_target_body); + const auto numerical_d_error_d_world_T_source_body = + gtsam::numericalDerivative31( + boost::function( + boost::bind(>sam::InverseDepthProjectionFactor::evaluateError, factor, _1, _2, _3, boost::none, boost::none, + boost::none)), + world_T_source_body, inverse_depth_measurement, world_T_target_body, 1e-6); + EXPECT_MATRIX_NEAR(numerical_d_error_d_world_T_source_body, d_error_d_world_T_source_body, 1e-6); + const auto numerical_d_error_d_inverse_depth = + gtsam::numericalDerivative32( + boost::function( + boost::bind(>sam::InverseDepthProjectionFactor::evaluateError, factor, _1, _2, _3, boost::none, boost::none, + boost::none)), + world_T_source_body, inverse_depth_measurement, world_T_target_body); + EXPECT_MATRIX_NEAR(numerical_d_error_d_inverse_depth, d_error_d_inverse_depth, 1e-6); + const auto numerical_d_error_d_world_T_target_body = + gtsam::numericalDerivative33( + boost::function( + boost::bind(>sam::InverseDepthProjectionFactor::evaluateError, factor, _1, _2, _3, boost::none, boost::none, + boost::none)), + world_T_source_body, inverse_depth_measurement, world_T_target_body, 1e-6); + EXPECT_MATRIX_NEAR(numerical_d_error_d_world_T_target_body, d_error_d_world_T_target_body, 1e-6); + } +} + +TEST(InverseDepthProjectionFactorTester, Optimization) { + constexpr double translation_stddev = 0.05; + constexpr double rotation_stddev = 1; + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 source_cam_t_measurement = lc::RandomFrontFacingPoint(); + const gtsam::Pose3 body_T_cam = lc::RandomPose(); + const Eigen::Matrix3d intrinsics = lc::RandomIntrinsics(); + const auto source_measurement = vc::Project(source_cam_t_measurement, intrinsics); + const double inverse_depth = 1.0 / source_cam_t_measurement.z(); + vc::InverseDepthMeasurement inverse_depth_measurement(inverse_depth, source_measurement, intrinsics, body_T_cam); + // Make sure point is still likely to project into target camera frame + const gtsam::Pose3 source_cam_T_target_cam = + lc::GtPose(lc::RandomIdentityCenteredIsometry3d(translation_stddev, rotation_stddev)); + const gtsam::Pose3 world_T_source_body = lc::RandomPose(); + const gtsam::Pose3 world_T_target_body = + world_T_source_body * body_T_cam * source_cam_T_target_cam * body_T_cam.inverse(); + const auto perfect_target_measurement = inverse_depth_measurement.Project(world_T_source_body, world_T_target_body); + ASSERT_TRUE(perfect_target_measurement != boost::none); + const auto noise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector2(1e-6, 1e-6)); + const gtsam::Key world_T_source_body_key = 1; + const gtsam::Key inverse_depth_measurement_key = 2; + const gtsam::Key world_T_target_body_key = 3; + const gtsam::InverseDepthProjectionFactor factor(*perfect_target_measurement, noise, world_T_source_body_key, + inverse_depth_measurement_key, world_T_target_body_key); + // No noise + { + gtsam::NonlinearFactorGraph graph; + graph.add(factor); + gtsam::Values values; + values.insert(world_T_source_body_key, world_T_source_body); + values.insert(inverse_depth_measurement_key, inverse_depth_measurement); + values.insert(world_T_target_body_key, world_T_target_body); + const auto result = gtsam::LevenbergMarquardtOptimizer(graph, values).optimize(); + EXPECT_MATRIX_NEAR(world_T_source_body, result.at(world_T_source_body_key), 1e-6); + EXPECT_NEAR(inverse_depth, + (result.at(inverse_depth_measurement_key)).inverse_depth(), 1e-6); + EXPECT_MATRIX_NEAR(world_T_target_body, result.at(world_T_target_body_key), 1e-6); + } + // Noisy inverse depth + { + gtsam::NonlinearFactorGraph graph; + graph.add(factor); + gtsam::Values values; + values.insert(world_T_source_body_key, world_T_source_body); + // Make sure inverse depth is still positive but starts with a lot of noise + const double noisy_inverse_depth = + inverse_depth + lc::RandomDouble(-1.0 * inverse_depth + 1e-6, 2.0 * inverse_depth); + const auto noisy_inverse_depth_measurement = + vc::InverseDepthMeasurement(noisy_inverse_depth, source_measurement, intrinsics, body_T_cam); + values.insert(inverse_depth_measurement_key, noisy_inverse_depth_measurement); + values.insert(world_T_target_body_key, world_T_target_body); + // Set non noisy values constant + gtsam::NonlinearEquality1 world_T_source_body_equality_factor(world_T_source_body, + world_T_source_body_key); + graph.add(world_T_source_body_equality_factor); + gtsam::NonlinearEquality1 world_T_target_body_equality_factor(world_T_target_body, + world_T_target_body_key); + graph.add(world_T_target_body_equality_factor); + const auto result = gtsam::LevenbergMarquardtOptimizer(graph, values).optimize(); + EXPECT_MATRIX_NEAR(world_T_source_body, result.at(world_T_source_body_key), 1e-6); + EXPECT_NEAR(inverse_depth, + (result.at(inverse_depth_measurement_key)).inverse_depth(), 1e-6); + EXPECT_MATRIX_NEAR(world_T_target_body, result.at(world_T_target_body_key), 1e-6); + } + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_factors/test/test_inverse_depth_projection_factor.test b/localization/graph_factors/test/test_inverse_depth_projection_factor.test new file mode 100644 index 0000000000..ad2a4bf727 --- /dev/null +++ b/localization/graph_factors/test/test_inverse_depth_projection_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.cc b/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.cc new file mode 100644 index 0000000000..bf09a16143 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.cc @@ -0,0 +1,87 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace sym = gtsam::symbol_shorthand; +TEST(PointToHandrailEndpointFactorTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 sensor_t_point = lc::RandomPoint3d(); + const gtsam::Point3 world_t_handrail_endpoint_a = lc::RandomPoint3d(); + const gtsam::Point3 world_t_handrail_endpoint_b = lc::RandomPoint3d(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body = lc::RandomPose(); + // Ignore case where sensor point is directly between two endpoints as this leads to a known + // discontinuity in the Jacobian. + { + const gtsam::Point3 world_t_point = world_T_body * body_T_sensor * sensor_t_point; + const double distance_to_endpoint_a = (world_t_point - world_t_handrail_endpoint_a).norm(); + const double distance_to_endpoint_b = (world_t_point - world_t_handrail_endpoint_b).norm(); + if (std::abs(distance_to_endpoint_a - distance_to_endpoint_b) < 1e-4) continue; + } + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToHandrailEndpointFactor factor(sensor_t_point, world_t_handrail_endpoint_a, + world_t_handrail_endpoint_b, body_T_sensor, noise, sym::P(0)); + gtsam::Matrix H; + const auto factor_error = factor.evaluateError(world_T_body, H); + const auto numerical_H = gtsam::numericalDerivative11( + boost::function( + boost::bind(>sam::PointToHandrailEndpointFactor::evaluateError, factor, _1, boost::none)), + world_T_body); + EXPECT_MATRIX_NEAR(numerical_H, H, 1e-6); + } +} + +TEST(PointToHandrailEndpointFactorTester, SelectingCorrectEndpoint) { + const gtsam::Point3 world_t_handrail_endpoint_a(1, 0, 0); + const gtsam::Point3 world_t_handrail_endpoint_b(2, 0, 0); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const auto noise = gtsam::noiseModel::Unit::Create(3); + // Closer to endpoint a + { + const gtsam::Point3 sensor_t_point(1.2, 0, 0); + const gtsam::PointToHandrailEndpointFactor factor(sensor_t_point, world_t_handrail_endpoint_a, + world_t_handrail_endpoint_b, body_T_sensor, noise, sym::P(0)); + const auto error = factor.evaluateError(world_T_body); + EXPECT_MATRIX_NEAR(error, gtsam::Vector3(0.2, 0, 0), 1e-6); + } + // Closer to endpoint b + { + const gtsam::Point3 sensor_t_point(1.6, 0, 0); + const gtsam::PointToHandrailEndpointFactor factor(sensor_t_point, world_t_handrail_endpoint_a, + world_t_handrail_endpoint_b, body_T_sensor, noise, sym::P(0)); + const auto error = factor.evaluateError(world_T_body); + EXPECT_MATRIX_NEAR(error, gtsam::Vector3(-0.4, 0, 0), 1e-6); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.test b/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.test new file mode 100644 index 0000000000..5229c12165 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_factors/test/test_point_to_line_factor.cc b/localization/graph_factors/test/test_point_to_line_factor.cc new file mode 100644 index 0000000000..7e76120d1c --- /dev/null +++ b/localization/graph_factors/test/test_point_to_line_factor.cc @@ -0,0 +1,145 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace sym = gtsam::symbol_shorthand; +TEST(PointToLineFactorTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 sensor_t_point = lc::RandomPoint3d(); + const gtsam::Pose3 world_T_line = lc::RandomPose(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body = lc::RandomPose(); + const auto noise = gtsam::noiseModel::Unit::Create(2); + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + gtsam::Matrix H; + const auto factor_error = factor.evaluateError(world_T_body, H); + const auto numerical_H = gtsam::numericalDerivative11( + boost::function( + boost::bind(>sam::PointToLineFactor::evaluateError, factor, _1, boost::none)), + world_T_body); + EXPECT_MATRIX_NEAR(numerical_H, H, 1e-6); + } +} + +TEST(PointToLineFactorTester, InvariantToZTranslation) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 5.0); + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_DOUBLE_EQ(error_1_norm, error_2_norm); +} + +TEST(PointToLineFactorTester, InvariantToRotationAboutZAxis) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const auto noise = gtsam::noiseModel::Unit::Create(2); + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + { + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Rot3 z_axis_rotation = gtsam::Rot3::Rz(2.13); + const gtsam::Point3 sensor_t_rotated_point = z_axis_rotation * sensor_t_point; + const gtsam::PointToLineFactor factor(sensor_t_rotated_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_DOUBLE_EQ(error_1_norm, error_2_norm); +} + +TEST(PointToLineFactorTester, IncreaseErrorWithIncreasedXDistance) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Point3 sensor_t_point(30.3, 2.0, 3.0); + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_LT(error_1_norm, error_2_norm); +} + +TEST(PointToLineFactorTester, IncreaseErrorWithIncreasedYDistance) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Point3 sensor_t_point(1.0, -17.2, 3.0); + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_LT(error_1_norm, error_2_norm); +} +TEST(PointToLineFactorTester, ZeroErrorForZeroOffest) { + double error_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const auto noise = gtsam::noiseModel::Unit::Create(2); + const gtsam::Point3 sensor_t_point(0, 0, 0); + const gtsam::PointToLineFactor factor(sensor_t_point, world_T_line, body_T_sensor, noise, sym::P(0)); + error_norm = (factor.evaluateError(world_T_body)).norm(); + EXPECT_DOUBLE_EQ(error_norm, 0); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_factors/test/test_point_to_line_factor.test b/localization/graph_factors/test/test_point_to_line_factor.test new file mode 100644 index 0000000000..7a3e23b464 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_line_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_factors/test/test_point_to_line_segment_factor.cc b/localization/graph_factors/test/test_point_to_line_segment_factor.cc new file mode 100644 index 0000000000..1922d23341 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_line_segment_factor.cc @@ -0,0 +1,319 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace sym = gtsam::symbol_shorthand; +TEST(PointToLineSegmentFactorTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 sensor_t_point = lc::RandomPoint3d(); + const gtsam::Pose3 world_T_line = lc::RandomPose(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body = lc::RandomPose(); + const double line_length = lc::RandomPositiveDouble(); + const auto noise = gtsam::noiseModel::Unit::Create(2); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + gtsam::Matrix H; + const auto factor_error = factor.evaluateError(world_T_body, H); + const auto numerical_H = gtsam::numericalDerivative11( + boost::function( + boost::bind(>sam::PointToLineSegmentFactor::evaluateError, factor, _1, boost::none)), + world_T_body); + EXPECT_MATRIX_NEAR(numerical_H, H, 1e-6); + } +} + +TEST(PointToLineSegmentFactorTester, JacobianWithSilu) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 sensor_t_point = lc::RandomPoint3d(); + const gtsam::Pose3 world_T_line = lc::RandomPose(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body = lc::RandomPose(); + const double line_length = lc::RandomPositiveDouble(); + const auto noise = gtsam::noiseModel::Unit::Create(2); + const bool use_silu = true; + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), use_silu); + gtsam::Matrix H; + const auto factor_error = factor.evaluateError(world_T_body, H); + const auto numerical_H = gtsam::numericalDerivative11( + boost::function( + boost::bind(>sam::PointToLineSegmentFactor::evaluateError, factor, _1, boost::none)), + world_T_body); + EXPECT_MATRIX_NEAR(numerical_H, H, 1e-6); + } +} + +TEST(PointToLineSegmentFactorTester, ZeroZErrorInBetweenSegmentEndpoints) { + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 0.1); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + const auto error = factor.evaluateError(world_T_body); + EXPECT_DOUBLE_EQ(error.z(), 0); + } + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, -0.1); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + const auto error = factor.evaluateError(world_T_body); + EXPECT_DOUBLE_EQ(error.z(), 0); + } + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + const auto error = factor.evaluateError(world_T_body); + EXPECT_DOUBLE_EQ(error.z(), 0); + } +} + +TEST(PointToLineSegmentFactorTester, NonZeroZErrorOutisdeOfSegmentEndpoints) { + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 1); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + const auto error = factor.evaluateError(world_T_body); + EXPECT_DOUBLE_EQ(std::abs(error.z()), 0.85); + } + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, -1); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + const auto error = factor.evaluateError(world_T_body); + EXPECT_DOUBLE_EQ(std::abs(error.z()), 0.85); + } +} + +TEST(PointToLineSegmentFactorTester, InvariantToRotationAboutZAxis) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + { + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Rot3 z_axis_rotation = gtsam::Rot3::Rz(2.13); + const gtsam::Point3 sensor_t_rotated_point = z_axis_rotation * sensor_t_point; + const gtsam::PointToLineSegmentFactor factor(sensor_t_rotated_point, world_T_line, body_T_sensor, line_length, + noise, sym::P(0)); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_DOUBLE_EQ(error_1_norm, error_2_norm); +} + +TEST(PointToLineSegmentFactorTester, IncreaseErrorWithIncreasedXDistance) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Point3 sensor_t_point(30.3, 2.0, 3.0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_LT(error_1_norm, error_2_norm); +} + +TEST(PointToLineSegmentFactorTester, IncreaseErrorWithIncreasedYDistance) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Point3 sensor_t_point(1.0, -17.2, 3.0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0)); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_LT(error_1_norm, error_2_norm); +} + +// Tests with Silu +TEST(PointToLineSegmentFactorTester, ZeroZErrorInBetweenSegmentEndpointsWithSilu) { + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 0.1); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + const auto error = factor.evaluateError(world_T_body); + EXPECT_NEAR(error.z(), 0, 1e-1); + } + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, -0.1); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + const auto error = factor.evaluateError(world_T_body); + EXPECT_NEAR(error.z(), 0, 1e-1); + } + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + const auto error = factor.evaluateError(world_T_body); + EXPECT_NEAR(error.z(), 0, 1e-1); + } +} + +TEST(PointToLineSegmentFactorTester, NonZeroZErrorOutisdeOfSegmentEndpointsWithSilu) { + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 1); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + const auto error = factor.evaluateError(world_T_body); + EXPECT_NEAR(std::abs(error.z()), 0.85, 3e-1); + } + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, -1); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + const auto error = factor.evaluateError(world_T_body); + EXPECT_NEAR(std::abs(error.z()), 0.85, 3e-1); + } +} + +TEST(PointToLineSegmentFactorTester, InvariantToRotationAboutZAxisWithSilu) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + { + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Rot3 z_axis_rotation = gtsam::Rot3::Rz(2.13); + const gtsam::Point3 sensor_t_rotated_point = z_axis_rotation * sensor_t_point; + const gtsam::PointToLineSegmentFactor factor(sensor_t_rotated_point, world_T_line, body_T_sensor, line_length, + noise, sym::P(0), true); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_DOUBLE_EQ(error_1_norm, error_2_norm); +} + +TEST(PointToLineSegmentFactorTester, IncreaseErrorWithIncreasedXDistanceWithSilu) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Point3 sensor_t_point(30.3, 2.0, 3.0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_LT(error_1_norm, error_2_norm); +} + +TEST(PointToLineSegmentFactorTester, IncreaseErrorWithIncreasedYDistanceWithSilu) { + double error_1_norm; + double error_2_norm; + const gtsam::Pose3 world_T_line = gtsam::Pose3::identity(); + const gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + const gtsam::Pose3 world_T_body = gtsam::Pose3::identity(); + const double line_length = 0.3; + const auto noise = gtsam::noiseModel::Unit::Create(2); + { + const gtsam::Point3 sensor_t_point(1.0, 2.0, 3.0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + error_1_norm = (factor.evaluateError(world_T_body)).norm(); + } + { + const gtsam::Point3 sensor_t_point(1.0, -17.2, 3.0); + const gtsam::PointToLineSegmentFactor factor(sensor_t_point, world_T_line, body_T_sensor, line_length, noise, + sym::P(0), true); + error_2_norm = (factor.evaluateError(world_T_body)).norm(); + } + EXPECT_LT(error_1_norm, error_2_norm); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_factors/test/test_point_to_line_segment_factor.test b/localization/graph_factors/test/test_point_to_line_segment_factor.test new file mode 100644 index 0000000000..f9e7fb9327 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_line_segment_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_factors/test/test_point_to_plane_factor.cc b/localization/graph_factors/test/test_point_to_plane_factor.cc new file mode 100644 index 0000000000..63422c1e76 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_plane_factor.cc @@ -0,0 +1,62 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; + +localization_measurements::Plane RandomPlane() { + gtsam::Point3 point = lc::RandomPoint3d(); + gtsam::Vector3 normal = lc::RandomVector3d().normalized(); + return localization_measurements::Plane(point, normal); +} + +TEST(PointToPlaneFactorTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 sensor_t_point = lc::RandomPoint3d(); + const lm::Plane world_T_handrail_plane = RandomPlane(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body = lc::RandomPose(); + const auto noise = gtsam::noiseModel::Unit::Create(1); + const gtsam::PointToPlaneFactor factor(sensor_t_point, world_T_handrail_plane, body_T_sensor, noise, sym::P(0)); + gtsam::Matrix H; + const auto factor_error = factor.evaluateError(world_T_body, H); + const auto numerical_H = gtsam::numericalDerivative11( + boost::function( + boost::bind(>sam::PointToPlaneFactor::evaluateError, factor, _1, boost::none)), + world_T_body); + EXPECT_MATRIX_NEAR(numerical_H, H, 1e-6); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_factors/test/test_point_to_plane_factor.test b/localization/graph_factors/test/test_point_to_plane_factor.test new file mode 100644 index 0000000000..961c1a8276 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_plane_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_factors/test/test_point_to_point_between_factor.cc b/localization/graph_factors/test/test_point_to_point_between_factor.cc new file mode 100644 index 0000000000..356c548757 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_point_between_factor.cc @@ -0,0 +1,111 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace sym = gtsam::symbol_shorthand; + +TEST(PointToPointBetweenFactorTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 sensor_t_point_source = lc::RandomPoint3d(); + const gtsam::Point3 sensor_t_point_target = lc::RandomPoint3d(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body_source = lc::RandomPose(); + const gtsam::Pose3 world_T_body_target = lc::RandomPose(); + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToPointBetweenFactor factor(sensor_t_point_source, sensor_t_point_target, body_T_sensor, noise, + sym::P(0), sym::P(1)); + gtsam::Matrix H1; + gtsam::Matrix H2; + const auto factor_error = factor.evaluateError(world_T_body_source, world_T_body_target, H1, H2); + const auto numerical_H1 = gtsam::numericalDerivative21( + boost::function( + boost::bind(>sam::PointToPointBetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), + world_T_body_source, world_T_body_target, 1e-5); + EXPECT_MATRIX_NEAR(numerical_H1, H1, 1e-6); + const auto numerical_H2 = gtsam::numericalDerivative22( + boost::function( + boost::bind(>sam::PointToPointBetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), + world_T_body_source, world_T_body_target, 1e-5); + EXPECT_MATRIX_NEAR(numerical_H2, H2, 1e-6); + } +} + +TEST(PointToPointBetweenFactorTester, SamePointAndPoseError) { + for (int i = 0; i < 50; ++i) { + const gtsam::Point3 sensor_t_point_source = lc::RandomPoint3d(); + const gtsam::Point3 sensor_t_point_target = sensor_t_point_source; + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body_source = lc::RandomPose(); + const gtsam::Pose3 world_T_body_target = world_T_body_source; + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToPointBetweenFactor factor(sensor_t_point_source, sensor_t_point_target, body_T_sensor, noise, + sym::P(0), sym::P(1)); + const auto factor_error = factor.evaluateError(world_T_body_source, world_T_body_target); + EXPECT_MATRIX_NEAR(factor_error, Eigen::Vector3d::Zero(), 1e-6); + } +} + +TEST(PointToPointBetweenFactorTester, DifferentPointIdentityPosesError) { + for (int i = 0; i < 50; ++i) { + const gtsam::Point3 sensor_t_point_source = lc::RandomPoint3d(); + const gtsam::Point3 sensor_t_point_target = lc::RandomPoint3d(); + const gtsam::Pose3 body_T_sensor = lc::GtPose(Eigen::Isometry3d::Identity()); + const gtsam::Pose3 world_T_body_source = lc::GtPose(Eigen::Isometry3d::Identity()); + const gtsam::Pose3 world_T_body_target = world_T_body_source; + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToPointBetweenFactor factor(sensor_t_point_source, sensor_t_point_target, body_T_sensor, noise, + sym::P(0), sym::P(1)); + const auto factor_error = factor.evaluateError(world_T_body_source, world_T_body_target); + EXPECT_MATRIX_NEAR(factor_error, (sensor_t_point_source - sensor_t_point_target), 1e-6); + } +} + +TEST(PointToPointBetweenFactorTester, DifferentPointSamePoseError) { + for (int i = 0; i < 50; ++i) { + const gtsam::Point3 sensor_t_point_source = lc::RandomPoint3d(); + const gtsam::Point3 sensor_t_point_target = lc::RandomPoint3d(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body_source = lc::RandomPose(); + const gtsam::Pose3 world_T_body_target = world_T_body_source; + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToPointBetweenFactor factor(sensor_t_point_source, sensor_t_point_target, body_T_sensor, noise, + sym::P(0), sym::P(1)); + const auto factor_error = factor.evaluateError(world_T_body_source, world_T_body_target); + EXPECT_MATRIX_NEAR( + factor_error, + (world_T_body_source.rotation() * body_T_sensor.rotation() * (sensor_t_point_source - sensor_t_point_target)), + 1e-6); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_factors/test/test_point_to_point_between_factor.test b/localization/graph_factors/test/test_point_to_point_between_factor.test new file mode 100644 index 0000000000..caaae6b4f4 --- /dev/null +++ b/localization/graph_factors/test/test_point_to_point_between_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_factors/test/test_rotation_factor.cc b/localization/graph_factors/test/test_rotation_factor.cc new file mode 100644 index 0000000000..e67d6c3caf --- /dev/null +++ b/localization/graph_factors/test/test_rotation_factor.cc @@ -0,0 +1,60 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sym = gtsam::symbol_shorthand; +TEST(RotationFactorTester, Jacobian) { + const gtsam::Pose3 world_T_body_1; + const gtsam::Point3 body_1_t_body_2(0.1, 0.1, 0.1); + const gtsam::Rot3 body_1_R_body_2(gtsam::Rot3::RzRyRx(3.0 * M_PI / 180.0, 2.0 * M_PI / 180.0, 1.0 * M_PI / 180.0)); + const gtsam::Pose3 body_1_T_body_2(body_1_R_body_2, body_1_t_body_2); + const gtsam::Pose3 world_T_body_2 = world_T_body_1 * body_1_T_body_2; + // Use body_1_T_body_2 as a pertubation + const gtsam::Pose3 world_T_perturbed_body_2 = world_T_body_2 * body_1_T_body_2; + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PoseRotationFactor rotation_factor(body_1_R_body_2, noise, sym::P(0), sym::P(1)); + gtsam::Matrix factor_H1, factor_H2; + const auto factor_error = + rotation_factor.evaluateError(world_T_body_1, world_T_perturbed_body_2, factor_H1, factor_H2); + const auto numerical_H1 = gtsam::numericalDerivative21( + boost::function( + boost::bind(>sam::PoseRotationFactor::evaluateError, rotation_factor, _1, _2, boost::none, boost::none)), + world_T_body_1, world_T_perturbed_body_2); + EXPECT_MATRIX_NEAR(numerical_H1, factor_H1, 1e-6); + const auto numerical_H2 = gtsam::numericalDerivative22( + boost::function( + boost::bind(>sam::PoseRotationFactor::evaluateError, rotation_factor, _1, _2, boost::none, boost::none)), + world_T_body_1, world_T_perturbed_body_2); + EXPECT_MATRIX_NEAR(numerical_H2, factor_H2, 1e-6); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_factors/test/test_rotation_factor.test b/localization/graph_factors/test/test_rotation_factor.test new file mode 100644 index 0000000000..78500e974d --- /dev/null +++ b/localization/graph_factors/test/test_rotation_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_factors/test/test_silu.cc b/localization/graph_factors/test/test_silu.cc new file mode 100644 index 0000000000..1e47aeb513 --- /dev/null +++ b/localization/graph_factors/test/test_silu.cc @@ -0,0 +1,189 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include + +#include + +namespace gf = graph_factors; +namespace lc = localization_common; +TEST(SiluTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const double x = lc::RandomDouble(); + gtsam::Matrix H; + gf::Silu(x, H); + const auto numerical_H = gtsam::numericalDerivative11( + boost::function(boost::bind(&gf::Silu, _1, boost::none)), x); + ASSERT_TRUE(gtsam::equal_with_abs_tol(numerical_H, H.matrix(), 1e-6)); + } +} + +TEST(SiluWithOffsetTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const double x = lc::RandomDouble(); + const double offset = lc::RandomPositiveDouble(); + gtsam::Matrix H; + gf::SiluWithOffset(x, offset, H); + const auto numerical_H = gtsam::numericalDerivative21( + boost::function(boost::bind(&gf::SiluWithOffset, _1, _2, boost::none)), x, + offset); + ASSERT_TRUE(gtsam::equal_with_abs_tol(numerical_H, H.matrix(), 1e-6)); + } +} + +TEST(SiluWithOffsetTester, XMuchGreaterThanOffset) { + const double x = 3001.3; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffset(x, offset); + EXPECT_NEAR(x - offset, silu_x, 1e-6); +} + +TEST(SiluWithOffsetTester, XGreaterThanOffset) { + const double x = 13.2; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffset(x, offset); + EXPECT_NEAR(x - offset, silu_x, 1e-2); +} + +TEST(SiluWithOffsetTester, XSlightlyGreaterThanOffset) { + const double x = 5.2; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffset(x, offset); + EXPECT_NEAR(x - offset, silu_x, 5e-2); +} + +TEST(SiluWithOffsetTester, XSlightlyLessThanOffset) { + const double x = 5.0; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffset(x, offset); + EXPECT_NEAR(0.0, silu_x, 5e-2); +} + +TEST(SiluWithOffsetTester, XLessThanOffset) { + const double x = 1.0; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffset(x, offset); + EXPECT_NEAR(0.0, silu_x, 1e-1); +} + +TEST(SiluWithOffsetTester, XMuchLessThanOffset) { + const double x = 3.0; + const double offset = 1003.3; + const double silu_x = gf::SiluWithOffset(x, offset); + EXPECT_NEAR(0.0, silu_x, 1e-6); +} + +TEST(SiluWithOffsetTwoWayTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const double x = lc::RandomDouble(); + const double offset = lc::RandomPositiveDouble(); + gtsam::Matrix H; + gf::SiluWithOffsetTwoWay(x, offset, H); + const auto numerical_H = gtsam::numericalDerivative21( + boost::function(boost::bind(&gf::SiluWithOffsetTwoWay, _1, _2, boost::none)), + x, offset); + ASSERT_TRUE(gtsam::equal_with_abs_tol(numerical_H, H.matrix(), 1e-6)); + } +} + +TEST(SiluWithOffsetTwoWayTester, XMuchGreaterThanOffset) { + const double x = 3001.3; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(x - offset, silu_x, 1e-6); +} + +TEST(SiluWithOffsetTwoWayTester, XGreaterThanOffset) { + const double x = 13.2; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(x - offset, silu_x, 1e-2); +} + +TEST(SiluWithOffsetTwoWayTester, XSlightlyGreaterThanOffset) { + const double x = 5.2; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(x - offset, silu_x, 5e-2); +} + +TEST(SiluWithOffsetTwoWayTester, XSlightlyLessThanOffset) { + const double x = 5.0; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(0.0, silu_x, 5e-2); +} + +TEST(SiluWithOffsetTwoWayTester, XLessThanOffset) { + const double x = 1.0; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(0.0, silu_x, 1e-1); +} + +TEST(SiluWithOffsetTwoWayTester, XMuchLessThanOffset) { + const double x = 3.0; + const double offset = 1003.3; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(0.0, silu_x, 1e-6); +} + +TEST(SiluWithOffsetTwoWayTester, NegativeXLessThanOffset) { + const double x = -13.2; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(x + offset, silu_x, 1e-2); +} + +TEST(SiluWithOffsetTwoWayTester, NegativeXSlightlyLessThanOffset) { + const double x = -5.2; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(x + offset, silu_x, 5e-2); +} + +TEST(SiluWithOffsetTwoWayTester, NegativeXSlightlyGreaterThanOffset) { + const double x = -5.0; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(0.0, silu_x, 5e-2); +} + +TEST(SiluWithOffsetTwoWayTester, NegativeXGreaterThanOffset) { + const double x = -1.0; + const double offset = 5.1; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(0.0, silu_x, 1e-1); +} + +TEST(SiluWithOffsetTwoWayTester, NegativeXMuchGreaterThanOffset) { + const double x = -3.0; + const double offset = 1003.3; + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); + EXPECT_NEAR(0.0, silu_x, 1e-6); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_factors/test/test_silu.test b/localization/graph_factors/test/test_silu.test new file mode 100644 index 0000000000..c896939e47 --- /dev/null +++ b/localization/graph_factors/test/test_silu.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_localizer/CMakeLists.txt b/localization/graph_localizer/CMakeLists.txt new file mode 100644 index 0000000000..79376275b8 --- /dev/null +++ b/localization/graph_localizer/CMakeLists.txt @@ -0,0 +1,92 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(graph_localizer) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + factor_adders + graph_factors + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer +) + +# Find OpenCV +find_package(OpenCV 4.0 REQUIRED) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} + CATKIN_DEPENDS + factor_adders + graph_factors + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer.h b/localization/graph_localizer/include/graph_localizer/graph_localizer.h new file mode 100644 index 0000000000..a9c58e05b2 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer.h @@ -0,0 +1,88 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ +#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +// Siding window graph optimizer that uses matched projections and odomery poses to perform localization. +// Uses the PoseNodeAdder to add relative odometry pose factors between graph nodes. +// Matched projections typically come from matching images to a map of image +// features. +class GraphLocalizer : public sliding_window_graph_optimizer::SlidingWindowGraphOptimizer { + public: + explicit GraphLocalizer(const GraphLocalizerParams& params); + + // For Serialization Only + GraphLocalizer() {} + + // Adds pose measurement to the pose node adder. + void AddPoseMeasurement(const localization_measurements::PoseWithCovarianceMeasurement& pose_measurement); + + // Adds sparse map matched projections measurement to loc factor adder. + void AddSparseMapMatchedProjectionsMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); + + // Adds AR tag matched projections measurement to loc factor adder. + void AddArTagMatchedProjectionsMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); + + // Returns a const reference to pose nodes. + const nodes::TimestampedNodes& pose_nodes() const; + + // Sets pose covariance interpolater for relative odometry pose node creation + void SetPoseCovarianceInterpolater( + const std::shared_ptr>& + pose_covariance_interpolater); + + private: + // bool ValidGraph() const final; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(sliding_window_graph_optimizer::SlidingWindowGraphOptimizer); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(sparse_map_loc_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(ar_tag_loc_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(pose_node_adder_); + } + + GraphLocalizerParams params_; + + // Factor Adders + std::shared_ptr> sparse_map_loc_factor_adder_; + std::shared_ptr> ar_tag_loc_factor_adder_; + + // Node Adders + std::shared_ptr pose_node_adder_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h new file mode 100644 index 0000000000..d079aebfb1 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_H_ +#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_H_ + +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +struct GraphLocalizerParams { + factor_adders::LocFactorAdderParams ar_tag_loc_factor_adder; + factor_adders::LocFactorAdderParams sparse_map_loc_factor_adder; + node_adders::PoseNodeAdderParams pose_node_adder; + node_adders::TimestampedNodeAdderModelParams pose_node_adder_model; + optimizers::NonlinearOptimizerParams nonlinear_optimizer; + sliding_window_graph_optimizer::SlidingWindowGraphOptimizerParams sliding_window_graph_optimizer; + // Max gap between vio measurements. If this is exceeded, graph localizer is reset. + double max_vio_measurement_gap; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(ar_tag_loc_factor_adder); + ar& BOOST_SERIALIZATION_NVP(sparse_map_loc_factor_adder); + ar& BOOST_SERIALIZATION_NVP(pose_node_adder); + ar& BOOST_SERIALIZATION_NVP(pose_node_adder_model); + ar& BOOST_SERIALIZATION_NVP(nonlinear_optimizer); + ar& BOOST_SERIALIZATION_NVP(sliding_window_graph_optimizer); + ar& BOOST_SERIALIZATION_NVP(max_vio_measurement_gap); + } +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_H_ diff --git a/localization/graph_localizer/package.xml b/localization/graph_localizer/package.xml new file mode 100644 index 0000000000..b819f20d9b --- /dev/null +++ b/localization/graph_localizer/package.xml @@ -0,0 +1,29 @@ + + graph_localizer + 1.0.0 + + The graph localizer package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + factor_adders + graph_factors + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + factor_adders + graph_factors + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + diff --git a/localization/graph_localizer/readme.md b/localization/graph_localizer/readme.md new file mode 100644 index 0000000000..be8456496e --- /dev/null +++ b/localization/graph_localizer/readme.md @@ -0,0 +1,22 @@ +\page graphlocalizer Graph Localizer + +# Package Overview +Performs sliding-window graph based optimization using a LocFactorAdder. Uses a PoseNodeAdder for creating pose nodes at required timestamps using relative odometry measurements. + +# Background +For more information on the theory behind the GraphLocalizer and the factors used, please see our paper: +* Ryan Soussan, Varsha Kumar, Brian Coltin, and Trey Smith, "Astroloc: An efficient and robust localizer for a free-flying robot", Int. Conf. on Robotics and Automation (ICRA), 2022. [Link](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9811919 "Link") + +# Graph Optimization Structure +

+ +

+ +## Factor Adders +* LocFactorAdder +## Graph Factors +* gtsam::BetweenFactor +* LocPoseFactor +* LocProjectionFactor +## Node Adders +* PoseNodeAdder diff --git a/localization/graph_localizer/src/graph_localizer.cc b/localization/graph_localizer/src/graph_localizer.cc new file mode 100644 index 0000000000..7f36a25e6e --- /dev/null +++ b/localization/graph_localizer/src/graph_localizer.cc @@ -0,0 +1,70 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +namespace graph_localizer { +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; + +GraphLocalizer::GraphLocalizer(const GraphLocalizerParams& params) + : SlidingWindowGraphOptimizer(params.sliding_window_graph_optimizer, + std::make_unique(params.nonlinear_optimizer)), + params_(params) { + // Initialize sliding window node adders + pose_node_adder_ = + std::make_shared(params_.pose_node_adder, params_.pose_node_adder_model, values()); + AddSlidingWindowNodeAdder(pose_node_adder_); + // Initialize factor adders + sparse_map_loc_factor_adder_ = + std::make_shared>(params_.sparse_map_loc_factor_adder, pose_node_adder_); + AddFactorAdder(sparse_map_loc_factor_adder_); + ar_tag_loc_factor_adder_ = + std::make_shared>(params_.ar_tag_loc_factor_adder, pose_node_adder_); + AddFactorAdder(ar_tag_loc_factor_adder_); +} + +void GraphLocalizer::AddPoseMeasurement(const lm::PoseWithCovarianceMeasurement& pose_measurement) { + pose_node_adder_->AddMeasurement(pose_measurement); +} + +void GraphLocalizer::AddSparseMapMatchedProjectionsMeasurement( + const lm::MatchedProjectionsMeasurement& matched_projections_measurement) { + if (params_.sparse_map_loc_factor_adder.enabled) + sparse_map_loc_factor_adder_->AddMeasurement(matched_projections_measurement); +} + +void GraphLocalizer::AddArTagMatchedProjectionsMeasurement( + const lm::MatchedProjectionsMeasurement& matched_projections_measurement) { + if (params_.ar_tag_loc_factor_adder.enabled) + ar_tag_loc_factor_adder_->AddMeasurement(matched_projections_measurement); +} + +const no::TimestampedNodes& GraphLocalizer::pose_nodes() const { return pose_node_adder_->nodes(); } + +void GraphLocalizer::SetPoseCovarianceInterpolater( + const std::shared_ptr>& + pose_covariance_interpolater) { + pose_node_adder_->node_adder_model().pose_interpolater().params().pose_covariance_interpolater = + pose_covariance_interpolater; +} +} // namespace graph_localizer diff --git a/localization/graph_optimizer/CMakeLists.txt b/localization/graph_optimizer/CMakeLists.txt new file mode 100644 index 0000000000..2af998da04 --- /dev/null +++ b/localization/graph_optimizer/CMakeLists.txt @@ -0,0 +1,91 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(graph_optimizer) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + factor_adders + localization_common + node_adders + optimizers +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS factor_adders localization_common node_adders optimizers +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/graph_optimizer.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_graph_optimizer + test/test_graph_optimizer.test + test/test_graph_optimizer.cc + ) + target_link_libraries(test_graph_optimizer + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/graph_optimizer/doc/images/factor_updating.png b/localization/graph_optimizer/doc/images/factor_updating.png new file mode 100644 index 0000000000..9aaf669e55 Binary files /dev/null and b/localization/graph_optimizer/doc/images/factor_updating.png differ diff --git a/localization/graph_optimizer/doc/images/graph_optimizer.png b/localization/graph_optimizer/doc/images/graph_optimizer.png new file mode 100644 index 0000000000..0cbd67125d Binary files /dev/null and b/localization/graph_optimizer/doc/images/graph_optimizer.png differ diff --git a/localization/graph_optimizer/doc/images/measurement_adding.png b/localization/graph_optimizer/doc/images/measurement_adding.png new file mode 100644 index 0000000000..3058b39fb3 Binary files /dev/null and b/localization/graph_optimizer/doc/images/measurement_adding.png differ diff --git a/localization/graph_optimizer/doc/images/update.png b/localization/graph_optimizer/doc/images/update.png new file mode 100644 index 0000000000..cdf710950f Binary files /dev/null and b/localization/graph_optimizer/doc/images/update.png differ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h new file mode 100644 index 0000000000..f29a92c346 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h @@ -0,0 +1,209 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_H_ +#define GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace graph_optimizer { +// Time-based factor graph optimizer that uses FactorAdders to generate and add factors for a given time range to the +// graph and NodeAdders to provide corresponding nodes for these factors at required timestamps. Generally FactorAdders +// are measurement-based and generate factors for a certain type of measurement at given timestamps. NodeAdders are also +// typically measurement-based and create linked nodes using relative factors that span the graph. As an example, a +// Visual-Inertial Odometry (VIO) graph may contain an image-based FactorAdder that generates visual-odometry factors +// using image measurements. A VIO graph will solve for Pose/Velocity/IMU Bias (PVB) values at different timestamps, so +// it will also contain a PVB NodeAdder that creates timestamped PVB nodes for the visual-odometry factors and links +// these nodes using IMU measurements. See the FactorAdder and NodeAdder packages for more information and different +// types of FactorAdders and NodeAdders. Maintains a set of values used for optimization. All NodeAdders added to the +// GraphOptimizer should be constructed using the GraphOptimizer's values (accessable with the values() member +// function). Acts as a base class for the SlidingWindowGraphOptimizer. +class GraphOptimizer { + public: + // Construct GraphOptimizer with provided optimizer. + GraphOptimizer(const GraphOptimizerParams& params, std::unique_ptr optimizer); + + // Default constructor for serialization only + GraphOptimizer() {} + + virtual ~GraphOptimizer() = default; + + // Adds node adder used for graph optimization. + // Initializes the nodes and priors for the node adder. + void AddNodeAdder(std::shared_ptr node_adder); + + // Adds factor adder used for graph optimization. + void AddFactorAdder(std::shared_ptr factor_adder); + + // Adds factors for each factor in valid time range to the graph. + // Since factor adders are linked to the node adder they use, this also adds required + // nodes for these factors. + // Return the number of factors added by the factor adders, not including the relative factors added by the node + // adders. + // TODO(rsoussan): Return both? + int AddFactors(const localization_common::Time start_time, const localization_common::Time end_time); + + // Wrapper for AddFactors passing a pair of timestamps. + int AddFactors(const std::pair& start_and_end_time); + + // Performs optimization on the factor graph. + bool Optimize(); + + // Calculates the covariance matrix for the provided node's key. + // Requires a successful round of optimization to have been performed. + boost::optional Covariance(const gtsam::Key& key) const; + + // Calculates the covariance matrix wrt two nodes for the provided keys. + // Requires a successful round of optimization to have been performed. + boost::optional Covariance(const gtsam::Key& key_a, const gtsam::Key& key_b) const; + + // Returns set of factors currently in the graph. + const gtsam::NonlinearFactorGraph& factors() const; + + // Returns set of factors currently in the graph. + gtsam::NonlinearFactorGraph& factors(); + + // Returns set of factors of FactorType currently in the graph. + template + std::vector> Factors() const; + + // Returns number of factors currently in the graph. + int num_factors() const; + + // Returns number of factors of provided FactorType currently in the graph. + template + int NumFactors() const; + + // Returns number of values currently in the graph. + int num_values() const; + + // Graph optimizer params. + const GraphOptimizerParams& params() const; + + // Returns a shared pointer to the values used by the graph optimizer. + // All node adders added to the graph optimizer should be constructed + // with these values. + std::shared_ptr values(); + + // Returns a const reference to the gtsam Values used within the Values object. + const gtsam::Values& gtsam_values() const; + + // Returns a reference to the stats logger + localization_common::StatsLogger& stats_logger(); + + // Returns a const reference to the optimization timer + const localization_common::Timer& optimization_timer() const; + + // Returns a const reference to the optimization_iterations averager + const localization_common::Averager& optimization_iterations_averager() const; + + // Sum of factor errors for each factor in the graph + double TotalGraphError() const; + + // Optional validity check for graph before optimizing. + // If this fails, no optimization is performed. + // Default behavior always returns true. + virtual bool ValidGraph() const; + + // Prints factor graph information and logs stats. + virtual void Print() const; + + // Saves the graph to a dot file, can be used to generate visual representation + // of the graph. + void SaveGraphDotFile(const std::string& output_path = "graph.dot") const; + + // Returns marginals if they have been calculated. + boost::optional marginals() const; + + private: + // Add averagers and timers for logging. + void AddAveragersAndTimers(); + + // Returns a reference to the gtsam Values used within the Values object. + gtsam::Values& gtsam_values(); + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(optimizer_); + ar& BOOST_SERIALIZATION_NVP(factors_); + ar& BOOST_SERIALIZATION_NVP(values_); + ar& BOOST_SERIALIZATION_NVP(factor_adders_); + ar& BOOST_SERIALIZATION_NVP(node_adders_); + ar& BOOST_SERIALIZATION_NVP(stats_logger_); + ar& BOOST_SERIALIZATION_NVP(optimization_timer_); + ar& BOOST_SERIALIZATION_NVP(optimization_timer_); + ar& BOOST_SERIALIZATION_NVP(optimization_iterations_averager_); + ar& BOOST_SERIALIZATION_NVP(total_error_averager_); + } + + GraphOptimizerParams params_; + std::unique_ptr optimizer_; + gtsam::NonlinearFactorGraph factors_; + std::shared_ptr values_; + std::vector> factor_adders_; + std::vector> node_adders_; + localization_common::StatsLogger stats_logger_; + + // Logging + localization_common::Timer optimization_timer_ = localization_common::Timer("Optimization"); + localization_common::Averager optimization_iterations_averager_ = + localization_common::Averager("Optimization Iterations"); + localization_common::Averager total_error_averager_ = localization_common::Averager("Total Factor Error"); +}; + +// Implementation +template +std::vector> GraphOptimizer::Factors() const { + std::vector> factors_vector; + for (const auto& factor : factors()) { + const auto casted_factor = boost::dynamic_pointer_cast(factor); + if (casted_factor) factors_vector.emplace_back(casted_factor); + } + return factors_vector; +} + +template +int GraphOptimizer::NumFactors() const { + int num_factors = 0; + for (const auto& factor : factors()) { + const auto casted_factor = boost::dynamic_pointer_cast(factor); + if (casted_factor) ++num_factors; + } + return num_factors; +} +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h new file mode 100644 index 0000000000..61bca72125 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h @@ -0,0 +1,32 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_PARAMS_H_ +#define GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_PARAMS_H_ + +namespace graph_optimizer { +struct GraphOptimizerParams { + // Huber k used for all factors. + double huber_k; + // Log statistics after destruction. + bool log_stats_on_destruction; + // Log factor graph info and stats after optimization. + bool print_after_optimization; +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_PARAMS_H_ diff --git a/localization/graph_optimizer/package.xml b/localization/graph_optimizer/package.xml new file mode 100644 index 0000000000..846f534031 --- /dev/null +++ b/localization/graph_optimizer/package.xml @@ -0,0 +1,25 @@ + + graph_optimizer + 1.0.0 + + The graph optimizer package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + factor_adders + localization_common + node_adders + optimizers + factor_adders + localization_common + node_adders + optimizers + diff --git a/localization/graph_optimizer/readme.md b/localization/graph_optimizer/readme.md new file mode 100644 index 0000000000..9a39039b61 --- /dev/null +++ b/localization/graph_optimizer/readme.md @@ -0,0 +1,10 @@ +\page graphoptimizer Graph Optimizer + +# Package Overview +The graph optimizer contains factor and node adders and uses these to generate factors and nodes for optimization. During an optimize call, it queries each factor adder and adds new factors and nodes to an internal factor graph before calling the respective optimizer. See the factor_adder and node_adder packages for more information on each of these classes. + +

+ +

+ + diff --git a/localization/graph_optimizer/src/graph_optimizer.cc b/localization/graph_optimizer/src/graph_optimizer.cc new file mode 100644 index 0000000000..b2ff8a1db2 --- /dev/null +++ b/localization/graph_optimizer/src/graph_optimizer.cc @@ -0,0 +1,140 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include + +namespace graph_optimizer { +namespace fa = factor_adders; +namespace lc = localization_common; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; + +GraphOptimizer::GraphOptimizer(const GraphOptimizerParams& params, std::unique_ptr optimizer) + : params_(params), + optimizer_(std::move(optimizer)), + values_(new no::Values()), + stats_logger_(params_.log_stats_on_destruction) { + AddAveragersAndTimers(); +} + +void GraphOptimizer::AddNodeAdder(std::shared_ptr node_adder) { + node_adder->AddInitialNodesAndPriors(factors_); + node_adders_.emplace_back(std::move(node_adder)); +} + +void GraphOptimizer::AddFactorAdder(std::shared_ptr factor_adder) { + factor_adders_.emplace_back(std::move(factor_adder)); +} + +int GraphOptimizer::AddFactors(const lc::Time start_time, const lc::Time end_time) { + int num_added_factors = 0; + for (const auto& factor_adder : factor_adders_) { + num_added_factors += factor_adder->AddFactors(start_time, end_time, factors_); + } + return num_added_factors; +} + +int GraphOptimizer::AddFactors(const std::pair& start_and_end_time) { + return AddFactors(start_and_end_time.first, start_and_end_time.second); +} + +bool GraphOptimizer::Optimize() { + if (!ValidGraph()) { + LogError("Optimize: Invalid graph, not optimizing."); + return false; + } + + // Optimize + LogDebug("Optimize: Optimizing."); + optimization_timer_.Start(); + const bool successful_optimization = optimizer_->Optimize(factors_, gtsam_values()); + optimization_timer_.Stop(); + + optimization_iterations_averager_.Update(optimizer_->iterations()); + total_error_averager_.Update(TotalGraphError()); + + if (params_.print_after_optimization) Print(); + return successful_optimization; +} + +boost::optional GraphOptimizer::Covariance(const gtsam::Key& key) const { + return optimizer_->Covariance(key); +} + +boost::optional GraphOptimizer::Covariance(const gtsam::Key& key_a, const gtsam::Key& key_b) const { + return optimizer_->Covariance(key_a, key_b); +} + +const gtsam::NonlinearFactorGraph& GraphOptimizer::factors() const { return factors_; } + +gtsam::NonlinearFactorGraph& GraphOptimizer::factors() { return factors_; } + +int GraphOptimizer::num_factors() const { return factors_.size(); } + +int GraphOptimizer::num_values() const { return values_->size(); } + +const GraphOptimizerParams& GraphOptimizer::params() const { return params_; } + +std::shared_ptr GraphOptimizer::values() { return values_; } + +const gtsam::Values& GraphOptimizer::gtsam_values() const { return values_->gtsam_values(); } + +gtsam::Values& GraphOptimizer::gtsam_values() { return values_->gtsam_values(); } + +lc::StatsLogger& GraphOptimizer::stats_logger() { return stats_logger_; } + +const lc::Timer& GraphOptimizer::optimization_timer() const { return optimization_timer_; } + +const localization_common::Averager& GraphOptimizer::optimization_iterations_averager() const { + return optimization_iterations_averager_; +} + +double GraphOptimizer::TotalGraphError() const { + double total_error = 0; + for (const auto& factor : factors_) { + const double error = factor->error(gtsam_values()); + total_error += error; + } + return total_error; +} + +bool GraphOptimizer::ValidGraph() const { return true; } + +void GraphOptimizer::Print() const { + factors_.print(); + stats_logger_.Log(); +} + +void GraphOptimizer::SaveGraphDotFile(const std::string& output_path) const { + std::ofstream of(output_path.c_str()); + factors_.saveGraph(of, gtsam_values()); +} + +boost::optional GraphOptimizer::marginals() const { return optimizer_->marginals(); } + +void GraphOptimizer::AddAveragersAndTimers() { + stats_logger_.AddTimer(optimization_timer_); + stats_logger_.AddAverager(optimization_iterations_averager_); + stats_logger_.AddAverager(total_error_averager_); +} +} // namespace graph_optimizer diff --git a/localization/graph_optimizer/test/test_graph_optimizer.cc b/localization/graph_optimizer/test/test_graph_optimizer.cc new file mode 100644 index 0000000000..7615523b35 --- /dev/null +++ b/localization/graph_optimizer/test/test_graph_optimizer.cc @@ -0,0 +1,191 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace fa = factor_adders; +namespace go = graph_optimizer; +namespace lc = localization_common; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; + +// Test node adder that just returns keys that should be used. +// Key values are calculated using the integer timestamps passed. +// TODO(rsoussan): Unify this with VO factor_adder test +class SimpleNodeAdder : public na::NodeAdder { + public: + explicit SimpleNodeAdder(std::shared_ptr values) : values_(values) {} + + // Add prior factor and node + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& factors) final { + AddNode(0, factors); + const auto keys = Keys(0); + const gtsam::Vector6 noise_sigmas((gtsam::Vector(6) << 0.1, 0.1, 0.1, 0.2, 0.2, 0.2).finished()); + const auto noise = + lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(noise_sigmas)), huber_k); + gtsam::PriorFactor::shared_ptr prior_factor( + new gtsam::PriorFactor(keys[0], gtsam::Pose3::identity(), noise)); + factors.push_back(prior_factor); + } + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { + values_->Add(gtsam::Pose3::identity()); + return true; + } + + bool CanAddNode(const localization_common::Time timestamp) const final { return true; } + + // Assumes integer timestamps that perfectly cast to ints. + // First key is pose key. + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { + gtsam::KeyVector keys; + // Offset by 1 since node keys start at 1 + keys.emplace_back(gtsam::Key(static_cast(timestamp + 1))); + return keys; + } + + private: + std::shared_ptr values_; + const double huber_k = 1.345; +}; + +class SimpleFactorAdder : public fa::FactorAdder { + public: + SimpleFactorAdder(const fa::FactorAdderParams& params, std::shared_ptr node_adder) + : fa::FactorAdder(params), node_adder_(node_adder) {} + int AddFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, gtsam::NonlinearFactorGraph& factors) final { + node_adder_->AddNode(oldest_allowed_time, factors); + const auto keys = node_adder_->Keys(oldest_allowed_time); + const gtsam::Vector6 noise_sigmas((gtsam::Vector(6) << 0.1, 0.1, 0.1, 0.2, 0.2, 0.2).finished()); + const auto noise = + lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(noise_sigmas)), params_.huber_k); + gtsam::PriorFactor::shared_ptr prior_factor( + new gtsam::PriorFactor(keys[0], gtsam::Pose3::identity(), noise)); + factors.push_back(prior_factor); + return 1; + } + + std::shared_ptr node_adder_; +}; + +class SimpleOptimizer : public op::Optimizer { + public: + explicit SimpleOptimizer(const op::OptimizerParams& params) : op::Optimizer(params) {} + + bool Optimize(const gtsam::NonlinearFactorGraph& factors, gtsam::Values& values) final { return true; } + + boost::optional Covariance(const gtsam::Key& key) const final { + return gtsam::Matrix(gtsam::Matrix6::Identity()); + } + + int iterations() const final { return 0; } +}; + +class GraphOptimizerTest : public ::testing::Test { + public: + GraphOptimizerTest() {} + + void SetUp() final {} + + void Initialize(const go::GraphOptimizerParams& params) { + std::unique_ptr optimizer(new SimpleOptimizer(DefaultOptimizerParams())); + graph_optimizer_.reset(new go::GraphOptimizer(params, std::move(optimizer))); + node_adder_.reset(new SimpleNodeAdder(graph_optimizer_->values())); + factor_adder_.reset(new SimpleFactorAdder(DefaultFactorAdderParams(), node_adder_)); + graph_optimizer_->AddNodeAdder(node_adder_); + graph_optimizer_->AddFactorAdder(factor_adder_); + } + + go::GraphOptimizerParams DefaultParams() { + go::GraphOptimizerParams params; + params.huber_k = 1.345; + params.log_stats_on_destruction = false; + params.print_after_optimization = false; + return params; + } + + fa::FactorAdderParams DefaultFactorAdderParams() { + fa::FactorAdderParams params; + params.enabled = true; + params.huber_k = 1.345; + return params; + } + + op::OptimizerParams DefaultOptimizerParams() { + op::OptimizerParams params; + params.marginals_factorization = "qr"; + return params; + } + + std::unique_ptr graph_optimizer_; + std::shared_ptr factor_adder_; + std::shared_ptr node_adder_; +}; + +TEST_F(GraphOptimizerTest, AddFactors) { + auto params = DefaultParams(); + Initialize(params); + // Node and factor should be added for initial node adder node and prior + EXPECT_EQ(graph_optimizer_->num_factors(), 1); + EXPECT_EQ(graph_optimizer_->num_values(), 1); + // Add first factors + EXPECT_EQ(graph_optimizer_->AddFactors(0, 1), 1); + EXPECT_EQ(graph_optimizer_->factors().size(), 2); + EXPECT_EQ(graph_optimizer_->num_factors(), 2); + EXPECT_EQ(graph_optimizer_->num_values(), 2); + EXPECT_TRUE(graph_optimizer_->Optimize()); + // Add second factors + EXPECT_EQ(graph_optimizer_->AddFactors(1, 2), 1); + EXPECT_EQ(graph_optimizer_->factors().size(), 3); + EXPECT_EQ(graph_optimizer_->num_factors(), 3); + EXPECT_EQ(graph_optimizer_->num_values(), 3); + EXPECT_TRUE(graph_optimizer_->Optimize()); +} + +TEST_F(GraphOptimizerTest, Covariance) { + auto params = DefaultParams(); + Initialize(params); + EXPECT_EQ(graph_optimizer_->num_factors(), 1); + // Add first factors + EXPECT_EQ(graph_optimizer_->AddFactors(0, 1), 1); + EXPECT_EQ(graph_optimizer_->factors().size(), 2); + EXPECT_EQ(graph_optimizer_->num_factors(), 2); + EXPECT_EQ(graph_optimizer_->num_values(), 2); + EXPECT_TRUE(graph_optimizer_->Optimize()); + const auto keys = node_adder_->Keys(0); + const auto covariance = graph_optimizer_->Covariance(keys[0]); + EXPECT_TRUE(covariance != boost::none); + EXPECT_MATRIX_NEAR(gtsam::Matrix(*covariance), gtsam::Matrix6::Identity(), 1e-6); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_optimizer/test/test_graph_optimizer.test b/localization/graph_optimizer/test/test_graph_optimizer.test new file mode 100644 index 0000000000..94268240d4 --- /dev/null +++ b/localization/graph_optimizer/test/test_graph_optimizer.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_vio/CMakeLists.txt b/localization/graph_vio/CMakeLists.txt new file mode 100644 index 0000000000..400726e509 --- /dev/null +++ b/localization/graph_vio/CMakeLists.txt @@ -0,0 +1,97 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(graph_vio) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + factor_adders + graph_factors + imu_integration + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + vision_common +) + +# Find OpenCV +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV4WithXFeatures REQUIRED) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} + CATKIN_DEPENDS + factor_adders + graph_factors + imu_integration + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + vision_common +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/graph_vio/include/graph_vio/graph_vio.h b/localization/graph_vio/include/graph_vio/graph_vio.h new file mode 100644 index 0000000000..6b06bb5466 --- /dev/null +++ b/localization/graph_vio/include/graph_vio/graph_vio.h @@ -0,0 +1,114 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_VIO_GRAPH_VIO_H_ +#define GRAPH_VIO_GRAPH_VIO_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace graph_vio { +// Siding window graph optimizer that uses IMU and optical flow feature point measurements +// with VO smart projection factors to perform VIO. +// Uses the CombinedNavStateNodeAdder to add relative IMU factors between graph nodes. +// Also adds standstill pose and velocity factors when standstill is detected +// using the history of feature point measurements. +class GraphVIO : public sliding_window_graph_optimizer::SlidingWindowGraphOptimizer { + public: + explicit GraphVIO(const GraphVIOParams& params); + + // For Serialization Only + GraphVIO() {} + + // Adds imu measurement to combined nav state node adder. + void AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); + + // Sets the fan speed mode in the combined nav state node model's IMU integrator + void SetFanSpeedMode(const localization_measurements::FanSpeedMode& fan_speed_mode); + + // Adds feature points measurement to vo smart projection factor adder. + void AddFeaturePointsMeasurement( + const localization_measurements::FeaturePointsMeasurement& feature_points_measurement); + + // Adds depth odometry measurement for depth odometry relative pose factor adder. + void AddDepthOdometryMeasurement( + const localization_measurements::DepthOdometryMeasurement& depth_odometry_measurement); + + // Returns a const reference to combined nav state nodes. + const nodes::CombinedNavStateNodes& combined_nav_state_nodes() const; + + // Returns whether standstill is detected or not. + bool standstill() const; + + // Const accesor to feature tracker used for smart factor creation + const vision_common::SpacedFeatureTracker& feature_tracker() const; + + // Construct pose covariance interpolater using latest marginals and nodes + std::shared_ptr> + MarginalsPoseCovarianceInterpolater(); + + private: + // Uses the latest feature track points to detect standstill. + bool CheckForStandstill(const localization_common::Time oldest_allowed_time); + + // bool ValidGraph() const final; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(sliding_window_graph_optimizer::SlidingWindowGraphOptimizer); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(vo_smart_projection_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(standstill_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(depth_odometry_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(combined_nav_state_node_adder_); + } + + GraphVIOParams params_; + std::shared_ptr combined_nav_state_node_adder_; + bool standstill_; + + // Factor Adders + std::shared_ptr> + depth_odometry_factor_adder_; + std::shared_ptr> + vo_smart_projection_factor_adder_; + std::shared_ptr> + standstill_factor_adder_; + + // Node Adders + // std::shared_ptr combined_nav_state_node_adder_; +}; +} // namespace graph_vio + +#endif // GRAPH_VIO_GRAPH_VIO_H_ diff --git a/localization/graph_vio/include/graph_vio/graph_vio_params.h b/localization/graph_vio/include/graph_vio/graph_vio_params.h new file mode 100644 index 0000000000..d95ae9e453 --- /dev/null +++ b/localization/graph_vio/include/graph_vio/graph_vio_params.h @@ -0,0 +1,59 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_VIO_GRAPH_VIO_PARAMS_H_ +#define GRAPH_VIO_GRAPH_VIO_PARAMS_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace graph_vio { +struct GraphVIOParams { + factor_adders::DepthOdometryFactorAdderParams depth_odometry_factor_adder; + factor_adders::StandstillFactorAdderParams standstill_factor_adder; + factor_adders::VoSmartProjectionFactorAdderParams vo_smart_projection_factor_adder; + node_adders::CombinedNavStateNodeAdder::Params combined_nav_state_node_adder; + node_adders::CombinedNavStateNodeAdderModelParams combined_nav_state_node_adder_model; + optimizers::NonlinearOptimizerParams nonlinear_optimizer; + sliding_window_graph_optimizer::SlidingWindowGraphOptimizerParams sliding_window_graph_optimizer; + vision_common::StandstillParams standstill; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(depth_odometry_factor_adder); + ar& BOOST_SERIALIZATION_NVP(standstill_factor_adder); + ar& BOOST_SERIALIZATION_NVP(vo_smart_projection_factor_adder); + ar& BOOST_SERIALIZATION_NVP(combined_nav_state_node_adder); + ar& BOOST_SERIALIZATION_NVP(nonlinear_optimizer); + ar& BOOST_SERIALIZATION_NVP(sliding_window_graph_optimizer); + ar& BOOST_SERIALIZATION_NVP(standstill); + } +}; +} // namespace graph_vio + +#endif // GRAPH_VIO_GRAPH_VIO_PARAMS_H_ diff --git a/localization/graph_vio/package.xml b/localization/graph_vio/package.xml new file mode 100644 index 0000000000..786b39650a --- /dev/null +++ b/localization/graph_vio/package.xml @@ -0,0 +1,36 @@ + + graph_vio + 1.0.0 + + The graph VIO package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + factor_adders + graph_factors + imu_integration + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + vision_common + factor_adders + graph_factors + imu_integration + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + vision_common + + + + diff --git a/localization/graph_vio/readme.md b/localization/graph_vio/readme.md new file mode 100644 index 0000000000..d525e31fb0 --- /dev/null +++ b/localization/graph_vio/readme.md @@ -0,0 +1,25 @@ +\page graphvio Graph VIO + +# Package Overview +Performs sliding-window graph based visual-intertial odometry (VIO) using a VOSmartProjectionFactorAdder and StandstillFactorAdder. Uses a CombinedNavStateNodeAdder for creating combined nav state nodes (containing a pose, velocity, and IMU bias) at required timestamps using IMU measurements. Optionally adds depth image correspondences as point-to-point between factors. + +# Background +For more information on the theory behind the GraphVIO and the factors used, please see our paper: +* Ryan Soussan, Varsha Kumar, Brian Coltin, and Trey Smith, "Astroloc: An efficient and robust localizer for a free-flying robot", Int. Conf. on Robotics and Automation (ICRA), 2022. [Link](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9811919 "Link") + +# Graph Optimization Structure +

+ +

+ +## Factor Adders +* DepthOdometryFactorAdder +* VoSmartProjectionFactorAdder +* StandstillFactorAdder +## Graph Factors +* Point BetweenFactors (for depth odometry correspondences) +* CombinedIMUFactor +* RobustSmartProjectionFactor +* Standstill Factors (zero velocity prior and identity relative transform between factors) +## Node Adders +* CombinedNavStateNodeAdder diff --git a/localization/graph_vio/src/graph_vio.cc b/localization/graph_vio/src/graph_vio.cc new file mode 100644 index 0000000000..6fdf359d02 --- /dev/null +++ b/localization/graph_vio/src/graph_vio.cc @@ -0,0 +1,97 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +namespace graph_vio { +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; +namespace vc = vision_common; + +GraphVIO::GraphVIO(const GraphVIOParams& params) + : SlidingWindowGraphOptimizer(params.sliding_window_graph_optimizer, + std::make_unique(params.nonlinear_optimizer)), + params_(params), + standstill_(false) { + // Initialize sliding window node adders + combined_nav_state_node_adder_ = std::make_shared( + params_.combined_nav_state_node_adder, params_.combined_nav_state_node_adder_model, values()); + AddSlidingWindowNodeAdder(combined_nav_state_node_adder_); + // Initialize factor adders + vo_smart_projection_factor_adder_ = std::make_shared>( + params_.vo_smart_projection_factor_adder, combined_nav_state_node_adder_); + AddFactorAdder(vo_smart_projection_factor_adder_); + standstill_factor_adder_ = std::make_shared>( + params_.standstill_factor_adder, combined_nav_state_node_adder_); + AddFactorAdder(standstill_factor_adder_); + depth_odometry_factor_adder_ = std::make_shared>( + params_.depth_odometry_factor_adder, combined_nav_state_node_adder_); + AddFactorAdder(depth_odometry_factor_adder_); +} + +void GraphVIO::AddImuMeasurement(const lm::ImuMeasurement& imu_measurement) { + combined_nav_state_node_adder_->AddMeasurement(imu_measurement); +} + +void GraphVIO::SetFanSpeedMode(const localization_measurements::FanSpeedMode& fan_speed_mode) { + combined_nav_state_node_adder_->node_adder_model().SetFanSpeedMode(fan_speed_mode); +} + +void GraphVIO::AddFeaturePointsMeasurement(const lm::FeaturePointsMeasurement& feature_points_measurement) { + if (params_.vo_smart_projection_factor_adder.enabled) + vo_smart_projection_factor_adder_->AddMeasurement(feature_points_measurement); + + // Check for standstill and optionally add standstill measurement if detected. + const auto& feature_tracks = vo_smart_projection_factor_adder_->feature_tracker().feature_tracks(); + standstill_ = vc::Standstill(feature_tracks, params_.standstill); + if (standstill_ && params_.standstill_factor_adder.enabled) { + const lc::Time latest_timestamp = feature_points_measurement.timestamp; + const auto previous_timestamp = feature_tracks.cbegin()->second.SecondLatestTimestamp(); + standstill_factor_adder_->AddMeasurement(lm::StandstillMeasurement(latest_timestamp, *previous_timestamp)); + } +} + +void GraphVIO::AddDepthOdometryMeasurement( + const localization_measurements::DepthOdometryMeasurement& depth_odometry_measurement) { + // Don't add depth odom measurements when at standstill to avoid + // introducing pose and velocity noise + if (params_.depth_odometry_factor_adder.enabled && !standstill_) + depth_odometry_factor_adder_->AddMeasurement(depth_odometry_measurement); +} + +const no::CombinedNavStateNodes& GraphVIO::combined_nav_state_nodes() const { + return combined_nav_state_node_adder_->nodes(); +} + +bool GraphVIO::standstill() const { return standstill_; } + +const vc::SpacedFeatureTracker& GraphVIO::feature_tracker() const { + return vo_smart_projection_factor_adder_->feature_tracker(); +} + +std::shared_ptr> +GraphVIO::MarginalsPoseCovarianceInterpolater() { + return std::make_shared>( + combined_nav_state_node_adder_->nodes_ptr(), marginals()); +} +} // namespace graph_vio diff --git a/localization/ground_truth_localizer/CMakeLists.txt b/localization/ground_truth_localizer/CMakeLists.txt new file mode 100644 index 0000000000..74cdf06197 --- /dev/null +++ b/localization/ground_truth_localizer/CMakeLists.txt @@ -0,0 +1,88 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(ground_truth_localizer) + +if (USE_ROS) + + ## Compile as C++14, supported in ROS Kinetic and newer + add_compile_options(-std=c++14) + + ## Find catkin macros and libraries + find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + ff_msgs + ff_util + localization_common + ) + + # System dependencies are found with CMake's conventions + find_package(Eigen3 REQUIRED) + + # include ff_nodelet to get ff_util header files since these aren't exposed elsewhere + catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS roscpp ff_msgs ff_util localization_common nodelet + ) + + ########### + ## Build ## + ########### + # Specify additional locations of header files + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ) + + # Declare C++ libraries + add_library(${PROJECT_NAME} + src/ground_truth_localizer_nodelet.cc + src/utilities.cc + ) + add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_LIBRARIES}) + + + ############# + ## Install ## + ############# + + # Mark libraries for installation + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + + # Mark launch files for installation + install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + +else (USE_ROS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_ROS) diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h new file mode 100644 index 0000000000..7b82a7a8c0 --- /dev/null +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h @@ -0,0 +1,75 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ +#define GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace ground_truth_localizer { +class GroundTruthLocalizerNodelet : public ff_util::FreeFlyerNodelet { + public: + GroundTruthLocalizerNodelet(); + + private: + void Initialize(ros::NodeHandle* nh); + + void SubscribeAndAdvertise(ros::NodeHandle* nh); + + bool SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res); + + bool DefaultServiceResponse(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + + void PoseCallback(geometry_msgs::PoseStamped::ConstPtr const& pose); + + void TwistCallback(geometry_msgs::TwistStamped::ConstPtr const& twist); + + void PublishLocState(const localization_common::Time& timestamp); + + std::string platform_name_; + ros::Time last_time_; + boost::optional pose_; + boost::optional twist_; + int input_mode_ = ff_msgs::SetEkfInputRequest::MODE_TRUTH; + ros::Subscriber pose_sub_, twist_sub_; + ros::Publisher state_pub_, pose_pub_, twist_pub_, heartbeat_pub_, reset_pub_; + ff_msgs::Heartbeat heartbeat_; + tf2_ros::TransformBroadcaster transform_pub_; + ros::ServiceServer input_mode_srv_, bias_srv_, bias_from_file_srv_, reset_srv_; +}; +} // namespace ground_truth_localizer + +#endif // GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/twist.h b/localization/ground_truth_localizer/include/ground_truth_localizer/twist.h new file mode 100644 index 0000000000..91f06b29af --- /dev/null +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/twist.h @@ -0,0 +1,30 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef GROUND_TRUTH_LOCALIZER_TWIST_H_ +#define GROUND_TRUTH_LOCALIZER_TWIST_H_ + +#include + +namespace ground_truth_localizer { +struct Twist { + Eigen::Vector3d linear_velocity; + Eigen::Vector3d angular_velocity; +}; +} // namespace ground_truth_localizer + +#endif // GROUND_TRUTH_LOCALIZER_TWIST_H_ diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/utilities.h b/localization/ground_truth_localizer/include/ground_truth_localizer/utilities.h new file mode 100644 index 0000000000..2e5dd75558 --- /dev/null +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/utilities.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef GROUND_TRUTH_LOCALIZER_UTILITIES_H_ +#define GROUND_TRUTH_LOCALIZER_UTILITIES_H_ + +#include +#include +#include + +#include +#include +#include + +#include + +namespace ground_truth_localizer { +Eigen::Isometry3d PoseFromMsg(const geometry_msgs::PoseStamped& pose_msg); +Twist TwistFromMsg(const geometry_msgs::TwistStamped& twist_msg); +ff_msgs::EkfState LocStateMsg(const Eigen::Isometry3d& pose, const Twist& twist, const localization_common::Time& time); +} // namespace ground_truth_localizer + +#endif // GROUND_TRUTH_LOCALIZER_UTILITIES_H_ diff --git a/localization/ground_truth_localizer/launch/ground_truth_localizer.launch b/localization/ground_truth_localizer/launch/ground_truth_localizer.launch new file mode 100644 index 0000000000..469707ae7f --- /dev/null +++ b/localization/ground_truth_localizer/launch/ground_truth_localizer.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/ground_truth_localizer/nodelet_plugins.xml b/localization/ground_truth_localizer/nodelet_plugins.xml new file mode 100644 index 0000000000..c43d1234dc --- /dev/null +++ b/localization/ground_truth_localizer/nodelet_plugins.xml @@ -0,0 +1,11 @@ + + + + This nodelet wraps ground truth localizer + + + + + diff --git a/localization/ground_truth_localizer/package.xml b/localization/ground_truth_localizer/package.xml new file mode 100644 index 0000000000..7d9c801f35 --- /dev/null +++ b/localization/ground_truth_localizer/package.xml @@ -0,0 +1,30 @@ + + ground_truth_localizer + 1.0.0 + + The ground_truth localizer package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + roscpp + nodelet + ff_msgs + ff_util + localization_common + roscpp + nodelet + ff_msgs + ff_util + localization_common + + + + diff --git a/localization/ground_truth_localizer/readme.md b/localization/ground_truth_localizer/readme.md new file mode 100644 index 0000000000..c57a676be9 --- /dev/null +++ b/localization/ground_truth_localizer/readme.md @@ -0,0 +1,5 @@ +\page groundtruthlocalizer Ground Truth Localizer + +# Package overview +Echos ground truth poses and twists. + diff --git a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc new file mode 100644 index 0000000000..38e399878c --- /dev/null +++ b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc @@ -0,0 +1,109 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include + +#include + +#include + +namespace ground_truth_localizer { +namespace lc = localization_common; +GroundTruthLocalizerNodelet::GroundTruthLocalizerNodelet() : ff_util::FreeFlyerNodelet(NODE_SIM_LOC, true) {} + +void GroundTruthLocalizerNodelet::Initialize(ros::NodeHandle* nh) { + platform_name_ = GetPlatform(); + platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); + ff_common::InitFreeFlyerApplication(getMyArgv()); + SubscribeAndAdvertise(nh); +} + +void GroundTruthLocalizerNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { + pose_pub_ = nh->advertise(TOPIC_LOCALIZATION_POSE, 1); + twist_pub_ = nh->advertise(TOPIC_LOCALIZATION_TWIST, 1); + state_pub_ = nh->advertise(TOPIC_GNC_EKF, 1); + heartbeat_pub_ = nh->advertise(TOPIC_HEARTBEAT, 5, true); + reset_pub_ = nh->advertise(TOPIC_GNC_EKF_RESET, 10); + + pose_sub_ = nh->subscribe(TOPIC_LOCALIZATION_TRUTH, 1, &GroundTruthLocalizerNodelet::PoseCallback, this, + ros::TransportHints().tcpNoDelay()); + twist_sub_ = nh->subscribe(TOPIC_LOCALIZATION_TRUTH_TWIST, 1, &GroundTruthLocalizerNodelet::TwistCallback, this, + ros::TransportHints().tcpNoDelay()); + + input_mode_srv_ = nh->advertiseService(SERVICE_GNC_EKF_SET_INPUT, &GroundTruthLocalizerNodelet::SetMode, this); + bias_srv_ = + nh->advertiseService(SERVICE_GNC_EKF_INIT_BIAS, &GroundTruthLocalizerNodelet::DefaultServiceResponse, this); + bias_from_file_srv_ = nh->advertiseService(SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE, + &GroundTruthLocalizerNodelet::DefaultServiceResponse, this); + reset_srv_ = nh->advertiseService(SERVICE_GNC_EKF_RESET, &GroundTruthLocalizerNodelet::DefaultServiceResponse, this); +} + +bool GroundTruthLocalizerNodelet::SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res) { + input_mode_ = req.mode; + return true; +} + +bool GroundTruthLocalizerNodelet::DefaultServiceResponse(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res) { + return true; +} + +void GroundTruthLocalizerNodelet::PoseCallback(geometry_msgs::PoseStamped::ConstPtr const& pose) { + assert(pose->header.frame_id == "world"); + pose_ = PoseFromMsg(*pose); + pose_pub_.publish(pose); + const lc::Time timestamp = lc::TimeFromHeader(pose->header); + PublishLocState(timestamp); + heartbeat_.header.stamp = ros::Time::now(); + // Publish heartbeat for graph localizer and pose extrapolator since flight software expects this + // and this runs in place of them + heartbeat_.node = NODE_GRAPH_LOC; + heartbeat_pub_.publish(heartbeat_); + heartbeat_.node = NODE_POSE_EXTR; + heartbeat_pub_.publish(heartbeat_); +} + +void GroundTruthLocalizerNodelet::TwistCallback(geometry_msgs::TwistStamped::ConstPtr const& twist) { + assert(twist->header.frame_id == "world"); + twist_ = TwistFromMsg(*twist); + twist_pub_.publish(twist); + const lc::Time timestamp = lc::TimeFromHeader(twist->header); + PublishLocState(timestamp); +} + +void GroundTruthLocalizerNodelet::PublishLocState(const lc::Time& timestamp) { + if (!twist_ || !pose_) return; + const auto loc_state_msg = LocStateMsg(*pose_, *twist_, timestamp); + state_pub_.publish(loc_state_msg); + + // Also publish world_T_body TF + const auto world_T_body_tf = lc::PoseToTF(*pose_, "world", "body", timestamp, platform_name_); + + // If the rate is higher than the sim time, prevent repeated timestamps + if (world_T_body_tf.header.stamp == last_time_) return; + last_time_ = world_T_body_tf.header.stamp; + + transform_pub_.sendTransform(world_T_body_tf); +} +} // namespace ground_truth_localizer + +PLUGINLIB_EXPORT_CLASS(ground_truth_localizer::GroundTruthLocalizerNodelet, nodelet::Nodelet); diff --git a/localization/ground_truth_localizer/src/utilities.cc b/localization/ground_truth_localizer/src/utilities.cc new file mode 100644 index 0000000000..a58e5703f3 --- /dev/null +++ b/localization/ground_truth_localizer/src/utilities.cc @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +namespace ground_truth_localizer { +namespace lc = localization_common; +namespace mc = msg_conversions; + +Eigen::Isometry3d PoseFromMsg(const geometry_msgs::PoseStamped& pose_msg) { + Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); + pose.translation() = mc::VectorFromMsg(pose_msg.pose.position); + pose.linear() = + mc::RotationFromMsg(pose_msg.pose.orientation).toRotationMatrix(); + return pose; +} + +Twist TwistFromMsg(const geometry_msgs::TwistStamped& twist_msg) { + Twist twist; + twist.linear_velocity = mc::VectorFromMsg(twist_msg.twist.linear); + twist.angular_velocity = mc::VectorFromMsg(twist_msg.twist.angular); + return twist; +} + +ff_msgs::EkfState LocStateMsg(const Eigen::Isometry3d& pose, const Twist& twist, + const localization_common::Time& timestamp) { + ff_msgs::EkfState loc_msg; + mc::EigenPoseToMsg(pose, loc_msg.pose); + mc::VectorToMsg(twist.linear_velocity, loc_msg.velocity); + mc::VectorToMsg(twist.angular_velocity, loc_msg.omega); + lc::TimeToHeader(timestamp, loc_msg.header); + loc_msg.confidence = lc::Confidence::kGood; + return loc_msg; +} +} // namespace ground_truth_localizer diff --git a/localization/handrail_detect/CMakeLists.txt b/localization/handrail_detect/CMakeLists.txt index 9887b0ecf1..f393a39ac0 100644 --- a/localization/handrail_detect/CMakeLists.txt +++ b/localization/handrail_detect/CMakeLists.txt @@ -15,30 +15,66 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(handrail_detect) -if (USE_ROS) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + tf2 + nodelet + visualization_msgs + ff_msgs + ff_util +) + +# Find OpenCV +find_package(OpenCV 4.0 REQUIRED) catkin_package( LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS tf2 visualization_msgs roscpp ff_msgs nodelet - DEPENDS ${PROJECT_NAME} + CATKIN_DEPENDS roscpp tf2 nodelet visualization_msgs ff_msgs ff_util ) -set(CMAKE_CXX_FLAGS_HACKY "${CMAKE_CXX_FLAGS} -Wno-return-type") +########### +## Build ## +########### -create_library(TARGET ${PROJECT_NAME} - LIBS ${catkin_LIBRARIES} ${OROCOS_KDL_LIBRARIES} ff_common config_reader msg_conversions ff_nodelet - INC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - DEPS ff_msgs +# Specify additional locations of header files +include_directories( + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) -set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${CMAKE_CXX_FLAGS_HACKY}) +# Declare C++ libraries +add_library(handrail_detect + src/handrail_detect.cc +) +add_dependencies(handrail_detect ${catkin_EXPORTED_TARGETS}) +target_link_libraries(handrail_detect ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) -create_tool_targets(DIR tools - LIBS ${PROJECT_NAME} +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) -endif (USE_ROS) +install(DIRECTORY config/ DESTINATION config) diff --git a/localization/handrail_detect/package.xml b/localization/handrail_detect/package.xml index 7ca3c4bf22..a4bb30a8ae 100644 --- a/localization/handrail_detect/package.xml +++ b/localization/handrail_detect/package.xml @@ -17,7 +17,6 @@ catkin roscpp nodelet - pluginlib tf2 tf2_ros tf2_geometry_msgs @@ -29,7 +28,6 @@ ff_util roscpp nodelet - pluginlib tf2 tf2_ros tf2_geometry_msgs diff --git a/localization/handrail_detect/ros/enable_handrail_detect.cc b/localization/handrail_detect/ros/enable_handrail_detect.cc index 0a4aac7f8f..dc8cbc49a9 100644 --- a/localization/handrail_detect/ros/enable_handrail_detect.cc +++ b/localization/handrail_detect/ros/enable_handrail_detect.cc @@ -18,7 +18,7 @@ #include #include -#include +#include // Simple executable wrapper int main(int argc, char** argv) { diff --git a/localization/handrail_detect/src/handrail_detect.cc b/localization/handrail_detect/src/handrail_detect.cc index 869486f280..b2942d3731 100644 --- a/localization/handrail_detect/src/handrail_detect.cc +++ b/localization/handrail_detect/src/handrail_detect.cc @@ -52,7 +52,7 @@ #include #include -// Eigen incliudes +// Eigen includes #include #include #include @@ -183,6 +183,12 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { if (!config_.GetReal("max_depth_dist", &max_depth_dist_)) ROS_FATAL("Unspecified max_depth_dist."); + if (!config_.GetInt("max_num_line_points", &max_num_line_points_)) + ROS_FATAL("Unspecified max_num_line_points."); + + if (!config_.GetInt("max_num_plane_points", &max_num_plane_points_)) + ROS_FATAL("Unspecified max_num_plane_points."); + // Maximum RANSAC iteration for line estimation if (!config_.GetInt("RANSAC_line_iteration", &RANSAC_line_iteration_)) ROS_FATAL("Unspecified RANSAC_line_iteration."); @@ -284,6 +290,13 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { HandrailCallback(); } + void Clear() { + dl_.landmarks.clear(); + dl_.sensor_t_line_points.clear(); + dl_.sensor_t_line_endpoints.clear(); + dl_.sensor_t_plane_points.clear(); + } + // Performs handrail detection void HandrailCallback() { // Publish registration @@ -294,7 +307,7 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { r.camera_id = frame_count_; registration_pub_.publish(r); ros::spinOnce(); - dl_.landmarks.clear(); + Clear(); dl_.end_seen = 0; // Publish depth image @@ -428,11 +441,11 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { rolling_window_pos_(0, rolling_window_cnt_) = rolling_window_pos_(0, previous_index); rolling_window_pos_(1, rolling_window_cnt_) = rolling_window_pos_(1, previous_index); rolling_window_pos_(2, rolling_window_cnt_) = rolling_window_pos_(2, previous_index); - dl_.local_pose.position.x = rolling_window_pos_(0, previous_index); - dl_.local_pose.position.y = rolling_window_pos_(1, previous_index); - dl_.local_pose.position.z = rolling_window_pos_(2, previous_index); + dl_.sensor_T_handrail.position.x = rolling_window_pos_(0, previous_index); + dl_.sensor_T_handrail.position.y = rolling_window_pos_(1, previous_index); + dl_.sensor_T_handrail.position.z = rolling_window_pos_(2, previous_index); // Don't publish landmark if it refuses point - dl_.landmarks.clear(); + Clear(); // Since we copy the previous point in the hope of rejecting 1 or two wrong samples // We have to stop if we copy too many points in a row. This would imply we are lost @@ -441,7 +454,7 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { // Restart handrail detect if we are lost start_ = false; // Don't publish landmark if we are lost - dl_.landmarks.clear(); + Clear(); return false; } } else { @@ -452,9 +465,9 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { rolling_window_pos_(0, rolling_window_cnt_) = i2h.translation()(0); rolling_window_pos_(1, rolling_window_cnt_) = i2h.translation()(1); rolling_window_pos_(2, rolling_window_cnt_) = i2h.translation()(2); - dl_.local_pose.position.x = i2h.translation()(0); - dl_.local_pose.position.y = i2h.translation()(1); - dl_.local_pose.position.z = i2h.translation()(2); + dl_.sensor_T_handrail.position.x = i2h.translation()(0); + dl_.sensor_T_handrail.position.y = i2h.translation()(1); + dl_.sensor_T_handrail.position.z = i2h.translation()(2); } } else { // If we don't have 5 positions saved, the outlier rejection averaging filter @@ -462,14 +475,19 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { rolling_window_pos_(0, rolling_window_cnt_) = i2h.translation()(0); rolling_window_pos_(1, rolling_window_cnt_) = i2h.translation()(1); rolling_window_pos_(2, rolling_window_cnt_) = i2h.translation()(2); - dl_.local_pose.position.x = i2h.translation()(0); - dl_.local_pose.position.y = i2h.translation()(1); - dl_.local_pose.position.z = i2h.translation()(2); + dl_.sensor_T_handrail.position.x = i2h.translation()(0); + dl_.sensor_T_handrail.position.y = i2h.translation()(1); + dl_.sensor_T_handrail.position.z = i2h.translation()(2); + } + dl_.sensor_T_handrail.orientation.x = rtoq.x(); + dl_.sensor_T_handrail.orientation.y = rtoq.y(); + dl_.sensor_T_handrail.orientation.z = rtoq.z(); + dl_.sensor_T_handrail.orientation.w = rtoq.w(); + + // Add endpoints + for (const auto& line_endpoint : end_point) { + dl_.sensor_t_line_endpoints.push_back(line_endpoint); } - dl_.local_pose.orientation.x = rtoq.x(); - dl_.local_pose.orientation.y = rtoq.y(); - dl_.local_pose.orientation.z = rtoq.z(); - dl_.local_pose.orientation.w = rtoq.w(); rolling_window_cnt_++; @@ -1538,18 +1556,19 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { void GetFeaturePoints(const std::vector& plane_inliers, const std::vector& line_inliers) { - int num_line_features = 4; - int num_plane_features = 6; - int tot_features = num_line_features + num_plane_features; + int tot_features = max_num_line_points_ + max_num_plane_points_; std::vector feature_idx; feature_idx.reserve(tot_features); + std::vector line_point_indices; + std::vector plane_point_indices; // Get feature points for each step from handrail - int line_step = line_inliers.size() / num_line_features; + int line_step = line_inliers.size() / max_num_line_points_; int steps = 0; - for (int i = 0; i < num_line_features && steps < static_cast(line_inliers.size()); ++i) { + for (int i = 0; i < max_num_line_points_ && steps < static_cast(line_inliers.size()); ++i) { feature_idx.push_back(line_inliers[steps]); + line_point_indices.push_back(line_inliers[steps]); steps += line_step; } @@ -1560,12 +1579,12 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { float fov_y_half_tan = static_cast(tan(fov_y * 0.5 * (M_PI / 180.0))); float half_tan = std::min(fov_x_half_tan, fov_y_half_tan); float feature_dist_thres = (half_tan * cloud_.points[plane_inliers[plane_inliers.size() / 2]].z) - / sqrt(static_cast((tot_features - num_line_features))); + / sqrt(static_cast((tot_features - max_num_line_points_))); Eigen::Vector2f rp, tp; int rv, escape_cnt = 0, escape_cnt2 = 0; - for (int i = num_line_features; i < tot_features; ++i) { + for (int i = max_num_line_points_; i < tot_features; ++i) { bool g2g = false; escape_cnt2 = 0; @@ -1586,6 +1605,7 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { } if (g2g) { feature_idx.push_back(plane_inliers[rv]); + plane_point_indices.push_back(plane_inliers[rv]); break; } else if (escape_cnt2 > static_cast(feature_idx.size())) { ++escape_cnt; @@ -1610,6 +1630,13 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { dl_.landmarks[lm_cnt].w = cloud_.points[itr].z; ++lm_cnt; } + + for (const auto line_point_index : line_point_indices) { + dl_.sensor_t_line_points.push_back(cloud_.points[line_point_index]); + } + for (const auto plane_point_index : plane_point_indices) { + dl_.sensor_t_plane_points.push_back(cloud_.points[plane_point_index]); + } } bool GetXYScale(const std::vector& plane_inliers) { @@ -1869,6 +1896,8 @@ class HandrailDetect : public ff_util::FreeFlyerNodelet { float min_measure_gap_; float min_depth_dist_; float max_depth_dist_; + int max_num_line_points_; + int max_num_plane_points_; // RANSAC related parameters int RANSAC_line_iteration_; diff --git a/localization/imu_integration/CMakeLists.txt b/localization/imu_integration/CMakeLists.txt new file mode 100644 index 0000000000..88d2de82c9 --- /dev/null +++ b/localization/imu_integration/CMakeLists.txt @@ -0,0 +1,92 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(imu_integration) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + config_reader + localization_common + localization_measurements +) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} + INCLUDE_DIRS include ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS config_reader localization_common localization_measurements +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/dynamic_imu_filter.cc + src/identity_filter.cc + src/imu_filter.cc + src/imu_integrator.cc + src/parameter_reader.cc + src/test_utilities.cc + src/utilities.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_imu_integrator + test/test_imu_integrator.test + test/test_imu_integrator.cc + ) + target_link_libraries(test_imu_integrator + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/imu_integration/include/imu_integration/butterO1.h b/localization/imu_integration/include/imu_integration/butterO1.h new file mode 100644 index 0000000000..e5c9163119 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterO1.h @@ -0,0 +1,119 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_BUTTERO1_H_ +#define IMU_INTEGRATION_BUTTERO1_H_ + +#include + +#include + +namespace imu_integration { +template +class ButterO1 : public Filter { + public: + ButterO1() : initialized_(false) {} + // Returns filtered value and timestamp + double AddValue(const double value) final { + if (!initialized_) Initialize(value, Params::kGain); + + // Shift input vals + xv_[0] = xv_[1]; + xv_[1] = xv_[2]; + xv_[2] = xv_[3]; + // Add new value + xv_[3] = value / Params::kGain; + // Shift output vals + yv_[0] = yv_[1]; + yv_[1] = yv_[2]; + yv_[2] = yv_[3]; + // Generate new output + yv_[3] = (xv_[0] + xv_[3]) + Params::kX12 * (xv_[1] + xv_[2]) + (Params::kY2 * yv_[2]); + // Return most recent output + return yv_[3]; + } + + private: + void Initialize(const double first_value, const double gain) { + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; + } + + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; + +// 62.5 Hz +struct ParamsButterO1S62_5Lp3N29_16 { + static constexpr double kGain = 2.999100930e+01; + static constexpr double kX12 = 2.9560160746; + static constexpr double kY2 = 0.7361865327; +}; +using ButterO1S62_5Lp3N29_16 = ButterO1; + +struct ParamsButterO1S62_5Lp3N20_83 { + static constexpr double kGain = 2.2743339590e+01; + static constexpr double kX12 = 1.9999996372; + static constexpr double kY2 = 0.7361865327; +}; +using ButterO1S62_5Lp3N20_83 = ButterO1; + +struct ParamsButterO1S62_5Lp3N15_83 { + static constexpr double kGain = 1.5479761468e+01; + static constexpr double kX12 = 1.0418847728; + static constexpr double kY2 = 0.7361865327; +}; +using ButterO1S62_5Lp3N15_83 = ButterO1; + +// 125Hz +// 2500 rpm: 41.666Hz +struct ParamsButterO1S125Lp3N41_66 { + static constexpr double kGain = 4.270814376e+01; + static constexpr double kX12 = 1.9996372182; + static constexpr double kY2 = 0.8595285604; +}; +using ButterO1S125Lp3N41_66 = ButterO1; + +// 2000 rpm: 33.333Hz +struct ParamsButterO1S125Lp3N33_33 { + static constexpr double kGain = 3.144018095e+01; + static constexpr double kX12 = 1.2082237395; + static constexpr double kY2 = 0.8595285604; +}; +using ButterO1S125Lp3N33_33 = ButterO1; + +// 2800 rpm: 46.666Hz +struct ParamsButterO1S125Lp3N46_66 { + static constexpr double kGain = 4.839840415e+01; + static constexpr double kX12 = 2.3992967530; + static constexpr double kY2 = 0.8595285604; +}; +using ButterO1S125Lp3N46_66 = ButterO1; + +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERO1_H_ diff --git a/localization/imu_integration/include/imu_integration/butterO10.h b/localization/imu_integration/include/imu_integration/butterO10.h new file mode 100644 index 0000000000..1b04d0e24b --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterO10.h @@ -0,0 +1,98 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_BUTTERO10_H_ +#define IMU_INTEGRATION_BUTTERO10_H_ + +#include + +#include + +#include + +namespace imu_integration { +template +class ButterO10 : public Filter { + public: + ButterO10() : initialized_(false) {} + // Returns filtered value and timestamp + double AddValue(const double value) final { + if (!initialized_) Initialize(value, Params::kGain); + + const int last_index = xv_.size() - 1; + // Shift input and output vals + for (int i = 0; i < last_index; ++i) { + xv_[i] = xv_[i + 1]; + yv_[i] = yv_[i + 1]; + } + // Add new values + xv_[last_index] = value / Params::kGain; + // Generate new output + yv_[last_index] = (xv_[0] + xv_[12]) + Params::kX111 * (xv_[1] + xv_[11]) + Params::kX210 * (xv_[2] + xv_[10]) + + Params::kX39 * (xv_[3] + xv_[9]) + Params::kX48 * (xv_[4] + xv_[8]) + + Params::kX57 * (xv_[5] + xv_[7]) + Params::kX6 * xv_[6] + (Params::kY2 * yv_[2]) + + (Params::kY3 * yv_[3]) + (Params::kY4 * yv_[4]) + (Params::kY5 * yv_[5]) + + (Params::kY6 * yv_[6]) + (Params::kY7 * yv_[7]) + (Params::kY8 * yv_[8]) + + (Params::kY9 * yv_[9]) + (Params::kY10 * yv_[10]) + (Params::kY11 * yv_[11]); + + // Return most recent output + return yv_[last_index]; + } + + private: + void Initialize(const double first_value, const double gain) { + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; + } + + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; + +struct ParamsButterO10S62_5Lp3N20_83 { + static constexpr double kGain = 1.204389877e+09; + static constexpr double kX111 = 10.9994195280; + static constexpr double kX210 = 55.9941952810; + static constexpr double kX39 = 174.9738787600; + static constexpr double kX48 = 374.930343370; + static constexpr double kX57 = 581.8781008900; + static constexpr double kX6 = 671.8537210700; + static constexpr double kY2 = -0.1440551095; + static constexpr double kY3 = 1.7182080129; + static constexpr double kY4 = -9.2532994947; + static constexpr double kY5 = 29.6350557040; + static constexpr double kY6 = -62.5163645520; + static constexpr double kY7 = 90.7861740400; + static constexpr double kY8 = -91.9332301310; + static constexpr double kY9 = 64.1155101770; + static constexpr double kY10 = -29.4805657920; + static constexpr double kY11 = 8.0725645969; +}; +using ButterO10S62_5Lp3N20_83 = ButterO10; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERO10_H_ diff --git a/localization/imu_integration/include/imu_integration/butterO3.h b/localization/imu_integration/include/imu_integration/butterO3.h new file mode 100644 index 0000000000..91beb11b1b --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterO3.h @@ -0,0 +1,134 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_BUTTERO3_H_ +#define IMU_INTEGRATION_BUTTERO3_H_ + +#include + +#include + +#include + +namespace imu_integration { +template +class ButterO3 : public Filter { + public: + ButterO3() : initialized_(false) {} + // Returns filtered value and timestamp + double AddValue(const double value) final { + if (!initialized_) Initialize(value, Params::kGain); + + const int last_index = xv_.size() - 1; + // Shift input and output vals + for (int i = 0; i < last_index; ++i) { + xv_[i] = xv_[i + 1]; + yv_[i] = yv_[i + 1]; + } + // Add new values + xv_[last_index] = value / Params::kGain; + // Generate new output + yv_[last_index] = (xv_[0] + xv_[5]) + Params::kX14 * (xv_[1] + xv_[4]) + Params::kX23 * (xv_[2] + xv_[3]) + + (Params::kY2 * yv_[2]) + (Params::kY3 * yv_[3]) + (Params::kY4 * yv_[4]); + // Return most recent output + return yv_[last_index]; + } + + private: + void Initialize(const double first_value, const double gain) { + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; + } + + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; + +struct ParamsButterO3S62_5Lp3N29_16 { + static constexpr double kGain = 1.526307756e+03; + static constexpr double kX14 = 4.9560160746; + static constexpr double kX23 = 9.8680482239; + static constexpr double kY2 = 0.5457868345; + static constexpr double kY3 = -1.9654812209; + static constexpr double kY4 = 2.3989592965; +}; +using ButterO3S62_5Lp3N29_16 = ButterO3; + +struct ParamsButterO3S62_5Lp3N20_83 { + static constexpr double kGain = 1.1572342485e+03; + static constexpr double kX14 = 3.9994195281; + static constexpr double kX23 = 6.9982585842; + static constexpr double kY2 = 5.4578683446e-01; + static constexpr double kY3 = -1.9654812209e+00; + static constexpr double kY4 = 2.3989592965e+00; +}; +using ButterO3S62_5Lp3N20_83 = ButterO3; + +struct ParamsButterO3S62_5Lp3N15_83 { + static constexpr double kGain = 7.8754026491e+02; + static constexpr double kX14 = 3.0412147780e+00; + static constexpr double kX23 = 4.1236443339e+00; + static constexpr double kY2 = 5.4578683446e-01; + static constexpr double kY3 = -1.9654812209e+00; + static constexpr double kY4 = 2.3989592965e+00; +}; +using ButterO3S62_5Lp3N15_83 = ButterO3; + +// 125Hz +struct ParamsButterO3S125Lp3N41_66 { + static constexpr double kGain = 8.092181701e+03; + static constexpr double kX14 = 3.9996372182; + static constexpr double kX23 = 6.9989116546; + static constexpr double kY2 = 0.7394265720; + static constexpr double kY3 = -2.4410797085; + static constexpr double kY4 = 2.6986876695; +}; +using ButterO3S125Lp3N41_66 = ButterO3; + +struct ParamsButterO3S125Lp3N46_66 { + static constexpr double kGain = 9.170351272e+03; + static constexpr double kX14 = 4.3992967530; + static constexpr double kX23 = 8.1978902589; + static constexpr double kY2 = 0.7394265720; + static constexpr double kY3 = -2.4410797085; + static constexpr double kY4 = 2.6986876695; +}; +using ButterO3S125Lp3N46_66 = ButterO3; + +struct ParamsButterO3S125Lp3N33_33 { + static constexpr double kGain = 5.957169629e+03; + static constexpr double kX14 = 3.2082237395; + static constexpr double kX23 = 4.6246712185; + static constexpr double kY2 = 0.7394265720; + static constexpr double kY3 = -2.4410797085; + static constexpr double kY4 = 2.6986876695; +}; +using ButterO3S125Lp3N33_33 = ButterO3; + +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERO3_H_ diff --git a/localization/imu_integration/include/imu_integration/butterO5.h b/localization/imu_integration/include/imu_integration/butterO5.h new file mode 100644 index 0000000000..e44cd8e8f3 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterO5.h @@ -0,0 +1,178 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_BUTTERO5_H_ +#define IMU_INTEGRATION_BUTTERO5_H_ + +#include + +#include + +#include + +namespace imu_integration { +template +class ButterO5 : public Filter { + public: + ButterO5() : initialized_(false) {} + // Returns filtered value and timestamp + double AddValue(const double value) final { + if (!initialized_) Initialize(value, Params::kGain); + + const int last_index = xv_.size() - 1; + // Shift input and output vals + for (int i = 0; i < last_index; ++i) { + xv_[i] = xv_[i + 1]; + yv_[i] = yv_[i + 1]; + } + // Add new values + xv_[last_index] = value / Params::kGain; + // Generate new output + yv_[last_index] = (xv_[0] + xv_[7]) + Params::kX16 * (xv_[1] + xv_[6]) + Params::kX25 * (xv_[2] + xv_[5]) + + Params::kX34 * (xv_[3] + xv_[4]) + (Params::kY2 * yv_[2]) + (Params::kY3 * yv_[3]) + + (Params::kY4 * yv_[4]) + (Params::kY5 * yv_[5]) + (Params::kY6 * yv_[6]); + + // Return most recent output + return yv_[last_index]; + } + + private: + void Initialize(const double first_value, const double gain) { + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; + } + + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; + +struct ParamsButterO5S62_5Lp3N29_16 { + static constexpr double kGain = 7.974174280e+04; + static constexpr double kX16 = 6.9560160746; + static constexpr double kX25 = 20.7800803730; + static constexpr double kX34 = 34.5601607460; + static constexpr double kY2 = 0.3750929134; + static constexpr double kY3 = -2.2411521809; + static constexpr double kY4 = 5.3982113474; + static constexpr double kY5 = -6.5588143289; + static constexpr double kY6 = 4.0250747177; +}; +using ButterO5S62_5Lp3N29_16 = ButterO5; + +struct ParamsButterO5S62_5Lp1N29_16 { + static constexpr double kGain = 1.444638217e+07; + static constexpr double kX16 = 6.9560160746; + static constexpr double kX25 = 20.7800803730; + static constexpr double kX34 = 34.5601607460; + static constexpr double kY2 = 0.7221701429; + static constexpr double kY3 = -3.8457619644; + static constexpr double kY4 = 8.2000057707; + static constexpr double kY5 = -8.7511375257; + static constexpr double kY6 = 4.6747148135; +}; +using ButterO5S62_5Lp1N29_16 = ButterO5; + +struct ParamsButterO5S62_5Lp0_5N29_16 { + static constexpr double kGain = 4.274918013e+08; + static constexpr double kX16 = 6.9560160746; + static constexpr double kX25 = 20.7800803730; + static constexpr double kX34 = 34.5601607460; + static constexpr double kY2 = 0.8498599655; + static constexpr double kY3 = -4.3875359464; + static constexpr double kY4 = 9.0628533836; + static constexpr double kY5 = -9.3625201736; + static constexpr double kY6 = 4.8373424748; +}; +using ButterO5S62_5Lp0_5N29_16 = ButterO5; + +struct ParamsButterO5S62_5Lp3N20_83 { + static constexpr double kGain = 6.0459547194e+04; + static constexpr double kX16 = 5.9994195281e+00; + static constexpr double kX25 = 1.5997097640e+01; + static constexpr double kX34 = 2.4994195281e+01; + static constexpr double kY2 = 3.7509291343e-01; + static constexpr double kY3 = -2.2411521809e+00; + static constexpr double kY4 = 5.3982113474e+00; + static constexpr double kY5 = -6.5588143289e+00; + static constexpr double kY6 = 4.0250747177e+00; +}; +using ButterO5S62_5Lp3N20_83 = ButterO5; + +struct ParamsButterO5S62_5Lp3N15_83 { + static constexpr double kGain = 4.1144934894e+04; + static constexpr double kX16 = 5.0412147780e+00; + static constexpr double kX25 = 1.1206073890e+01; + static constexpr double kX34 = 1.5412147780e+01; + static constexpr double kY2 = 3.7509291343e-01; + static constexpr double kY3 = -2.2411521809e+00; + static constexpr double kY4 = 5.3982113474e+00; + static constexpr double kY5 = -6.5588143289e+00; + static constexpr double kY6 = 4.0250747177e+00; +}; +using ButterO5S62_5Lp3N15_83 = ButterO5; + +struct ParamsButterO5S125Lp3N41_66 { + static constexpr double kGain = 1.556798187e+06; + static constexpr double kX16 = 5.9996372182; + static constexpr double kX25 = 15.9981860910; + static constexpr double kX34 = 24.9963721820; + static constexpr double kY2 = 0.6135091304; + static constexpr double kY3 = -3.3668482801; + static constexpr double kY4 = 7.4066054096; + static constexpr double kY5 = -8.1654743335; + static constexpr double kY6 = 4.5121464160; +}; +using ButterO5S125Lp3N41_66 = ButterO5; + +struct ParamsButterO5S125Lp3N46_66 { + static constexpr double kGain = 1.764219683e+06; + static constexpr double kX16 = 6.3992967530; + static constexpr double kX25 = 17.9964837650; + static constexpr double kX34 = 28.9929675300; + static constexpr double kY2 = 0.6135091304; + static constexpr double kY3 = -3.3668482801; + static constexpr double kY4 = 7.4066054096; + static constexpr double kY5 = -8.1654743335; + static constexpr double kY6 = 4.5121464160; +}; +using ButterO5S125Lp3N46_66 = ButterO5; + +struct ParamsButterO5S125Lp3N33_33 { + static constexpr double kGain = 1.146058161e+06; + static constexpr double kX16 = 5.2082237395; + static constexpr double kX25 = 12.0411186980; + static constexpr double kX34 = 17.0822373950; + static constexpr double kY2 = 0.6135091304; + static constexpr double kY3 = -3.3668482801; + static constexpr double kY4 = 7.4066054096; + static constexpr double kY5 = -8.1654743335; + static constexpr double kY6 = 4.5121464160; +}; +using ButterO5S125Lp3N33_33 = ButterO5; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERO5_H_ diff --git a/localization/imu_integration/include/imu_integration/butterO7.h b/localization/imu_integration/include/imu_integration/butterO7.h new file mode 100644 index 0000000000..635321e58c --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterO7.h @@ -0,0 +1,91 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_BUTTERO7_H_ +#define IMU_INTEGRATION_BUTTERO7_H_ + +#include + +#include + +#include + +namespace imu_integration { +template +class ButterO7 : public Filter { + public: + ButterO7() : initialized_(false) {} + // Returns filtered value and timestamp + double AddValue(const double value) final { + if (!initialized_) Initialize(value, Params::kGain); + + const int last_index = xv_.size() - 1; + // Shift input and output vals + for (int i = 0; i < last_index; ++i) { + xv_[i] = xv_[i + 1]; + yv_[i] = yv_[i + 1]; + } + // Add new values + xv_[last_index] = value / Params::kGain; + // Generate new output + yv_[last_index] = (xv_[0] + xv_[9]) + Params::kX18 * (xv_[1] + xv_[8]) + Params::kX27 * (xv_[2] + xv_[7]) + + Params::kX36 * (xv_[3] + xv_[6]) + Params::kX45 * (xv_[4] + xv_[5]) + (Params::kY2 * yv_[2]) + + (Params::kY3 * yv_[3]) + (Params::kY4 * yv_[4]) + (Params::kY5 * yv_[5]) + + (Params::kY6 * yv_[6]) + (Params::kY7 * yv_[7]) + (Params::kY8 * yv_[8]); + + // Return most recent output + return yv_[last_index]; + } + + private: + void Initialize(const double first_value, const double gain) { + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; + } + + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; + +struct ParamsButterO7S62_5Lp3N20_83 { + static constexpr double kGain = 3.168744781e+06; + static constexpr double kX18 = 7.9994195281; + static constexpr double kX27 = 28.9959366960; + static constexpr double kX36 = 62.9878100890; + static constexpr double kX45 = 90.9796834820; + static constexpr double kY2 = 0.2561484929; + static constexpr double kY3 = -2.1400274434; + static constexpr double kY4 = 7.7016541929; + static constexpr double kY5 = -15.4832538240; + static constexpr double kY6 = 18.7878862180; + static constexpr double kY7 = -13.7678927060; + static constexpr double kY8 = 5.6453639087; +}; +using ButterO7S62_5Lp3N20_83 = ButterO7; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERO7_H_ diff --git a/localization/imu_integration/include/imu_integration/dynamic_imu_filter.h b/localization/imu_integration/include/imu_integration/dynamic_imu_filter.h new file mode 100644 index 0000000000..e90d47bab4 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/dynamic_imu_filter.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_DYNAMIC_IMU_FILTER_H_ +#define IMU_INTEGRATION_DYNAMIC_IMU_FILTER_H_ + +#include +#include +#include +#include +#include + +#include + +#include + +namespace imu_integration { +class DynamicImuFilter { + public: + DynamicImuFilter(const ImuFilterParams& params, const localization_measurements::FanSpeedMode initial_fan_speed_mode = + localization_measurements::FanSpeedMode::kNominal); + // Returns filtered measurement if one is available + boost::optional AddMeasurement( + const localization_measurements::ImuMeasurement& imu_measurement); + + // Notch filter depends on fan speed, change filter as fan speed changes + void SetFanSpeedMode(const localization_measurements::FanSpeedMode fan_speed_mode, + const bool ignore_saved_fan_speed_mode = false); + + localization_measurements::FanSpeedMode fan_speed_mode() const; + + private: + // Acceleration Filters + std::unique_ptr acceleration_x_filter_; + std::unique_ptr acceleration_y_filter_; + std::unique_ptr acceleration_z_filter_; + // Angular Velocity Filters + std::unique_ptr angular_velocity_x_filter_; + std::unique_ptr angular_velocity_y_filter_; + std::unique_ptr angular_velocity_z_filter_; + ImuFilterParams params_; + localization_measurements::FanSpeedMode fan_speed_mode_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_DYNAMIC_IMU_FILTER_H_ diff --git a/localization/imu_integration/include/imu_integration/filter.h b/localization/imu_integration/include/imu_integration/filter.h new file mode 100644 index 0000000000..dc0fc42a0c --- /dev/null +++ b/localization/imu_integration/include/imu_integration/filter.h @@ -0,0 +1,31 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_FILTER_H_ +#define IMU_INTEGRATION_FILTER_H_ + +namespace imu_integration { +class Filter { + public: + virtual ~Filter() = default; + // Returns filtered value and timestamp + virtual double AddValue(const double value) = 0; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_FILTER_H_ diff --git a/localization/imu_integration/include/imu_integration/identity_filter.h b/localization/imu_integration/include/imu_integration/identity_filter.h new file mode 100644 index 0000000000..2a3725a3fb --- /dev/null +++ b/localization/imu_integration/include/imu_integration/identity_filter.h @@ -0,0 +1,33 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_IDENTITY_FILTER_H_ +#define IMU_INTEGRATION_IDENTITY_FILTER_H_ + +#include + +namespace imu_integration { +class IdentityFilter : public Filter { + public: + IdentityFilter(); + // Returns filtered value and timestamp + double AddValue(const double value) final; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IDENTITY_FILTER_H_ diff --git a/localization/imu_integration/include/imu_integration/imu_filter.h b/localization/imu_integration/include/imu_integration/imu_filter.h new file mode 100644 index 0000000000..108835dd91 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/imu_filter.h @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_IMU_FILTER_H_ +#define IMU_INTEGRATION_IMU_FILTER_H_ + +#include +#include +#include +#include + +#include + +#include + +namespace imu_integration { +class ImuFilter { + public: + explicit ImuFilter(const ImuFilterParams& params); + // Returns filtered measurement if one is available + boost::optional AddMeasurement( + const localization_measurements::ImuMeasurement& imu_measurement); + + private: + // Acceleration Filters + std::unique_ptr acceleration_x_filter_; + std::unique_ptr acceleration_y_filter_; + std::unique_ptr acceleration_z_filter_; + // Angular Velocity Filters + std::unique_ptr angular_velocity_x_filter_; + std::unique_ptr angular_velocity_y_filter_; + std::unique_ptr angular_velocity_z_filter_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IMU_FILTER_H_ diff --git a/localization/imu_integration/include/imu_integration/imu_filter_params.h b/localization/imu_integration/include/imu_integration/imu_filter_params.h new file mode 100644 index 0000000000..fd5805d3e1 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/imu_filter_params.h @@ -0,0 +1,36 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef IMU_INTEGRATION_IMU_FILTER_PARAMS_H_ +#define IMU_INTEGRATION_IMU_FILTER_PARAMS_H_ + +#include + +namespace imu_integration { +struct ImuFilterParams { + // Filter for fan speed modes (quiet = 2000 rpm, nominal = 2500 rpm, aggressive = 2800 rpm) + // none, butter, butter20_83, butter3, butter5, butter5_1, butter5_05 + std::string quiet_accel = "none"; + std::string quiet_ang_vel = "none"; + std::string nominal_accel = "none"; + std::string nominal_ang_vel = "none"; + std::string aggressive_accel = "none"; + std::string aggressive_ang_vel = "none"; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IMU_FILTER_PARAMS_H_ diff --git a/localization/imu_integration/include/imu_integration/imu_integrator.h b/localization/imu_integration/include/imu_integration/imu_integrator.h new file mode 100644 index 0000000000..5f57793219 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/imu_integrator.h @@ -0,0 +1,78 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_IMU_INTEGRATOR_H_ +#define IMU_INTEGRATION_IMU_INTEGRATOR_H_ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace imu_integration { +// Integrates imu measurements and propagates uncertainties. +// Maintains a window of measurements so that any interval of measurements in +// that window can be integrated into a pim. +class ImuIntegrator : public localization_common::TimestampedSet { + public: + explicit ImuIntegrator(const ImuIntegratorParams& params = ImuIntegratorParams()); + + // Buffers an imu measurement. + void AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); + + // Creates an integrated GTSAM Pim using the provided bias and imu measurements + // between the provided start and end times. + boost::optional IntegratedPim( + const gtsam::imuBias::ConstantBias& bias, const localization_common::Time start_time, + const localization_common::Time end_time) const; + + // Extrapolates the provided combined nav state using imu measurements up to the provided + // end time. + boost::optional Extrapolate( + const localization_common::CombinedNavState& combined_nav_state, const localization_common::Time end_time) const; + + // Extrapolates using all IMU measurements more recent than the combined nav state + boost::optional ExtrapolateLatest( + const localization_common::CombinedNavState& combined_nav_state) const; + + void SetFanSpeedMode(const localization_measurements::FanSpeedMode fan_speed_mode); + + localization_measurements::FanSpeedMode fan_speed_mode() const; + + private: + // Returns last integrated measurement timestamp. + boost::optional IntegrateImuMeasurements( + const localization_common::Time start_time, const localization_common::Time end_time, + gtsam::PreintegratedCombinedMeasurements& pim) const; + + ImuIntegratorParams params_; + boost::shared_ptr pim_params_; + std::unique_ptr imu_filter_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IMU_INTEGRATOR_H_ diff --git a/localization/imu_integration/include/imu_integration/imu_integrator_params.h b/localization/imu_integration/include/imu_integration/imu_integrator_params.h new file mode 100644 index 0000000000..03e2d542ab --- /dev/null +++ b/localization/imu_integration/include/imu_integration/imu_integrator_params.h @@ -0,0 +1,43 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef IMU_INTEGRATION_IMU_INTEGRATOR_PARAMS_H_ +#define IMU_INTEGRATION_IMU_INTEGRATOR_PARAMS_H_ + +#include + +#include +#include + +#include + +namespace imu_integration { +struct ImuIntegratorParams { + gtsam::Vector3 gravity; + gtsam::Pose3 body_T_imu; + ImuFilterParams filter; + // From gtsam: Angular and velocity random walk expressed in degrees respectively m/s per sqrt(hr). + double gyro_sigma; + double accel_sigma; + double accel_bias_sigma; + double gyro_bias_sigma; + double integration_variance; + double bias_acc_omega_int; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IMU_INTEGRATOR_PARAMS_H_ diff --git a/localization/imu_integration/include/imu_integration/parameter_reader.h b/localization/imu_integration/include/imu_integration/parameter_reader.h new file mode 100644 index 0000000000..bc5ebdd112 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/parameter_reader.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_PARAMETER_READER_H_ +#define IMU_INTEGRATION_PARAMETER_READER_H_ + +#include +#include +#include +#include + +#include + +namespace imu_integration { +void LoadImuIntegratorParams(config_reader::ConfigReader& config, ImuIntegratorParams& params); + +void LoadImuFilterParams(config_reader::ConfigReader& config, ImuFilterParams& params); + +std::unique_ptr LoadFilter(const std::string& filter_type); +} // namespace imu_integration + +#endif // IMU_INTEGRATION_PARAMETER_READER_H_ diff --git a/localization/imu_integration/include/imu_integration/test_utilities.h b/localization/imu_integration/include/imu_integration/test_utilities.h new file mode 100644 index 0000000000..7d06b6435f --- /dev/null +++ b/localization/imu_integration/include/imu_integration/test_utilities.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_TEST_UTILITIES_H_ +#define IMU_INTEGRATION_TEST_UTILITIES_H_ + +#include +#include +#include +#include + +#include + +namespace imu_integration { +ImuIntegratorParams DefaultImuIntegratorParams(); + +std::vector ConstantMeasurements(const Eigen::Vector3d& acceleration, + const Eigen::Vector3d& angular_velocity, + const int num_measurements, + const localization_common::Time start_time, + const double time_increment); + +gtsam::Rot3 IntegrateAngularVelocities(const std::vector& imu_measurements, + const gtsam::Rot3& starting_orientation, + const localization_common::Time starting_time); + +sensor_msgs::Imu ImuMsg(const localization_measurements::ImuMeasurement& imu_measurement); +} // namespace imu_integration + +#endif // IMU_INTEGRATION_TEST_UTILITIES_H_ diff --git a/localization/imu_integration/include/imu_integration/utilities.h b/localization/imu_integration/include/imu_integration/utilities.h new file mode 100644 index 0000000000..4490c3d1b6 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/utilities.h @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_UTILITIES_H_ +#define IMU_INTEGRATION_UTILITIES_H_ + +#include +#include +#include + +#include +#include + +#include + +#include + +namespace imu_integration { +namespace sym = gtsam::symbol_shorthand; +boost::optional Interpolate( + const localization_measurements::ImuMeasurement& imu_measurement_a, + const localization_measurements::ImuMeasurement& imu_measurement_b, const localization_common::Time timestamp); + +gtsam::PreintegratedCombinedMeasurements Pim( + const gtsam::imuBias::ConstantBias& bias, + const boost::shared_ptr& params); + +void AddMeasurement(const localization_measurements::ImuMeasurement& imu_measurement, + localization_common::Time& last_added_imu_measurement_time, + gtsam::PreintegratedCombinedMeasurements& pim); + +localization_common::CombinedNavState PimPredict(const localization_common::CombinedNavState& combined_nav_state, + const gtsam::PreintegratedCombinedMeasurements& pim); +} // namespace imu_integration + +#endif // IMU_INTEGRATION_UTILITIES_H_ diff --git a/localization/imu_integration/package.xml b/localization/imu_integration/package.xml new file mode 100644 index 0000000000..f4bccb1e22 --- /dev/null +++ b/localization/imu_integration/package.xml @@ -0,0 +1,24 @@ + + imu_integration + 1.0.0 + + Imu integration package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + config_reader + localization_common + localization_measurements + config_reader + localization_common + localization_measurements + + diff --git a/localization/imu_integration/readme.md b/localization/imu_integration/readme.md new file mode 100644 index 0000000000..ddd4c800f9 --- /dev/null +++ b/localization/imu_integration/readme.md @@ -0,0 +1,14 @@ +\page imuintegration Imu Integration + +# Package Overview +The imu integration package integrates imu measurements using the gtsam PreintegratedCombinedMeasurements (pim) class. As measurements are added to the pim, their acceleration and angular velocities are integrated after removing imu biases and uncertainties are propogated accordingly (_Dellaert, The new Imu Factor, https://github.com/borglab/gtsam/blob/develop/doc/ImuFactor.pdf_). For efficieny and particulary for use in an optimization procedure, the pim uses the initial biases and velocity estimate and assumes these are constant for the integration procedure. The pim also assumes gravity is constantly oriented for the integration procedure, which can lead to accumulated errors if there are large orientation changes before reinitializing the pim. Therefore there is a tradeoff of efficiency and accuracy, where the most accurate integration would reset the pim after each measurement integration. + +## Important Classes +# ImuIntegrator +Maintains a history of measurements and allows for the create of pims using different sets of measurements. + +# LatestImuIntegrator +Adds on to the ImuIntegrator by keeping track of the latest integrated measurement and allowing for the integration of the latest measurements up to a certain time. + +# Filters +There are various filters provided that enable filtering of imu data before it is added to an imu integrator. Filter params are provided in their header files, and vary by filter order and cutoff frequency. Each butterworth filter is a lowpass filter. diff --git a/localization/imu_integration/src/dynamic_imu_filter.cc b/localization/imu_integration/src/dynamic_imu_filter.cc new file mode 100644 index 0000000000..e2c88cf510 --- /dev/null +++ b/localization/imu_integration/src/dynamic_imu_filter.cc @@ -0,0 +1,94 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#include +#include +#include +#include + +namespace imu_integration { +namespace lm = localization_measurements; +DynamicImuFilter::DynamicImuFilter(const ImuFilterParams& params, const lm::FanSpeedMode initial_fan_speed_mode) + : params_(params), fan_speed_mode_(initial_fan_speed_mode) { + SetFanSpeedMode(fan_speed_mode_, true); +} + +boost::optional DynamicImuFilter::AddMeasurement(const lm::ImuMeasurement& imu_measurement) { + const double filtered_acceleration_x = acceleration_x_filter_->AddValue(imu_measurement.acceleration.x()); + const double filtered_acceleration_y = acceleration_y_filter_->AddValue(imu_measurement.acceleration.y()); + const double filtered_acceleration_z = acceleration_z_filter_->AddValue(imu_measurement.acceleration.z()); + const double filtered_angular_velocity_x = angular_velocity_x_filter_->AddValue(imu_measurement.angular_velocity.x()); + const double filtered_angular_velocity_y = angular_velocity_y_filter_->AddValue(imu_measurement.angular_velocity.y()); + const double filtered_angular_velocity_z = angular_velocity_z_filter_->AddValue(imu_measurement.angular_velocity.z()); + + // Use original timestamp + auto filtered_imu_measurement = imu_measurement; + filtered_imu_measurement.acceleration << filtered_acceleration_x, filtered_acceleration_y, filtered_acceleration_z; + filtered_imu_measurement.angular_velocity << filtered_angular_velocity_x, filtered_angular_velocity_y, + filtered_angular_velocity_z; + return filtered_imu_measurement; +} + +void DynamicImuFilter::SetFanSpeedMode(const lm::FanSpeedMode fan_speed_mode, const bool ignore_saved_fan_speed_mode) { + if (fan_speed_mode != fan_speed_mode_ || ignore_saved_fan_speed_mode) { + switch (fan_speed_mode) { + case lm::FanSpeedMode::kOff: { + acceleration_x_filter_ = LoadFilter("none"); + acceleration_y_filter_ = LoadFilter("none"); + acceleration_z_filter_ = LoadFilter("none"); + angular_velocity_x_filter_ = LoadFilter("none"); + angular_velocity_y_filter_ = LoadFilter("none"); + angular_velocity_z_filter_ = LoadFilter("none"); + break; + } + case lm::FanSpeedMode::kQuiet: { + acceleration_x_filter_ = LoadFilter(params_.quiet_accel); + acceleration_y_filter_ = LoadFilter(params_.quiet_accel); + acceleration_z_filter_ = LoadFilter(params_.quiet_accel); + angular_velocity_x_filter_ = LoadFilter(params_.quiet_ang_vel); + angular_velocity_y_filter_ = LoadFilter(params_.quiet_ang_vel); + angular_velocity_z_filter_ = LoadFilter(params_.quiet_ang_vel); + break; + } + case lm::FanSpeedMode::kNominal: { + acceleration_x_filter_ = LoadFilter(params_.nominal_accel); + acceleration_y_filter_ = LoadFilter(params_.nominal_accel); + acceleration_z_filter_ = LoadFilter(params_.nominal_accel); + angular_velocity_x_filter_ = LoadFilter(params_.nominal_ang_vel); + angular_velocity_y_filter_ = LoadFilter(params_.nominal_ang_vel); + angular_velocity_z_filter_ = LoadFilter(params_.nominal_ang_vel); + break; + } + case lm::FanSpeedMode::kAggressive: { + acceleration_x_filter_ = LoadFilter(params_.aggressive_accel); + acceleration_y_filter_ = LoadFilter(params_.aggressive_accel); + acceleration_z_filter_ = LoadFilter(params_.aggressive_accel); + angular_velocity_x_filter_ = LoadFilter(params_.aggressive_ang_vel); + angular_velocity_y_filter_ = LoadFilter(params_.aggressive_ang_vel); + angular_velocity_z_filter_ = LoadFilter(params_.aggressive_ang_vel); + break; + } + default: { + break; // Shouldn't get here + } + } + fan_speed_mode_ = fan_speed_mode; + } +} + +lm::FanSpeedMode DynamicImuFilter::fan_speed_mode() const { return fan_speed_mode_; } +} // namespace imu_integration diff --git a/localization/imu_integration/src/identity_filter.cc b/localization/imu_integration/src/identity_filter.cc new file mode 100644 index 0000000000..02bc892d1c --- /dev/null +++ b/localization/imu_integration/src/identity_filter.cc @@ -0,0 +1,26 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +namespace imu_integration { +IdentityFilter::IdentityFilter() {} + +double IdentityFilter::AddValue(const double value) { return value; } +} // namespace imu_integration diff --git a/localization/imu_integration/src/imu_filter.cc b/localization/imu_integration/src/imu_filter.cc new file mode 100644 index 0000000000..ec19bec26e --- /dev/null +++ b/localization/imu_integration/src/imu_filter.cc @@ -0,0 +1,50 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#include +#include +#include +#include + +namespace imu_integration { +namespace lm = localization_measurements; +ImuFilter::ImuFilter(const ImuFilterParams& params) { + // Default to nominal filters + acceleration_x_filter_ = LoadFilter(params.nominal_accel); + acceleration_y_filter_ = LoadFilter(params.nominal_accel); + acceleration_z_filter_ = LoadFilter(params.nominal_accel); + angular_velocity_x_filter_ = LoadFilter(params.nominal_ang_vel); + angular_velocity_y_filter_ = LoadFilter(params.nominal_ang_vel); + angular_velocity_z_filter_ = LoadFilter(params.nominal_ang_vel); +} + +boost::optional ImuFilter::AddMeasurement(const lm::ImuMeasurement& imu_measurement) { + const double filtered_acceleration_x = acceleration_x_filter_->AddValue(imu_measurement.acceleration.x()); + const double filtered_acceleration_y = acceleration_y_filter_->AddValue(imu_measurement.acceleration.y()); + const double filtered_acceleration_z = acceleration_z_filter_->AddValue(imu_measurement.acceleration.z()); + const double filtered_angular_velocity_x = angular_velocity_x_filter_->AddValue(imu_measurement.angular_velocity.x()); + const double filtered_angular_velocity_y = angular_velocity_y_filter_->AddValue(imu_measurement.angular_velocity.y()); + const double filtered_angular_velocity_z = angular_velocity_z_filter_->AddValue(imu_measurement.angular_velocity.z()); + + // Use original timestamp + auto filtered_imu_measurement = imu_measurement; + filtered_imu_measurement.acceleration << filtered_acceleration_x, filtered_acceleration_y, filtered_acceleration_z; + filtered_imu_measurement.angular_velocity << filtered_angular_velocity_x, filtered_angular_velocity_y, + filtered_angular_velocity_z; + return filtered_imu_measurement; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/imu_integrator.cc b/localization/imu_integration/src/imu_integrator.cc new file mode 100644 index 0000000000..cbafff7fa2 --- /dev/null +++ b/localization/imu_integration/src/imu_integrator.cc @@ -0,0 +1,150 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +namespace imu_integration { +namespace lc = localization_common; +namespace lm = localization_measurements; +ImuIntegrator::ImuIntegrator(const ImuIntegratorParams& params) : params_(params) { + imu_filter_.reset(new DynamicImuFilter(params_.filter)); + LogDebug("ImuIntegrator: Gravity vector: " << std::endl << params_.gravity.matrix()); + pim_params_.reset(new gtsam::PreintegratedCombinedMeasurements::Params(params_.gravity)); + // Set sensor covariances + pim_params_->gyroscopeCovariance = params_.gyro_sigma * params_.gyro_sigma * gtsam::I_3x3; + pim_params_->accelerometerCovariance = params_.accel_sigma * params_.accel_sigma * gtsam::I_3x3; + pim_params_->integrationCovariance = params_.integration_variance * gtsam::I_3x3; + // Set bias random walk covariances + pim_params_->biasAccCovariance = params_.accel_bias_sigma * params_.accel_bias_sigma * gtsam::I_3x3; + pim_params_->biasOmegaCovariance = params_.gyro_bias_sigma * params_.gyro_bias_sigma * gtsam::I_3x3; + // Set bias covariance used for pim integration + pim_params_->biasAccOmegaInt = params_.bias_acc_omega_int * gtsam::I_6x6; + // Set imu calibration relative pose + pim_params_->setBodyPSensor(params_.body_T_imu); +} + +void ImuIntegrator::AddImuMeasurement(const lm::ImuMeasurement& imu_measurement) { + const auto filtered_imu_measurement = imu_filter_->AddMeasurement(imu_measurement); + if (filtered_imu_measurement) { + Add(filtered_imu_measurement->timestamp, *filtered_imu_measurement); + } +} + +boost::optional ImuIntegrator::IntegrateImuMeasurements(const lc::Time start_time, const lc::Time end_time, + gtsam::PreintegratedCombinedMeasurements& pim) const { + if (size() < 2) { + LogError("IntegrateImuMeasurements: Less than 2 measurements available."); + return boost::none; + } + if (end_time < *OldestTimestamp()) { + LogError("IntegrateImuMeasurements: End time occurs before first measurement."); + return boost::none; + } + if (end_time > *LatestTimestamp()) { + LogError("IntegrateImuMeasurements: End time occurs after last measurement."); + return boost::none; + } + if (start_time > end_time) { + LogError("IntegrateImuMeasurements: Start time occurs after end time."); + return boost::none; + } + + // Start with least upper bound measurement + // Don't add measurements with same timestamp as start_time + // since these would have a dt of 0 (wrt the pim start time) and cause errors for the pim + auto measurement_it = set().upper_bound(start_time); + lc::Time last_added_imu_measurement_time = start_time; + int num_measurements_added = 0; + for (; measurement_it != set().cend() && measurement_it->first <= end_time; ++measurement_it) { + AddMeasurement(measurement_it->second, last_added_imu_measurement_time, pim); + ++num_measurements_added; + } + + // Add final interpolated measurement if necessary + if (last_added_imu_measurement_time != end_time) { + const auto interpolated_measurement = + Interpolate(std::prev(measurement_it)->second, measurement_it->second, end_time); + if (!interpolated_measurement) { + LogError("IntegrateImuMeasurements: Failed to interpolate final measurement."); + return boost::none; + } + AddMeasurement(*interpolated_measurement, last_added_imu_measurement_time, pim); + ++num_measurements_added; + } + + if (last_added_imu_measurement_time != end_time) { + LogError("IntegrateImuMeasurements: Last added time not equal to end time."); + return boost::none; + } + + LogDebug("IntegrateImuMeasurements: Num imu measurements integrated: " << num_measurements_added); + LogDebug( + "IntegrateImuMeasurements: Total Num Imu Measurements after " + "integrating: " + << size()); + return last_added_imu_measurement_time; +} + +boost::optional ImuIntegrator::IntegratedPim( + const gtsam::imuBias::ConstantBias& bias, const lc::Time start_time, const lc::Time end_time) const { + auto pim = Pim(bias, pim_params_); + const auto last_integrated_measurement_time = IntegrateImuMeasurements(start_time, end_time, pim); + if (!last_integrated_measurement_time) { + LogError("IntegratedPim: Failed to integrate imu measurments."); + return boost::none; + } + return pim; +} + +boost::optional ImuIntegrator::Extrapolate(const lc::CombinedNavState& combined_nav_state, + const lc::Time end_time) const { + const auto pim = IntegratedPim(combined_nav_state.bias(), combined_nav_state.timestamp(), end_time); + if (!pim) { + LogError("Extrapolate: Failed to get pim."); + return boost::none; + } + + return PimPredict(combined_nav_state, *pim); +} + +boost::optional ImuIntegrator::ExtrapolateLatest( + const localization_common::CombinedNavState& combined_nav_state) const { + const auto latest = Latest(); + if (!latest) { + LogError("ExtrapolateLatest: Failed to get latest measurement."); + return boost::none; + } + + const auto extrapolated_combined_nav_state = Extrapolate(combined_nav_state, latest->timestamp); + if (!extrapolated_combined_nav_state) { + LogError("ExtrapolateLatest: Failed to extrapolate combined nav state."); + return boost::none; + } + + return *extrapolated_combined_nav_state; +} + +void ImuIntegrator::SetFanSpeedMode(const lm::FanSpeedMode fan_speed_mode) { + imu_filter_->SetFanSpeedMode(fan_speed_mode); +} + +lm::FanSpeedMode ImuIntegrator::fan_speed_mode() const { return imu_filter_->fan_speed_mode(); } +} // namespace imu_integration diff --git a/localization/imu_integration/src/parameter_reader.cc b/localization/imu_integration/src/parameter_reader.cc new file mode 100644 index 0000000000..86f192ebf0 --- /dev/null +++ b/localization/imu_integration/src/parameter_reader.cc @@ -0,0 +1,109 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace imu_integration { +namespace lc = localization_common; +namespace mc = msg_conversions; +void LoadImuIntegratorParams(config_reader::ConfigReader& config, ImuIntegratorParams& params) { + params.gravity = lc::LoadVector3(config, "world_gravity_vector"); + const bool ignore_gravity = mc::LoadBool(config, "ignore_gravity"); + if (ignore_gravity) params.gravity = gtsam::Vector3::Zero(); + params.body_T_imu = lc::LoadTransform(config, "imu_transform"); + LoadImuFilterParams(config, params.filter); + params.gyro_sigma = mc::LoadDouble(config, "gyro_sigma"); + params.accel_sigma = mc::LoadDouble(config, "accel_sigma"); + params.accel_bias_sigma = mc::LoadDouble(config, "accel_bias_sigma"); + params.gyro_bias_sigma = mc::LoadDouble(config, "gyro_bias_sigma"); + params.integration_variance = mc::LoadDouble(config, "integration_variance"); + params.bias_acc_omega_int = mc::LoadDouble(config, "bias_acc_omega_int"); +} + +void LoadImuFilterParams(config_reader::ConfigReader& config, ImuFilterParams& params) { + params.quiet_accel = mc::LoadString(config, "imu_filter_quiet_accel"); + params.quiet_ang_vel = mc::LoadString(config, "imu_filter_quiet_ang_vel"); + params.nominal_accel = mc::LoadString(config, "imu_filter_nominal_accel"); + params.nominal_ang_vel = mc::LoadString(config, "imu_filter_nominal_ang_vel"); + params.aggressive_accel = mc::LoadString(config, "imu_filter_aggressive_accel"); + params.aggressive_ang_vel = mc::LoadString(config, "imu_filter_aggressive_ang_vel"); +} + +std::unique_ptr LoadFilter(const std::string& filter_type) { + // 1st Order + if (filter_type == "ButterO1S62_5Lp3N29_16") { + return std::unique_ptr(new ButterO1S62_5Lp3N29_16()); + } else if (filter_type == "ButterO1S62_5Lp3N20_83") { + return std::unique_ptr(new ButterO1S62_5Lp3N20_83()); + } else if (filter_type == "ButterO1S62_5Lp3N15_83") { + return std::unique_ptr(new ButterO1S62_5Lp3N15_83()); + } else if (filter_type == "ButterO1S125Lp3N33_33") { // 125Hz 1st Order + return std::unique_ptr(new ButterO1S125Lp3N33_33()); + } else if (filter_type == "ButterO1S125Lp3N41_66") { + return std::unique_ptr(new ButterO1S125Lp3N41_66()); + } else if (filter_type == "ButterO1S125Lp3N46_66") { + return std::unique_ptr(new ButterO1S125Lp3N46_66()); + } else if (filter_type == "ButterO3S62_5Lp3N29_16") { // 3rd Order + return std::unique_ptr(new ButterO3S62_5Lp3N29_16()); + } else if (filter_type == "ButterO3S62_5Lp3N20_83") { + return std::unique_ptr(new ButterO3S62_5Lp3N20_83()); + } else if (filter_type == "ButterO3S62_5Lp3N15_83") { + return std::unique_ptr(new ButterO3S62_5Lp3N15_83()); + } else if (filter_type == "ButterO3S125Lp3N33_33") { // 125Hz 3rd Order + return std::unique_ptr(new ButterO3S125Lp3N33_33()); + } else if (filter_type == "ButterO3S125Lp3N41_66") { + return std::unique_ptr(new ButterO3S125Lp3N41_66()); + } else if (filter_type == "ButterO3S125Lp3N46_66") { + return std::unique_ptr(new ButterO3S125Lp3N46_66()); + } else if (filter_type == "ButterO5S62_5Lp3N29_16") { // 5th Order + return std::unique_ptr(new ButterO5S62_5Lp3N29_16()); + } else if (filter_type == "ButterO5S62_5Lp3N20_83") { + return std::unique_ptr(new ButterO5S62_5Lp3N20_83()); + } else if (filter_type == "ButterO5S62_5Lp3N15_83") { + return std::unique_ptr(new ButterO5S62_5Lp3N15_83()); + } else if (filter_type == "ButterO5S125Lp3N41_66") { // 125Hz + return std::unique_ptr(new ButterO5S125Lp3N41_66()); + } else if (filter_type == "ButterO5S125Lp3N46_66") { + return std::unique_ptr(new ButterO5S125Lp3N46_66()); + } else if (filter_type == "ButterO5S125Lp3N33_33") { + return std::unique_ptr(new ButterO5S125Lp3N33_33()); + } else if (filter_type == "ButterO7S62_5Lp3N20_83") { // 7th Order + return std::unique_ptr(new ButterO7S62_5Lp3N20_83()); + } else if (filter_type == "ButterO10S62_5Lp3N20_83") { // 10th Order + return std::unique_ptr(new ButterO10S62_5Lp3N20_83()); + } else if (filter_type == "ButterO5S62_5Lp1N29_16") { // Lower pass band + return std::unique_ptr(new ButterO5S62_5Lp1N29_16()); + } else if (filter_type == "ButterO5S62_5Lp0_5N29_16") { + return std::unique_ptr(new ButterO5S62_5Lp0_5N29_16()); + } else if (filter_type == "none") { + return std::unique_ptr(new IdentityFilter()); + } else { + LogFatal("LoadFilter: Invalid filter selection."); + return std::unique_ptr(new IdentityFilter()); + } +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/test_utilities.cc b/localization/imu_integration/src/test_utilities.cc new file mode 100644 index 0000000000..2dbb62b846 --- /dev/null +++ b/localization/imu_integration/src/test_utilities.cc @@ -0,0 +1,79 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +namespace imu_integration { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +ImuIntegratorParams DefaultImuIntegratorParams() { + ImuIntegratorParams params; + params.gravity = gtsam::Vector3::Zero(); + params.body_T_imu = gtsam::Pose3::identity(); + // Filer params are already default none + params.filter = imu_integration::ImuFilterParams(); + params.gyro_sigma = 0.1; + params.accel_sigma = 0.1; + params.accel_bias_sigma = 0.1; + params.gyro_bias_sigma = 0.1; + params.integration_variance = 0.1; + params.bias_acc_omega_int = 0.1; + return params; +} + +std::vector ConstantMeasurements(const Eigen::Vector3d& acceleration, + const Eigen::Vector3d& angular_velocity, + const int num_measurements, const lc::Time start_time, + const double time_increment) { + std::vector imu_measurements; + for (int i = 0; i < num_measurements; ++i) { + const lc::Time time = start_time + i * time_increment; + imu_measurements.emplace_back(lm::ImuMeasurement(acceleration, angular_velocity, time)); + } + return imu_measurements; +} + +gtsam::Rot3 IntegrateAngularVelocities(const std::vector& imu_measurements, + const gtsam::Rot3& starting_orientation, + const localization_common::Time starting_time) { + gtsam::Rot3 integrated_orientation = starting_orientation; + lc::Time integrated_time = starting_time; + for (const auto& imu_measurement : imu_measurements) { + if (imu_measurement.timestamp <= integrated_time) continue; + const double dt = imu_measurement.timestamp - integrated_time; + integrated_time = imu_measurement.timestamp; + // TODO(rsoussan): subtract ang vel bias first!! add this as param!! + const gtsam::Rot3 orientation_update = gtsam::Rot3::Expmap(imu_measurement.angular_velocity * dt); + integrated_orientation = integrated_orientation * orientation_update; + } + return integrated_orientation; +} + +sensor_msgs::Imu ImuMsg(const localization_measurements::ImuMeasurement& imu_measurement) { + sensor_msgs::Imu imu_msg; + msg_conversions::VectorToMsg(imu_measurement.acceleration, imu_msg.linear_acceleration); + msg_conversions::VectorToMsg(imu_measurement.angular_velocity, imu_msg.angular_velocity); + lc::TimeToHeader(imu_measurement.timestamp, imu_msg.header); + return imu_msg; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/utilities.cc b/localization/imu_integration/src/utilities.cc new file mode 100644 index 0000000000..a502b3cda0 --- /dev/null +++ b/localization/imu_integration/src/utilities.cc @@ -0,0 +1,72 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +namespace imu_integration { +namespace lc = localization_common; +namespace lm = localization_measurements; + +boost::optional Interpolate(const lm::ImuMeasurement& imu_measurement_a, + const lm::ImuMeasurement& imu_measurement_b, const lc::Time timestamp) { + if (timestamp < imu_measurement_a.timestamp || timestamp > imu_measurement_b.timestamp) { + LogError( + "Interpolate: Interpolation timestamp out of range of imu " + "measurements."); + return boost::none; + } + + const double alpha = + (timestamp - imu_measurement_a.timestamp) / (imu_measurement_b.timestamp - imu_measurement_a.timestamp); + const Eigen::Vector3d interpolated_acceleration = + (1.0 - alpha) * imu_measurement_a.acceleration + alpha * imu_measurement_b.acceleration; + const Eigen::Vector3d interpolated_angular_velocity = + (1.0 - alpha) * imu_measurement_a.angular_velocity + alpha * imu_measurement_b.angular_velocity; + + return lm::ImuMeasurement(interpolated_acceleration, interpolated_angular_velocity, timestamp); +} + +gtsam::PreintegratedCombinedMeasurements Pim( + const gtsam::imuBias::ConstantBias& bias, + const boost::shared_ptr& params) { + gtsam::PreintegratedCombinedMeasurements pim(params); + pim.resetIntegrationAndSetBias(bias); + return pim; +} + +void AddMeasurement(const lm::ImuMeasurement& imu_measurement, lc::Time& last_added_imu_measurement_time, + gtsam::PreintegratedCombinedMeasurements& pim) { + const double dt = imu_measurement.timestamp - last_added_imu_measurement_time; + // TODO(rsoussan): check if dt too large? Pass threshold param? + if (dt == 0) { + LogError("AddMeasurement: Timestamp difference 0, failed to add measurement."); + return; + } + pim.integrateMeasurement(imu_measurement.acceleration, imu_measurement.angular_velocity, dt); + last_added_imu_measurement_time = imu_measurement.timestamp; +} + +lc::CombinedNavState PimPredict(const lc::CombinedNavState& combined_nav_state, + const gtsam::PreintegratedCombinedMeasurements& pim) { + const gtsam::NavState predicted_nav_state = pim.predict(combined_nav_state.nav_state(), pim.biasHat()); + const lc::Time timestamp = combined_nav_state.timestamp() + pim.deltaTij(); + return lc::CombinedNavState(predicted_nav_state, pim.biasHat(), timestamp); +} +} // namespace imu_integration diff --git a/localization/imu_integration/test/test_imu_integrator.cc b/localization/imu_integration/test/test_imu_integrator.cc new file mode 100644 index 0000000000..7480fdfd78 --- /dev/null +++ b/localization/imu_integration/test/test_imu_integrator.cc @@ -0,0 +1,250 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include + +#include + +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; + +namespace { +// Defaults to random start pose and velocity and zero IMU/IMU bias values +struct TestParams { + Eigen::Vector3d acceleration = Eigen::Vector3d::Zero(); + Eigen::Vector3d angular_velocity = Eigen::Vector3d::Zero(); + Eigen::Vector3d accelerometer_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyroscope_bias = Eigen::Vector3d::Zero(); + gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); + gtsam::Pose3 initial_pose = lc::RandomPose(); + Eigen::Vector3d initial_velocity = lc::RandomVector3d(); + double integration_start_time = 0; +}; + +Eigen::Vector3d AccelerationOnlyIntegratedPosition(const TestParams& params, const double duration) { + return params.initial_pose.translation() + params.initial_velocity * duration + + params.initial_pose.rotation() * (params.acceleration - params.accelerometer_bias) * 0.5 * + std::pow(duration, 2); +} + +Eigen::Vector3d AccelerationOnlyIntegratedVelocity(const TestParams& params, const double duration) { + return params.initial_velocity + + params.initial_pose.rotation() * (params.acceleration - params.accelerometer_bias) * duration; +} +} // namespace + +class ConstantIMUTest : public ::testing::Test { + public: + // Start at time increment so first IMU measurement is after starting combined nav state time + ConstantIMUTest() : time_increment_(1.0 / 125.0), start_time_(time_increment_), num_measurements_(20) { + const ii::ImuIntegratorParams params = ii::DefaultImuIntegratorParams(); + imu_integrator_.reset(new ii::ImuIntegrator(params)); + } + + void SetUp() final {} + + void SetAndAddMeasurements(const Eigen::Vector3d& acceleration, const Eigen::Vector3d& angular_velocity) { + acceleration_ = acceleration; + angular_velocity_ = angular_velocity; + imu_measurements_ = + ii::ConstantMeasurements(acceleration_, angular_velocity_, num_measurements_, start_time_, time_increment_); + for (const auto& imu_measurement : imu_measurements_) { + imu_integrator_->AddImuMeasurement(imu_measurement); + } + } + + void SetAndAddMeasurements(const double acceleration = 0, const double angular_velocity = 0) { + SetAndAddMeasurements(Eigen::Vector3d(acceleration, acceleration, acceleration), + Eigen::Vector3d(angular_velocity, angular_velocity, angular_velocity)); + } + + void TestAccelerationOnly(const TestParams& params) { + SetAndAddMeasurements(params.acceleration, params.angular_velocity); + const lc::CombinedNavState initial_state( + params.initial_pose, params.initial_velocity, + gtsam::imuBias::ConstantBias(params.accelerometer_bias, params.gyroscope_bias), params.integration_start_time); + const auto imu_augmented_state = imu_integrator().ExtrapolateLatest(initial_state); + ASSERT_TRUE(imu_augmented_state); + const double duration = Duration() - params.integration_start_time; + EXPECT_NEAR(imu_augmented_state->timestamp(), Duration(), 1e-6); + const Eigen::Vector3d expected_velocity = AccelerationOnlyIntegratedVelocity(params, duration); + EXPECT_MATRIX_NEAR(imu_augmented_state->velocity(), expected_velocity, 1e-6); + const Eigen::Vector3d expected_position = AccelerationOnlyIntegratedPosition(params, duration); + EXPECT_MATRIX_NEAR(imu_augmented_state->pose().translation(), expected_position, 1e-6); + EXPECT_MATRIX_NEAR(imu_augmented_state->bias().accelerometer(), initial_state.bias().accelerometer(), 1e-6); + EXPECT_MATRIX_NEAR(imu_augmented_state->bias().gyroscope(), initial_state.bias().gyroscope(), 1e-6); + } + + void Test(const TestParams& params) { + SetAndAddMeasurements(params.acceleration, params.angular_velocity); + const lc::CombinedNavState initial_state( + params.initial_pose, params.initial_velocity, + gtsam::imuBias::ConstantBias(params.accelerometer_bias, params.gyroscope_bias), params.integration_start_time); + + const auto imu_augmented_state = imu_integrator().ExtrapolateLatest(initial_state); + ASSERT_TRUE(imu_augmented_state); + + const Eigen::Vector3d corrected_angular_velocity = + params.body_T_sensor.rotation() * (params.angular_velocity - params.gyroscope_bias); + const Eigen::Vector3d centrifugal_acceleration = + corrected_angular_velocity.cross(corrected_angular_velocity.cross(params.body_T_sensor.translation())); + const Eigen::Vector3d corrected_acceleration = + params.body_T_sensor.rotation() * (params.acceleration - params.accelerometer_bias) - centrifugal_acceleration; + Eigen::Vector3d velocity = params.initial_velocity; + gtsam::Pose3 pose = params.initial_pose; + for (const auto& imu_measurement : imu_measurements()) { + if (imu_measurement.timestamp <= params.integration_start_time) continue; + // Catch float equality + if (std::abs(imu_measurement.timestamp - params.integration_start_time) < 1e-6) continue; + const Eigen::Matrix3d relative_orientation = + (gtsam::Rot3::Expmap(corrected_angular_velocity * time_increment())).matrix(); + const Eigen::Vector3d relative_velocity = + pose.rotation() * (corrected_acceleration * time_increment()) + params.gravity * time_increment(); + // Convert velocity to body frame + const Eigen::Vector3d relative_translation = + pose.rotation().inverse() * (velocity * time_increment()) + + + pose.rotation().inverse() * (0.5 * params.gravity * time_increment() * time_increment()) + + 0.5 * corrected_acceleration * time_increment() * time_increment(); + velocity += relative_velocity; + const Eigen::Isometry3d relative_pose = lc::Isometry3d(relative_translation, relative_orientation); + pose = pose * lc::GtPose(relative_pose); + } + EXPECT_MATRIX_NEAR(imu_augmented_state->velocity(), velocity, 1e-6); + EXPECT_MATRIX_NEAR(imu_augmented_state->pose(), pose, 1e-6); + } + + ii::ImuIntegrator& imu_integrator() { return *imu_integrator_; } + + void SetImuIntegrator(const ii::ImuIntegratorParams& params) { imu_integrator_.reset(new ii::ImuIntegrator(params)); } + + const Eigen::Vector3d& acceleration() const { return acceleration_; } + + const Eigen::Vector3d& angular_velocity() const { return angular_velocity_; } + + const std::vector& imu_measurements() const { return imu_measurements_; } + + double time_increment() const { return time_increment_; } + + double Duration() const { return num_measurements_ * time_increment_; } + + double start_time() const { return start_time_; } + + int num_measurements() const { return num_measurements_; } + + private: + std::unique_ptr imu_integrator_; + Eigen::Vector3d acceleration_; + Eigen::Vector3d angular_velocity_; + std::vector imu_measurements_; + const double time_increment_; + const lc::Time start_time_; + const int num_measurements_; +}; + +TEST_F(ConstantIMUTest, ConstVelocity) { + TestParams params; + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstVelocityNonZeroAccBias) { + TestParams params; + params.accelerometer_bias = lc::RandomVector3d(); + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAcc) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccHalfMeasurements) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.integration_start_time = Duration() / 2; + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccNonZeroAccBias) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.accelerometer_bias = lc::RandomVector3d(); + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAngularVelocity) { + TestParams params; + params.angular_velocity = lc::RandomVector3d(); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAngularVelocityNonZeroAngBias) { + TestParams params; + params.angular_velocity = lc::RandomVector3d(); + params.gyroscope_bias = lc::RandomVector3d(); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccelerationAngularVelocity) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.angular_velocity = lc::RandomVector3d(); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccelerationAngularVelocityGravity) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.angular_velocity = lc::RandomVector3d(); + params.gravity = lc::RandomVector3d(); + auto integrator_params = ii::DefaultImuIntegratorParams(); + integrator_params.gravity = params.gravity; + SetImuIntegrator(integrator_params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccelerationAngularVelocityGravitySensorOffset) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.angular_velocity = lc::RandomVector3d(); + params.gravity = lc::RandomVector3d(); + params.body_T_sensor = lc::RandomPose(); + auto integrator_params = ii::DefaultImuIntegratorParams(); + integrator_params.gravity = params.gravity; + integrator_params.body_T_imu = params.body_T_sensor; + SetImuIntegrator(integrator_params); + Test(params); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/imu_integration/test/test_imu_integrator.test b/localization/imu_integration/test/test_imu_integrator.test new file mode 100644 index 0000000000..1f067076eb --- /dev/null +++ b/localization/imu_integration/test/test_imu_integrator.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/interest_point/CMakeLists.txt b/localization/interest_point/CMakeLists.txt index ac23ad3d52..acdf0dd57f 100644 --- a/localization/interest_point/CMakeLists.txt +++ b/localization/interest_point/CMakeLists.txt @@ -15,52 +15,84 @@ # License for the specific language governing permissions and limitations # under the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0) project(interest_point) -set(LIBS - ${GLOG_LIBRARIES} - ${GFLAGS_LIBRARIES} - ${OpenCV_LIBRARIES} - ${OPENMVG_LIBRARIES} - ${CERES_LIBRARY} - ) -set(INCLUDES - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${CMAKE_SOURCE_DIR}/common/include - ${CERES_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${OPENMVG_INCLUDE_DIRS} - ${GFLAGS_INCLUDE_DIRS} - ${GLOG_INCLUDE_DIRS} +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp ) +# System dependencies are found with CMake's conventions +find_package(cmake_modules REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(OpenMVG QUIET REQUIRED) + +# Find OpenCV +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV4WithXFeatures REQUIRED) + catkin_package( INCLUDE_DIRS include - LIBRARIES interest_point + LIBRARIES interest_point ${OpenCV_LIBRARIES} CATKIN_DEPENDS # DEPENDS system_lib ) -create_library(TARGET interest_point - LIBS ${LIBS} - INC ${INCLUDES} -) - -set(INTEREST_LIBS - interest_point - ff_common - ${LIBS}) +########### +## Build ## +########### -create_tool_targets(DIR tools - LIBS ${INTEREST_LIBS} - INC ${INCLUDES} +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${OpenCV_INCLUDE_DIRS} + ${OPENMVG_INCLUDE_DIRS} ) -create_test_targets(DIR test - LIBS ${INTEREST_LIBS} - INC ${INCLUDES} + +# Declare C++ libraries +add_library(interest_point + src/agast_score.cc + src/brisk.cc + src/essential.cc + src/matching.cc + src/BAD.cpp ) +add_dependencies(interest_point ${catkin_EXPORTED_TARGETS}) +target_link_libraries(interest_point ${OPENMVG_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_rostest_gtest(test_matching + test/test_matching.test + test/test_matching.cc + ) + target_link_libraries(test_matching + interest_point glog + ) -set(INTEREST_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include PARENT_SCOPE) +endif() +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/interest_point/include/interest_point/BAD.h b/localization/interest_point/include/interest_point/BAD.h new file mode 100644 index 0000000000..8cf3ae96aa --- /dev/null +++ b/localization/interest_point/include/interest_point/BAD.h @@ -0,0 +1,104 @@ +/** + * @copyright 2021 Xoan Iago Suarez Canosa. All rights reserved. + * Constact: iago.suarez@thegraffter.com + * Software developed in the PhD: Low-level vision for resource-limited devices + * Repo link: https://github.com/iago-suarez/efficient-descriptors + * License: Apache 2 + * TODO(rsoussan): Remove this and BAD.cpp and switch to opencv TEBLID if opencv + * version upgraded. + */ + +#ifndef INTEREST_POINT_BAD_H_ +#define INTEREST_POINT_BAD_H_ + +#include + +#include +#include + +namespace upm { + +/** + * Implementation of the Box Average Difference (BAD) descriptor. The method uses features + * computed from the difference of the average gray values of two boxes in the patch. + * + * Each pair of boxes is represented with a BoxPairParams struct. After obtaining the feature + * from them, the i'th feature is thresholded with thresholds_[i], producing the binary + * weak-descriptor. + */ +class BAD : public cv::Feature2D { + public: + /** + * @brief Descriptor number of bits, each bit is a weak-descriptor. + * The user can choose between 512 or 256 bits. + */ + enum BadSize { + SIZE_512_BITS = 100, SIZE_256_BITS = 101, + }; + + /** + * @param scale_factor Adjust the sampling window around detected keypoints: + - 1.00f should be the scale for ORB keypoints + - 6.75f should be the scale for SIFT detected keypoints + - 6.25f is default and fits for KAZE, SURF detected keypoints + - 5.00f should be the scale for AKAZE, MSD, AGAST, FAST, BRISK keypoints + * @param n_bits Determine the number of bits in the descriptor. Should be either + BAD::SIZE_512_BITS or BAD::SIZE_256_BITS. + */ + explicit BAD(float scale_factor = 1.0f, BadSize n_bits = SIZE_512_BITS); + + /** + * @brief Creates the BAD descriptor. + * @param scale_factor Adjust the sampling window around detected keypoints: + - 1.00f should be the scale for ORB keypoints + - 6.75f should be the scale for SIFT detected keypoints + - 6.25f is default and fits for KAZE, SURF detected keypoints + - 5.00f should be the scale for AKAZE, MSD, AGAST, FAST, BRISK keypoints + * @param n_bits + * @return + */ + static cv::Ptr create(float scale_factor = 1.0f, BadSize n_bits = SIZE_512_BITS) { + return cv::makePtr(scale_factor, n_bits); + } + + /** @brief Computes the descriptors for a set of keypoints detected in an image (first variant) or image set + (second variant). + + @param image Image. + @param keypoints Input collection of keypoints. Keypoints for which a descriptor cannot be + computed are removed. Sometimes new keypoints can be added, for example: SIFT duplicates keypoint + with several dominant orientations (for each orientation). + @param descriptors Computed descriptors. In the second variant of the method descriptors[i] are + descriptors computed for a keypoints[i]. Row j is the keypoints (or keypoints[i]) is the + descriptor for keypoint j-th keypoint. + */ + void compute(cv::InputArray image, + CV_OUT CV_IN_OUT std::vector &keypoints, + cv::OutputArray descriptors) override; + + int descriptorSize() const override { return box_params_.size() / 8; } + int descriptorType() const override { return CV_8UC1; } + int defaultNorm() const override { return cv::NORM_HAMMING; } + bool empty() const override { return false; } + + cv::String getDefaultName() const override { return std::string("BAD") + std::to_string(box_params_.size()); } + + // Struct representing a pair of boxes in the patch + struct BoxPairParams { + int x1, x2, y1, y2, boxRadius; + }; + + protected: + // Computes the BADdescriptor + void computeBAD(const cv::Mat &integral_img, + const std::vector &keypoints, + cv::Mat &descriptors); + + std::vector thresholds_; + std::vector box_params_; + float scale_factor_ = 1; + cv::Size patch_size_ = {32, 32}; +}; + +} // namespace upm +#endif // INTEREST_POINT_BAD_H_ diff --git a/localization/interest_point/include/interest_point/essential.h b/localization/interest_point/include/interest_point/essential.h index 0d86d4990c..89d37ae71a 100644 --- a/localization/interest_point/include/interest_point/essential.h +++ b/localization/interest_point/include/interest_point/essential.h @@ -27,22 +27,17 @@ namespace interest_point { // Performs a robust, ransac, solving for the essential matrix // between interest point measurements in x1 and x2. - bool RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, - Eigen::Matrix2Xd const& x1, Eigen::Matrix2Xd const& x2, - Eigen::Matrix3d * e, - std::vector * vec_inliers, - std::pair const& size1, - std::pair const& size2, - double * error_max, - double precision); +bool RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, Eigen::Matrix2Xd const& x1, + Eigen::Matrix2Xd const& x2, Eigen::Matrix3d* e, std::vector* vec_inliers, + std::pair const& size1, std::pair const& size2, double* error_max, + double precision, const int ransac_iterations = 4096); - // Solves for the RT (Rotation and Translation) from the essential - // matrix and x1 and x2. There are 4 possible, and this returns the - // best of the 4 solutions. - bool EstimateRTFromE(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, - Eigen::Matrix2Xd const& x1, Eigen::Matrix2Xd const& x2, - Eigen::Matrix3d const& e, std::vector const& vec_inliers, - Eigen::Matrix3d * r, Eigen::Vector3d * t); +// Solves for the RT (Rotation and Translation) from the essential +// matrix and x1 and x2. There are 4 possible, and this returns the +// best of the 4 solutions. +bool EstimateRTFromE(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, Eigen::Matrix2Xd const& x1, + Eigen::Matrix2Xd const& x2, Eigen::Matrix3d const& e, std::vector const& vec_inliers, + Eigen::Matrix3d* r, Eigen::Vector3d* t); } // end namespace interest_point diff --git a/localization/interest_point/include/interest_point/matching.h b/localization/interest_point/include/interest_point/matching.h index e2c2040e9f..aeb38dbf80 100644 --- a/localization/interest_point/include/interest_point/matching.h +++ b/localization/interest_point/include/interest_point/matching.h @@ -18,6 +18,7 @@ #ifndef INTEREST_POINT_MATCHING_H_ #define INTEREST_POINT_MATCHING_H_ +#include #include #include @@ -30,7 +31,8 @@ namespace interest_point { class DynamicDetector { public: DynamicDetector(int min_features, int max_features, int retries, - double min_thresh, double default_thresh, double max_thresh); + double min_thresh, double default_thresh, double max_thresh, + double too_many_ratio, double too_few_ratio); virtual ~DynamicDetector(void) {} void Detect(const cv::Mat& image, std::vector* keypoints, @@ -42,12 +44,15 @@ namespace interest_point { cv::Mat* keypoints_description) = 0; virtual void TooFew(void) = 0; virtual void TooMany(void) = 0; - void GetDetectorParams(int & min_features, int & max_features, int & max_retries, - double & min_thresh, double & default_thresh, double & max_thresh); + void GetDetectorParams(int& min_features, int& max_features, int& max_retries, double& min_thresh, + double& default_thresh, double& max_thresh, double& too_many_ratio, double& too_few_ratio); + + int last_keypoint_count(void) { return last_keypoint_count_; } protected: - unsigned int min_features_, max_features_, max_retries_; - double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_; + int min_features_, max_features_, max_retries_; + double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_, too_few_ratio_, too_many_ratio_; + int last_keypoint_count_; }; class FeatureDetector { @@ -61,36 +66,40 @@ namespace interest_point { public: // Here on purpose invalid values are set, so the user explicitly sets them. - FeatureDetector(std::string const& detector_name = "SURF", - int min_features = 0, int max_features = 0, int retries = 0, - double min_thresh = 0, double default_thresh = 0, double max_thresh = 0); + FeatureDetector(std::string const& detector_name = "SURF", int min_features = 0, int max_features = 0, + int retries = 0, double min_thresh = 0, double default_thresh = 0, double max_thresh = 0, + double too_many_ratio = 0, double too_few_ratio = 0); ~FeatureDetector(void); - void Reset(std::string const& detector_name, - int min_features = 0, int max_features = 0, int retries = 0, - double min_thresh = 0, double default_thresh = 0, double max_thresh = 0); + void Reset(std::string const& detector_name, int min_features = 0, int max_features = 0, int retries = 0, + double min_thresh = 0, double default_thresh = 0, double max_thresh = 0, double too_many_ratio = 0, + double too_few_ratio = 0); void Detect(const cv::Mat& image, std::vector* keypoints, cv::Mat* keypoints_description); std::string GetDetectorName() const {return detector_name_;} - void GetDetectorParams(int & min_features, int & max_features, int & max_retries, - double & min_thresh, double & default_thresh, double & max_thresh); + void GetDetectorParams(int& min_features, int& max_features, int& max_retries, double& min_thresh, + double& default_thresh, double& max_thresh, double& too_many_ratio, double& too_few_ratio); friend bool operator== (FeatureDetector const& A, FeatureDetector const& B) { return (A.detector_name_ == B.detector_name_); } + + DynamicDetector& dynamic_detector() { return *detector_; } }; /** * descriptor is what opencv descriptor was used to make the descriptors * the descriptor maps are the features in the two images * matches is output to contain the matching features between the two images + * Optionally pass hamming distance and goodness ratio, otherwise flag + * values are used. **/ - void FindMatches(const cv::Mat & img1_descriptor_map, - const cv::Mat & img2_descriptor_map, - std::vector * matches); + void FindMatches(const cv::Mat& img1_descriptor_map, const cv::Mat& img2_descriptor_map, + std::vector* matches, boost::optional hamming_distance = boost::none, + boost::optional goodness_ratio = boost::none); } // namespace interest_point #endif // INTEREST_POINT_MATCHING_H_ diff --git a/localization/interest_point/src/BAD.cpp b/localization/interest_point/src/BAD.cpp new file mode 100644 index 0000000000..47835952a9 --- /dev/null +++ b/localization/interest_point/src/BAD.cpp @@ -0,0 +1,638 @@ +/** + * @copyright 2021 Xoan Iago Suarez Canosa. All rights reserved. + * Constact: iago.suarez@thegraffter.com + * Software developed in the PhD: Low-level vision for resource-limited devices + */ +#include +#include +#include + +#define UPM_DEGREES_TO_RADS 0.017453292519943295 +#define UPM_ROUNDNUM(x) (static_cast(x + 0.5f)) +#define UPM_BAD_EXTRA_RATIO_MARGIN 1.75 + +namespace upm { +/** + * @brief Rectifies the coordinates of the box pairs measurement functions with the keypoint + * location parameters. + * @param boxes_params The input parameters defining the location of each pair of boxes inside + * the normalized (32x32) patch. + * @param out_params The output parameters, now the boxes are located in the full image. + * @param kp The keypoint defining the offset, rotation and scale to be applied + * @param scale_factor A scale factor that magnifies the measurement functions w.r.t. the keypoint. + * @param patch_size The size of the normalized patch where the measurement functions were learnt. + */ +static inline void rectifyBoxes(const std::vector &boxes_params, + std::vector &out_params, + const cv::KeyPoint &kp, + float scale_factor = 1, + const cv::Size &patch_size = cv::Size(32, 32)) { + float m00, m01, m02, m10, m11, m12; + float s, cosine, sine; + + s = scale_factor * kp.size / (0.5f * (patch_size.width + patch_size.height)); + out_params.resize(boxes_params.size()); + + if (kp.angle == -1) { + m00 = s; + m01 = 0.0f; + m02 = -0.5f * s * patch_size.width + kp.pt.x; + m10 = 0.0f; + m11 = s; + m12 = -s * 0.5f * patch_size.height + kp.pt.y; + } else { + cosine = (kp.angle >= 0) ? static_cast(cos(kp.angle * UPM_DEGREES_TO_RADS)) : 1.f; + sine = (kp.angle >= 0) ? static_cast(sin(kp.angle * UPM_DEGREES_TO_RADS)) : 0.f; + + m00 = s * cosine; + m01 = -s * sine; + m02 = (-s * cosine + s * sine) * patch_size.width * 0.5f + kp.pt.x; + m10 = s * sine; + m11 = s * cosine; + m12 = (-s * sine - s * cosine) * patch_size.height * 0.5f + kp.pt.y; + } + + for (size_t i = 0; i < boxes_params.size(); i++) { + out_params[i].x1 = UPM_ROUNDNUM(m00 * boxes_params[i].x1 + m01 * boxes_params[i].y1 + m02); + out_params[i].y1 = UPM_ROUNDNUM(m10 * boxes_params[i].x1 + m11 * boxes_params[i].y1 + m12); + out_params[i].x2 = UPM_ROUNDNUM(m00 * boxes_params[i].x2 + m01 * boxes_params[i].y2 + m02); + out_params[i].y2 = UPM_ROUNDNUM(m10 * boxes_params[i].x2 + m11 * boxes_params[i].y2 + m12); + out_params[i].boxRadius = UPM_ROUNDNUM(s * boxes_params[i].boxRadius); + } +} + +/** + * @brief Computes the Box Average Difference, measuring the difference of gray level in the two + * square regions. + * @param box_params The weak-learner parameter defining the size and locations of each box. + * @param integral_img The integral image used to compute the average gray value in the square regions. + * @return The difference of gray level in the two squares defined by box_params + */ +static inline float computeBadResponse(const BAD::BoxPairParams &box_params, + const cv::Mat &integral_img) { + assert(!integral_img.empty()); + assert(integral_img.type() == CV_32SC1); + + int frame_w, frame_h, box1x1, box1y1, box1x2, box1y2, box2x1, box2y1, box2x2, box2y2; + int A, B, C, D; + int box_area1, box_area2; + float sum1, sum2, average1, average2; + // Since the integral image has one extra row and col, calculate the patch dimensions + frame_w = integral_img.cols; + frame_h = integral_img.rows; + + // For the first box, we calculate its margin coordinates + box1x1 = box_params.x1 - box_params.boxRadius; + if (box1x1 < 0) + box1x1 = 0; + else if (box1x1 >= frame_w - 1) + box1x1 = frame_w - 2; + box1y1 = box_params.y1 - box_params.boxRadius; + if (box1y1 < 0) + box1y1 = 0; + else if (box1y1 >= frame_h - 1) + box1y1 = frame_h - 2; + box1x2 = box_params.x1 + box_params.boxRadius + 1; + if (box1x2 <= 0) + box1x2 = 1; + else if (box1x2 >= frame_w) + box1x2 = frame_w - 1; + box1y2 = box_params.y1 + box_params.boxRadius + 1; + if (box1y2 <= 0) + box1y2 = 1; + else if (box1y2 >= frame_h) + box1y2 = frame_h - 1; + assert((box1x1 < box1x2 && box1y1 < box1y2) && "Box 1 has size 0"); + + // For the second box, we calculate its margin coordinates + box2x1 = box_params.x2 - box_params.boxRadius; + if (box2x1 < 0) + box2x1 = 0; + else if (box2x1 >= frame_w - 1) + box2x1 = frame_w - 2; + box2y1 = box_params.y2 - box_params.boxRadius; + if (box2y1 < 0) + box2y1 = 0; + else if (box2y1 >= frame_h - 1) + box2y1 = frame_h - 2; + box2x2 = box_params.x2 + box_params.boxRadius + 1; + if (box2x2 <= 0) + box2x2 = 1; + else if (box2x2 >= frame_w) + box2x2 = frame_w - 1; + box2y2 = box_params.y2 + box_params.boxRadius + 1; + if (box2y2 <= 0) + box2y2 = 1; + else if (box2y2 >= frame_h) + box2y2 = frame_h - 1; + assert((box2x1 < box2x2 && box2y1 < box2y2) && "Box 2 has size 0"); + + // Read the integral image values for the first box + A = integral_img.at(box1y1, box1x1); + B = integral_img.at(box1y1, box1x2); + C = integral_img.at(box1y2, box1x1); + D = integral_img.at(box1y2, box1x2); + + // Calculate the mean intensity value of the pixels in the box + sum1 = static_cast(A + D - B - C); + box_area1 = (box1y2 - box1y1) * (box1x2 - box1x1); + assert(box_area1 > 0); + average1 = sum1 / box_area1; + + // Calculate the indices on the integral image where the box falls + A = integral_img.at(box2y1, box2x1); + B = integral_img.at(box2y1, box2x2); + C = integral_img.at(box2y2, box2x1); + D = integral_img.at(box2y2, box2x2); + + // Calculate the mean intensity value of the pixels in the box + sum2 = static_cast(A + D - B - C); + box_area2 = (box2y2 - box2y1) * (box2x2 - box2x1); + assert(box_area2 > 0); + average2 = sum2 / box_area2; + + return average1 - average2; +} + +/** + * @brief Function that determines if a keypoint is close to the image border. + * @param kp The detected keypoint + * @param img_size The size of the image + * @param patch_size The size of the normalized patch where the measurement functions were learnt. + * @param scale_factor A scale factor that magnifies the measurement functions w.r.t. the keypoint. + * @return true if the keypoint is in the border, false otherwise + */ +static inline bool isKeypointInTheBorder(const cv::KeyPoint &kp, + const cv::Size &img_size, + const cv::Size &patch_size = {32, 32}, + float scale_factor = 1) { + // This would be the correct measure but since we will compare with half of the size, use this as border size + float s = scale_factor * kp.size / (patch_size.width + patch_size.height); + cv::Size2f border(patch_size.width * s * UPM_BAD_EXTRA_RATIO_MARGIN, + patch_size.height * s * UPM_BAD_EXTRA_RATIO_MARGIN); + + if (kp.pt.x < border.width || kp.pt.x + border.width >= img_size.width) + return true; + + if (kp.pt.y < border.height || kp.pt.y + border.height >= img_size.height) + return true; + + return false; +} + +/////////////////////////////////////////////////////////////////////////////// + +BAD::BAD(float scale_factor, BadSize n_bits) { + scale_factor_ = scale_factor; + + if (n_bits == SIZE_512_BITS) { + // With data augmentation + // box_params_ = {{17,18,12,15,2}, {13,14,5,7,5}, {21,16,16,14,1}, {27,11,18,20,3}, {17,13,16,19,2}, + // {18,24,18,16,5}, {12,11,10,25,6}, {14,17,14,13,1}, {7,4,4,15,4}, {27,27,23,8,4}, {19,13,19,6,6}, {14,15,10,16,1}, + // {13,15,12,22,1}, {8,22,3,27,3}, {13,19,8,13,1}, {18,16,17,12,1}, {27,7,25,11,4}, {24,20,20,15,2}, {16,24,14,3,3}, + // {23,18,7,18,7}, {8,7,2,1,1}, {17,28,17,26,3}, {17,13,17,10,2}, {10,18,10,11,1}, {11,28,7,22,2}, {18,13,15,15,1}, + // {7,14,3,20,3}, {17,19,14,15,1}, {14,12,14,8,2}, {14,12,13,11,1}, {21,9,19,19,2}, {4,28,3,10,3}, {27,27,26,26,4}, + // {19,22,19,19,2}, {12,25,12,20,1}, {19,12,15,12,1}, {28,21,23,21,2}, {10,15,7,18,2}, {12,7,10,3,3}, + // {21,16,19,15,1}, {19,20,18,17,1}, {26,2,19,7,2}, {18,2,15,22,2}, {24,26,24,22,5}, {15,26,15,19,1}, + // {13,19,11,20,1}, {5,14,4,10,4}, {15,7,15,4,2}, {13,16,11,7,1}, {15,22,15,18,1}, {24,8,23,4,4}, {13,11,11,14,1}, + // {4,19,3,19,3}, {22,12,19,10,1}, {24,27,15,22,2}, {12,13,10,10,1}, {11,25,9,29,2}, {15,21,15,10,1}, + // {19,16,18,19,1}, {29,13,24,8,2}, {17,16,16,20,1}, {12,17,12,15,1}, {28,4,2,11,2}, {7,25,5,19,3}, {22,13,20,16,1}, + // {14,16,13,17,1}, {10,3,8,11,3}, {18,7,17,11,2}, {27,11,25,22,2}, {5,26,3,28,3}, {28,13,27,13,3}, {22,20,20,28,3}, + // {12,6,5,2,2}, {14,18,13,16,1}, {17,29,3,25,2}, {20,20,19,19,1}, {15,12,14,15,1}, {12,14,12,13,1}, + // {17,14,10,26,3}, {11,15,6,12,6}, {9,22,9,19,1}, {19,18,19,14,1}, {23,15,12,18,2}, {12,15,11,14,1}, {28,2,27,9,2}, + // {11,19,11,11,7}, {13,29,13,23,2}, {27,19,22,17,3}, {17,3,17,2,2}, {4,6,3,3,3}, {19,15,16,16,1}, {22,5,20,9,2}, + // {14,6,13,9,2}, {17,16,13,16,2}, {24,18,12,6,6}, {20,14,18,15,2}, {20,9,18,13,1}, {18,20,17,8,2}, {10,15,9,15,2}, + // {13,7,12,26,2}, {13,12,11,19,2}, {15,2,2,29,2}, {15,12,14,13,1}, {20,30,19,26,1}, {28,26,28,4,3}, + // {16,13,15,12,1}, {18,11,17,25,2}, {3,17,1,24,1}, {21,18,19,22,1}, {9,13,9,8,2}, {19,18,16,16,1}, {21,22,17,20,1}, + // {13,4,13,3,3}, {24,15,21,9,1}, {24,25,19,17,6}, {4,14,3,14,2}, {17,13,14,19,1}, {7,19,4,16,3}, {4,20,1,5,1}, + // {15,13,12,14,3}, {19,26,19,21,2}, {11,26,10,18,5}, {17,16,17,13,1}, {19,16,19,11,1}, {4,26,4,23,4}, + // {14,19,14,13,5}, {10,13,8,13,2}, {14,12,14,10,1}, {29,24,26,19,2}, {26,9,19,19,5}, {16,23,16,17,1}, {4,13,3,4,3}, + // {13,16,7,21,2}, {17,16,16,17,1}, {29,15,5,18,2}, {29,2,23,5,2}, {9,17,9,14,2}, {25,26,25,22,5}, {13,21,13,20,1}, + // {23,12,7,20,6}, {6,8,6,3,3}, {13,19,13,17,1}, {25,21,22,20,1}, {24,17,23,15,2}, {20,8,17,4,1}, {11,19,10,17,1}, + // {9,11,6,9,1}, {25,9,24,14,1}, {18,20,13,14,3}, {26,23,25,23,5}, {14,20,11,4,4}, {28,7,25,13,3}, {13,13,12,12,1}, + // {7,29,2,2,2}, {16,17,16,8,5}, {20,6,19,12,3}, {19,7,19,6,6}, {20,13,19,14,1}, {19,24,16,29,2}, {8,15,4,13,1}, + // {7,9,2,10,2}, {15,14,14,13,1}, {18,13,18,11,1}, {8,19,5,23,2}, {3,13,1,14,1}, {23,20,16,14,1}, {17,15,13,18,2}, + // {16,16,9,14,5}, {15,28,15,27,3}, {18,20,16,19,1}, {16,17,16,11,2}, {30,1,10,19,1}, {12,19,9,23,2}, + // {25,13,21,13,1}, {9,23,5,24,5}, {13,20,13,18,1}, {13,13,12,13,3}, {29,18,25,2,2}, {30,30,25,26,1}, + // {16,20,15,11,1}, {18,16,18,14,1}, {15,18,5,7,4}, {16,13,15,19,1}, {26,24,16,9,5}, {1,28,1,5,1}, {20,17,20,16,1}, + // {15,19,10,17,4}, {12,9,10,5,1}, {30,29,28,29,1}, {29,17,27,18,2}, {17,29,15,27,2}, {9,29,9,28,2}, + // {23,24,21,22,1}, {22,2,1,1,1}, {20,4,20,1,1}, {5,30,4,25,1}, {20,8,17,12,7}, {10,7,3,17,3}, {21,17,14,15,5}, + // {14,10,13,8,1}, {4,21,4,13,3}, {30,1,24,10,1}, {15,17,14,16,3}, {21,23,20,15,3}, {17,20,17,18,3}, {12,11,12,6,5}, + // {15,15,12,17,1}, {25,9,16,25,6}, {22,28,22,27,3}, {5,8,3,3,3}, {9,5,9,1,1}, {30,12,29,23,1}, {20,21,5,9,5}, + // {15,21,15,20,1}, {11,17,10,23,2}, {16,11,15,13,1}, {16,12,16,10,1}, {15,6,14,3,3}, {2,4,1,1,1}, {15,16,11,15,1}, + // {24,6,24,2,2}, {8,15,6,12,1}, {21,27,1,30,1}, {17,10,14,16,3}, {13,9,7,7,7}, {22,17,19,17,1}, {16,14,14,13,2}, + // {14,21,13,23,1}, {18,2,15,7,2}, {3,25,1,24,1}, {24,20,7,14,7}, {26,25,24,19,2}, {6,25,6,23,6}, {15,24,15,17,7}, + // {22,14,16,15,1}, {17,25,17,23,1}, {12,18,2,26,2}, {30,30,26,11,1}, {22,8,16,14,5}, {9,16,8,20,1}, {4,14,2,13,2}, + // {28,7,27,8,1}, {10,22,9,24,1}, {14,16,13,18,3}, {28,26,3,15,2}, {12,15,10,15,1}, {18,17,17,15,1}, + // {30,10,28,14,1}, {30,14,28,30,1}, {30,18,7,13,1}, {3,19,2,20,1}, {16,19,14,13,2}, {11,9,5,27,4}, {16,19,15,15,2}, + // {24,22,18,19,7}, {12,17,12,12,1}, {28,5,28,1,1}, {4,29,2,30,1}, {27,11,27,8,1}, {8,3,8,1,1}, {15,10,15,8,3}, + // {12,27,11,18,4}, {25,6,22,8,6}, {15,3,15,2,2}, {19,22,17,19,1}, {24,21,24,16,2}, {9,7,6,6,6}, {13,26,11,27,2}, + // {24,10,19,12,4}, {22,17,22,9,2}, {17,14,14,11,1}, {13,4,13,3,1}, {15,18,15,17,1}, {29,30,29,24,1}, + // {29,29,20,17,2}, {6,12,2,27,2}, {18,17,14,13,2}, {11,27,11,26,4}, {22,12,3,18,3}, {15,13,13,9,1}, {12,20,7,18,1}, + // {16,6,15,9,1}, {3,6,1,7,1}, {12,17,11,19,1}, {15,8,8,18,7}, {11,19,11,5,3}, {17,20,16,23,3}, {12,6,9,13,1}, + // {2,1,1,2,1}, {14,26,13,21,3}, {25,16,16,14,3}, {30,14,29,14,1}, {27,25,15,22,4}, {13,10,8,7,2}, {18,19,13,14,1}, + // {28,28,28,22,3}, {8,14,8,11,1}, {23,28,22,24,2}, {8,2,3,18,2}, {22,24,22,23,7}, {20,17,15,16,1}, {8,11,6,4,4}, + // {25,13,23,13,2}, {18,18,16,15,1}, {20,16,16,15,1}, {18,20,14,26,3}, {17,12,17,8,1}, {1,5,1,3,1}, {22,13,13,20,2}, + // {17,16,17,14,3}, {27,17,25,17,2}, {8,23,6,29,2}, {15,4,14,18,1}, {10,24,10,17,4}, {25,30,25,28,1}, {3,22,1,29,1}, + // {24,8,23,17,1}, {26,3,26,1,1}, {18,22,18,17,2}, {9,17,8,10,2}, {29,22,29,2,2}, {19,4,5,10,3}, {3,28,3,27,1}, + // {12,15,11,18,1}, {30,3,28,4,1}, {7,9,7,8,1}, {24,15,8,14,7}, {30,6,20,16,1}, {18,18,1,10,1}, {30,20,28,21,1}, + // {15,15,13,14,1}, {6,3,5,1,1}, {3,8,1,17,1}, {3,2,2,2,2}, {19,28,18,20,1}, {20,20,20,17,2}, {21,30,19,29,1}, + // {12,19,12,13,1}, {29,10,29,4,2}, {20,16,20,14,1}, {15,9,11,16,2}, {8,13,6,26,4}, {13,11,12,8,2}, {17,27,17,26,4}, + // {29,29,14,12,1}, {29,2,28,3,2}, {9,15,7,9,4}, {27,28,12,30,1}, {14,30,2,28,1}, {19,12,18,14,1}, {26,5,24,15,5}, + // {2,24,2,2,2}, {6,21,5,21,1}, {22,16,9,17,2}, {16,19,15,17,1}, {2,29,2,28,2}, {25,11,24,1,1}, {16,30,16,29,1}, + // {14,20,14,17,3}, {15,14,11,17,3}, {18,17,16,21,1}, {17,8,17,4,2}, {11,4,11,3,3}, {25,16,9,17,6}, {18,8,18,6,6}, + // {17,22,17,19,1}, {8,20,3,11,3}, {20,17,4,17,1}, {29,12,12,19,2}, {14,29,14,28,2}, {12,18,10,18,1}, + // {13,15,13,11,2}, {18,15,14,15,2}, {19,17,17,19,1}, {22,17,12,16,6}, {30,22,29,18,1}, {30,2,29,20,1}, + // {12,3,1,1,1}, {4,7,1,7,1}, {27,10,21,13,4}, {18,21,18,13,3}, {12,4,3,6,2}, {12,10,9,3,2}, {3,28,2,29,2}, + // {22,2,20,5,2}, {27,18,20,3,3}, {6,24,6,23,1}, {27,26,9,16,4}, {5,18,5,11,5}, {20,14,15,12,3}, {19,16,19,15,1}, + // {27,4,21,9,4}, {3,19,2,29,1}, {20,24,18,22,1}, {18,7,18,2,1}, {28,30,28,28,1}, {11,24,10,9,1}, {21,18,21,14,3}, + // {27,19,26,18,2}, {16,18,10,6,6}, {11,18,5,19,1}, {24,16,22,16,1}, {17,15,17,9,5}, {27,29,20,11,2}, + // {29,25,28,22,1}, {21,11,21,5,1}, {12,15,8,16,2}, {2,29,1,30,1}, {18,12,4,21,3}, {18,9,11,13,3}, {18,3,10,21,3}, + // {17,11,16,16,1}, {15,17,13,14,1}, {7,7,7,5,5}, {9,29,5,18,2}, {10,11,10,6,6}, {28,26,25,26,1}, {19,30,8,20,1}, + // {8,15,7,29,2}, {21,18,19,17,1}, {2,22,1,22,1}, {12,20,4,17,1}, {27,8,4,14,2}, {26,10,25,13,1}, {19,13,19,8,3}, + // {12,16,7,18,7}, {20,26,12,3,3}, {6,10,3,10,2}, {25,25,25,21,2}, {12,3,7,16,2}, {8,4,4,17,4}, {12,20,5,8,5}, + // {22,15,8,13,7}, {12,13,12,8,2}, {20,15,19,13,1}, {30,5,29,8,1}, {14,29,13,23,2}, {18,19,9,10,7}, {2,11,1,10,1}, + // {12,13,12,11,1}, {27,15,9,5,4}, {13,12,7,17,2}, {8,17,1,26,1}, {20,24,11,12,4}, {12,24,10,22,6}, {19,29,14,20,1}, + // {20,27,20,25,2}, {9,25,8,27,1}, {7,11,5,11,1}, {20,11,11,8,1}, {9,8,9,5,1}, {27,9,25,10,1}, {30,20,22,20,1}, + // {26,21,26,20,1}, {30,14,27,16,1}, {12,16,11,19,3}, {7,28,6,29,1}, {17,23,17,22,2}, {12,17,2,2,1}, + // {17,14,17,13,1}, {18,12,16,16,1}, {7,23,7,17,1}, {25,12,9,15,4}, {16,6,16,5,5}, {8,16,7,16,7}, {6,7,5,7,5}, + // {15,13,15,12,2}, {13,15,13,13,3}, {16,12,16,11,1}, {18,15,15,14,3}, {17,8,14,5,4}, {9,26,6,22,5}, + // {17,16,14,17,3}, {25,1,24,2,1}, {14,16,14,15,1}, {24,22,4,23,4}, {30,29,27,29,1}, {17,18,17,17,1}, + // {19,30,19,28,1}, {21,27,21,23,3}, {16,18,15,20,1}, {27,27,13,12,4}, {30,25,27,26,1}, {4,21,3,7,1}, {10,5,10,4,4}, + // {14,14,5,3,1}, {23,6,21,3,3}, {9,20,2,15,2}, {23,9,20,13,1}, {15,14,12,3,3}, {19,25,19,18,4}, {27,25,24,22,4}, + // {15,15,15,11,1}, {17,16,14,13,1}, {12,18,12,17,1}, {30,3,30,2,1}, {21,20,18,28,3}, {25,25,7,14,5}, {3,11,2,3,2}, + // {25,5,9,21,4}, {6,15,4,28,3}, {9,9,3,3,3}, {16,19,14,16,2}, {10,25,10,20,1}, {2,17,2,15,1}, {17,15,15,16,1}, + // {20,15,19,15,1}, {22,2,22,1,1}, {15,19,15,18,1}, {15,16,10,12,1}, {28,2,23,14,2}, {11,3,9,2,1}}; thresholds_ = + // {14.45,4.15,7.75,9.65,2.25,0.15,0.45,-0.95,3.65,-1.75,1.05,5.45,-0.05,-2.65,3.35,1.65,-1.55,2.85,3.05,22.05, + // -3.65,-0.15,-0.55,-0.05,3.25,-2.85,-1.25,10.45,-1.05,1.25,3.15,2.05,-0.55,-1.25,3.45,4.35,2.45,2.55,1.35,1.25, + // 2.75,-0.15,8.35,0.35,-1.25,0.75,-0.45,-0.05,0.85,3.65,1.55,1.75,-0.35,1.35,4.85,-2.25,0.25,-2.15,3.35,1.95,6.25, + // 0.35,24.45,-1.15,0.85,-1.95,5.15,1.35,0.85,0.35,-0.45,4.95,-0.25,2.45,11.75,0.85,-1.65,0.05,4.05,-0.35,-0.95, + // -0.25,49.35,0.85,1.95,0.25,1.15,-2.65,-0.25,0.85,-5.65,1.15,3.05,4.05,7.35,9.05,0.35,1.65,1.65,2.55,6.95,1.75, + // 0.85,-1.15,1.35,5.45,1.35,-2.35,-0.15,0.85,-3.05,0.05,-0.35,-0.65,11.65,-0.85,1.15,2.35,-9.25,16.05,-1.45,1.95, + // 0.35,-0.35,0.15,0.25,-1.35,0.65,-4.05,-2.25,1.05,-0.05,-1.55,0.25,69.45,0.15,-1.25,-1.85,-0.65,8.75,-0.95,1.95, + // 1.05,-1.45,2.15,-1.85,-1.75,-2.95,2.65,0.65,-1.05,4.35,0.25,19.65,0.35,1.65,0.65,2.75,2.85,-10.95,3.65,-4.15, + // 0.25,-0.65,-2.25,3.75,35.75,3.15,-0.55,1.95,-6.55,88.35,7.25,1.75,-4.15,0.25,0.25,0.65,3.75,1.65,2.85,42.15, + // 11.75, -1.25,-8.25,0.05,2.15,0.65,-1.55,-2.75,1.15,-0.15,-0.75,16.85,1.15,1.45,15.35,19.45,8.65,-1.25,0.25, + // 2.15,0.15,2.85, -2.25,1.75,-15.15,4.15,-0.45,-8.25,1.25,-0.85,3.55,0.45,-1.25,9.65,1.25,3.15,-2.05,-6.85,-0.05, + // 3.25,7.05,2.35, -1.45,-1.05,20.25,1.15,1.35,1.05,5.15,2.55,0.55,0.55,-0.15,0.65,-28.35,3.85,2.05,-1.15,3.05, + // -0.95,-1.55,-0.35, 101.95,0.35,-2.05,-2.15,4.25,105.15,1.15,27.45,30.15,18.45,-2.45,1.85,2.95,-3.75,-0.65,0.05, + // 0.25,34.45,-2.05, 0.25,3.25,2.05,2.85,3.45,20.95,1.45,-1.95,-0.35,-6.85,1.15,-31.95,11.85,2.75,0.15,8.35,1.15, + // 2.15,-1.15,-1.05, 2.45,1.45,0.15,-1.65,41.85,0.35,0.65,-0.35,0.25,56.75,23.55,88.85,-7.25,1.05,0.75,-1.05,-1.65, + // -9.75,-16.95,-0.05, -10.05,34.15,14.75,-0.95,1.45,4.85,-0.05,-0.35,0.75,64.75,24.25,-0.35,-13.65,2.85,1.75, + // -0.35,0.25,-12.15,108.05, -0.35,-3.35,1.95,-0.35,21.25,-20.05,95.85,-1.05,-17.75,-0.55,2.75,0.65,0.75,-1.85, + // 2.65,-2.15,1.05,-0.05,4.25,3.75, -13.55,-0.05,105.95,0.45,3.05,9.35,79.25,1.75,7.65,-0.45,0.95,79.15,28.05,0.05, + // 0.65,-0.35,0.95,2.05,1.15,2.75, -1.65,8.65,3.55,-0.25,-28.95,-7.35,122.25,0.55,4.75,1.75,11.15,0.15,2.55,0.05, + // -7.95,51.05,6.25,0.05,-1.15,0.75, 26.65,-1.25,1.15,-0.55,0.05,5.85,1.55,2.25,-1.65,-16.85,-62.65,-1.25,-1.05, + // -1.55,1.35,0.25,-0.65,0.85,17.45, 0.25, -9.35,34.35,0.95,-0.95,-10.05,1.45,-6.05,93.25,3.15,-10.85,-1.65,-0.15, + // -2.45,-0.35,0.15,118.15,-81.85,4.05,1.05, -0.85,152.35,0.65,1.35,-12.35,95.35,-23.25,-0.75,75.65,50.85,47.85, + // 2.55,0.55,0.15,0.25,38.15,-2.65,-1.95,-1.05, 110.25,35.25,2.65,3.65,18.75,89.45,-0.55,0.35,0.25,105.05,0.25, + // 1.25,-34.65,-0.55,-0.15,0.75,-0.15,-0.15,-94.25, -12.05,-15.35,-1.75,31.35,0.15,-2.15,-0.15,-12.05,-0.35,-0.65, + // -0.55,28.95,39.05,7.05,0.65,-1.65,2.85,4.85,-1.85, 1.75,-31.65,0.05,13.75,-0.35,0.35,-5.75,83.85,0.95,27.95, + // -0.25,-19.05,2.45,-13.25,44.05,51.85,-0.05,0.05,79.35,111.25,-61.65,3.05,-69.95,35.65,62.05,-0.25,-1.05,-2.95, + // 0.95,-0.15,-18.15,-18.65,74.55,2.65}; + + // Without data augmentation + box_params_ = { + {17, 12, 18, 15, 2}, {13, 5, 14, 7, 5}, {21, 16, 16, 14, 1}, {27, 18, 11, 20, 3}, {17, 16, 13, 19, 2}, + {18, 18, 24, 16, 5}, {12, 10, 11, 25, 6}, {14, 14, 17, 13, 1}, {7, 4, 4, 15, 4}, {27, 23, 27, 8, 4}, + {19, 19, 13, 6, 6}, {14, 10, 15, 16, 1}, {13, 12, 15, 22, 1}, {8, 3, 22, 27, 3}, {13, 8, 19, 13, 1}, + {18, 17, 16, 12, 1}, {27, 25, 7, 11, 4}, {24, 20, 20, 15, 2}, {16, 14, 24, 3, 3}, {23, 7, 18, 18, 7}, + {8, 2, 7, 1, 1}, {17, 17, 28, 26, 3}, {17, 17, 13, 10, 2}, {10, 10, 18, 11, 1}, {11, 7, 28, 22, 2}, + {18, 15, 13, 15, 1}, {7, 3, 14, 20, 3}, {17, 14, 19, 15, 1}, {14, 14, 12, 8, 2}, {14, 13, 12, 11, 1}, + {21, 19, 9, 19, 2}, {4, 3, 28, 10, 3}, {27, 26, 27, 26, 4}, {19, 19, 22, 19, 2}, {12, 12, 25, 20, 1}, + {19, 15, 12, 12, 1}, {28, 23, 21, 21, 2}, {10, 7, 15, 18, 2}, {12, 10, 7, 3, 3}, {21, 19, 16, 15, 1}, + {19, 18, 20, 17, 1}, {26, 19, 2, 7, 2}, {18, 15, 2, 22, 2}, {24, 24, 26, 22, 5}, {15, 15, 26, 19, 1}, + {13, 11, 19, 20, 1}, {5, 4, 14, 10, 4}, {15, 15, 7, 4, 2}, {13, 11, 16, 7, 1}, {15, 15, 22, 18, 1}, + {24, 23, 8, 4, 4}, {13, 11, 11, 14, 1}, {4, 3, 19, 19, 3}, {22, 19, 12, 10, 1}, {24, 15, 27, 22, 2}, + {12, 10, 13, 10, 1}, {11, 9, 25, 29, 2}, {15, 15, 21, 10, 1}, {19, 18, 16, 19, 1}, {29, 24, 13, 8, 2}, + {17, 16, 16, 20, 1}, {12, 12, 17, 15, 1}, {28, 2, 4, 11, 2}, {7, 5, 25, 19, 3}, {22, 20, 13, 16, 1}, + {14, 13, 16, 17, 1}, {10, 8, 3, 11, 3}, {18, 17, 7, 11, 2}, {27, 25, 11, 22, 2}, {5, 3, 26, 28, 3}, + {28, 27, 13, 13, 3}, {22, 20, 20, 28, 3}, {12, 5, 6, 2, 2}, {14, 13, 18, 16, 1}, {17, 3, 29, 25, 2}, + {20, 19, 20, 19, 1}, {15, 14, 12, 15, 1}, {12, 12, 14, 13, 1}, {17, 10, 14, 26, 3}, {11, 6, 15, 12, 6}, + {9, 9, 22, 19, 1}, {19, 19, 18, 14, 1}, {23, 12, 15, 18, 2}, {12, 11, 15, 14, 1}, {28, 27, 2, 9, 2}, + {11, 11, 19, 11, 7}, {13, 13, 29, 23, 2}, {27, 22, 19, 17, 3}, {17, 17, 3, 2, 2}, {4, 3, 6, 3, 3}, + {19, 16, 15, 16, 1}, {22, 20, 5, 9, 2}, {14, 13, 6, 9, 2}, {17, 13, 16, 16, 2}, {24, 12, 18, 6, 6}, + {20, 18, 14, 15, 2}, {20, 18, 9, 13, 1}, {18, 17, 20, 8, 2}, {10, 9, 15, 15, 2}, {13, 12, 7, 26, 2}, + {13, 11, 12, 19, 2}, {15, 2, 2, 29, 2}, {15, 14, 12, 13, 1}, {20, 19, 30, 26, 1}, {28, 28, 26, 4, 3}, + {16, 15, 13, 12, 1}, {18, 17, 11, 25, 2}, {3, 1, 17, 24, 1}, {21, 19, 18, 22, 1}, {9, 9, 13, 8, 2}, + {19, 16, 18, 16, 1}, {21, 17, 22, 20, 1}, {13, 13, 4, 3, 3}, {24, 21, 15, 9, 1}, {24, 19, 25, 17, 6}, + {4, 3, 14, 14, 2}, {17, 14, 13, 19, 1}, {7, 4, 19, 16, 3}, {4, 1, 20, 5, 1}, {15, 12, 13, 14, 3}, + {19, 19, 26, 21, 2}, {11, 10, 26, 18, 5}, {17, 17, 16, 13, 1}, {19, 19, 16, 11, 1}, {4, 4, 26, 23, 4}, + {14, 14, 19, 13, 5}, {10, 8, 13, 13, 2}, {14, 14, 12, 10, 1}, {29, 26, 24, 19, 2}, {26, 19, 9, 19, 5}, + {16, 16, 23, 17, 1}, {4, 3, 13, 4, 3}, {13, 7, 16, 21, 2}, {17, 16, 16, 17, 1}, {29, 5, 15, 18, 2}, + {29, 23, 2, 5, 2}, {9, 9, 17, 14, 2}, {25, 25, 26, 22, 5}, {13, 13, 21, 20, 1}, {23, 7, 12, 20, 6}, + {6, 6, 8, 3, 3}, {13, 13, 19, 17, 1}, {25, 22, 21, 20, 1}, {24, 23, 17, 15, 2}, {20, 17, 8, 4, 1}, + {11, 10, 19, 17, 1}, {9, 6, 11, 9, 1}, {25, 24, 9, 14, 1}, {18, 13, 20, 14, 3}, {26, 25, 23, 23, 5}, + {14, 11, 20, 4, 4}, {28, 25, 7, 13, 3}, {13, 12, 13, 12, 1}, {7, 2, 29, 2, 2}, {16, 16, 17, 8, 5}, + {20, 19, 6, 12, 3}, {19, 19, 7, 6, 6}, {20, 19, 13, 14, 1}, {19, 16, 24, 29, 2}, {8, 4, 15, 13, 1}, + {7, 2, 9, 10, 2}, {15, 14, 14, 13, 1}, {18, 18, 13, 11, 1}, {8, 5, 19, 23, 2}, {3, 1, 13, 14, 1}, + {23, 16, 20, 14, 1}, {17, 13, 15, 18, 2}, {16, 9, 16, 14, 5}, {15, 15, 28, 27, 3}, {18, 16, 20, 19, 1}, + {16, 16, 17, 11, 2}, {30, 10, 1, 19, 1}, {12, 9, 19, 23, 2}, {25, 21, 13, 13, 1}, {9, 5, 23, 24, 5}, + {13, 13, 20, 18, 1}, {13, 12, 13, 13, 3}, {29, 25, 18, 2, 2}, {30, 25, 30, 26, 1}, {16, 15, 20, 11, 1}, + {18, 18, 16, 14, 1}, {15, 5, 18, 7, 4}, {16, 15, 13, 19, 1}, {26, 16, 24, 9, 5}, {1, 1, 28, 5, 1}, + {20, 20, 17, 16, 1}, {15, 10, 19, 17, 4}, {12, 10, 9, 5, 1}, {30, 28, 29, 29, 1}, {29, 27, 17, 18, 2}, + {17, 15, 29, 27, 2}, {9, 9, 29, 28, 2}, {23, 21, 24, 22, 1}, {22, 1, 2, 1, 1}, {20, 20, 4, 1, 1}, + {5, 4, 30, 25, 1}, {20, 17, 8, 12, 7}, {10, 3, 7, 17, 3}, {21, 14, 17, 15, 5}, {14, 13, 10, 8, 1}, + {4, 4, 21, 13, 3}, {30, 24, 1, 10, 1}, {15, 14, 17, 16, 3}, {21, 20, 23, 15, 3}, {17, 17, 20, 18, 3}, + {12, 12, 11, 6, 5}, {15, 12, 15, 17, 1}, {25, 16, 9, 25, 6}, {22, 22, 28, 27, 3}, {5, 3, 8, 3, 3}, + {9, 9, 5, 1, 1}, {30, 29, 12, 23, 1}, {20, 5, 21, 9, 5}, {15, 15, 21, 20, 1}, {11, 10, 17, 23, 2}, + {16, 15, 11, 13, 1}, {16, 16, 12, 10, 1}, {15, 14, 6, 3, 3}, {2, 1, 4, 1, 1}, {15, 11, 16, 15, 1}, + {24, 24, 6, 2, 2}, {8, 6, 15, 12, 1}, {21, 1, 27, 30, 1}, {17, 14, 10, 16, 3}, {13, 7, 9, 7, 7}, + {22, 19, 17, 17, 1}, {16, 14, 14, 13, 2}, {14, 13, 21, 23, 1}, {18, 15, 2, 7, 2}, {3, 1, 25, 24, 1}, + {24, 7, 20, 14, 7}, {26, 24, 25, 19, 2}, {6, 6, 25, 23, 6}, {15, 15, 24, 17, 7}, {22, 16, 14, 15, 1}, + {17, 17, 25, 23, 1}, {12, 2, 18, 26, 2}, {30, 26, 30, 11, 1}, {22, 16, 8, 14, 5}, {9, 8, 16, 20, 1}, + {4, 2, 14, 13, 2}, {28, 27, 7, 8, 1}, {10, 9, 22, 24, 1}, {14, 13, 16, 18, 3}, {28, 3, 26, 15, 2}, + {12, 10, 15, 15, 1}, {18, 17, 17, 15, 1}, {30, 28, 10, 14, 1}, {30, 28, 14, 30, 1}, {30, 7, 18, 13, 1}, + {3, 2, 19, 20, 1}, {16, 14, 19, 13, 2}, {11, 5, 9, 27, 4}, {16, 15, 19, 15, 2}, {24, 18, 22, 19, 7}, + {12, 12, 17, 12, 1}, {28, 28, 5, 1, 1}, {4, 2, 29, 30, 1}, {27, 27, 11, 8, 1}, {8, 8, 3, 1, 1}, + {15, 15, 10, 8, 3}, {12, 11, 27, 18, 4}, {25, 22, 6, 8, 6}, {15, 15, 3, 2, 2}, {19, 17, 22, 19, 1}, + {24, 24, 21, 16, 2}, {9, 6, 7, 6, 6}, {13, 11, 26, 27, 2}, {24, 19, 10, 12, 4}, {22, 22, 17, 9, 2}, + {17, 14, 14, 11, 1}, {13, 13, 4, 3, 1}, {15, 15, 18, 17, 1}, {29, 29, 30, 24, 1}, {29, 20, 29, 17, 2}, + {6, 2, 12, 27, 2}, {18, 14, 17, 13, 2}, {11, 11, 27, 26, 4}, {22, 3, 12, 18, 3}, {15, 13, 13, 9, 1}, + {12, 7, 20, 18, 1}, {16, 15, 6, 9, 1}, {3, 1, 6, 7, 1}, {12, 11, 17, 19, 1}, {15, 8, 8, 18, 7}, + {11, 11, 19, 5, 3}, {17, 16, 20, 23, 3}, {12, 9, 6, 13, 1}, {2, 1, 1, 2, 1}, {14, 13, 26, 21, 3}, + {25, 16, 16, 14, 3}, {30, 29, 14, 14, 1}, {27, 15, 25, 22, 4}, {13, 8, 10, 7, 2}, {18, 13, 19, 14, 1}, + {28, 28, 28, 22, 3}, {8, 8, 14, 11, 1}, {23, 22, 28, 24, 2}, {8, 3, 2, 18, 2}, {22, 22, 24, 23, 7}, + {20, 15, 17, 16, 1}, {8, 6, 11, 4, 4}, {25, 23, 13, 13, 2}, {18, 16, 18, 15, 1}, {20, 16, 16, 15, 1}, + {18, 14, 20, 26, 3}, {17, 17, 12, 8, 1}, {1, 1, 5, 3, 1}, {22, 13, 13, 20, 2}, {17, 17, 16, 14, 3}, + {27, 25, 17, 17, 2}, {8, 6, 23, 29, 2}, {15, 14, 4, 18, 1}, {10, 10, 24, 17, 4}, {25, 25, 30, 28, 1}, + {3, 1, 22, 29, 1}, {24, 23, 8, 17, 1}, {26, 26, 3, 1, 1}, {18, 18, 22, 17, 2}, {9, 8, 17, 10, 2}, + {29, 29, 22, 2, 2}, {19, 5, 4, 10, 3}, {3, 3, 28, 27, 1}, {12, 11, 15, 18, 1}, {30, 28, 3, 4, 1}, + {7, 7, 9, 8, 1}, {24, 8, 15, 14, 7}, {30, 20, 6, 16, 1}, {18, 1, 18, 10, 1}, {30, 28, 20, 21, 1}, + {15, 13, 15, 14, 1}, {6, 5, 3, 1, 1}, {3, 1, 8, 17, 1}, {3, 2, 2, 2, 2}, {19, 18, 28, 20, 1}, + {20, 20, 20, 17, 2}, {21, 19, 30, 29, 1}, {12, 12, 19, 13, 1}, {29, 29, 10, 4, 2}, {20, 20, 16, 14, 1}, + {15, 11, 9, 16, 2}, {8, 6, 13, 26, 4}, {13, 12, 11, 8, 2}, {17, 17, 27, 26, 4}, {29, 14, 29, 12, 1}, + {29, 28, 2, 3, 2}, {9, 7, 15, 9, 4}, {27, 12, 28, 30, 1}, {14, 2, 30, 28, 1}, {19, 18, 12, 14, 1}, + {26, 24, 5, 15, 5}, {2, 2, 24, 2, 2}, {6, 5, 21, 21, 1}, {22, 9, 16, 17, 2}, {16, 15, 19, 17, 1}, + {2, 2, 29, 28, 2}, {25, 24, 11, 1, 1}, {16, 16, 30, 29, 1}, {14, 14, 20, 17, 3}, {15, 11, 14, 17, 3}, + {18, 16, 17, 21, 1}, {17, 17, 8, 4, 2}, {11, 11, 4, 3, 3}, {25, 9, 16, 17, 6}, {18, 18, 8, 6, 6}, + {17, 17, 22, 19, 1}, {8, 3, 20, 11, 3}, {20, 4, 17, 17, 1}, {29, 12, 12, 19, 2}, {14, 14, 29, 28, 2}, + {12, 10, 18, 18, 1}, {13, 13, 15, 11, 2}, {18, 14, 15, 15, 2}, {19, 17, 17, 19, 1}, {22, 12, 17, 16, 6}, + {30, 29, 22, 18, 1}, {30, 29, 2, 20, 1}, {12, 1, 3, 1, 1}, {4, 1, 7, 7, 1}, {27, 21, 10, 13, 4}, + {18, 18, 21, 13, 3}, {12, 3, 4, 6, 2}, {12, 9, 10, 3, 2}, {3, 2, 28, 29, 2}, {22, 20, 2, 5, 2}, + {27, 20, 18, 3, 3}, {6, 6, 24, 23, 1}, {27, 9, 26, 16, 4}, {5, 5, 18, 11, 5}, {20, 15, 14, 12, 3}, + {19, 19, 16, 15, 1}, {27, 21, 4, 9, 4}, {3, 2, 19, 29, 1}, {20, 18, 24, 22, 1}, {18, 18, 7, 2, 1}, + {28, 28, 30, 28, 1}, {11, 10, 24, 9, 1}, {21, 21, 18, 14, 3}, {27, 26, 19, 18, 2}, {16, 10, 18, 6, 6}, + {11, 5, 18, 19, 1}, {24, 22, 16, 16, 1}, {17, 17, 15, 9, 5}, {27, 20, 29, 11, 2}, {29, 28, 25, 22, 1}, + {21, 21, 11, 5, 1}, {12, 8, 15, 16, 2}, {2, 1, 29, 30, 1}, {18, 4, 12, 21, 3}, {18, 11, 9, 13, 3}, + {18, 10, 3, 21, 3}, {17, 16, 11, 16, 1}, {15, 13, 17, 14, 1}, {7, 7, 7, 5, 5}, {9, 5, 29, 18, 2}, + {10, 10, 11, 6, 6}, {28, 25, 26, 26, 1}, {19, 8, 30, 20, 1}, {8, 7, 15, 29, 2}, {21, 19, 18, 17, 1}, + {2, 1, 22, 22, 1}, {12, 4, 20, 17, 1}, {27, 4, 8, 14, 2}, {26, 25, 10, 13, 1}, {19, 19, 13, 8, 3}, + {12, 7, 16, 18, 7}, {20, 12, 26, 3, 3}, {6, 3, 10, 10, 2}, {25, 25, 25, 21, 2}, {12, 7, 3, 16, 2}, + {8, 4, 4, 17, 4}, {12, 5, 20, 8, 5}, {22, 8, 15, 13, 7}, {12, 12, 13, 8, 2}, {20, 19, 15, 13, 1}, + {30, 29, 5, 8, 1}, {14, 13, 29, 23, 2}, {18, 9, 19, 10, 7}, {2, 1, 11, 10, 1}, {12, 12, 13, 11, 1}, + {27, 9, 15, 5, 4}, {13, 7, 12, 17, 2}, {8, 1, 17, 26, 1}, {20, 11, 24, 12, 4}, {12, 10, 24, 22, 6}, + {19, 14, 29, 20, 1}, {20, 20, 27, 25, 2}, {9, 8, 25, 27, 1}, {7, 5, 11, 11, 1}, {20, 11, 11, 8, 1}, + {9, 9, 8, 5, 1}, {27, 25, 9, 10, 1}, {30, 22, 20, 20, 1}, {26, 26, 21, 20, 1}, {30, 27, 14, 16, 1}, + {12, 11, 16, 19, 3}, {7, 6, 28, 29, 1}, {17, 17, 23, 22, 2}, {12, 2, 17, 2, 1}, {17, 17, 14, 13, 1}, + {18, 16, 12, 16, 1}, {7, 7, 23, 17, 1}, {25, 9, 12, 15, 4}, {16, 16, 6, 5, 5}, {8, 7, 16, 16, 7}, + {6, 5, 7, 7, 5}, {15, 15, 13, 12, 2}, {13, 13, 15, 13, 3}, {16, 16, 12, 11, 1}, {18, 15, 15, 14, 3}, + {17, 14, 8, 5, 4}, {9, 6, 26, 22, 5}, {17, 14, 16, 17, 3}, {25, 24, 1, 2, 1}, {14, 14, 16, 15, 1}, + {24, 4, 22, 23, 4}, {30, 27, 29, 29, 1}, {17, 17, 18, 17, 1}, {19, 19, 30, 28, 1}, {21, 21, 27, 23, 3}, + {16, 15, 18, 20, 1}, {27, 13, 27, 12, 4}, {30, 27, 25, 26, 1}, {4, 3, 21, 7, 1}, {10, 10, 5, 4, 4}, + {14, 5, 14, 3, 1}, {23, 21, 6, 3, 3}, {9, 2, 20, 15, 2}, {23, 20, 9, 13, 1}, {15, 12, 14, 3, 3}, + {19, 19, 25, 18, 4}, {27, 24, 25, 22, 4}, {15, 15, 15, 11, 1}, {17, 14, 16, 13, 1}, {12, 12, 18, 17, 1}, + {30, 30, 3, 2, 1}, {21, 18, 20, 28, 3}, {25, 7, 25, 14, 5}, {3, 2, 11, 3, 2}, {25, 9, 5, 21, 4}, + {6, 4, 15, 28, 3}, {9, 3, 9, 3, 3}, {16, 14, 19, 16, 2}, {10, 10, 25, 20, 1}, {2, 2, 17, 15, 1}, + {17, 15, 15, 16, 1}, {20, 19, 15, 15, 1}, {22, 22, 2, 1, 1}, {15, 15, 19, 18, 1}, {15, 10, 16, 12, 1}, + {28, 23, 2, 14, 2}, {11, 9, 3, 2, 1}, + }; + thresholds_ = { + 14.45, 4.15, 7.75, 9.65, 2.25, 0.15, 0.45, -0.95, 3.65, -1.75, 1.05, 5.45, -0.05, -2.65, + 3.35, 1.65, -1.55, 2.85, 3.05, 22.05, -3.65, -0.15, -0.55, -0.05, 3.25, -2.85, -1.25, 10.45, + -1.05, 1.25, 3.15, 2.05, -0.55, -1.25, 3.45, 4.35, 2.45, 2.55, 1.35, 1.25, 2.75, -0.15, + 8.35, 0.35, -1.25, 0.75, -0.45, -0.05, 0.85, 3.65, 1.55, 1.75, -0.35, 1.35, 4.85, -2.25, + 0.25, -2.15, 3.35, 1.95, 6.25, 0.35, 24.45, -1.15, 0.85, -1.95, 5.15, 1.35, 0.85, 0.35, + -0.45, 4.95, -0.25, 2.45, 11.75, 0.85, -1.65, 0.05, 4.05, -0.35, -0.95, -0.25, 49.35, 0.85, + 1.95, 0.25, 1.15, -2.65, -0.25, 0.85, -5.65, 1.15, 3.05, 4.05, 7.35, 9.05, 0.35, 1.65, + 1.65, 2.55, 6.95, 1.75, 0.85, -1.15, 1.35, 5.45, 1.35, -2.35, -0.15, 0.85, -3.05, 0.05, + -0.35, -0.65, 11.65, -0.85, 1.15, 2.35, -9.25, 16.05, -1.45, 1.95, 0.35, -0.35, 0.15, 0.25, + -1.35, 0.65, -4.05, -2.25, 1.05, -0.05, -1.55, 0.25, 69.45, 0.15, -1.25, -1.85, -0.65, 8.75, + -0.95, 1.95, 1.05, -1.45, 2.15, -1.85, -1.75, -2.95, 2.65, 0.65, -1.05, 4.35, 0.25, 19.65, + 0.35, 1.65, 0.65, 2.75, 2.85, -10.95, 3.65, -4.15, 0.25, -0.65, -2.25, 3.75, 35.75, 3.15, + -0.55, 1.95, -6.55, 88.35, 7.25, 1.75, -4.15, 0.25, 0.25, 0.65, 3.75, 1.65, 2.85, 42.15, + 11.75, -1.25, -8.25, 0.05, 2.15, 0.65, -1.55, -2.75, 1.15, -0.15, -0.75, 16.85, 1.15, 1.45, + 15.35, 19.45, 8.65, -1.25, 0.25, 2.15, 0.15, 2.85, -2.25, 1.75, -15.15, 4.15, -0.45, -8.25, + 1.25, -0.85, 3.55, 0.45, -1.25, 9.65, 1.25, 3.15, -2.05, -6.85, -0.05, 3.25, 7.05, 2.35, + -1.45, -1.05, 20.25, 1.15, 1.35, 1.05, 5.15, 2.55, 0.55, 0.55, -0.15, 0.65, -28.35, 3.85, + 2.05, -1.15, 3.05, -0.95, -1.55, -0.35, 101.95, 0.35, -2.05, -2.15, 4.25, 105.15, 1.15, 27.45, + 30.15, 18.45, -2.45, 1.85, 2.95, -3.75, -0.65, 0.05, 0.25, 34.45, -2.05, 0.25, 3.25, 2.05, + 2.85, 3.45, 20.95, 1.45, -1.95, -0.35, -6.85, 1.15, -31.95, 11.85, 2.75, 0.15, 8.35, 1.15, + 2.15, -1.15, -1.05, 2.45, 1.45, 0.15, -1.65, 41.85, 0.35, 0.65, -0.35, 0.25, 56.75, 23.55, + 88.85, -7.25, 1.05, 0.75, -1.05, -1.65, -9.75, -16.95, -0.05, -10.05, 34.15, 14.75, -0.95, 1.45, + 4.85, -0.05, -0.35, 0.75, 64.75, 24.25, -0.35, -13.65, 2.85, 1.75, -0.35, 0.25, -12.15, 108.05, + -0.35, -3.35, 1.95, -0.35, 21.25, -20.05, 95.85, -1.05, -17.75, -0.55, 2.75, 0.65, 0.75, -1.85, + 2.65, -2.15, 1.05, -0.05, 4.25, 3.75, -13.55, -0.05, 105.95, 0.45, 3.05, 9.35, 79.25, 1.75, + 7.65, -0.45, 0.95, 79.15, 28.05, 0.05, 0.65, -0.35, 0.95, 2.05, 1.15, 2.75, -1.65, 8.65, + 3.55, -0.25, -28.95, -7.35, 122.25, 0.55, 4.75, 1.75, 11.15, 0.15, 2.55, 0.05, -7.95, 51.05, + 6.25, 0.05, -1.15, 0.75, 26.65, -1.25, 1.15, -0.55, 0.05, 5.85, 1.55, 2.25, -1.65, -16.85, + -62.65, -1.25, -1.05, -1.55, 1.35, 0.25, -0.65, 0.85, 17.45, 0.25, -9.35, 34.35, 0.95, -0.95, + -10.05, 1.45, -6.05, 93.25, 3.15, -10.85, -1.65, -0.15, -2.45, -0.35, 0.15, 118.15, -81.85, 4.05, + 1.05, -0.85, 152.35, 0.65, 1.35, -12.35, 95.35, -23.25, -0.75, 75.65, 50.85, 47.85, 2.55, 0.55, + 0.15, 0.25, 38.15, -2.65, -1.95, -1.05, 110.25, 35.25, 2.65, 3.65, 18.75, 89.45, -0.55, 0.35, + 0.25, 105.05, 0.25, 1.25, -34.65, -0.55, -0.15, 0.75, -0.15, -0.15, -94.25, -12.05, -15.35, -1.75, + 31.35, 0.15, -2.15, -0.15, -12.05, -0.35, -0.65, -0.55, 28.95, 39.05, 7.05, 0.65, -1.65, 2.85, + 4.85, -1.85, 1.75, -31.65, 0.05, 13.75, -0.35, 0.35, -5.75, 83.85, 0.95, 27.95, -0.25, -19.05, + 2.45, -13.25, 44.05, 51.85, -0.05, 0.05, 79.35, 111.25, -61.65, 3.05, -69.95, 35.65, 62.05, -0.25, + -1.05, -2.95, 0.95, -0.15, -18.15, -18.65, 74.55, 2.65}; + } else { + // With data augmentation + // box_params_ = {}; + // thresholds_ = {}; + + // Without data augmentation + box_params_ = { + {25, 13, 14, 15, 6}, {16, 14, 15, 11, 1}, {14, 7, 14, 8, 6}, {10, 6, 9, 20, 6}, {13, 13, 26, 19, 5}, + {19, 19, 14, 5, 4}, {16, 15, 19, 13, 2}, {26, 21, 26, 12, 5}, {18, 15, 23, 20, 2}, {12, 10, 15, 20, 1}, + {26, 18, 4, 8, 3}, {8, 2, 21, 29, 2}, {19, 17, 16, 19, 1}, {10, 5, 3, 13, 3}, {16, 10, 10, 14, 1}, + {19, 18, 12, 17, 1}, {21, 21, 26, 19, 5}, {6, 5, 7, 5, 5}, {22, 20, 12, 14, 2}, {14, 13, 12, 17, 1}, + {11, 10, 16, 13, 2}, {7, 7, 23, 17, 3}, {27, 25, 13, 8, 4}, {20, 16, 19, 14, 1}, {27, 24, 10, 16, 2}, + {13, 13, 12, 6, 2}, {14, 13, 18, 23, 1}, {14, 11, 8, 1, 1}, {14, 12, 23, 9, 2}, {6, 2, 19, 13, 2}, + {8, 6, 19, 19, 3}, {18, 17, 28, 25, 3}, {29, 25, 28, 22, 2}, {15, 15, 19, 17, 3}, {23, 19, 21, 19, 1}, + {20, 20, 20, 16, 3}, {29, 25, 4, 8, 2}, {17, 16, 6, 25, 2}, {12, 8, 21, 29, 1}, {14, 9, 15, 17, 2}, + {18, 17, 5, 3, 3}, {21, 18, 12, 10, 1}, {17, 14, 14, 14, 2}, {5, 3, 26, 6, 3}, {16, 15, 13, 14, 1}, + {28, 24, 21, 22, 3}, {13, 13, 12, 10, 1}, {22, 21, 3, 11, 3}, {27, 4, 27, 16, 4}, {12, 7, 13, 10, 1}, + {15, 15, 25, 22, 2}, {19, 18, 10, 12, 1}, {17, 17, 16, 9, 2}, {21, 21, 17, 14, 2}, {13, 12, 19, 16, 1}, + {11, 9, 11, 15, 1}, {15, 14, 26, 28, 3}, {17, 17, 22, 20, 1}, {10, 2, 26, 27, 2}, {28, 26, 12, 23, 3}, + {4, 3, 5, 14, 3}, {17, 17, 7, 4, 3}, {19, 17, 15, 15, 1}, {7, 2, 8, 5, 2}, {22, 19, 15, 14, 2}, + {15, 12, 16, 20, 1}, {13, 12, 19, 20, 1}, {17, 17, 10, 8, 2}, {26, 19, 16, 15, 4}, {9, 8, 14, 20, 2}, + {27, 27, 14, 4, 4}, {17, 15, 14, 9, 1}, {5, 5, 4, 3, 3}, {15, 9, 30, 5, 1}, {7, 7, 25, 23, 6}, + {12, 11, 24, 16, 1}, {20, 20, 29, 20, 2}, {19, 15, 18, 19, 1}, {9, 7, 11, 11, 7}, {27, 26, 26, 15, 4}, + {10, 10, 28, 27, 3}, {8, 8, 12, 6, 3}, {21, 16, 23, 22, 1}, {22, 4, 7, 25, 4}, {17, 16, 19, 15, 1}, + {28, 11, 21, 15, 3}, {15, 15, 3, 2, 2}, {16, 14, 16, 17, 3}, {10, 7, 17, 18, 3}, {12, 12, 18, 15, 1}, + {18, 16, 16, 13, 1}, {20, 19, 16, 15, 1}, {16, 11, 15, 11, 1}, {4, 2, 14, 13, 2}, {29, 27, 18, 17, 2}, + {16, 14, 18, 16, 1}, {23, 22, 29, 27, 2}, {18, 18, 13, 11, 1}, {26, 21, 23, 27, 4}, {18, 17, 22, 18, 1}, + {3, 2, 11, 21, 2}, {13, 13, 18, 9, 3}, {15, 14, 14, 5, 2}, {1, 1, 14, 1, 1}, {29, 5, 2, 9, 2}, + {12, 11, 17, 17, 1}, {13, 12, 10, 25, 4}, {5, 1, 13, 25, 1}, {13, 13, 16, 12, 1}, {16, 16, 23, 12, 1}, + {27, 22, 14, 14, 2}, {29, 27, 29, 27, 2}, {23, 22, 6, 4, 4}, {22, 22, 16, 8, 3}, {14, 11, 1, 9, 1}, + {12, 10, 11, 8, 2}, {24, 7, 19, 16, 7}, {5, 2, 29, 20, 2}, {19, 19, 15, 13, 1}, {15, 8, 18, 24, 2}, + {4, 1, 24, 30, 1}, {17, 17, 30, 26, 1}, {9, 7, 8, 5, 2}, {15, 15, 20, 18, 1}, {27, 14, 5, 26, 4}, + {18, 18, 19, 15, 1}, {24, 9, 14, 12, 1}, {20, 18, 6, 10, 1}, {21, 21, 23, 21, 1}, {19, 6, 17, 6, 6}, + {10, 6, 13, 12, 3}, {30, 27, 10, 14, 1}, {9, 6, 5, 3, 3}, {26, 18, 21, 19, 2}, {23, 23, 5, 4, 4}, + {14, 11, 11, 12, 1}, {18, 16, 13, 13, 1}, {7, 3, 8, 16, 3}, {16, 16, 15, 12, 2}, {25, 24, 20, 25, 3}, + {20, 19, 14, 14, 1}, {12, 12, 29, 5, 1}, {23, 13, 17, 13, 5}, {27, 23, 27, 22, 4}, {11, 11, 4, 3, 3}, + {9, 7, 18, 15, 1}, {18, 18, 17, 14, 1}, {28, 6, 2, 17, 2}, {5, 3, 20, 22, 3}, {30, 30, 30, 2, 1}, + {16, 15, 8, 13, 1}, {15, 14, 16, 13, 1}, {28, 27, 5, 5, 3}, {13, 12, 13, 12, 1}, {7, 6, 8, 7, 6}, + {10, 10, 21, 17, 1}, {11, 3, 17, 30, 1}, {16, 9, 17, 14, 7}, {17, 9, 16, 14, 1}, {14, 13, 29, 27, 2}, + {19, 19, 5, 3, 2}, {18, 14, 16, 14, 1}, {10, 8, 23, 25, 2}, {17, 15, 17, 18, 1}, {16, 16, 22, 16, 6}, + {29, 27, 11, 11, 2}, {13, 7, 9, 11, 1}, {18, 17, 23, 19, 4}, {12, 11, 14, 17, 1}, {13, 11, 23, 18, 2}, + {27, 23, 8, 20, 4}, {18, 18, 18, 11, 4}, {8, 5, 21, 8, 5}, {23, 21, 5, 10, 1}, {16, 16, 16, 12, 1}, + {18, 14, 17, 19, 1}, {16, 16, 27, 24, 2}, {21, 15, 17, 15, 1}, {16, 15, 5, 9, 2}, {24, 1, 16, 30, 1}, + {15, 14, 14, 19, 1}, {19, 12, 12, 14, 2}, {5, 3, 5, 4, 3}, {16, 16, 11, 9, 1}, {16, 6, 9, 18, 6}, + {25, 23, 24, 14, 1}, {5, 5, 26, 17, 5}, {9, 6, 16, 18, 1}, {29, 9, 25, 24, 2}, {25, 24, 22, 30, 1}, + {22, 20, 2, 5, 2}, {27, 25, 1, 11, 1}, {15, 14, 12, 10, 1}, {17, 16, 6, 8, 1}, {28, 23, 8, 7, 3}, + {24, 23, 24, 22, 7}, {7, 5, 18, 20, 3}, {22, 20, 15, 20, 1}, {30, 28, 21, 20, 1}, {3, 2, 18, 18, 2}, + {6, 5, 14, 15, 1}, {15, 15, 18, 16, 1}, {7, 5, 11, 2, 1}, {17, 13, 17, 15, 3}, {12, 7, 15, 15, 5}, + {16, 15, 12, 18, 1}, {14, 14, 26, 25, 5}, {11, 8, 17, 18, 1}, {23, 15, 13, 21, 7}, {10, 10, 9, 2, 2}, + {17, 12, 13, 19, 1}, {20, 19, 25, 22, 1}, {9, 8, 26, 21, 1}, {19, 19, 22, 18, 1}, {8, 3, 15, 12, 1}, + {26, 16, 13, 19, 5}, {24, 21, 12, 13, 1}, {12, 12, 14, 9, 1}, {3, 1, 7, 1, 1}, {16, 15, 9, 3, 3}, + {23, 23, 20, 8, 7}, {24, 22, 16, 15, 1}, {20, 20, 19, 14, 1}, {30, 29, 27, 22, 1}, {27, 4, 17, 16, 4}, + {8, 5, 13, 13, 5}, {19, 10, 8, 16, 3}, {30, 30, 11, 4, 1}, {14, 14, 21, 20, 1}, {14, 13, 11, 13, 1}, + {30, 28, 2, 5, 1}, {17, 12, 29, 24, 2}, {15, 6, 25, 30, 1}, {4, 1, 1, 1, 1}, {12, 5, 16, 20, 5}, + {16, 14, 20, 15, 1}, {6, 6, 17, 9, 3}, {20, 12, 17, 20, 4}, {15, 12, 15, 4, 4}, {28, 22, 20, 21, 3}, + {14, 9, 18, 18, 5}, {26, 23, 1, 5, 1}, {21, 11, 24, 10, 7}, {15, 14, 19, 12, 1}, {27, 11, 29, 16, 1}, + {23, 22, 19, 29, 1}, {2, 2, 30, 29, 1}, {14, 6, 16, 5, 3}, {17, 14, 13, 16, 1}, {19, 15, 14, 16, 1}, + {20, 13, 25, 15, 6}, {19, 11, 18, 12, 5}, {30, 30, 30, 13, 1}, {3, 1, 14, 9, 1}, {20, 1, 17, 18, 1}, + {16, 12, 20, 19, 1}}; + thresholds_ = { + 21.65, 5.65, 4.95, 2.45, 2.25, 0.85, 3.35, 1.75, 4.55, -1.55, 4.55, -5.05, 3.15, 4.85, 9.95, + 1.35, -2.05, -0.15, 1.55, 3.35, 0.25, 0.35, 2.45, 2.75, -1.65, -0.05, -0.75, 0.85, 2.95, -1.65, + -0.05, -0.25, -3.85, -0.05, 3.35, 0.05, -3.55, 2.65, 1.95, 6.35, 0.85, 2.65, 12.45, 0.05, 3.35, + 1.75, -1.05, -1.05, 28.25, 0.35, -0.15, 2.05, 2.55, 0.85, 1.35, 1.15, 1.25, 1.35, 1.85, 3.95, + 0.75, 1.65, -3.15, -6.35, 2.05, -5.15, 1.75, -0.65, -0.65, 1.05, -0.85, 0.85, -0.35, 9.05, 0.75, + -1.75, 0.75, 16.05, 0.35, 0.75, 0.05, 0.05, 3.75, 14.15, -8.95, 67.25, -0.45, 1.65, -1.95, 1.15, + 1.85, 3.95, -1.75, 0.45, -1.55, 1.05, -0.25, -1.05, 3.05, -1.05, 1.95, -0.05, 0.85, 3.05, 34.85, + -0.15, 4.35, -10.65, 2.35, -1.35, 0.05, 1.05, 1.05, -0.15, 0.45, -0.55, 10.45, 1.35, -0.95, 0.45, + -0.85, 1.45, -1.85, 1.65, 2.75, 1.05, 81.45, 3.35, 0.85, 2.65, 9.35, 1.15, 1.35, -1.55, 0.85, + 20.65, 2.05, 12.85, 7.95, 2.25, 0.05, 0.85, 8.75, -8.25, -0.35, 1.65, -3.95, 92.55, 0.55, 0.35, + -0.75, -12.25, 0.55, 1.05, 0.95, 1.15, -43.25, 3.05, 4.35, 7.15, 0.15, 57.95, 4.35, 0.75, 0.05, + 0.05, 5.45, 0.55, 0.95, 20.55, -4.45, 0.75, 4.55, -0.15, 8.65, 42.65, -0.45, -1.25, -1.75, 11.25, + -8.15, 2.85, -2.85, -5.05, 44.65, 1.45, -0.75, 11.85, 2.05, 1.25, 4.45, -1.35, 5.95, 1.35, -2.55, + 5.05, -2.85, 7.35, -1.35, -0.45, 0.45, -11.85, -39.65, 1.65, -0.05, 3.65, -0.35, 0.05, 1.85, -0.45, + -1.75, 3.95, 5.25, -1.05, -11.95, 37.05, -1.15, 1.25, 0.75, -6.05, -1.55, -1.65, 0.85, 0.35, 101.55, + -5.05, 3.65, -2.35, -0.35, -1.65, 0.65, 6.35, 2.85, 5.25, 24.05, 38.15, -1.05, 3.05, 0.35, -16.05, + -1.25, 0.25, 1.95, -0.85, 107.65, -1.55, -0.25, 26.95, 35.95, -4.85, 1.55, 10.85, -7.15, -4.25, -25.15, + 2.75}; + } +} + +void BAD::compute(cv::InputArray _image, + std::vector &keypoints, + cv::OutputArray &_descriptors) { + cv::Mat image = _image.getMat(); + + if (image.empty()) + return; + + if (keypoints.empty()) { + // clean output buffer (it may be reused with "allocated" data) + _descriptors.release(); + return; + } + + assert(image.type() == CV_8UC1); + cv::Mat integral_img; + + // compute the integral image + cv::integral(image, integral_img); + + // Create the output array of descriptors + _descriptors.create(static_cast(keypoints.size()), descriptorSize(), descriptorType()); + + // descriptor storage + cv::Mat descriptors = _descriptors.getMat(); + assert(descriptors.type() == CV_8UC1); + + computeBAD(integral_img, keypoints, descriptors); +} + +void BAD::computeBAD(const cv::Mat &integral_img, + const std::vector &keypoints, + cv::Mat &descriptors) { + assert(!integral_img.empty()); + assert(descriptors.rows == keypoints.size()); + + const int *integral_ptr = integral_img.ptr(); + cv::Size frame_size(integral_img.cols - 1, integral_img.rows - 1); + + /////////////////// Parallel Loop to process descriptors //////////////////// + +#ifndef UPM_BAD_PARALLEL + const cv::Range range(0, keypoints.size()); +#else + cv::parallel_for_(cv::Range(0, static_cast(keypoints.size())), [&](const cv::Range &range){ // NOLINT +#endif + // Get a pointer to the first element in the range + // Get a pointer to the first element in the range + BoxPairParams *box_pair; + float response_fun; + int area_response_fun, kpIdx; + size_t box_pair_idx; + int box1x1, box1y1, box1x2, box1y2, box2x1, box2y1, box2x2, box2y2, bit_idx, side; + uchar byte = 0; + std::vector img_boxes(box_params_.size()); + uchar *d = &descriptors.at(range.start, 0); + + for (kpIdx = range.start; kpIdx < range.end; kpIdx++) { + // Rectify the weak learners coordinates using the keypoint information + // Rectify the weak learners coordinates using the keypoint information + rectifyBoxes(box_params_, img_boxes, keypoints[kpIdx], scale_factor_, patch_size_); + if (isKeypointInTheBorder(keypoints[kpIdx], frame_size, patch_size_, scale_factor_)) { + // Code to process the keypoints in the image margins + for (box_pair_idx = 0; box_pair_idx < box_params_.size(); box_pair_idx++) { + bit_idx = 7 - static_cast(box_pair_idx % 8); + response_fun = computeBadResponse(img_boxes[box_pair_idx], integral_img); + // Set the bit to 1 if the response function is less or equal to the threshod + byte |= (response_fun <= thresholds_[box_pair_idx]) << bit_idx; + // If we filled the byte, save it + if (bit_idx == 0) { + *d = byte; + byte = 0; + d++; + } + } + } else { + // Code to process the keypoints in the image center + box_pair = img_boxes.data(); + for (box_pair_idx = 0; box_pair_idx < box_params_.size(); box_pair_idx++) { + bit_idx = 7 - static_cast(box_pair_idx % 8); + + // For the first box, we calculate its margin coordinates + box1x1 = box_pair->x1 - box_pair->boxRadius; + box1y1 = (box_pair->y1 - box_pair->boxRadius) * integral_img.cols; + box1x2 = box_pair->x1 + box_pair->boxRadius + 1; + box1y2 = (box_pair->y1 + box_pair->boxRadius + 1) * integral_img.cols; + // For the second box, we calculate its margin coordinates + box2x1 = box_pair->x2 - box_pair->boxRadius; + box2y1 = (box_pair->y2 - box_pair->boxRadius) * integral_img.cols; + box2x2 = box_pair->x2 + box_pair->boxRadius + 1; + box2y2 = (box_pair->y2 + box_pair->boxRadius + 1) * integral_img.cols; + side = 1 + (box_pair->boxRadius << 1); + + // Get the difference between the average level of the two boxes + area_response_fun = (integral_ptr[box1y1 + box1x1] // A of Box1 + + integral_ptr[box1y2 + box1x2] // D of Box1 + - integral_ptr[box1y1 + box1x2] // B of Box1 + - integral_ptr[box1y2 + box1x1] // C of Box1 + - integral_ptr[box2y1 + box2x1] // A of Box2 + - integral_ptr[box2y2 + box2x2] // D of Box2 + + integral_ptr[box2y1 + box2x2] // B of Box2 + + integral_ptr[box2y2 + box2x1]); // C of Box2 + + // Set the bit to 1 if the response function is less or equal to the threshod + byte |= (area_response_fun <= (thresholds_[box_pair_idx] * (side * side))) << bit_idx; + box_pair++; + // If we filled the byte, save it + if (bit_idx == 0) { + *d = byte; + byte = 0; + d++; + } + } // End of for each dimension + } // End of else (of pixels in the image center) + } // End of for each keypoint +#ifdef UPM_BAD_PARALLEL + }); +#endif + } // namespace upm ## NOLINT +} diff --git a/localization/interest_point/src/essential.cc b/localization/interest_point/src/essential.cc index 7fae3debf0..9382ec9bb9 100644 --- a/localization/interest_point/src/essential.cc +++ b/localization/interest_point/src/essential.cc @@ -38,7 +38,7 @@ bool interest_point::RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d std::pair const& size1, std::pair const& size2, double * error_max, - double precision) { + double precision, const int ransac_iterations) { CHECK(e) << "Missing e argument"; CHECK(vec_inliers) << "Missing vec inliers argument"; @@ -53,7 +53,7 @@ bool interest_point::RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d x2, size2.first, size2.second, k1, k2); std::pair ransac_output = - openMVG::robust::ACRANSAC(kernel, *vec_inliers, 4096 /* iterations */, + openMVG::robust::ACRANSAC(kernel, *vec_inliers, ransac_iterations /* iterations */, e, precision, false); *error_max = ransac_output.first; diff --git a/localization/interest_point/src/matching.cc b/localization/interest_point/src/matching.cc index 37ea1ab808..4e3d3234bb 100644 --- a/localization/interest_point/src/matching.cc +++ b/localization/interest_point/src/matching.cc @@ -16,8 +16,9 @@ * under the License. */ -#include +#include #include +#include #include #include @@ -31,10 +32,10 @@ // same settings! // TODO(oalexan1): Ideally the settings used here must be saved in the // map file, for the localize executable to read them from there. -DEFINE_int32(hamming_distance, 90, +DEFINE_int32(hamming_distance, 85, "A smaller value keeps fewer but more reliable binary descriptor matches."); DEFINE_double(goodness_ratio, 0.8, - "A smaller value keeps fewer but more reliable float descriptor matches."); + "A smaller value keeps fewer but more reliable descriptor matches."); DEFINE_int32(orgbrisk_octaves, 4, "Number of octaves, or scale spaces, that BRISK will evaluate."); DEFINE_double(orgbrisk_pattern_scale, 1.0, @@ -54,32 +55,56 @@ DEFINE_double(default_surf_threshold, 10, "Default threshold for feature detection using SURF."); DEFINE_double(max_surf_threshold, 1000, "Maximum threshold for feature detection using SURF."); -// ORGBRISK detector -DEFINE_int32(min_brisk_features, 400, +DEFINE_double(surf_too_many_ratio, 1.1, + "Ratio to increase the dynamic feature threshold by if too many features are detected."); +DEFINE_double(surf_too_few_ratio, 0.9, + "Ratio to reduce the dynamic feature threshold by if too few features are detected."); + +// Binary detector +DEFINE_int32(min_brisk_features, 1000, "Minimum number of features to be computed using ORGBRISK."); -DEFINE_int32(max_brisk_features, 800, +DEFINE_int32(max_brisk_features, 5000, "Maximum number of features to be computed using ORGBRISK."); -DEFINE_double(min_brisk_threshold, 20, +DEFINE_double(min_brisk_threshold, 10, "Minimum threshold for feature detection using ORGBRISK."); DEFINE_double(default_brisk_threshold, 90, "Default threshold for feature detection using ORGBRISK."); DEFINE_double(max_brisk_threshold, 110, "Maximum threshold for feature detection using ORGBRISK."); +DEFINE_double(brisk_too_many_ratio, 1.25, + "Ratio to increase the dynamic feature threshold by if too many features are detected."); +DEFINE_double(brisk_too_few_ratio, 0.8, + "Ratio to reduce the dynamic feature threshold by if too few features are detected."); + + namespace interest_point { - DynamicDetector::DynamicDetector(int min_features, int max_features, int max_retries, - double min_thresh, double default_thresh, double max_thresh): - min_features_(min_features), max_features_(max_features), max_retries_(max_retries), - min_thresh_(min_thresh), default_thresh_(default_thresh), max_thresh_(max_thresh), - dynamic_thresh_(default_thresh) {} +DynamicDetector::DynamicDetector(int min_features, int max_features, int max_retries, double min_thresh, + double default_thresh, double max_thresh, double too_many_ratio, double too_few_ratio) + : min_features_(min_features), + max_features_(max_features), + max_retries_(max_retries), + min_thresh_(min_thresh), + default_thresh_(default_thresh), + max_thresh_(max_thresh), + dynamic_thresh_(default_thresh), + too_many_ratio_(too_many_ratio), + too_few_ratio_(too_few_ratio), + last_keypoint_count_(0) {} - void DynamicDetector::GetDetectorParams(int & min_features, int & max_features, int & max_retries, - double & min_thresh, double & default_thresh, - double & max_thresh) { - min_features = min_features_; max_features = max_features_; max_retries = max_retries_; - min_thresh = min_thresh_; default_thresh = default_thresh_; max_thresh = max_thresh_; - } +void DynamicDetector::GetDetectorParams(int& min_features, int& max_features, int& max_retries, double& min_thresh, + double& default_thresh, double& max_thresh, double& too_many_ratio, + double& too_few_ratio) { + min_features = min_features_; + max_features = max_features_; + max_retries = max_retries_; + min_thresh = min_thresh_; + default_thresh = default_thresh_; + max_thresh = max_thresh_; + too_many_ratio = too_many_ratio_; + too_few_ratio = too_few_ratio_; +} void DynamicDetector::Detect(const cv::Mat& image, std::vector* keypoints, @@ -92,6 +117,7 @@ namespace interest_point { for (unsigned int i = 0; i < max_retries_; i++) { keypoints->clear(); DetectImpl(image, keypoints); + last_keypoint_count_ = keypoints->size(); if (keypoints->size() < min_features_) TooFew(); else if (keypoints->size() > max_features_) @@ -104,10 +130,10 @@ namespace interest_point { class BriskDynamicDetector : public DynamicDetector { public: - BriskDynamicDetector(int min_features, int max_features, int max_retries, - double min_thresh, double default_thresh, double max_thresh) - : DynamicDetector(min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh) { + BriskDynamicDetector(int min_features, int max_features, int max_retries, double min_thresh, double default_thresh, + double max_thresh, double too_many_ratio, double too_few_ratio) + : DynamicDetector(min_features, max_features, max_retries, min_thresh, default_thresh, max_thresh, + too_many_ratio, too_few_ratio) { Reset(); } @@ -124,14 +150,14 @@ namespace interest_point { brisk_->compute(image, *keypoints, *keypoints_description); } virtual void TooMany(void) { - dynamic_thresh_ *= 1.25; + dynamic_thresh_ *= too_many_ratio_; dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ > max_thresh_) dynamic_thresh_ = max_thresh_; brisk_->setThreshold(dynamic_thresh_); } virtual void TooFew(void) { - dynamic_thresh_ *= 0.8; + dynamic_thresh_ *= too_few_ratio_; dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ < min_thresh_) dynamic_thresh_ = min_thresh_; @@ -144,10 +170,10 @@ namespace interest_point { class SurfDynamicDetector : public DynamicDetector { public: - SurfDynamicDetector(int min_features, int max_features, int max_retries, - double min_thresh, double default_thresh, double max_thresh) - : DynamicDetector(min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh) { + SurfDynamicDetector(int min_features, int max_features, int max_retries, double min_thresh, double default_thresh, + double max_thresh, double too_many_ratio, double too_few_ratio) + : DynamicDetector(min_features, max_features, max_retries, min_thresh, default_thresh, max_thresh, + too_many_ratio, too_few_ratio) { surf_ = cv::xfeatures2d::SURF::create(dynamic_thresh_); } @@ -159,13 +185,13 @@ namespace interest_point { surf_->compute(image, *keypoints, *keypoints_description); } virtual void TooMany(void) { - dynamic_thresh_ *= 1.1; + dynamic_thresh_ *= too_many_ratio_; if (dynamic_thresh_ > max_thresh_) dynamic_thresh_ = max_thresh_; surf_->setHessianThreshold(static_cast(dynamic_thresh_)); } virtual void TooFew(void) { - dynamic_thresh_ *= 0.9; + dynamic_thresh_ *= too_few_ratio_; if (dynamic_thresh_ < min_thresh_) dynamic_thresh_ = min_thresh_; surf_->setHessianThreshold(static_cast(dynamic_thresh_)); @@ -175,21 +201,81 @@ namespace interest_point { cv::Ptr surf_; }; - FeatureDetector::FeatureDetector(std::string const& detector_name, - int min_features, int max_features, int retries, - double min_thresh, double default_thresh, double max_thresh) { + class TeblidDynamicDetector : public DynamicDetector { + public: + TeblidDynamicDetector(int min_features, int max_features, int max_retries, double min_thresh, double default_thresh, + double max_thresh, double too_many_ratio, double too_few_ratio, bool use_512 = true) + : DynamicDetector(min_features, max_features, max_retries, min_thresh, default_thresh, max_thresh, + too_many_ratio, too_few_ratio) { + if (use_512) { + teblid_ = upm::BAD::create(5.0, upm::BAD::SIZE_512_BITS); + } else { + teblid_ = upm::BAD::create(5.0, upm::BAD::SIZE_256_BITS); + } + Reset(); + } + + void Reset(void) { + brisk_ = interest_point::BRISK::create(dynamic_thresh_, FLAGS_orgbrisk_octaves, + FLAGS_orgbrisk_pattern_scale); + } + + virtual void DetectImpl(const cv::Mat& image, std::vector* keypoints) { + brisk_->detect(image, *keypoints); + } + virtual void ComputeImpl(const cv::Mat& image, std::vector* keypoints, + cv::Mat* keypoints_description) { + teblid_->compute(image, *keypoints, *keypoints_description); + } + virtual void TooMany(void) { + double threshold_ratio = too_many_ratio_; + const int keypoint_diff = std::abs(last_keypoint_count_ - min_features_); + // Scale ratio as get close to edge of too few keypoints to avoid undershoot + constexpr double kKeypointDiff = 500; + if (keypoint_diff < kKeypointDiff) { + threshold_ratio += (1.0 - threshold_ratio)*(kKeypointDiff - keypoint_diff)/kKeypointDiff; + } + dynamic_thresh_ *= threshold_ratio; + dynamic_thresh_ = std::lround(dynamic_thresh_); // for backwards compatibility + if (dynamic_thresh_ > max_thresh_) + dynamic_thresh_ = max_thresh_; + brisk_->setThreshold(dynamic_thresh_); + } + virtual void TooFew(void) { + double threshold_ratio = too_few_ratio_; + const int keypoint_diff = std::abs(last_keypoint_count_ - max_features_); + // Scale ratio as get close to edge of too many keypoints to avoid overshoot + constexpr double kKeypointDiff = 1000; + if (keypoint_diff < kKeypointDiff) { + threshold_ratio += (1.0 - threshold_ratio)*(kKeypointDiff - keypoint_diff)/kKeypointDiff; + } + dynamic_thresh_ *= threshold_ratio; + dynamic_thresh_ = std::lround(dynamic_thresh_); // for backwards compatibility + if (dynamic_thresh_ < min_thresh_) + dynamic_thresh_ = min_thresh_; + brisk_->setThreshold(dynamic_thresh_); + } + + private: + cv::Ptr teblid_; + cv::Ptr brisk_; + }; + + FeatureDetector::FeatureDetector(std::string const& detector_name, int min_features, int max_features, int retries, + double min_thresh, double default_thresh, double max_thresh, double too_many_ratio, + double too_few_ratio) { detector_ = NULL; Reset(detector_name, min_features, max_features, retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); } void FeatureDetector::GetDetectorParams(int & min_features, int & max_features, int & max_retries, double & min_thresh, double & default_thresh, - double & max_thresh) { + double & max_thresh, double & too_many_ratio, double & too_few_ratio) { if (detector_ == NULL) LOG(FATAL) << "The detector was not set."; detector_->GetDetectorParams(min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); } FeatureDetector::~FeatureDetector(void) { @@ -199,9 +285,9 @@ namespace interest_point { } } - void FeatureDetector::Reset(std::string const& detector_name, - int min_features, int max_features, int retries, - double min_thresh, double default_thresh, double max_thresh) { + void FeatureDetector::Reset(std::string const& detector_name, int min_features, int max_features, int retries, + double min_thresh, double default_thresh, double max_thresh, double too_many_ratio, + double too_few_ratio) { detector_name_ = detector_name; if (detector_ != NULL) { @@ -218,13 +304,18 @@ namespace interest_point { min_thresh = FLAGS_min_surf_threshold; default_thresh = FLAGS_default_surf_threshold; max_thresh = FLAGS_max_surf_threshold; - } else if (detector_name == "ORGBRISK") { + too_many_ratio = FLAGS_surf_too_many_ratio; + too_few_ratio = FLAGS_surf_too_few_ratio; + } else if (detector_name == "ORGBRISK" || detector_name == "TEBLID512" || + detector_name == "TEBLID256") { min_features = FLAGS_min_brisk_features; max_features = FLAGS_max_brisk_features; retries = FLAGS_detection_retries; min_thresh = FLAGS_min_brisk_threshold; default_thresh = FLAGS_default_brisk_threshold; max_thresh = FLAGS_max_brisk_threshold; + too_many_ratio = FLAGS_brisk_too_many_ratio; + too_few_ratio = FLAGS_brisk_too_few_ratio; } else { LOG(FATAL) << "Unimplemented feature detector: " << detector_name; } @@ -233,12 +324,20 @@ namespace interest_point { // Loading the detector if (detector_name == "ORGBRISK") detector_ = new BriskDynamicDetector(min_features, max_features, retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); else if (detector_name == "SURF") detector_ = new SurfDynamicDetector(min_features, max_features, retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); + else if (detector_name == "TEBLID512") + detector_ = new TeblidDynamicDetector(min_features, max_features, retries, + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio, true); + else if (detector_name == "TEBLID256") + detector_ = new TeblidDynamicDetector(min_features, max_features, retries, + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio, false); else LOG(FATAL) << "Unimplemented feature detector: " << detector_name; + + LOG(INFO) << "Using descriptor: " << detector_name; } void FeatureDetector::Detect(const cv::Mat& image, @@ -256,12 +355,16 @@ namespace interest_point { } } - void FindMatches(const cv::Mat & img1_descriptor_map, - const cv::Mat & img2_descriptor_map, std::vector * matches) { + void FindMatches(const cv::Mat& img1_descriptor_map, const cv::Mat& img2_descriptor_map, + std::vector* matches, boost::optional hamming_distance, + boost::optional goodness_ratio) { CHECK(img1_descriptor_map.depth() == img2_descriptor_map.depth()) << "Mixed descriptor types. Did you mash BRISK with SIFT/SURF?"; + if (!hamming_distance) hamming_distance = FLAGS_hamming_distance; + if (!goodness_ratio) goodness_ratio = FLAGS_goodness_ratio; + // Check for early exit conditions matches->clear(); if (img1_descriptor_map.rows == 0 || @@ -270,22 +373,23 @@ namespace interest_point { if (img1_descriptor_map.depth() == CV_8U) { // Binary descriptor - - // cv::BFMatcher matcher(cv::NORM_HAMMING, true /* Forward & Backward matching */); cv::FlannBasedMatcher matcher(cv::makePtr(3, 18, 2)); - matcher.match(img1_descriptor_map, img2_descriptor_map, *matches); - - // Select only inlier matches that meet a BRISK threshold of - // of FLAGS_hamming_distance. - // TODO(oalexan1) This needs further study. - std::vector inlier_matches; - inlier_matches.reserve(matches->size()); // This saves time in allocation - for (cv::DMatch const& dmatch : *matches) { - if (dmatch.distance < FLAGS_hamming_distance) { - inlier_matches.push_back(dmatch); + std::vector > possible_matches; + matcher.knnMatch(img1_descriptor_map, img2_descriptor_map, possible_matches, 2); + matches->clear(); + matches->reserve(possible_matches.size()); + for (std::vector const& best_pair : possible_matches) { + if (best_pair.size() == 0 || best_pair.at(0).distance > *hamming_distance) continue; + if (best_pair.size() == 1) { + // This was the only best match, push it. + matches->push_back(best_pair.at(0)); + } else { + // Push back a match only if it is a certain percent better than the next best. + if (best_pair.at(0).distance < *goodness_ratio * best_pair.at(1).distance) { + matches->push_back(best_pair[0]); + } } } - matches->swap(inlier_matches); // Doesn't invoke a copy of all elements. } else { // Traditional floating point descriptor cv::FlannBasedMatcher matcher; @@ -299,7 +403,7 @@ namespace interest_point { matches->push_back(best_pair.at(0)); } else { // Push back a match only if it is 25% better than the next best. - if (best_pair.at(0).distance < FLAGS_goodness_ratio * best_pair.at(1).distance) { + if (best_pair.at(0).distance < *goodness_ratio * best_pair.at(1).distance) { matches->push_back(best_pair[0]); } } diff --git a/localization/interest_point/test/test_matching.cc b/localization/interest_point/test/test_matching.cc new file mode 100644 index 0000000000..a9d1220bbc --- /dev/null +++ b/localization/interest_point/test/test_matching.cc @@ -0,0 +1,81 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +class MatchingTest : public ::testing::Test { + protected: + virtual void SetUp() { + // TEST_DIR is defined as this modules test directory. However + // that is not where the test imagery is located. + std::string data_dir = std::string(std::getenv("DATA_DIR")); + image1 = cv::imread(data_dir + "/m0004000.jpg", + cv::IMREAD_GRAYSCALE); + image2 = cv::imread(data_dir + "/m0004025.jpg", + cv::IMREAD_GRAYSCALE); + + // Decimate the image to speed things up for our unit tests + cv::resize(image1, image1, cv::Size(), 0.7, 0.7, cv::INTER_AREA); + cv::resize(image2, image2, cv::Size(), 0.7, 0.7, cv::INTER_AREA); + } + + void DetectKeyPoints(std::string const& detector) { + interest_point::FeatureDetector ipdetect(detector); + ipdetect.Detect(image1, &keypoints1, &descriptor1); + ipdetect.Detect(image2, &keypoints2, &descriptor2); + } + + cv::Mat image1, image2; + std::vector keypoints1, keypoints2; + cv::Mat descriptor1, descriptor2; + std::vector matches; +}; + +TEST_F(MatchingTest, SURF) { + DetectKeyPoints("SURF"); + interest_point::FindMatches(descriptor1, descriptor2, &matches); + EXPECT_NEAR(keypoints1.size(), 2023u, 10); + EXPECT_NEAR(keypoints2.size(), 2047u, 10); + EXPECT_EQ(descriptor1.cols, 64); + // KNN matching seems to be variable + EXPECT_NEAR(matches.size(), 800u, 10); +} + +TEST_F(MatchingTest, ORGBRISK) { + DetectKeyPoints("ORGBRISK"); + interest_point::FindMatches(descriptor1, descriptor2, &matches); + EXPECT_LT(100u, keypoints1.size()); + EXPECT_LT(100u, keypoints2.size()); + EXPECT_EQ(64, descriptor1.cols); + EXPECT_LT(50u, matches.size()); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/interest_point/test/test_matching.cxx b/localization/interest_point/test/test_matching.cxx deleted file mode 100644 index 828105f8ff..0000000000 --- a/localization/interest_point/test/test_matching.cxx +++ /dev/null @@ -1,75 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include -#include -#include -#include - -#include -#include - -class MatchingTest : public ::testing::Test { - protected: - virtual void SetUp() { - // TEST_DIR is defined as this modules test directory. However - // that is not where the test imagery is located. - std::string data_dir = std::string(TEST_DIR) + "/../../sparse_mapping/test/data"; - image1 = cv::imread(data_dir + "/m0004000.jpg", - CV_LOAD_IMAGE_GRAYSCALE); - image2 = cv::imread(data_dir + "/m0004025.jpg", - CV_LOAD_IMAGE_GRAYSCALE); - - // Decimate the image to speed things up for our unit tests - cv::resize(image1, image1, cv::Size(), 0.7, 0.7, cv::INTER_AREA); - cv::resize(image2, image2, cv::Size(), 0.7, 0.7, cv::INTER_AREA); - } - - void DetectKeyPoints(std::string const& detector) { - interest_point::FeatureDetector ipdetect(detector); - ipdetect.Detect(image1, &keypoints1, &descriptor1); - ipdetect.Detect(image2, &keypoints2, &descriptor2); - } - - cv::Mat image1, image2; - std::vector keypoints1, keypoints2; - cv::Mat descriptor1, descriptor2; - std::vector matches; -}; - -TEST_F(MatchingTest, SURF) { - DetectKeyPoints("SURF"); - interest_point::FindMatches(descriptor1, descriptor2, &matches); - EXPECT_NEAR(keypoints1.size(), 2023u, 10); - EXPECT_NEAR(keypoints2.size(), 2047u, 10); - EXPECT_EQ(descriptor1.cols, 64); - // KNN matching seems to be variable - EXPECT_NEAR(matches.size(), 798u, 10); -} - -TEST_F(MatchingTest, ORGBRISK) { - DetectKeyPoints("ORGBRISK"); - interest_point::FindMatches(descriptor1, descriptor2, &matches); - EXPECT_LT(100u, keypoints1.size()); - EXPECT_LT(100u, keypoints2.size()); - EXPECT_EQ(64, descriptor1.cols); - EXPECT_LT(50u, matches.size()); -} diff --git a/localization/interest_point/test/test_matching.test b/localization/interest_point/test/test_matching.test new file mode 100644 index 0000000000..6c673b54cc --- /dev/null +++ b/localization/interest_point/test/test_matching.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/lk_optical_flow/CMakeLists.txt b/localization/lk_optical_flow/CMakeLists.txt index 5ff2d04f17..79b87e341a 100644 --- a/localization/lk_optical_flow/CMakeLists.txt +++ b/localization/lk_optical_flow/CMakeLists.txt @@ -15,22 +15,70 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(lk_optical_flow) -if (USE_ROS) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + cv_bridge + image_transport + nodelet + camera + ff_util +) catkin_package( + INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp cv_bridge image_transport nodelet - DEPENDS lk_optical_flow + CATKIN_DEPENDS roscpp cv_bridge image_transport nodelet camera ff_util +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_library(TARGET lk_optical_flow - LIBS ff_nodelet camera config_reader ${catkin_LIBRARIES} - INC ${catkin_INCLUDE_DIRS} - DEPS ff_msgs config_reader camera +# Declare C++ libraries +add_library(lk_optical_flow + src/lk_optical_flow.cc + src/lk_optical_flow_nodelet.cc ) +add_dependencies(lk_optical_flow ${catkin_EXPORTED_TARGETS}) +target_link_libraries(lk_optical_flow ${catkin_LIBRARIES}) + +############# +## Install ## +############# -install_launch_files() +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) -endif (USE_ROS) +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/localization/lk_optical_flow/launch/lk_optical_flow.launch b/localization/lk_optical_flow/launch/lk_optical_flow.launch index 1ffcd6cac6..d05dce51f5 100644 --- a/localization/lk_optical_flow/launch/lk_optical_flow.launch +++ b/localization/lk_optical_flow/launch/lk_optical_flow.launch @@ -17,9 +17,11 @@ + + - \ No newline at end of file + diff --git a/localization/lk_optical_flow/package.xml b/localization/lk_optical_flow/package.xml index 4ae2b03065..33799fcf96 100644 --- a/localization/lk_optical_flow/package.xml +++ b/localization/lk_optical_flow/package.xml @@ -19,11 +19,15 @@ roscpp nodelet ff_msgs + camera + ff_util cv_bridge image_transport roscpp nodelet ff_msgs + camera + ff_util diff --git a/localization/lk_optical_flow/src/lk_optical_flow.cc b/localization/lk_optical_flow/src/lk_optical_flow.cc index e336cbbd92..09ea17e170 100644 --- a/localization/lk_optical_flow/src/lk_optical_flow.cc +++ b/localization/lk_optical_flow/src/lk_optical_flow.cc @@ -19,12 +19,14 @@ #include #include +#include #include #include #include #include #include + namespace lk_optical_flow { LKOpticalFlow::LKOpticalFlow(void) : diff --git a/localization/localization_common/CMakeLists.txt b/localization/localization_common/CMakeLists.txt new file mode 100644 index 0000000000..1b7acd8457 --- /dev/null +++ b/localization/localization_common/CMakeLists.txt @@ -0,0 +1,135 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(localization_common) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + astrobee + camera + config_reader + msg_conversions + ff_msgs +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# Allow other packages to use python scripts from this package +catkin_python_setup() + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} ${GTSAM_LIBRARIES} gtsam + CATKIN_DEPENDS + astrobee + camera + config_reader + msg_conversions + ff_msgs +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${GTSAM_INCLUDE_DIR} + ${EIGEN3_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/averager.cc + src/combined_nav_state.cc + src/combined_nav_state_covariances.cc + src/distance_scaled_pose_covariance_interpolater.cc + src/pose_covariance_interpolater.cc + src/pose_interpolater.cc + src/pose_with_covariance_interpolater.cc + src/rate_timer.cc + src/ros_timer.cc + src/stats_logger.cc + src/time.cc + src/timer.cc + src/test_utilities.cc + src/utilities.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} gtsam) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_pose_interpolater + test/test_pose_interpolater.test + test/test_pose_interpolater.cc + ) + target_link_libraries(test_pose_interpolater + ${PROJECT_NAME} + ) + + add_rostest_gtest(test_pose_with_covariance_interpolater + test/test_pose_with_covariance_interpolater.test + test/test_pose_with_covariance_interpolater.cc + ) + target_link_libraries(test_pose_with_covariance_interpolater + ${PROJECT_NAME} + ) + + add_rostest_gtest(test_timestamped_set + test/test_timestamped_set.test + test/test_timestamped_set.cc + ) + target_link_libraries(test_timestamped_set + ${PROJECT_NAME} + ) + add_rostest_gtest(test_utilities_localization_common + test/test_utilities.test + test/test_utilities.cc + ) + target_link_libraries(test_utilities_localization_common + ${PROJECT_NAME} + ) +endif() + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/localization_common/include/localization_common/averager.h b/localization/localization_common/include/localization_common/averager.h new file mode 100644 index 0000000000..e7165acb2b --- /dev/null +++ b/localization/localization_common/include/localization_common/averager.h @@ -0,0 +1,72 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_AVERAGER_H_ +#define LOCALIZATION_COMMON_AVERAGER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace localization_common { +class Averager { + public: + explicit Averager(const std::string& averager_name = "", const std::string& type_name = "", + const std::string& units = "", const bool log_on_destruction = false); + ~Averager(); + void Update(const double value); + double average() const; + int count() const; + double last_value() const; + void Log() const; + void LogToFile(std::ofstream& ofstream) const; + void LogToCsv(std::ofstream& ofstream) const; + void LogEveryN(const int num_events_per_log) const; + void UpdateAndLog(const double value); + void UpdateAndLogEveryN(const double value, const int num_events_per_log); + void Vlog(const int level = 2) const; + void VlogEveryN(const int num_events_per_log, const int level) const; + + private: + std::string StatsString() const; + std::string CsvStatsString() const; + std::string LastValueString() const; + + std::string name_; + std::string type_name_; + std::string units_; + std::string spacer_; + double last_value_; + bool log_on_destruction_; + boost::accumulators::accumulator_set< + double, boost::accumulators::stats> + accumulator_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_AVERAGER_H_ diff --git a/localization/localization_common/include/localization_common/combined_nav_state.h b/localization/localization_common/include/localization_common/combined_nav_state.h new file mode 100644 index 0000000000..1ecc7da0ca --- /dev/null +++ b/localization/localization_common/include/localization_common/combined_nav_state.h @@ -0,0 +1,77 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_COMBINED_NAV_STATE_H_ +#define LOCALIZATION_COMMON_COMBINED_NAV_STATE_H_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace localization_common { +struct CombinedNavStateNoise { + gtsam::SharedNoiseModel pose_noise; + gtsam::SharedNoiseModel velocity_noise; + gtsam::SharedNoiseModel bias_noise; + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(pose_noise); + ar& BOOST_SERIALIZATION_NVP(velocity_noise); + ar& BOOST_SERIALIZATION_NVP(bias_noise); + } +}; + +class CombinedNavState { + public: + CombinedNavState(const gtsam::NavState& nav_state, const gtsam::imuBias::ConstantBias& bias, const Time timestamp); + CombinedNavState(const gtsam::Pose3& pose, const gtsam::Velocity3& velocity, const gtsam::imuBias::ConstantBias& bias, + const Time timestamp); + CombinedNavState() = default; + Time timestamp() const { return timestamp_; } + const gtsam::NavState& nav_state() const { return nav_state_; } + gtsam::Pose3 pose() const { return nav_state().pose(); } + const gtsam::Velocity3& velocity() const { return nav_state().velocity(); } + const gtsam::imuBias::ConstantBias& bias() const { return bias_; } + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(nav_state_); + ar& BOOST_SERIALIZATION_NVP(bias_); + ar& BOOST_SERIALIZATION_NVP(timestamp_); + } + + gtsam::NavState nav_state_; + gtsam::imuBias::ConstantBias bias_; + Time timestamp_; +}; + +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_COMBINED_NAV_STATE_H_ diff --git a/localization/localization_common/include/localization_common/combined_nav_state_covariances.h b/localization/localization_common/include/localization_common/combined_nav_state_covariances.h new file mode 100644 index 0000000000..4db6a6570a --- /dev/null +++ b/localization/localization_common/include/localization_common/combined_nav_state_covariances.h @@ -0,0 +1,63 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_COMBINED_NAV_STATE_COVARIANCES_H_ +#define LOCALIZATION_COMMON_COMBINED_NAV_STATE_COVARIANCES_H_ + +#include +#include + +namespace localization_common { +enum Confidence { kGood, kPoor, kLost }; +class CombinedNavStateCovariances { + public: + CombinedNavStateCovariances(const Eigen::Matrix& pose_covariance, + const Eigen::Matrix3d& velocity_covariance, + const Eigen::Matrix& bias_covariance); + CombinedNavStateCovariances(const Eigen::Vector3d& position_variances, const Eigen::Vector3d& orientation_variances, + const Eigen::Vector3d& velocity_variances, + const Eigen::Vector3d& accelerometer_bias_variances, + const Eigen::Vector3d& gyro_bias_variances); + CombinedNavStateCovariances() = default; + // create constructor from marginals! -> put this in + // localization_common! + Confidence PoseConfidence(const double position_log_det_threshold, const double orientation_log_det_threshold) const; + + const Eigen::Matrix& pose_covariance() const { return pose_covariance_; } + const Eigen::Matrix3d& velocity_covariance() const { return velocity_covariance_; } + const Eigen::Matrix& bias_covariance() const { return bias_covariance_; } + + // Diagonal Accessors for Loc msg + Eigen::Vector3d position_variances() const { return pose_covariance_.block<3, 3>(0, 0).diagonal(); } + Eigen::Vector3d orientation_variances() const { return pose_covariance_.block<3, 3>(3, 3).diagonal(); } + Eigen::Vector3d velocity_variances() const { return velocity_covariance_.diagonal(); } + Eigen::Vector3d gyro_bias_variances() const { return bias_covariance_.block<3, 3>(3, 3).diagonal(); } + Eigen::Vector3d accel_bias_variances() const { return bias_covariance_.block<3, 3>(0, 0).diagonal(); } + + double LogDeterminantPositionCovariance() const; + + double LogDeterminantOrientationCovariance() const; + + private: + Eigen::Matrix pose_covariance_; + Eigen::Matrix3d velocity_covariance_; + Eigen::Matrix bias_covariance_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_COMBINED_NAV_STATE_COVARIANCES_H_ diff --git a/localization/localization_common/include/localization_common/distance_scaled_pose_covariance_interpolater.h b/localization/localization_common/include/localization_common/distance_scaled_pose_covariance_interpolater.h new file mode 100644 index 0000000000..c470565ec1 --- /dev/null +++ b/localization/localization_common/include/localization_common/distance_scaled_pose_covariance_interpolater.h @@ -0,0 +1,44 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_DISTANCE_SCALED_POSE_COVARIANCE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_DISTANCE_SCALED_POSE_COVARIANCE_INTERPOLATER_H_ + +#include + +namespace localization_common { +struct DistanceScaledPoseCovarianceInterpolaterParams { + double min_covariance = 1e-4; + double translation_scale = 1; + double orientation_scale = 0.01; +}; + +class DistanceScaledPoseCovarianceInterpolater : public PoseCovarianceInterpolater { + public: + DistanceScaledPoseCovarianceInterpolater( + const DistanceScaledPoseCovarianceInterpolaterParams& params = DistanceScaledPoseCovarianceInterpolaterParams()); + + PoseCovariance Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, const Time& timestamp_a, + const Time& timestamp_b) final; + + private: + DistanceScaledPoseCovarianceInterpolaterParams params_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_DISTANCE_SCALED_POSE_COVARIANCE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/feature_point_3d.h b/localization/localization_common/include/localization_common/feature_point_3d.h new file mode 100644 index 0000000000..c440991630 --- /dev/null +++ b/localization/localization_common/include/localization_common/feature_point_3d.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_FEATURE_POINT_3D_H_ +#define LOCALIZATION_COMMON_FEATURE_POINT_3D_H_ + +#include + +#include + +namespace localization_common { +using FeatureId = int; + +struct FeaturePoint3dNoise { + gtsam::SharedNoiseModel noise; +}; + +struct FeaturePoint3d { + FeaturePoint3d(const gtsam::Point3& global_t_point, const FeatureId feature_id) + : global_t_point(global_t_point), feature_id(feature_id) {} + FeaturePoint3d() {} + gtsam::Point3 global_t_point; + FeatureId feature_id; + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(feature_id); + ar& BOOST_SERIALIZATION_NVP(global_t_point); + } +}; + +using FeaturePoint3ds = std::vector; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_FEATURE_POINT_3D_H_ diff --git a/localization/localization_common/include/localization_common/image_correspondences.h b/localization/localization_common/include/localization_common/image_correspondences.h new file mode 100644 index 0000000000..192d4659f9 --- /dev/null +++ b/localization/localization_common/include/localization_common/image_correspondences.h @@ -0,0 +1,43 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef LOCALIZATION_COMMON_IMAGE_CORRESPONDENCES_H_ +#define LOCALIZATION_COMMON_IMAGE_CORRESPONDENCES_H_ + +#include + +#include + +namespace localization_common { +struct ImageCorrespondences { + ImageCorrespondences(const std::vector& image_points, const std::vector& points_3d) + : image_points(image_points), points_3d(points_3d) {} + ImageCorrespondences() {} + + void AddCorrespondence(const Eigen::Vector2d& image_point, const Eigen::Vector3d& point_3d) { + image_points.emplace_back(image_point); + points_3d.emplace_back(point_3d); + } + + size_t size() const { return image_points.size(); } + + std::vector image_points; + std::vector points_3d; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_IMAGE_CORRESPONDENCES_H_ diff --git a/localization/localization_common/include/localization_common/logger.h b/localization/localization_common/include/localization_common/logger.h new file mode 100644 index 0000000000..d0623f3e7c --- /dev/null +++ b/localization/localization_common/include/localization_common/logger.h @@ -0,0 +1,167 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_LOGGER_H_ +#define LOCALIZATION_COMMON_LOGGER_H_ + +// Needed for ros include +#include + +#include + +#include + +#include +#include + +// Select logger here +// TODO(rsoussan): Add compile definition for this? +#define USE_ROS_LOGGING +// #define USE_GLOG_LOGGING + +#ifdef USE_ROS_LOGGING +#define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) + +// clang-format off +#define LogInfo(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_INFO_STREAM(ss.str()); \ + } while (0) + +#define LogWarning(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_WARN_STREAM(ss.str()); \ + } while (0) + +#define LogError(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_ERROR_STREAM(ss.str()); \ + } while (0) + +#define LogFatal(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_FATAL_STREAM(ss.str()); \ + } while (0) + +#define LogDebug(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_DEBUG_STREAM(ss.str()); \ + } while (0) + +#define LogInfoEveryN(n, msg) \ + do { \ + static int count = 0; \ + ++count; \ + if (count % n == 0) { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_INFO_STREAM(ss.str()); \ + } \ + } while (0) + +#define LogWarningEveryN(n, msg) \ + do { \ + static int count = 0; \ + ++count; \ + if (count % n == 0) { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_WARN_STREAM(ss.str()); \ + } \ + } while (0) + +#define LogErrorEveryN(n, msg) \ + do { \ + static int count = 0; \ + ++count; \ + if (count % n == 0) { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_ERROR_STREAM(ss.str()); \ + } \ + } while (0) + +#define LogDebugEveryN(n, msg) \ + do { \ + static int count = 0; \ + ++count; \ + if (count % n == 0) { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_DEBUG_STREAM(ss.str()); \ + } \ + } while (0) + +#elif defined(USE_GLOG_LOGGING) +#define LogInfo(msg) \ + do { \ + LOG(INFO) << msg; \ + } while (0) +#define LogWarning(msg) \ + do { \ + LOG(WARNING) << msg; \ + } while (0) + +#define LogError(msg) \ + do { \ + LOG(ERROR) << msg; \ + } while (0) + +#define LogFatal(msg) \ + do { \ + LOG(FATAL) << msg; \ + } while (0) + +#define LogDebug(msg) \ + do { \ + VLOG(2) << msg; \ + } while (0) + +#define LogInfoEveryN(n, msg) \ + do { \ + LOG_EVERY_N(INFO, n) << msg; \ + } while (0) + +#define LogWarningEveryN(n, msg) \ + do { \ + LOG_EVERY_N(WARNING, n) << msg; \ + } while (0) + +#define LogErrorEveryN(n, msg) \ + do { \ + LOG_EVERY_N(ERROR, n) << msg; \ + } while (0) + +#define LogDebugEveryN(n, msg) \ + do { \ + DLOG_EVERY_N(INFO, n) << msg; \ + } while (0) + +#endif +// clang-format on +#endif // LOCALIZATION_COMMON_LOGGER_H_ diff --git a/localization/localization_common/include/localization_common/marginals_pose_covariance_interpolater.h b/localization/localization_common/include/localization_common/marginals_pose_covariance_interpolater.h new file mode 100644 index 0000000000..d79e08113b --- /dev/null +++ b/localization/localization_common/include/localization_common/marginals_pose_covariance_interpolater.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_MARGINALS_POSE_COVARIANCE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_MARGINALS_POSE_COVARIANCE_INTERPOLATER_H_ + +#include +#include + +#include +#include + +namespace localization_common { +template +class MarginalsPoseCovarianceInterpolater : public PoseCovarianceInterpolater { + public: + MarginalsPoseCovarianceInterpolater(const std::shared_ptr& nodes, + const boost::optional& marginals = boost::none); + PoseCovariance Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, const Time& timestamp_a, + const Time& timestamp_b) final; + + private: + boost::optional marginals_; + std::shared_ptr nodes_; +}; + +// Implementation +template +MarginalsPoseCovarianceInterpolater::MarginalsPoseCovarianceInterpolater( + const std::shared_ptr& nodes, const boost::optional& marginals) + : nodes_(nodes), marginals_(marginals) {} + +template +PoseCovariance MarginalsPoseCovarianceInterpolater::Interpolate(const PoseWithCovariance& a, + const PoseWithCovariance& b, + const Time& timestamp_a, + const Time& timestamp_b) { + boost::optional covariance_a_b; + // Set covariance_a_b if available + if (marginals_ && nodes_) { + const auto closest_t_a = nodes_->ClosestTimestamp(timestamp_a); + const auto closest_t_b = nodes_->ClosestTimestamp(timestamp_b); + if (!closest_t_a || !closest_t_b) { + LogError("Interpolate: Failed to get closest timestamps."); + } else { + // TODO(rsoussan): Add fallback covariance/relative covariance if gap too large + if (std::abs(*closest_t_a - timestamp_a) > 3.0 || std::abs(*closest_t_b - timestamp_b) > 3.0) { + LogWarning("Interpolate: Timestamps far from closest timestamps available."); + } + const auto keys_a = nodes_->Keys(*closest_t_a); + const auto keys_b = nodes_->Keys(*closest_t_b); + covariance_a_b = marginals_->jointMarginalCovariance({keys_a[0], keys_b[0]}).fullMatrix(); + } + } + return RelativePoseCovariance(a, b, covariance_a_b); +} +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_MARGINALS_POSE_COVARIANCE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/pose_covariance_interpolater.h b/localization/localization_common/include/localization_common/pose_covariance_interpolater.h new file mode 100644 index 0000000000..1431c79aa3 --- /dev/null +++ b/localization/localization_common/include/localization_common/pose_covariance_interpolater.h @@ -0,0 +1,34 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_POSE_COVARIANCE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_POSE_COVARIANCE_INTERPOLATER_H_ + +#include + +namespace localization_common { +class PoseCovarianceInterpolater { + public: + PoseCovarianceInterpolater() = default; + virtual ~PoseCovarianceInterpolater() = default; + virtual PoseCovariance Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, const Time& timestamp_a, + const Time& timestamp_b); +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_POSE_COVARIANCE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/pose_interpolater.h b/localization/localization_common/include/localization_common/pose_interpolater.h new file mode 100644 index 0000000000..242fbc02f9 --- /dev/null +++ b/localization/localization_common/include/localization_common/pose_interpolater.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_POSE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_POSE_INTERPOLATER_H_ + +#include + +#include + +namespace localization_common { +using PoseInterpolater = TimestampedInterpolater; + +template <> +Eigen::Isometry3d PoseInterpolater::Interpolate(const Eigen::Isometry3d& a, const Eigen::Isometry3d& b, + const double alpha) const; + +template <> +Eigen::Isometry3d PoseInterpolater::Relative(const Eigen::Isometry3d& a, const Eigen::Isometry3d& b) const; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_POSE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/pose_with_covariance.h b/localization/localization_common/include/localization_common/pose_with_covariance.h new file mode 100644 index 0000000000..4e70146d53 --- /dev/null +++ b/localization/localization_common/include/localization_common/pose_with_covariance.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_H_ +#define LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_H_ + +#include + +#include + +namespace localization_common { +using PoseCovariance = Eigen::Matrix; +struct PoseWithCovariance { + PoseWithCovariance(const Eigen::Isometry3d& pose, const PoseCovariance& covariance) + : pose(pose), covariance(covariance) {} + PoseWithCovariance(const Eigen::Isometry3d& pose, const PoseCovariance& covariance, + const TimestampedSet& correlation_covariances) + : pose(pose), covariance(covariance), correlation_covariances(correlation_covariances) {} + + PoseWithCovariance() {} + + Eigen::Isometry3d pose; + PoseCovariance covariance; + // Optional correlated covariances wrt other timestamped poses + TimestampedSet correlation_covariances; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_H_ diff --git a/localization/localization_common/include/localization_common/pose_with_covariance_interpolater.h b/localization/localization_common/include/localization_common/pose_with_covariance_interpolater.h new file mode 100644 index 0000000000..89465d1d4f --- /dev/null +++ b/localization/localization_common/include/localization_common/pose_with_covariance_interpolater.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_INTERPOLATER_H_ + +#include +#include +#include + +namespace localization_common { +struct PoseWithCovarianceInterpolaterParams { + PoseWithCovarianceInterpolaterParams(const std::shared_ptr& pose_covariance_interpolater = + std::make_shared()) + : pose_covariance_interpolater(pose_covariance_interpolater) {} + std::shared_ptr pose_covariance_interpolater; +}; + +using PoseWithCovarianceInterpolater = + TimestampedInterpolater; + +template <> +PoseWithCovariance PoseWithCovarianceInterpolater::Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, + const double alpha) const; + +template <> +boost::optional PoseWithCovarianceInterpolater::Relative(const Time timestamp_a, + const Time timestamp_b) const; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/rate_timer.h b/localization/localization_common/include/localization_common/rate_timer.h new file mode 100644 index 0000000000..6f58715ab9 --- /dev/null +++ b/localization/localization_common/include/localization_common/rate_timer.h @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_RATE_TIMER_H_ +#define LOCALIZATION_COMMON_RATE_TIMER_H_ + +#include +#include + +#include + +#include +#include + +namespace localization_common { +class RateTimer { + public: + explicit RateTimer(const std::string& timer_name = ""); + boost::optional LastValue(); + void Record(); + void Log() const; + void LogEveryN(const int num_events_per_log) const; + void VlogEveryN(const int num_events_per_log, const int level) const; + void RecordAndLog(); + void RecordAndLogEveryN(const int num_events_per_log); + void RecordAndVlogEveryN(const int num_events_per_log, const int level); + void Vlog(const int level = 2) const; + + private: + std::chrono::time_point start_time_; + Averager averager_; + int num_events_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_RATE_TIMER_H_ diff --git a/localization/localization_common/include/localization_common/ros_timer.h b/localization/localization_common/include/localization_common/ros_timer.h new file mode 100644 index 0000000000..105dba12c6 --- /dev/null +++ b/localization/localization_common/include/localization_common/ros_timer.h @@ -0,0 +1,49 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_ROS_TIMER_H_ +#define LOCALIZATION_COMMON_ROS_TIMER_H_ + +#include +#include + +#include +#include + +#include + +namespace localization_common { +class RosTimer { + public: + explicit RosTimer(const std::string& timer_name); + void Start(); + // Uses header time as start time and ros::Time::now as stop time + void HeaderDiff(const std_msgs::Header& header); + void Stop(); + void Log() const; + void Vlog(const int level = 2) const; + void LogEveryN(const int num_timing_events_per_log) const; + void VlogEveryN(const int num_timing_events_per_log, const int level = 2) const; + + private: + Averager averager_; + ros::Time start_time_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_ROS_TIMER_H_ diff --git a/localization/localization_common/include/localization_common/stats_logger.h b/localization/localization_common/include/localization_common/stats_logger.h new file mode 100644 index 0000000000..904b4adf5a --- /dev/null +++ b/localization/localization_common/include/localization_common/stats_logger.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef LOCALIZATION_COMMON_STATS_LOGGER_H_ +#define LOCALIZATION_COMMON_STATS_LOGGER_H_ + +#include +#include + +#include + +namespace localization_common { +// Logs various statistics and runtimes to a desired output (command line, CSV file, text file) using averager and timer +// objects. Uses references to provided averagers and timers to log min/max/mean/stddev values and runtimes +// respectively. +class StatsLogger { + public: + explicit StatsLogger(const bool log_on_destruction = true); + + ~StatsLogger(); + + // Add averager for logging + void AddAverager(localization_common::Averager& averager); + + // Add timer for logging + void AddTimer(localization_common::Timer& timer); + + // Log averages and times to command line. + void Log() const; + + // Log averages and times to a text file. + void LogToFile(std::ofstream& ofstream) const; + + // Log averages and times to a CSV file. + void LogToCsv(std::ofstream& ofstream) const; + + std::vector> timers_; + std::vector> averagers_; + + private: + template + void LogToFile(const std::vector>& loggers, std::ofstream& ofstream) const { + for (const auto& logger : loggers) logger.get().LogToFile(ofstream); + } + + template + void LogToCsv(const std::vector>& loggers, std::ofstream& ofstream) const { + for (const auto& logger : loggers) logger.get().LogToCsv(ofstream); + } + + template + void Log(const std::vector>& loggers) const { + for (const auto& logger : loggers) logger.get().Log(); + } + + bool log_on_destruction_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_STATS_LOGGER_H_ diff --git a/localization/localization_common/include/localization_common/test_utilities.h b/localization/localization_common/include/localization_common/test_utilities.h new file mode 100644 index 0000000000..116f3719dd --- /dev/null +++ b/localization/localization_common/include/localization_common/test_utilities.h @@ -0,0 +1,157 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_TEST_UTILITIES_H_ +#define LOCALIZATION_COMMON_TEST_UTILITIES_H_ + +#include +#include +#include + +#include +#include + +#include + +#include + +namespace localization_common { +class Sampler { + public: + Sampler(const double min, const double max, const double count); + double Sample(const int increment) const; + + private: + const double scale_; + const double min_; +}; + +// Selected from [-100, 100] +double RandomDouble(); + +// Selected from [0, 100] +double RandomPositiveDouble(); + +// Selected from [min, max] +double RandomDouble(const double min, const double max); + +double RandomGaussianDouble(const double mean, const double stddev); + +bool RandomBool(); + +Time RandomTime(); + +// Returns white noise with set stddev +double Noise(const double stddev); + +// Each index ranges from [-100, 100] +template +Eigen::Matrix RandomVector(); + +Eigen::Vector3d RandomVector3d(); + +Eigen::Vector3d RandomPoint3d(); + +Eigen::Vector3d RandomVelocity(); + +Eigen::Vector2d RandomVector2d(); + +Eigen::Vector2d RandomPoint2d(); + +template +Eigen::Matrix RandomCovariance(); + +// Translation ranges from [-100, 100] +// Rotation spans all possible rotations +gtsam::Pose3 RandomPose(); + +PoseCovariance RandomPoseCovariance(); + +PoseWithCovariance RandomPoseWithCovariance(); + +Eigen::Isometry3d RandomIsometry3d(); + +Eigen::Affine3d RandomAffine3d(); + +CombinedNavState RandomCombinedNavState(); + +// Focal lengths and principal points selected from [0.1, 1000] +Eigen::Matrix3d RandomIntrinsics(); + +// Adds noise to identity Isometry3d +Eigen::Isometry3d RandomIdentityCenteredIsometry3d(const double translation_stddev, const double rotation_stddev); + +Eigen::Isometry3d AddNoiseToIsometry3d(const Eigen::Isometry3d& pose, const double translation_stddev, + const double rotation_stddev); + +gtsam::Pose3 AddNoiseToPose(const gtsam::Pose3& pose, const double translation_stddev, const double rotation_stddev); + +template +Eigen::Matrix AddNoiseToVector(const Eigen::Matrix& vector, const double noise_stddev); + +// Samples in cylindrical coordinates for pose translation to keep pose in view frustrum. +// Samples z using scaled rho value to prevent large z vals with small rho values +// that may move the pose out of the view frustrum. +Eigen::Isometry3d RandomFrontFacingPose(const double rho_min, const double rho_max, const double phi_min, + const double phi_max, const double z_rho_scale, const double yaw_min, + const double yaw_max, const double pitch_min, const double pitch_max, + const double roll_min, const double roll_max); + +Eigen::Isometry3d RandomFrontFacingPose(); + +std::vector RandomFrontFacingPoints(const int num_points); + +Eigen::Vector3d RandomFrontFacingPoint(); + +Eigen::Vector3d RandomFrontFacingPoint(const double rho_min, const double rho_max, const double phi_min, + const double phi_max, const double z_rho_scale); + +bool MatrixEquality(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double tolerance); + +// Implementation +template +Eigen::Matrix RandomVector() { + // Eigen::Matrix::Random() is constrained to [-1, 1] + return RandomDouble() * Eigen::Matrix::Random(); +} + +template +Eigen::Matrix RandomCovariance() { + const Eigen::Matrix random_matrix = Eigen::Matrix::Random(); + // Random covariance is a random symmetric matrix + return random_matrix.transpose() * random_matrix; +} + +template +Eigen::Matrix AddNoiseToVector(const Eigen::Matrix& vector, const double noise_stddev) { + Eigen::Matrix noisy_vector = vector; + for (int i = 0; i < vector.rows(); ++i) { + noisy_vector(i, 0) += Noise(noise_stddev); + } + return noisy_vector; +} +} // namespace localization_common + +// Add GTEST like MACROS with no namespace so these mirror GTEST calls +// clang-format off +#define EXPECT_MATRIX_NEAR(lhs, rhs, tol) \ + do { \ + EXPECT_PRED3(localization_common::MatrixEquality, lhs.matrix(), rhs.matrix(), tol); \ + } while (0) + +#endif // LOCALIZATION_COMMON_TEST_UTILITIES_H_ diff --git a/localization/localization_common/include/localization_common/time.h b/localization/localization_common/include/localization_common/time.h new file mode 100644 index 0000000000..6e1c62d83e --- /dev/null +++ b/localization/localization_common/include/localization_common/time.h @@ -0,0 +1,27 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_TIME_H_ +#define LOCALIZATION_COMMON_TIME_H_ + +namespace localization_common { +using Time = double; +Time GetTime(const int seconds, const int nanoseconds); +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_TIME_H_ diff --git a/localization/localization_common/include/localization_common/timer.h b/localization/localization_common/include/localization_common/timer.h new file mode 100644 index 0000000000..66deea7307 --- /dev/null +++ b/localization/localization_common/include/localization_common/timer.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_TIMER_H_ +#define LOCALIZATION_COMMON_TIMER_H_ + +#include +#include + +#include +#include +#include + +namespace localization_common { +class Timer { + public: + explicit Timer(const std::string& timer_name = ""); + void Start(); + void Stop(); + void Log() const; + void LogToFile(std::ofstream& ofstream) const; + void LogToCsv(std::ofstream& ofstream) const; + void LogEveryN(const int num_events_per_log) const; + void VlogEveryN(const int num_events_per_log, const int level) const; + void StopAndLog(); + void StopAndLogEveryN(const int num_events_per_log); + void StopAndVlogEveryN(const int num_events_per_log, const int level); + void Vlog(const int level = 2) const; + double last_value() const; + + private: + std::chrono::time_point start_time_; + Averager averager_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_TIMER_H_ diff --git a/localization/localization_common/include/localization_common/timestamped_interpolater.h b/localization/localization_common/include/localization_common/timestamped_interpolater.h new file mode 100644 index 0000000000..d1c55a705c --- /dev/null +++ b/localization/localization_common/include/localization_common/timestamped_interpolater.h @@ -0,0 +1,137 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_TIMESTAMPED_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_TIMESTAMPED_INTERPOLATER_H_ + +#include +#include +#include +#include + +#include + +#include + +#include + +namespace localization_common { +// Empty default interpolater params +struct DefaultInterpolationParams {}; + +// Provides interpolation functions that extends a timestamp set for interpolatable objects. +// Optionally stores params to help with interpolation. +template +class TimestampedInterpolater : public TimestampedSet { + public: + explicit TimestampedInterpolater(const boost::optional max_size = boost::none); + TimestampedInterpolater(const std::vector
 --map-file -o /dir/where/to/save/output.bag -c ` + +## 3. Generate pose/ML features database. +- Inputs: activity\_name, activity\_date, bagOfInterest.bag +- Tools: ~/coverage\_analysis/activity\_db\_generator.py +- Outputs: activity\_database\_file with recommended naming convention "YYYYMMDD\_map\_activity\_db.csv" (The coordinates in the CSV output file +are in the Astrobee's body inertial frame already). + +Run in a terminal +`python ~/coverage_analysis/activity_db_generator.py ` +where +activity date = 20210726 +map name = dali +activity name = KiboEventRehRun1 +location to save = /dir/to/database/file/db.csv +The final file will be called /dir/to/database/file/20210726_dali_KiboEventRehRun1_db.csv + +## 4. Generate a coverage database +- Inputs: activity\_database\_file +- Tools: ~/coverage\_analysis/coverage_analyzer.py +- Outputs: activity\_coverage\_database_file. +- Temporary files: output\_features\_database.csv, output\_features\_database\_nonrepeat.csv + +This step first extracts from activity\_database.csv only the ML feature poses into a temporary file called, for example, 20210518\_cabanel\_features_db.csv or 20210726\_dali\_KiboEvRhRun1\_features_db.csv. + +It then identifies repeated ML features and copies only one of the repeated ML features into a temporary file called, for example, 20210518\_cabanel\_features\_db\_nonrepeat.csv or 20210726\_dali\_KiboEvRhRun1\_features\_db\_nonrepeat.csv These repeated features are unique, however it should be kept in mind that multiple cameras may be able to see the same feature. For visualization purposes is faster to have unique features. + +The next step generates the list of cube centers of size defined by grid\_size along the different walls in the JEM (Overhead, Aft, Forward, Deck). These walls are divided into the 8 bays of the JEM, where 1 corresponds to the bay closest to the entry node and 8 to the bay closest to the airlock. + +If a feature is within the volume of the cube centered at the pose being searched, a counter is increased and the final number of matches and the center of the cube is saved in the activity\_coverage\_database.csv file called, for example, 20210518\_cabanel\_coverage\_db.csv or 20210726\_dali\_KiboEvRhRun1\_coverage\_db.csv. + +The format of this file consist of Number-of-ML-Features-found-within-the-cube-defined-by X-Y-Z-Pose-of-the-center-of-the-cube-checked. + +Run in a terminal +`python ~/coverage_analysis/coverage_analyzer.py ` +where +activity\_database\_file = /dir/where/database/file/is/20210726\_dali\_KiboEvRhRun1\_db.csv, +database\_fileOut = /dir/where/results/are/going/to/be/placed/20210726\_dali\_KiboEvRhRun1\_features\_db.csv, +feat\_only\_fileOut = /dir/where/results/are/going/to/be/placed/20210726\_dali\_KiboEvRhRun1\_features\_db\_nonrepeat.csv, +activity\_coverage\_database\_file = /dir/where/results/are/going/to/be/placed/20210518\_cabanel\_coverage\_db.csv +or +activity\_coverage\_database\_file = /dir/where/results/are/going/to/be/placed/20210726\_dali\_KiboEvRhRun1\_db.csv + +At this point, the user has a few options: +- **1.** Generate a 3D heatmap visualization in Gazebo of the studied map's general coverage of the JEM +This is accomplished following Step 4. +- **2.** Generate a 3D animation and visualization in Gazebo of the studied robot's trajectory coverage at each pose of the trajectory +This is accomplished following Step 5. +- **3.** Generate a statistics report in PDF of the general and detailed coverage of the studied map or robot's trajectory's coverage +This is accomplished following Step 6. + +## 5. Generate Walls Heatmap visualization in Gazebo +- Inputs: activity\_coverage\_database\_file +- Tools: ~/coverage_painter.py, roslaunch +- Outputs: 30cm^3 grid painted as shown in rviz (Gazebo) + +In a terminal, run +`roslaunch astrobee sim.launch dds:=false speed:=0.75 rviz:=true` +to bring RViz up. If the Rviz display looks too cluttered, most topics can be turned off except "Registration". In another terminal run +`python ~/coverage_anaylisis/coverage_painter.py map_coverage` +where +activity\_coverage\_database\_file = /dir/where/results/are/going/to/be/placed/20210518\_cabanel\_coverage\_db.csv + +This publishes cubes representing the map coverage in 20210518_cabanel_coverage_db.csv according to the following color scheme: +- red if 0 registered ML features found in any cube +- orange if 1-10 registered ML features found in any cube +- yellow if 11-20 registered ML features found in any cube +- green if 21-40 registered ML features found in any cube +- blue if 40+ registered ML features found in any cube + +## 6. Generate Robot's Trajectory Heatmap visualization in RViz +- Inputs: activity\_database\_file +- Tools: ~/coverage\_painter.py, roslaunch +- Outputs: Animated trajectory consisting of each of the trajectory poses colored coded according to their coverage level + +In a terminal, run +`roslaunch astrobee sim.launch dds:=false speed:=0.75 rviz:=true` +to bring RViz up. If the Rviz display looks too cluttered, most topics can be turned off except "Registration". In another terminal run +`python ~/coverage/anaylisis/coverage_painter.py robot_coverage` +where +activity\_database\_file = /dir/where/database/file/is/20210726\_dali\_KiboEvRhRun1\_db.csv + +This will publish each pose contained in the trajectory and will color code them according to the following color scheme: +- red if 0 registered ML features found in any cube +- orange if 1-10 registered ML features found in any cube +- yellow if 11-20 registered ML features found in any cube +- green if 21-40 registered ML features found in any cube +- blue if 40+ registered ML features found in any cube + +## 7. Generate stats report +- Inputs: activity\_coverage\_database\_file = /dir/where/map-coverage/file/is/20210518\_cabanel\_coverage\_db.csv or /dir/where/robot-trajectory-coverage/file/is/20210726\_dali\_KiboEvRhRun1\_coverage\_db.csv +- Tools: ~/coverage\_stats\_reporter.py +- Outputs: CSV and PDF Stats reports, saved at the input file's same location +1) CSV: /dir/where/map-coverage/file/is/20210707\_dali\_coverage\_db\_stats.csv or /dir/where/robot-trajector-coverage/file/is/20210726\_dali\_KiboEvRhRun1\_coverage\_db\_stats.csv +2) PDF: /dir/where/map-coverage/file/is/20210707\_dali\_coverage\_db\_stats\_report.pdf or /dir/where/robot-trajector-coverage/file/is/20210726\_dali\_KiboEvRhRun1\_coverage\_db\_stats\_report.pdf + +Generates a statistics report in CSV and PDF of the coverage on each wall (Overhead, Aft, Forward, Deck) and airlock based on the number of registered ML features found on every cube analyzed. +The report consists of two images: +- the general map or bag coverage distribution and the +- the detailed map or bag coverage distribution. + +It provides the coverage percentage based on the number of registered ML features at each wall distributed across five ranges: 0ML, 1-10ML, 11-20ML, 21-40ML, and 40+ML, as well as the total number of registered ML features in the analized wall, and the percentage of the total number of ML features analyzed within the JEM walls and airlock. +In a terminal run +`python ~/coverage_analysis/coverage_stats_reporter.py ` +where +activity\_coverage\_database\_file = /dir/where/map-coverage/file/is/20210518\_cabanel\_coverage\_db.csv +or +activity\_coverage\_database\_file = /dir/where/robot-trajectory-coverage/file/is/20210726\_dali\_KiboEvRhRun1\_coverage\_db.csv + + diff --git a/scripts/run_tests.sh b/scripts/run_tests.sh new file mode 100755 index 0000000000..41726e9e02 --- /dev/null +++ b/scripts/run_tests.sh @@ -0,0 +1,52 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# This script is intended to be run from within your docker +# container. It closely mimics test_astrobee.Dockerfile and the +# testing phase of the GitHub CI. + +script_folder=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) +catkin_ws=$( cd -- "${script_folder}/../.." &> /dev/null && pwd ) + +set -e +set -x + +# The package argument is optional. Default is to test all packages. +package=$1 + +cd ${catkin_ws} + +# compile the tests +catkin build --no-status --force-color ${package} --make-args tests + +# run the tests and build the formal test results +{ catkin build --no-status --force-color ${package} --make-args test -j1 || true; } + +set +x +{ source devel/setup.sh || true; } +set -x + +# collect the test results to determine global success/failure +{ success="true" && catkin_test_results build/${package} || success="false" || true; } + +# if any tests failed, re-run in a different way that provides more debug output +{ [ "$success" = "true" ] || catkin run_tests --no-status --force-color -j1 ${package} || true; } + +# return global success/failure +[ "$success" = "true" ] diff --git a/scripts/setup/add_local_repository.sh b/scripts/setup/add_local_repository.sh index af3514ad4b..1e16ef353d 100755 --- a/scripts/setup/add_local_repository.sh +++ b/scripts/setup/add_local_repository.sh @@ -33,4 +33,7 @@ sudo touch $arssrc #sudo /bin/bash -c "echo \"deb [arch=amd64] http://127.0.0.1:8765/software xenial main\" > $arssrc" || exit 1 #sudo /bin/bash -c "echo \"deb-src http://127.0.0.1:8765/software xenial main\" >> $arssrc" || exit 1 -sudo apt-key add $scriptdir/../../submodules/platform/rootfs/keys/astrobee.key || exit 1 +sudo apt-key add $scriptdir/../../submodules/platform/rootfs/multistrap/keys/astrobee.key || exit 1 + +# Import ssl certificate for access to astrobee.ndc.nasa.gov +$scriptdir/../../submodules/platform/tools/add_local_certs.sh diff --git a/scripts/setup/debians/.gitignore b/scripts/setup/debians/.gitignore index f8d83c9d00..c566fabe07 100644 --- a/scripts/setup/debians/.gitignore +++ b/scripts/setup/debians/.gitignore @@ -6,10 +6,16 @@ *.deb *.ddeb *.buildinfo -libalvar +libar_track_alvar_msgs +libar_track_alvar libdeepdive libdbow2 +libgtsam +libgtsamlib libdbowdlib libopenmvg libdecomputil libjps3d +libmiro +libsoracore +libopencv-xfeatures2d diff --git a/scripts/setup/debians/alvar/changelog b/scripts/setup/debians/alvar/changelog deleted file mode 100644 index 118ea478b8..0000000000 --- a/scripts/setup/debians/alvar/changelog +++ /dev/null @@ -1,29 +0,0 @@ -libalvar (2.0-4) unstable; urgency=medium - - * Ros updated opencv, changed path, needs updated rpath. - - -- Brian Coltin Mon, 30 Apr 2018 13:05:46 -0700 - -libalvar (2.0-3) unstable; urgency=medium - - * Bump ros-kinetic-opencv version. - - -- Brian Coltin Mon, 11 Dec 2017 17:35:08 -0800 - -libalvar (2.0-2) unstable; urgency=medium - - * Build from original tarball now. - - -- Brian Coltin Tue, 01 Aug 2017 15:31:34 -0700 - -libalvar (2.0-1) stable; urgency=medium - - * Changed to the correct version number. - - -- Brian Coltin Fri, 05 May 2017 14:25:42 -0700 - -libalvar (0.1-1) unstable; urgency=medium - - * Initial release. - - -- Brian Coltin Wed, 18 Jan 2017 13:43:06 -0800 diff --git a/scripts/setup/debians/alvar/control b/scripts/setup/debians/alvar/control deleted file mode 100644 index c4ccf79e93..0000000000 --- a/scripts/setup/debians/alvar/control +++ /dev/null @@ -1,22 +0,0 @@ -Source: libalvar -Priority: optional -Maintainer: Brian Coltin -Build-Depends: debhelper (>=9), cmake (>=3.5), ros-kinetic-opencv3 (>=3.3.1-5), libtinyxml-dev (>=2.6) -Standards-Version: 3.9.7 -Section: libs -Homepage: http://virtual.vtt.fi/virtual/proj2/multimedia/alvar/index.html -#Vcs-Git: git://anonscm.debian.org/collab-maint/libalvar.git -#Vcs-Browser: https://anonscm.debian.org/cgit/collab-maint/libalvar.git - -Package: libalvar-dev -Section: libdevel -Architecture: any -Depends: libalvar2 (= ${binary:Version}), ${misc:Depends} -Description: A library for detecting AR tags. Development packcage. - Alvar detects AR tags. The development packcage. - -Package: libalvar2 -Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends} -Description: A library for detecting AR tags. - Alvar detects AR tags. diff --git a/scripts/setup/debians/alvar/copyright b/scripts/setup/debians/alvar/copyright deleted file mode 100644 index 8747885c32..0000000000 --- a/scripts/setup/debians/alvar/copyright +++ /dev/null @@ -1,37 +0,0 @@ -Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ -Upstream-Name: libalvar -Source: http://virtual.vtt.fi/virtual/proj2/multimedia/alvar/ - -Files: * -Copyright: 2016 Alvar Authors -License: LGPL - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - . - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - . - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - -# If you want to use GPL v2 or later for the /debian/* files use -# the following clauses, or change it to suit. Delete these two lines -Files: debian/* -Copyright: 2017 NASA -License: Apache-2 - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - . - http://www.apache.org/licenses/LICENSE-2.0 - . - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/scripts/setup/debians/alvar/libalvar-dev.install b/scripts/setup/debians/alvar/libalvar-dev.install deleted file mode 100644 index c5008f6606..0000000000 --- a/scripts/setup/debians/alvar/libalvar-dev.install +++ /dev/null @@ -1,2 +0,0 @@ -usr/include/* -usr/share/alvar/cmake/* diff --git a/scripts/setup/debians/alvar/libalvar-docs.docs b/scripts/setup/debians/alvar/libalvar-docs.docs deleted file mode 100644 index 7319041140..0000000000 --- a/scripts/setup/debians/alvar/libalvar-docs.docs +++ /dev/null @@ -1,2 +0,0 @@ -README.source -README.Debian diff --git a/scripts/setup/debians/alvar/libalvar2.install b/scripts/setup/debians/alvar/libalvar2.install deleted file mode 100644 index dc21b896ac..0000000000 --- a/scripts/setup/debians/alvar/libalvar2.install +++ /dev/null @@ -1 +0,0 @@ -usr/lib/lib*.so diff --git a/scripts/setup/debians/alvar/patches/better_export.patch b/scripts/setup/debians/alvar/patches/better_export.patch deleted file mode 100644 index 72d63f5168..0000000000 --- a/scripts/setup/debians/alvar/patches/better_export.patch +++ /dev/null @@ -1,60 +0,0 @@ -Index: libalvar/src/CMakeLists.txt -=================================================================== ---- libalvar.orig/src/CMakeLists.txt -+++ libalvar/src/CMakeLists.txt -@@ -42,8 +42,8 @@ set(ALVAR_HEADERS - Container3d.h - Draw.h - EC.h -- FernImageDetector.h -- FernPoseEstimator.h -+ #FernImageDetector.h -+ #FernPoseEstimator.h - FileFormat.h - FileFormatUtils.h - Filter.h -@@ -77,8 +77,8 @@ set(ALVAR_SOURCES - ConnectedComponents.cpp - Draw.cpp - EC.cpp -- FernImageDetector.cpp -- FernPoseEstimator.cpp -+ #FernImageDetector.cpp -+ #FernPoseEstimator.cpp - FileFormatUtils.cpp - Filter.cpp - IntegralImage.cpp -@@ -110,9 +110,9 @@ add_library(ALVAR - ${ALVAR_HEADERS} - ${ALVAR_SOURCES} - ) --set_target_properties(ALVAR PROPERTIES OUTPUT_NAME alvar${ALVAR_VERSION_NODOTS}) -+set_target_properties(ALVAR PROPERTIES OUTPUT_NAME alvar) - set_target_properties(ALVAR PROPERTIES DEBUG_POSTFIX d) --set_target_properties(ALVAR PROPERTIES PUBLIC_HEADER "${ALVAR_HEADERS}") -+set_target_properties(ALVAR PROPERTIES PUBLIC_HEADER "${ALVAR_HEADERS};${ALVAR_GLOBAL_HEADER}") - - # link alvar library target to required libraries - target_link_libraries(ALVAR -@@ -139,8 +139,10 @@ configure_file(../cmake/AlvarConfig.cmak - configure_file(../cmake/AlvarConfigVersion.cmake.in "${PROJECT_BINARY_DIR}/AlvarConfigVersion.cmake" @ONLY) - install(FILES "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/AlvarConfig.cmake" - "${PROJECT_BINARY_DIR}/AlvarConfigVersion.cmake" -- DESTINATION "${CMAKE_INSTALL_PREFIX}/share/alvar/cmake") --install(EXPORT AlvarTargets DESTINATION ${CMAKE_INSTALL_PREFIX}/share/alvar/cmake) -+ DESTINATION "share/alvar/cmake") -+install(EXPORT AlvarTargets DESTINATION lib) -+install(EXPORT AlvarTargets -+ DESTINATION share/alvar/cmake FILE AlvarTargets.cmake) - - # set include directories and libraries variables for alvar - list(APPEND ALVAR_INCLUDE_DIRS -@@ -177,7 +179,7 @@ else(NOT ${MSVC_IDE}) - endif(NOT ${MSVC_IDE}) - - # process sub directories --add_subdirectory(platform) -+#add_subdirectory(platform) - - # add include directories and libraries variables to the cache - list(REMOVE_DUPLICATES ALVAR_INCLUDE_DIRS) diff --git a/scripts/setup/debians/alvar/patches/compile_fixes.patch b/scripts/setup/debians/alvar/patches/compile_fixes.patch deleted file mode 100644 index ab4c3fcde9..0000000000 --- a/scripts/setup/debians/alvar/patches/compile_fixes.patch +++ /dev/null @@ -1,487 +0,0 @@ -Use external tinyxml, fix install for ubuntu, add cmake config script. -Index: libalvar/cmake/AlvarConfig.cmake.in -=================================================================== ---- /dev/null -+++ libalvar/cmake/AlvarConfig.cmake.in -@@ -0,0 +1,8 @@ -+get_filename_component(ALVAR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) -+set(ALVAR_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") -+ -+if (NOT TARGET ALVAR AND NOT ALVAR_BINARY_DIR) -+ include("${ALVAR_CMAKE_DIR}/AlvarTargets.cmake") -+endif() -+ -+set(ALVAR_LIBRARIES ALVAR) -Index: libalvar/cmake/AlvarConfigVersion.cmake.in -=================================================================== ---- /dev/null -+++ libalvar/cmake/AlvarConfigVersion.cmake.in -@@ -0,0 +1,11 @@ -+set(PACKAGE_VERSION "@ALVAR_VERSION@") -+ -+# Check whether the requested PACKAGE_FIND_VERSION is compatible -+if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") -+ set(PACKAGE_VERSION_COMPATIBLE FALSE) -+else() -+ set(PACKAGE_VERSION_COMPATIBLE TRUE) -+ if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") -+ set(PACKAGE_VERSION_EXACT TRUE) -+ endif() -+endif() -Index: libalvar/cmake/FindTinyXML.cmake -=================================================================== ---- /dev/null -+++ libalvar/cmake/FindTinyXML.cmake -@@ -0,0 +1,74 @@ -+################################################################################################## -+# -+# CMake script for finding TinyXML. -+# -+# Input variables: -+# -+# - TinyXML_ROOT_DIR (optional): When specified, header files and libraries will be searched for in -+# ${TinyXML_ROOT_DIR}/include -+# ${TinyXML_ROOT_DIR}/libs -+# respectively, and the default CMake search order will be ignored. When unspecified, the default -+# CMake search order is used. -+# This variable can be specified either as a CMake or environment variable. If both are set, -+# preference is given to the CMake variable. -+# Use this variable for finding packages installed in a nonstandard location, or for enforcing -+# that one of multiple package installations is picked up. -+# -+# -+# Cache variables (not intended to be used in CMakeLists.txt files) -+# -+# - TinyXML_INCLUDE_DIR: Absolute path to package headers. -+# - TinyXML_LIBRARY: Absolute path to library. -+# -+# -+# Output variables: -+# -+# - TinyXML_FOUND: Boolean that indicates if the package was found -+# - TinyXML_INCLUDE_DIRS: Paths to the necessary header files -+# - TinyXML_LIBRARIES: Package libraries -+# -+# -+# Example usage: -+# -+# find_package(TinyXML) -+# if(NOT TinyXML_FOUND) -+# # Error handling -+# endif() -+# ... -+# include_directories(${TinyXML_INCLUDE_DIRS} ...) -+# ... -+# target_link_libraries(my_target ${TinyXML_LIBRARIES}) -+# -+################################################################################################## -+ -+# Get package location hint from environment variable (if any) -+if(NOT TinyXML_ROOT_DIR AND DEFINED ENV{TinyXML_ROOT_DIR}) -+ set(TinyXML_ROOT_DIR "$ENV{TinyXML_ROOT_DIR}" CACHE PATH -+ "TinyXML base directory location (optional, used for nonstandard installation paths)") -+endif() -+ -+# Search path for nonstandard package locations -+if(TinyXML_ROOT_DIR) -+ set(TinyXML_INCLUDE_PATH PATHS "${TinyXML_ROOT_DIR}/include" NO_DEFAULT_PATH) -+ set(TinyXML_LIBRARY_PATH PATHS "${TinyXML_ROOT_DIR}/lib" NO_DEFAULT_PATH) -+endif() -+ -+# Find headers and libraries -+find_path(TinyXML_INCLUDE_DIR NAMES tinyxml.h PATH_SUFFIXES "tinyxml" ${TinyXML_INCLUDE_PATH}) -+find_library(TinyXML_LIBRARY NAMES tinyxml PATH_SUFFIXES "tinyxml" ${TinyXML_LIBRARY_PATH}) -+ -+mark_as_advanced(TinyXML_INCLUDE_DIR -+ TinyXML_LIBRARY) -+ -+# Output variables generation -+include(FindPackageHandleStandardArgs) -+find_package_handle_standard_args(TinyXML DEFAULT_MSG TinyXML_LIBRARY -+ TinyXML_INCLUDE_DIR) -+ -+set(TinyXML_FOUND ${TINYXML_FOUND}) # Enforce case-correctness: Set appropriately cased variable... -+unset(TINYXML_FOUND) # ...and unset uppercase variable generated by find_package_handle_standard_args -+ -+if(TinyXML_FOUND) -+ set(TinyXML_INCLUDE_DIRS ${TinyXML_INCLUDE_DIR}) -+ set(TinyXML_LIBRARIES ${TinyXML_LIBRARY}) -+endif() -Index: libalvar/CMakeLists.txt -=================================================================== ---- libalvar.orig/CMakeLists.txt -+++ libalvar/CMakeLists.txt -@@ -1,10 +1,4 @@ --# minimum version and module directory --if(WIN32) -- cmake_minimum_required(VERSION 2.8.3) --else(WIN32) -- cmake_minimum_required(VERSION 2.8) --endif(WIN32) --set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/build/cmake") -+cmake_minimum_required(VERSION 2.8.3) - - # define project - project(ALVAR) -@@ -12,298 +6,18 @@ project(ALVAR) - # set project version - include(version.cmake OPTIONAL) - --# set project packaging --if(ALVAR_VERSION) # compiling from src package -- if(ALVAR_PACKAGE) # force package -- set(_package ${ALVAR_PACKAGE}) -- else(ALVAR_PACKAGE) # use default src package -- set(_package src) -- endif(ALVAR_PACKAGE) -- set(ALVAR_PACKAGE ${_package} CACHE STRING "ALVAR package type (src: only building source, sdk: building sdk package, bin: building bin package)") --endif(ALVAR_VERSION) -+set(BUILD_SHARED_LIBS ON) -+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - --# set library type --if(ALVAR_PACKAGE) -- if(NOT DEFINED ALVAR_STATIC) -- set(ALVAR_STATIC FALSE) -- endif(NOT DEFINED ALVAR_STATIC) -- set(ALVAR_STATIC ${ALVAR_STATIC} CACHE BOOL "ALVAR library type") -+list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") -+list(APPEND CMAKE_PREFIX_PATH "${CMAKE_SOURCE_DIR}/cmake") - -- if(ALVAR_STATIC) -- set(BUILD_SHARED_LIBS FALSE) -- add_definitions(-DALVAR_STATIC) -- else(ALVAR_STATIC) -- set(BUILD_SHARED_LIBS TRUE) -- endif(ALVAR_STATIC) --endif(ALVAR_PACKAGE) -- --# determine compiler --if(ALVAR_PACKAGE) -- if(WIN32) -- file(WRITE "${CMAKE_BINARY_DIR}/return0.cpp" -- "#include \n" -- "int main() {\n" -- " std::cout << _MSC_VER << std::endl;\n" -- " return 0;\n" -- "}\n" -- ) -- try_run(_run_result _compile_result -- "${CMAKE_BINARY_DIR}" "${CMAKE_BINARY_DIR}/return0.cpp" -- RUN_OUTPUT_VARIABLE _output) -- file(REMOVE "${CMAKE_BINARY_DIR}/return0.cpp") -- if(NOT _run_result AND _compile_result AND _output) -- string(REGEX REPLACE "^\([0-9][0-9][0-9][0-9]\).*" "\\1" _version "${_output}") -- endif(NOT _run_result AND _compile_result AND _output) -- if(_version MATCHES "^1400$") -- set(ALVAR_COMPILER vs2005) -- elseif(_version MATCHES "^1500$") -- set(ALVAR_COMPILER vs2008) -- elseif(_version MATCHES "^1600$") -- set(ALVAR_COMPILER vs2010) -- endif(_version MATCHES "^1400$") -- else(WIN32) -- execute_process(COMMAND ${CMAKE_C_COMPILER} "-dumpversion" RESULT_VARIABLE _result OUTPUT_VARIABLE _output) -- if(NOT _result AND _output) -- string(REGEX REPLACE "^\([0-9]+\)\\.\([0-9]+\)\\.[0-9]+[^0-9]*" "\\1\\2" _version "${_output}") -- endif(NOT _result AND _output) -- set(ALVAR_COMPILER "gcc${_version}") -- endif(WIN32) --endif(ALVAR_PACKAGE) -- --# find package dependencies --if($ENV{OPENCV_ROOT}) -- set(OpenCV_ROOT_DIR $ENV{OPENCV_ROOT} CACHE PATH "The OpenCV root directory") --endif($ENV{OPENCV_ROOT}) --find_package(OpenCV COMPONENTS calib3d core features2d flann highgui imgproc legacy ml video) --if(NOT OpenCV_FOUND) -- find_package(OpenCV COMPONENTS calib3d core features2d flann highgui imgproc legacy ml video -- NO_MODULE NAMES OpenCV -- PATHS ${OpenCV_ROOT_DIR} ${OpenCV_ROOT_DIR}/build ${OpenCV_ROOT_DIR}/opencv/build) -- list(GET OpenCV_LIB_DIR 0 OpenCV_ROOT_DIR) -- string(REGEX REPLACE "/lib$" "" OpenCV_ROOT_DIR ${OpenCV_ROOT_DIR}) -- if(OpenCV_ROOT_DIR MATCHES x86) -- string(REGEX REPLACE "x86" "common/tbb/ia32" OpenCV_TBB_ROOT_DIR ${OpenCV_ROOT_DIR}) -- else(OpenCV_ROOT_DIR MATCHES x86) -- string(REGEX REPLACE "x64" "common/tbb/intel64" OpenCV_TBB_ROOT_DIR ${OpenCV_ROOT_DIR}) -- endif(OpenCV_ROOT_DIR MATCHES x86) -- if(NOT OpenCV_FOUND) -- mark_as_advanced(OpenCV_DIR) -- message(FATAL_ERROR "Could NOT find OpenCV") -- endif(NOT OpenCV_FOUND) --endif(NOT OpenCV_FOUND) --if(WIN32) -- add_definitions(-DNOMINMAX) -- add_definitions(-D_CRT_SECURE_NO_WARNINGS) -- add_definitions(-D_CRT_SECURE_NO_DEPRECATE) --endif(WIN32) -- --find_package(OpenGL) --if(OPENGL_FOUND) -- message(STATUS "Found OpenGL") --else(OPENGL_FOUND) -- message(STATUS "Could NOT find OpenGL") -- if(NOT ALVAR_NOGLUT) -- message(FATAL_ERROR "Failing since GLUT support was not turned off via ALVAR_NOGLUT") -- endif(NOT ALVAR_NOGLUT) --endif(OPENGL_FOUND) -- --set(GLUT_ROOT_PATH $ENV{GLUT_ROOT} CACHE PATH "The GLUT root directory") --if(WIN32) -- # Overriding versions in FindGLUT -- find_path(GLUT_INCLUDE_DIR NAMES glut.h -- PATHS -- ${GLUT_ROOT_PATH} -- ${GLUT_ROOT_PATH}/GL -- ${GLUT_ROOT_PATH}/include) -- find_library(GLUT_glut_LIBRARY NAMES glut32 -- PATHS -- ${GLUT_ROOT_PATH} -- ${GLUT_ROOT_PATH}/lib -- ${GLUT_ROOT_PATH}/Release) --endif(WIN32) --find_package(GLUT) --if(GLUT_FOUND) -- message(STATUS "Found GLUT: ${GLUT_ROOT_PATH}") --else(GLUT_FOUND) -- message(STATUS "Could NOT find GLUT") -- message(STATUS " Some samples will not compile!") -- message(STATUS " Some tests will not compile!") -- if(NOT ALVAR_NOGLUT) -- message(FATAL_ERROR "Failing since GLUT support was not turned off via ALVAR_NOGLUT") -- endif(NOT ALVAR_NOGLUT) --endif(GLUT_FOUND) -- --set(OSG_ROOT_DIR $ENV{OSG_ROOT} CACHE PATH "The OSG root directory") --set(ENV{OSG_DIR} ${OSG_ROOT_DIR}) --set(OpenSceneGraph_MARK_AS_ADVANCED TRUE) --find_package(OpenSceneGraph COMPONENTS osgViewer osgGA osgDB osgUtil osgText) -- --if(ALVAR_PACKAGE) -- find_package(CMU) -- -- # the PointGrey plugin is currently experimental and not included in the build by default -- set(ALVAR_USE_FLYCAPTURE2 ${ALVAR_USE_FLYCAPTURE2} CACHE BOOL "To enable PointGrey FlyCapture2 plugin") -- if(ALVAR_USE_FLYCAPTURE2) -- set(FLYCAPTURE2_ROOT_PATH $ENV{FLYCAPTURE2_ROOT_PATH} CACHE PATH "The FlyCapture2 root directory") -- set(FLYCAPTURE2_INCLUDE_DIRS "${FLYCAPTURE2_ROOT_PATH}/include") -- set(FLYCAPTURE2_LIBRARIES "${FLYCAPTURE2_ROOT_PATH}/lib/FlyCapture2.lib") -- set(FLYCAPTURE2_LIBRARIES_DEBUG "${FLYCAPTURE2_ROOT_PATH}/lib/FlyCapture2.lib") -- endif(ALVAR_USE_FLYCAPTURE2) -- -- # the DSCapture plugin is currently experimental and not included in the build by default -- set(ALVAR_USE_DSCAPTURE ${ALVAR_USE_DSCAPTURE} CACHE BOOL "To enable DSCapture plugin") -- if(ALVAR_USE_DSCAPTURE) -- set(DSCAPTURE_ROOT_PATH $ENV{DSCAPTURE_ROOT_PATH} CACHE PATH "The DSCapture root directory") -- set(DSCAPTURE_INCLUDE_DIRS "${DSCAPTURE_ROOT_PATH}") -- set(DSCAPTURE_LIBRARIES "${DSCAPTURE_ROOT_PATH}/lib/DSCapture.lib") -- set(DSCAPTURE_LIBRARIES_DEBUG "${DSCAPTURE_ROOT_PATH}/lib/DSCaptured.lib") -- endif(ALVAR_USE_DSCAPTURE) --endif(ALVAR_PACKAGE) -- --if(NOT ALVAR_PACKAGE) -- set(ALVAR_ROOT_DIR ${CMAKE_SOURCE_DIR}) -- find_package(ALVAR REQUIRED) --endif(NOT ALVAR_PACKAGE) -- --if(NOT ALVAR_PACKAGE) -- # set install target -- set(CMAKE_INSTALL_PREFIX ${CMAKE_BINARY_DIR}/../../ CACHE STRING "Directory to install files for packaging" FORCE) --endif(NOT ALVAR_PACKAGE) -- --if(ALVAR_PACKAGE) -- # add debug definition on unix -- if(UNIX) -- if(CMAKE_BUILD_TYPE MATCHES Debug) -- add_definitions(-D_DEBUG) -- endif(CMAKE_BUILD_TYPE MATCHES Debug) -- endif(UNIX) -- -- # do not include manifests for plugins (workaround for windows xp sp2) -- if(MSVC) -- if(MSVC_VERSION GREATER 1310) -- set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} /MANIFEST:NO") -- endif(MSVC_VERSION GREATER 1310) -- endif(MSVC) --endif(ALVAR_PACKAGE) -- --# set install rpath to current directory such that applications can find libraries that are located in the same directory --SET(CMAKE_INSTALL_RPATH ".") -- --# disable warning C4251 and C4275 on windows --if(WIN32) -- set(_build_types -- Debug -- Release -- RelWithDebInfo -- MinSizeRel -- ) -- foreach(_type ${_build_types}) -- string(TOUPPER ${_type} _type) -- set(CMAKE_C_FLAGS_${_type} "${CMAKE_C_FLAGS_${_type}} /wd4251 /wd4275" CACHE STRING "Force static linking of runtime libraries" FORCE) -- set(CMAKE_CXX_FLAGS_${_type} "${CMAKE_CXX_FLAGS_${_type}} /wd4251 /wd4275" CACHE STRING "Force static linking of runtime libraries" FORCE) -- endforeach(_type ${CMAKE_CONFIGURATION_TYPES}) --endif(WIN32) -- --# install runtime libraries --if(ALVAR_PACKAGE) -- set(CMAKE_INSTALL_SYSTEM_RUNTIME_LIBS_SKIP TRUE) -- include(InstallRequiredSystemLibraries) -- install(PROGRAMS ${CMAKE_INSTALL_SYSTEM_RUNTIME_LIBS} DESTINATION bin) --endif(ALVAR_PACKAGE) -- --# install files --if(ALVAR_PACKAGE MATCHES sdk) -- install(FILES CMakeLists.txt DESTINATION .) -- install(FILES README DESTINATION .) --endif(ALVAR_PACKAGE MATCHES sdk) --install(FILES LICENSE DESTINATION .) -- --# hack to setup environment runtime path --set(ALVAR_RUNTIME) --list(APPEND ALVAR_RUNTIME -- ${OpenCV_ROOT_DIR}/bin\; -- ${GLUT_ROOT_PATH}\; -- ${OSG_ROOT_DIR}/bin\; --) --if(OpenCV_TBB_ROOT_DIR) -- list(APPEND ALVAR_RUNTIME -- ${OpenCV_TBB_ROOT_DIR}\; -- ) --endif(OpenCV_TBB_ROOT_DIR) --if(NOT ALVAR_PACKAGE) -- list(APPEND ALVAR_RUNTIME -- ${ALVAR_ROOT_DIR}/bin\; -- ) --endif(NOT ALVAR_PACKAGE) -- --# attempt to find testing framework --if(ALVAR_PACKAGE) -- find_package(CxxTest) -- find_package(Perl) -- if(CxxTest_FOUND AND PERL_FOUND) -- enable_testing() -- if(ALVAR_PACKAGE MATCHES sdk|bin) -- include(CTest) -- endif(ALVAR_PACKAGE MATCHES sdk|bin) -- else(CxxTest_FOUND AND PERL_FOUND) -- message(STATUS " Tests will not run!") -- endif(CxxTest_FOUND AND PERL_FOUND) --endif(ALVAR_PACKAGE) -- --# attempt to enable packaging support --if(ALVAR_PACKAGE MATCHES sdk|bin) -- include(CPackConfig.cmake) -- include(CPack) --endif(ALVAR_PACKAGE MATCHES sdk|bin) -+list(APPEND CMAKE_INSTALL_RPATH "/opt/ros/kinetic/lib" "/opt/ros/kinetic/lib/x86_64-linux-gnu" "/opt/ros/kinetic/lib/arm-linux-gnueabihf") # find opencv3 -+find_path(OpenCV_DIR OpenCVConfig.cmake PATHS /opt/ros/kinetic/share/ -+ PATH_SUFFIXES OpenCV-3.1.0 OpenCV-3.1.0-dev OpenCV-3.2.0 OpenCV-3.2.0-dev OpenCV-3.3.1 OpenCV-3.3.1-dev) -+find_package(OpenCV REQUIRED) -+find_package(TinyXML REQUIRED) - - # process sub directories --if(ALVAR_PACKAGE) -- add_subdirectory(3rdparty) -- add_subdirectory(src) -- add_subdirectory(doc) -- add_subdirectory(test) --endif(ALVAR_PACKAGE) --add_subdirectory(sample) --if(OPENSCENEGRAPH_FOUND) -- add_subdirectory(demo) --endif(OPENSCENEGRAPH_FOUND) --add_subdirectory(build) --add_subdirectory(data) -- --# hack macro to help copy runtime dependencies --macro(copy_runtime _libraries _runtimes _prefixes) -- set(_type none) -- foreach(_library ${_libraries}) -- if(_library MATCHES "^optimized$") -- set(_add FALSE) -- set(_type optimized) -- elseif(_library MATCHES "^debug$") -- set(_add FALSE) -- set(_type debug) -- else(_library MATCHES "^optimized$") -- if(_type MATCHES none|optimized) -- -- string(REGEX REPLACE "^.*/([a-zA-Z0-9_\\-]+)\\.lib$" "\\1" _name "${_library}") -- set(_runtime _runtime-NOTFOUND) -- find_file(_runtime ${_name}.dll PATHS ${_runtimes} NO_DEFAULT_PATH) -- foreach(_prefix ${_prefixes}) -- find_file(_runtime ${_prefix}${_name}.dll PATHS ${_runtimes} NO_DEFAULT_PATH) -- endforeach(_prefix) -- -- install(FILES ${_runtime} DESTINATION bin) -- -- endif(_type MATCHES none|optimized) -- set(_type none) -- endif(_library MATCHES "^optimized$") -- endforeach(_library) --endmacro(copy_runtime) -+add_subdirectory(src) - --# hack to copy runtime dependencies --if(ALVAR_PACKAGE MATCHES bin AND WIN32) -- copy_runtime("${OpenCV_LIBRARIES}" "${ALVAR_RUNTIME}" "") -- copy_runtime("${GLUT_LIBRARIES}" "${ALVAR_RUNTIME}" "") -- set(_prefixes osg55- osg74- ot11-) -- copy_runtime("${OPENSCENEGRAPH_LIBRARIES}" "${ALVAR_RUNTIME}" "${_prefixes}") -- find_file(_osgplugin osgdb_osg.dll PATHS ${ALVAR_RUNTIME} PATH_SUFFIXES osgPlugins-${OPENSCENEGRAPH_VERSION} NO_DEFAULT_PATH) -- install(FILES ${_osgplugin} DESTINATION bin) --endif(ALVAR_PACKAGE MATCHES bin AND WIN32) -Index: libalvar/src/CMakeLists.txt -=================================================================== ---- libalvar.orig/src/CMakeLists.txt -+++ libalvar/src/CMakeLists.txt -@@ -112,11 +112,12 @@ add_library(ALVAR - ) - set_target_properties(ALVAR PROPERTIES OUTPUT_NAME alvar${ALVAR_VERSION_NODOTS}) - set_target_properties(ALVAR PROPERTIES DEBUG_POSTFIX d) -+set_target_properties(ALVAR PROPERTIES PUBLIC_HEADER "${ALVAR_HEADERS}") - - # link alvar library target to required libraries - target_link_libraries(ALVAR - ${OpenCV_LIBRARIES} -- TinyXML -+ ${TinyXML_LIBRARIES} - ) - - # install headers -@@ -125,7 +126,21 @@ if(ALVAR_PACKAGE MATCHES sdk) - endif(ALVAR_PACKAGE MATCHES sdk) - - # install target --install(TARGETS ALVAR DESTINATION bin) -+install(TARGETS ALVAR -+ EXPORT AlvarTargets -+ LIBRARY DESTINATION lib -+ PUBLIC_HEADER DESTINATION include/alvar) -+ -+file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/share/alvar/cmake" "${CMAKE_INSTALL_PREFIX}/include") -+set(CONF_INCLUDE_DIRS "\${ALVAR_CMAKE_DIR}/${REL_INCLUDE_DIR}") -+export(TARGETS ALVAR FILE "${PROJECT_BINARY_DIR}/AlvarTargets.cmake") -+export(PACKAGE ALVAR) -+configure_file(../cmake/AlvarConfig.cmake.in "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/AlvarConfig.cmake" @ONLY) -+configure_file(../cmake/AlvarConfigVersion.cmake.in "${PROJECT_BINARY_DIR}/AlvarConfigVersion.cmake" @ONLY) -+install(FILES "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/AlvarConfig.cmake" -+ "${PROJECT_BINARY_DIR}/AlvarConfigVersion.cmake" -+ DESTINATION "${CMAKE_INSTALL_PREFIX}/share/alvar/cmake") -+install(EXPORT AlvarTargets DESTINATION ${CMAKE_INSTALL_PREFIX}/share/alvar/cmake) - - # set include directories and libraries variables for alvar - list(APPEND ALVAR_INCLUDE_DIRS -Index: libalvar/src/Util.h -=================================================================== ---- libalvar.orig/src/Util.h -+++ libalvar/src/Util.h -@@ -39,6 +39,7 @@ - #include - #include - #include -+#include - #include //for abs - #include - diff --git a/scripts/setup/debians/alvar/patches/fix_tinyxml_cross_compile.patch b/scripts/setup/debians/alvar/patches/fix_tinyxml_cross_compile.patch deleted file mode 100644 index 224a9a768e..0000000000 --- a/scripts/setup/debians/alvar/patches/fix_tinyxml_cross_compile.patch +++ /dev/null @@ -1,19 +0,0 @@ -Index: libalvar/cmake/FindTinyXML.cmake -=================================================================== ---- libalvar.orig/cmake/FindTinyXML.cmake -+++ libalvar/cmake/FindTinyXML.cmake -@@ -68,7 +68,13 @@ find_package_handle_standard_args(TinyXM - set(TinyXML_FOUND ${TINYXML_FOUND}) # Enforce case-correctness: Set appropriately cased variable... - unset(TINYXML_FOUND) # ...and unset uppercase variable generated by find_package_handle_standard_args - -+get_filename_component(TINYXML_LIB_TMP "${TinyXML_LIBRARY}" PATH) -+set(TINYXML_LIB_DIR ${TINYXML_LIB_TMP} CACHE INTERNAL "TinyXML library directory." FORCE) -+add_library(tinyxml UNKNOWN IMPORTED ) -+set_property(TARGET tinyxml PROPERTY IMPORTED_LOCATION "${TinyXML_LIBRARY}" ) -+set_property(TARGET tinyxml PROPERTY INTERFACE_INCLUDE_DIRECTORIES "${TinyXML_INCLUDE_DIR}") -+ - if(TinyXML_FOUND) - set(TinyXML_INCLUDE_DIRS ${TinyXML_INCLUDE_DIR}) -- set(TinyXML_LIBRARIES ${TinyXML_LIBRARY}) -+ set(TinyXML_LIBRARIES tinyxml) - endif() diff --git a/scripts/setup/debians/alvar/patches/remove_print.patch b/scripts/setup/debians/alvar/patches/remove_print.patch deleted file mode 100644 index 5a85495a1b..0000000000 --- a/scripts/setup/debians/alvar/patches/remove_print.patch +++ /dev/null @@ -1,21 +0,0 @@ -Index: libalvar/src/Alvar.cpp -=================================================================== ---- libalvar.orig/src/Alvar.cpp -+++ libalvar/src/Alvar.cpp -@@ -29,11 +29,11 @@ namespace alvar { - - void alvarInfo() - { -- std::cerr << "ALVAR " << ALVAR_VERSION << " - A Library for Virtual and Augmented Reality" << std::endl; -- std::cerr << "Copyright 2007-2012 VTT Technical Research Centre of Finland" << std::endl; -- std::cerr << "Licensed under the GNU Lesser General Public License" << std::endl; -- std::cerr << "Built on " << ALVAR_DATE << " for " << ALVAR_SYSTEM << std::endl; -- std::cerr << std::endl; -+ //std::cerr << "ALVAR " << ALVAR_VERSION << " - A Library for Virtual and Augmented Reality" << std::endl; -+ //std::cerr << "Copyright 2007-2012 VTT Technical Research Centre of Finland" << std::endl; -+ //std::cerr << "Licensed under the GNU Lesser General Public License" << std::endl; -+ //std::cerr << "Built on " << ALVAR_DATE << " for " << ALVAR_SYSTEM << std::endl; -+ //std::cerr << std::endl; - } - - struct AlvarLoader { diff --git a/scripts/setup/debians/alvar/patches/series b/scripts/setup/debians/alvar/patches/series deleted file mode 100644 index 06b9fe08b2..0000000000 --- a/scripts/setup/debians/alvar/patches/series +++ /dev/null @@ -1,4 +0,0 @@ -compile_fixes.patch -fix_tinyxml_cross_compile.patch -better_export.patch -remove_print.patch diff --git a/scripts/setup/debians/alvar/rules b/scripts/setup/debians/alvar/rules deleted file mode 100755 index 381ca0ea81..0000000000 --- a/scripts/setup/debians/alvar/rules +++ /dev/null @@ -1,25 +0,0 @@ -#!/usr/bin/make -f -# See debhelper(7) (uncomment to enable) -# output every command that modifies files on the build system. -#export DH_VERBOSE = 1 - - -# see FEATURE AREAS in dpkg-buildflags(1) -#export DEB_BUILD_MAINT_OPTIONS = hardening=+all - -# see ENVIRONMENT in dpkg-buildflags(1) -# package maintainers to append CFLAGS -#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic -# package maintainers to append LDFLAGS -#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed - - -%: - dh $@ --buildsystem=cmake - - -# dh_make generated override targets -# This is example for Cmake (See https://bugs.debian.org/641051 ) -#override_dh_auto_configure: -# dh_auto_configure -- # -DCMAKE_LIBRARY_PATH=$(DEB_HOST_MULTIARCH) - diff --git a/scripts/setup/debians/ar-track-alvar-msgs/changelog b/scripts/setup/debians/ar-track-alvar-msgs/changelog new file mode 100644 index 0000000000..0bc4661761 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar-msgs/changelog @@ -0,0 +1,7 @@ +ros-noetic-ar-track-alvar-msgs (0.7.1-0astrobee) focal; urgency=high + + * Package for ROS Noetic + + -- Ruben Garcia Thu, 2 May 2024 10:00:00 -0000 + + diff --git a/scripts/setup/debians/alvar/compat b/scripts/setup/debians/ar-track-alvar-msgs/compat similarity index 100% rename from scripts/setup/debians/alvar/compat rename to scripts/setup/debians/ar-track-alvar-msgs/compat diff --git a/scripts/setup/debians/ar-track-alvar-msgs/control b/scripts/setup/debians/ar-track-alvar-msgs/control new file mode 100644 index 0000000000..c3f3794274 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar-msgs/control @@ -0,0 +1,12 @@ +Source: ros-noetic-ar-track-alvar-msgs +Section: misc +Priority: optional +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-geometry-msgs, ros-noetic-message-generation, ros-noetic-std-msgs +Homepage: http://ros.org/wiki/ar_track_alvar +Standards-Version: 3.9.2 + +Package: ros-noetic-ar-track-alvar-msgs +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-geometry-msgs, ros-noetic-message-runtime, ros-noetic-std-msgs +Description: This package is a ROS wrapper for Alvar, an open source AR tag tracking library. diff --git a/scripts/setup/debians/ar-track-alvar-msgs/rules b/scripts/setup/debians/ar-track-alvar-msgs/rules new file mode 100755 index 0000000000..d8e0969f0e --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar-msgs/rules @@ -0,0 +1,67 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG +ifneq ($(filter nocheck,$(DEB_BUILD_OPTIONS)),) + BUILD_TESTING_ARG=-DBUILD_TESTING=OFF -DCATKIN_ENABLE_TESTING=OFF +endif + +DEB_HOST_GNU_TYPE ?= $(shell dpkg-architecture -qDEB_HOST_GNU_TYPE) + +%: + dh $@ -v --buildsystem=cmake --builddirectory=.obj-$(DEB_HOST_GNU_TYPE) + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" \ + $(BUILD_TESTING_ARG) + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-ar-track-alvar-msgs//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/alvar/source/format b/scripts/setup/debians/ar-track-alvar-msgs/source/format similarity index 100% rename from scripts/setup/debians/alvar/source/format rename to scripts/setup/debians/ar-track-alvar-msgs/source/format diff --git a/scripts/setup/debians/ar-track-alvar-msgs/source/options b/scripts/setup/debians/ar-track-alvar-msgs/source/options new file mode 100644 index 0000000000..8bc9182a24 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar-msgs/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/scripts/setup/debians/ar-track-alvar/changelog b/scripts/setup/debians/ar-track-alvar/changelog new file mode 100644 index 0000000000..b406e20fb4 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar/changelog @@ -0,0 +1,5 @@ +ros-noetic-ar-track-alvar (0.7.1-0astrobee) focal; urgency=high + + * Package for ROS Noetic + + -- Ruben Garcia Thu, 2 May 2024 10:00:00 -0000 diff --git a/scripts/setup/debians/ar-track-alvar/compat b/scripts/setup/debians/ar-track-alvar/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/ar-track-alvar/control b/scripts/setup/debians/ar-track-alvar/control new file mode 100644 index 0000000000..22439d6a73 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar/control @@ -0,0 +1,12 @@ +Source: ros-noetic-ar-track-alvar +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), libtinyxml-dev, ros-noetic-ar-track-alvar-msgs, ros-noetic-catkin, ros-noetic-cmake-modules, ros-noetic-cv-bridge, ros-noetic-dynamic-reconfigure, ros-noetic-geometry-msgs, ros-noetic-image-transport, ros-noetic-message-generation, ros-noetic-resource-retriever, ros-noetic-rosbag, ros-noetic-roscpp, ros-noetic-rospy, ros-noetic-rostest, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-tf, ros-noetic-tf2, ros-noetic-visualization-msgs +Homepage: http://ros.org/wiki/ar_track_alvar +Standards-Version: 3.9.2 + +Package: ros-noetic-ar-track-alvar +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, libtinyxml-dev, ros-noetic-ar-track-alvar-msgs, ros-noetic-cv-bridge, ros-noetic-dynamic-reconfigure, ros-noetic-geometry-msgs, ros-noetic-image-transport, ros-noetic-message-runtime, ros-noetic-resource-retriever, ros-noetic-roscpp, ros-noetic-rospy, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-tf, ros-noetic-tf2, ros-noetic-visualization-msgs +Description: This package is a ROS wrapper for Alvar, an open source AR tag tracking library. diff --git a/scripts/setup/debians/ar-track-alvar/patches/remove_pcl.patch b/scripts/setup/debians/ar-track-alvar/patches/remove_pcl.patch new file mode 100644 index 0000000000..53da8f3ef0 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar/patches/remove_pcl.patch @@ -0,0 +1,117 @@ +Index: ar_track_alvar/CMakeLists.txt +=================================================================== +--- ar_track_alvar.orig/CMakeLists.txt ++++ ar_track_alvar/CMakeLists.txt +@@ -9,8 +9,6 @@ find_package(catkin REQUIRED COMPONENTS + geometry_msgs + image_transport + message_generation +- pcl_conversions +- pcl_ros + resource_retriever + roscpp + sensor_msgs +@@ -22,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS + find_package(Eigen3 REQUIRED) + find_package(OpenCV REQUIRED) + find_package(TinyXML REQUIRED) ++find_package(PCL REQUIRED COMPONENTS common) + + # dynamic reconfigure support + generate_dynamic_reconfigure_options(cfg/Params.cfg) +@@ -36,8 +35,6 @@ catkin_package( + geometry_msgs + image_transport + message_runtime +- pcl_conversions +- pcl_ros + resource_retriever + roscpp + sensor_msgs +@@ -51,6 +48,7 @@ include_directories(include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${TinyXML_INCLUDE_DIRS} ++ ${PCL_INCLUDE_DIRS} + + ) + +@@ -85,43 +83,10 @@ add_library(${PROJECT_NAME} + src/Threads_unix.cpp + src/Util.cpp + ) +-target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${TinyXML_LIBRARIES} ${catkin_LIBRARIES}) ++target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${TinyXML_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) + add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +-# Kinect filtering code +-add_library(kinect_filtering src/kinect_filtering.cpp) +-target_link_libraries(kinect_filtering ${catkin_LIBRARIES}) +-add_dependencies(kinect_filtering ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +- +-add_library(medianFilter src/medianFilter.cpp) +-target_link_libraries(medianFilter ${PROJECT_NAME} ${catkin_LIBRARIES}) +-add_dependencies(medianFilter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +- +-add_executable(individualMarkers nodes/IndividualMarkers.cpp) +-target_link_libraries(individualMarkers ${PROJECT_NAME} kinect_filtering ${catkin_LIBRARIES}) +-add_dependencies(individualMarkers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +- +-add_executable(individualMarkersNoKinect nodes/IndividualMarkersNoKinect.cpp) +-target_link_libraries(individualMarkersNoKinect ${PROJECT_NAME} ${catkin_LIBRARIES}) +-add_dependencies(individualMarkersNoKinect ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +- +-add_executable(trainMarkerBundle nodes/TrainMarkerBundle.cpp) +-target_link_libraries(trainMarkerBundle ${PROJECT_NAME} ${catkin_LIBRARIES}) +-add_dependencies(trainMarkerBundle ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +- +-add_executable(findMarkerBundles nodes/FindMarkerBundles.cpp) +-target_link_libraries(findMarkerBundles ${PROJECT_NAME} kinect_filtering medianFilter ${catkin_LIBRARIES}) +-add_dependencies(findMarkerBundles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +- +-add_executable(findMarkerBundlesNoKinect nodes/FindMarkerBundlesNoKinect.cpp) +-target_link_libraries(findMarkerBundlesNoKinect ${PROJECT_NAME} ${catkin_LIBRARIES}) +-add_dependencies(findMarkerBundlesNoKinect ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +- +-add_executable(createMarker src/SampleMarkerCreator.cpp) +-target_link_libraries(createMarker ${PROJECT_NAME} ${catkin_LIBRARIES}) +-add_dependencies(createMarker ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +- +-install(TARGETS ${PROJECT_NAME} createMarker findMarkerBundles findMarkerBundlesNoKinect individualMarkers individualMarkersNoKinect kinect_filtering medianFilter trainMarkerBundle ++install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +Index: ar_track_alvar/include/ar_track_alvar/filter/kinect_filtering.h +=================================================================== +--- ar_track_alvar.orig/include/ar_track_alvar/filter/kinect_filtering.h ++++ ar_track_alvar/include/ar_track_alvar/filter/kinect_filtering.h +@@ -39,7 +39,6 @@ + #ifndef AR_TRACK_ALVAR_KINECT_FILTERING_H + #define AR_TRACK_ALVAR_KINECT_FILTERING_H + +-#include + #include + #include + #include +@@ -51,7 +50,6 @@ + #include + #include + #include +-#include + #include + #include + #include +Index: ar_track_alvar/package.xml +=================================================================== +--- ar_track_alvar.orig/package.xml ++++ ar_track_alvar/package.xml +@@ -22,8 +22,6 @@ + dynamic_reconfigure + geometry_msgs + image_transport +- pcl_ros +- pcl_conversions + resource_retriever + roscpp + sensor_msgs diff --git a/scripts/setup/debians/ar-track-alvar/patches/series b/scripts/setup/debians/ar-track-alvar/patches/series new file mode 100644 index 0000000000..d9bd0fcfb6 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar/patches/series @@ -0,0 +1 @@ +remove_pcl.patch \ No newline at end of file diff --git a/scripts/setup/debians/ar-track-alvar/rules b/scripts/setup/debians/ar-track-alvar/rules new file mode 100755 index 0000000000..22a44276fc --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar/rules @@ -0,0 +1,62 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +export DH_OPTIONS=-v --buildsystem=cmake +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-ar-track-alvar//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/ar-track-alvar/source/format b/scripts/setup/debians/ar-track-alvar/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/ar-track-alvar/source/options b/scripts/setup/debians/ar-track-alvar/source/options new file mode 100644 index 0000000000..8bc9182a24 --- /dev/null +++ b/scripts/setup/debians/ar-track-alvar/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/scripts/setup/debians/build_alvar.sh b/scripts/setup/debians/build_alvar.sh deleted file mode 100755 index 0b92adb9b7..0000000000 --- a/scripts/setup/debians/build_alvar.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -PACKAGE_NAME=libalvar -ORIG_TAR=libalvar_2.0.orig.tar.gz -DEB_DIR=alvar - -if [ -d $PACKAGE_NAME ]; then - rm -rf $PACKAGE_NAME -fi -git clone --quiet https://github.com/astanin/mirror-alvar.git $PACKAGE_NAME 2>&1 || exit 1 -cd $PACKAGE_NAME -git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 -cp -r ../$DEB_DIR debian -debuild -us -uc || exit 1 -cd .. diff --git a/scripts/setup/debians/build_ar-track-alvar-msgs.sh b/scripts/setup/debians/build_ar-track-alvar-msgs.sh new file mode 100755 index 0000000000..64634a6a85 --- /dev/null +++ b/scripts/setup/debians/build_ar-track-alvar-msgs.sh @@ -0,0 +1,35 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=libar_track_alvar_msgs +ORIG_TAR=ros-noetic-ar-track-alvar-msgs_0.7.1.orig.tar.gz +DEB_DIR=ar_track_alvar_msgs +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone --quiet -b noetic-devel https://github.com/ros-perception/ar_track_alvar.git $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME/$DEB_DIR +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../../ar-track-alvar-msgs debian +debuild -us -uc || exit 1 +cd ../.. + +mv $PACKAGE_NAME/*ar-track-alvar*{.deb,.debian.tar.xz,.orig.tar.gz,.dsc} . \ No newline at end of file diff --git a/scripts/setup/debians/build_ar-track-alvar.sh b/scripts/setup/debians/build_ar-track-alvar.sh new file mode 100755 index 0000000000..f47866773e --- /dev/null +++ b/scripts/setup/debians/build_ar-track-alvar.sh @@ -0,0 +1,35 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=libar_track_alvar +ORIG_TAR=ros-noetic-ar-track-alvar_0.7.1.orig.tar.gz +DEB_DIR=ar_track_alvar +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone --quiet -b noetic-devel https://github.com/ros-perception/ar_track_alvar.git $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME/$DEB_DIR +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../../ar-track-alvar debian +debuild -us -uc || exit 1 +cd ../.. + +mv $PACKAGE_NAME/*ar-track-alvar*{.deb,.debian.tar.xz,.orig.tar.gz,.dsc} . \ No newline at end of file diff --git a/scripts/setup/debians/build_dbow2.sh b/scripts/setup/debians/build_dbow2.sh index f44a088591..28144754de 100755 --- a/scripts/setup/debians/build_dbow2.sh +++ b/scripts/setup/debians/build_dbow2.sh @@ -2,15 +2,15 @@ # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -20,6 +20,7 @@ PACKAGE_NAME=libdbow2 ORIG_TAR=libdbow2_0.1.orig.tar.gz DEB_DIR=dbow2 +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) if [ -d $PACKAGE_NAME ]; then rm -rf $PACKAGE_NAME @@ -28,5 +29,6 @@ git clone --quiet https://github.com/dorian3d/DBoW2.git $PACKAGE_NAME --branch v cd $PACKAGE_NAME git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 cp -r ../$DEB_DIR debian +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" debuild -us -uc || exit 1 cd .. diff --git a/scripts/setup/debians/build_debians.sh b/scripts/setup/debians/build_debians.sh new file mode 100755 index 0000000000..afd9587220 --- /dev/null +++ b/scripts/setup/debians/build_debians.sh @@ -0,0 +1,91 @@ +#!/bin/bash -e +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Build and install flight software custom dependencies. +set -e + +debian_loc=$(dirname "$(readlink -f "$0")") +dist=$(. /etc/os-release && echo $UBUNTU_CODENAME) +build_list=() +install_debians=false +install_deps=false + +die () { + printf "${c_red}${c_bold}[ FATAL ]${c_reset} ${c_bold}%b${c_reset}\\n" "$1" >&2 + exit "${2:-1}" +} + +# Set LANG so perl doesn't complain all the time on chroot +[[ -z $LANG ]] && export LANG=C + +# delete old files (-f avoids 'no such file' warning on first run) +rm -f *.deb *.debian.tar.xz *.orig.tar.gz *.dsc *.build *.buildinfo *.changes *.ddeb + +# Process arguments and take action +while [[ $# -gt 0 ]]; do + case "$1" in + -i|--install) + install_debians=true + ;; + -d|--install-with-deps) + install_debians=true + install_deps=true + # Packages to find and build dependencies + sudo apt-get install -y devscripts equivs libproj-dev || die + # Dependencies for jps3d + if [[ $dist == "focal" ]]; then + sudo apt-get install -y libvtk7.1p libboost-filesystem1.71.0 libboost-system1.71.0 || die + fi + ;; + esac + shift +done + +# Build opencv if ubuntu 18 or 20 +#[[ "$dist" =~ ^bionic|focal$ ]] && build_list+=( opencv ) + + +# Add public debians to build list +build_list+=( alvar dlib dbow2 gtsam decomputil jps3d openmvg ) +# If restricted rti-dev debian is present, add miro and soracore as well +dpkg-query -W -f='${Status}\n' rti-dev 2>&1 | grep -q "install ok installed" && +echo "Package rti-dev exists. Including miro and soracore to build list..." && +build_list+=( miro soracore ) + +echo "Building debians, this may take a while..." + +export DEBEMAIL="nospam@nospam.org" +for pkg in ${build_list[@]} +do + # Dependencies + if $install_deps ; then + cd ${debian_loc}/$pkg || die + sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control || + die "Failed to install dependencies for $pkg" + fi + + # Building + cd "$debian_loc" + ./build_${pkg}.sh || die "Failed to build $pkg" + + # Installing + if $install_debians ; then + sudo dpkg -i *${pkg}*.deb || die "Failed to install $pkg" + fi +done diff --git a/scripts/setup/debians/build_decomputil.sh b/scripts/setup/debians/build_decomputil.sh index 1186ee4ed2..4a28f6cf84 100755 --- a/scripts/setup/debians/build_decomputil.sh +++ b/scripts/setup/debians/build_decomputil.sh @@ -2,15 +2,15 @@ # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -20,6 +20,7 @@ PACKAGE_NAME=libdecomputil ORIG_TAR=libdecomputil_0.1.orig.tar.gz DEB_DIR=decomputil +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) if [ -d $PACKAGE_NAME ]; then rm -rf $PACKAGE_NAME @@ -29,5 +30,6 @@ cd $PACKAGE_NAME git checkout --quiet 5d652b8d144c8075b272094fb95b90ab9ff0e48e 2>&1 git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 cp -r ../$DEB_DIR debian +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" debuild -us -uc || exit 1 cd .. diff --git a/scripts/setup/debians/build_dlib.sh b/scripts/setup/debians/build_dlib.sh index e3f1a50eb5..0226461726 100755 --- a/scripts/setup/debians/build_dlib.sh +++ b/scripts/setup/debians/build_dlib.sh @@ -3,13 +3,15 @@ PACKAGE_NAME=libdbowdlib ORIG_TAR=libdbowdlib_0.1.orig.tar.gz DEB_DIR=dlib +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) if [ -d $PACKAGE_NAME ]; then rm -rf $PACKAGE_NAME fi -git clone --quiet https://github.com/dorian3d/DLib.git $PACKAGE_NAME --branch v1.1-free 2>&1 || exit 1 +git clone --quiet https://github.com/ana-GT/DLib.git $PACKAGE_NAME --branch v1.1-free-opencv4 2>&1 || exit 1 cd $PACKAGE_NAME git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 cp -r ../$DEB_DIR debian +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" debuild -us -uc || exit 1 cd .. diff --git a/scripts/setup/debians/build_gtsam.sh b/scripts/setup/debians/build_gtsam.sh new file mode 100755 index 0000000000..3343b4cbc1 --- /dev/null +++ b/scripts/setup/debians/build_gtsam.sh @@ -0,0 +1,34 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=libgtsam +ORIG_TAR=libgtsam_4.1.0.orig.tar.gz +DEB_DIR=gtsam +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone --quiet https://github.com/borglab/gtsam.git $PACKAGE_NAME --branch 4.1rc 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/build_install_debians.sh b/scripts/setup/debians/build_install_debians.sh index 67f25b144b..df1d13402d 100755 --- a/scripts/setup/debians/build_install_debians.sh +++ b/scripts/setup/debians/build_install_debians.sh @@ -1,16 +1,16 @@ -#/bin/bash -e +#!/bin/bash -e # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -19,47 +19,30 @@ # # Install the dependencies needed for the debians. Build and install flight # software debians. +set -e -DEBIAN_LOC=$(dirname "$(readlink -f "$0")") +debian_loc=$(dirname "$(readlink -f "$0")") +dist=$(. /etc/os-release && echo $UBUNTU_CODENAME) +build_list=() sudo apt-get install -y devscripts equivs libproj-dev - -# delete old debians (-f avoids 'no such file' warning on first run) -rm -f *_amd64.deb - -cd ${DEBIAN_LOC}/alvar -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_alvar.sh || exit 1 -sudo dpkg -i libalvar*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/dlib -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_dlib.sh || exit 1 -sudo dpkg -i libdbowdlib*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/dbow2 -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_dbow2.sh || exit 1 -sudo dpkg -i libdbow*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/decomputil -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_decomputil.sh || exit 1 -sudo dpkg -i libdecomputil*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/jps3d -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_jps3d.sh || exit 1 -sudo dpkg -i libjps3d*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/openmvg -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_openmvg.sh || exit 1 -sudo dpkg -i libopenmvg*_amd64.deb || exit 1 - +# delete old files (-f avoids 'no such file' warning on first run) +rm -f *.deb *.debian.tar.xz *.orig.tar.gz *.dsc *.build *.buildinfo *.changes *.ddeb + +# Add public debians to build list +build_list+=( ar-track-alvar-msgs ar-track-alvar dlib dbow2 gtsam decomputil jps3d openmvg opencv-xfeatures2d) + +# If restricted rti-dev debian is present, add miro and soracore as well +dpkg-query -W -f='${Status}\n' rti-dev 2>&1 | grep -q "install ok installed" && +echo "Package rti-dev exists. Including miro and soracore to build list..." && +build_list+=( miro soracore ) + +export DEBEMAIL="nospam@nospam.org" +for pkg in ${build_list[@]} +do + cd ${debian_loc}/$pkg && + sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control && + cd ${debian_loc} && + ./build_${pkg}.sh && + sudo dpkg -i *${pkg}*.deb || exit 1 +done diff --git a/scripts/setup/debians/build_install_debians_18_04.sh b/scripts/setup/debians/build_install_debians_18_04.sh deleted file mode 100755 index ed4a7e4c82..0000000000 --- a/scripts/setup/debians/build_install_debians_18_04.sh +++ /dev/null @@ -1,73 +0,0 @@ -#/bin/bash -e -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. -# -# Install the dependencies needed for the debians. Build and install flight -# software debians. - -DEBIAN_LOC=$(dirname "$(readlink -f "$0")") - -sudo apt-get install -y devscripts equivs libproj-dev - -# delete old debians (-f avoids 'no such file' warning on first run) -rm -f *_amd64.deb - -cp ${DEBIAN_LOC}/files_18_04/alvar_rules ${DEBIAN_LOC}/alvar/rules -cp ${DEBIAN_LOC}/files_18_04/alvar_control ${DEBIAN_LOC}/alvar/control -cd ${DEBIAN_LOC}/alvar -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_alvar.sh || exit 1 -sudo dpkg -i libalvar*_amd64.deb || exit 1 - -cp ${DEBIAN_LOC}/files_18_04/dlib_rules ${DEBIAN_LOC}/dlib/rules -cp ${DEBIAN_LOC}/files_18_04/dlib_control ${DEBIAN_LOC}/dlib/control -cd ${DEBIAN_LOC}/dlib -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_dlib.sh || exit 1 -sudo dpkg -i libdbowdlib*_amd64.deb || exit 1 - -cp ${DEBIAN_LOC}/files_18_04/dbow2_rules ${DEBIAN_LOC}/dbow2/rules -cp ${DEBIAN_LOC}/files_18_04/dbow2_control ${DEBIAN_LOC}/dbow2/control -cd ${DEBIAN_LOC}/dbow2 -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_dbow2.sh || exit 1 -sudo dpkg -i libdbow*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/decomputil -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_decomputil.sh || exit 1 -sudo dpkg -i libdecomputil*_amd64.deb || exit 1 - -# install depend libraries -sudo apt-get install -y libvtk6.3 libboost-filesystem1.62.0 libboost-system1.62.0 -cd ${DEBIAN_LOC}/jps3d -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_jps3d.sh || exit 1 -sudo dpkg -i libjps3d*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/openmvg -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_openmvg.sh || exit 1 -sudo dpkg -i libopenmvg*_amd64.deb || exit 1 - diff --git a/scripts/setup/debians/build_jps3d.sh b/scripts/setup/debians/build_jps3d.sh index ef320fae70..540a801250 100755 --- a/scripts/setup/debians/build_jps3d.sh +++ b/scripts/setup/debians/build_jps3d.sh @@ -2,15 +2,15 @@ # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -20,6 +20,7 @@ PACKAGE_NAME=libjps3d ORIG_TAR=libjps3d_0.1.orig.tar.gz DEB_DIR=jps3d +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) if [ -d $PACKAGE_NAME ]; then rm -rf $PACKAGE_NAME @@ -30,5 +31,6 @@ cd $PACKAGE_NAME git checkout --quiet 85e6b22171478684119a119c45dbf6b42653d828 2>&1 git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 cp -r ../$DEB_DIR debian +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" debuild -us -uc || exit 1 cd .. diff --git a/scripts/setup/debians/build_miro.sh b/scripts/setup/debians/build_miro.sh new file mode 100755 index 0000000000..6fc0100ebc --- /dev/null +++ b/scripts/setup/debians/build_miro.sh @@ -0,0 +1,38 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# This package depends on rti which is a proprietary package and therefore +# cannot be shared. This debian build is only useful for NASA maintenence +# of this repo. + +PACKAGE_NAME=libmiro +ORIG_TAR=libmiro_0.3.2.orig.tar.gz +DEB_DIR=miro +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone --quiet https://github.com/hhutz/Miro.git --branch catkin_build $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/build_opencv-xfeatures2d.sh b/scripts/setup/debians/build_opencv-xfeatures2d.sh new file mode 100755 index 0000000000..16949c1158 --- /dev/null +++ b/scripts/setup/debians/build_opencv-xfeatures2d.sh @@ -0,0 +1,37 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=libopencv-xfeatures2d +ORIG_TAR=libopencv-xfeatures2d_4.2.0.orig.tar.gz +DEB_DIR=opencv-xfeatures2d +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone --quiet https://github.com/opencv/opencv.git --branch 4.2.0 $PACKAGE_NAME/opencv 2>&1 || exit 1 +git clone --quiet https://github.com/opencv/opencv_contrib.git --branch 4.2.0 $PACKAGE_NAME/contrib 2>&1 || exit 1 +cd $PACKAGE_NAME/opencv +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../../$DEB_DIR debian +rm -r .github +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" +debuild -us -uc || exit 1 +cd ../.. +mv $PACKAGE_NAME/libopencv*{.deb,.debian.tar.xz,.orig.tar.gz,.dsc} . diff --git a/scripts/setup/debians/build_openmvg.sh b/scripts/setup/debians/build_openmvg.sh index c9fd515d02..fce2f989a5 100755 --- a/scripts/setup/debians/build_openmvg.sh +++ b/scripts/setup/debians/build_openmvg.sh @@ -2,15 +2,15 @@ # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. -# +# # All rights reserved. -# +# # The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -20,6 +20,7 @@ PACKAGE_NAME=libopenmvg ORIG_TAR=libopenmvg_1.1.orig.tar.gz DEB_DIR=openmvg +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) if [ -d $PACKAGE_NAME ]; then rm -rf $PACKAGE_NAME @@ -28,5 +29,6 @@ git clone --quiet https://github.com/openMVG/openMVG.git $PACKAGE_NAME --branch cd $PACKAGE_NAME git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 cp -r ../$DEB_DIR debian +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" debuild -us -uc || exit 1 cd .. diff --git a/scripts/setup/debians/build_soracore.sh b/scripts/setup/debians/build_soracore.sh new file mode 100755 index 0000000000..5f55cab4af --- /dev/null +++ b/scripts/setup/debians/build_soracore.sh @@ -0,0 +1,38 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# This package depends on rti which is a proprietary package and therefore +# cannot be shared. It is not necessary for running the astrobee simulation. +# This debian build is only useful for NASA maintenence of this repo. + +PACKAGE_NAME=libsoracore +ORIG_TAR=libsoracore_2.0.orig.tar.gz +DEB_DIR=soracore +DIST=$(grep -oP "(?<=VERSION_CODENAME=).*" /etc/os-release) + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone https://github.com/marinagmoreira/soraCore.git --branch catkin_build $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +dch -l"+$DIST" -D"$DIST" "Set distribution '$DIST' for local build" +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/dbow2/changelog b/scripts/setup/debians/dbow2/changelog index e5428d0ca4..9d56fbf703 100644 --- a/scripts/setup/debians/dbow2/changelog +++ b/scripts/setup/debians/dbow2/changelog @@ -1,14 +1,26 @@ +libdbow2 (0.1-7) unstable; urgency=medium + + * Upgrade to OpenCV 4 + + -- Ruben Garcia Fri, 03 May 2024 10:08:52 -0700 + +libdbow2 (0.1-6) unstable; urgency=medium + + * Added reportMemoryUsage() methods. + + -- Trey Smith Thu, 16 Mar 2023 13:31:16 -0500 + libdbow2 (0.1-5) unstable; urgency=medium * Ros moved opencv library which broke build, fixed it. - -- Brian Coltin Mon, 30 Apr 2018 13:31:16 -0700 + -- Brian Coltin Mon, 30 Apr 2018 13:31:16 -0700 libdbow2 (0.1-4) unstable; urgency=medium * Bump ros-kinetic-opencv version. - -- Brian Coltin Mon, 11 Dec 2017 17:34:43 -0800 + -- Brian Coltin Mon, 11 Dec 2017 17:34:43 -0800 libdbow2 (0.1-3) unstable; urgency=medium diff --git a/scripts/setup/debians/dbow2/control b/scripts/setup/debians/dbow2/control index a2e0c590f8..da66c214cb 100644 --- a/scripts/setup/debians/dbow2/control +++ b/scripts/setup/debians/dbow2/control @@ -1,7 +1,7 @@ Source: libdbow2 Priority: optional Maintainer: Brian Coltin -Build-Depends: debhelper (>=9), cmake (>=3.5), ros-kinetic-opencv3 (>=3.3.1-5), libboost-dev (>=1.58), libdbowdlib-dev (>=0.1) +Build-Depends: debhelper (>=9), cmake (>=3.5), libopencv-dev, libboost-dev (>=1.58), libdbowdlib-dev (>=0.1) Standards-Version: 3.9.7 Section: libs Homepage: https://github.com/dorian3d/DBoW2 diff --git a/scripts/setup/debians/dbow2/patches/fix_opencv.patch b/scripts/setup/debians/dbow2/patches/fix_opencv.patch index 8f6588de35..6689464305 100644 --- a/scripts/setup/debians/dbow2/patches/fix_opencv.patch +++ b/scripts/setup/debians/dbow2/patches/fix_opencv.patch @@ -9,17 +9,16 @@ Index: libdbow2/CMakeLists.txt include(ExternalProject) option(BUILD_DBoW2 "Build DBoW2" ON) -@@ -33,6 +33,9 @@ set(SRCS +@@ -33,7 +33,7 @@ set(SRCS set(DEPENDENCY_DIR ${CMAKE_CURRENT_BINARY_DIR}/dependencies) set(DEPENDENCY_INSTALL_DIR ${DEPENDENCY_DIR}/install) -+list(APPEND CMAKE_INSTALL_RPATH "/opt/ros/kinetic/lib" "/opt/ros/kinetic/lib/x86_64-linux-gnu" "/opt/ros/kinetic/lib/arm-linux-gnueabihf") # find opencv3 -+find_path(OpenCV_DIR OpenCVConfig.cmake PATHS /opt/ros/kinetic/share/ -+ PATH_SUFFIXES OpenCV-3.1.0 OpenCV-3.1.0-dev OpenCV-3.2.0 OpenCV-3.2.0-dev OpenCV-3.3.1 OpenCV-3.3.1-dev) - find_package(OpenCV REQUIRED) +-find_package(OpenCV REQUIRED) ++find_package(OpenCV 4.0 REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) -@@ -49,13 +52,15 @@ else(${DLib_FOUND}) + find_package(Boost REQUIRED) +@@ -49,13 +49,15 @@ else(${DLib_FOUND}) message("DLib library not found in the system, it will be downloaded on build") option(DOWNLOAD_DLib_dependency "Download DLib dependency" ON) if(${DOWNLOAD_DLib_dependency}) @@ -36,7 +35,7 @@ Index: libdbow2/CMakeLists.txt else() message(SEND_ERROR "Please, activate DOWNLOAD_DLib_dependency option or download manually") endif(${DOWNLOAD_DLib_dependency}) -@@ -74,15 +79,39 @@ if(BUILD_Demo) +@@ -74,15 +76,39 @@ if(BUILD_Demo) file(COPY demo/images DESTINATION ${CMAKE_BINARY_DIR}/) endif(BUILD_Demo) diff --git a/scripts/setup/debians/dbow2/patches/report_memory_usage.patch b/scripts/setup/debians/dbow2/patches/report_memory_usage.patch new file mode 100644 index 0000000000..fb84bd5fae --- /dev/null +++ b/scripts/setup/debians/dbow2/patches/report_memory_usage.patch @@ -0,0 +1,170 @@ +diff --git a/include/DBoW2/TemplatedDatabase.h b/include/DBoW2/TemplatedDatabase.h +index 96cbe8e..10fe5b4 100644 +--- a/include/DBoW2/TemplatedDatabase.h ++++ b/include/DBoW2/TemplatedDatabase.h +@@ -16,6 +16,7 @@ + #include + #include + #include ++#include + + #include "TemplatedVocabulary.h" + #include "QueryResults.h" +@@ -223,6 +224,13 @@ public: + virtual void load(const cv::FileStorage &fs, + const std::string &name = "database"); + ++ /** ++ * Estimates memory usage of the database. ++ * @param partsOutput Statistics concerning parts of the data structure will be appended to this vector. ++ * @return Estimated total usage. ++ */ ++ size_t reportMemoryUsage(std::vector >& partsOutput); ++ + protected: + + /// Query with L1 scoring +@@ -1345,6 +1353,64 @@ std::ostream& operator<<(std::ostream &os, + + // -------------------------------------------------------------------------- + ++template ++size_t TemplatedDatabase::reportMemoryUsage(std::vector >& partsOutput) ++{ ++ typedef std::pair PartsEntry; ++ ++#define FF_REPORT(field) \ ++ partsOutput.push_back(PartsEntry(#field, field)); ++#define FF_COMMENT(label) \ ++ partsOutput.push_back(PartsEntry("*" label, 0)); ++ ++ FF_COMMENT(" DBow database:"); ++ ++ size_t num_rows = m_ifile.size(); ++ FF_REPORT(num_rows); ++ ++ size_t num_ifpairs = 0; ++ for (typename InvertedFile::const_iterator it = m_ifile.begin(); it != m_ifile.end(); it++) { ++ num_ifpairs += it->size(); ++ } ++ FF_REPORT(num_ifpairs); ++ ++ FF_COMMENT(""); ++ ++ // typedef std::list IFRow; ++ // typedef std::vector InvertedFile; ++ const size_t list_bytes_per_node = 2 * sizeof(void *); ++ size_t m_ifile_bytes = m_ifile.size() * sizeof(std::list); ++ for (typename InvertedFile::const_iterator it = m_ifile.begin(); it != m_ifile.end(); it++) { ++ m_ifile_bytes += it->size() * (list_bytes_per_node + sizeof(IFPair)); ++ } ++ FF_REPORT(m_ifile_bytes); ++ ++ ++ // class FeatureVector: public std::map > ++ // typedef std::vector DirectFile; ++ // some of this is platform- and data-dependent, just rough approximation ++ const size_t map_node_bytes = sizeof(int) + 3 * sizeof(void *); // red/black tree node ~overhead ++ size_t m_dfile_bytes = m_dfile.size() * sizeof(FeatureVector); ++ for (typename DirectFile::const_iterator it = m_dfile.begin(); it != m_dfile.end(); it++) { ++ m_dfile_bytes += it->size() ++ * (map_node_bytes + sizeof(NodeId) + sizeof(std::vector)); ++ for (typename FeatureVector::const_iterator jt = it->begin(); jt != it->end(); jt++) { ++ m_dfile_bytes += jt->second.size() * sizeof(unsigned int); ++ } ++ } ++ FF_REPORT(m_dfile_bytes); ++ ++ FF_COMMENT(""); ++ size_t m_voc_bytes = m_voc->reportMemoryUsage(partsOutput); ++ ++#undef FF_REPORT ++#undef FF_COMMENT ++ ++ return m_ifile_bytes + m_dfile_bytes + m_voc_bytes; ++} ++ ++// -------------------------------------------------------------------------- ++ + } // namespace DBoW2 + + #endif +diff --git a/include/DBoW2/TemplatedVocabulary.h b/include/DBoW2/TemplatedVocabulary.h +index 53a0e30..55d2dec 100644 +--- a/include/DBoW2/TemplatedVocabulary.h ++++ b/include/DBoW2/TemplatedVocabulary.h +@@ -17,6 +17,7 @@ + #include + #include + #include ++#include + #include + + #include "FeatureVector.h" +@@ -30,6 +31,7 @@ namespace DBoW2 { + /// @param TDescriptor class of descriptor + /// @param F class of descriptor functions + template ++ + /// Generic Vocabulary + class TemplatedVocabulary + { +@@ -266,6 +268,13 @@ public: + */ + virtual int stopWords(double minWeight); + ++ /** ++ * Estimates memory usage of the vocabulary. ++ * @param partsOutput Statistics concerning parts of the data structure will be appended to this vector. ++ * @return Estimated total usage. ++ */ ++ virtual size_t reportMemoryUsage(std::vector >& partsOutput); ++ + protected: + + /// Pointer to descriptor +@@ -1524,6 +1533,44 @@ std::ostream& operator<<(std::ostream &os, + return os; + } + ++// -------------------------------------------------------------------------- ++ ++template ++size_t TemplatedVocabulary::reportMemoryUsage(std::vector >& partsOutput) ++{ ++ typedef std::pair PartsEntry; ++ ++#define FF_REPORT(field) \ ++ partsOutput.push_back(PartsEntry(#field, field)); ++#define FF_COMMENT(label) \ ++ partsOutput.push_back(PartsEntry("*" label, 0)); ++ ++ FF_COMMENT(" DBow vocabulary:"); ++ ++ size_t num_nodes = m_nodes.size(); ++ FF_REPORT(num_nodes); ++ ++ size_t num_words = m_words.size(); ++ FF_REPORT(num_words); ++ ++ FF_COMMENT(""); ++ ++ size_t m_nodes_bytes = m_nodes.size() * sizeof(Node); ++ for (typename std::vector::const_iterator it = m_nodes.begin(); it != m_nodes.end(); it++) { ++ m_nodes_bytes += it->children.size() * sizeof(NodeId); ++ } ++ FF_REPORT(m_nodes_bytes); ++ ++ size_t m_words_bytes = m_words.size() * sizeof(Node*); ++ FF_REPORT(m_words_bytes); ++ ++#undef FF_REPORT ++#undef FF_COMMENT ++ ++ return m_nodes_bytes + m_words_bytes; ++} ++ ++ + } // namespace DBoW2 + + #endif diff --git a/scripts/setup/debians/dbow2/patches/series b/scripts/setup/debians/dbow2/patches/series index 63022dba13..69637b5c3c 100644 --- a/scripts/setup/debians/dbow2/patches/series +++ b/scripts/setup/debians/dbow2/patches/series @@ -1,3 +1,4 @@ fix_opencv.patch bytes_descriptor.patch fix_path.patch +report_memory_usage.patch diff --git a/scripts/setup/debians/dlib/changelog b/scripts/setup/debians/dlib/changelog index 1f15abd86e..972bd5b4b4 100644 --- a/scripts/setup/debians/dlib/changelog +++ b/scripts/setup/debians/dlib/changelog @@ -1,14 +1,20 @@ +libdbowdlib (0.1-4) unstable; urgency=medium + + * Upgrade to OpenCV 4 + + -- Ruben Garcia Fri, 03 May 2024 10:12:38 -0700 + libdbowdlib (0.1-3) unstable; urgency=medium * Ros moved opencv library which broke build, fixed it. - -- Brian Coltin Mon, 30 Apr 2018 13:31:51 -0700 + -- Brian Coltin Mon, 30 Apr 2018 13:31:51 -0700 libdbowdlib (0.1-2) unstable; urgency=medium * Bump version number due to ros opencv upgrade. - -- Brian Coltin Mon, 11 Dec 2017 17:31:09 -0800 + -- Brian Coltin Mon, 11 Dec 2017 17:31:09 -0800 libdbowdlib (0.1-1) unstable; urgency=medium diff --git a/scripts/setup/debians/dlib/control b/scripts/setup/debians/dlib/control index e7476eab26..258a4c3cac 100644 --- a/scripts/setup/debians/dlib/control +++ b/scripts/setup/debians/dlib/control @@ -1,7 +1,7 @@ Source: libdbowdlib Priority: optional Maintainer: Brian Coltin -Build-Depends: debhelper (>=9), cmake (>=3.5), ros-kinetic-opencv3 (>=3.3.1-5) +Build-Depends: debhelper (>=9), cmake (>=3.5), libopencv-dev Standards-Version: 3.9.7 Section: libs Homepage: https://github.com/dorian3d/DLib diff --git a/scripts/setup/debians/dlib/patches/astrobee.patch b/scripts/setup/debians/dlib/patches/astrobee.patch index 31a8a19469..5f2343f825 100644 --- a/scripts/setup/debians/dlib/patches/astrobee.patch +++ b/scripts/setup/debians/dlib/patches/astrobee.patch @@ -3,12 +3,12 @@ Index: libdbowdlib/CMakeLists.txt --- libdbowdlib.orig/CMakeLists.txt +++ libdbowdlib/CMakeLists.txt @@ -5,6 +5,10 @@ option(BUILD_DUtils "Build DUtils (bas - option(BUILD_DUtilsCV "Build DUtilsCV (OpenCV functions, requires DUtils)." ON) + option(BUILD_DUtilsCV "Build DUtilsCV (OpenCV functions, requires DUtils)." OFF) option(BUILD_DVision "Build DVision (computer vision functions, requires DUtilsCV)." ON) -+list(APPEND CMAKE_INSTALL_RPATH "/opt/ros/kinetic/lib" "/opt/ros/kinetic/lib/x86_64-linux-gnu" "/opt/ros/kinetic/lib/arm-linux-gnueabihf") # find opencv3 -+find_path(OpenCV_DIR OpenCVConfig.cmake PATHS /opt/ros/kinetic/share/ -+ PATH_SUFFIXES OpenCV-3.1.0 OpenCV-3.1.0-dev OpenCV-3.2.0 OpenCV-3.2.0-dev OpenCV-3.3.1 OpenCV-3.3.1-dev) ++list(APPEND CMAKE_INSTALL_RPATH "/opt/ros/kinetic/lib" "/opt/ros/kinetic/lib/x86_64-linux-gnu" "/opt/ros/kinetic/lib/arm-linux-gnueabihf") ++find_path(OpenCV_DIR OpenCVConfig.cmake PATHS /opt/ros/kinetic/share/ /usr/share/ ++ PATH_SUFFIXES OpenCV-4.2.0 OpenCV-4.2.0-dev OpenCV) + if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE) diff --git a/scripts/setup/debians/files_18_04/alvar_control b/scripts/setup/debians/files_18_04/alvar_control deleted file mode 100644 index 48ca143aa0..0000000000 --- a/scripts/setup/debians/files_18_04/alvar_control +++ /dev/null @@ -1,22 +0,0 @@ -Source: libalvar -Priority: optional -Maintainer: Brian Coltin -Build-Depends: debhelper (>=9), cmake (>=3.5), libtinyxml-dev (>=2.6) -Standards-Version: 3.9.7 -Section: libs -Homepage: http://virtual.vtt.fi/virtual/proj2/multimedia/alvar/index.html -#Vcs-Git: git://anonscm.debian.org/collab-maint/libalvar.git -#Vcs-Browser: https://anonscm.debian.org/cgit/collab-maint/libalvar.git - -Package: libalvar-dev -Section: libdevel -Architecture: any -Depends: libalvar2 (= ${binary:Version}), ${misc:Depends} -Description: A library for detecting AR tags. Development packcage. - Alvar detects AR tags. The development packcage. - -Package: libalvar2 -Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends} -Description: A library for detecting AR tags. - Alvar detects AR tags. diff --git a/scripts/setup/debians/files_18_04/alvar_rules b/scripts/setup/debians/files_18_04/alvar_rules deleted file mode 100755 index 0ac0aed132..0000000000 --- a/scripts/setup/debians/files_18_04/alvar_rules +++ /dev/null @@ -1,25 +0,0 @@ -#!/usr/bin/make -f -# See debhelper(7) (uncomment to enable) -# output every command that modifies files on the build system. -#export DH_VERBOSE = 1 - - -# see FEATURE AREAS in dpkg-buildflags(1) -#export DEB_BUILD_MAINT_OPTIONS = hardening=+all - -# see ENVIRONMENT in dpkg-buildflags(1) -# package maintainers to append CFLAGS -#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic -# package maintainers to append LDFLAGS -#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed - - -%: - dh $@ --buildsystem=cmake --dpkg-shlibdeps-params=--ignore-missing-info - - -# dh_make generated override targets -# This is example for Cmake (See https://bugs.debian.org/641051 ) -#override_dh_auto_configure: -# dh_auto_configure -- # -DCMAKE_LIBRARY_PATH=$(DEB_HOST_MULTIARCH) - diff --git a/scripts/setup/debians/files_18_04/dbow2_control b/scripts/setup/debians/files_18_04/dbow2_control deleted file mode 100644 index 0d64873c42..0000000000 --- a/scripts/setup/debians/files_18_04/dbow2_control +++ /dev/null @@ -1,20 +0,0 @@ -Source: libdbow2 -Priority: optional -Maintainer: Brian Coltin -Build-Depends: debhelper (>=9), cmake (>=3.5), libboost-dev (>=1.58), libdbowdlib-dev (>=0.1) -Standards-Version: 3.9.7 -Section: libs -Homepage: https://github.com/dorian3d/DBoW2 - -Package: libdbow2-dev -Section: libdevel -Architecture: any -Depends: libdbow21 (= ${binary:Version}), libdbowdlib-dev ${misc:Depends} -Description: Database of visual features. - Library with a database of visual features for finding similar images. - -Package: libdbow21 -Architecture: any -Depends: libdbowdlib1 (>=0.1), ${shlibs:Depends}, ${misc:Depends} -Description: Database of visual features. - Library with a database of visual features for finding similar images. diff --git a/scripts/setup/debians/files_18_04/dbow2_rules b/scripts/setup/debians/files_18_04/dbow2_rules deleted file mode 100755 index 0ac0aed132..0000000000 --- a/scripts/setup/debians/files_18_04/dbow2_rules +++ /dev/null @@ -1,25 +0,0 @@ -#!/usr/bin/make -f -# See debhelper(7) (uncomment to enable) -# output every command that modifies files on the build system. -#export DH_VERBOSE = 1 - - -# see FEATURE AREAS in dpkg-buildflags(1) -#export DEB_BUILD_MAINT_OPTIONS = hardening=+all - -# see ENVIRONMENT in dpkg-buildflags(1) -# package maintainers to append CFLAGS -#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic -# package maintainers to append LDFLAGS -#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed - - -%: - dh $@ --buildsystem=cmake --dpkg-shlibdeps-params=--ignore-missing-info - - -# dh_make generated override targets -# This is example for Cmake (See https://bugs.debian.org/641051 ) -#override_dh_auto_configure: -# dh_auto_configure -- # -DCMAKE_LIBRARY_PATH=$(DEB_HOST_MULTIARCH) - diff --git a/scripts/setup/debians/files_18_04/dlib_control b/scripts/setup/debians/files_18_04/dlib_control deleted file mode 100644 index 955756f6dc..0000000000 --- a/scripts/setup/debians/files_18_04/dlib_control +++ /dev/null @@ -1,20 +0,0 @@ -Source: libdbowdlib -Priority: optional -Maintainer: Brian Coltin -Build-Depends: debhelper (>=9), cmake (>=3.5) -Standards-Version: 3.9.7 -Section: libs -Homepage: https://github.com/dorian3d/DLib - -Package: libdbowdlib-dev -Section: libdevel -Architecture: any -Depends: libdbowdlib1 (= ${binary:Version}), ${misc:Depends} -Description: Database of visual features helper. - Helper ibrary for a database of visual features for finding similar images. - -Package: libdbowdlib1 -Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends} -Description: Database of visual features helper. - Helper library for a database of visual features for finding similar images. diff --git a/scripts/setup/debians/files_18_04/dlib_rules b/scripts/setup/debians/files_18_04/dlib_rules deleted file mode 100755 index 0ac0aed132..0000000000 --- a/scripts/setup/debians/files_18_04/dlib_rules +++ /dev/null @@ -1,25 +0,0 @@ -#!/usr/bin/make -f -# See debhelper(7) (uncomment to enable) -# output every command that modifies files on the build system. -#export DH_VERBOSE = 1 - - -# see FEATURE AREAS in dpkg-buildflags(1) -#export DEB_BUILD_MAINT_OPTIONS = hardening=+all - -# see ENVIRONMENT in dpkg-buildflags(1) -# package maintainers to append CFLAGS -#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic -# package maintainers to append LDFLAGS -#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed - - -%: - dh $@ --buildsystem=cmake --dpkg-shlibdeps-params=--ignore-missing-info - - -# dh_make generated override targets -# This is example for Cmake (See https://bugs.debian.org/641051 ) -#override_dh_auto_configure: -# dh_auto_configure -- # -DCMAKE_LIBRARY_PATH=$(DEB_HOST_MULTIARCH) - diff --git a/scripts/setup/debians/gtsam/changelog b/scripts/setup/debians/gtsam/changelog new file mode 100644 index 0000000000..ff2770caae --- /dev/null +++ b/scripts/setup/debians/gtsam/changelog @@ -0,0 +1,5 @@ +libgtsam (4.1.0-1) unstable; urgency=medium + + * Initial release. + + -- Ryan Soussan Mon, 31 Aug 2020 11:21:32 -0800 diff --git a/scripts/setup/debians/gtsam/compat b/scripts/setup/debians/gtsam/compat new file mode 100644 index 0000000000..ec635144f6 --- /dev/null +++ b/scripts/setup/debians/gtsam/compat @@ -0,0 +1 @@ +9 diff --git a/scripts/setup/debians/gtsam/control b/scripts/setup/debians/gtsam/control new file mode 100644 index 0000000000..0ac6b96352 --- /dev/null +++ b/scripts/setup/debians/gtsam/control @@ -0,0 +1,18 @@ +Source: libgtsam +Priority: optional +Maintainer: Ryan Soussan +Build-Depends: debhelper (>=9), cmake (>=3.5), libeigen3-dev (>=3.2), libboost-all-dev (>=1.58) +Standards-Version: 3.9.2 +Section: libs +Homepage: https://github.com/borglab/gtsam + +Package: libgtsam-dev +Section: libdevel +Architecture: any +Depends: libgtsam (= ${binary:Version}), ${misc:Depends} +Description: Smoothing and mapping library for robotics and vision. The development package. + +Package: libgtsam +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends} +Description: Smoothing and mapping library for robotics and vision. diff --git a/scripts/setup/debians/gtsam/copyright b/scripts/setup/debians/gtsam/copyright new file mode 100644 index 0000000000..13b222d3e6 --- /dev/null +++ b/scripts/setup/debians/gtsam/copyright @@ -0,0 +1,31 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ +Upstream-Name: libgtsam +Source: https://github.com/borglab/gtsam + +Files: * +Copyright: 2010 Georgia Tech Research Corporation +License: BSD +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright 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. + +Files: debian/* +Copyright: 2017 NASA +License: Apache-2 + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + . + http://www.apache.org/licenses/LICENSE-2.0 + . + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/scripts/setup/debians/alvar/libalvar-dev.dirs b/scripts/setup/debians/gtsam/libgtsam-dev.dirs similarity index 100% rename from scripts/setup/debians/alvar/libalvar-dev.dirs rename to scripts/setup/debians/gtsam/libgtsam-dev.dirs diff --git a/scripts/setup/debians/gtsam/libgtsam-dev.install b/scripts/setup/debians/gtsam/libgtsam-dev.install new file mode 100644 index 0000000000..35970b90ac --- /dev/null +++ b/scripts/setup/debians/gtsam/libgtsam-dev.install @@ -0,0 +1,2 @@ +usr/include/* +usr/lib/cmake/GTSAM/* usr/share/gtsam/cmake diff --git a/scripts/setup/debians/alvar/libalvar2.dirs b/scripts/setup/debians/gtsam/libgtsam.dirs similarity index 100% rename from scripts/setup/debians/alvar/libalvar2.dirs rename to scripts/setup/debians/gtsam/libgtsam.dirs diff --git a/scripts/setup/debians/gtsam/libgtsam.install b/scripts/setup/debians/gtsam/libgtsam.install new file mode 100644 index 0000000000..703ad8b313 --- /dev/null +++ b/scripts/setup/debians/gtsam/libgtsam.install @@ -0,0 +1 @@ +usr/lib/*/lib*.so* diff --git a/scripts/setup/debians/gtsam/rules b/scripts/setup/debians/gtsam/rules new file mode 100755 index 0000000000..bf97609122 --- /dev/null +++ b/scripts/setup/debians/gtsam/rules @@ -0,0 +1,26 @@ +#!/usr/bin/make -f +# See debhelper(7) (uncomment to enable) +# output every command that modifies files on the build system. +#export DH_VERBOSE = 1 + + +# see FEATURE AREAS in dpkg-buildflags(1) +#export DEB_BUILD_MAINT_OPTIONS = hardening=+all + +# see ENVIRONMENT in dpkg-buildflags(1) +# package maintainers to append CFLAGS +#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic +# package maintainers to append LDFLAGS +#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed + +%: + dh $@ --buildsystem=cmake + + +# dh_make generated override targets +# This is example for Cmake (See https://bugs.debian.org/641051 ) +#override_dh_auto_configure: +# dh_auto_configure -- # -DCMAKE_LIBRARY_PATH=$(DEB_HOST_MULTIARCH) +override_dh_auto_configure: + dh_auto_configure -- -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_WITH_TBB=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DCMAKE_BUILD_TYPE=Release + diff --git a/scripts/setup/debians/gtsam/source/format b/scripts/setup/debians/gtsam/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/gtsam/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/jps3d/control b/scripts/setup/debians/jps3d/control index 27e95e140c..13927bc634 100644 --- a/scripts/setup/debians/jps3d/control +++ b/scripts/setup/debians/jps3d/control @@ -1,7 +1,7 @@ Source: libjps3d Priority: optional Maintainer: Brian Coltin -Build-Depends: debhelper (>=9), cmake (>=3.5), libeigen3-dev (>=3.2), libvtk6-dev (>=6.2), libboost-filesystem-dev (>=1.58), libboost-system-dev (>=1.58), libyaml-cpp-dev (>=0.5.2) +Build-Depends: debhelper (>=9), cmake (>=3.5), libeigen3-dev (>=3.2), libvtk6-dev (>=6.2) | libvtk7-dev (>=7.1), libboost-filesystem-dev (>=1.58), libboost-system-dev (>=1.58), libyaml-cpp-dev (>=0.5.2) Standards-Version: 3.9.6 Section: libs Homepage: https://github.com/mwatterson/jps3d @@ -11,12 +11,12 @@ Homepage: https://github.com/mwatterson/jps3d Package: libjps3d-dev Section: libdevel Architecture: any -Depends: libjps3d0 (= ${binary:Version}), libvtk6-dev (>=6.2), libboost-filesystem-dev (>=1.58), libboost-system-dev (>=1.58), libyaml-cpp-dev (>=0.5.2), ${misc:Depends} +Depends: libjps3d0 (= ${binary:Version}), libvtk6-dev (>=6.2) | libvtk7-dev (>=7.1), libboost-filesystem-dev (>=1.58), libboost-system-dev (>=1.58), libyaml-cpp-dev (>=0.5.2), ${misc:Depends} Description: Jps3d. Jps3d. Package: libjps3d0 Architecture: any -Depends: libvtk6.2 | libvtk6.3, libboost-filesystem1.58.0 | libboost-filesystem1.62.0, libboost-system1.58.0 | libboost-system1.62.0, libyaml-cpp0.5v5 (>=0.5), ${shlibs:Depends}, ${misc:Depends} +Depends: libvtk6.2 | libvtk6.3 | libvtk7.1p, libboost-filesystem1.58.0 | libboost-filesystem1.62.0 | libboost-filesystem1.71.0, libboost-system1.58.0 | libboost-system1.62.0 | libboost-system1.71.0, libyaml-cpp0.5v5 (>=0.5) | libyaml-cpp0.6, ${shlibs:Depends}, ${misc:Depends} Description: Jps3d. Jps3d. diff --git a/scripts/setup/debians/miro/README.Debian b/scripts/setup/debians/miro/README.Debian new file mode 100644 index 0000000000..6bad7a7f1f --- /dev/null +++ b/scripts/setup/debians/miro/README.Debian @@ -0,0 +1,6 @@ +libmiro for Debian +----------------- + +Created initial version. + + -- Brian Coltin Mon, 08 May 2017 12:55:37 -0700 diff --git a/scripts/setup/debians/miro/changelog b/scripts/setup/debians/miro/changelog new file mode 100644 index 0000000000..176e9c103c --- /dev/null +++ b/scripts/setup/debians/miro/changelog @@ -0,0 +1,11 @@ +libmiro (0.3.2-1) unstable; urgency=medium + + * Fix compile error in ubuntu 18.04 + + -- Brian Coltin Mon, 04 May 2021 11:45:00 -0700 + +libmiro (0.3.1-1) unstable; urgency=medium + + * Initial release (Closes: #nnnn) + + -- Brian Coltin Mon, 08 May 2017 12:55:37 -0700 diff --git a/scripts/setup/debians/miro/compat b/scripts/setup/debians/miro/compat new file mode 100644 index 0000000000..ec635144f6 --- /dev/null +++ b/scripts/setup/debians/miro/compat @@ -0,0 +1 @@ +9 diff --git a/scripts/setup/debians/miro/control b/scripts/setup/debians/miro/control new file mode 100644 index 0000000000..7d16394134 --- /dev/null +++ b/scripts/setup/debians/miro/control @@ -0,0 +1,20 @@ +Source: libmiro +Priority: optional +Maintainer: Brian Coltin +Build-Depends: debhelper (>=9), cmake (>=3.5), rti-dev (>=5.1), qt5-default | libqt4-qt3support (>=4), libace-dev (>=6.3) +Standards-Version: 3.9.7 +Section: libs +Homepage: https://github.com/hhutz/Miro + +Package: libmiro-dev +Section: libdevel +Architecture: any +Depends: libmiro0 (= ${binary:Version}), ${misc:Depends} +Description: Middleware for Robots + Middleware for Robots. + +Package: libmiro0 +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends} +Description: Middleware for Robots. + Middleware for Robots. diff --git a/scripts/setup/debians/miro/copyright b/scripts/setup/debians/miro/copyright new file mode 100644 index 0000000000..03297673a9 --- /dev/null +++ b/scripts/setup/debians/miro/copyright @@ -0,0 +1,40 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ +Upstream-Name: miro +Source: https://github.com/hhutz/Miro + +Files: * +Copyright: 2017 MIRO Authors +License: LGPL + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + . + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + . + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + +Files: debian/* +Copyright: 2017 Brian Coltin +License: GPL-2+ + This package is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + . + This package is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + . + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + On Debian systems, the complete text of the GNU General + Public License version 2 can be found in "/usr/share/common-licenses/GPL-2". + diff --git a/scripts/setup/debians/miro/libmiro-dev.debhelper.log b/scripts/setup/debians/miro/libmiro-dev.debhelper.log new file mode 100644 index 0000000000..c9ed471d30 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro-dev.debhelper.log @@ -0,0 +1,2 @@ +dh_update_autotools_config +dh_auto_configure diff --git a/scripts/setup/debians/miro/libmiro-dev.dirs b/scripts/setup/debians/miro/libmiro-dev.dirs new file mode 100644 index 0000000000..d8226e61e7 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro-dev.dirs @@ -0,0 +1,3 @@ +usr/lib +usr/lib/cmake +usr/include diff --git a/scripts/setup/debians/miro/libmiro-dev.install b/scripts/setup/debians/miro/libmiro-dev.install new file mode 100644 index 0000000000..f8e7cff6b1 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro-dev.install @@ -0,0 +1,3 @@ +usr/include/* +usr/lib/lib*.so +usr/lib/cmake/* diff --git a/scripts/setup/debians/miro/libmiro0.debhelper.log b/scripts/setup/debians/miro/libmiro0.debhelper.log new file mode 100644 index 0000000000..c9ed471d30 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro0.debhelper.log @@ -0,0 +1,2 @@ +dh_update_autotools_config +dh_auto_configure diff --git a/scripts/setup/debians/miro/libmiro0.dirs b/scripts/setup/debians/miro/libmiro0.dirs new file mode 100644 index 0000000000..d92f1599ee --- /dev/null +++ b/scripts/setup/debians/miro/libmiro0.dirs @@ -0,0 +1,3 @@ +usr/bin +usr/etc +usr/lib diff --git a/scripts/setup/debians/miro/libmiro0.install b/scripts/setup/debians/miro/libmiro0.install new file mode 100644 index 0000000000..689294bc36 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro0.install @@ -0,0 +1,3 @@ +usr/bin/* +usr/etc/* +usr/lib/lib*.so.* diff --git a/scripts/setup/debians/miro/rules b/scripts/setup/debians/miro/rules new file mode 100755 index 0000000000..985ad16eba --- /dev/null +++ b/scripts/setup/debians/miro/rules @@ -0,0 +1,25 @@ +#!/usr/bin/make -f +# See debhelper(7) (uncomment to enable) +# output every command that modifies files on the build system. +#export DH_VERBOSE = 1 + + +# see FEATURE AREAS in dpkg-buildflags(1) +#export DEB_BUILD_MAINT_OPTIONS = hardening=+all + +# see ENVIRONMENT in dpkg-buildflags(1) +# package maintainers to append CFLAGS +#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic +# package maintainers to append LDFLAGS +export DEB_LDFLAGS_MAINT_APPEND = -Wl,--no-as-needed -Wl,-allow-multiple-definition + + +%: + dh $@ --buildsystem=cmake + + +# dh_make generated override targets +# This is example for Cmake (See https://bugs.debian.org/641051 ) +override_dh_auto_configure: + dh_auto_configure -- -Dqt_gui_cpp_USE_QT_MAJOR_VERSION=5 + diff --git a/scripts/setup/debians/miro/source/format b/scripts/setup/debians/miro/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/miro/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/mirror/publish_snapshot.sh b/scripts/setup/debians/mirror/publish_snapshot.sh index 2b494e9e54..74a7dfc635 100755 --- a/scripts/setup/debians/mirror/publish_snapshot.sh +++ b/scripts/setup/debians/mirror/publish_snapshot.sh @@ -24,12 +24,13 @@ $APTLY snapshot merge -latest $1 astrobee_$1 main_$1 security_$1 updates_$1 ros_ # need to delete old version first $APTLY publish drop xenial -$APTLY publish snapshot -distribution="xenial" $1 +$APTLY publish snapshot -gpg-key=33C0A17A -distribution="xenial" $1 || exit 0 PUBLISH_DIR=`$APTLY config show | grep "rootDir" | sed -r 's|^[^:]+: "([^"]+)",$|\1|'`/public chmod g+w -R --silent $PUBLISH_DIR +kinit || exit 1 rsync -ah --no-t --delete $PUBLISH_DIR/dists/ astrobee.ndc.nasa.gov:/home/irg/software/dists rsync -ah --no-t --delete $PUBLISH_DIR/pool/ astrobee.ndc.nasa.gov:/home/irg/software/pool diff --git a/scripts/setup/debians/opencv-xfeatures2d/README.Debian b/scripts/setup/debians/opencv-xfeatures2d/README.Debian new file mode 100644 index 0000000000..dc7361713d --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/README.Debian @@ -0,0 +1,41 @@ +Notes for the Debian OpenCV packages +-------------------------------------- + +The opencv package of Debian is divided every function. +This is because a user does not install an unnecessary +unction. For example, the libopencv_video library is not +necessary when Video function is not necessary. + + - libopencv-core* + - libopencv-imgproc* + - libopencv-flann* + - libopencv-features2d* + - libopencv-calib3d* + - libopencv-objdetect* + - libopencv-legacy* + - libopencv-video* + - libopencv-ml* + - libopencv-gpu* + - libopencv-superres* + - libopencv-imgproc* + - libopencv-highgui* + - libopencv-contrib* + - python-opencv + - and transition packages. + +Please install the package including a necessary library +when you want to make a package using opencv. + +And, opencv provides pkg-config file and cmake config file. +It is necessary you build it, and to Build-Depneds on +libopencv-dev package if the package which you want to use +pkg-config or cmake config file. + +When you build it and do not Build-Depends, please be careful +because it becomes the build error. + +We ask an up stream to divide opencv.pc into every function.[0] + + [0]: https://code.ros.org/trac/opencv/ticket/332 + +-- Nobuhiro Iwamatsu Thu, 02 Feb 2012 04:18:49 +0900 diff --git a/scripts/setup/debians/opencv-xfeatures2d/README.source b/scripts/setup/debians/opencv-xfeatures2d/README.source new file mode 100644 index 0000000000..b4b2036b34 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/README.source @@ -0,0 +1,23 @@ +opencv source package infomation + +- Remove source code + OpenCV's debian source package deletes some source codes. + + The deleted source code is the following directories and files. + * modules/nonfree + * 3rdparty/* + * modules/highgui/src/files_Qt/Milky + * lena.jpg and lena.png + + The module/nonfree contains algorithms that may be patented in some countries or have + some other limitations on the use. + This description is in modules/nonfree/doc/nonfree.rst of original. + + 3rdparty directory has some source code and binaries of 3rdparty project + (libjpeg, libpng, etc...). Since they use the binary of Debian, I have been removed. + + The Milky icons is not DFSG-free. + + lena.jpg and lena.png is not DFSG-free. + + -- Nobuhiro Iwamatsu Tue, 08 Dec 2015 23:08:51 +0900 diff --git a/scripts/setup/debians/opencv-xfeatures2d/changelog b/scripts/setup/debians/opencv-xfeatures2d/changelog new file mode 100644 index 0000000000..1b37a249e8 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/changelog @@ -0,0 +1,6 @@ +libopencv-xfeatures2d (4.2.0-1) unstable; urgency=low + + * Initial release. + + -- Marina Moreira Thu, 20 May 2021 16:42:18 -0700 + diff --git a/scripts/setup/debians/opencv-xfeatures2d/compat b/scripts/setup/debians/opencv-xfeatures2d/compat new file mode 100644 index 0000000000..ec635144f6 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/compat @@ -0,0 +1 @@ +9 diff --git a/scripts/setup/debians/opencv-xfeatures2d/control b/scripts/setup/debians/opencv-xfeatures2d/control new file mode 100644 index 0000000000..9e31650266 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/control @@ -0,0 +1,84 @@ +Source: libopencv-xfeatures2d +Priority: optional +Section: devel +Maintainer: Ubuntu Developers +XSBC-Original-Maintainer: Debian Science Team +Uploaders: + Sam Hocevar (Debian packages) , + Nobuhiro Iwamatsu , +Build-Depends: + cmake (>= 2.8.7), + debhelper (>= 9), + doxygen, + libavcodec-dev, + libavformat-dev, + libavresample-dev, + libdc1394-22-dev [linux-any], + libeigen3-dev, + libgdal-dev, + libgdcm2-dev [!hppa !m68k !powerpcspe !sh4] | libgdcm-dev, + libgl1-mesa-dev, + libglu1-mesa-dev, + libgoogle-glog-dev, + libgphoto2-dev, + libgtk-3-dev, + libjpeg-dev, + liblapack-dev (>= 3.2.1), + libopenexr-dev (>= 1.6.1-8.1), + libpng-dev, + libprotobuf-dev, + libraw1394-dev [linux-any], + libswscale-dev, + libtbb-dev [amd64 arm64 armel armhf i386 mips mips64el mipsel ppc64el s390x powerpc powerpcspe ppc64 sh4 sparc64], + libtiff-dev, + libv4l-dev [linux-any], + libvtk6-dev | libvtk7-dev, + protobuf-compiler, + dh-python, + python3-dev, + python3-numpy, + zlib1g-dev (>= 1.2.5), +Standards-Version: 4.1.1 +Vcs-Git: https://anonscm.debian.org/git/debian-science/packages/opencv.git +Vcs-Browser: https://anonscm.debian.org/git/debian-science/packages/opencv.git +Homepage: https://opencv.org + +Package: libopencv-xfeatures2d-dev +Section: libdevel +Architecture: any +Depends: + libopencv-xfeatures2d (= ${binary:Version}), + pkg-config, + ${misc:Depends}, + ${shlibs:Depends}, +Description: development files for libopencv-xfeatures2d + This is a metapackage providing development package necessary for + development of OpenCV (Open Computer Vision). + . + The Open Computer Vision Library is a collection of algorithms and sample + code for various computer vision problems. The library is compatible with + IPL (Intel's Image Processing Library) and, if available, can use IPP + (Intel's Integrated Performance Primitives) for better performance. + . + OpenCV provides low level portable data types and operators, and a set + of high level functionalities for video acquisition, image processing and + analysis, structural analysis, motion analysis and object tracking, object + recognition, camera calibration and 3D reconstruction. + +Package: libopencv-xfeatures2d +Section: libdevel +Architecture: any +Multi-Arch: same +Depends: + ${misc:Depends}, +Description: computer vision x Feature Detection and Descriptor Extraction library + The Open Computer Vision Library is a collection of algorithms and sample + code for various computer vision problems. The library is compatible with + IPL (Intel's Image Processing Library) and, if available, can use IPP + (Intel's Integrated Performance Primitives) for better performance. + . + OpenCV provides low level portable data types and operators, and a set + of high level functionalities for video acquisition, image processing and + analysis, structural analysis, motion analysis and object tracking, object + recognition, camera calibration and 3D reconstruction. + diff --git a/scripts/setup/debians/opencv-xfeatures2d/copyright b/scripts/setup/debians/opencv-xfeatures2d/copyright new file mode 100644 index 0000000000..ac6fb76ed3 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/copyright @@ -0,0 +1,63 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0 +Upstream-Name: OpenCV +Source: https://opencv.org +Files-Excluded: + 3rdparty + doc/tutorials/introduction/biicode/images/bii_lena.png + doc/tutorials/introduction/clojure_dev_intro/images/lena.png + doc/tutorials/introduction/desktop_java/images/lena.png + modules/highgui/src/files_Qt/Milky + modules/java/common_test/res/drawable/lena.png + samples/data/lena.jpg + samples/data/lena_tmpl.jpg + samples/java/clojure/simple-sample/resources + samples/java/clojure/simple-sample/resources/images + samples/java/clojure/simple-sample/resources/images/lena.png + samples/winrt/OcvImageProcessing/OcvImageProcessing/Assets/Lena.png + samples/winrt_universal/PhoneTutorial/Lena.png + samples/wp8/OcvRotatingCube/PhoneXamlDirect3DApp1/PhoneXamlDirect3DApp1/Assets/Lena.png + samples/wp8/OpenCVXaml/OpenCVXaml/Assets/Lena.png +Files-Excluded-contrib: + modules/dnn/3rdparty + modules/xfeatures2d + + +Files: * +Copyright: 2000-2015 Intel Corporation + 2009-2011 Willow Garage Inc. + 2009-2015 NVIDIA Corporation + 2010-2013 Advanced Micro Devices, Inc. + 2015 OpenCV Foundation + 2015 Itseez Inc. +License: BSD-3-clause + +Files: debian/* +Copyright: 2004-2008 Sam Hocevar + 2009-2017 Nobuhiro Iwamatsu + 2016-2017 Mattia Rizzolo +License: BSD-3-clause + +License: BSD-3-clause + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + . + * Redistribution's [sic] of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + . + * Redistribution's [sic] in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + . + * The name of the copyright holders may not be used to endorse or promote products + derived from this software without specific prior written permission. + . + This software is provided by the copyright holders and contributors "as is" and + any express or implied warranties, including, but not limited to, the implied + warranties of merchantability and fitness for a particular purpose are disclaimed. + In no event shall the Intel Corporation or contributors be liable for any direct, + indirect, incidental, special, exemplary, or consequential damages + (including, but not limited to, procurement of substitute goods or services; + loss of use, data, or profits; or business interruption) however caused + and on any theory of liability, whether in contract, strict liability, + or tort (including negligence or otherwise) arising in any way out of + the use of this software, even if advised of the possibility of such damage. diff --git a/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d-dev.install b/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d-dev.install new file mode 100644 index 0000000000..edd160bc71 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d-dev.install @@ -0,0 +1,2 @@ +usr/include/opencv4/opencv2/xfeatures2d/* +usr/include/opencv4/opencv2/*xfeatures2d* diff --git a/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d.install.amd64 b/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d.install.amd64 new file mode 100644 index 0000000000..ee910e35c0 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d.install.amd64 @@ -0,0 +1 @@ +usr/lib/x86_64-linux-gnu/*xfeatures2d* diff --git a/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d.install.armhf b/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d.install.armhf new file mode 100644 index 0000000000..688ed56db1 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/libopencv-xfeatures2d.install.armhf @@ -0,0 +1 @@ +usr/lib/arm-linux-gnueabihf/*xfeatures2d* diff --git a/scripts/setup/debians/opencv-xfeatures2d/not-installed b/scripts/setup/debians/opencv-xfeatures2d/not-installed new file mode 100644 index 0000000000..2483ddb088 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/not-installed @@ -0,0 +1,22 @@ +usr/bin/* +usr/share/opencv4/* +usr/share/licenses/* +usr/include/opencv4/opencv2/opencv.hpp +usr/include/opencv4/opencv2/cvconfig.h +usr/include/opencv4/opencv2/opencv_modules.hpp +usr/include/opencv4/opencv2/core/* +usr/include/opencv4/opencv2/core.hpp +usr/include/opencv4/opencv2/calib3d/* +usr/include/opencv4/opencv2/calib3d.hpp +usr/include/opencv4/opencv2/features2d/* +usr/include/opencv4/opencv2/features2d.hpp +usr/include/opencv4/opencv2/flann/* +usr/include/opencv4/opencv2/flann.hpp +usr/include/opencv4/opencv2/imgproc/* +usr/include/opencv4/opencv2/imgproc.hpp +usr/lib/*/libopencv_core* +usr/lib/*/libopencv_calib* +usr/lib/*/libopencv_features2d* +usr/lib/*/libopencv_flann* +usr/lib/*/libopencv_imgproc* +usr/lib/*/cmake/opencv4/* diff --git a/scripts/setup/debians/opencv-xfeatures2d/rules b/scripts/setup/debians/opencv-xfeatures2d/rules new file mode 100755 index 0000000000..74d1165759 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/rules @@ -0,0 +1,69 @@ +#!/usr/bin/make -f + +export DEB_BUILD_MAINT_OPTIONS=hardening=+all +include /usr/share/dpkg/architecture.mk + +BUILDDIR = obj-$(DEB_HOST_MULTIARCH) + + +CMAKE_FLAGS = -DCMAKE_BUILD_TYPE=RELEASE \ + -DCMAKE_INSTALL_PREFIX="/usr" \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_DOCS=OFF \ + -DBUILD_TESTS=OFF \ + -DBUILD_PERF_TESTS=OFF \ + -DOPENCV_ENABLE_NONFREE=ON \ + -DOPENCV_EXTRA_MODULES_PATH="$(CURDIR)/../contrib/modules" \ + -DBUILD_LIST=xfeatures2d .. + +%: + dh $@ --with python3 --parallel + +override_dh_clean: + rm -rvf modules/python/src2/hdr_parser.pyc + rm -rvf modules/refman.rst + + dh_clean + -rm -rvf modules/python/src2/__pycache__ + +override_dh_auto_clean: + dh_auto_clean -B $(BUILDDIR) + + +override_dh_auto_configure: + dh_auto_configure -B $(BUILDDIR) \ + -- $(CMAKE_FLAGS) \ + -DCMAKE_SHARED_LINKER_FLAGS_RELEASE="$(LDFLAGS)" \ + -DWITH_V4L=OFF \ + -DWITH_LIBV4L=OFF \ + -DWITH_CUDA=OFF + + +override_dh_auto_build: + dh_auto_build -B $(BUILDDIR) + dh_auto_build -B $(BUILDDIR) + +override_dh_auto_test: + +override_dh_auto_install: + dh_auto_install -B $(BUILDDIR) + +override_dh_install: + dh_install + dh_missing --fail-missing + +override_dh_usrlocal: + +PKG = opencv +UVER = $(shell dpkg-parsechangelog -S Version | sed 's,-.*,,') +DTYPE = +dfsg +VER ?= $(subst $(DTYPE),,$(UVER)) +get-orig-source: + @echo "# OpenCV Downloading..." + uscan --no-conf --verbose --force-download --download-version $(VER) --no-symlink + @echo "# Packing..." + mk-origtargz --repack --compression xz -S $(DTYPE) -C .. ../opencv_$(VER).orig.tar.gz -v $(VER) + + @echo "# OpenCV contrib Downloading..." + uscan --no-conf --verbose --force-download --download-version $(VER) --no-symlink + mk-origtargz --repack --compression xz -S $(DTYPE) -C .. ../opencv_$(VER).orig-contrib.tar.gz -v $(VER) -c contrib diff --git a/scripts/setup/debians/opencv-xfeatures2d/samples/c/Makefile.debian b/scripts/setup/debians/opencv-xfeatures2d/samples/c/Makefile.debian new file mode 100644 index 0000000000..66afb331eb --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/samples/c/Makefile.debian @@ -0,0 +1,15 @@ +all: contours convert_cascade delaunay fback_c morphology motempl \ + polar_transforms pyramid_segmentation adaptiveskindetector \ + bgfg_codebook blobtrack_sample facedetect find_obj_calonder \ + find_obj find_obj_ferns latentsvmdetect mser_sample mushroom \ + one_way_sample tree_engine example_cmake/minarea + +example_cmake/minarea: example_cmake/minarea.c + +CPPFLAGS += $(shell pkg-config opencv --cflags) +LOADLIBES += $(shell pkg-config opencv --libs) +LOADLIBES += -lm +CFLAGS += -Wall +CXXFLAGS += -Wall +CFLAGS += -O2 +CXXFLAGS += -O2 diff --git a/scripts/setup/debians/opencv-xfeatures2d/source/format b/scripts/setup/debians/opencv-xfeatures2d/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/opencv-xfeatures2d/source/lintian-overrides b/scripts/setup/debians/opencv-xfeatures2d/source/lintian-overrides new file mode 100644 index 0000000000..a2a757936a --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/source/lintian-overrides @@ -0,0 +1,2 @@ +# https://bugs.debian.org/505857 (in lintian) +debian-watch-file-should-mangle-version diff --git a/scripts/setup/debians/opencv-xfeatures2d/watch b/scripts/setup/debians/opencv-xfeatures2d/watch new file mode 100644 index 0000000000..8585ba8db6 --- /dev/null +++ b/scripts/setup/debians/opencv-xfeatures2d/watch @@ -0,0 +1,20 @@ +version=4 + +opts="\ +compression=xz, \ +repacksuffix=+dfsg, \ +dversionmangle=s/\+(dfsg|ds)\d?$//, \ +filenamemangle=s%(?:.*?)?v?(\d[\d.]*)\.(.+)$%@PACKAGE@_$1.orig.$2%, \ +" \ + https://github.com/opencv/opencv/tags \ + (?:.*?/)?v?(\d[\d.]*)@ARCHIVE_EXT@ debian + +opts="\ +compression=xz, \ +repacksuffix=+dfsg, \ +component=contrib, \ +dversionmangle=s/\+(dfsg|ds)\d?$//, \ +filenamemangle=s%(?:.*?)?v?(\d[\d.]*)\.(.+)$%@PACKAGE@_$1.orig-contrib.$2%, \ +" \ + https://github.com/opencv/opencv_contrib/tags \ + (?:.*?/)?v?(\d[\d.]*)@ARCHIVE_EXT@ same diff --git a/scripts/setup/debians/openmvg/changelog b/scripts/setup/debians/openmvg/changelog index ca04f40ae1..ec43285353 100644 --- a/scripts/setup/debians/openmvg/changelog +++ b/scripts/setup/debians/openmvg/changelog @@ -1,3 +1,16 @@ +libopenmvg (1.1-3) unstable; urgency=medium + + * Minor OpenMVGConfig.cmake syntax fix + + -- Ruben Garcia Fri, 03 May 2024 10:16:40 -0700 + +libopenmvg (1.1-2) unstable; urgency=medium + + * Remove libceres-dev from libopenmvg1 depends since it is not + needed in the robots. This saves space. + + -- Ruben Garcia Wed, 05 Apr 2023 11:49:59 -0800 + libopenmvg (1.1-1) unstable; urgency=medium * Initial release diff --git a/scripts/setup/debians/openmvg/control b/scripts/setup/debians/openmvg/control index 0b07a6347e..4fa385772a 100644 --- a/scripts/setup/debians/openmvg/control +++ b/scripts/setup/debians/openmvg/control @@ -17,6 +17,6 @@ Description: Multi-view geometry library. Package: libopenmvg1 Architecture: any -Depends: libceres-dev (>=1.12), ${shlibs:Depends}, ${misc:Depends} +Depends: ${shlibs:Depends}, ${misc:Depends} Description: Multi-view geometry library. diff --git a/scripts/setup/debians/openmvg/patches/fix_cmake.patch b/scripts/setup/debians/openmvg/patches/fix_cmake.patch new file mode 100644 index 0000000000..242519a629 --- /dev/null +++ b/scripts/setup/debians/openmvg/patches/fix_cmake.patch @@ -0,0 +1,13 @@ +diff --git a/src/cmakeFindModules/OpenMVGConfig.cmake.in b/src/cmakeFindModules/OpenMVGConfig.cmake.in +index 01d4df98..64a4739d 100644 +--- a/src/cmakeFindModules/OpenMVGConfig.cmake.in ++++ b/src/cmakeFindModules/OpenMVGConfig.cmake.in +@@ -42,7 +42,7 @@ MACRO(OPENMVG_REPORT_NOT_FOUND REASON_MSG) + # FindPackage() use the camelcase library name, not uppercase. + IF (OPENMVG_FIND_QUIETLY) + MESSAGE(STATUS "Failed to find OPENMVG - " ${REASON_MSG} ${ARGN}) +- ELSE (OPENMVG_FIND_REQUIRED) ++ ELSEIF (OPENMVG_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Failed to find OPENMVG - " ${REASON_MSG} ${ARGN}) + ELSE() + # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error diff --git a/scripts/setup/debians/openmvg/patches/series b/scripts/setup/debians/openmvg/patches/series index 81a6cb10b8..d5bb3c044a 100644 --- a/scripts/setup/debians/openmvg/patches/series +++ b/scripts/setup/debians/openmvg/patches/series @@ -6,3 +6,4 @@ shared_libraries.patch shared_internal_libs.patch find_quiet.patch no_infty_computer_vision.patch +fix_cmake.patch diff --git a/scripts/setup/debians/readme.md b/scripts/setup/debians/readme.md index a00fd65b65..1b6bf83692 100644 --- a/scripts/setup/debians/readme.md +++ b/scripts/setup/debians/readme.md @@ -22,3 +22,67 @@ number with a new entry in the changelog. If changes to the patches are needed, work in the libxxx directory created by the script. Use `quilt` to modify the patches. +# Building for the robot (armhf) + +Note 1. In order to create the required chroot you must have access to the +astrobee server (it requires VPN). + +Note 2. These steps assume you have access and have checkout both the astrobee and +astrobee_platform repositories. + +## Environment + +Select a directory where to install the Astrobee debbuild chroot + + B= + mkdir -p $B + +Define the location of the astrobee and astrobee_platform repository + + P= + S= + + +Select which distribution you want to build for: + + # xenial for Ubuntu 16 or focal for Ubuntu 20 + D= + +## Create a debbuild chroot + + $P/rootfs/make_chroot.sh $D debbuild $B/debbuild_${D} + +## Copy our debian scripts to your chroot + + sudo cp -a $S/scripts/setup/debians $B/debbuild_${D}/data/ + +## Test your chroot + +Access the chroot shell + + sudo $P/rootfs/chroot.sh $B/debbuild_${D} + +From the chroot shell, ensure Internet access + + wget www.github.com --no-check-certificate + +You should now have an index.html file in the current directory. + +Note: In case of failure please review your chroot and host computer DNS settings. +For example, you may want to try editing the /etc/resolv.conf file on your chroot: + + sudo vim $B/debbuild_${D}/etc/resolv.conf + # You can try moving the 8.8.8.8 server to the top of the file + +## Build debians + +From your chroot shell, execute the following script to build all debians. +This will also install them in order to test them. You may omit the `--install` +flag if desired. + + cd /data/debians + ./build_debians.sh --install + +Inspect generated files: + + ls $B/debbuild_${D}/data/debians diff --git a/scripts/setup/debians/rosjava/.gitignore b/scripts/setup/debians/rosjava/.gitignore new file mode 100644 index 0000000000..27736c0d69 --- /dev/null +++ b/scripts/setup/debians/rosjava/.gitignore @@ -0,0 +1,12 @@ +*.build +*.changes +*.dsc +*.tar.xz +*.tar.gz +*.deb +*.ddeb +*.buildinfo +ros-noetic-rosjava-build-tools +ros-noetic-rosjava-bootstrap +ros-noetic-genjava +ros-noetic-rosjava-messages diff --git a/scripts/setup/debians/rosjava/bootstrap/changelog b/scripts/setup/debians/rosjava/bootstrap/changelog new file mode 100644 index 0000000000..651cb4957c --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/changelog @@ -0,0 +1,5 @@ +ros-noetic-rosjava-bootstrap (0.3.3-1astrobeefocal) focal; urgency=high + + * Add Python3 basic support + + -- Ruben Garcia Thu, 15 Feb 2024 02:00:00 -0000 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/bootstrap/compat b/scripts/setup/debians/rosjava/bootstrap/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/bootstrap/control b/scripts/setup/debians/rosjava/bootstrap/control new file mode 100644 index 0000000000..4b61941a65 --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/control @@ -0,0 +1,12 @@ +Source: ros-noetic-rosjava-bootstrap +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-rosjava-build-tools +Homepage: http://ros.org/wiki/rosjava_bootstrap +Standards-Version: 3.9.2 + +Package: ros-noetic-rosjava-bootstrap +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-rosjava-build-tools +Description: Bootstrap utilities for rosjava builds. diff --git a/scripts/setup/debians/rosjava/bootstrap/rules b/scripts/setup/debians/rosjava/bootstrap/rules new file mode 100755 index 0000000000..cebf5a8ec7 --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-rosjava-bootstrap//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/rosjava/bootstrap/source/format b/scripts/setup/debians/rosjava/bootstrap/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/rosjava/bootstrap/source/options b/scripts/setup/debians/rosjava/bootstrap/source/options new file mode 100644 index 0000000000..8bc9182a24 --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/scripts/setup/debians/rosjava/build-tools/changelog b/scripts/setup/debians/rosjava/build-tools/changelog new file mode 100644 index 0000000000..da3394c467 --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/changelog @@ -0,0 +1,5 @@ +ros-noetic-rosjava-build-tools (0.3.3-1astrobeefocal) focal; urgency=high + + * Add Python3 basic support + + -- Ruben Garcia Thu, 15 Feb 2024 02:00:00 -0000 diff --git a/scripts/setup/debians/rosjava/build-tools/compat b/scripts/setup/debians/rosjava/build-tools/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/build-tools/control b/scripts/setup/debians/rosjava/build-tools/control new file mode 100644 index 0000000000..584edbee6f --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/control @@ -0,0 +1,12 @@ +Source: ros-noetic-rosjava-build-tools +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), ant, openjdk-8-jdk, ros-noetic-catkin +Homepage: http://ros.org/wiki/rosjava_build_tools +Standards-Version: 3.9.2 + +Package: ros-noetic-rosjava-build-tools +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ant, openjdk-8-jdk, ros-noetic-catkin +Description: Simple tools and catkin modules for rosjava development. diff --git a/scripts/setup/debians/rosjava/build-tools/patches/python3_compat.patch b/scripts/setup/debians/rosjava/build-tools/patches/python3_compat.patch new file mode 100644 index 0000000000..1420cf5ced --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/patches/python3_compat.patch @@ -0,0 +1,121 @@ +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/__init__.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/__init__.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/__init__.py +@@ -4,10 +4,11 @@ + # Imports + ############################################################################## + +-import console +-from create_package import init_android_package, init_rosjava_package +-from create_android_project import create_android_project +-from create_rosjava_project import create_rosjava_project, create_rosjava_msg_project, create_rosjava_library_project +-from utils import which +-from release import scrape_for_release_message_packages +-import catkin ++from rosjava_build_tools import console ++from rosjava_build_tools.create_package import init_android_package, init_rosjava_package ++from rosjava_build_tools.create_android_project import create_android_project ++from rosjava_build_tools.create_rosjava_project import create_rosjava_project, create_rosjava_msg_project, create_rosjava_library_project ++from rosjava_build_tools.utils import which ++from rosjava_build_tools.release import scrape_for_release_message_packages ++from rosjava_build_tools import catkin ++ +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_android_project.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/create_android_project.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_android_project.py +@@ -11,11 +11,10 @@ import sys + import argparse + import subprocess + import shutil +-import exceptions + + # local imports +-import utils +-import console ++from rosjava_build_tools import utils ++from rosjava_build_tools import console + + ############################################################################## + # Methods +@@ -72,9 +71,9 @@ def actually_create_android_project(pack + except subprocess.CalledProcessError: + print("Error") + raise subprocess.CalledProcessError("failed to create android project.") +- except exceptions.OSError as e: ++ except OSError as e: + print("OS error" + str(e)) +- raise exceptions.OSError() ++ raise OSError() + + # This is in the old form, let's shovel the shit around to the new form + utils.mkdir_p(os.path.join(path, 'src', 'main', 'java')) +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_package.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/create_package.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_package.py +@@ -12,8 +12,8 @@ import catkin_pkg + from catkin_pkg.package_templates import create_package_xml, PackageTemplate + + # local imports +-import utils +-import console ++from rosjava_build_tools import utils ++from rosjava_build_tools import console + + ############################################################################## + # Methods +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_rosjava_project.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/create_rosjava_project.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_rosjava_project.py +@@ -13,8 +13,8 @@ import argparse + import xml.etree.ElementTree as ElementTree + + # local imports +-import utils +-import console ++from rosjava_build_tools import utils ++from rosjava_build_tools import console + + ############################################################################## + # Methods +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/release.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/release.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/release.py +@@ -6,7 +6,7 @@ + + import rosdistro + import catkin_pkg +-from . import catkin ++from rosjava_build_tools import catkin + + ############################################################################## + # Imports +@@ -16,9 +16,9 @@ from . import catkin + def scrape_for_release_message_packages(track): + url = rosdistro.get_index_url() + index = rosdistro.get_index(url) +- cache = rosdistro.get_release_cache(index, 'kinetic') ++ cache = rosdistro.get_release_cache(index, 'noetic') + packages = [] +- for package_name, package_string in cache.package_xmls.iteritems(): ++ for package_name, package_string in cache.package_xmls.items(): + package = catkin_pkg.package.parse_package_string(package_string) + #print(" Name: %s" % package_name) + #print(" Buildtool Depends %s" % package.build) +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/utils.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/utils.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/utils.py +@@ -8,7 +8,7 @@ import os + import sys + import errno + import pwd +-import console ++from rosjava_build_tools import console + + ############################################################################## + # Methods diff --git a/scripts/setup/debians/rosjava/build-tools/patches/series b/scripts/setup/debians/rosjava/build-tools/patches/series new file mode 100644 index 0000000000..8e52d4741b --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/patches/series @@ -0,0 +1 @@ +python3_compat.patch diff --git a/scripts/setup/debians/rosjava/build-tools/rules b/scripts/setup/debians/rosjava/build-tools/rules new file mode 100755 index 0000000000..8f579861ce --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-rosjava-build-tools//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/rosjava/build-tools/source/format b/scripts/setup/debians/rosjava/build-tools/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/rosjava/build-tools/source/options b/scripts/setup/debians/rosjava/build-tools/source/options new file mode 100644 index 0000000000..8bc9182a24 --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/scripts/setup/debians/rosjava/build_bootstrap.sh b/scripts/setup/debians/rosjava/build_bootstrap.sh new file mode 100755 index 0000000000..02cfe6f566 --- /dev/null +++ b/scripts/setup/debians/rosjava/build_bootstrap.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=ros-noetic-rosjava-bootstrap +ORIG_TAR=ros-noetic-rosjava-bootstrap_0.3.3.orig.tar.gz +DEB_DIR=bootstrap + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi + +git clone --quiet https://github.com/rosjava/rosjava_bootstrap.git --branch kinetic $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/rosjava/build_build-tools.sh b/scripts/setup/debians/rosjava/build_build-tools.sh new file mode 100755 index 0000000000..87486116cd --- /dev/null +++ b/scripts/setup/debians/rosjava/build_build-tools.sh @@ -0,0 +1,36 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# make sure setuptools from pip3 matches apt version +# you may need to fix it like this: pip3 install setuptools==45.2.0 && pip3 install -U testresources + +PACKAGE_NAME=ros-noetic-rosjava-build-tools +ORIG_TAR=ros-noetic-rosjava-build-tools_0.3.3.orig.tar.gz +DEB_DIR=build-tools + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi + +git clone --quiet https://github.com/rosjava/rosjava_build_tools.git --branch kinetic $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/rosjava/build_genjava.sh b/scripts/setup/debians/rosjava/build_genjava.sh new file mode 100755 index 0000000000..10624dfd7b --- /dev/null +++ b/scripts/setup/debians/rosjava/build_genjava.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=ros-noetic-genjava +ORIG_TAR=ros-noetic-genjava_0.3.4.orig.tar.gz +DEB_DIR=genjava + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi + +git clone --quiet https://github.com/rosjava/genjava.git --branch kinetic $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/rosjava/build_messages.sh b/scripts/setup/debians/rosjava/build_messages.sh new file mode 100755 index 0000000000..853edd4f9d --- /dev/null +++ b/scripts/setup/debians/rosjava/build_messages.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=ros-noetic-rosjava-messages +ORIG_TAR=ros-noetic-rosjava-messages_0.3.0.orig.tar.gz +DEB_DIR=messages + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi + +git clone --quiet https://github.com/rosjava/rosjava_messages.git --branch kinetic $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/rosjava/build_rosjava_debians.sh b/scripts/setup/debians/rosjava/build_rosjava_debians.sh new file mode 100755 index 0000000000..42ffd4c232 --- /dev/null +++ b/scripts/setup/debians/rosjava/build_rosjava_debians.sh @@ -0,0 +1,78 @@ +#!/bin/bash -e +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Build and install flight software custom dependencies. +set -e + +debian_loc=$(dirname "$(readlink -f "$0")") +build_list=() +install_debians=false +install_deps=false + +die () { + printf "${c_red}${c_bold}[ FATAL ]${c_reset} ${c_bold}%b${c_reset}\\n" "$1" >&2 + exit "${2:-1}" +} + +cleanup() { + rm -rf ros-noetic-rosjava-build-tools ros-noetic-rosjava-bootstrap \ + ros-noetic-genjava ros-noetic-rosjava-messages +} + +# delete old files (-f avoids 'no such file' warning on first run) +rm -f *.deb *.debian.tar.xz *.orig.tar.gz *.dsc *.build *.buildinfo *.changes *.ddeb + +# Process arguments and take action +while [[ $# -gt 0 ]]; do + case "$1" in + -i|--install) + install_debians=true + ;; + -d|--install-with-deps) + install_debians=true + install_deps=true + ;; + esac + shift +done + +# Add public debians to build list +build_list+=( build-tools bootstrap genjava messages ) + +echo "Building debians" +trap 'cleanup' EXIT + +for pkg in ${build_list[@]} +do + # Dependencies + if $install_deps ; then + cd ${debian_loc}/$pkg || die + sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control || + die "Failed to install dependencies for $pkg" + fi + + # Building + cd "$debian_loc" + ./build_${pkg}.sh || die "Failed to build $pkg" + + # Installing + if $install_debians ; then + sudo dpkg -i *${pkg}*.deb || die "Failed to install $pkg" + fi +done diff --git a/scripts/setup/debians/rosjava/genjava/changelog b/scripts/setup/debians/rosjava/genjava/changelog new file mode 100644 index 0000000000..8d1af52dec --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/changelog @@ -0,0 +1,5 @@ +ros-noetic-genjava (0.3.4-1astrobeefocal) focal; urgency=high + + * Add Python3 basic support + + -- Ruben Garcia Thu, 15 Feb 2024 02:00:00 -0000 diff --git a/scripts/setup/debians/rosjava/genjava/compat b/scripts/setup/debians/rosjava/genjava/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/genjava/control b/scripts/setup/debians/rosjava/genjava/control new file mode 100644 index 0000000000..f86ceb1948 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/control @@ -0,0 +1,12 @@ +Source: ros-noetic-genjava +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), python3-catkin-pkg, python3-rospkg, ros-noetic-catkin, ros-noetic-genmsg, ros-noetic-rosjava-bootstrap, ros-noetic-rosjava-build-tools +Homepage: http://www.ros.org/wiki/genjava +Standards-Version: 3.9.2 + +Package: ros-noetic-genjava +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, python3-catkin-pkg, python3-rospkg, ros-noetic-genmsg, ros-noetic-rosjava-bootstrap, ros-noetic-rosjava-build-tools +Description: Java ROS message and service generators. diff --git a/scripts/setup/debians/rosjava/genjava/patches/python3_compat.patch b/scripts/setup/debians/rosjava/genjava/patches/python3_compat.patch new file mode 100644 index 0000000000..e374e1e638 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/patches/python3_compat.patch @@ -0,0 +1,13 @@ +Index: ros-noetic-genjava/src/genjava/gradle_project.py +=================================================================== +--- ros-noetic-genjava.orig/src/genjava/gradle_project.py ++++ ros-noetic-genjava/src/genjava/gradle_project.py +@@ -79,7 +79,7 @@ def get_templates(): + + def populate_project(project_name, project_version, pkg_directory, gradle_project_dir, msg_dependencies): + author = author_name() +- for filename, template in get_templates().iteritems(): ++ for filename, template in get_templates().items(): + contents = instantiate_genjava_template(template, project_name, project_version, pkg_directory, author, msg_dependencies) + try: + p = os.path.abspath(os.path.join(gradle_project_dir, filename)) diff --git a/scripts/setup/debians/rosjava/genjava/patches/series b/scripts/setup/debians/rosjava/genjava/patches/series new file mode 100644 index 0000000000..8e52d4741b --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/patches/series @@ -0,0 +1 @@ +python3_compat.patch diff --git a/scripts/setup/debians/rosjava/genjava/rules b/scripts/setup/debians/rosjava/genjava/rules new file mode 100755 index 0000000000..6a5918ca68 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-genjava//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/rosjava/genjava/source/format b/scripts/setup/debians/rosjava/genjava/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/rosjava/genjava/source/options b/scripts/setup/debians/rosjava/genjava/source/options new file mode 100644 index 0000000000..8bc9182a24 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/scripts/setup/debians/rosjava/messages/changelog b/scripts/setup/debians/rosjava/messages/changelog new file mode 100644 index 0000000000..30b0aaebcc --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/changelog @@ -0,0 +1,5 @@ +ros-noetic-rosjava-messages (0.3.0-0astrobeefocal) focal; urgency=high + + * Generate only Astrobee dependencies + + -- Ruben Garcia Thu, 15 Feb 2024 02:00:00 -0000 diff --git a/scripts/setup/debians/rosjava/messages/compat b/scripts/setup/debians/rosjava/messages/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/messages/control b/scripts/setup/debians/rosjava/messages/control new file mode 100644 index 0000000000..2871a23c61 --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/control @@ -0,0 +1,12 @@ +Source: ros-noetic-rosjava-messages +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), ros-noetic-actionlib-msgs, ros-noetic-catkin, ros-noetic-genjava, ros-noetic-geometry-msgs, ros-noetic-roscpp, ros-noetic-rosjava-build-tools, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-trajectory-msgs +Homepage: http://ros.org/wiki/rosjava_messages +Standards-Version: 3.9.2 + +Package: ros-noetic-rosjava-messages +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-genjava, ros-noetic-rosjava-build-tools +Description: Message generation for rosjava. diff --git a/scripts/setup/debians/rosjava/messages/patches/reduce_package_selection.patch b/scripts/setup/debians/rosjava/messages/patches/reduce_package_selection.patch new file mode 100644 index 0000000000..7bb9b4d339 --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/patches/reduce_package_selection.patch @@ -0,0 +1,67 @@ +Index: ros-noetic-rosjava-messages/CMakeLists.txt +=================================================================== +--- ros-noetic-rosjava-messages.orig/CMakeLists.txt ++++ ros-noetic-rosjava-messages/CMakeLists.txt +@@ -14,36 +14,36 @@ find_package(catkin REQUIRED genjava) + generate_rosjava_messages( + PACKAGES + std_msgs +- rosgraph_msgs # ros/ros_comm_msgs +- std_srvs +- rosjava_test_msgs # rosjava/rosjava_test_msgs ++ #rosgraph_msgs # ros/ros_comm_msgs ++ #std_srvs ++ #rosjava_test_msgs # rosjava/rosjava_test_msgs + actionlib_msgs # ros/common_msgs +- common_msgs +- diagnostic_msgs ++ #common_msgs ++ #diagnostic_msgs + geometry_msgs +- nav_msgs ++ #nav_msgs + sensor_msgs +- shape_msgs +- stereo_msgs ++ #shape_msgs ++ #stereo_msgs + trajectory_msgs +- visualization_msgs +- tf2_msgs # geometry_experimental/tf2_msgs +- ar_track_alvar_msgs # sniekum/ar_track_alvar_msgs +- uuid_msgs # ros-geographic-info/unique_identifier +- yocs_msgs # yujinrobot/yocs_msgs +- concert_msgs # robotics-in-concert/rocon_msgs +- concert_service_msgs +- gateway_msgs +- rocon_app_manager_msgs +- rocon_device_msgs +- rocon_interaction_msgs +- rocon_service_pair_msgs +- rocon_std_msgs +- rocon_tutorial_msgs +- world_canvas_msgs +- scheduler_msgs +- move_base_msgs # ros-planning/navigation +- map_store # ros-planning/map_store ++ #visualization_msgs ++ #tf2_msgs # geometry_experimental/tf2_msgs ++ #ar_track_alvar_msgs # sniekum/ar_track_alvar_msgs ++ #uuid_msgs # ros-geographic-info/unique_identifier ++ #yocs_msgs # yujinrobot/yocs_msgs ++ #concert_msgs # robotics-in-concert/rocon_msgs ++ #concert_service_msgs ++ #gateway_msgs ++ #rocon_app_manager_msgs ++ #rocon_device_msgs ++ #rocon_interaction_msgs ++ #rocon_service_pair_msgs ++ #rocon_std_msgs ++ #rocon_tutorial_msgs ++ #world_canvas_msgs ++ #scheduler_msgs ++ #move_base_msgs # ros-planning/navigation ++ #map_store # ros-planning/map_store + + roscpp # !!!!!!! REMOVE THIS ONCE https://github.com/ros-planning/navigation/pull/312 is fixed! !!!!!!!! + ) diff --git a/scripts/setup/debians/rosjava/messages/patches/series b/scripts/setup/debians/rosjava/messages/patches/series new file mode 100644 index 0000000000..388ec32e1e --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/patches/series @@ -0,0 +1 @@ +reduce_package_selection.patch diff --git a/scripts/setup/debians/rosjava/messages/rules b/scripts/setup/debians/rosjava/messages/rules new file mode 100755 index 0000000000..cbe2127f2a --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/rules @@ -0,0 +1,62 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +export DH_OPTIONS=-v --buildsystem=cmake +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-rosjava-messages//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/rosjava/messages/source/format b/scripts/setup/debians/rosjava/messages/source/format new file mode 100644 index 0000000000..46ebe02665 --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/source/format @@ -0,0 +1 @@ +3.0 (quilt) \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/readme.md b/scripts/setup/debians/rosjava/readme.md new file mode 100644 index 0000000000..498e28ba63 --- /dev/null +++ b/scripts/setup/debians/rosjava/readme.md @@ -0,0 +1,48 @@ +# Java Generator for ROS Noetic + +These scripts are meant to patch and repackage the Java Generator for ROS +Noetic. + +Generated Debians include: + +- ros-noetic-rosjava-build-tools +- ros-noetic-rosjava-bootstrap +- ros-noetic-genjava +- ros-noetic-rosjava-messages + +These are based on official ros-kinetic versions. Minimal patches have been +applied to make them compatible with Astrobee usage. + +These Debians are not meant for general rosjava use. They are made with +Astrobee-specific needs in mind. + +## Notes + +### Building multiple times + +Depending on the system, some of these Debians may not be able to compile again +if their package is installed. If you need to recompile these Debians for some +reason, please remove them before: + +```shell +sudo apt remove ros-noetic-rosjava-messages ros-noetic-genjava ros-noetic-rosjava-bootstrap ros-noetic-rosjava-build-tools +``` + +### Build-tools and Python setuptools + +The build-tools Debian may have issues compiling if the pip3 setuptools +package does not match the default Ubuntu 20 version. + +**If pip3 is not installed in your system this may not apply to you.** + +```shell +# Versions should match (default: 45.2.0) +pip3 list | grep setuptools +apt show python3-setuptools +``` + +If they don't match, adjust the pip3 version. Example: + +```shell +pip3 install setuptools==45.2.0 && pip3 install -U testresources +``` \ No newline at end of file diff --git a/scripts/setup/debians/soracore/changelog b/scripts/setup/debians/soracore/changelog new file mode 100644 index 0000000000..61d89f836f --- /dev/null +++ b/scripts/setup/debians/soracore/changelog @@ -0,0 +1,11 @@ +libsoracore (2.0-1) unstable; urgency=medium + + * Ubuntu 18 compatible. + + -- Brian Coltin Tue, 11 May 2021 10:23:43 -0700 + +libsoracore (1.0-1) unstable; urgency=medium + + * Initial release. + + -- Brian Coltin Tue, 09 May 2017 10:23:43 -0700 diff --git a/scripts/setup/debians/soracore/compat b/scripts/setup/debians/soracore/compat new file mode 100644 index 0000000000..ec635144f6 --- /dev/null +++ b/scripts/setup/debians/soracore/compat @@ -0,0 +1 @@ +9 diff --git a/scripts/setup/debians/soracore/control b/scripts/setup/debians/soracore/control new file mode 100644 index 0000000000..f66163383e --- /dev/null +++ b/scripts/setup/debians/soracore/control @@ -0,0 +1,25 @@ +Source: libsoracore +Priority: optional +Maintainer: Brian Coltin +Build-Depends: debhelper (>=9), rti-dev (>=5.1), libmiro-dev (>=0.3), libeigen3-dev (>=3.0) +Standards-Version: 3.9.6 +Section: libs +Homepage: https://github.com/mallanmba/soraCore/ + +Package: libsoracore-dev +Section: libdevel +Architecture: any +Depends: rti-dev (>=5.1), + libmiro-dev (>=0.3), + libeigen3-dev (>=3.0), + libsoracore1 (= ${binary:Version}), ${misc:Depends} +Description: Soracore rover software. + Rover software core libraries. + +Package: libsoracore1 +Architecture: any +Depends: rti (>=5.1), + libmiro0 (>=0.3), + ${shlibs:Depends}, ${misc:Depends} +Description: Soracore rover software. + Rover software core libraries. diff --git a/scripts/setup/debians/soracore/copyright b/scripts/setup/debians/soracore/copyright new file mode 100644 index 0000000000..da3c1edc4f --- /dev/null +++ b/scripts/setup/debians/soracore/copyright @@ -0,0 +1,37 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ +Upstream-Name: libsoracore +Source: https://github.com/mallanmba/soraCore/ + +Files: * +Copyright: 2017 SoraCore Developers +License: Apache2 + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + . + http://www.apache.org/licenses/LICENSE-2.0 + . + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +Files: debian/* +Copyright: 2017 Brian Coltin +License: GPL-2+ + This package is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + . + This package is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + . + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + On Debian systems, the complete text of the GNU General + Public License version 2 can be found in "/usr/share/common-licenses/GPL-2". diff --git a/scripts/setup/debians/soracore/files b/scripts/setup/debians/soracore/files new file mode 100644 index 0000000000..ff7d04d3ae --- /dev/null +++ b/scripts/setup/debians/soracore/files @@ -0,0 +1,4 @@ +libsoracore-dev_2.0-1_amd64.deb libdevel optional +libsoracore1-dbgsym_2.0-1_amd64.ddeb debug optional +libsoracore1_2.0-1_amd64.deb libs optional +libsoracore_2.0-1_amd64.buildinfo libs optional diff --git a/scripts/setup/debians/soracore/libsoracore-dev.dirs b/scripts/setup/debians/soracore/libsoracore-dev.dirs new file mode 100644 index 0000000000..d8226e61e7 --- /dev/null +++ b/scripts/setup/debians/soracore/libsoracore-dev.dirs @@ -0,0 +1,3 @@ +usr/lib +usr/lib/cmake +usr/include diff --git a/scripts/setup/debians/soracore/libsoracore-dev.install b/scripts/setup/debians/soracore/libsoracore-dev.install new file mode 100644 index 0000000000..cbc01d537c --- /dev/null +++ b/scripts/setup/debians/soracore/libsoracore-dev.install @@ -0,0 +1,2 @@ +usr/include/* +usr/lib/cmake/* diff --git a/scripts/setup/debians/soracore/libsoracore1.dirs b/scripts/setup/debians/soracore/libsoracore1.dirs new file mode 100644 index 0000000000..8109f4f9fa --- /dev/null +++ b/scripts/setup/debians/soracore/libsoracore1.dirs @@ -0,0 +1,4 @@ +usr/lib +usr/bin +usr/etc +usr/idl diff --git a/scripts/setup/debians/soracore/libsoracore1.install b/scripts/setup/debians/soracore/libsoracore1.install new file mode 100644 index 0000000000..566eb422a6 --- /dev/null +++ b/scripts/setup/debians/soracore/libsoracore1.install @@ -0,0 +1,4 @@ +usr/lib/lib*.so +usr/bin/* +usr/etc/* +usr/idl/* diff --git a/gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_ref.rsp b/scripts/setup/debians/soracore/patches/series similarity index 100% rename from gnc/matlab/code_generation/bpm_blower_2_propulsion_module_ert_rtw/bpm_blower_2_propulsion_module_ref.rsp rename to scripts/setup/debians/soracore/patches/series diff --git a/scripts/setup/debians/soracore/rules b/scripts/setup/debians/soracore/rules new file mode 100755 index 0000000000..ac7053cc91 --- /dev/null +++ b/scripts/setup/debians/soracore/rules @@ -0,0 +1,23 @@ +#!/usr/bin/make -f +# See debhelper(7) (uncomment to enable) +# output every command that modifies files on the build system. +#export DH_VERBOSE = 1 + + +# see FEATURE AREAS in dpkg-buildflags(1) +#export DEB_BUILD_MAINT_OPTIONS = hardening=+all + +# see ENVIRONMENT in dpkg-buildflags(1) +# package maintainers to append CFLAGS +#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic +# package maintainers to append LDFLAGS +#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed + +export NDDSHOME=/opt/rti/ndds +export NDDSARCH=$(shell ls $(NDDSHOME)/lib | grep -v jdk) + +%: + dh $@ --parallel --buildsystem=cmake + +override_dh_auto_configure: + dh_auto_configure -- -DCMAKE_SHARED_LINKER_FLAGS="-Wl,-allow-multiple-definition" -DMIRO_BUILD_WITH_QT5=true diff --git a/scripts/setup/debians/soracore/source/format b/scripts/setup/debians/soracore/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/soracore/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/update_release.sh b/scripts/setup/debians/update_release.sh index 088a80ceac..303e2232ba 100755 --- a/scripts/setup/debians/update_release.sh +++ b/scripts/setup/debians/update_release.sh @@ -12,7 +12,7 @@ cd $DIR/../../.. dch -c debian/changelog -v $1 -sed -i -e "s/set(ASTROBEE_VERSION .*)/set(ASTROBEE_VERSION $1)/g" CMakeLists.txt +sed -i -e "s/set(ASTROBEE_VERSION .*)/set(ASTROBEE_VERSION $1)/g" astrobee/CMakeLists.txt sed -i -e "s/\# Astrobee Robot Software v1/\# Astrobee Robot Software v1\n\n\#\# Release $1\n\nINSERT DESCRIPTION HERE/g" RELEASE.md diff --git a/scripts/setup/gen_packages.sh b/scripts/setup/gen_packages.sh deleted file mode 100755 index c9fbb9a600..0000000000 --- a/scripts/setup/gen_packages.sh +++ /dev/null @@ -1,35 +0,0 @@ -#!/bin/sh -e -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -# Generate the packages.lst file used by the install_desktop_packages.sh -# script. This script needs to know the location of the "platform" directory -# where the rootfs *.conf files are located. - -platform_dir=${1:-../../submodules/platform} -platform_dir=$(realpath "$platform_dir") - -if [ ! -d "$platform_dir/rootfs" ]; then - echo "$platform_dir is not a valid platform directory. aborting." - exit 1 -fi - -conf_files="$platform_dir/rootfs/ros_16_04.conf $platform_dir/rootfs/dev_16_04.conf" - -fgrep "packages=" ${conf_files} | cut -f 2 -d "=" | tr ' ' '\n' >packages_base.lst - diff --git a/scripts/setup/install_desktop_16_04_packages.sh b/scripts/setup/install_desktop_16_04_packages.sh deleted file mode 100755 index a75d33aee0..0000000000 --- a/scripts/setup/install_desktop_16_04_packages.sh +++ /dev/null @@ -1,76 +0,0 @@ -#!/bin/bash -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -# Install a set of packages to build an Ubuntu 16.04 distro with -# all the development tools required for Astrobee work. -# -# The script uses the package list files for the list of -# packages to install. - -scriptdir=$(dirname "$(readlink -f "$0")") -arssrc=/etc/apt/sources.list.d/astrobee-latest.list - -pkg_files=${1:-$scriptdir/packages_base.lst $scriptdir/packages_desktop.lst} -echo $pkg_files - -pkgs='' -for i in $pkg_files; do - [ -r "$i" ] || continue - - # this is a non-portable bash-ism. - pkgs="$pkgs $(<"$i")" -done - -# if this file exists, we are using the astrobee repository -# need to set up tunnel through m (NASA users only) -if [ -e $arssrc ]; -then - username=${NDC_USERNAME:+${NDC_USERNAME}@} - - # Add these packages to the apt sources (we remove them later, so apt update succeeds) - - NO_TUNNEL=${NO_TUNNEL:-0} # Override this with 1 before invoking if the tunnel is not desired - - if [ "${NO_TUNNEL}" -eq 1 ]; then - echo "Getting the custom Debian without tunnel" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://astrobee.ndc.nasa.gov/software xenial main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://astrobee.ndc.nasa.gov/software xenial main\" >> $arssrc" || exit 1 - else - echo "Tunnelling to get the custom Debian" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://127.0.0.1:8765/software xenial main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://127.0.0.1:8765/software xenial main\" >> $arssrc" || exit 1 - ssh -N -L 8765:astrobee.ndc.nasa.gov:80 ${username}m.ndc.nasa.gov & - fi - - trap "kill $! 2> /dev/null; sudo truncate -s 0 $arssrc; wait $!" 0 HUP QUIT ILL ABRT FPE SEGV PIPE TERM INT - sleep 1 -fi - -sudo apt-get update || exit 1 - -if ! sudo apt-get install -m -y $pkgs; then - filter_pkgs="libroyale-dev rti-dev libsoracore-dev libmiro-dev libroyale1 rti libmiro0 libsoracore1" - for p in $filter_pkgs; do - pkgs=${pkgs//$p/} - done - sudo apt-get install -m -y $pkgs || { - echo "Couldn't install a necessary package." - exit 1 - } -fi diff --git a/scripts/setup/install_desktop_18_04_packages.sh b/scripts/setup/install_desktop_18_04_packages.sh deleted file mode 100755 index ff59883f34..0000000000 --- a/scripts/setup/install_desktop_18_04_packages.sh +++ /dev/null @@ -1,76 +0,0 @@ -#!/bin/bash -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -# Install a set of packages to build an Ubuntu 16.04 distro with -# all the development tools required for Astrobee work. -# -# The script uses the package list files for the list of -# packages to install. - -scriptdir=$(dirname "$(readlink -f "$0")") -arssrc=/etc/apt/sources.list.d/astrobee-latest.list - -pkg_files=${1:-$scriptdir/packages_base_18.lst $scriptdir/packages_desktop_18.lst} -echo $pkg_files - -pkgs='' -for i in $pkg_files; do - [ -r "$i" ] || continue - - # this is a non-portable bash-ism. - pkgs="$pkgs $(<"$i")" -done - -# if this file exists, we are using the astrobee repository -# need to set up tunnel through m (NASA users only) -if [ -e $arssrc ]; -then - username=${NDC_USERNAME:+${NDC_USERNAME}@} - - # Add these packages to the apt sources (we remove them later, so apt update succeeds) - - NO_TUNNEL=${NO_TUNNEL:-0} # Override this with 1 before invoking if the tunnel is not desired - - if [ "${NO_TUNNEL}" -eq 1 ]; then - echo "Getting the custom Debian without tunnel" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://astrobee.ndc.nasa.gov/software xenial main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://astrobee.ndc.nasa.gov/software xenial main\" >> $arssrc" || exit 1 - else - echo "Tunnelling to get the custom Debian" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://127.0.0.1:8765/software xenial main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://127.0.0.1:8765/software xenial main\" >> $arssrc" || exit 1 - ssh -N -L 8765:astrobee.ndc.nasa.gov:80 ${username}m.ndc.nasa.gov & - fi - - trap "kill $! 2> /dev/null; sudo truncate -s 0 $arssrc; wait $!" 0 HUP QUIT ILL ABRT FPE SEGV PIPE TERM INT - sleep 1 -fi - -sudo apt-get update || exit 1 - -if ! sudo apt-get install -m -y $pkgs; then - filter_pkgs="libroyale-dev rti-dev libsoracore-dev libmiro-dev libroyale1 rti libmiro0 libsoracore1" - for p in $filter_pkgs; do - pkgs=${pkgs//$p/} - done - sudo apt-get install -m -y $pkgs || { - echo "Couldn't install a necessary package." - exit 1 - } -fi diff --git a/scripts/setup/install_desktop_packages.sh b/scripts/setup/install_desktop_packages.sh new file mode 100755 index 0000000000..9b280a96a4 --- /dev/null +++ b/scripts/setup/install_desktop_packages.sh @@ -0,0 +1,85 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# Install a set of packages to build an Ubuntu 16.04 distro with +# all the development tools required for Astrobee work. +# +# The script uses the package list files for the list of +# packages to install. + +scriptdir=$(dirname "$(readlink -f "$0")") +arssrc=/etc/apt/sources.list.d/astrobee-latest.list +DIST=`cat /etc/os-release | grep -oP "(?<=VERSION_CODENAME=).*"` + +pkg_files=${1:-$scriptdir/packages_base_"${DIST}".lst $scriptdir/packages_desktop_${DIST}.lst} +echo "$pkg_files ${DIST}" + +pkgs='' +for i in $pkg_files; do + [ -r "$i" ] || continue + + # this is a non-portable bash-ism. + pkgs="$pkgs $(<"$i")" +done + +# if this file exists, we are using the astrobee repository +# need to set up tunnel through m (NASA users only) +if [ -e $arssrc ]; +then + username=${NDC_USERNAME:+${NDC_USERNAME}@} + + # Add these packages to the apt sources (we remove them later, so apt update succeeds) + + NO_TUNNEL=${NO_TUNNEL:-0} # Override this with 1 before invoking if the tunnel is not desired + + if [ "${NO_TUNNEL}" -eq 1 ]; then + echo "Getting the custom Debian without tunnel" + sudo /bin/bash -c "echo \"deb [arch=amd64] https://astrobee.ndc.nasa.gov/software_new ${DIST} main\" > $arssrc" || exit 1 + sudo /bin/bash -c "echo \"deb-src https://astrobee.ndc.nasa.gov/software_new ${DIST} main\" >> $arssrc" || exit 1 + else + echo "Tunnelling to get the custom Debian" + sudo /bin/bash -c "echo \"deb [arch=amd64] https://127.0.0.1:8765/software_new ${DIST} main\" > $arssrc" || exit 1 + sudo /bin/bash -c "echo \"deb-src https://127.0.0.1:8765/software_new ${DIST} main\" >> $arssrc" || exit 1 + ssh -N -L 127.0.0.1:8765:astrobee.ndc.nasa.gov:443 ${username}m.ndc.nasa.gov & + fi + + trap "kill $! 2> /dev/null; sudo truncate -s 0 $arssrc; wait $!" 0 HUP QUIT ILL ABRT FPE SEGV PIPE TERM INT + sleep 1 +fi + +sudo apt-get update || exit 1 + +if ! sudo apt-get install -m -y $pkgs; then + filter_pkgs="libroyale-dev rti-dev libsoracore-dev libmiro-dev libroyale1 rti libmiro0 libsoracore1" + for p in $filter_pkgs; do + pkgs=${pkgs//$p/} + done + sudo apt-get install -m -y $pkgs || { + echo "Couldn't install a necessary package." + exit 1 + } +fi + +DIST=`cat /etc/os-release | grep -oP "(?<=VERSION_CODENAME=).*"` +if [ "$DIST" == "focal" ]; then + echo "Ubuntu 20.04 Focal Fossa detected" + + echo "If 'python' interpreter is missing, point to 'python3'" + which python >/dev/null || sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 1 +fi diff --git a/scripts/setup/install_luajit.sh b/scripts/setup/install_luajit.sh deleted file mode 100755 index 96e8d25523..0000000000 --- a/scripts/setup/install_luajit.sh +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/bash -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -CURRENT_LOC=$(dirname "$(readlink -f "$0")") -echo ${CURRENT_LOC} -# cd /usr/local/lib -PACKAGE_NAME=luajit-2.0 - -if [ -d $PACKAGE_NAME ]; then - sudo rm -rf $PACKAGE_NAME -fi -sudo git clone --quiet https://luajit.org/git/luajit-2.0.git $PACKAGE_NAME 2>&1 || exit 1 -cd $PACKAGE_NAME -make && sudo make install -# cd ${CURRENT_LOC} diff --git a/scripts/setup/install_opencv.sh b/scripts/setup/install_opencv.sh deleted file mode 100755 index 18c94fa67a..0000000000 --- a/scripts/setup/install_opencv.sh +++ /dev/null @@ -1,77 +0,0 @@ -#!/bin/bash -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -# short help -usage_string="$scriptname [-h] [-p ]" -#[-t make_target] - -usage() -{ - echo "usage: sysinfo_page [[[-f file ] [-i]] | [-h]]" -} - -while [ "$1" != "" ]; do - case $1 in - -p | --path ) shift - install_path=$1 - ;; - -h | --help ) usage - exit - ;; - * ) usage - exit 1 - esac - shift -done - -echo "Installing in: "${install_path:-/usr/local} - -echo "Install the required dependencies" -sudo apt install -y build-essential cmake git pkg-config libgtk-3-dev \ - libavcodec-dev libavformat-dev libswscale-dev libv4l-dev \ - libxvidcore-dev libx264-dev libjpeg-dev libpng-dev libtiff-dev \ - gfortran openexr libatlas-base-dev python3-dev python3-numpy \ - libtbb2 libtbb-dev libdc1394-22-dev - -echo "Downloading OpenCV repo & switching to 3.3.1 branch" -mkdir ~/opencv_build && cd ~/opencv_build -git clone https://github.com/opencv/opencv.git -cd opencv && git checkout 3.3.1 && cd .. -git clone https://github.com/opencv/opencv_contrib.git -cd opencv_contrib && git checkout 3.3.1 && cd .. - -echo "Building OpenCV" -cd ~/opencv_build/opencv -mkdir build && cd build - -cmake -D CMAKE_BUILD_TYPE=RELEASE \ - -D CMAKE_INSTALL_PREFIX=${install_path:-/usr/local} \ - -D INSTALL_C_EXAMPLES=ON \ - -D INSTALL_PYTHON_EXAMPLES=ON \ - -D OPENCV_GENERATE_PKGCONFIG=ON \ - -D OPENCV_EXTRA_MODULES_PATH=~/opencv_build/opencv_contrib/modules \ - -D BUILD_EXAMPLES=ON \ - -D OPENCV_ENABLED_NONFREE=YES \ - -D ENABLE_PRECOMPILED_HEADERS=OFF .. - -make -j8 -echo "Installing OpenCV" -sudo make install -echo "Version installed:" -pkg-config --modversion opencv diff --git a/scripts/setup/packages_base.lst b/scripts/setup/packages_base.lst deleted file mode 100644 index 184ba07789..0000000000 --- a/scripts/setup/packages_base.lst +++ /dev/null @@ -1,121 +0,0 @@ -libluajit-5.1-2 -libjsoncpp1 -libprotoc9v5 -libgoogle-glog0v5 -libtinyxml2.6.2v5 -libyaml-cpp0.5v5 -libace-6.3.3 -libqt4-qt3support -libqt4-xml -libamd2.4.1 -libcamd2.4.1 -libbtf1.2.1 -libcolamd2.9.1 -libccolamd2.9.1 -libcholmod3.0.6 -libcsparse3.1.4 -libcxsparse3.1.4 -libklu1.3.3 -libblas3 -liblapack3 -libatlas3-base -libv4l-0 -libv4lconvert0 -libv4l2rds0 -libgl1-mesa-glx -libgmp10 -libusb-1.0-0 -libtinyxml2.6.2v5 -libpcl-common1.7 -libjson-c2 -libargtable2-0 -libboost-program-options1.58.0 -libboost-signals1.58.0 -libboost-system1.58.0 -libboost-chrono1.58.0 -libboost-thread1.58.0 -libboost-filesystem1.58.0 -ros-kinetic-ros-base -ros-kinetic-cpp-common -ros-kinetic-rosbag -ros-kinetic-common-msgs -ros-kinetic-nodelet -ros-kinetic-pluginlib -ros-kinetic-actionlib -ros-kinetic-image-transport -ros-kinetic-compressed-image-transport -ros-kinetic-tf2 -ros-kinetic-tf2-eigen -ros-kinetic-tf2-geometry-msgs -ros-kinetic-tf2-kdl -ros-kinetic-tf2-msgs -ros-kinetic-tf2-ros -ros-kinetic-tf2-sensor-msgs -ros-kinetic-message-filters -ros-kinetic-robot-state-publisher -ros-kinetic-rosconsole -ros-kinetic-roscpp -ros-kinetic-rosgraph -ros-kinetic-rosgraph-msgs -ros-kinetic-roslaunch -ros-kinetic-rosmaster -ros-kinetic-rosmsg -ros-kinetic-rosnode -ros-kinetic-rosout -ros-kinetic-rosparam -ros-kinetic-rosservice -ros-kinetic-rostopic -ros-kinetic-roswtf -ros-kinetic-std-srvs -ros-kinetic-topic-tools -ros-kinetic-xmlrpcpp -ros-kinetic-opencv3 -ros-kinetic-mavlink -ros-kinetic-xacro -ros-kinetic-octomap -libalvar2 -libdbow21 -libopenmvg1 -libroyale1 -libceres1 -rti -libmiro0 -libsoracore1 -libdecomputil0 -libjps3d0 -build-essential -cmake -libgoogle-glog-dev -libgtest-dev -libgflags-dev -libace-dev -libqt4-dev -protobuf-compiler -libv4l-dev -libeigen3-dev -libluajit-5.1-dev -libjsoncpp-dev -libtinyxml-dev -libyaml-cpp-dev -libusb-1.0-0-dev -libpcl-dev -libboost-program-options-dev -libboost-signals-dev -libboost-system-dev -libboost-chrono-dev -libboost-thread-dev -libboost-filesystem-dev -libsuitesparse-dev -libatlas-dev -libjson-c-dev -libargtable2-dev -libalvar-dev -libdbow2-dev -libopenmvg-dev -libroyale-dev -libceres-dev -rti-dev -libsoracore-dev -libmiro-dev -libdecomputil-dev -libjps3d-dev diff --git a/scripts/setup/packages_base_18.lst b/scripts/setup/packages_base_18.lst deleted file mode 100644 index d78a7f00bc..0000000000 --- a/scripts/setup/packages_base_18.lst +++ /dev/null @@ -1,124 +0,0 @@ -libluajit-5.1-2 -libjsoncpp1 -libprotoc10 -libgoogle-glog0v5 -libtinyxml2.6.2v5 -libyaml-cpp0.5v5 -libace-6.4.5 -libqt4-qt3support -libqt4-xml -libamd2 -libcamd2 -libbtf1 -libcolamd2 -libccolamd2 -libcholmod3 -libsuitesparse-dev -libklu1 -libblas3 -liblapack3 -libatlas3-base -libv4l-0 -libv4lconvert0 -libv4l2rds0 -libgl1-mesa-glx -libgmp10 -libusb-1.0-0 -libtinyxml2.6.2v5 -libpcl-common1.8 -libjson-c3 -libargtable2-0 -libboost-program-options1.62.0 -libboost-signals1.62.0 -libboost-system1.62.0 -libboost-chrono1.62.0 -libboost-thread1.62.0 -libboost-program-options1.62.0 -libboost-signals1.62.0 -libboost-system1.62.0 -libboost-chrono1.62.0 -libboost-thread1.62.0 -libboost-filesystem1.65.1 -ros-melodic-ros-base -ros-melodic-cpp-common -ros-melodic-rosbag -ros-melodic-common-msgs -ros-melodic-nodelet -ros-melodic-pluginlib -ros-melodic-actionlib -ros-melodic-image-transport -ros-melodic-compressed-image-transport -ros-melodic-tf2 -ros-melodic-tf2-eigen -ros-melodic-tf2-geometry-msgs -ros-melodic-tf2-kdl -ros-melodic-tf2-msgs -ros-melodic-tf2-ros -ros-melodic-tf2-sensor-msgs -ros-melodic-message-filters -ros-melodic-robot-state-publisher -ros-melodic-rosconsole -ros-melodic-roscpp -ros-melodic-rosgraph -ros-melodic-rosgraph-msgs -ros-melodic-roslaunch -ros-melodic-rosmaster -ros-melodic-rosmsg -ros-melodic-rosnode -ros-melodic-rosout -ros-melodic-rosparam -ros-melodic-rosservice -ros-melodic-rostopic -ros-melodic-roswtf -ros-melodic-std-srvs -ros-melodic-topic-tools -ros-melodic-xmlrpcpp -ros-melodic-mavlink -ros-melodic-xacro -ros-melodic-octomap -libalvar2 -libdbow21 -libopenmvg1 -libroyale1 -libceres1 -rti -libmiro0 -libsoracore1 -libdecomputil0 -libjps3d0 -build-essential -cmake -libgoogle-glog-dev -libgtest-dev -libgflags-dev -libace-dev -libqt4-dev -protobuf-compiler -libv4l-dev -libeigen3-dev -libluajit-5.1-dev -libjsoncpp-dev -libtinyxml-dev -libyaml-cpp-dev -libusb-1.0-0-dev -libpcl-dev -libboost-program-options-dev -libboost-signals-dev -libboost-system-dev -libboost-chrono-dev -libboost-thread-dev -libboost-filesystem-dev -libsuitesparse-dev -libatlas-base-dev -libjson-c-dev -libargtable2-dev -libalvar-dev -libdbow2-dev -libopenmvg-dev -libroyale-dev -libceres-dev -rti-dev -libsoracore-dev -libmiro-dev -libdecomputil-dev -libjps3d-dev diff --git a/scripts/setup/packages_base_focal.lst b/scripts/setup/packages_base_focal.lst new file mode 100644 index 0000000000..23db0b4b51 --- /dev/null +++ b/scripts/setup/packages_base_focal.lst @@ -0,0 +1,133 @@ +libjsoncpp1 +libprotoc17 +libgoogle-glog0v5 +libtinyxml2.6.2v5 +libyaml-cpp0.6 +libace-6.4.5 +libamd2 +libcamd2 +libbtf1 +libcolamd2 +libccolamd2 +libcholmod3 +libklu1 +libblas3 +liblapack3 +libatlas3-base +libv4l-0 +libv4lconvert0 +libv4l2rds0 +libgl1-mesa-glx +libgmp10 +libusb-1.0-0 +libpcl-common1.10 +libpcl-features1.10 +libpcl-filters1.10 +libpcl-kdtree1.10 +libpcl-keypoints1.10 +libpcl-octree1.10 +libpcl-registration1.10 +libpcl-sample-consensus1.10 +libpcl-search1.10 +libjson-c4 +libargtable2-0 +libvtk7.1p +libboost-program-options1.71.0 +libboost-signals1.67.0 +libboost-system1.71.0 +libboost-chrono1.71.0 +libboost-thread1.71.0 +libboost-filesystem1.71.0 +ros-noetic-ros-base +ros-noetic-cpp-common +ros-noetic-rosbag +ros-noetic-common-msgs +ros-noetic-nodelet +ros-noetic-pluginlib +ros-noetic-actionlib +ros-noetic-image-transport +ros-noetic-compressed-image-transport +ros-noetic-tf2 +ros-noetic-tf2-eigen +ros-noetic-tf2-geometry-msgs +ros-noetic-tf2-kdl +ros-noetic-tf2-msgs +ros-noetic-tf2-ros +ros-noetic-tf2-sensor-msgs +ros-noetic-message-filters +ros-noetic-robot-state-publisher +ros-noetic-rosconsole +ros-noetic-roscpp +ros-noetic-rosgraph +ros-noetic-rosgraph-msgs +ros-noetic-roslaunch +ros-noetic-rosmaster +ros-noetic-rosmsg +ros-noetic-rosnode +ros-noetic-rosout +ros-noetic-rosparam +ros-noetic-rosservice +ros-noetic-rostopic +ros-noetic-roswtf +ros-noetic-std-srvs +ros-noetic-topic-tools +ros-noetic-xmlrpcpp +ros-noetic-mavlink +ros-noetic-xacro +ros-noetic-octomap +ros-noetic-catkin +ros-noetic-pcl-conversions +ros-noetic-ar-track-alvar +libdbow21 +libgtsam +libopenmvg1 +libroyale1 +libceres1 +rti +libmiro0 +libsoracore1 +libdecomputil0 +libjps3d0 +libopencv-core4.2 +libopencv-calib3d4.2 +libopencv-highgui4.2 +libopencv-imgcodecs4.2 +libopencv-imgproc4.2 +libopencv-features2d4.2 +libopencv-xfeatures2d +build-essential +cmake +libgoogle-glog-dev +libgtest-dev +libgflags-dev +libace-dev +protobuf-compiler +libv4l-dev +libeigen3-dev +libluajit-5.1-dev +libjsoncpp-dev +libtinyxml-dev +libyaml-cpp-dev +libusb-1.0-0-dev +libvtk7-dev +libpcl-dev +libboost-program-options-dev +libboost-system-dev +libboost-chrono-dev +libboost-thread-dev +libboost-filesystem-dev +libsuitesparse-dev +libatlas-base-dev +libjson-c-dev +libargtable2-dev +libdbow2-dev +libgtsam-dev +libopenmvg-dev +libroyale-dev +libceres-dev +rti-dev +libsoracore-dev +libmiro-dev +libdecomputil-dev +libjps3d-dev +python3-catkin-tools diff --git a/scripts/setup/packages_desktop.lst b/scripts/setup/packages_desktop.lst deleted file mode 100644 index ac087e91c9..0000000000 --- a/scripts/setup/packages_desktop.lst +++ /dev/null @@ -1,10 +0,0 @@ -ros-kinetic-rviz -gazebo7 -gazebo7-common -ros-kinetic-gazebo-ros-pkgs -libgazebo7-dev -python-pyqtgraph -doxygen -zlib1g-dev -libjson0-dev -libusb-1.0-0-dev diff --git a/scripts/setup/packages_desktop_18.lst b/scripts/setup/packages_desktop_18.lst deleted file mode 100644 index e66e540706..0000000000 --- a/scripts/setup/packages_desktop_18.lst +++ /dev/null @@ -1,9 +0,0 @@ -ros-melodic-rviz -gazebo9 -gazebo9-common -ros-melodic-gazebo-ros-pkgs -libgazebo9-dev -python-pyqtgraph -doxygen -zlib1g-dev -libusb-1.0-0-dev \ No newline at end of file diff --git a/scripts/setup/packages_desktop_focal.lst b/scripts/setup/packages_desktop_focal.lst new file mode 100644 index 0000000000..beb101f105 --- /dev/null +++ b/scripts/setup/packages_desktop_focal.lst @@ -0,0 +1,10 @@ +ros-noetic-rviz +ros-noetic-gazebo-dev +ros-noetic-gazebo-plugins +ros-noetic-gazebo-ros +ros-noetic-gazebo-ros-pkgs +doxygen +zlib1g-dev +libusb-1.0-0-dev +clang-format-8 +python3-rosdep diff --git a/scripts/setup/utils_16_04.conf b/scripts/setup/utils_16_04.conf deleted file mode 100644 index bcac73d5a4..0000000000 --- a/scripts/setup/utils_16_04.conf +++ /dev/null @@ -1,15 +0,0 @@ -# -# Fake multistrap config to feed apt-get install -# -# just to list some packages to install using the same parsing syntax -# used to parse the real platform multistrap conf files. -# (this allow to put some comments in the file) - -# editing tools -packages=vim emacs24 - -# we need java for plantuml -packages=openjdk-8-jdk-headless - -# documentation tools -packages=graphviz plantuml doxygen diff --git a/scripts/teleop/load_nodelet.sh b/scripts/teleop/load_nodelet.sh new file mode 100755 index 0000000000..5c4c69fb28 --- /dev/null +++ b/scripts/teleop/load_nodelet.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +name=$1 +nodelet_type=$2 +manager_name=$3 +bond_id=$4 + +if [[ $# -eq 1 ]]; then + rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "loadNodelet", subsys_name: "Astrobee", args: [{data_type: 5, s: '"$name"'}]}' +elif [[ $# -eq 2 ]]; then + rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "loadNodelet", subsys_name: "Astrobee", args: [{data_type: 5, s: '"$name"'}, {data_type: 5, s: '"$nodelet_type"'}]}' +elif [[ $# -eq 3 ]]; then + rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "loadNodelet", subsys_name: "Astrobee", args: [{data_type: 5, s: '"$name"'}, {data_type: 5, s: '"$nodelet_type"'}, {data_type: 5, s: '"$manager_name"'}]}' +elif [[ $# -eq 4 ]]; then + rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "loadNodelet", subsys_name: "Astrobee", args: [{data_type: 5, s: '"$name"'}, {data_type: 5, s: '"$nodelet_type"'}, {data_type: 5, s: '"$manager_name"'}, {data_type: 5, s: '"$bond_id"'}]}' +fi diff --git a/scripts/teleop/run_estimate_bias.sh b/scripts/teleop/run_estimate_bias.sh new file mode 100755 index 0000000000..632a3ba07d --- /dev/null +++ b/scripts/teleop/run_estimate_bias.sh @@ -0,0 +1,20 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +rosservice call /gnc/ekf/init_bias diff --git a/scripts/teleop/run_reset_localization.sh b/scripts/teleop/run_reset_localization.sh new file mode 100755 index 0000000000..9ecaf7c917 --- /dev/null +++ b/scripts/teleop/run_reset_localization.sh @@ -0,0 +1,20 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +rosservice call /gnc/ekf/reset diff --git a/scripts/teleop/set_data_to_disk.sh b/scripts/teleop/set_data_to_disk.sh new file mode 100755 index 0000000000..36b6a8ffe9 --- /dev/null +++ b/scripts/teleop/set_data_to_disk.sh @@ -0,0 +1,21 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +rosrun executive data_to_disk_pub "$1" +echo "All done" diff --git a/scripts/teleop/unload_nodelet.sh b/scripts/teleop/unload_nodelet.sh new file mode 100755 index 0000000000..a5afff9c16 --- /dev/null +++ b/scripts/teleop/unload_nodelet.sh @@ -0,0 +1,27 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +name=$1 +manager_name=$2 + +if [[ $# -eq 1 ]]; then + rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "unloadNodelet", subsys_name: "Astrobee", args: [{data_type: 5, s: '"$name"'}]}' +elif [[ $# -eq 2 ]]; then + rostopic pub --once /command ff_msgs/CommandStamped '{cmd_name: "unloadNodelet", subsys_name: "Astrobee", args: [{data_type: 5, s: '"$name"'}, {data_type: 5, s: '"$manager_name"'}]}' +fi diff --git a/shared/CMakeLists.txt b/shared/CMakeLists.txt deleted file mode 100644 index 5b256d3f05..0000000000 --- a/shared/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -add_subdirectory(ff_common) -add_subdirectory(config_reader) -add_subdirectory(jsonloader) -if(USE_ROS) - add_subdirectory(ff_util) - add_subdirectory(msg_conversions) -endif(USE_ROS) diff --git a/shared/config_reader/CMakeLists.txt b/shared/config_reader/CMakeLists.txt index cc2f9d9fee..972a2b8e41 100644 --- a/shared/config_reader/CMakeLists.txt +++ b/shared/config_reader/CMakeLists.txt @@ -15,21 +15,63 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(config_reader) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + ff_common +) + +## System dependencies are found with CMake's conventions +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(Luajit20 REQUIRED) + catkin_package( - INCLUDE_DIRS include ${GFLAGS_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${LUAJIT_INCLUDE_DIR} - LIBRARIES config_reader - CATKIN_DEPENDS ff_common + INCLUDE_DIRS include ${LUAJIT_INCLUDE_DIR} + LIBRARIES config_reader ${LUAJIT_LIBRARY} ${LUAJIT_MATH_LIBRARY} + CATKIN_DEPENDS roscpp nodelet ff_common ) +########### +## Build ## +########### + +# Specify additional locations of header files +# Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} + ${LUAJIT_INCLUDE_DIR} +) + +# Declare C++ libraries +add_library(config_reader + src/config_reader.cc + src/watch_files.cc +) +add_dependencies(config_reader ${catkin_EXPORTED_TARGETS}) +target_link_libraries(config_reader gflags glog ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) -create_library(TARGET config_reader - INC ${GFLAGS_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${LUAJIT_INCLUDE_DIR} ff_common - ${CMAKE_CURRENT_BINARY_DIR}/include - LIBS ${GFLAGS_LIBRARIES} ${GLOG_LIBRARIES} ${LUAJIT_LIBRARIES} ff_common +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE ) diff --git a/shared/config_reader/include/config_reader/config_reader.h b/shared/config_reader/include/config_reader/config_reader.h index dd95cdf595..bb3ec748c6 100644 --- a/shared/config_reader/include/config_reader/config_reader.h +++ b/shared/config_reader/include/config_reader/config_reader.h @@ -88,6 +88,7 @@ class ConfigReader { bool GetReal(int index, float *val, float min, float max); bool GetReal(const char *exp, double *val, double min, double max); bool GetReal(int index, double *val, double min, double max); + bool IsNumber(const char *exp); ConfigReader *Config() { return config_; } private: @@ -164,7 +165,7 @@ class ConfigReader { bool ReadInt(const char *exp, int *val, int min, int max); bool ReadReal(const char *exp, float *val, float min, float max); bool ReadReal(const char *exp, double *val, double min, double max); - + bool IsNumber(); std::vector files_; // list of config files to load lua_State * l_; // lua interpreter (valid between readFiles() and clear()) diff --git a/shared/config_reader/src/config_reader.cc b/shared/config_reader/src/config_reader.cc index f02e4e44db..6267dad98e 100644 --- a/shared/config_reader/src/config_reader.cc +++ b/shared/config_reader/src/config_reader.cc @@ -449,6 +449,18 @@ bool ConfigReader::Table::GetReal(int index, double *val, double min, (full_exp_ + '[' + std::to_string(index) + ']').c_str(), val, min, max); } +bool ConfigReader::Table::IsNumber(const char *exp) { + if (!IsInit()) { + return false; + } + + if (!config_->PutObjectOnLuaStack(exp, ref_)) { + return false; + } + + return config_->IsNumber(); +} + //====================================================================// ConfigReader::FileHeader::FileHeader(const FileHeader &fh) { filename_ = fh.filename_; @@ -689,13 +701,6 @@ bool ConfigReader::InitLua() { // load libraries luaL_openlibs(l_); - // report error if lua was unable to open the libraries - if (!lua_isnil(l_, -1)) { - LOG(ERROR) << "ConfigReader: Unable to load libraries!! This is very bad!"; - CloseLua(); - return false; - } - // discard any results from initialization lua_settop(l_, 0); @@ -1103,4 +1108,10 @@ bool ConfigReader::ReadReal(const char *exp, double *val, double min, return true; } +bool ConfigReader::IsNumber() { + // The function PutObjectOnLuaStack() must have been called before + // this, which would populate l_. + return lua_isnumber(l_, -1); +} + } // namespace config_reader diff --git a/shared/ff_common/CMakeLists.txt b/shared/ff_common/CMakeLists.txt index ee6d0ca378..32a5fa9ea2 100644 --- a/shared/ff_common/CMakeLists.txt +++ b/shared/ff_common/CMakeLists.txt @@ -15,24 +15,77 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(ff_common) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + std_msgs + nodelet +) + +find_package(Eigen3 REQUIRED) + catkin_package( INCLUDE_DIRS include LIBRARIES ff_common - CATKIN_DEPENDS + CATKIN_DEPENDS roscpp std_msgs nodelet ) +########### +## Build ## +########### + +# Specify additional locations of header files +# Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +# Declare C++ libraries +add_library(ff_common + src/init.cc + src/thread.cc + src/utils.cc ) +add_dependencies(ff_common ${catkin_EXPORTED_TARGETS}) +target_link_libraries(ff_common gflags glog ${catkin_LIBRARIES} ) -create_library(TARGET ff_common - INC ${GFLAGS_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} - ${CMAKE_CURRENT_BINARY_DIR}/include - LIBS ${GFLAGS_LIBRARIES} ${GLOG_LIBRARIES}) -create_test_targets(DIR test - LIBS ff_common - DEPS ff_common) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_rostest_gtest(test_thread + test/test_thread.test + test/test_thread.cc + ) + target_link_libraries(test_thread + ff_common + ) + +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/sparse_mapping/include/sparse_mapping/eigen_vectors.h b/shared/ff_common/include/ff_common/eigen_vectors.h similarity index 87% rename from localization/sparse_mapping/include/sparse_mapping/eigen_vectors.h rename to shared/ff_common/include/ff_common/eigen_vectors.h index cd5e43a11f..7e0ce3b0b4 100644 --- a/localization/sparse_mapping/include/sparse_mapping/eigen_vectors.h +++ b/shared/ff_common/include/ff_common/eigen_vectors.h @@ -1,14 +1,14 @@ /* Copyright (c) 2017, United States Government, as represented by the * Administrator of the National Aeronautics and Space Administration. - * + * * All rights reserved. - * + * * The Astrobee platform is licensed under the Apache License, Version 2.0 * (the "License"); you may not use this file except in compliance with the * License. You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -21,8 +21,8 @@ * It should be included in any file that uses vectors of Eigen objects. **/ -#ifndef SPARSE_MAPPING_EIGEN_VECTORS_H_ -#define SPARSE_MAPPING_EIGEN_VECTORS_H_ +#ifndef FF_COMMON_EIGEN_VECTORS_H_ +#define FF_COMMON_EIGEN_VECTORS_H_ #include #include @@ -30,7 +30,7 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Affine3d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Isometry3d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2Xd) -#endif // SPARSE_MAPPING_EIGEN_VECTORS_H_ - +#endif // FF_COMMON_EIGEN_VECTORS_H_ diff --git a/shared/ff_util/include/ff_util/ff_names.h b/shared/ff_common/include/ff_common/ff_names.h similarity index 84% rename from shared/ff_util/include/ff_util/ff_names.h rename to shared/ff_common/include/ff_common/ff_names.h index 4e15de5906..3bb69a8979 100644 --- a/shared/ff_util/include/ff_util/ff_names.h +++ b/shared/ff_common/include/ff_common/ff_names.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef FF_UTIL_FF_NAMES_H_ -#define FF_UTIL_FF_NAMES_H_ +#ifndef FF_COMMON_FF_NAMES_H_ +#define FF_COMMON_FF_NAMES_H_ // UNIVERSAL /////////////////////////////////////////////////////////////////// @@ -95,7 +95,9 @@ #define SUBSYSTEM_COMMUNICATIONS "comm" #define NODE_DDS_ROS_BRIDGE "dds_ros_bridge" +#define NODE_ASTROBEE_ASTROBEE_BRIDGE "astrobee_astrobee_bridge" +#define SERVICE_COMMUNICATIONS_ENABLE_ASTROBEE_INTERCOMMS "comm/astrobee_astrobee_bridge/enable" #define SERVICE_COMMUNICATIONS_DDS_SET_TELEM_RATES "comm/dds/set_telem_rate" #define TOPIC_COMMUNICATIONS_DDS_COMMAND "comm/dds/command" @@ -113,8 +115,20 @@ #define NODE_CTL "ctl" #define NODE_PERCH_CTL "perch_ctl" #define NODE_EKF "ekf" +#define NODE_GRAPH_LOC "graph_loc" +#define NODE_GRAPH_VIO "graph_vio" +#define NODE_POSE_EXTR "pose_extr" +#define NODE_SIM_LOC "sim_loc" #define NODE_FAM "fam" -#define NODE_SIM_WRAPPER "sim_wrapper" + +#define TOPIC_GRAPH_LOC "graph_loc/graph" +#define TOPIC_GRAPH_VIO "graph_vio/graph" +#define TOPIC_GRAPH_LOC_STATE "graph_loc/state" +#define TOPIC_GRAPH_VIO_STATE "graph_vio/state" +#define TOPIC_AR_TAG_POSE "ar_tag/pose" +#define TOPIC_SPARSE_MAPPING_POSE "sparse_mapping/pose" +#define TOPIC_IMU_BIAS_EXTRAPOLATOR_POSE "imu_bias_extrapolator/pose" +#define TOPIC_IMU_BIAS_EXTRAPOLATOR_VELOCITY "imu_bias_extrapolator/velocity" #define ACTION_GNC_CTL_CONTROL "gnc/control" @@ -131,6 +145,7 @@ #define SERVICE_GNC_EKF_RESET "gnc/ekf/reset" #define SERVICE_GNC_EKF_RESET_HR "gnc/ekf/reset_hr" #define SERVICE_GNC_EKF_INIT_BIAS "gnc/ekf/init_bias" +#define SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE "gnc/ekf/init_bias_from_file" #define SERVICE_GNC_EKF_SET_INPUT "gnc/ekf/set_input" #define SERVICE_GNC_CTL_ENABLE "gnc/ctl/enable" @@ -165,6 +180,7 @@ #define TOPIC_MANAGEMENT_ACCESS_CONTROL_STATE "mgt/access_control/state" #define TOPIC_MANAGEMENT_CPU_MONITOR_STATE "mgt/cpu_monitor/state" #define TOPIC_MANAGEMENT_DISK_MONITOR_STATE "mgt/disk_monitor/state" +#define TOPIC_MANAGEMENT_MEM_MONITOR_STATE "mgt/mem_monitor/state" #define TOPIC_MANAGEMENT_EXEC_AGENT_STATE "mgt/executive/agent_state" #define TOPIC_MANAGEMENT_EXEC_CF_ACK "mgt/executive/cf_ack" #define TOPIC_MANAGEMENT_EXEC_COMMAND "mgt/executive/command" @@ -173,7 +189,7 @@ #define TOPIC_MANAGEMENT_SYS_MONITOR_CONFIG "mgt/sys_monitor/config" #define TOPIC_MANAGEMENT_SYS_MONITOR_STATE "mgt/sys_monitor/state" #define TOPIC_MANAGEMENT_SYS_MONITOR_HEARTBEAT "mgt/sys_monitor/heartbeat" -#define TOPIC_MANAGEMENT_SYS_MONITOR_TIME_DIFF "mgt/sys_monitor/time_diff" +#define TOPIC_MANAGEMENT_SYS_MONITOR_TIME_SYNC "mgt/sys_monitor/time_sync" #define TOPIC_MANAGEMENT_DATA_BAGGER_STATE "mgt/data_bagger/state" #define TOPIC_MANAGEMENT_DATA_BAGGER_TOPICS "mgt/data_bagger/topics" #define TOPIC_MANAGEMENT_CAMERA_STATE "mgt/camera_state" @@ -188,14 +204,14 @@ #define SERVICE_MANAGEMENT_DATA_BAGGER_ENABLE_RECORDING "mgt/data_bagger/enable_recording" #define SERVICE_MANAGEMENT_DATA_BAGGER_SET_DATA_TO_DISK "mgt/data_bagger/set_data_to_disk" -#define SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_NAV "mgt/img_sampler/nav_cam/configure" -#define SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_DOCK "mgt/img_sampler/dock_cam/configure" -#define SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_HAZ "mgt/img_sampler/haz_cam/configure" -#define SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_PERCH "mgt/img_sampler/perch_cam/configure" -#define SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_NAV "mgt/img_sampler/nav_cam/enable" -#define SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_DOCK "mgt/img_sampler/dock_cam/enable" -#define SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_HAZ "mgt/img_sampler/haz_cam/enable" -#define SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_PERCH "mgt/img_sampler/perch_cam/enable" +#define SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_NAV "mgt/img_sampler/nav_cam/configure" +#define SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_DOCK "mgt/img_sampler/dock_cam/configure" +#define SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_HAZ "mgt/img_sampler/haz_cam/configure" +#define SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_PERCH "mgt/img_sampler/perch_cam/configure" +#define SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_NAV "mgt/img_sampler/nav_cam/enable" +#define SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_DOCK "mgt/img_sampler/dock_cam/enable" +#define SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_HAZ "mgt/img_sampler/haz_cam/enable" +#define SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_PERCH "mgt/img_sampler/perch_cam/enable" #define SERVICE_MANAGEMENT_SYS_MONITOR_UNLOAD_LOAD_NODELET "mgt/sys_monitor/unload_load_nodelet" #define SERVICE_MANAGEMENT_SCI_CAM_CONFIG "configure_sci_cam" #define SERVICE_MANAGEMENT_SCI_CAM_ENABLE "enable_sci_cam" @@ -217,10 +233,14 @@ #define SERVICE_MOBILITY_SET_INERTIA "mob/set_inertia" #define SERVICE_MOBILITY_SET_ZONES "mob/set_zones" #define SERVICE_MOBILITY_GET_ZONES "mob/get_zones" +#define SERVICE_MOBILITY_GET_ZONES_MAP "mob/get_zones_map" #define SERVICE_MOBILITY_SET_STATE "mob/set_state" -#define SERVICE_MOBILITY_UPDATE_MAP_RESOLUTION "mob/mapper/update_resolution" -#define SERVICE_MOBILITY_UPDATE_MEMORY_TIME "mob/mapper/update_memory_time" -#define SERVICE_MOBILITY_UPDATE_INFLATION "mob/mapper/update_inflation_radius" +#define SERVICE_MOBILITY_SET_MAP_RESOLUTION "mob/mapper/set_map_resolution" +#define SERVICE_MOBILITY_SET_MEMORY_TIME "mob/mapper/set_memory_time" +#define SERVICE_MOBILITY_SET_COLLISION_DISTANCE "mob/mapper/set_collision_distance" +#define SERVICE_MOBILITY_GET_MAP_RESOLUTION "mob/mapper/get_map_resolution" +#define SERVICE_MOBILITY_GET_MEMORY_TIME "mob/mapper/get_memory_time" +#define SERVICE_MOBILITY_GET_MAP_INFLATION "mob/mapper/get_map_inflation" #define SERVICE_MOBILITY_RESET_MAP "mob/mapper/reset_map" #define SERVICE_MOBILITY_GET_FREE_MAP "mob/mapper/get_free_map" #define SERVICE_MOBILITY_GET_OBSTACLE_MAP "mob/mapper/get_obstacle_map" @@ -259,12 +279,14 @@ #define SUBSYSTEM_LOCALIZATION "loc" #define NODE_HANDRAIL_DETECT "handrail_detect" +#define NODE_DEPTH_ODOM "depth_odom" #define NODE_OPTICAL_FLOW "of" #define NODE_LOCALIZATION_MANAGER "localization_manager" #define NODE_AR_TAGS "ar_tags" #define NODE_MAPPED_LANDMARKS "ml" #define NODE_VIVE_LOCALIZATION "vive_localization" +#define TOPIC_VIO_POSE "vio/pose" #define TOPIC_LOCALIZATION_POSE "loc/pose" #define TOPIC_LOCALIZATION_TWIST "loc/twist" #define TOPIC_LOCALIZATION_TRUTH "loc/truth/pose" @@ -282,14 +304,18 @@ #define TOPIC_LOCALIZATION_HR_IMAGE "loc/hr/image" #define TOPIC_LOCALIZATION_HR_CLOUD "loc/hr/cloud" #define TOPIC_LOCALIZATION_HR_MARKER "loc/hr/marker" +#define TOPIC_LOCALIZATION_DEPTH_ODOM "loc/depth/odom" #define TOPIC_LOCALIZATION_OF_DEBUG "loc/of/debug_img" #define SERVICE_LOCALIZATION_OF_ENABLE "loc/of/enable" +#define SERVICE_LOCALIZATION_DO_ENABLE "loc/do/enable" #define SERVICE_LOCALIZATION_AR_ENABLE "loc/ar/enable" #define SERVICE_LOCALIZATION_ML_ENABLE "loc/ml/enable" #define SERVICE_LOCALIZATION_HR_ENABLE "loc/hr/enable" #define SERVICE_LOCALIZATION_PL_ENABLE "loc/pl/enable" +#define SERVICE_LOCALIZATION_RESET_MAP "loc/ml/reset_map" +#define SERVICE_LOCALIZATION_RESET_MAP_LOC "loc/ml/reset_map_loc" // Localization manager @@ -297,6 +323,7 @@ #define ACTION_LOCALIZATION_MANAGER_LOCALIZATION "loc/manager/localization" #define SERVICE_LOCALIZATION_MANAGER_SET_STATE "loc/manager/set_state" #define SERVICE_LOCALIZATION_MANAGER_GET_PIPELINES "loc/manager/get_pipelines" +#define SERVICE_LOCALIZATION_MANAGER_GET_CURR_PIPELINE "loc/manager/get_curr_pipeline" // Poses and camera info as output by the simulator #define TOPIC_NAV_CAM_SIM_POSE "sim/nav_cam/pose" @@ -359,6 +386,9 @@ #define TOPIC_HARDWARE_IMU "hw/imu" #define TOPIC_HARDWARE_NAV_CAM "hw/cam_nav" #define TOPIC_HARDWARE_DOCK_CAM "hw/cam_dock" +#define TOPIC_HARDWARE_CAM_SUFFIX_BAYER_RAW "_bayer" +#define TOPIC_HARDWARE_CAM_SUFFIX_BAYER_COLOR "_color" +#define TOPIC_HARDWARE_CAM_INFO "/camera_info" #define TOPIC_HARDWARE_SCI_CAM "hw/cam_sci" #define TOPIC_HARDWARE_LIGHT_FRONT "hw/light_front" #define TOPIC_HARDWARE_LIGHT_AFT "hw/light_aft" @@ -380,6 +410,7 @@ #define TOPIC_HARDWARE_PICOFLEXX_SUFFIX "/points" #define TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED "/extended" #define TOPIC_HARDWARE_PICOFLEXX_SUFFIX_DEPTH_IMAGE "/depth_image" +#define TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE "/amplitude_int" #define TOPIC_HARDWARE_NAME_HAZ_CAM "haz" #define TOPIC_HARDWARE_NAME_PERCH_CAM "perch" #define TOPIC_HARDWARE_SPEED_CAM_CAMERA_IMAGE "hw/speed_cam/camera_image" @@ -409,17 +440,20 @@ #define SERVICE_HARDWARE_EPS_GET_BOARD_INFO "hw/eps/get_board_info" #define SERVICE_HARDWARE_EPS_CLEAR_TERMINATE "hw/eps/clear_terminate" +#define SERVICE_HARDWARE_PERCHING_ARM_ENABLE "hw/arm/enable_arm" #define SERVICE_HARDWARE_PERCHING_ARM_DIST_VEL "hw/arm/set_dist_vel" #define SERVICE_HARDWARE_PERCHING_ARM_PROX_VEL "hw/arm/set_prox_vel" #define SERVICE_HARDWARE_PERCHING_ARM_PROX_SERVO "hw/arm/enable_proximal_servo" #define SERVICE_HARDWARE_PERCHING_ARM_DIST_SERVO "hw/arm/enable_distal_servo" #define SERVICE_HARDWARE_PERCHING_ARM_GRIP_SERVO "hw/arm/enable_gripper_servo" #define SERVICE_HARDWARE_PERCHING_ARM_CALIBRATE "hw/arm/calibrate_gripper" -#define SERVICE_HARDWARE_LIGHT_FRONT_CONTROL "hw/light_front/control" -#define SERVICE_HARDWARE_LIGHT_AFT_CONTROL "hw/light_aft/control" +#define SERVICE_HARDWARE_LIGHT_FRONT_CONTROL "hw/flashlight_front/control" +#define SERVICE_HARDWARE_LIGHT_AFT_CONTROL "hw/flashlight_aft/control" #define SERVICE_HARDWARE_LASER_ENABLE "hw/laser/enable" #define SERVICE_HARDWARE_PMC_ENABLE "hw/pmc/enable" +#define SERVICE_HARDWARE_PMC_TIMEOUT "hw/pmc/set_timeout" #define SERVICE_STREAMING_LIGHTS "hw/signal_lights/streaming" +#define SERVICE_SET_EXPOSURE "/set_exposure" -#endif // FF_UTIL_FF_NAMES_H_ +#endif // FF_COMMON_FF_NAMES_H_ diff --git a/shared/ff_common/package.xml b/shared/ff_common/package.xml index c14b84ddeb..388d39df21 100644 --- a/shared/ff_common/package.xml +++ b/shared/ff_common/package.xml @@ -15,10 +15,10 @@ catkin roscpp - roslib + std_msgs nodelet roscpp - roslib + std_msgs nodelet diff --git a/shared/ff_common/src/init.cc b/shared/ff_common/src/init.cc index f74dec2b13..5a839a2b05 100644 --- a/shared/ff_common/src/init.cc +++ b/shared/ff_common/src/init.cc @@ -40,7 +40,7 @@ namespace ff_common { google::InitGoogleLogging((*argv)[0]); logging_initialized = true; } - FREEFLYER_GFLAGS_NAMESPACE::ParseCommandLineFlags(argc, argv, true); + google::ParseCommandLineFlags(argc, argv, true); } // ros nodelets get commandline arguments in this form... @@ -57,7 +57,7 @@ namespace ff_common { if (main_thread) { InitFreeFlyerApplication(&argc, &casted_argv); } else { - FREEFLYER_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &casted_argv, true); + google::ParseCommandLineFlags(&argc, &casted_argv, true); } free(argv); } diff --git a/shared/ff_common/test/test_thread.cc b/shared/ff_common/test/test_thread.cc new file mode 100644 index 0000000000..bd6ed85ae6 --- /dev/null +++ b/shared/ff_common/test/test_thread.cc @@ -0,0 +1,82 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Test thread +// Creates a thread pool and adds multiple tasks testing the +// ff_common::ThreadPool class + +#include + +#include + +#include + +void Simple(int a, int b) { + EXPECT_EQ(2, a); + EXPECT_EQ(3, b); +} + +void SimpleVecPass(std::vector const& vec1, + std::vector const& vec2) { + EXPECT_EQ(&vec1, &vec2); +} + +void SimpleVecFail(std::vector const& vec1, + std::vector const& vec2) { + EXPECT_NE(&vec1, &vec2); +} + +void AppendToVec(std::vector * vec) { + vec->push_back(3); +} + +TEST(thread, thread_pool) { + FLAGS_num_threads = 1; + ff_common::ThreadPool pool; + + // Verify we can add tasks contain lvalues and rvalues. + pool.AddTask(Simple, 2, 3); + pool.AddTask(Simple, 2, 3); + int lvalue = 2; + pool.AddTask(Simple, lvalue, 3); + pool.Join(); + + // Verify we can send complex types without copy + std::vector vec(3, 1); + pool.AddTask(SimpleVecPass, std::ref(vec), std::ref(vec)); + pool.AddTask(SimpleVecPass, std::ref(vec), std::ref(vec)); + pool.AddTask(SimpleVecPass, std::ref(vec), std::ref(vec)); + pool.Join(); + + // Verify/Show that not using std ref causes a copy to happen + pool.AddTask(SimpleVecFail, std::ref(vec), vec); + pool.AddTask(SimpleVecFail, vec, std::ref(vec)); + pool.AddTask(SimpleVecFail, vec, vec); + pool.Join(); + + // Verify we can send pointers + pool.AddTask(AppendToVec, &vec); + pool.Join(); + EXPECT_EQ(4u, vec.size()); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/shared/ff_common/test/test_thread.cxx b/shared/ff_common/test/test_thread.cxx deleted file mode 100644 index c9ee9ae2c9..0000000000 --- a/shared/ff_common/test/test_thread.cxx +++ /dev/null @@ -1,72 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include - -#include - -void Simple(int a, int b) { - EXPECT_EQ(2, a); - EXPECT_EQ(3, b); -} - -void SimpleVecPass(std::vector const& vec1, - std::vector const& vec2) { - EXPECT_EQ(&vec1, &vec2); -} - -void SimpleVecFail(std::vector const& vec1, - std::vector const& vec2) { - EXPECT_NE(&vec1, &vec2); -} - -void AppendToVec(std::vector * vec) { - vec->push_back(3); -} - -TEST(thread, thread_pool) { - FLAGS_num_threads = 1; - ff_common::ThreadPool pool; - - // Verify we can add tasks contain lvalues and rvalues. - pool.AddTask(Simple, 2, 3); - pool.AddTask(Simple, 2, 3); - int lvalue = 2; - pool.AddTask(Simple, lvalue, 3); - pool.Join(); - - // Verify we can send complex types without copy - std::vector vec(3, 1); - pool.AddTask(SimpleVecPass, std::ref(vec), std::ref(vec)); - pool.AddTask(SimpleVecPass, std::ref(vec), std::ref(vec)); - pool.AddTask(SimpleVecPass, std::ref(vec), std::ref(vec)); - pool.Join(); - - // Verify/Show that not using std ref causes a copy to happen - pool.AddTask(SimpleVecFail, std::ref(vec), vec); - pool.AddTask(SimpleVecFail, vec, std::ref(vec)); - pool.AddTask(SimpleVecFail, vec, vec); - pool.Join(); - - // Verify we can send pointers - pool.AddTask(AppendToVec, &vec); - pool.Join(); - EXPECT_EQ(4u, vec.size()); -} diff --git a/shared/ff_common/test/test_thread.test b/shared/ff_common/test/test_thread.test new file mode 100644 index 0000000000..77dd27e90b --- /dev/null +++ b/shared/ff_common/test/test_thread.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/shared/ff_util/CMakeLists.txt b/shared/ff_util/CMakeLists.txt index 6d2942752e..cc1a72d969 100644 --- a/shared/ff_util/CMakeLists.txt +++ b/shared/ff_util/CMakeLists.txt @@ -15,46 +15,80 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(ff_util) +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + config_reader + roscpp + nodelet + dynamic_reconfigure + ff_msgs diagnostic_msgs + tf2_geometry_msgs + actionlib + msg_conversions +) + catkin_package( INCLUDE_DIRS include - LIBRARIES ff_nodelet config_server config_client perf_timer - CATKIN_DEPENDS config_reader roscpp nodelet dynamic_reconfigure ff_msgs diagnostic_msgs tf2_geometry_msgs actionlib + LIBRARIES ff_nodelet ff_flight config_server config_client perf_timer + CATKIN_DEPENDS config_reader roscpp nodelet dynamic_reconfigure ff_msgs diagnostic_msgs tf2_geometry_msgs actionlib msg_conversions +) + +########### +## Build ## +########### + +# Specify additional locations of header files +# Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${JSONCPP_INCLUDE_DIRS} ) -create_library(TARGET ff_nodelet - DIR src/ff_nodelet - LIBS ${roscpp_LIBRARIES} ${nodelet_LIBRARIES} config_reader - INC ${catkin_INCLUDE_DIRS} - DEPS ff_msgs +# Declare C++ libraries +add_library(ff_nodelet + src/ff_nodelet/ff_nodelet.cc ) +add_dependencies(ff_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(ff_nodelet ${catkin_LIBRARIES}) -create_library(TARGET ff_flight - DIR src/ff_flight - LIBS ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES} config_reader msg_conversions - INC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - DEPS ff_msgs +# Declare C++ libraries +add_library(ff_flight + src/ff_flight/ff_flight.cc ) +add_dependencies(ff_flight ${catkin_EXPORTED_TARGETS}) +target_link_libraries(ff_flight ${catkin_LIBRARIES}) -create_library(TARGET config_server - DIR src/config_server - LIBS ${catkin_LIBRARIES} config_reader - INC ${catkin_INCLUDE_DIRS} - DEPS config_reader diagnostic_msgs +# Declare C++ libraries +add_library(config_server + src/config_server/config_server.cc ) +add_dependencies(config_server ${catkin_EXPORTED_TARGETS}) +target_link_libraries(config_server ${catkin_LIBRARIES}) -create_library(TARGET config_client - DIR src/config_client - LIBS ${catkin_LIBRARIES} - INC ${catkin_INCLUDE_DIRS} +# Declare C++ libraries +add_library(config_client + src/config_client/config_client.cc ) +add_dependencies(config_client ${catkin_EXPORTED_TARGETS}) +target_link_libraries(config_client ${catkin_LIBRARIES}) -create_library(TARGET perf_timer - DIR src/perf_timer - LIBS ${catkin_LIBRARIES} - INC ${catkin_INCLUDE_DIRS} +# Declare C++ libraries +add_library(perf_timer + src/perf_timer/perf_timer.cc ) +add_dependencies(perf_timer ${catkin_EXPORTED_TARGETS}) +target_link_libraries(perf_timer ${catkin_LIBRARIES}) + +########## +## Test ## +########## # Only test if it is enabled if (CATKIN_ENABLE_TESTING) @@ -64,7 +98,6 @@ if (CATKIN_ENABLE_TESTING) include_directories(${catkin_INCLUDE_DIRS} include) # ff_action - add_rostest_gtest(ff_action_nominal_behaviour test/ff_action_nominal_behaviour.test test/ff_action_nominal_behaviour.cc) @@ -108,4 +141,25 @@ if (CATKIN_ENABLE_TESTING) endif() -install_launch_files() +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ff_nodelet ff_flight config_server config_client perf_timer + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/shared/ff_util/include/ff_util/config_client.h b/shared/ff_util/include/ff_util/config_client.h index 3f011e63ed..1fec674d39 100644 --- a/shared/ff_util/include/ff_util/config_client.h +++ b/shared/ff_util/include/ff_util/config_client.h @@ -21,7 +21,7 @@ #include -#include +#include #include diff --git a/shared/ff_util/include/ff_util/config_server.h b/shared/ff_util/include/ff_util/config_server.h index 69ff535b4c..3223f487b8 100644 --- a/shared/ff_util/include/ff_util/config_server.h +++ b/shared/ff_util/include/ff_util/config_server.h @@ -21,7 +21,7 @@ #include -#include +#include #include diff --git a/shared/ff_util/include/ff_util/ff_action.h b/shared/ff_util/include/ff_util/ff_action.h index 3548395e5b..8556c3eb48 100644 --- a/shared/ff_util/include/ff_util/ff_action.h +++ b/shared/ff_util/include/ff_util/ff_action.h @@ -26,7 +26,7 @@ #include #include -// C++11 includes +// C++ includes #include #include #include @@ -161,11 +161,12 @@ class FreeFlyerActionClient { WAITING_FOR_RESPONSE, /*!< Goal accepted but no feedback/result yet */ WAITING_FOR_DEADLINE /*!< Deadline */ }; - static constexpr double DEFAULT_TIMEOUT_CONNECTED = 0.0; - static constexpr double DEFAULT_TIMEOUT_ACTIVE = 0.0; - static constexpr double DEFAULT_TIMEOUT_RESPONSE = 0.0; - static constexpr double DEFAULT_TIMEOUT_DEADLINE = 0.0; - static constexpr double DEFAULT_POLL_DURATION = 0.1; + static constexpr double DEFAULT_TIMEOUT_CONNECTED = 0.0; + static constexpr double DEFAULT_TIMEOUT_ACTIVE = 0.0; + static constexpr double DEFAULT_TIMEOUT_RESPONSE = 0.0; + static constexpr double DEFAULT_TIMEOUT_DEADLINE = 0.0; + static constexpr double DEFAULT_POLL_DURATION = 0.1; + static constexpr double DEFAULT_TIMEOUT_RESPONSE_DELAY = 0.1; public: // Templated action definition @@ -183,7 +184,8 @@ class FreeFlyerActionClient { to_active_(DEFAULT_TIMEOUT_ACTIVE), to_response_(DEFAULT_TIMEOUT_RESPONSE), to_deadline_(DEFAULT_TIMEOUT_DEADLINE), - to_poll_(DEFAULT_POLL_DURATION) {} + to_poll_(DEFAULT_POLL_DURATION), + to_response_delay_(DEFAULT_TIMEOUT_RESPONSE_DELAY) {} // Destructor ~FreeFlyerActionClient() {} @@ -214,6 +216,8 @@ class FreeFlyerActionClient { &FreeFlyerActionClient::DeadlineTimeoutCallback, this, true, false); timer_poll_ = nh->createTimer(to_poll_, &FreeFlyerActionClient::ConnectPollCallback, this, false, false); + timer_response_delay_ = nh->createTimer(to_response_delay_, + &FreeFlyerActionClient::ResultDelayCallback, this, true, false); // Initialize the action client sac_ = std::shared_ptr < actionlib::SimpleActionClient < ActionSpec > > ( new actionlib::SimpleActionClient < ActionSpec > (*nh, topic, false)); @@ -361,13 +365,30 @@ class FreeFlyerActionClient { // Feedback has been received after a result before. Stop tracking the goal // so that this doesn't happen sac_->stopTrackingGoal(); + // The response we send depends on the state + result_ = result; + // The response we send depends on the state if (action_state == actionlib::SimpleClientGoalState::SUCCEEDED) - Complete(FreeFlyerActionState::SUCCESS, result); + state_response_ = FreeFlyerActionState::SUCCESS; else if (action_state == actionlib::SimpleClientGoalState::PREEMPTED) - Complete(FreeFlyerActionState::PREEMPTED, result); + state_response_ = FreeFlyerActionState::PREEMPTED; else - Complete(FreeFlyerActionState::ABORTED, result); + state_response_ = FreeFlyerActionState::ABORTED; + + StartOptionalTimer(timer_response_delay_, to_response_delay_); + } + + // This delayed callback is necessary because on Ubuntu 20 / ROS noetic, + // an action is only considered finished once the ResultCallback returns. + // This raises the problem where, if another action of the same type is + // called in the ResultCallback or immediately afterwards, it returns + // failed because the previous action is technically not finished and + // returns an error. + void ResultDelayCallback(ros::TimerEvent const& event) { + // Call the result callback on the client side + Complete(state_response_, result_); + // Return to waiting for a goal state_ = WAITING_FOR_GOAL; } @@ -379,6 +400,7 @@ class FreeFlyerActionClient { ros::Duration to_response_; ros::Duration to_deadline_; ros::Duration to_poll_; + ros::Duration to_response_delay_; std::shared_ptr < actionlib::SimpleActionClient < ActionSpec > > sac_; FeedbackCallbackType cb_feedback_; ResultCallbackType cb_result_; @@ -389,6 +411,10 @@ class FreeFlyerActionClient { ros::Timer timer_response_; ros::Timer timer_deadline_; ros::Timer timer_poll_; + ros::Timer timer_response_delay_; + // Save response + FreeFlyerActionState::Enum state_response_; + ResultConstPtr result_; }; } // namespace ff_util diff --git a/shared/ff_util/include/ff_util/ff_faults.h b/shared/ff_util/include/ff_util/ff_faults.h index f028f48c88..9ef76da471 100644 --- a/shared/ff_util/include/ff_util/ff_faults.h +++ b/shared/ff_util/include/ff_util/ff_faults.h @@ -48,12 +48,11 @@ enum FaultKeys { DISK_USAGE_HIGH, HARDWARE_ERROR_FAULT_CODE, BAD_GYRO_DATA, - CONTROL_CYCLE_OVERRUN, IMU_OVERTEMP, IMU_ACCEL_ABOVE_LIMITS }; -constexpr int kFaultKeysSize = 25; +constexpr int kFaultKeysSize = 24; static std::string fault_keys[] = { "UNKNOWN_FAULT_KEY", @@ -78,7 +77,6 @@ static std::string fault_keys[] = { "DISK_USAGE_HIGH", "HARDWARE_ERROR_FAULT_CODE", "BAD_GYRO_DATA", - "CONTROL_CYCLE_OVERRUN", "IMU_OVERTEMP", "IMU_ACCEL_ABOVE_LIMITS" }; diff --git a/shared/ff_util/include/ff_util/ff_fsm.h b/shared/ff_util/include/ff_util/ff_fsm.h index 74a2a1f2b4..1294abfa00 100644 --- a/shared/ff_util/include/ff_util/ff_fsm.h +++ b/shared/ff_util/include/ff_util/ff_fsm.h @@ -19,7 +19,7 @@ #ifndef FF_UTIL_FF_FSM_H_ #define FF_UTIL_FF_FSM_H_ -// C++11 includes +// C++ includes #include #include #include diff --git a/shared/ff_util/include/ff_util/ff_nodelet.h b/shared/ff_util/include/ff_util/ff_nodelet.h index 30d107060c..1b358ed79a 100644 --- a/shared/ff_util/include/ff_util/ff_nodelet.h +++ b/shared/ff_util/include/ff_util/ff_nodelet.h @@ -33,7 +33,7 @@ #include #include -#include +#include #include #include @@ -101,7 +101,7 @@ class FreeFlyerNodelet : public nodelet::Nodelet { // The set function does all of the internal work. We have moved this out // of the onInit() call, so that it can be invoked when a nodelet is not used // for example, in simulation, where the dynamic loading is within gazebo... - void Setup(ros::NodeHandle & nh, ros::NodeHandle & nh_mt); + void Setup(ros::NodeHandle & nh, ros::NodeHandle & nh_mt, std::string plugin_name); std::map faults_; diff --git a/shared/ff_util/include/ff_util/ff_service.h b/shared/ff_util/include/ff_util/ff_service.h index 24e41fbee1..d3b5b84e11 100644 --- a/shared/ff_util/include/ff_util/ff_service.h +++ b/shared/ff_util/include/ff_util/ff_service.h @@ -22,7 +22,7 @@ // ROS includes #include -// C++11 includes +// C++ includes #include #include #include diff --git a/shared/ff_util/launch/ff_nodelet.launch b/shared/ff_util/launch/ff_nodelet.launch index 2727d74832..595db21429 100644 --- a/shared/ff_util/launch/ff_nodelet.launch +++ b/shared/ff_util/launch/ff_nodelet.launch @@ -31,6 +31,7 @@ + @@ -62,7 +63,7 @@ + args="manager" launch-prefix="$(arg prefix) $(arg terminal)" output="$(arg output)"/> ff_msgs tf2_geometry_msgs config_reader + msg_conversions roscpp roslib nodelet @@ -34,5 +35,6 @@ ff_msgs tf2_geometry_msgs config_reader + msg_conversions diff --git a/shared/ff_util/src/config_server/config_server.cc b/shared/ff_util/src/config_server/config_server.cc index 1ad2d383a7..1c8bced301 100644 --- a/shared/ff_util/src/config_server/config_server.cc +++ b/shared/ff_util/src/config_server/config_server.cc @@ -173,7 +173,6 @@ bool ConfigServer::Listen() { } } } - nh_.getParam(id, strs_[id]); nh_.setParam(id, strs_[id]); // *** INTEGERS *** // } else if (!type.compare("integer")) { @@ -219,7 +218,6 @@ bool ConfigServer::Listen() { } } } - nh_.getParam(id, ints_[id]); nh_.setParam(id, ints_[id]); // *** BOOLEANS *** // } else if (!type.compare("boolean")) { @@ -259,8 +257,7 @@ bool ConfigServer::Listen() { } } } - // Grab the persistent value from the parameter server - nh_.getParam(id, bools_[id]); + // Set the value to the parameter server nh_.setParam(id, bools_[id]); // *** DOUBLES *** // } else if (!type.compare("double")) { @@ -306,7 +303,6 @@ bool ConfigServer::Listen() { } } } - nh_.getParam(id, doubles_[id]); nh_.setParam(id, doubles_[id]); } else { ROS_ERROR_STREAM("Could not understand type " << type << " of parameter " << id); diff --git a/shared/ff_util/src/ff_flight/ff_flight.cc b/shared/ff_util/src/ff_flight/ff_flight.cc index cfa9c54f1c..bdc25c458f 100644 --- a/shared/ff_util/src/ff_flight/ff_flight.cc +++ b/shared/ff_util/src/ff_flight/ff_flight.cc @@ -171,11 +171,11 @@ namespace ff_util { if ( !flight_modes.GetTable(j + 1, &mode) || !mode.GetStr("name", &info.name) || !mode.GetBool("control_enabled", &control_enabled) - || !mode.GetReal("collision_radius", &info.collision_radius) || !mode.GetReal("hard_limit_omega", &info.hard_limit_omega) || !mode.GetReal("hard_limit_vel", &info.hard_limit_vel) || !mode.GetReal("hard_limit_alpha", &info.hard_limit_alpha) || !mode.GetReal("hard_limit_accel", &info.hard_limit_accel) + || !mode.GetReal("tolerance_pos_endpoint", &info.tolerance_pos_endpoint) || !mode.GetReal("tolerance_pos", &info.tolerance_pos) || !mode.GetReal("tolerance_att", &info.tolerance_att) || !mode.GetReal("tolerance_omega", &info.tolerance_omega) diff --git a/shared/ff_util/src/ff_nodelet/ff_nodelet.cc b/shared/ff_util/src/ff_nodelet/ff_nodelet.cc index c01daba2c9..14d7be0026 100644 --- a/shared/ff_util/src/ff_nodelet/ff_nodelet.cc +++ b/shared/ff_util/src/ff_nodelet/ff_nodelet.cc @@ -54,7 +54,7 @@ FreeFlyerNodelet::~FreeFlyerNodelet() { } // Called directly by Gazebo and indirectly through onInit() by nodelet -void FreeFlyerNodelet::Setup(ros::NodeHandle & nh, ros::NodeHandle & nh_mt) { +void FreeFlyerNodelet::Setup(ros::NodeHandle & nh, ros::NodeHandle & nh_mt, std::string plugin_name) { // Copy the node handles nh_ = nh; nh_mt_ = nh_mt; @@ -65,7 +65,9 @@ void FreeFlyerNodelet::Setup(ros::NodeHandle & nh, ros::NodeHandle & nh_mt) { // If not set, try and grab the node name from the launch file if (node_.empty()) { - if (getName().empty()) { + if (plugin_name != "") { + node_ = plugin_name; + } else if (getName().empty()) { FF_FATAL("Node name not specified."); } else { // If a robot name has been specified, remove it from the node name @@ -94,18 +96,15 @@ void FreeFlyerNodelet::Setup(ros::NodeHandle & nh, ros::NodeHandle & nh_mt) { heartbeat_.nodelet_manager = ros::this_node::getName(); // Immediately, setup a publisher for faults coming from this node - // Topic needs to be latched for initialization faults pub_heartbeat_ = nh_.advertise( - TOPIC_HEARTBEAT, heartbeat_queue_size_, true); + TOPIC_HEARTBEAT, heartbeat_queue_size_); pub_diagnostics_ = nh_.advertise( TOPIC_DIAGNOSTICS, 5); - // Setup a heartbeat timer for this node if auto start was requested - if (autostart_hb_timer_) { - // Don't autostart until nodelet finishes initialization function - timer_heartbeat_ = nh_.createTimer(ros::Rate(1.0), + // Setup a heartbeat timer regardless of if we use it or not + // Don't autostart until nodelet finishes initialization function + timer_heartbeat_ = nh_.createTimer(ros::Rate(1.0), &FreeFlyerNodelet::HeartbeatCallback, this, false, false); - } // Defer the initialization of the node to prevent a race condition with // nodelet registration. See this issue for more details: @@ -117,7 +116,7 @@ void FreeFlyerNodelet::Setup(ros::NodeHandle & nh, ros::NodeHandle & nh_mt) { // Called by the nodelet framework to initialize the nodelet. Note that this is // *NOT* called by the Gazebo plugins They enter directly via a Setup(...) call. void FreeFlyerNodelet::onInit() { - Setup(getNodeHandle(), getMTNodeHandle()); + Setup(getNodeHandle(), getMTNodeHandle(), ""); } void FreeFlyerNodelet::ReadConfig() { @@ -313,6 +312,10 @@ void FreeFlyerNodelet::InitCallback(ros::TimerEvent const& ev) { // was if (heartbeat_.faults.size() > 0) { PublishHeartbeat(); + // Start heartbeat timer to ensure the system monitor gets the + // initialization fault. Doesn't matter if the node doesn't want to publish + // a heartbeat as the system monitor will unload the nodelet. + timer_heartbeat_.start(); return; } diff --git a/shared/ff_util/test/ff_action_active_timeout.cc b/shared/ff_util/test/ff_action_active_timeout.cc index 3fd2588738..3e0c2183b7 100644 --- a/shared/ff_util/test/ff_action_active_timeout.cc +++ b/shared/ff_util/test/ff_action_active_timeout.cc @@ -16,6 +16,11 @@ * under the License. */ +// Tests action active timeout +// This tests allows the client to connect to the server, and then kills the +// server. When a goal is sent to the server, after 4 seconds, an active +// timeout result should be triggered showing that it did not receive the goal. + // Required for the test framework #include @@ -29,7 +34,7 @@ // Borrow the example from actionlib #include -// C++11 incliudes +// C++ includes #include #include diff --git a/shared/ff_util/test/ff_action_cancel_own_goal.cc b/shared/ff_util/test/ff_action_cancel_own_goal.cc index dd3f60ae13..833fa444b7 100644 --- a/shared/ff_util/test/ff_action_cancel_own_goal.cc +++ b/shared/ff_util/test/ff_action_cancel_own_goal.cc @@ -16,6 +16,12 @@ * under the License. */ +// Test action cancel own goal +// In this test the client sends a goal when the server connects. After the +// action is active (ActiveCallback) a timer starts, 2 seconds later it +// sends a cancel goal. The test is successful if the server cancels the goal. +// If 2 seconds pass and the action was not cancelled it fails. + // Required for the test framework #include @@ -29,7 +35,7 @@ // Borrow the example from actionlib #include -// C++11 incliudes +// C++ includes #include #include @@ -55,6 +61,8 @@ class Server : ff_util::FreeFlyerNodelet { void CancelCallback() { ROS_INFO("S:CancelCallback()"); + // Success + ros::shutdown(); } void PreemptCallback() { @@ -132,8 +140,8 @@ class Client : ff_util::FreeFlyerNodelet { // Timer callback void TimerWaitCallback(ros::TimerEvent const& event) { - timer_wait_.stop(); - ros::shutdown(); + // It did not cancel the goal, ending the test + EXPECT_TRUE(false); } private: @@ -159,6 +167,6 @@ TEST(ff_action, cancel_own_goal) { // Required for the test framework int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "ff_action_cancel_own_goal"); + ros::init(argc, argv, "ff_action_active_timeout"); return RUN_ALL_TESTS(); } diff --git a/shared/ff_util/test/ff_action_connect_timeout.cc b/shared/ff_util/test/ff_action_connect_timeout.cc index 40d3635080..1ebd79e9fe 100644 --- a/shared/ff_util/test/ff_action_connect_timeout.cc +++ b/shared/ff_util/test/ff_action_connect_timeout.cc @@ -16,6 +16,10 @@ * under the License. */ +// Tests action connect timeout +// In this test a server is not spawned, therefore the client can't connect. +// This test passes if a timeout on connect result is issued. + // Required for the test framework #include @@ -29,7 +33,7 @@ // Borrow the example from actionlib #include -// C++11 incliudes +// C++ includes #include #include @@ -95,6 +99,6 @@ TEST(ff_action, connect_timeout) { // Required for the test framework int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "ff_action_connect_timeout"); + ros::init(argc, argv, "ff_action_active_timeout"); return RUN_ALL_TESTS(); } diff --git a/shared/ff_util/test/ff_action_deadline_timeout.cc b/shared/ff_util/test/ff_action_deadline_timeout.cc index dcaaac511b..74f19d9aba 100644 --- a/shared/ff_util/test/ff_action_deadline_timeout.cc +++ b/shared/ff_util/test/ff_action_deadline_timeout.cc @@ -16,6 +16,11 @@ * under the License. */ +// Test action deadline timeout +// In this test the client sends a goal when the server connects. The deadline +// for the action is set to 10 seconds. After the 10 seconds, even if it receives +// feedback, given that no result was issued, a deadline timeout is issued. + // Required for the test framework #include @@ -29,7 +34,7 @@ // Borrow the example from actionlib #include -// C++11 incliudes +// C++ includes #include #include @@ -139,6 +144,6 @@ TEST(ff_action, deadline_timeout) { // Required for the test framework int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "ff_action_deadline_timeout"); + ros::init(argc, argv, "ff_action_active_timeout"); return RUN_ALL_TESTS(); } diff --git a/shared/ff_util/test/ff_action_nominal_behaviour.cc b/shared/ff_util/test/ff_action_nominal_behaviour.cc index d8d7c875c5..9dbdea5121 100644 --- a/shared/ff_util/test/ff_action_nominal_behaviour.cc +++ b/shared/ff_util/test/ff_action_nominal_behaviour.cc @@ -16,6 +16,10 @@ * under the License. */ +// Test action nominal behaviour +// Client seds a goal, after 5 messages a SUCCESS result is issued. +// Test succeeds if the client recieves the success response. + // Required for the test framework #include @@ -29,7 +33,7 @@ // Borrow the example from actionlib #include -// C++11 incliudes +// C++ includes #include #include @@ -159,6 +163,6 @@ TEST(ff_action, nominal_behaviour) { // Required for the test framework int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "ff_action_nominal_behaviour"); + ros::init(argc, argv, "ff_action_active_timeout"); return RUN_ALL_TESTS(); } diff --git a/shared/ff_util/test/ff_action_preempt_others_goal.cc b/shared/ff_util/test/ff_action_preempt_others_goal.cc index cb5fce02e6..243a0303df 100644 --- a/shared/ff_util/test/ff_action_preempt_others_goal.cc +++ b/shared/ff_util/test/ff_action_preempt_others_goal.cc @@ -16,6 +16,11 @@ * under the License. */ +// Test action preempt others goal +// Client #1 sends a goal on the server connected callback. Client #2 starts a timer +// on the server connected callback. When the timer triggers, the action is preempted. +// The test is successful if Client #1 receives the result that the action was preempted. + // Required for the test framework #include @@ -29,7 +34,7 @@ // Borrow the example from actionlib #include -// C++11 incliudes +// C++ includes #include #include @@ -200,6 +205,6 @@ TEST(ff_action, preempt_others_goal) { // Required for the test framework int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "ff_action_preempt_others_goal"); + ros::init(argc, argv, "ff_action_active_timeout"); return RUN_ALL_TESTS(); } diff --git a/shared/ff_util/test/ff_action_preempt_own_goal.cc b/shared/ff_util/test/ff_action_preempt_own_goal.cc index c646aaac9a..97333782cb 100644 --- a/shared/ff_util/test/ff_action_preempt_own_goal.cc +++ b/shared/ff_util/test/ff_action_preempt_own_goal.cc @@ -16,6 +16,13 @@ * under the License. */ +// Test action preempt own goal +// In this test the client sends a goal when the server connects. After the +// action is active (ActiveCallback) a timer starts, 2 seconds later it +// sends another goal. The test is successful if the new goal starts and +// another active callback is triggered, showing that it started a new action. +// If 2 seconds pass and the action was not preempted it fails. + // Required for the test framework #include @@ -29,7 +36,7 @@ // Borrow the example from actionlib #include -// C++11 incliudes +// C++ includes #include #include @@ -95,9 +102,9 @@ class Client : ff_util::FreeFlyerNodelet { action_.SetDeadlineTimeout(10.0); // Call connect action_.Create(nh, "two_ints_action"); - // Cancel the goal after 3 seconds - timer_cancel_ = nh->createTimer(ros::Duration(2.0), - &Client::TimerCancelCallback, this, true, false); + // Preempt the goal after 2 seconds + timer_preempt_ = nh->createTimer(ros::Duration(2.0), + &Client::TimerPreemptCallback, this, true, false); timer_wait_ = nh->createTimer(ros::Duration(2.0), &Client::TimerWaitCallback, this, true, false); } @@ -120,12 +127,19 @@ class Client : ff_util::FreeFlyerNodelet { void ActiveCallback() { ROS_INFO("C:ActiveCallback()"); - timer_cancel_.start(); + // In 2 seconds it will issue a preempt command + if (preemted_ == false) { + timer_preempt_.start(); + preemted_ = true; + } else { + // It preempted and has the other goal active + ros::shutdown(); + } } // Timer callback - void TimerCancelCallback(ros::TimerEvent const& event) { - timer_cancel_.stop(); + void TimerPreemptCallback(ros::TimerEvent const& event) { + timer_preempt_.stop(); actionlib::TwoIntsGoal goal; action_.SendGoal(goal); timer_wait_.start(); @@ -133,14 +147,15 @@ class Client : ff_util::FreeFlyerNodelet { // Timer callback void TimerWaitCallback(ros::TimerEvent const& event) { - timer_wait_.stop(); - ros::shutdown(); + // It did not preempt the goal, ending the test + EXPECT_TRUE(false); } private: ff_util::FreeFlyerActionClient < actionlib::TwoIntsAction > action_; - ros::Timer timer_cancel_; + ros::Timer timer_preempt_; ros::Timer timer_wait_; + bool preemted_ = false; }; // Perform a test of the simple action client @@ -160,6 +175,6 @@ TEST(ff_action, preempt_own_goal) { // Required for the test framework int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "ff_action_preempt_own_goal"); + ros::init(argc, argv, "ff_action_active_timeout"); return RUN_ALL_TESTS(); } diff --git a/shared/ff_util/test/ff_action_response_timeout.cc b/shared/ff_util/test/ff_action_response_timeout.cc index 62c701befd..4eb7c5713f 100644 --- a/shared/ff_util/test/ff_action_response_timeout.cc +++ b/shared/ff_util/test/ff_action_response_timeout.cc @@ -29,10 +29,14 @@ // Borrow the example from actionlib #include -// C++11 incliudes +// C++ includes #include #include +// Test action response timeout +// This test sends a goal with a 4 second timeout result. The server does +// nothing and if a timeout is triggered then the test passes. + // SERVER CALLBACKS class Server : ff_util::FreeFlyerNodelet { @@ -129,6 +133,6 @@ TEST(ff_action, response_timeout) { // Required for the test framework int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "ff_action_response_timeout"); + ros::init(argc, argv, "ff_action_active_timeout"); return RUN_ALL_TESTS(); } diff --git a/shared/jsonloader/CMakeLists.txt b/shared/jsonloader/CMakeLists.txt index 5ee73a9285..d3c678c1ed 100644 --- a/shared/jsonloader/CMakeLists.txt +++ b/shared/jsonloader/CMakeLists.txt @@ -15,31 +15,116 @@ # License for the specific language governing permissions and limitations # under the License. -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused-variable") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-unused-variable") - -set(LIBS - ff_common - ${GLOG_LIBRARIES} - ${JSONCPP_LIBRARIES} - ${Boost_SYSTEM_LIBRARY} - ${Boost_FILESYSTEM_LIBRARY} - ) -set(INCLUDES - ff_common - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${EIGEN3_INCLUDE_DIRS} - ${GLOG_INCLUDE_DIRS} +cmake_minimum_required(VERSION 3.0) +project(jsonloader) + + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + std_msgs + nodelet +) + +find_package(Eigen3 REQUIRED) +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(JsonCpp REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES jsonloader + CATKIN_DEPENDS roscpp std_msgs nodelet +) + +########### +## Build ## +########### + +# Specify additional locations of header files +# Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} ${JSONCPP_INCLUDE_DIRS} - ${Boost_INCLUDE_DIR} +) + +# Declare C++ libraries +add_library(jsonloader + src/command.cc + src/command_repo.cc + src/keepout.cc + src/keepoutio.cc + src/plan.cc + src/planio.cc + src/validation.cc +) +add_dependencies(jsonloader ${catkin_EXPORTED_TARGETS}) +target_link_libraries(jsonloader gflags glog ${JSONCPP_LIBRARIES} ${catkin_LIBRARIES} ) + + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_rostest_gtest(test_command + test/test_command.test + test/test_command.cxx + ) + target_link_libraries(test_command + jsonloader + ) + + add_rostest_gtest(test_keepout + test/test_keepout.test + test/test_keepout.cxx + ) + target_link_libraries(test_keepout + jsonloader + ) + + add_rostest_gtest(test_keepoutio + test/test_keepoutio.test + test/test_keepoutio.cxx + ) + target_link_libraries(test_keepoutio + jsonloader + ) + + add_rostest_gtest(test_plan + test/test_plan.test + test/test_plan.cxx ) + target_link_libraries(test_plan + jsonloader + ) + + add_rostest_gtest(test_validation + test/test_validation.test + test/test_validation.cxx + ) + target_link_libraries(test_validation + jsonloader + ) + +endif() + +############# +## Install ## +############# -create_library(TARGET jsonloader - LIBS ${LIBS} - INC ${INCLUDES} - DEPS ${DEPS}) -create_test_targets(DIR test - LIBS jsonloader - INC ${INCLUDES} - DEPS ${DEPS}) +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/shared/jsonloader/include/jsonloader/command_repo.h b/shared/jsonloader/include/jsonloader/command_repo.h index 389e256029..3174f36c1c 100644 --- a/shared/jsonloader/include/jsonloader/command_repo.h +++ b/shared/jsonloader/include/jsonloader/command_repo.h @@ -35,8 +35,6 @@ constexpr char kCmdPause[] = "pausePlan"; constexpr char kCmdArmPanTilt[] = "armPanAndTilt"; constexpr char kCmdStationKeep[] = "stationKeep"; constexpr char kCmdWait[] = "wait"; -constexpr char kCmdClearData[] = "clearData"; -constexpr char kCmdDownload[] = "downloadData"; constexpr char kCmdPayloadOn[] = "payloadOn"; constexpr char kCmdPowerOn[] = "powerOnItem"; constexpr char kCmdPayloadOff[] = "payloadOff"; @@ -47,7 +45,6 @@ constexpr char kCmdGuestCmd[] = "guestScience"; constexpr char kCmdCustomGuest[] = "customGuestScience"; constexpr char kCmdGripper[] = "gripperControl"; constexpr char kCmdFlashlight[] = "setFlashlightBrightness"; -constexpr char kCmdGenericCmd[] = "genericCommand"; constexpr char kCmdIdleProp[] = "idlePropulsion"; constexpr char kCmdSetCamera[] = "setCamera"; constexpr char kCmdStreamCamera[] = "setCameraStreaming"; @@ -61,6 +58,7 @@ constexpr char kCmdTelemRate[] = "setTelemetryRate"; constexpr char kCmdSwitchLocal[] = "switchLocalization"; constexpr char kCmdStartRecord[] = "startRecording"; constexpr char kCmdStopRecord[] = "stopRecording"; +constexpr char kCmdEnableAstrobeeIntercomms[] = "enableAstrobeeIntercomms"; class DockCommand : public Command { public: @@ -141,16 +139,6 @@ class PowerItemCommand : public Command { std::string which_; }; -class DataCommand : public Command { - public: - explicit DataCommand(Json::Value const& obj); - - std::string const& data_method() const noexcept; - - private: - std::string data_method_; -}; - class GuestScienceCommand : public Command { public: explicit GuestScienceCommand(Json::Value const& obj); @@ -185,18 +173,6 @@ class FlashlightCommand : public Command { float brightness_; }; -class GenericCommand : public Command { - public: - explicit GenericCommand(Json::Value const& obj); - - std::string const& name() const noexcept; - std::string const& param() const noexcept; - - private: - std::string name_; - std::string param_; -}; - class IdlePropulsionCommand : public Command { public: explicit IdlePropulsionCommand(Json::Value const& obj); @@ -330,6 +306,11 @@ class StopRecordingCommand : public Command { explicit StopRecordingCommand(Json::Value const& obj); }; +class EnableAstrobeeIntercommsCommand : public Command { + public: + explicit EnableAstrobeeIntercommsCommand(Json::Value const& obj); +}; + } // end namespace jsonloader #endif // JSONLOADER_COMMAND_REPO_H_ diff --git a/shared/jsonloader/package.xml b/shared/jsonloader/package.xml new file mode 100644 index 0000000000..62ad5593f4 --- /dev/null +++ b/shared/jsonloader/package.xml @@ -0,0 +1,24 @@ + + jsonloader + 1.0.0 + + Package for loading json from fplans + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + roscpp + std_msgs + nodelet + roscpp + std_msgs + nodelet + + diff --git a/shared/jsonloader/src/command_repo.cc b/shared/jsonloader/src/command_repo.cc index 87c3a7abf5..1df1e44ce1 100644 --- a/shared/jsonloader/src/command_repo.cc +++ b/shared/jsonloader/src/command_repo.cc @@ -47,14 +47,11 @@ extern const InsensitiveMap kCommandMap = { { kCmdPayloadOff, &CreateCommand }, { kCmdPowerOff, &CreateCommand }, { kCmdGripper, &CreateCommand }, - { kCmdClearData, &CreateCommand }, - { kCmdDownload, &CreateCommand }, { kCmdStartGuest, &CreateCommand }, { kCmdCustomGuest, &CreateCommand }, { kCmdGuestCmd, &CreateCommand }, { kCmdStopGuest, &CreateCommand }, { kCmdFlashlight, &CreateCommand }, - { kCmdGenericCmd, &CreateCommand }, { kCmdSetCamera, &CreateCommand }, { kCmdStreamCamera, &CreateCommand }, { kCmdRecordCamera, &CreateCommand }, @@ -69,6 +66,7 @@ extern const InsensitiveMap kCommandMap = { { kCmdSwitchLocal, &CreateCommand }, { kCmdStartRecord, &CreateCommand }, { kCmdStopRecord, &CreateCommand }, + { kCmdEnableAstrobeeIntercomms, &CreateCommand } }; // Command names that need the normalized @@ -105,10 +103,6 @@ const Fields powerFields { new Field("which", Json::stringValue) }; -const Fields dataFields { - new Field("dataMethod", Json::stringValue) -}; - const Fields guestScienceFields { new Field("apkName", Json::stringValue), }; @@ -123,11 +117,6 @@ const Fields flashlightFields { new Field("brightness", Json::realValue) }; -const Fields genericCommandFields { - new Field("commandName", Json::stringValue), - new Field("param", Json::stringValue) -}; - const Fields cameraFields { new EnumField("cameraName", { "Science", @@ -184,7 +173,8 @@ const Fields telemetryRateFields { "EkfState", "GncState", "PmcCmdState", - "Position" + "Position", + "SparseMappingPose" }), new Field("rate", Json::realValue) }; @@ -317,23 +307,6 @@ std::string const& PowerItemCommand::which() const noexcept { return which_; } -DataCommand::DataCommand(Json::Value const& obj) - : Command(obj), data_method_("") { - set_valid(true); - if (!Validate(obj, dataFields)) { - LOG(ERROR) << "invalid DataCommand."; - return; - } - - data_method_ = obj["dataMethod"].asString(); - - set_valid(true); -} - -std::string const& DataCommand::data_method() const noexcept { - return data_method_; -} - GuestScienceCommand::GuestScienceCommand(Json::Value const& obj) : Command(obj), apk_("") { if (!Validate(obj, guestScienceFields)) { @@ -392,27 +365,6 @@ float FlashlightCommand::brightness() const noexcept { return brightness_; } -GenericCommand::GenericCommand(Json::Value const& obj) - : Command(obj), name_(""), param_("") { - if (!Validate(obj, genericCommandFields)) { - LOG(ERROR) << "invalid GenericCommand."; - return; - } - - name_ = obj["commandName"].asString(); - param_ = obj["param"].asString(); - - set_valid(true); -} - -std::string const& GenericCommand::name() const noexcept { - return name_; -} - -std::string const& GenericCommand::param() const noexcept { - return param_; -} - IdlePropulsionCommand::IdlePropulsionCommand(Json::Value const& obj) : Command(obj) { set_valid(true); @@ -623,4 +575,9 @@ StopRecordingCommand::StopRecordingCommand(Json::Value const& obj) set_valid(true); } +EnableAstrobeeIntercommsCommand::EnableAstrobeeIntercommsCommand( + Json::Value const& obj) + : Command(obj) { + set_valid(true); +} } // namespace jsonloader diff --git a/shared/jsonloader/test/test_command.cxx b/shared/jsonloader/test/test_command.cxx index 07573bbebe..3d38557e76 100644 --- a/shared/jsonloader/test/test_command.cxx +++ b/shared/jsonloader/test/test_command.cxx @@ -16,6 +16,9 @@ * under the License. */ +// Test commands +// These tests check that commands are parsed correctly using the jsonloader + #include #include @@ -114,31 +117,6 @@ TEST(Command, VaildGripperCommand) { ASSERT_EQ(g_cmd->open(), true); } -TEST(Command, VaildDataCommand) { - const std::string data = u8R"({ - "type" : "downloadData", - "dataMethod" : "Immediate", - "blocking" : true, - "color" : "#555555", - "scopeTerminate" : true, - "name" : "3.1 DownloadData", - "id" : "0f15c5e7-dc59-457e-9cda-62394754b9de" - })"; - Json::Value v; - Json::Reader().parse(data, v, false); - - Command *cmd = Command::Make(v); - - ASSERT_NE(cmd, nullptr); - ASSERT_TRUE(cmd->valid()); - ASSERT_STREQ(cmd->type().data(), jsonloader::kCmdDownload); - - const jsonloader::DataCommand *d_cmd = - dynamic_cast(cmd); - - ASSERT_STREQ(d_cmd->data_method().data(), "Immediate"); -} - TEST(Command, ValidGuestScienceCommand) { const std::string data = u8R"({ "type" : "startGuestScience", @@ -191,32 +169,6 @@ TEST(Command, ValidGuestCustomCommand) { ASSERT_STREQ(gs_cmd->command().data(), "{name=Take, num=5, time_between=10}"); } -TEST(Command, VaildGenericCommand) { - const std::string data = u8R"({ - "type" : "genericCommand", - "commandName" : "CommandName", - "param" : "Parameters", - "blocking" : true, - "color" : "#555555", - "scopeTerminate" : true, - "name" : "0.0 GenericCommand", - "id" : "1ea5674c-802e-469f-b8a7-be5b7875b71b" - })"; - Json::Value v; - Json::Reader().parse(data, v, false); - - Command *cmd = Command::Make(v); - - ASSERT_NE(cmd, nullptr); - ASSERT_STREQ(cmd->type().data(), jsonloader::kCmdGenericCmd); - - const jsonloader::GenericCommand *g_cmd = - dynamic_cast(cmd); - - ASSERT_STREQ(g_cmd->name().data(), "CommandName"); - ASSERT_STREQ(g_cmd->param().data(), "Parameters"); -} - TEST(Command, VaildSetCameraCommand) { const std::string data = u8R"({ "type" : "setCamera", @@ -462,3 +414,9 @@ TEST(Command, ValidStartRecording) { ASSERT_STREQ(s_cmd->description().data(), "mom"); } + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/shared/jsonloader/test/test_command.test b/shared/jsonloader/test/test_command.test new file mode 100644 index 0000000000..f083b37a9d --- /dev/null +++ b/shared/jsonloader/test/test_command.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/jsonloader/test/test_keepout.cxx b/shared/jsonloader/test/test_keepout.cxx index 50075ec9b0..65847da773 100644 --- a/shared/jsonloader/test/test_keepout.cxx +++ b/shared/jsonloader/test/test_keepout.cxx @@ -16,6 +16,10 @@ * under the License. */ +// Test keepout +// This test tests the creation of keepout zones using bounding boxes, the +// isSafe condition and that merging bouding boxes gives expected result. + #include #include @@ -59,3 +63,9 @@ TEST(Keepout, MergeCorrect) { a.Merge(b); EXPECT_EQ(a.GetBoxes().size(), 2); } + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/shared/jsonloader/test/test_keepout.test b/shared/jsonloader/test/test_keepout.test new file mode 100644 index 0000000000..d2a1607f83 --- /dev/null +++ b/shared/jsonloader/test/test_keepout.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/jsonloader/test/test_keepoutio.cxx b/shared/jsonloader/test/test_keepoutio.cxx index 732b37512d..2141500276 100644 --- a/shared/jsonloader/test/test_keepoutio.cxx +++ b/shared/jsonloader/test/test_keepoutio.cxx @@ -16,6 +16,9 @@ * under the License. */ +// Test keepoutio +// Loads a good and a faulty keepout descriptor file and asserts result + #include #include @@ -24,7 +27,7 @@ #include // TEST_DIR is defined by cmake as the test directory of our module -const std::string kDataDir = std::string(TEST_DIR) + "/data/"; +const std::string kDataDir = std::string(std::getenv("DATA_DIR")); TEST(KeepoutIO, LoadGoodKeepout) { jsonloader::Keepout k(true); // opposite of what it should be @@ -60,3 +63,9 @@ TEST(KeepoutIO, LoadDir) { EXPECT_EQ(safeZone.GetBoxes().size(), 1); EXPECT_EQ(dangerZone.GetBoxes().size(), 2); } + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/shared/jsonloader/test/test_keepoutio.test b/shared/jsonloader/test/test_keepoutio.test new file mode 100644 index 0000000000..559baa9702 --- /dev/null +++ b/shared/jsonloader/test/test_keepoutio.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/jsonloader/test/test_plan.cxx b/shared/jsonloader/test/test_plan.cxx index 4f4bbf6323..3d30ede81a 100644 --- a/shared/jsonloader/test/test_plan.cxx +++ b/shared/jsonloader/test/test_plan.cxx @@ -16,6 +16,10 @@ * under the License. */ +// Test plan +// Tests that plan contruction elements are loaded correctly, detected and +// asserted. + #include #include @@ -33,7 +37,7 @@ namespace fs = boost::filesystem; -const std::string kDataDir = std::string(TEST_DIR) + "/data/"; +const std::string kDataDir = std::string(std::getenv("DATA_DIR")); std::string SlurpFile(std::string const& f_name) { std::ifstream file(f_name, std::ios::in | std::ios::binary); @@ -311,3 +315,9 @@ TEST(PlanIO, PlanEvolution) { LOG(INFO) << "Operating Limits: " << ol.valid(); } } + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/shared/jsonloader/test/test_plan.test b/shared/jsonloader/test/test_plan.test new file mode 100644 index 0000000000..6b19ad8f80 --- /dev/null +++ b/shared/jsonloader/test/test_plan.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/jsonloader/test/test_validation.cxx b/shared/jsonloader/test/test_validation.cxx index ba942a91c4..d5fd191cf1 100644 --- a/shared/jsonloader/test/test_validation.cxx +++ b/shared/jsonloader/test/test_validation.cxx @@ -16,6 +16,10 @@ * under the License. */ +// Test validation +// Tests the checks on the individual fields, aka maximum and minumum range, +// field type and structure + #include #include @@ -175,3 +179,8 @@ TEST(Validation, EnumField) { EXPECT_TRUE(optional.Validate(v)); } +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/shared/jsonloader/test/test_validation.test b/shared/jsonloader/test/test_validation.test new file mode 100644 index 0000000000..6cada9502d --- /dev/null +++ b/shared/jsonloader/test/test_validation.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/msg_conversions/CMakeLists.txt b/shared/msg_conversions/CMakeLists.txt index 2f207ac81a..4a66b1750e 100644 --- a/shared/msg_conversions/CMakeLists.txt +++ b/shared/msg_conversions/CMakeLists.txt @@ -15,22 +15,87 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(msg_conversions) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + message_runtime + std_msgs + geometry_msgs + config_reader + ff_msgs + tf2 + tf2_ros +) + +find_package(Eigen3 REQUIRED) + catkin_package( LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include - CATKIN_DEPENDS roscpp message_runtime std_msgs geometry_msgs sensor_msgs - DEPENDS ff_msgs + CATKIN_DEPENDS roscpp message_runtime std_msgs geometry_msgs config_reader tf2 tf2_ros +) + +########### +## Build ## +########### + +# Specify additional locations of header files +# Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} ) -create_library(TARGET msg_conversions - LIBS ${catkin_LIBRARIES} config_reader - DEPS ff_msgs eigen - INC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} +# Declare C++ libraries +add_library(msg_conversions + src/msg_conversions.cc ) +add_dependencies(msg_conversions ${catkin_EXPORTED_TARGETS}) +target_link_libraries(msg_conversions ${EIGEN_LIBRARIES} ${catkin_LIBRARIES}) + +## Declare a C++ executable: inspection_tool +add_executable(landmark_msg_cnv tools/landmark_msg_cnv.cc) +add_dependencies(landmark_msg_cnv ${catkin_EXPORTED_TARGETS}) +target_link_libraries(landmark_msg_cnv + msg_conversions gflags ${catkin_LIBRARIES}) + +## Declare a C++ executable: inspection_tool +add_executable(pose_stamped_msg_cnv tools/pose_stamped_msg_cnv.cc) +add_dependencies(pose_stamped_msg_cnv ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pose_stamped_msg_cnv + msg_conversions gflags ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Install C++ executables +install(TARGETS landmark_msg_cnv DESTINATION bin) +install(TARGETS pose_stamped_msg_cnv DESTINATION bin) +install(CODE "execute_process( + COMMAND ln -s ../../bin/landmark_msg_cnv share/${PROJECT_NAME} + COMMAND ln -s ../../bin/pose_stamped_msg_cnv share/${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} + OUTPUT_QUIET + ERROR_QUIET + )") -create_tool_targets(DIR tools - LIBS ${catkin_LIBRARIES} ${GFLAGS_LIBRARIES} ff_common - INC ${catkin_INCLUDE_DIRS} ${GFLAGS_INCLUDE_DIRS} - DEPS ff_msgs +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE ) diff --git a/shared/msg_conversions/include/msg_conversions/msg_conversions.h b/shared/msg_conversions/include/msg_conversions/msg_conversions.h index 213216c046..39a63ed062 100644 --- a/shared/msg_conversions/include/msg_conversions/msg_conversions.h +++ b/shared/msg_conversions/include/msg_conversions/msg_conversions.h @@ -1,14 +1,14 @@ /* Copyright (c) 2017, United States Government, as represented by the * Administrator of the National Aeronautics and Space Administration. - * + * * All rights reserved. - * + * * The Astrobee platform is licensed under the Apache License, Version 2.0 * (the "License"); you may not use this file except in compliance with the * License. You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -21,60 +21,163 @@ #include -#include -#include +#include +#include #include +#include #include #include #include +#include #include +// TF2 support +#include + +#include + +// Helper macro to automatically load a parameter for a basic type (int/bool/double). +// Assumes function is called as LOAD_PARAM(param.val, config, prefix) and assumes the format of +// param.y for the variable name (passing simply y will fail.) +#define LOAD_PARAM(val, config, prefix) \ + msg_conversions::Load((config), (val), std::string(#val).substr(std::string(#val).find_last_of(".") + 1), (prefix)) + namespace msg_conversions { - Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point & p); - Eigen::Vector3d ros_to_eigen_vector(const geometry_msgs::Vector3 & v); - geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d & v); - void eigen_to_array_vector(const Eigen::Vector3d & v, float* array); - void ros_to_array_vector(const geometry_msgs::Vector3 & v, float* array); - geometry_msgs::Vector3 array_to_ros_vector(float* array); - - geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d & v); - void ros_to_array_point(const geometry_msgs::Point & p, float* array); - geometry_msgs::Point array_to_ros_point(float* array); - - Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion & q); - geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond & q); - geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Vector4d & v); - void eigen_to_array_quat(const Eigen::Quaterniond & q, float* array); - void ros_to_array_quat(const geometry_msgs::Quaternion & q, float* array); - geometry_msgs::Quaternion array_to_ros_quat(float* array); - Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose & p); - Eigen::Affine3d ros_to_eigen_transform(const geometry_msgs::Transform & p); - - // load from config file - bool config_read_quat(config_reader::ConfigReader* config, - const char* name, Eigen::Quaterniond* quat); - bool config_read_vector(config_reader::ConfigReader* config, - const char* name, Eigen::Vector3d* vec); - bool config_read_array(config_reader::ConfigReader* config, - const char* name, int length, float* dest); - bool config_read_matrix(config_reader::ConfigReader* config, const char* name, - int rows, int cols, float* dest); - bool config_read_transform(config_reader::ConfigReader* config, const char* name, - Eigen::Vector3d* vec, Eigen::Quaterniond* quat); - bool config_read_transform(config_reader::ConfigReader* config, const char* name, - geometry_msgs::Vector3* vec, geometry_msgs::Quaternion* quat); - bool config_read_transform(config_reader::ConfigReader* config, const char* name, - Eigen::Affine3d* transform); - bool config_read_transform(config_reader::ConfigReader* config, const char* name, - geometry_msgs::Transform* transform); - - bool config_read_quat(config_reader::ConfigReader::Table* t, Eigen::Quaterniond* quat); - bool config_read_vector(config_reader::ConfigReader::Table* t, Eigen::Vector3d* vec); - bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quaternion* quat); - bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Vector3* vec); - bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Point* point); +Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point& p); +Eigen::Vector3d ros_to_eigen_vector(const geometry_msgs::Vector3& v); +geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d& v); +void eigen_to_array_vector(const Eigen::Vector3d& v, float* array); +void ros_to_array_vector(const geometry_msgs::Vector3& v, float* array); +geometry_msgs::Vector3 array_to_ros_vector(float* array); + +geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d& v); +void ros_to_array_point(const geometry_msgs::Point& p, float* array); +geometry_msgs::Point array_to_ros_point(float* array); + +Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion& q); +geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond& q); +geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Vector4d& v); +geometry_msgs::Quaternion tf2_quat_to_ros_quat(const tf2::Quaternion& q); +void eigen_to_array_quat(const Eigen::Quaterniond& q, float* array); +void ros_to_array_quat(const geometry_msgs::Quaternion& q, float* array); +geometry_msgs::Quaternion array_to_ros_quat(float* array); + +Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose& p); +Eigen::Affine3d ros_to_eigen_transform(const geometry_msgs::Transform& p); +geometry_msgs::Pose ros_transform_to_ros_pose(const geometry_msgs::Transform& p); +geometry_msgs::Pose tf2_transform_to_ros_pose(const tf2::Transform& p); +geometry_msgs::Pose eigen_transform_to_ros_pose(const Eigen::Affine3d& p); +geometry_msgs::Transform eigen_transform_to_ros_transform(const Eigen::Affine3d& p); +tf2::Transform ros_tf_to_tf2_transform(const geometry_msgs::Transform& p); +tf2::Transform ros_pose_to_tf2_transform(const geometry_msgs::Pose& p); + +// load from config file +bool config_read_quat(config_reader::ConfigReader* config, const char* name, Eigen::Quaterniond* quat); +bool config_read_vector(config_reader::ConfigReader* config, const char* name, Eigen::Vector3d* vec); +bool config_read_array(config_reader::ConfigReader* config, const char* name, int length, float* dest); +bool config_read_matrix(config_reader::ConfigReader* config, const char* name, int rows, int cols, float* dest); +bool config_read_transform(config_reader::ConfigReader* config, const char* name, Eigen::Vector3d* vec, + Eigen::Quaterniond* quat); +bool config_read_transform(config_reader::ConfigReader* config, const char* name, geometry_msgs::Vector3* vec, + geometry_msgs::Quaternion* quat); +bool config_read_transform(config_reader::ConfigReader* config, const char* name, Eigen::Affine3d* transform); +bool config_read_transform(config_reader::ConfigReader* config, const char* name, geometry_msgs::Transform* transform); + +bool config_read_quat(config_reader::ConfigReader::Table* t, Eigen::Quaterniond* quat); +bool config_read_vector(config_reader::ConfigReader::Table* t, Eigen::Vector3d* vec); +bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quaternion* quat); +bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Vector3* vec); +bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Point* point); + +bool SingleBoolTrue(const std::initializer_list& bools); + +// Alternative format for loading configs +Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const std::string& transform_config_name, + const std::string& prefix = ""); +double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix = ""); +float LoadFloat(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix = ""); +int LoadInt(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix = ""); +bool LoadBool(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix = ""); +std::string LoadString(config_reader::ConfigReader& config, const std::string& config_name, + const std::string& prefix = ""); +// Overloads for parameter loading that enable LOAD_PARAM macro to work +void Load(config_reader::ConfigReader& config, float& val, const std::string& config_name, + const std::string& prefix = ""); +void Load(config_reader::ConfigReader& config, double& val, const std::string& config_name, + const std::string& prefix = ""); +void Load(config_reader::ConfigReader& config, int& val, const std::string& config_name, + const std::string& prefix = ""); +void Load(config_reader::ConfigReader& config, bool& val, const std::string& config_name, + const std::string& prefix = ""); +void Load(config_reader::ConfigReader& config, std::string& val, const std::string& config_name, + const std::string& prefix = ""); + +void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Pose& msg_pose); +void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Transform& msg_transform); +void VariancesToCovDiag(const Eigen::Vector3d& variances, float* const cov_diag); +Eigen::Vector3d CovDiagToVariances(const float* const cov_diag); +void EigenPoseCovarianceToMsg(const Eigen::Isometry3d& pose, const Eigen::Matrix& covariance, + geometry_msgs::PoseWithCovarianceStamped& pose_cov_msg); +void EigenPoseCovarianceToMsg(const Eigen::Isometry3d& pose, const Eigen::Matrix& covariance, + geometry_msgs::PoseWithCovariance& pose_cov_msg); + +template +VectorType VectorFromMsg(const MsgVectorType& msg_vector) { + return VectorType(msg_vector.x, msg_vector.y, msg_vector.z); +} + +template +VectorType Vector2dFromMsg(const MsgVectorType& msg_vector) { + return VectorType(msg_vector.x, msg_vector.y); +} + +template +void VectorToMsg(const VectorType& vector, MsgVectorType& msg_vector) { + msg_vector.x = vector.x(); + msg_vector.y = vector.y(); + msg_vector.z = vector.z(); +} + +template +void Vector2dToMsg(const VectorType& vector, MsgVectorType& msg_vector) { + msg_vector.x = vector.x(); + msg_vector.y = vector.y(); +} + +template +RotationType RotationFromMsg(const MsgRotationType& msg_rotation) { + return RotationType(msg_rotation.w, msg_rotation.x, msg_rotation.y, msg_rotation.z); +} + +template +void RotationToMsg(const RotationType& rotation, MsgRotationType& msg_rotation) { + msg_rotation.w = rotation.w(); + msg_rotation.x = rotation.x(); + msg_rotation.y = rotation.y(); + msg_rotation.z = rotation.z(); +} + +template +void EigenCovarianceToMsg(const Eigen::Matrix& covariance, ArrayType& covariance_array) { + for (int i = 0; i < Dim; ++i) { + for (int j = 0; j < Dim; ++j) { + covariance_array[i*Dim + j] = covariance(i, j); + } + } +} + +template +Eigen::Matrix EigenCovarianceFromMsg(const ArrayType& covariance_array) { + Eigen::Matrix covariance; + for (int i = 0; i < Dim; ++i) { + for (int j = 0; j < Dim; ++j) { + covariance(i, j) = covariance_array[i*Dim + j]; + } + } + return covariance; +} } // namespace msg_conversions #endif // MSG_CONVERSIONS_MSG_CONVERSIONS_H_ diff --git a/shared/msg_conversions/package.xml b/shared/msg_conversions/package.xml index d926f2347c..3969db8423 100644 --- a/shared/msg_conversions/package.xml +++ b/shared/msg_conversions/package.xml @@ -15,14 +15,22 @@ catkin roscpp + message_runtime std_msgs sensor_msgs geometry_msgs ff_msgs + config_reader + tf2 + tf2_ros roscpp message_runtime + message_runtime std_msgs sensor_msgs geometry_msgs ff_msgs + config_reader + tf2 + tf2_ros diff --git a/shared/msg_conversions/src/msg_conversions.cc b/shared/msg_conversions/src/msg_conversions.cc index cd6604a84a..1e1e8f4e2f 100644 --- a/shared/msg_conversions/src/msg_conversions.cc +++ b/shared/msg_conversions/src/msg_conversions.cc @@ -1,14 +1,14 @@ /* Copyright (c) 2017, United States Government, as represented by the * Administrator of the National Aeronautics and Space Administration. - * + * * All rights reserved. - * + * * The Astrobee platform is licensed under the Apache License, Version 2.0 * (the "License"); you may not use this file except in compliance with the * License. You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -18,18 +18,15 @@ #include -namespace msg_conversions { +#include -Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point & p) { - return Eigen::Vector3d(p.x, p.y, p.z); -} +namespace msg_conversions { +Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point& p) { return Eigen::Vector3d(p.x, p.y, p.z); } -Eigen::Vector3d ros_to_eigen_vector(const geometry_msgs::Vector3 & v) { - return Eigen::Vector3d(v.x, v.y, v.z); -} +Eigen::Vector3d ros_to_eigen_vector(const geometry_msgs::Vector3& v) { return Eigen::Vector3d(v.x, v.y, v.z); } -geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d & v) { +geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d& v) { geometry_msgs::Vector3 n; n.x = v[0]; n.y = v[1]; @@ -37,7 +34,7 @@ geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d & v) { return n; } -geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d & v) { +geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d& v) { geometry_msgs::Point n; n.x = v[0]; n.y = v[1]; @@ -45,25 +42,19 @@ geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d & v) { return n; } -Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion & q) { +Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion& q) { return Eigen::Quaterniond(q.w, q.x, q.y, q.z); } -geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond & q) { +geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond& q) { geometry_msgs::Quaternion out; - out.x = q.x(); - out.y = q.y(); - out.z = q.z(); - out.w = q.w(); + RotationToMsg(q, out); return out; } -geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Vector4d & v) { +geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Vector4d& v) { geometry_msgs::Quaternion out; - out.x = v.x(); - out.y = v.y(); - out.z = v.z(); - out.w = v.w(); + RotationToMsg(v, out); return out; } @@ -92,63 +83,59 @@ geometry_msgs::Quaternion array_to_ros_quat(float* array) { return q; } -void ros_to_array_vector(const geometry_msgs::Vector3 & v, float* array) { +void ros_to_array_vector(const geometry_msgs::Vector3& v, float* array) { array[0] = v.x; array[1] = v.y; array[2] = v.z; } -void ros_to_array_point(const geometry_msgs::Point & p, float* array) { +void ros_to_array_point(const geometry_msgs::Point& p, float* array) { array[0] = p.x; array[1] = p.y; array[2] = p.z; } -void ros_to_array_quat(const geometry_msgs::Quaternion & q, float* array) { +void ros_to_array_quat(const geometry_msgs::Quaternion& q, float* array) { array[0] = q.x; array[1] = q.y; array[2] = q.z; array[3] = q.w; } -void eigen_to_array_vector(const Eigen::Vector3d & v, float* array) { +void eigen_to_array_vector(const Eigen::Vector3d& v, float* array) { array[0] = v.x(); array[1] = v.y(); array[2] = v.z(); } -void eigen_to_array_quat(const Eigen::Quaterniond & q, float* array) { +void eigen_to_array_quat(const Eigen::Quaterniond& q, float* array) { array[0] = q.x(); array[1] = q.y(); array[2] = q.z(); array[3] = q.w(); } -bool config_read_quat(config_reader::ConfigReader* config, - const char* name, Eigen::Quaterniond* quat) { +bool config_read_quat(config_reader::ConfigReader* config, const char* name, Eigen::Quaterniond* quat) { config_reader::ConfigReader::Table t(config, name); return config_read_quat(&t, quat); } -bool config_read_vector(config_reader::ConfigReader* config, - const char* name, Eigen::Vector3d* vec) { +bool config_read_vector(config_reader::ConfigReader* config, const char* name, Eigen::Vector3d* vec) { config_reader::ConfigReader::Table t(config, name); return config_read_vector(&t, vec); } -bool config_read_array(config_reader::ConfigReader* config, - const char* name, int length, float* dest) { +bool config_read_array(config_reader::ConfigReader* config, const char* name, int length, float* dest) { config_reader::ConfigReader::Table array(config, name); for (int i = 0; i < length; i++) { - if (!array.GetReal(i+1, &dest[i])) { + if (!array.GetReal(i + 1, &dest[i])) { return false; } } return true; } -bool config_read_matrix(config_reader::ConfigReader* config, const char* name, - int rows, int cols, float* dest) { +bool config_read_matrix(config_reader::ConfigReader* config, const char* name, int rows, int cols, float* dest) { config_reader::ConfigReader::Table matrix(config, name); int index = 0; for (int i = 1; i <= rows; i++) { @@ -163,8 +150,8 @@ bool config_read_matrix(config_reader::ConfigReader* config, const char* name, return true; } -bool config_read_transform(config_reader::ConfigReader* config, const char* name, - Eigen::Vector3d* vec, Eigen::Quaterniond* quat) { +bool config_read_transform(config_reader::ConfigReader* config, const char* name, Eigen::Vector3d* vec, + Eigen::Quaterniond* quat) { config_reader::ConfigReader::Table t(config, name); config_reader::ConfigReader::Table v(&t, "trans"); config_reader::ConfigReader::Table r(&t, "rot"); @@ -173,8 +160,8 @@ bool config_read_transform(config_reader::ConfigReader* config, const char* name return success; } -bool config_read_transform(config_reader::ConfigReader* config, const char* name, - geometry_msgs::Vector3* vec, geometry_msgs::Quaternion* quat) { +bool config_read_transform(config_reader::ConfigReader* config, const char* name, geometry_msgs::Vector3* vec, + geometry_msgs::Quaternion* quat) { config_reader::ConfigReader::Table t(config, name); config_reader::ConfigReader::Table v(&t, "trans"); config_reader::ConfigReader::Table r(&t, "rot"); @@ -183,30 +170,25 @@ bool config_read_transform(config_reader::ConfigReader* config, const char* name return success; } -bool config_read_transform(config_reader::ConfigReader* config, const char* name, - Eigen::Affine3d* transform) { +bool config_read_transform(config_reader::ConfigReader* config, const char* name, Eigen::Affine3d* transform) { Eigen::Vector3d trans; Eigen::Quaterniond rot; - if (!config_read_transform(config, name, &trans, &rot)) - return false; - *transform = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); + if (!config_read_transform(config, name, &trans, &rot)) return false; + *transform = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * Eigen::Affine3d(rot); return true; } -bool config_read_transform(config_reader::ConfigReader* config, const char* name, - geometry_msgs::Transform* transform) { +bool config_read_transform(config_reader::ConfigReader* config, const char* name, geometry_msgs::Transform* transform) { geometry_msgs::Vector3 trans; geometry_msgs::Quaternion rot; - if (!config_read_transform(config, name, &trans, &rot)) - return false; + if (!config_read_transform(config, name, &trans, &rot)) return false; transform->translation = trans; transform->rotation = rot; return true; } bool config_read_quat(config_reader::ConfigReader::Table* t, Eigen::Quaterniond* quat) { - bool success = t->GetReal("x", &quat->x()); + bool success = t->GetReal("x", &quat->x()); success = success && t->GetReal("y", &quat->y()); success = success && t->GetReal("z", &quat->z()); success = success && t->GetReal("w", &quat->w()); @@ -214,14 +196,14 @@ bool config_read_quat(config_reader::ConfigReader::Table* t, Eigen::Quaterniond* } bool config_read_vector(config_reader::ConfigReader::Table* t, Eigen::Vector3d* vec) { - bool success = t->GetReal(1, &vec->x()); + bool success = t->GetReal(1, &vec->x()); success = success && t->GetReal(2, &vec->y()); success = success && t->GetReal(3, &vec->z()); return success; } bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quaternion* quat) { - bool success = t->GetReal("x", &quat->x); + bool success = t->GetReal("x", &quat->x); success = success && t->GetReal("y", &quat->y); success = success && t->GetReal("z", &quat->z); success = success && t->GetReal("w", &quat->w); @@ -229,33 +211,198 @@ bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quat } bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Vector3* vec) { - bool success = t->GetReal(1, &vec->x); + bool success = t->GetReal(1, &vec->x); success = success && t->GetReal(2, &vec->y); success = success && t->GetReal(3, &vec->z); return success; } bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Point* point) { - bool success = t->GetReal(1, &point->x); + bool success = t->GetReal(1, &point->x); success = success && t->GetReal(2, &point->y); success = success && t->GetReal(3, &point->z); return success; } -Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose & p) { +bool SingleBoolTrue(const std::initializer_list& bools) { + int num_true_bools = 0; + for (const auto bool_value : bools) { + if (bool_value) ++num_true_bools; + if (num_true_bools > 1) return false; + } + return num_true_bools == 1; +} + +Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose& p) { Eigen::Affine3d transform; transform.translation() = Eigen::Vector3d(p.position.x, p.position.y, p.position.z); - transform.linear() = Eigen::Quaterniond( - p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z).toRotationMatrix(); + transform.linear() = + Eigen::Quaterniond(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z).toRotationMatrix(); return transform; } -Eigen::Affine3d ros_to_eigen_transform(const geometry_msgs::Transform & p) { +Eigen::Affine3d ros_to_eigen_transform(const geometry_msgs::Transform& p) { Eigen::Affine3d transform; transform.translation() = Eigen::Vector3d(p.translation.x, p.translation.y, p.translation.z); - transform.linear() = Eigen::Quaterniond( - p.rotation.w, p.rotation.x, p.rotation.y, p.rotation.z).toRotationMatrix(); + transform.linear() = Eigen::Quaterniond(p.rotation.w, p.rotation.x, p.rotation.y, p.rotation.z).toRotationMatrix(); + return transform; +} + +geometry_msgs::Pose ros_transform_to_ros_pose(const geometry_msgs::Transform& p) { + geometry_msgs::Pose transform; + transform.position.x = p.translation.x; + transform.position.y = p.translation.y; + transform.position.z = p.translation.z; + transform.orientation = p.rotation; + return transform; +} + +geometry_msgs::Quaternion tf2_quat_to_ros_quat(const tf2::Quaternion& q) { + geometry_msgs::Quaternion out; + RotationToMsg(q, out); + return out; +} + +geometry_msgs::Pose tf2_transform_to_ros_pose(const tf2::Transform& p) { + geometry_msgs::Pose transform; + VectorToMsg(p.getOrigin(), transform.position); + transform.orientation = tf2_quat_to_ros_quat(p.getRotation()); + return transform; +} + +geometry_msgs::Pose eigen_transform_to_ros_pose(const Eigen::Affine3d& p) { + geometry_msgs::Pose transform; + transform.position = eigen_to_ros_point(p.translation()); + transform.orientation = eigen_to_ros_quat((Eigen::Quaterniond)p.linear()); + return transform; +} + +geometry_msgs::Transform eigen_transform_to_ros_transform(const Eigen::Affine3d& p) { + geometry_msgs::Transform transform; + transform.translation = eigen_to_ros_vector(p.translation()); + transform.rotation = eigen_to_ros_quat((Eigen::Quaterniond)p.linear()); + return transform; +} + +tf2::Transform ros_tf_to_tf2_transform(const geometry_msgs::Transform& p) { + tf2::Transform transform; + transform.setOrigin(tf2::Vector3( + p.translation.x, + p.translation.y, + p.translation.z)); + transform.setRotation(tf2::Quaternion( + p.rotation.x, + p.rotation.y, + p.rotation.z, + p.rotation.w)); + return transform; +} + +tf2::Transform ros_pose_to_tf2_transform(const geometry_msgs::Pose& p) { + tf2::Transform transform; + transform.setOrigin(tf2::Vector3( + p.position.x, + p.position.y, + p.position.z)); + transform.setRotation(tf2::Quaternion( + p.orientation.x, + p.orientation.y, + p.orientation.z, + p.orientation.w)); return transform; } +Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const std::string& transform_config_name, + const std::string& prefix) { + Eigen::Vector3d body_t_sensor; + Eigen::Quaterniond body_Q_sensor; + if (!msg_conversions::config_read_transform(&config, (prefix + transform_config_name).c_str(), &body_t_sensor, + &body_Q_sensor)) + ROS_FATAL_STREAM("Unspecified transform config: " << prefix + transform_config_name); + Eigen::Isometry3d body_T_sensor = Eigen::Isometry3d::Identity(); + body_T_sensor.translation() = body_t_sensor; + body_T_sensor.linear() = body_Q_sensor.toRotationMatrix(); + return body_T_sensor; +} + +float LoadFloat(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { + float val; + if (!config.GetReal((prefix + config_name).c_str(), &val)) + ROS_FATAL_STREAM("Failed to load " << prefix + config_name); + return val; +} + +double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { + double val; + if (!config.GetReal((prefix + config_name).c_str(), &val)) + ROS_FATAL_STREAM("Failed to load " << prefix + config_name); + return val; +} + +int LoadInt(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { + int val; + if (!config.GetInt((prefix + config_name).c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << prefix + config_name); + return val; +} + +bool LoadBool(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { + bool val; + if (!config.GetBool((prefix + config_name).c_str(), &val)) + ROS_FATAL_STREAM("Failed to load " << prefix + config_name); + return val; +} + +std::string LoadString(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { + std::string val; + if (!config.GetStr((prefix + config_name).c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << prefix + config_name); + return val; +} + +void Load(config_reader::ConfigReader& config, float& val, const std::string& config_name, const std::string& prefix) { + val = LoadFloat(config, config_name, prefix); +} +void Load(config_reader::ConfigReader& config, double& val, const std::string& config_name, const std::string& prefix) { + val = LoadDouble(config, config_name, prefix); +} +void Load(config_reader::ConfigReader& config, int& val, const std::string& config_name, const std::string& prefix) { + val = LoadInt(config, config_name, prefix); +} +void Load(config_reader::ConfigReader& config, bool& val, const std::string& config_name, const std::string& prefix) { + val = LoadBool(config, config_name, prefix); +} +void Load(config_reader::ConfigReader& config, std::string& val, const std::string& config_name, + const std::string& prefix) { + val = LoadString(config, config_name, prefix); +} + +void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Pose& msg_pose) { + RotationToMsg(Eigen::Quaterniond(pose.linear()), msg_pose.orientation); + VectorToMsg(pose.translation(), msg_pose.position); +} + +void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Transform& msg_transform) { + RotationToMsg(Eigen::Quaterniond(pose.linear()), msg_transform.rotation); + VectorToMsg(pose.translation(), msg_transform.translation); +} + +void VariancesToCovDiag(const Eigen::Vector3d& variances, float* const cov_diag) { + cov_diag[0] = variances.x(); + cov_diag[1] = variances.y(); + cov_diag[2] = variances.z(); +} + +Eigen::Vector3d CovDiagToVariances(const float* const cov_diag) { + return Eigen::Vector3d(cov_diag[0], cov_diag[1], cov_diag[2]); +} + +void EigenPoseCovarianceToMsg(const Eigen::Isometry3d& pose, const Eigen::Matrix& covariance, + geometry_msgs::PoseWithCovarianceStamped& pose_cov_msg) { + EigenPoseCovarianceToMsg(pose, covariance, pose_cov_msg.pose); +} + +void EigenPoseCovarianceToMsg(const Eigen::Isometry3d& pose, const Eigen::Matrix& covariance, + geometry_msgs::PoseWithCovariance& pose_cov_msg) { + EigenPoseToMsg(pose, pose_cov_msg.pose); + EigenCovarianceToMsg(covariance, pose_cov_msg.covariance); +} } // end namespace msg_conversions diff --git a/shared/readme.md b/shared/readme.md index 562ca5ad0e..d55f034c78 100644 --- a/shared/readme.md +++ b/shared/readme.md @@ -1,6 +1,9 @@ \page shared Shared \subpage config_reader + \subpage ff_common + \subpage ff_util + \subpage msg_conversions \ No newline at end of file diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt index 7e19c6e741..1ad7b83ee1 100644 --- a/simulation/CMakeLists.txt +++ b/simulation/CMakeLists.txt @@ -15,11 +15,39 @@ # License for the specific language governing permissions and limitations # under the License. +cmake_minimum_required(VERSION 3.0) project(astrobee_gazebo) +if (ENABLE_GAZEBO) +## Compile as C++17 +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + visualization_msgs + tf2 + tf2_ros + tf2_eigen + tf2_msgs + gazebo_ros + sensor_msgs + ff_msgs + ff_hw_msgs + tf2_geometry_msgs + ff_util + pmc + camera + cv_bridge + jsonloader +) + # Find packages find_package(gazebo REQUIRED) +# Find jsoncpp +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../cmake") +find_package(JsonCpp REQUIRED) + # Add gazebo to the link directorries link_directories(${GAZEBO_LIBRARY_DIRS}) @@ -28,18 +56,16 @@ catkin_package( INCLUDE_DIRS include LIBRARIES - gazebo_model_plugin_dock gazebo_model_plugin_truth gazebo_model_plugin_eps gazebo_model_plugin_pmc gazebo_model_plugin_perching_arm gazebo_model_plugin_laser - gazebo_model_plugin_flashlight_front - gazebo_model_plugin_flashlight_aft + gazebo_model_plugin_flashlight gazebo_model_plugin_llp_disk_monitor - gazebo_model_plugin_llp_cpu_monitor + gazebo_model_plugin_llp_cpu_mem_monitor gazebo_model_plugin_mlp_disk_monitor - gazebo_model_plugin_mlp_cpu_monitor + gazebo_model_plugin_mlp_cpu_mem_monitor gazebo_model_plugin_speed_cam gazebo_model_plugin_signal_lights gazebo_sensor_plugin_imu @@ -51,7 +77,6 @@ catkin_package( gazebo_sensor_plugin_perch_cam #gazebo_sensor_plugin_handrail_detect gazebo_sensor_plugin_haz_cam - gazebo_sensor_plugin_sci_cam gazebo_world_plugin_speed gazebo_system_plugin_client CATKIN_DEPENDS @@ -64,512 +89,272 @@ catkin_package( sensor_msgs ff_msgs ff_hw_msgs + tf2_geometry_msgs ff_util + pmc + camera + cv_bridge + jsonloader ) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} + ${JSONCPP_INCLUDE_DIRS} +) + +link_directories(${GAZEBO_LIBRARY_DIRS}) +list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") + # Create reusable classes for models and plugins -create_library( - DIR - src - TARGET - astrobee_gazebo - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - ff_nodelet - INC - ${GAZEBO_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(astrobee_gazebo + src/astrobee_gazebo.cc +) +add_dependencies(astrobee_gazebo ${catkin_EXPORTED_TARGETS}) +target_link_libraries(astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for the EPS system -create_library( - DIR - src/gazebo_model_plugin_eps - TARGET - gazebo_model_plugin_eps - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +add_library(gazebo_model_plugin_eps + src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc ) - -# Create a model plugin for the dock -create_library( - DIR - src/gazebo_model_plugin_dock - TARGET - gazebo_model_plugin_dock - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_dependencies(gazebo_model_plugin_eps ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_eps astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for aerodynamic drag -create_library( - DIR - src/gazebo_model_plugin_drag - TARGET - gazebo_model_plugin_drag - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(gazebo_model_plugin_drag + src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc +) +add_dependencies(gazebo_model_plugin_drag ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_drag astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for the ground truth -create_library( - DIR - src/gazebo_model_plugin_truth - TARGET - gazebo_model_plugin_truth - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(gazebo_model_plugin_truth + src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc +) +add_dependencies(gazebo_model_plugin_truth ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_truth astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for the propulsion system -create_library( - DIR - src/gazebo_model_plugin_pmc - TARGET - gazebo_model_plugin_pmc - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - gnc_autocode - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(gazebo_model_plugin_pmc + src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc +) +add_dependencies(gazebo_model_plugin_pmc ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_pmc astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for the perching arm -create_library( - DIR - src/gazebo_model_plugin_perching_arm - TARGET - gazebo_model_plugin_perching_arm - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(gazebo_model_plugin_perching_arm + src/gazebo_model_plugin_perching_arm/gazebo_model_plugin_perching_arm.cc +) +add_dependencies(gazebo_model_plugin_perching_arm ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_perching_arm astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for the front flashlight -create_library( - DIR - src/gazebo_model_plugin_flashlight_front - TARGET - gazebo_model_plugin_flashlight_front - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +add_library(gazebo_model_plugin_flashlight + src/gazebo_model_plugin_flashlight/gazebo_model_plugin_flashlight.cc ) - -# Create a model plugin for the aft flashlight -create_library( - DIR - src/gazebo_model_plugin_flashlight_aft - TARGET - gazebo_model_plugin_flashlight_aft - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +add_dependencies(gazebo_model_plugin_flashlight ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_flashlight astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) - # Create a model plugin for the laser -create_library( - DIR - src/gazebo_model_plugin_laser - TARGET - gazebo_model_plugin_laser - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +add_library(gazebo_model_plugin_laser + src/gazebo_model_plugin_laser/gazebo_model_plugin_laser.cc +) +add_dependencies(gazebo_model_plugin_laser ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_laser astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) -# Create a model plugin for the LLP CPU monitor -create_library( - DIR - src/gazebo_model_plugin_llp_cpu_monitor - TARGET - gazebo_model_plugin_llp_cpu_monitor - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +# Create a model plugin for the LLP CPU mem monitor +add_library(gazebo_model_plugin_llp_cpu_mem_monitor + src/gazebo_model_plugin_llp_cpu_mem_monitor/gazebo_model_plugin_llp_cpu_mem_monitor.cc +) +add_dependencies(gazebo_model_plugin_llp_cpu_mem_monitor ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_llp_cpu_mem_monitor astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for the LLP disk monitor -create_library( - DIR - src/gazebo_model_plugin_llp_disk_monitor - TARGET - gazebo_model_plugin_llp_disk_monitor - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +add_library(gazebo_model_plugin_llp_disk_monitor + src/gazebo_model_plugin_llp_disk_monitor/gazebo_model_plugin_llp_disk_monitor.cc +) +add_dependencies(gazebo_model_plugin_llp_disk_monitor ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_llp_disk_monitor astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) -# Create a model plugin for the MLP CPU monitor -create_library( - DIR - src/gazebo_model_plugin_mlp_cpu_monitor - TARGET - gazebo_model_plugin_mlp_cpu_monitor - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +# Create a model plugin for the MLP CPU mem monitor +add_library(gazebo_model_plugin_mlp_cpu_mem_monitor + src/gazebo_model_plugin_mlp_cpu_mem_monitor/gazebo_model_plugin_mlp_cpu_mem_monitor.cc +) +add_dependencies(gazebo_model_plugin_mlp_cpu_mem_monitor ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_mlp_cpu_mem_monitor astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) -# Create a model plugin for the MMLP disk monitor -create_library( - DIR - src/gazebo_model_plugin_mlp_disk_monitor - TARGET - gazebo_model_plugin_mlp_disk_monitor - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +# Create a model plugin for the MLP disk monitor +add_library(gazebo_model_plugin_mlp_disk_monitor + src/gazebo_model_plugin_mlp_disk_monitor/gazebo_model_plugin_mlp_disk_monitor.cc +) +add_dependencies(gazebo_model_plugin_mlp_disk_monitor ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_mlp_disk_monitor astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for the speed camera -create_library( - DIR - src/gazebo_model_plugin_speed_cam - TARGET - gazebo_model_plugin_speed_cam - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +add_library(gazebo_model_plugin_speed_cam + src/gazebo_model_plugin_speed_cam/gazebo_model_plugin_speed_cam.cc +) +add_dependencies(gazebo_model_plugin_speed_cam ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_speed_cam astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for the signal lights -create_library( - DIR - src/gazebo_model_plugin_signal_lights - TARGET - gazebo_model_plugin_signal_lights - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_hw_msgs +add_library(gazebo_model_plugin_signal_lights + src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc +) +add_dependencies(gazebo_model_plugin_signal_lights ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_model_plugin_signal_lights astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a sensor plugin for the IMU -create_library( - DIR - src/gazebo_sensor_plugin_imu - TARGET - gazebo_sensor_plugin_imu - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(gazebo_sensor_plugin_imu + src/gazebo_sensor_plugin_imu/gazebo_sensor_plugin_imu.cc +) +add_dependencies(gazebo_sensor_plugin_imu ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_sensor_plugin_imu astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a sensor plugin for the perch camera -create_library( - DIR - src/gazebo_sensor_plugin_perch_cam - TARGET - gazebo_sensor_plugin_perch_cam - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(gazebo_sensor_plugin_perch_cam + src/gazebo_sensor_plugin_perch_cam/gazebo_sensor_plugin_perch_cam.cc +) +add_dependencies(gazebo_sensor_plugin_perch_cam ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_sensor_plugin_perch_cam astrobee_gazebo + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a sensor plugin for the handrail detection algorithm -#create_library( -# DIR -# src/gazebo_sensor_plugin_handrail_detect -# TARGET -# gazebo_sensor_plugin_handrail_detect -# LIBS -# ${GAZEBO_LIBRARIES} -# ${catkin_LIBRARIES} -# astrobee_gazebo -# camera -# INC -# ${GAZEBO_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# DEPS -# ff_util -# ff_msgs -#) +# add_library(gazebo_sensor_plugin_handrail_detect +# src/gazebo_sensor_plugin_handrail_detect/gazebo_sensor_plugin_handrail_detect.cc +# ) +# add_dependencies(gazebo_sensor_plugin_handrail_detect ${catkin_EXPORTED_TARGETS}) +# target_link_libraries(gazebo_sensor_plugin_handrail_detect +# ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} +# ) # Create a sensor plugin for the hazard camera -create_library( - DIR - src/gazebo_sensor_plugin_haz_cam - TARGET - gazebo_sensor_plugin_haz_cam - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(gazebo_sensor_plugin_haz_cam + src/gazebo_sensor_plugin_haz_cam/gazebo_sensor_plugin_haz_cam.cc +) +add_dependencies(gazebo_sensor_plugin_haz_cam ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_sensor_plugin_haz_cam + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a sensor plugin for the nav cam -create_library( - DIR - src/gazebo_sensor_plugin_nav_cam - TARGET - gazebo_sensor_plugin_nav_cam - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_msgs +add_library(gazebo_sensor_plugin_nav_cam + src/gazebo_sensor_plugin_nav_cam/gazebo_sensor_plugin_nav_cam.cc +) +add_dependencies(gazebo_sensor_plugin_nav_cam ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_sensor_plugin_nav_cam + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a sensor plugin for the dock cam -create_library( - DIR - src/gazebo_sensor_plugin_dock_cam - TARGET - gazebo_sensor_plugin_dock_cam - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util +add_library(gazebo_sensor_plugin_dock_cam + src/gazebo_sensor_plugin_dock_cam/gazebo_sensor_plugin_dock_cam.cc ) - -# Create a sensor plugin for the sci cam -create_library( - DIR - src/gazebo_sensor_plugin_sci_cam - TARGET - gazebo_sensor_plugin_sci_cam - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_msgs +add_dependencies(gazebo_sensor_plugin_dock_cam ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_sensor_plugin_dock_cam + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for sparse mapping -create_library( - DIR - src/gazebo_sensor_plugin_sparse_map - TARGET - gazebo_sensor_plugin_sparse_map - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - camera - INC - ${GAZEBO_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_msgs +add_library(gazebo_sensor_plugin_sparse_map + src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc +) +add_dependencies(gazebo_sensor_plugin_sparse_map ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_sensor_plugin_sparse_map + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for AR localization -create_library( - DIR - src/gazebo_sensor_plugin_ar_tags - TARGET - gazebo_sensor_plugin_ar_tags - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - camera - INC - ${GAZEBO_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_msgs +add_library(gazebo_sensor_plugin_ar_tags + src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc +) +add_dependencies(gazebo_sensor_plugin_ar_tags ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_sensor_plugin_ar_tags + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) # Create a model plugin for optical flow -create_library( - DIR - src/gazebo_sensor_plugin_optical_flow - TARGET - gazebo_sensor_plugin_optical_flow - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - astrobee_gazebo - camera - INC - ${GAZEBO_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - DEPS - ff_util - ff_msgs +add_library(gazebo_sensor_plugin_optical_flow + src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc ) - -# Create a model plugin for sparse mapping -create_library( - DIR - src/gazebo_world_plugin_speed - TARGET - gazebo_world_plugin_speed - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +add_dependencies(gazebo_sensor_plugin_optical_flow ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_sensor_plugin_optical_flow + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) +# Create a world plugin for speed +add_library(gazebo_world_plugin_speed + src/gazebo_world_plugin_speed/gazebo_world_plugin_speed.cc +) +add_dependencies(gazebo_world_plugin_speed ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_world_plugin_speed + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} +) -# Create a model plugin for the client gui -create_library( - DIR - src/gazebo_system_plugin_client - TARGET - gazebo_system_plugin_client - LIBS - ${GAZEBO_LIBRARIES} - ${catkin_LIBRARIES} - INC - ${GAZEBO_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +# Create a system plugin for the client gui +add_library(gazebo_system_plugin_client + src/gazebo_system_plugin_client/gazebo_system_plugin_client.cc +) +add_dependencies(gazebo_system_plugin_client ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_system_plugin_client + ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ) +############# +## Install ## +############# -# Install our launch files -install_launch_files() +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) # Also install some other resources install(DIRECTORY worlds DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -catkin_install_python(PROGRAMS scripts/spawn_model scripts/perf_wrapper scripts/start_server scripts/common +install(PROGRAMS scripts/perf_wrapper scripts/start_server scripts/common + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +catkin_install_python(PROGRAMS scripts/spawn_model DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +else (ENABLE_GAZEBO) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (ENABLE_GAZEBO) diff --git a/simulation/advanced_sim_info.md b/simulation/advanced_sim_info.md new file mode 100644 index 0000000000..a6b744430c --- /dev/null +++ b/simulation/advanced_sim_info.md @@ -0,0 +1,248 @@ +\page advanced-sim-info Advanced Simulation Information + +# Rviz in Localization Only Mode + +To run only localization nodes, and visualize localization running on a rosbag containing only imu data and NavCam images, run with the loc_only parameter. This can be done as described below: +1: Move a map file for your chosen world to astrobee/resources/rviz. For example, astrobee/resources/rviz/iss.map +2: Launch rviz using the loc_only command. For example: 'roslaunch astrobee sim.launch default:=false rviz:=true loc_only:=true' +3: In a separate command prompt, launch the astrobee localization software, and provide a global path to your rosbag. For example: 'roslaunch astrobee astrobee.launch robot:=bumble world:=iss loc_only:=true bag:=/path/to/bag/MyBag.bag' +There are several visualizations that can be enabled in the file astrobee/config/localization.config. matched_features_on enables a live camera feed with all sparse mapping features in use overlayed over each frame. all_features_on enables a live camera feed with all detected features in each frame overlayed. map_cloud_on draws the entire stored sparse map in rviz. + +# Collisions and performance + +One of the great sources of computational complexity in simulation is the calculation of collisions between objects. This is especially hard when the collision is a function of two complex meshes. Our simulation optimizes for performance by approximating the Free Flyer's complex meshes with geometric primitives. The Free Flyer collision mesh can be thought of as a sequence of boxes connected by joints, which never self-collide. Collisions are checked between robots and the ISS / Dock meshes, as well as between robots. + +# Frame consistency between simulation and perception + +Every sensor on the Free Flyer has a pose with respect to the body frame, which we generally call the *extrinsics* of the sensor. Having a good estimate of the true extrinsics is required for localization, control, etc. On a real robot we would run a calibration procedure to estimate for these extrinsics. In simulation we control the extrinsics, which is both advantageous and disadvantageous: we can test the effect of extrinsic estimation error, but we have to ensure that there is consistency between the simulation and perception. + +Achieving this consistency is difficult because of technical limitations. There are several options, each having its own challenges: + +1. Inject the correct transforms into the URDF so that the robot is spawned with the extrinsics set correctly -- auto-generation of the URDF is done using xacro. It is difficult to parse LUA variables from within xacro, especially because the robot might only be known at run-time through an environment variable, and not at parse time. +2. Have the plugins read the LUA to extract the transforms -- unfortunately the plugins are loaded into the gazebo server centrally. Since multiple heterogeneous robots might be loaded, the plugin does not know the robot config; it knows the namespace, but not the robot type. +3. Have the plugins listen to TF2 -- since framestore publishes namespace-prefixed transforms for a given Free Flyer, the plugin only needs to know its local frame and parent frame. +4. Just hard-code the transforms into the plugins, or as arguments to the plugins declared in the URDF -- leads to consistency problems. + +We opted for (3) above, because it's relatively easy to implement, maintains consistency between simulation and perception, and integrates well with our existing frame transform engine. However, special care had to be taken with the regular, wide angle and depth cameras. The complexity is handled by the FreeflyerModelPlugin and FreeflyerSensorPlugin abstract classes, provided the correct frame is used as the name for the plugin. + +# Development and debugging + +If you are actively debugging Gazebo plugins then you probably want to be able to launch the server in debug mode with logging enabled. To do this, change the following line in *simulation/launch/start_simulation.launch* from this... + + + +... to this ... + + + +# Changing Simulation properties + +All the simulation physical details are contained in the description folder. + + +**Changing the mass/inertia of the robot** + + description/description/model.urdf.xacro + - Here is the parent file of all details related to the Astrobee's configurations in the simulation. You can change the properties of the base robot. + + description/description/macro_perching_arm.urdf.urdf.xacro + - Here you can change the properties of the arm. + + description/description/model_carriage.urdf.urdf.xacro + - When testing the simulation in the granite table, one can change the carriage properties, namely the kp, kd, mu1, mu2 and fdir + +Customization: mass; origin (center of mass); inertia + +**Changing the sensor's properties** + All the Plugins that are contained in this folder are called from the model .xacro files in description/description, any change in the sensor parameters should be done there (explored further in next section). + +# Under the hood + +There are actually three classes of robot in our world: the ISS, the Dock and the Free Flyer. The first two are simple static meshes, and are inserted once into the simulation so that they show in both rviz and Gazebo. + +Every robot in the system is comprised of links and joints. The description of these links and joints are written as URDF to ROS parameters */dock/robot_description*, */iss/robot_description*, */%ns%/robot_description* for the Dock, ISS and FreeFlyer (where %ns% is the namespace). As its name suggests, writing these descriptions is the responsibility of the \ref description package. There is a good reason for having this done outside of the simulator -- in a real experiment, we might want to use KDL or a robot_state_publisher to use them calculate inertia or transform information. + +**Abstract classes** + +This is the class used for common traits all gazebo plugins have. Each plugin inherits either a *FreeflyerSensorPlugin* or a *FreeflyerModelPlugin*. Those both inherit from freeflyer nodelet so they produce heartbeats like all other freeflyer nodes. Each plugin also has its own queue thread for callbacks, allowing the plugins to subscribe and publish to multiple topics if necessary without any callback issues between plugins all running on the gzserver. + +The extrinsics for each sensor is also setup based on the tf2 transform being published for the frame attached to the sensor in it's URDF. If a frame isn't specified, the sensor name will be used to attempt to find a proper transform. For most sensors the pose from tf2 is directly transferred as the sensor pose. However there is a discrepancy between the pose used for the flight software cameras and the pose used by gazebo for cameras. In the flight software a camera frame is defined by z pointing into the camera frame and x pointing the the right. In gazebo a camera is defined with z pointing up and x pointing into the camera frame. To transform from flight software to gazebo a rotation about x is needed followed by a rotation about z. + +**Dock plugin** + +This plugin does nothing beyond repositioning the dock dynamically in simulation based on the global TF2 transform. + +**EPS plugin** + +This simulates the dock state produced by the actual EPS. It continually checks how close the Free Flyer is to the dock. When the Free Flier moves within a threshold distance, a virtual joint is created to lock the Free Flyer to the dock. The EPS then updates the state from DOCKING to DOCKED similar to the hardware driver. This provides sufficient information for the \ref dock to dock and undock a real or simulated Free Flyer. To test + + rosrun dock dock_tool -ns %ns% -dock + rosrun dock dock_tool -ns %ns% -undock + +Customization: description/description/model_eps.urdf.xacro + rate: Rate at which dock state checked + distance: Threshold distance for magnetism + delay: Delay between contact and docking + +**Flashlight plugin** + +This simulates front and aft facing flashlights. Unfortunately, Gazebo doesn't support model-fixed flashlights. So, for each flashlight a free-floating spot light is created in Gazebo, and the model plugin dynamically positions this light to be rigidly attached to the Free Flyer. To set the brightness of each light you can use the following commands: + + rosservice call /%ns%/hw/flashlight_front/control "brightness: 0" + rosservice call /%ns%/hw/flashlight_aft/control "brightness: 200" + +Customization: description/description/model_flashlights.urdf.xacro + rate: doesn't do anything (should this be removed?) + width: width of the light + height: height of the light + depth: depth of the light + +**Laser plugin** + +To toggle the laser model: + + rosservice call /%ns%/hw/laser/enable "enabled: true" + +Customization: description/description/model_laser.urdf.xacro + rate: doesn't do anything (should this be removed?) + range: range of the laser + width: width of the laser + +**Perching arm plugin** + +The perching arm plugin mimics the real hardware by accepting joint goals and broadcasting joint states to the /joint_goals and /joint_states respectively. In so doing it retains compatibility with the robot_state_publisher, allowing a consistent visualization between Gazebo and rviz. To test: + + rosrun arm arm_tool -ns %ns% -deploy + rosrun arm arm_tool -ns %ns% -pan 45 + rosrun arm arm_tool -ns %ns% -open + rosrun arm arm_tool -ns %ns% -stow + +Customization: description/description/macro_perching_arm.urdf.xacro + rate: rate at which the arm joints are published + proximal: initial position of the proximal joint + distal: initial position of the distal joint + gripper: initial position of the gripper (0 is closed) + +**PMC plugin** + +This plugin subscribes to the PMC command topic, which includes an impeller speed and nozzle apertures. When a new PMC command is received, this information is passed, along with the battery voltage and current velocity, into a dynamic model that converts this information to a force and torque to apply to the Free Flyer. The dynamic model takes into account the delay between spinning the impellers up and down. This force and torque is applied to the Gazebo model, allowing its physics engine to update the state of the Free Flyer. In addition to publishing basic telemetry (current impeller speed, etc) the PMC plugin also tracks the state of the PMC, thereby realistically simulating ramp-up and ramp-down delay. To test: + + rostopic echo /%ns$/hw/pmc/state + rostopic echo /%ns$/hw/pmc/telemetry + +Customization: description/description/macro_pmc.urdf.xacro + rate: rate is not used and the plugin doesn't even read it + bypass_blower_model: whether the commands in ctr are executed directly, don't put true unless there is a reason for it + +**NavCam, DockCam, and SciCam plugins** + +The NavCam and DockCam are forward and aft-facing grayscale fisheye cameras. The SciCam is a forward-facing camera with no distortion and typically of higher resolution than the other two. These plugins copy the image data from the gazebo camera sensor into sensor_msgs::Image messages and publishe them to the to the appropriate ROS topic. To test: + + rostopic hz /%ns$/hw/cam_nav + rostopic hz /%ns$/hw/cam_dock + rostopic hz /%ns$/hw/cam_sci + +Each camera also publishes its pose and intrinsics. Those can be seen, for example for sci cam, as: + + rostopic echo /sim/sci_cam/pose + rostopic echo /sim/sci_cam/info + +By default, the cameras are disabled, for performance reasons. To enable one of more of them, one can edit + + astrobee/config/simulation/simulation.config + +The sci cam is a special case. Even when enabled, so sci_cam_rate is positive, it will publish only camera pose info and no actual images until it receives a custom guest science command on topic /comm/dds/command to do so. (This can be overwritten however by setting sci_cam_continuous_picture_taking in simulation.config to true.) + +If the camera images are incorrect, it can be because the simulator cannot keep up. Then the speed of simulation can be decreased, for example, with: + + speed:=0.5 + +The resolution of NavCam and DockCam is 320 by 240 pixels, as set in sensor_nav_cam.urdf.xacro and sensor_dock_cam.urdf.xacro. The resolution can be increased to 1280 x 960 to agree with the real-world cameras. The SciCam properties can be modified in sensor_sci_cam.urdf.xacro. + +Customization: description/description/sensor_nav_cam.urdf.xacro + description/description/sensor_dock_cam.urdf.xacro + description/description/sensor_sci_cam.urdf.xacro + update_rate: camera update rate + horizontal_fov: horizontal field of view + image>width: image width pixels + image>height: image height pixels + image>format: image format + clip>near nearest capture distance + clip>far: maximum captured distance + distortion>k1: camera distortion + +**HazCam and PerchCam plugins** + +These are forward and aft facing depth cameras. The plugin copies the point cloud data from the gazebo depth camera sensor into a sensor_msgs::PointCloud2 message and publishes it to the appropriate ROS topic. To test: + + rostopic hz /%ns$/hw/depth_haz/points + rostopic hz /%ns$/hw/depth_perch/points + +The haz cam plugin also publishes the haz cam camera pose and image intensity measured at the points in the cloud. + +Customization: description/description/sensor_haz_cam.urdf.xacro + description/description/sensor_perch_cam.urdf.xacro + update_rate: camera update rate + horizontal_fov: horizontal field of view + image>width: image width pixels + image>height: image height pixels + image>format: image format + clip>near nearest capture distance + clip>far: maximum captured distance + distortion>k1: camera distortion + +**IMU plugin** + +On every update of the gazebo world, this plugin obtains the true angular velocity and linear acceleration of the Gazebo IMU sensor and publishes this information as a sensor_msgs::Imu message on the appropriate ROS topic. To test: + + rostopic echo /%ns$/hw/imu + +Customization: description/description/sensor_imu.urdf.xacro + pose: pose of the imu wrt the body reference frame + update_rate: imu update rate + +**Sparse Map plugin** + +This publishes camera registration and feature messages for sparse mapping localization. The plugin constructs a virtual image plane and casts rays through randomly-sampled points on this plane. It then calculates the intersection of these rays with the environment to synthetically produce features (image and world coordinate correspondences). A timer is used to introduce an artificial delay between the registration and feature messages being published to reproduce the latency of actual localization. To test: + + rosrun mobility teleop -ns %ns% -loc ml + rostopic hz /%ns$/loc/ml/registration + rostopic hz /%ns$/loc/ml/features + +**AR Tags plugin** + +This publishes camera registration and feature messages for AR marker tracking localization. The plugin randomly samples coordinates on the AR target centered on the dock. It then back-projects these coordinates to a virtual image plane to obtain image coordinates. A timer is used to introduce an artificial delay between the registration and feature messages being published to reproduce the latency of actual localization. To use this the Free Flyer must be in front a dock, facing away from the dock. To test: + + rosrun mobility teleop -ns %ns% -loc ar + rostopic hz /%ns$/loc/ar/registration + rostopic hz /%ns$/loc/ar/features + +**Ground truth plugin** + +This model plugin publishes the truth values of the pose, twist and acceleration values. These can be used to close the control loop in place of localization, allowing use to separate estimation from control error. + + rosrun mobility teleop -ns %ns% -loc gt + rostopic echo /%ns$/loc/truth/pose + rostopic echo /%ns$/loc/truth/twist + rostopic echo /%ns$/loc/truth/accel + +Customization: description/description/sensor_imu.urdf.xacro + rate: rate of which the ground truth is published + parent: the parent of the ground truth (world) + child: the child of the ground truth, the robot + tf: whether or not to publish the tf pose for visualization + pose: whether or not to publish the pose of the robot + twist: whether or not to publish linear and angular velocity + acc: whether or not to publish linear and angular acceleration + +**Drag plugin** +The drag plugin implements the standard drag formula: D = -0.5 * rho * C_D * area * v * |v| + +Customization: description/description/sensor_imu.urdf.xacro + coefficient: drag coefficient C_D + area: area exposed against the movement, can be approximated + density: air density diff --git a/simulation/include/astrobee_gazebo/astrobee_gazebo.h b/simulation/include/astrobee_gazebo/astrobee_gazebo.h index b9c37328f5..559527d643 100644 --- a/simulation/include/astrobee_gazebo/astrobee_gazebo.h +++ b/simulation/include/astrobee_gazebo/astrobee_gazebo.h @@ -68,7 +68,7 @@ class FreeFlyerPlugin : public ff_util::FreeFlyerNodelet { protected: // Initialize the plugin - void InitializePlugin(std::string const& robot_name); + void InitializePlugin(std::string const& robot_name, std::string const& plugin_name); // Some plugins might want the world as the parent frame void SetParentFrame(std::string const& parent); diff --git a/simulation/launch/start_simulation.launch b/simulation/launch/start_simulation.launch index b4db5e37ce..3bb006f677 100644 --- a/simulation/launch/start_simulation.launch +++ b/simulation/launch/start_simulation.launch @@ -21,6 +21,7 @@ + @@ -35,7 +36,7 @@ + args="-e $(arg physics) $(arg world)" /> diff --git a/simulation/package.xml b/simulation/package.xml index a0e6fa6be0..63fc9f33a2 100644 --- a/simulation/package.xml +++ b/simulation/package.xml @@ -25,7 +25,12 @@ sensor_msgs ff_msgs ff_hw_msgs + tf2_geometry_msgs ff_util + pmc + camera + cv_bridge + jsonloader visualization_msgs tf2 tf2_ros @@ -35,7 +40,12 @@ sensor_msgs ff_msgs ff_hw_msgs + tf2_geometry_msgs ff_util + pmc + camera + cv_bridge + jsonloader diff --git a/simulation/readme.md b/simulation/readme.md index c14205e566..cc18b349e8 100644 --- a/simulation/readme.md +++ b/simulation/readme.md @@ -1,8 +1,8 @@ -\page sim Simulation +\page sim-readme Simulation -This package provides the code required to simulate multiple Free-Flyers operating within the International Space Station (ISS). Our code is written as plugins for Gazebo, an open source robot simulator. In essence the plugins mimic the ros messages and services provided by real hardware, and as a result they act as drop-in replacements for the true hardware. Running a simulation is therefore as simple as loading all flight software subsystems except the hardware drivers, and spawning simulated hardware in stead of real drivers. +This package provides the code required to simulate multiple Free-Flyers operating within the International Space Station (ISS). Our code is written as plugins for Gazebo, an open source robot simulator. In essence the plugins mimic the ros messages and services provided by real hardware, and as a result they act as drop-in replacements for the true hardware. Running a simulation is therefore as simple as loading all flight software subsystems except the hardware drivers, and spawning simulated hardware instead of real drivers. -There is once key exception to the rule above: although we do simulate cameras, we do not simulate any vision-based localization. That is, for sparse mapping, optical flow and AR target tracking, in stead of running the feature detectors and localization algorithms on the images produced by the simulator, we draw features from the intersection of random rays cast through a virtual image plane with the environment. This approach provides immense performance gains that enables us to run simulations at many times wall clock speed. +There is once key exception to the rule above: although we do simulate cameras, we do not simulate any vision-based localization. That is, for sparse mapping, optical flow and AR target tracking, instead of running the feature detectors and localization algorithms on the images produced by the simulator, we draw features from the intersection of random rays cast through a virtual image plane with the environment. This approach provides immense performance gains that enables us to run simulations at many times wall clock speed. At its core, Gazebo provides a headless server which loads a collection of shared library plugins and performs the simulation itself. One can optionally load the client interface which provides a high-fidelity rendering of the world, and an interface that enables basic interaction with the elements. @@ -10,328 +10,17 @@ Note that if the simulation is run any speed over real-time then the NavCam, Doc ## Obtaining media -If you you are not cross-compiling, then the build process started with "make" will download and extract a correct version of the media automatically for you. This is done by the *description* package, as the media is shared between rviz and Gazebo. To see if the media was installed correctly, check that there are subdirectories in the *description/media* directory. +If you you are not cross-compiling, then the build process started with "make" will download and extract a correct version of the media automatically for you. This is done by the *description* package [\ref media], as the media is shared between rviz and Gazebo. To see if the media was installed correctly, check that there are subdirectories in the *description/media* directory. ## Running the Simulator -To launch the simulation without a robot automatically inserted, use the following command: +Please see the \ref running-the-sim documentation. - roslaunch astrobee sim.launch default:=false -Note that the following arguments are also supported: +## Advanced Simulator Information -To increase the simulation speed to 4x: +If you would like more information on running localization only mode, +collisions, performance, frame consistency, debugging, changing properties, or +the Astrobee specific plugins, please see the \ref advanced-sim-info. - speed:=4 - -To open an Rviz GUI: - - rviz:=true - -To open the Gazebo GUI: - - sviz:=true - -To view the GNC GUI: - - gviz:=true - -To view the GDS GUI: - - gds:=true - -To start in a perch ready position: - - perch:=true - -If the launch fails because of DDS-related errors, that functionality can be turned off with: - - dds:=false - -Note that all GUIs have a substantial effect on performance and care should be taken when speeding up the simulation with GUIs enabled. Under certain high-load conditions (many robots and/or a high speed factor) the outgoing heartbeat queue in simulation ends up being too small for the number of messages that must be delivered. The outcome is this error message in your console: - - [ERROR] [] : (ros.Astrobee) Fault with id 11 occurred. Fault error message: Didn't receive a heartbeat from - -You can fix this error by increasing the value of variable "heartbeat_queue_size" in the robot config. For example, if you are simulating the robot *bsharp*, you must open the LUA config file *astrobee/config/robots/bsharp.config*, and increase the value of this line: - - heartbeat_queue_size = - -## Spawning robots in the simulator - -To spawn a robot on the default "/" namespace at a given pose: - - roslaunch astrobee spawn.launch pose:="0 0 4.8 0 0 0 1" - -You can also supply a namespace to the simulation. Note that we currently only support the empty namespace, "bumble", "queen" and "honey". - - roslaunch astrobee spawn.launch ns:=honey pose:="2 0 4.8 0 0 0 1" - roslaunch astrobee spawn.launch ns:=bumble pose:="3 0 4.8 0 0 0 1" - roslaunch astrobee spawn.launch ns:=queen pose:="4 0 4.8 0 0 0 1" - -Each robot's ROS topics, services and actions are now prefixed with the supplied namespace, which allows you to interact with them individually. So, for example, to move bumble to a new position you would do the following: - - rosrun mobility teleop -ns bumble -move -pos "4 0" - -The teleop, arm_tool and dock_tool all support the -ns flag. - -## Auto-spawning robots in a simulation - -In many cases you'll only want a single robot spawned on the root namespace, so that you can interact with it either over GDS or through rviz. To do this, simply enable the debug robot: - - roslaunch astrobee sim.launch default:=true rviz:=true - -The robot will be on the root namespace and placed at a default location. You should see it appear in rviz within a few seconds of launch. - -## Rviz in Localization Only Mode - -To run only localization nodes, and visualize localization running on a rosbag containing only imu data and NavCam images, run with the loc_only parameter. This can be done as described below: -1: Move a map file for your chosen world to astrobee/resources/rviz. For example, astrobee/resources/rviz/iss.map -2: Launch rviz using the loc_only command. For example: 'roslaunch astrobee sim.launch default:=false rviz:=true loc_only:=true' -3: In a separate command prompt, launch the astrobee localization software, and provide a global path to your rosbag. For example: 'roslaunch astrobee astrobee.launch robot:=bumble world:=iss loc_only:=true bag:=/path/to/bag/MyBag.bag' -There are several visualizations that can be enabled in the file astrobee/config/localization.config. matched_features_on enables a live camera feed with all sparse mapping features in use overlayed over each frame. all_features_on enables a live camera feed with all detected features in each frame overlayed. map_cloud_on draws the entire stored sparse map in rviz. - -## Collisions and performance - -One of the great sources of computational complexity in simulation is the calculation of collisions between objects. This is especially hard when the collision is a function of two complex meshes. Our simulation optimizes for performance by approximating the Free Flyer's complex meshes with geometric primitives. The Free Flyer collision mesh can be thought of as a sequence of boxes connected by joints, which never self-collide. Collisions are checked between robots and the ISS / Dock meshes, as well as between robots. - -## Frame consistency between simulation and perception - -Every sensor on the Free Flyer has a pose with respect to the body frame, which we generally call the *extrinsics* of the sensor. Having a good estimate of the true extrinsics is required for localization, control, etc. On a real robot we would run a calibration procedure to estimate for these extrinsics. In simulation we control the extrinsics, which is both advantageous and disadvantageous: we can test the effect of extrinsic estimation error, but we have to ensure that there is consistency between the simulation and perception. - -Achieving this consistency is difficult because of technical limitations. There are several options, each having its own challenges: - -1. Inject the correct transforms into the URDF so that the robot is spawned with the extrinsics set correctly -- auto-generation of the URDF is done using xacro. It is difficult to parse LUA variables from within xacro, especially because the robot might only be known at run-time through an environment variable, and not at parse time. -2. Have the plugins read the LUA to extract the transforms -- unfortunately the plugins are loaded into the gazebo server centrally. Since multiple heterogeneous robots might be loaded, the plugin does not know the robot config; it knows the namespace, but not the robot type. -3. Have the plugins listen to TF2 -- since framestore publishes namespace-prefixed transforms for a given Free Flyer, the plugin only needs to know its local frame and parent frame. -4. Just hard-code the transforms into the plugins, or as arguments to the plugins declared in the URDF -- leads to consistency problems. - -We opted for (3) above, because it's relatively easy to implement, maintains consistency between simulation and perception, and integrates well with our existing frame transform engine. However, special care had to be taken with the regular, wide angle and depth cameras. The complexity is handled by the FreeflyerModelPlugin and FreeflyerSensorPlugin abstract classes, provided the correct frame is used as the name for the plugin. - -## Development and debugging - -If you are actively debugging Gazebo plugins then you probably want to be able to launch the server in debug mode with logging enabled. To do this, change the following line in *simulation/launch/start_simulation.launch* from this... - - - -... to this ... - - - -## Changing Simulation properties - -All the simulation physical details are contained in the description folder. - - -**Changing the mass/inertia of the robot** - - description/description/model.urdf.xacro - - Here is the parent file of all details related to the Astrobee's configurations in the simulation. You can change the properties of the base robot. - - description/description/macro_perching_arm.urdf.urdf.xacro - - Here you can change the properties of the arm. - - description/description/model_carriage.urdf.urdf.xacro - - When testing the simulation in the granite table, one can change the carriage properties, namely the kp, kd, mu1, mu2 and fdir - -Customization: mass; origin (center of mass); inertia - -**Changing the sensor's properties** - All the Plugins that are contained in this folder are called from the model .xacro files in description/description, any change in the sensor parameters should be done there (explored further in next section). - - - -## Under the hood - -There are actually three classes of robot in our world: the ISS, the Dock and the Free Flyer. The first two are simple static meshes, and are inserted once into the simulation so that they show in both rviz and Gazebo. - -Every robot in the system is comprised of links and joints. The description of these links and joints are written as URDF to ROS parameters */dock/robot_description*, */iss/robot_description*, */%ns%/robot_description* for the Dock, ISS and FreeFlyer (where %ns% is the namespace). As its name suggests, writing these descriptions is the responsibility of the \ref description package. There is a good reason for having this done outside of the simulator -- in a real experiment, we might want to use KDL or a robot_state_publisher to use them calculate inertia or transform information. - -**Abstract classes** - -This is the class used for common traits all gazebo plugins have. Each plugin inherits either a *FreeflyerSensorPlugin* or a *FreeflyerModelPlugin*. Those both inherit from freeflyer nodelet so they produce heartbeats like all other freeflyer nodes. Each plugin also has its own queue thread for callbacks, allowing the plugins to subscribe and publish to multiple topics if necessary without any callback issues between plugins all running on the gzserver. - -The extrinsics for each sensor is also setup based on the tf2 transform being published for the frame attached to the sensor in it's URDF. If a frame isn't specified, the sensor name will be used to attempt to find a proper transform. For most sensors the pose from tf2 is directly transferred as the sensor pose. However there is a discrepancy between the pose used for the flight software cameras and the pose used by gazebo for cameras. In the flight software a camera frame is defined by z pointing into the camera frame and x pointing the the right. In gazebo a camera is defined with z pointing up and x pointing into the camera frame. To transform from flight software to gazebo a rotation about x is needed followed by a rotation about z. - -**Dock plugin** - -This plugin does nothing beyond repositioning the dock dynamically in simulation based on the global TF2 transform. - -**EPS plugin** - -This simulates the dock state produced by the actual EPS. It continually checks how close the Free Flyer is to the dock. When the Free Flier moves within a threshold distance, a virtual joint is created to lock the Free Flyer to the dock. The EPS then updates the state from DOCKING to DOCKED similar to the hardware driver. This provides sufficient information for the \ref dock behavior to dock and undock a real or simulated Free Flyer. To test - - rosrun dock dock_tool -ns %ns% -dock - rosrun dock dock_tool -ns %ns% -undock - -Customization: description/description/model_eps.urdf.xacro - rate: Rate at which dock state checked - distance: Threshold distance for magnetism - delay: Delay between contact and docking - -**Flashlight plugin** - -This simulates front and aft facing flashlights. Unfortunately, Gazebo doesn't support model-fixed flashlights. So, for each flashlight a free-floating spot light is created in Gazebo, and the model plugin dynamically positions this light to be rigidly attached to the Free Flyer. To set the brightness of each light you can use the following commands: - - rosservice call /%ns%/hw/light_front/control "brightness: 0" - rosservice call /%ns%/hw/light_aft/control "brightness: 200" - -Customization: description/description/model_flashlights.urdf.xacro - rate: doesn't do anything (should this be removed?) - width: width of the light - height: height of the light - depth: depth of the light - -**Laser plugin** - -To toggle the laser model: - - rosservice call /%ns%/hw/laser/enable "enabled: true" - -Customization: description/description/model_laser.urdf.xacro - rate: doesn't do anything (should this be removed?) - range: range of the laser - width: width of the laser - -**Perching arm plugin** - -The perching arm plugin mimics the real hardware by accepting joint goals and broadcasting joint states to the /joint_goals and /joint_states respectively. In so doing it retains compatibility with the robot_state_publisher, allowing a consistent visualization between Gazebo and rviz. To test: - - rosrun arm arm_tool -ns %ns% -deploy - rosrun arm arm_tool -ns %ns% -pan 45 - rosrun arm arm_tool -ns %ns% -open - rosrun arm arm_tool -ns %ns% -stow - -Customization: description/description/macro_perching_arm.urdf.xacro - rate: rate at which the arm joints are published - proximal: initial position of the proximal joint - distal: initial position of the distal joint - gripper: initial position of the gripper (0 is closed) - -**PMC plugin** - -This plugin subscribes to the PMC command topic, which includes an impeller speed and nozzle apertures. When a new PMC command is received, this information is passed, along with the battery voltage and current velocity, into a dynamic model that converts this information to a force and torque to apply to the Free Flyer. The dynamic model takes into account the delay between spinning the impellers up and down. This force and torque is applied to the Gazebo model, allowing its physics engine to update the state of the Free Flyer. In addition to publishing basic telemetry (current impeller speed, etc) the PMC plugin also tracks the state of the PMC, thereby realistically simulating ramp-up and ramp-down delay. To test: - - rostopic echo /%ns$/hw/pmc/state - rostopic echo /%ns$/hw/pmc/telemetry - -Customization: description/description/macro_perching_arm.urdf.xacro - rate: rate is not used and the plugin doesn't even read it - bypass_blower_model: whether the commands in ctr are executed directly, don't put true unless there is a reason for it - -**NavCam, DockCam, and SciCam plugins** - -The NavCam and DockCam are forward and aft-facing grayscale fisheye cameras. The SciCam is a forward-facing camera with no distortion and typically of higher resolution than the other two. These plugins copy the image data from the gazebo camera sensor into sensor_msgs::Image messages and publishe them to the to the appropriate ROS topic. To test: - - rostopic hz /%ns$/hw/cam_nav - rostopic hz /%ns$/hw/cam_dock - rostopic hz /%ns$/hw/cam_sci - -Each camera also publishes its pose and intrinsics. Those can be seen, for example for sci cam, as: - - rostopic echo /sim/sci_cam/pose - rostopic echo /sim/sci_cam/info - -By default, the cameras are disabled, for performance reasons. To enable one of more of them, one can edit - - astrobee/config/simulation/simulation.config - -The sci cam is a special case. Even when enabled, so sci_cam_rate is positive, it will publish only camera pose info and no actual images until it receives a custom guest science command on topic /comm/dds/command to do so. (This can be overwritten however by setting sci_cam_continuous_picture_taking in simulation.config to true.) - -If the camera images are incorrect, it can be because the simulator cannot keep up. Then the speed of simulation can be decreased, for example, with: - - speed:=0.5 - -The resolution of NavCam and DockCam is 320 by 240 pixels, as set in sensor_nav_cam.urdf.xacro and sensor_dock_cam.urdf.xacro. The resolution can be increased to 1280 x 960 to agree with the real-world cameras. The SciCam properties can be modified in sensor_sci_cam.urdf.xacro. - -Customization: description/description/sensor_nav_cam.urdf.xacro - description/description/sensor_dock_cam.urdf.xacro - description/description/sensor_sci_cam.urdf.xacro - update_rate: camera update rate - horizontal_fov: horizontal field of view - image>width: image width pixels - image>height: image height pixels - image>format: image format - clip>near nearest capture distance - clip>far: maximum captured distance - distortion>k1: camera distortion - -**HazCam and PerchCam plugins** - -These are forward and aft facing depth cameras. The plugin copies the point cloud data from the gazebo depth camera sensor into a sensor_msgs::PointCloud2 message and publishes it to the appropriate ROS topic. To test: - - rostopic hz /%ns$/hw/depth_haz/points - rostopic hz /%ns$/hw/depth_perch/points - -The haz cam plugin also publishes the haz cam camera pose and image intensity measured at the points in the cloud. - -Customization: description/description/sensor_haz_cam.urdf.xacro - description/description/sensor_perch_cam.urdf.xacro - update_rate: camera update rate - horizontal_fov: horizontal field of view - image>width: image width pixels - image>height: image height pixels - image>format: image format - clip>near nearest capture distance - clip>far: maximum captured distance - distortion>k1: camera distortion - -**IMU plugin** - -On every update of the gazebo world, this plugin obtains the true angular velocity and linear acceleration of the Gazebo IMU sensor and publishes this information as a sensor_msgs::Imu message on the appropriate ROS topic. To test: - - rostopic echo /%ns$/hw/imu - -Customization: description/description/sensor_imu.urdf.xacro - pose: pose of the imu wrt the body reference frame - update_rate: imu update rate - -**Sparse Map plugin** - -This publishes camera registration and feature messages for sparse mapping localization. The plugin constructs a virtual image plane and casts rays through randomly-sampled points on this plane. It then calculates the intersection of these rays with the environment to synthetically produce features (image and world coordinate correspondences). A timer is used to introduce an artificial delay between the registration and feature messages being published to reproduce the latency of actual localization. To test: - - rosrun mobility teleop -ns %ns% -loc ml - rostopic hz /%ns$/loc/ml/registration - rostopic hz /%ns$/loc/ml/features - -**AR Tags plugin** - -This publishes camera registration and feature messages for AR marker tracking localization. The plugin randomly samples coordinates on the AR target centered on the dock. It then back-projects these coordinates to a virtual image plane to obtain image coordinates. A timer is used to introduce an artificial delay between the registration and feature messages being published to reproduce the latency of actual localization. To use this the Free Flyer must be in front a dock, facing away from the dock. To test: - - rosrun mobility teleop -ns %ns% -loc ar - rostopic hz /%ns$/loc/ar/registration - rostopic hz /%ns$/loc/ar/features - -**Ground truth plugin** - -This model plugin publishes the truth values of the pose, twist and acceleration values. These can be used to close the control loop in place of localization, allowing use to separate estimation from control error. - - rosrun mobility teleop -ns %ns% -loc gt - rostopic echo /%ns$/loc/truth/pose - rostopic echo /%ns$/loc/truth/twist - rostopic echo /%ns$/loc/truth/accel - -Customization: description/description/sensor_imu.urdf.xacro - rate: rate of which the ground truth is published - parent: the parent of the ground truth (world) - child: the child of the ground truth, the robot - tf: whether or not to publish the tf pose for visualization - pose: whether or not to publish the pose of the robot - twist: whether or not to publish linear and angular velocity - acc: whether or not to publish linear and angular acceleration - -**Drag plugin** -The drag plugin implements the standard drag formula: D = -0.5 * rho * C_D * area * v * |v| - -Customization: description/description/sensor_imu.urdf.xacro - coefficient: drag coefficient C_D - area: area exposed against the movement, can be approximated - density: air density - - - -\subpage sim_overview -\subpage sim_issues +For common simulation issues and workarounds, check \subpage sim-issues diff --git a/simulation/running_the_sim.md b/simulation/running_the_sim.md new file mode 100644 index 0000000000..e503a98c54 --- /dev/null +++ b/simulation/running_the_sim.md @@ -0,0 +1,380 @@ +\page running-the-sim Running the Astrobee Simulator + +This documentation assumes you have followed the \ref install-nonNASA or the +\ref install-NASA documentation. + +## Communication Nodes + +If you are an external user and would like to install and run the Astrobee +communication nodes, please contact your Astrobee point of contact. + +## Setting up your Environment + +You will need to setup your environment so that ROS knows about the new packages +provided by Astrobee flight software: + + pushd $BUILD_PATH + source devel/setup.bash + popd + +In case of a Zsh session, do + + pushd $BUILD_PATH + source devel/setup.zsh + popd + +After this command has completed, you should be able to run `ros` commands from +any directory in your Linux filesystem. + +## Running the Simulator + +To start a simulation of a single Astrobee docked in the ISS, run one of the +following: + +If you don't have the Astrobee communication nodes compiled or installed, +please run: + + roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true + +If you have the Astrobee communication nodes compiled or installed, please run: + + roslaunch astrobee sim.launch rviz:=true + +- Note 1: The very first time Gazebo starts, it could take a long time because + it loads some large models +- Note 2: To run the simulation with the Gazebo rendering, you can add the flag + `sviz:=true`. This allows one to see the ISS module used for the simulation. + However this is resource intensive. + +This command tells ROS to look for the `sim.launch` file provided by the +`astrobee` package, and use roslaunch to run it. Internally, ROS maintains a +cache of information about package locations, libraries and executables. If you +find that the above command doesn't work, try rebuilding the cache: + + rospack profile + +This command launches both the world and Astrobee. There are things in the +world that need to be started up before Astrobee is started up. There is a +small chance that this will not happen and the user will experience issues upon +start up. If this happens, please see the 'Issues Starting up the Simulator' +section in \ref sim-issues. + +**Important**: If you experience issues with the simulator and commanding the +robot, please see the \ref sim-issues before contacting the Astrobee team. + +### Simulator Flags + +There are many options when launching the simulator. This section will take you +through the most used options. If you are happy with the simulator setup from +above, you can skip this section and jump to the +[Moving the Robot](#moving-the-robot) section. + +**Important**: You can specify more than one flag meaning you could launch more +than one of the GUIs we offer. The simulator is very resource intensive. The +more GUIs running, the more likely it is that you will get tolerance violation +errors when you try to move. If you are running a virtual machine, please make +sure that it has access to your graphics card. + +#### pose + +This is the pose/location that Astrobee starts up at. The first 3 values are +position and the last 4 are the orientation expressed as a quaternion. By +default, Astrobee starts up docked in the JEM module of the ISS. To start up + + roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true pose:="11.25 -6.95 4.49 0 0 0 1" + +*Note: If you have the Astrobee communication nodes compiled or installed, +please omit the dds and robot flags.* + +#### gds + +This flag is used to start up the ground data system (GDS). For the flag to +work, gds must be setup in a certain location. See the +[Ground Data System](#ground-data-system) section for more about GDS and +instructions on how to set it up. Please note that GDS will be unable to connect +to and command Astrobee if the Astrobee communication nodes are not running. +Also note, GDS is the least computationally intensive GUI out of the 3 we offer +and it is the only GUI that allows users to send commands. Thus, it may be a +convenient tool for external users to use. + + roslaunch astrobee sim.launch gds:=true + +*Note: If you don't have the Astrobee communication nodes compiled or +installed, the roslaunch above will throw a dds ros bridge fault and you will +not be able to move Astrobee.* + +#### rviz + +This flag is used to start up the ros visualization system also known as RVIZ. +RVIZ is our second most computationally intensive GUI. + +*Note: `rviz` displays the robot perception of the world (estimated pose, sensor +measurements, etc.).* + +#### sviz + +This flag is used to start up the Gazebo GUI. This GUI is the most +computationally intensive GUI we have. We recommend using this GUI if you have a +high performance machine. + +*Note: `sviz` displays the model of the world (ground truth). This means that +the position represented in rviz or gds can differ from the position in sviz.* + +#### gviz + +This flag starts the gnc visualizer. This visualizer is mainly used to see how +the localization and gnc systems are performing. For more information no gviz, +please see the \ref gncvisualizer documentation. + +#### dds + +This flag is used to start the Astrobee communication nodes. There are two +communication nodes in the Astrobee flight software: the dds ros bridge and the +astrobee to astrobee communication node. If you are unable to compile or install +the Astrobee communication nodes, please set dds to 'false' and set robot to +'sim_pub'. + +#### speed + +This flag is the speed multiplier for the simulator. By default, it is set to 1 +meaning the simulator runs in real time. If you are having issues with simulator +performance, you can slow the simulator down by specifying a value less than +one. For example, the following would slow the simulator down to half speed: + + roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true speed:=0.5 + +*Note: If you have the Astrobee communication nodes compiled or installed, +please omit the dds and robot flags.* + +If you have a high performance machine, you can speed up the simulator by +specifying a value greater than 1. Please note, if you specify a speed up +greater than what your computer can handle, the simulator will not run at the +speed you specified. In this case, you can run gz stats in another terminal +to figure out what the actual speed up is. If the factor in gz stats doesn't +match the speed you specified, you can expect weird simulator issues. + +Also note that all GUIs have a substantial effect on performance and care should +be taken when speeding up the simulation with GUIs enabled. Under certain +high-load conditions (many robots and/or a high speed factor) the outgoing +heartbeat queue in simulation ends up being too small for the number of +messages that must be delivered. The outcome is this error message in your +console: + + [ERROR] [] : (ros.Astrobee) Fault with id # occurred. Fault error message: Didn't receive a heartbeat from + +Please see the 'Multiple Heartbeat Failures' section in \ref sim-issues on how +to fix this issue. + +#### ns + +This flag is the namespace of the simulated Astrobee. If you want to know more +about ROS namespaces, please see the ROS documentation. This flag is only needed +when running multiple robots in simulation. Please see the +[Launching Multiple Robots](#launching-multiple-robots) section for more +information. The default namespace is blank. + +#### robot + +This flag tells the flight software which configuration file to read. If you +don't have the Astrobee communication nodes compiled or installed, you need to +use the sim_pub configuration so that the system doesn't complain about the +missing bridge. Otherwise, we recommend not setting this flag because the +configuration for each Astrobee is specific to the robot's hardware and it is +unknown if the hardware configuration will break something in simulation. If you +need to run multiple robots in simulation, please leave the robot flag set to +sim_pub or the default, sim, and instead set the namespace flag when spawning +a robot. + +#### default_robot + +This flag tells ROS to launch the default robot into the world. If you are +having performance issues starting up the simulator, you may want to launch the +world without the default robot and then spawn the robot after the simulator +starts up. For more information on this, please see the 'Issues Starting up the +Simulator' section in \ref sim-issues. To launch the world without the default +robot, please run: + + roslaunch astrobee sim.launch default_robot:=false + +#### perch + +This flag tells the simulation to start Astrobee in a perch ready position. +After the simulation has started up, you should be able to issue a perch command +and Astrobee will perch on a handrail on the deck. + +#### world + +This flag tells ROS which world to launch. There are 2 Astrobee worlds; the ISS +and the granite lab (a model of the actual testbed at Ames). By default, this is +set to the ISS world. If you want to test in the granite lab, launch the +simulation using the following: + + roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true world:="granite" + +*Note: If you have the Astrobee communication nodes compiled or installed, +please omit the dds and robot flags.* + +#### debug + +The Astrobee flight software is still in development and there are bugs that we +don't always catch. If you come across a bug that crashes the flight software, +please help us fix it. To help us, we need to know what happened. The crashed +output should state the node that caused the crash. If it isn't clear which node +it was, please email us the output and we can help. Once you have the name of +the node, please set the debug flag to the node name. This flag will cause a GNU +Debugger(GDB) terminal to pop up. Once the code crashes, navigate to the GDB +terminal, type `bt`, and hit enter. This should display a backtrace. Please copy +the backtrace and send it to us. Here is an example of launching the simulator +and debugging the executive node: + + roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true debug:="executive" + +*Note: If you have the Astrobee communication nodes compiled or installed, +please omit the dds and robot flags.* + +## Moving the Robot + +There are currently 3 ways to move the robot in simulation: using the Ground +Data System gui, using the command line teleop tool, or programmatically by +creating a guest science application that uses the astrobee api library. + +### Ground Data System + +The Ground Data System or GDS is our control station. It sends commands to +Astrobee and displays telemetry from Astrobee. Using GDS is the preferred way to +for an operator to operate Astrobee. Please follow one of the two sections based +on the type of user you are. + +#### External Users + +You can find the GDS software on github in the +[astrobee_gds repo](github.com/nasa/astrobe_gds). If you want to compile and run +GDS, you will need to aquire the RTI libraries. If you just want to run GDS, we +are able to distribute the GDS executable. You will also need to have the +Astrobee communication nodes compiled or installed for GDS to communicate with +Astrobee. As with GDS, you need the RTI libraries in order to compile the +Astrobee communication nodes but we are able to distribute the communications +debians that allow you to install and run the Astrobee communication nodes +without compiling the code. Please contact your Astrobee point of contact if +you would like the GDS executable and/or the Astrobee communications debians. + +Our script to launch GDS expects GDS to be in a certain location on your +machine. Thus, if you would like to use simulator GDS flag, please make a +folder in your home directory called GDS and in that directory, make a symbolic +link called 'latest' to the unextracted gds folder you received from your +Astrobee point of contact. If you want, you can put the unextracted folder in +the gds folder you created. Here's a rough idea of what to do: + + mkdir ~/gds + pushd ~/gds + unzip ~/Downloads/AstroBeeWB.rbuild#.date-time-linux.gtk.x86_64.zip + ln -s AstroBeeWB.rbuild#.date-time latest + popd + +#### Internal Users + +If you are interested in running GDS, please follow the +[Running GDS in Linux](https://babelfish.arc.nasa.gov/confluence/display/FFFSW/Running+GDS+in+Linux) +page on the FreeFlyer FSW Confluence space. + +If you plan on working on GDS code, please checkout the bitbucket +[astrobee_gds repo](ssh://git@babelfish.arc.nasa.gov:7999/astrobee/astrobee_gds.git) +and follow the SETUP documentation. If you followed the NASA Astrobee install +instructions, you will have the RTI libraries already installed. Please make +sure you set the LD_LIBRARY_PATH to the correct RTI location when following the +'Run the Control Station for the first time' section. + +### Teleop Tool + +A teleop tool has been added to allow you to issue some of the basic Astrobee +commands and get a small set of telemetry from the robot. This should only be +used to play with the simulator and get a feel for how Astrobee works. As an +external user, we consider you to be a guest scientist and we expect you to +follow the guest science framework we have created. Please see the +[Guest Science Applications](#guest-science-applications) section for more +information. + +For information on how to use the teleop tool, please see the +\ref teleop. + +### Guest Science Applications + +If you are a guest scientist, please make sure you have the astrobee_android +NASA GitHub project checked out (if you followed the usage instructions, you +should have checked this out already). For more information on guest science +and how to create guest science applications, please see +[`astrobee_android/readme.md`](https://github.com/nasa/astrobee_android/blob/master/README.md) +located in the `astrobee_android/` folder. + +## Images + +When running RVIZ, you may notice the big "No Image" text in the two +`DEBUG` windows that are meant to show camera information. This is +because, by default, we don't publish simulated camera images for the +navigation and dock cameras due to the fact that simulating these +images is very resource intensive. If you do want to generate these +images, please open `astrobee/config/simulation/simulation.config` and +change one or more of `nav_cam_rate` and `dock_cam_rate` to something +other than 0. We recommend setting these values to 1 meaning these +images will be published once per second. After this, you should see +those images when you start the simulator. Please note this only works +when the namespace is blank. In other words, please don't use the ns flag when +launching the simulator if you want to see the images. To show sci camera +images, modify analogously the `sci_cam_rate` field, and enable this camera +from the `Panels` menu in RVIZ. + +**Important** Unless you have a high performance machine, there is a good chance +that these images will not initialize properly and you will be looking at the +inside of the robot. If this happens, you will need to launch the world and +Astrobee separately. Please follow the issues starting up the simulator section +in \ref sim-issues with one caveat. DO NOT start up Astrobee (i.e. don't issue +the second terminal command) until 30 to 40 seconds after RVIZ has started up. +If you have a low performance computer, this fix may not be the only thing you +have to do to fix the images. Please try reducing the speed of your simulator +as well. For information on how to reduce the simulator speed, please see the +[speed](#speed) flag section. + +## Launching Multiple Robots + +The Astrobee simulator supports running up to three robots. Please keep in mind +that each simulated robot takes computer resources and can cause simulator +issues if your machine cannot support the extra load. + +The robot namespaces we support are "bumble", "queen", and "honey". Please DO +NOT capitalize the namespace. When starting the simulator, you can either start +with no robots or you can start with the default robot and one of the +namespaces. To start the simulator without a robot, please run: + + roslaunch astrobee sim.launch default_robot:=false + +To start with the default robot using a namespace, please run: + + roslaunch astrobee sim.launch ns:=honey dds:=false robot:=sim_pub + +To spawn robots into the simulator, run: + + roslaunch astrobee spawn.launch ns:=bumble dds:=false robot:=sim_pub pose:="11 -7 4.5 0 0 0 1" + roslaunch astrobee spawn.launch ns:=queen dds:=false robot:=sim_pub pose:="11 -4 4.5 0 0 0 1" + +*Note: If you have the Astrobee communication nodes compiled or installed, +please omit the dds and robot flags.* + +**Important** If you issue the roslaunch commands too close together, the robot +may not start up properly. Please allow 15 to 20 seconds in between each +roslaunch command. + +Each robot's ROS topics, services and actions are now prefixed with the supplied +namespace, which allows you to interact with them individually. So, if you are +using the teleop tool to command the robots, be sure to specify the namespace of +the robot you want to command. For example: + + rosrun executive teleop_tool -ns "honey" -undock + +*Note: The simulator and teleop tool may be able to support more than 3 robots +if needed. However, GDS only supports 3 robots so the documentation limits +spawning to 3. We will leave it as an exercise for the user to launch more than +3 robots.* + +# Simulator Issues + +Please see \ref sim-issues. diff --git a/simulation/scripts/spawn_model b/simulation/scripts/spawn_model index 340920b586..359ea73147 100755 --- a/simulation/scripts/spawn_model +++ b/simulation/scripts/spawn_model @@ -18,22 +18,25 @@ # Author: John Hsu, Dave Coleman # -import rospy, sys, os, time +import os +import re import string +import sys +import time import warnings -import re - -from gazebo_ros import gazebo_interface +import rospy +import tf.transformations as tft from gazebo_msgs.msg import * from gazebo_msgs.srv import * -from std_srvs.srv import Empty +from gazebo_ros import gazebo_interface from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench -import tf.transformations as tft +from std_srvs.srv import Empty + def excepthook(*args): - print >> STDERR, 'caught' - print >> STDERR, args + print('caught', STDERR) + print(args, STDERR) sys.excepthook = excepthook @@ -46,7 +49,7 @@ model_database_template = """ """ def usage(): - print '''Commands: + print('''Commands: -[urdf|sdf|trimesh|gazebo] - specify incoming xml is urdf, sdf or trimesh format. gazebo arg is deprecated in ROS Hydro -[file|param|database] [||] - source of the model xml or the trimesh file -model - name of the model to be spawned. @@ -67,7 +70,7 @@ def usage(): -joint - optional: initialize the specified joint at the specified value -package_to_model - optional: convert urdf + +## Command Fails due to Robot State -This is not an error in our system. Please see the [Images](#images) section -for more information. +Astrobee only excepts a subset of commands in each operating and mobility state. +If you are using GDS, the mobility and operating states will be displayed in a +health section/subtab at the top left corner of most of the main tabs. If you +are using the teleop tool, please see the 'Get State' section in \ref teleop. -### Heartbeat Fault Detected +A general rule of thumb is Astrobee will not allow mobility commands or commands +that set mobility properties when executing a mobility command. For instance, +Astrobee will reject a move command when it is executing an undock command and +it will reject a set operating limits command when executing a move command. In +these instances, you can either wait for the command to complete or issue a stop +command which cancels the mobility command being executed. -### Command Fails due to Robot State -Cannot move when in ready and not stopped -Command not accepted in op state teleop +Futher, Astrobee accepts a limited set of commands when in the fault operating +state. If you are using the teleop tool, please see the 'Get Faults' section of +the \ref teleop to get the list of occurring faults. If you are using GDS, +please navigate to the 'Engineering' tab. The faults should be listed in the +bottom left corner. Most times a flight software/simulator restart will get +rid of the faults. However, if you are repeatedly getting heartbeat faults, +please see the [Heartbeat Fault Detected](#heartbeat-fault-detected) section. -### Tolerance Violated Errors -use gz stats to make sure you are running at your speed up factor +## Tolerance Violated Errors -### Keep in Zone Violation -Make sure the position you are trying to get to is in the world +Most of the time tolerance violated errors occur due to computer performance. To +see if this is the case, please run *gz stats* in a terminal that has its +environment setup while the simulator is running. If the factor doesn't roughly +match the speed you are running the simulator at, you will have to reduce the +speed of the simulator. Please see the 'speed' section in \ref running-the-sim +on how to change the simulator speed. In some cases, this means you will have to +run the simulator at a speed slower than real time. -### Unable to Plan a Segment -The only time we have seen this happen is when the op limits are not set -correctly. make sure your limits are correct +## Keep in Zone Violation -### Couldn't Configure the Mobility Node -Wait a little longer before trying to move robot +If you get a keep in zone violation failure when trying to move, please make +sure the position you are trying to get to is in the world Astrobee is operating +in. For reference, the point (0, 0, -0.67) is in the middle of the granite table +in the granite world but it is not inside the ISS in the ISS world. If you are +using the teleop tool, please see the 'Get Position' section in \ref teleop to +get the current positon of the robot. If you are using GDS, you can get the +current position by navigating to the 'Teleoperate' tab and clicking on the +'Snap Preview to Bee' buttonb. This will change the manual inputs to match the +current position of Astrobee. -### GDS Node - Process has Died -You don't have GDS as of yet, don't set the the gds flag to false +## Unable to Plan a Segment -### The robot is spinning, flying higher than the world, or jumping around erratically -need to reset ekf or initialize bias, see teleop section +The only time we have seen this happen is when the operating limits are not set +correctly. Please make sure the operating limits are within Astrobee's +capabilities. A flight software/simulator restart will reset the operating +limits back to the defaults. +## Couldn't Configure the Mobility Node -### Teleop Tool reports no publisher for acks topic -make sure the namespace matches the namespace you launched with +This error usually occurs when an operator issues a move command before the +flight software has finished starting up. Please wait a little longer after +starting the simulator before trying to issue a move command. + +## Could Not Query the Pose of Robot + +This error usually occurs when an operator issues a move command before the +flight software has finished starting up. Please wait a little longer after +starting the simulator before trying to issue a command. + +## Dock Action Server Not Connected + +This error usually occurs when an operator issues an undock command before the +flight software has finished starting up. Please wait a little longer after +starting the simulator before trying to issue a command. + +## GDS Node - Process has Died + +This error occurs if the gds flag was set to true but the Astrobee launch GDS +script is unable to find and start GDS. Please ensure that you have GDS and that +you have followed the GDS setup instructions in section 'Ground Data System' of +\ref running-the-sim. + +## The Robot is Spinning, Flying Higher Than the World, or Jumping Around Erratically + +To fix any of these issues, please try to reset the ekf first. If that doesn't +work, try initializing the bias. If you are using the teleop tool, please see +the 'Reset Ekf' and 'Reset Bias' sections in \ref teleop on how to do this. If +you are using GDS, please navigate to the 'Engineering' tab. The 'Reset EKF' and +'Initialize Bias' buttons should be in the top right corner. + + +### Teleop Tool Reports No Publisher for Acks Topic + +This issue occurs when a namespace is used for Astrobee but the namespace wasn't +provided to the teleop tool or the namespace provided doesn't match Astrobee's +namespace. Please check the namespace of Astrobee and make sure it matches the +namespace provided to the teleop tool. Remember, Astrobee namespaces should not +be capitialized. - +This error only occurs when running the simulator in a native linux machine +instead of a virtual machine. To get rid of it, you will have to open the +astrobee/config/communications/dds/RAPID_QOS_PROFILES.xml file and remove +ip address '128.102.\*' from line 118. + +## Gazebo shows up black in VM Ubuntu 20.04 + +This is a known issue not specific to our project reported [here](https://forum.parallels.com/threads/black-screen-with-3d-acceleration-enabled-for-gazebo-and-rviz-robotic.355825/). +The solution is to turn off 3D acceleration on the VM settings. Note that otherwise +the simulation will still work and the robot is capable of moving, but all scicam +data imagery will show up black. \ No newline at end of file diff --git a/simulation/sim_overview.md b/simulation/sim_overview.md deleted file mode 100644 index 18a1d78b1e..0000000000 --- a/simulation/sim_overview.md +++ /dev/null @@ -1,254 +0,0 @@ -\page sim_overview Simulator usage instructions for non-NASA users - -This documentation assumes you have followed the [install readme](../INSTALL.md). - -## Setting up your Environment - -You will need to setup your environment so that ROS knows about the new packages -provided by Astrobee flight software: - - pushd $BUILD_PATH - source devel/setup.bash - popd - -After this command has completed, you should be able to run `ros` commands from -any directory in your Linux filesystem. - -## Running the Simulator - -To start a simulation of a single Astrobee docked in the ISS, run the following: - - roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true - -- Note 1: The very first time Gazebo starts, it could take a long time because - it loads some large models -- Note 2: To run the simulation with the Gazebo rendering, you can add the flag - `sviz:=true`. This allows one to see the ISS module used for the simulation. - However this is resource intensive. - -This command tells ROS to look for the `sim.launch` file provided by the -`astrobee` package, and use roslaunch to run it. Internally, ROS maintains a -cache of information about package locations, libraries and executables. If you -find that the above command doesn't work, try rebuilding the cache: - - rospack profile - -This command launches both the world and Astrobee. There are things in the -world that need to be started up before Astrobee is started up. There is a -small chance that this will not happen and the user will experience issues upon -start up. If this happens, please see the -[Issues Starting up the Simulator](#issues-starting-up-the-simulator) section. - -**Important**: If you experience issues with the simulator and commanding the -robot, please see the [Simulator Issues](#simulator-issues) section before -contacting the Astrobee team. - -### Simulator Flags - -There are many options when launching the simulator. This section will take you -through the options we think are important to external users. If you are happy -with the simulator setup from above, you can skip this section and jump to the -[Moving the Robot](#moving-the-robot) section. - -**Important**: You can specify more than one flag meaning you could launch more -than one of the GUIs we offer. The simulator is very resource intensive. The -more GUIs running, the more likely it is that you will get tolerance violation -errors when you try to move. If you are running a virtual machine, please make -sure that it has access to your graphics card. - -#### pose - -This is the pose/location that Astrobee starts up at. The first 3 values are -position and the last 4 are the orientation expressed as a quaternion. By -default, the Astrobee starts up docked in the JEM module of the ISS. To start up -in the middle of the JEM, run the following: - - roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true pose:="11.25 -6.95 4.49 0 0 0 1" - -#### gds - -This flag is used to start up our ground data system (GDS). Currently, external -users do not have access to GDS. Please see the -[Ground Data System](#ground-data-system) section for more information. GDS is -the least computationally intensive GUI out of the 3 we offer and it is the only -GUI that allows users to send commands. Thus we would prefer it if users use -GDS when it is released to the public. - -#### rviz - -This flag is used to start up the ros visualization system also known as RVIZ. -RVIZ is our second most computationally intensive GUI. If you don't have access -to GDS, we recommend using RVIZ to visualize what the robot is doing. - -*Note: `rviz` displays the robot perception of the world (estimated pose, sensor -measurements, etc.).* - -#### sviz - -This flag is used to start up the Gazebo GUI. This GUI is the most -computationally intensive GUI we have. We recommend using this GUI if you have a -high performance machine. - -*Note: `sviz` displays the model of the world (ground truth). This means that -the position represented in rviz or gds can differ from the position in sviz.* - -#### dds - -This flag is used to tell ROS if it should start the DDS ROS bridge. This bridge -basically translates ROS to DDS and vice versa. DDS is used to communicate with -our GDS. Since GDS is not available to external users, please always set the dds -flag to false. - -#### speed - -This flag is the speed multiplier for the simulator. By default, it is set to 1 -meaning the simulator runs in real time. If you are having issues with simulator -performance, you can slow the simulator down by specifying a value less than -one. For example, the following would slow the simulator down to half speed: - - roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true speed:=0.5 - -If you have a high performance machine, you can speed up the simulator by -specifying a value greater than 1. Please note, if you specify a speed up -greater than what your computer can handle, the simulator will not run at the -speed you specified. In this case, you can run gz stats in another terminal -to figure out what the actual speed up is. If the factor in gz stats doesn't -match the speed you specified, you can expect weird simulator issues. - -#### ns - -This flag is the namespace of the simulated Astrobee. If you want to know more -about ROS namespaces, please see the ROS documentation. This flag is only needed -when running multiple robots in simulation. Please see the -[Launching Multiple Robots](#launching-multiple-robots) section for more -information. The default namespace is blank. - -**Important**: We only support 4 namespaces in simulation; the blank namespace, -honey, bumble, and queen. Please DO NOT capitalize the namespace as our GUIs -don't support this. - -#### robot - -This flag tells the flight software which configuration file to read. Since -external users don't have access to our GDS and thus don't run our dds ros -bridge, we have created a sim_pub configuration so that the system doesn't -complain about the missing bridge. Until GDS is public, please always set this -flag to sim_pub. - -#### default - -This flag tells ROS to launch the default robot into the world. If you want to -launch the world without the default robot, please set this to `false` (see the -[developers simulator introduction](readme.md) for an example). - -#### world - -This flag tells ROS which world to launch. There are 2 Astrobee worlds; the ISS -and the granite lab (a model of the actual testbed at Ames). By default, this is -set to the ISS world. If you want to test in the granite lab, you will have to -set up the robot for a world with gravity. To do this, first launch the -simulation using the following: - - roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true world:="granite" - -After the simulator is launched, you will have to reset the bias as we encode -gravity in our bias. Please see the Reset Bias section of the -[teleop tool readme](../management/executive/teleop_tool.md#reset-bias) -for instructions on how to send the reset bias command. After you do -this, the robot should be stable. If you want to go back to using the ISS world, -you will have to issue another reset bias command after launching the robot in -the ISS world. - -#### debug - -The Astrobee flight software is still in development and there are bugs that we -don't always catch. If you come across a bug that crashes the flight software, -please help us fix it. To help us, we need to know what happened. The crashed -output should state the node that caused the crash. If it isn't clear which node -it was, please email us the output and we can help. Once you have the name of -the node, please set the debug flag to the node name. This flag will cause a GNU -Debugger(GDB) terminal to pop up. Once the code crashes, navigate to the GDB -terminal, type `bt`, and hit enter. This should display a backtrace. Please copy -the backtrace and send it to us. Here is an example of launching the simulator -and debugging the executive node: - - roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true debug:="executive" - -## Moving the Robot - -There are currently 3 ways to move the robot in simulation. - -### Ground Data System - -The Ground Data System or GDS is our control station. It sends commands to -Astrobee and displays telemetry from Astrobee. Using GDS is the preferred way to -for an operator to operate Astrobee. However, GDS isn't currently open sourced. -Thus as an external user, you will not be able to use it until it is open -sourced. - -### Teleop Tool - -A teleop tool has been added to allow you to issue some commands and get some -telemetry from the robot. This should only be used to play with the simulator -and get a feel for how Astrobee works. As an external user, we consider you to -be a guest scientist and we expect you to follow the guest science framework we -have created. Please see the -[Guest Science Applications](#guest-science-applications) section for more -information. - -For information on how to use the teleop tool, please see the -[teleop tool instructions](../management/executive/teleop_tool.md). - -### Guest Science Applications - -If you are a guest scientist, please make sure you have the astrobee_android -NASA GitHub project checked out (if you followed the usage instructions, you -should have checked this out already). For more information on guest science -and how to create guest science applications, please see -[`astrobee_android/readme.md`](https://github.com/nasa/astrobee_android/blob/master/README.md) -located in the `astrobee_android/` folder. - -## Images - -When running RVIZ, you may notice the big "No Image" text in the two -`DEBUG` windows that are meant to show camera information. This is -because, by default, we don't publish simulated camera images for the -navigation and dock cameras due to the fact that simulating these -images is very resource intensive. If you do want to generate these -images, please open `astrobee/config/simulation/simulation.config` and -change one or more of `nav_cam_rate` and `dock_cam_rate` to something -other than 0. We recommend setting these values to 1 meaning these -images will be published once per second. After this, you should see -those images when you start the simulator. Please note this only works -when the namespace is blank. If you want to see these images, please -don't use the ns flag when launching the simulator. To show sci camera -images, modify analogously the `sci_cam_rate` field, and enable this camera -from the `Panels` menu in RVIZ. - -**Important** Unless you have a high performance machine, there is a good chance -that these images will not initialize properly and you will be looking at the -inside of the robot. If this happens, you will need to launch the world and -Astrobee separately. Please follow the -[Issues Starting up the Simulator](#issues-starting-up-the-Simulator) section -with one caveat. DO NOT start up Astrobee (i.e. don't issue the second terminal -command) until 30 to 40 seconds after RVIZ has started up. If you have a low -performance computer, this fix may not be the only thing you have to do to fix -the images. Please try reducing the speed of your simulator as well. For -information on how to reduce the simulator speed, please see the [speed](#speed) -flag section. - -## Launching Multiple Robots - -Coming soon!!! - - - -## Simulator Issues - -[List of common issue with the simulator](sim_issues.md) - diff --git a/simulation/src/astrobee_gazebo.cc b/simulation/src/astrobee_gazebo.cc index 96339b0610..855ac3b3ab 100644 --- a/simulation/src/astrobee_gazebo.cc +++ b/simulation/src/astrobee_gazebo.cc @@ -44,7 +44,7 @@ void FreeFlyerPlugin::SetParentFrame(std::string const& parent) { } // Load function -void FreeFlyerPlugin::InitializePlugin(std::string const& robot_name) { +void FreeFlyerPlugin::InitializePlugin(std::string const& robot_name, std::string const& plugin_name) { robot_name_ = robot_name; // Make sure the ROS node for Gazebo has already been initialized if (!ros::isInitialized()) @@ -60,7 +60,7 @@ void FreeFlyerPlugin::InitializePlugin(std::string const& robot_name) { // Assign special node handles that use custom callback queues to avoid // Gazebo locking up heartbeats from being sent to the system monitor. nh_ff_ = ros::NodeHandle(robot_name_); - Setup(nh_ff_, nh_ff_); + Setup(nh_ff_, nh_ff_, plugin_name); // If we have a frame then defer chainloading until we receive them timer_ = nh_.createTimer(ros::Duration(5.0), @@ -123,8 +123,17 @@ void FreeFlyerModelPlugin::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { std::string ns = model_->GetName(); if (ns == "bsharp") ns = "/"; + + // Read plugin custom name if specified + std::string plugin_name = ""; + if (sdf->HasElement("plugin_name")) + plugin_name = sdf->Get("plugin_name"); + // Read plugin custom frame if specified + if (sdf->HasElement("plugin_frame")) + plugin_frame_ = sdf->Get("plugin_frame"); + // Initialize the FreeFlyerPlugin - InitializePlugin(ns); + InitializePlugin(ns, plugin_name); // Now load the rest of the plugin LoadCallback(&nh_, model_, sdf_); @@ -195,8 +204,16 @@ void FreeFlyerSensorPlugin::Load(sensors::SensorPtr sensor, sdf::ElementPtr sdf) if (ns == "bsharp") ns = "/"; + // Read plugin custom name if specified + std::string plugin_name = ""; + if (sdf->HasElement("plugin_name")) + plugin_name = sdf->Get("plugin_name"); + // Read plugin custom frame if specified + if (sdf->HasElement("plugin_frame")) + plugin_frame_ = sdf->Get("plugin_frame"); + // Initialize the FreeFlyerPlugin - InitializePlugin(ns); + InitializePlugin(ns, plugin_name); // Now load the rest of the plugin LoadCallback(&nh_, sensor_, sdf_); diff --git a/simulation/src/gazebo_model_plugin_dock/gazebo_model_plugin_dock.cc b/simulation/src/gazebo_model_plugin_dock/gazebo_model_plugin_dock.cc deleted file mode 100644 index d6dc9296cb..0000000000 --- a/simulation/src/gazebo_model_plugin_dock/gazebo_model_plugin_dock.cc +++ /dev/null @@ -1,49 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// ROS includes -#include - -// Gazebo includes -#include - -namespace gazebo { - -// This class is a plugin that calls the GNC autocode to predict -// the forced to be applied to the rigid body -class GazeboModelPluginDock : public FreeFlyerModelPlugin { - public: - GazeboModelPluginDock() : FreeFlyerModelPlugin("gazebo_dock", "body") { - SetParentFrame(FRAME_NAME_WORLD); - } - - ~GazeboModelPluginDock() {} - - protected: - // Called when the plugin is loaded into the simulator - void LoadCallback( - ros::NodeHandle *nh, physics::ModelPtr model, sdf::ElementPtr sdf) {} - - // Called on simulation reset - void Reset() {} -}; - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginDock) - -} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc b/simulation/src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc index 967e2fd89d..b422456177 100644 --- a/simulation/src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc +++ b/simulation/src/gazebo_model_plugin_drag/gazebo_model_plugin_drag.cc @@ -76,19 +76,20 @@ class GazeboModelPluginDrag : public FreeFlyerModelPlugin { // Calculate drag #if GAZEBO_MAJOR_VERSION > 7 drag_ = GetLink()->RelativeLinearVel(); - drag_ = -0.5 * coefficient_ * area_ * density_ - * drag_.Length() * drag_.Length() * drag_.Normalize(); + vel_ = drag_.Length(); #else drag_ = GetLink()->GetRelativeLinearVel(); - drag_ = -0.5 * coefficient_ * area_ * density_ - * drag_.GetLength() * drag_.GetLength() * drag_.Normalize(); + vel_ = drag_.GetLength(); #endif + drag_ = -0.5 * coefficient_ * area_ * density_ + * vel_ * vel_ * drag_.Normalize(); + // Apply the force and torque to the model GetLink()->AddRelativeForce(drag_); } private: - double coefficient_, area_, density_; // Drag parameters + double coefficient_, area_, density_, vel_; // Drag parameters common::Time next_tick_; #if GAZEBO_MAJOR_VERSION > 7 ignition::math::Vector3d drag_; diff --git a/simulation/src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc b/simulation/src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc index 8ab8ff6000..24870bc6fc 100644 --- a/simulation/src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc +++ b/simulation/src/gazebo_model_plugin_eps/gazebo_model_plugin_eps.cc @@ -90,8 +90,8 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { GazeboModelPluginEps() : FreeFlyerModelPlugin("eps_driver", "", true), fsm_(UNKNOWN, std::bind(&GazeboModelPluginEps::StateCallback, this, std::placeholders::_1, std::placeholders::_2)), - rate_(10.0), distance_(0.05), delay_(5.0), lock_(false), - battery_capacity_(3.4), battery_charge_(3.0), + rate_(10.0), distance_near_(0.05), distance_far_(0.05), delay_(5.0), + lock_(false), battery_capacity_(3.4), battery_charge_(3.0), battery_discharge_rate_(0.005) { // In an unknown state, if we are sensed to be near or far from a berth // then update to either a docked or undocked state. @@ -195,8 +195,10 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { // Get parameters if (sdf->HasElement("rate")) rate_ = sdf->Get("rate"); - if (sdf->HasElement("distance")) - distance_ = sdf->Get("distance"); + if (sdf->HasElement("distance_near")) + distance_near_ = sdf->Get("distance_near"); + if (sdf->HasElement("distance_far")) + distance_far_ = sdf->Get("distance_far"); if (sdf->HasElement("delay")) delay_ = sdf->Get("delay"); // Setup telemetry publishers @@ -444,24 +446,32 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { // There are smarter ways to do this sort of search (kNN) but this we are // only expecting fewer than 6 berths, it seems like needless optimization. bool near = false; + bool far = true; for (nearest_ = berths_.begin(); nearest_ != berths_.end(); nearest_++) { #if GAZEBO_MAJOR_VERSION > 7 - if (GetModel()->WorldPose().Pos().Distance( - nearest_->second.Pos()) > distance_) continue; + double distance = GetModel()->WorldPose().Pos().Distance( + nearest_->second.Pos()); #else - if (GetModel()->GetWorldPose().Ign().Pos().Distance( - nearest_->second.Pos()) > distance_) continue; + double distance = GetModel()->GetWorldPose().Ign().Pos().Distance( + nearest_->second.Pos()); #endif - // Now, send an event to the FSM to signal that we are close! - fsm_.Update(SENSE_NEAR); + // There should always only be one dock that we are close to - near = true; - break; + if (distance < distance_near_) { + near = true; + break; + } + if (distance < distance_far_) + far = false; } - + // Send an NEAR event if we are close to a berth + if (near) { + fsm_.Update(SENSE_NEAR); // Send an FAR event if we aren't close to any berth - if (!near) + } else if (far) { fsm_.Update(SENSE_FAR); + } + // Whatever the result, publish the dock state to be used by other entites // in the system, and in particular the dock procedure. ff_hw_msgs::EpsDockStateStamped msg; @@ -667,7 +677,7 @@ class GazeboModelPluginEps : public FreeFlyerModelPlugin { private: ff_util::FSM fsm_; - double rate_, distance_, delay_; + double rate_, distance_near_, distance_far_ , delay_; bool lock_; double battery_capacity_, battery_charge_, battery_discharge_rate_; event::ConnectionPtr update_; diff --git a/simulation/src/gazebo_model_plugin_flashlight/gazebo_model_plugin_flashlight.cc b/simulation/src/gazebo_model_plugin_flashlight/gazebo_model_plugin_flashlight.cc new file mode 100644 index 0000000000..a8d70ed897 --- /dev/null +++ b/simulation/src/gazebo_model_plugin_flashlight/gazebo_model_plugin_flashlight.cc @@ -0,0 +1,240 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// ROS includes +#include + +// Transformation helper code +#include + +// RVIZ visualization +#include + +// Gazebo includes +#include + +// Freeflyer messages +#include + +// STL includes +#include +#include + +namespace gazebo { + +class GazeboModelPluginFlashlight : public FreeFlyerModelPlugin { + public: + GazeboModelPluginFlashlight() : FreeFlyerModelPlugin("", + "", true), rate_(10.0), + width_(0.03), height_(0.02), depth_(0.005) {} + + ~GazeboModelPluginFlashlight() { + if (update_) { + #if GAZEBO_MAJOR_VERSION > 7 + update_.reset(); + #else + event::Events::DisconnectWorldUpdateBegin(update_); + #endif + } + } + + protected: + // Called when the plugin is loaded into the simulator + void LoadCallback(ros::NodeHandle *nh, + physics::ModelPtr model, sdf::ElementPtr sdf) { + // Get parameters from the SDF + if (sdf->HasElement("rate")) + rate_ = sdf->Get("rate"); + if (sdf->HasElement("width")) + width_ = sdf->Get("width"); + if (sdf->HasElement("height")) + height_ = sdf->Get("height"); + if (sdf->HasElement("depth")) + depth_ = sdf->Get("depth"); + if (sdf->HasElement("plugin_frame")) + plugin_frame_ = sdf->Get("plugin_frame"); + + // Use the message system to toggle visibility of visual elements + gz_ = transport::NodePtr(new transport::Node()); + gz_->Init(); + pub_visual_ = gz_->Advertise("~/visual"); + pub_factory_ = gz_->Advertise("~/factory/light"); + pub_light_ = gz_->Advertise("~/light/modify"); + + // For the RVIZ marker array + pub_rviz_ = nh->advertise( + TOPIC_HARDWARE_LIGHTS_RVIZ, 0, true); + + // Rotate from the flaslight frame to the visual frame + pose_ = ignition::math::Pose3d(0.0, 0, 0, 0.70710678, 0, -0.70710678, 0); + } + + // Only send measurements when extrinsics are available + void OnExtrinsicsReceived(ros::NodeHandle *nh) { + srv_ = nh->advertiseService("hw/" + plugin_frame_ + "/control", + &GazeboModelPluginFlashlight::ToggleCallback, this); + } + + + // Manage the extrinsics based on the sensor type + bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf) { + if (!tf) { + ROS_WARN("Flashlight extrinsics are null"); + return false; + } + + // Create the rviz marker + marker_.header.stamp = ros::Time::now(); + marker_.header.frame_id = GetFrame(); + marker_.ns = GetFrame(plugin_frame_, "_"); + marker_.id = 0; + marker_.type = visualization_msgs::Marker::CUBE; + marker_.action = visualization_msgs::Marker::ADD; + marker_.pose.position.x = pose_.Pos().X(); + marker_.pose.position.y = pose_.Pos().Y(); + marker_.pose.position.z = pose_.Pos().Z(); + marker_.pose.orientation.x = pose_.Rot().X(); + marker_.pose.orientation.y = pose_.Rot().Y(); + marker_.pose.orientation.z = pose_.Rot().Z(); + marker_.pose.orientation.w = pose_.Rot().W(); + marker_.scale.x = depth_; + marker_.scale.y = width_; + marker_.scale.z = height_; + marker_.color.a = 0.0; + marker_.color.r = 1.0; + marker_.color.g = 1.0; + marker_.color.b = 1.0; + visualization_msgs::MarkerArray msg; + msg.markers.push_back(marker_); + pub_rviz_.publish(msg); + + // Aggregate pose + pose_ = pose_ + ignition::math::Pose3d( + tf->transform.translation.x, + tf->transform.translation.y, + tf->transform.translation.z, + tf->transform.rotation.w, + tf->transform.rotation.x, + tf->transform.rotation.y, + tf->transform.rotation.z); + + // Create the Gazebo visual + visual_.set_name(GetFrame(plugin_frame_ + "_visual", "_")); + visual_.set_parent_name(GetModel()->GetLink()->GetScopedName()); + msgs::Geometry *geometry = visual_.mutable_geometry(); + geometry->set_type(msgs::Geometry::BOX); + msgs::Set(geometry->mutable_box()->mutable_size(), + ignition::math::Vector3d(depth_, width_, height_)); + visual_.mutable_material()->mutable_script()->set_name("Astrobee/Flashlight"); + msgs::Set(visual_.mutable_pose(), pose_); + visual_.set_is_static(false); + visual_.set_visible(true); + visual_.set_cast_shadows(false); + visual_.set_transparency(1.0); + pub_visual_->Publish(visual_); + + // Create the gazebo light + light_.set_name(GetFrame(plugin_frame_ + "_front_light", "_")); + light_.set_type(msgs::Light::SPOT); + light_.set_attenuation_constant(1.0); + light_.set_attenuation_linear(0.02); + light_.set_attenuation_quadratic(0.0); + light_.set_range(10); + light_.set_cast_shadows(false); + light_.set_spot_inner_angle(0.6); + light_.set_spot_outer_angle(2.2); + light_.set_spot_falloff(1.0); + + // For some reason common::Color stopped existing in later versions + #if GAZEBO_MAJOR_VERSION < 11 + msgs::Set(light_.mutable_diffuse(), common::Color(0.5, 0.5, 0.5, 1)); + msgs::Set(light_.mutable_specular(), common::Color(0.1, 0.1, 0.1, 1)); + #endif + + #if GAZEBO_MAJOR_VERSION > 7 + msgs::Set(light_.mutable_pose(), pose_ + + GetModel()->GetLink()->WorldPose()); + #else + msgs::Set(light_.mutable_pose(), pose_ + + GetModel()->GetLink()->GetWorldPose().Ign()); + #endif + pub_factory_->Publish(light_); + + // Modify the new entity to be only visible in the GUI + update_ = event::Events::ConnectWorldUpdateBegin(std::bind( + &GazeboModelPluginFlashlight::WorldUpdateBegin, this)); + + // Success + return true; + } + + // Called when the laser needs to be toggled + bool ToggleCallback(ff_hw_msgs::SetFlashlight::Request &req, + ff_hw_msgs::SetFlashlight::Response &res) { + // Update the alpha channel in rviz + marker_.header.stamp = ros::Time::now(); + marker_.color.a = static_cast(req.brightness) / 200.0; + visualization_msgs::MarkerArray msg; + msg.markers.push_back(marker_); + pub_rviz_.publish(msg); + + // Update Gazebo visual + visual_.set_transparency(1.0 - marker_.color.a); + pub_visual_->Publish(visual_); + + // Update the gazebo light + light_.set_attenuation_constant(1.0 - marker_.color.a); + pub_light_->Publish(light_); + + // Print response + res.success = true; + res.status_message = "Flashlight toggled successfully"; + return true; + } + + // Called when a new entity is created + void WorldUpdateBegin() { + #if GAZEBO_MAJOR_VERSION > 7 + msgs::Set(light_.mutable_pose(), pose_ + + GetModel()->GetLink()->WorldPose()); + #else + msgs::Set(light_.mutable_pose(), pose_ + + GetModel()->GetLink()->GetWorldPose().Ign()); + #endif + pub_light_->Publish(light_); + } + + private: + double rate_, width_, height_, depth_; + transport::NodePtr gz_; + transport::PublisherPtr pub_visual_, pub_factory_, pub_light_; + event::ConnectionPtr update_; + ros::Timer timer_; + ros::ServiceServer srv_; + ros::Publisher pub_rviz_; + visualization_msgs::Marker marker_; + msgs::Light light_; + msgs::Visual visual_; + ignition::math::Pose3d pose_; + std::string plugin_frame_ = ""; +}; + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginFlashlight ) + +} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_flashlight_aft/gazebo_model_plugin_flashlight_aft.cc b/simulation/src/gazebo_model_plugin_flashlight_aft/gazebo_model_plugin_flashlight_aft.cc deleted file mode 100644 index 7922581b0b..0000000000 --- a/simulation/src/gazebo_model_plugin_flashlight_aft/gazebo_model_plugin_flashlight_aft.cc +++ /dev/null @@ -1,232 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// ROS includes -#include - -// Transformation helper code -#include - -// RVIZ visualization -#include - -// Gazebo includes -#include - -// Freeflyer messages -#include - -// STL includes -#include -#include - -namespace gazebo { - -class GazeboModelPluginFlashlightAft : public FreeFlyerModelPlugin { - public: - GazeboModelPluginFlashlightAft() : FreeFlyerModelPlugin("flashlight_aft", - "flashlight_aft", true), rate_(10.0), - width_(0.03), height_(0.02), depth_(0.005) {} - - ~GazeboModelPluginFlashlightAft() { - if (update_) { - #if GAZEBO_MAJOR_VERSION > 7 - update_.reset(); - #else - event::Events::DisconnectWorldUpdateBegin(update_); - #endif - } - } - - protected: - // Called when the plugin is loaded into the simulator - void LoadCallback(ros::NodeHandle *nh, - physics::ModelPtr model, sdf::ElementPtr sdf) { - // Get parameters from the SDF - if (sdf->HasElement("rate")) - rate_ = sdf->Get("rate"); - if (sdf->HasElement("width")) - width_ = sdf->Get("width"); - if (sdf->HasElement("height")) - height_ = sdf->Get("height"); - if (sdf->HasElement("depth")) - depth_ = sdf->Get("depth"); - - // Use the message system to toggle visibility of visual elements - gz_ = transport::NodePtr(new transport::Node()); - gz_->Init(); - pub_visual_ = gz_->Advertise("~/visual"); - pub_factory_ = gz_->Advertise("~/factory/light"); - pub_light_ = gz_->Advertise("~/light/modify"); - - // For the RVIZ marker array - pub_rviz_ = nh->advertise( - TOPIC_HARDWARE_LIGHTS_RVIZ, 0, true); - - // Rotate from the flaslight frame to the visual frame - pose_ = ignition::math::Pose3d(0.0, 0, 0, 0.70710678, 0, -0.70710678, 0); - } - - // Only send measurements when extrinsics are available - void OnExtrinsicsReceived(ros::NodeHandle *nh) { - srv_ = nh->advertiseService(SERVICE_HARDWARE_LIGHT_AFT_CONTROL, - &GazeboModelPluginFlashlightAft::ToggleCallback, this); - } - - - // Manage the extrinsics based on the sensor type - bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf) { - if (!tf) { - ROS_WARN("Front flashlight extrinsics are null"); - return false; - } - - // Create the rviz marker - marker_.header.stamp = ros::Time::now(); - marker_.header.frame_id = GetFrame(); - marker_.ns = GetFrame("flashlight_aft", "_"); - marker_.id = 0; - marker_.type = visualization_msgs::Marker::CUBE; - marker_.action = visualization_msgs::Marker::ADD; - marker_.pose.position.x = pose_.Pos().X(); - marker_.pose.position.y = pose_.Pos().Y(); - marker_.pose.position.z = pose_.Pos().Z(); - marker_.pose.orientation.x = pose_.Rot().X(); - marker_.pose.orientation.y = pose_.Rot().Y(); - marker_.pose.orientation.z = pose_.Rot().Z(); - marker_.pose.orientation.w = pose_.Rot().W(); - marker_.scale.x = depth_; - marker_.scale.y = width_; - marker_.scale.z = height_; - marker_.color.a = 0.0; - marker_.color.r = 1.0; - marker_.color.g = 1.0; - marker_.color.b = 1.0; - visualization_msgs::MarkerArray msg; - msg.markers.push_back(marker_); - pub_rviz_.publish(msg); - - // Aggregate pose - pose_ = pose_ + ignition::math::Pose3d( - tf->transform.translation.x, - tf->transform.translation.y, - tf->transform.translation.z, - tf->transform.rotation.w, - tf->transform.rotation.x, - tf->transform.rotation.y, - tf->transform.rotation.z); - - // Create the Gazebo visual - visual_.set_name(GetFrame("flashlight_aft_visual", "_")); - visual_.set_parent_name(GetModel()->GetLink()->GetScopedName()); - msgs::Geometry *geometry = visual_.mutable_geometry(); - geometry->set_type(msgs::Geometry::BOX); - msgs::Set(geometry->mutable_box()->mutable_size(), - ignition::math::Vector3d(depth_, width_, height_)); - visual_.mutable_material()->mutable_script()->set_name("Astrobee/Flashlight"); - msgs::Set(visual_.mutable_pose(), pose_); - visual_.set_is_static(false); - visual_.set_visible(true); - visual_.set_cast_shadows(false); - visual_.set_transparency(1.0); - pub_visual_->Publish(visual_); - - // Create the gazebo light - light_.set_name(GetFrame("flashlight_aft_light", "_")); - light_.set_type(msgs::Light::SPOT); - light_.set_attenuation_constant(1.0); - light_.set_attenuation_linear(0.02); - light_.set_attenuation_quadratic(0.0); - light_.set_range(10); - light_.set_cast_shadows(false); - light_.set_spot_inner_angle(0.6); - light_.set_spot_outer_angle(2.2); - light_.set_spot_falloff(1.0); - msgs::Set(light_.mutable_diffuse(), common::Color(0.5, 0.5, 0.5, 1)); - msgs::Set(light_.mutable_specular(), common::Color(0.1, 0.1, 0.1, 1)); - #if GAZEBO_MAJOR_VERSION > 7 - msgs::Set(light_.mutable_pose(), pose_ + - GetModel()->GetLink()->WorldPose()); - #else - msgs::Set(light_.mutable_pose(), pose_ + - GetModel()->GetLink()->GetWorldPose().Ign()); - #endif - pub_factory_->Publish(light_); - - // Modify the new entity to be only visible in the GUI - update_ = event::Events::ConnectWorldUpdateBegin(std::bind( - &GazeboModelPluginFlashlightAft::WorldUpdateBegin, this)); - - // Success - return true; - } - - // Called when the laser needs to be toggled - bool ToggleCallback(ff_hw_msgs::SetFlashlight::Request &req, - ff_hw_msgs::SetFlashlight::Response &res) { - // Update the alpha channel in rviz - marker_.header.stamp = ros::Time::now(); - marker_.color.a = static_cast(req.brightness) / 200.0; - visualization_msgs::MarkerArray msg; - msg.markers.push_back(marker_); - pub_rviz_.publish(msg); - - // Update Gazebo visual - visual_.set_transparency(1.0 - marker_.color.a); - pub_visual_->Publish(visual_); - - // Update the gazebo light - light_.set_attenuation_constant(1.0 - marker_.color.a); - pub_light_->Publish(light_); - - // Print response - res.success = true; - res.status_message = "Flashlight toggled successfully"; - return true; - } - - // Called when a new entity is created - void WorldUpdateBegin() { - #if GAZEBO_MAJOR_VERSION > 7 - msgs::Set(light_.mutable_pose(), pose_ + - GetModel()->GetLink()->WorldPose()); - #else - msgs::Set(light_.mutable_pose(), pose_ + - GetModel()->GetLink()->GetWorldPose().Ign()); - #endif - pub_light_->Publish(light_); - } - - private: - double rate_, width_, height_, depth_; - transport::NodePtr gz_; - transport::PublisherPtr pub_visual_, pub_factory_, pub_light_; - event::ConnectionPtr update_; - ros::Timer timer_; - ros::ServiceServer srv_; - ros::Publisher pub_rviz_; - visualization_msgs::Marker marker_; - msgs::Light light_; - msgs::Visual visual_; - ignition::math::Pose3d pose_; -}; - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginFlashlightAft) - -} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_flashlight_front/gazebo_model_plugin_flashlight_front.cc b/simulation/src/gazebo_model_plugin_flashlight_front/gazebo_model_plugin_flashlight_front.cc deleted file mode 100644 index eafac9d469..0000000000 --- a/simulation/src/gazebo_model_plugin_flashlight_front/gazebo_model_plugin_flashlight_front.cc +++ /dev/null @@ -1,232 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// ROS includes -#include - -// Transformation helper code -#include - -// RVIZ visualization -#include - -// Gazebo includes -#include - -// Freeflyer messages -#include - -// STL includes -#include -#include - -namespace gazebo { - -class GazeboModelPluginFlashlightFront : public FreeFlyerModelPlugin { - public: - GazeboModelPluginFlashlightFront() : FreeFlyerModelPlugin("flashlight_front", - "flashlight_front", true), rate_(10.0), - width_(0.03), height_(0.02), depth_(0.005) {} - - ~GazeboModelPluginFlashlightFront() { - if (update_) { - #if GAZEBO_MAJOR_VERSION > 7 - update_.reset(); - #else - event::Events::DisconnectWorldUpdateBegin(update_); - #endif - } - } - - protected: - // Called when the plugin is loaded into the simulator - void LoadCallback(ros::NodeHandle *nh, - physics::ModelPtr model, sdf::ElementPtr sdf) { - // Get parameters from the SDF - if (sdf->HasElement("rate")) - rate_ = sdf->Get("rate"); - if (sdf->HasElement("width")) - width_ = sdf->Get("width"); - if (sdf->HasElement("height")) - height_ = sdf->Get("height"); - if (sdf->HasElement("depth")) - depth_ = sdf->Get("depth"); - - // Use the message system to toggle visibility of visual elements - gz_ = transport::NodePtr(new transport::Node()); - gz_->Init(); - pub_visual_ = gz_->Advertise("~/visual"); - pub_factory_ = gz_->Advertise("~/factory/light"); - pub_light_ = gz_->Advertise("~/light/modify"); - - // For the RVIZ marker array - pub_rviz_ = nh->advertise( - TOPIC_HARDWARE_LIGHTS_RVIZ, 0, true); - - // Rotate from the flaslight frame to the visual frame - pose_ = ignition::math::Pose3d(0.0, 0, 0, 0.70710678, 0, -0.70710678, 0); - } - - // Only send measurements when extrinsics are available - void OnExtrinsicsReceived(ros::NodeHandle *nh) { - srv_ = nh->advertiseService(SERVICE_HARDWARE_LIGHT_FRONT_CONTROL, - &GazeboModelPluginFlashlightFront::ToggleCallback, this); - } - - - // Manage the extrinsics based on the sensor type - bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf) { - if (!tf) { - ROS_WARN("Front flashlight extrinsics are null"); - return false; - } - - // Create the rviz marker - marker_.header.stamp = ros::Time::now(); - marker_.header.frame_id = GetFrame(); - marker_.ns = GetFrame("flashlight_front", "_"); - marker_.id = 0; - marker_.type = visualization_msgs::Marker::CUBE; - marker_.action = visualization_msgs::Marker::ADD; - marker_.pose.position.x = pose_.Pos().X(); - marker_.pose.position.y = pose_.Pos().Y(); - marker_.pose.position.z = pose_.Pos().Z(); - marker_.pose.orientation.x = pose_.Rot().X(); - marker_.pose.orientation.y = pose_.Rot().Y(); - marker_.pose.orientation.z = pose_.Rot().Z(); - marker_.pose.orientation.w = pose_.Rot().W(); - marker_.scale.x = depth_; - marker_.scale.y = width_; - marker_.scale.z = height_; - marker_.color.a = 0.0; - marker_.color.r = 1.0; - marker_.color.g = 1.0; - marker_.color.b = 1.0; - visualization_msgs::MarkerArray msg; - msg.markers.push_back(marker_); - pub_rviz_.publish(msg); - - // Aggregate pose - pose_ = pose_ + ignition::math::Pose3d( - tf->transform.translation.x, - tf->transform.translation.y, - tf->transform.translation.z, - tf->transform.rotation.w, - tf->transform.rotation.x, - tf->transform.rotation.y, - tf->transform.rotation.z); - - // Create the Gazebo visual - visual_.set_name(GetFrame("flashlight_front_visual", "_")); - visual_.set_parent_name(GetModel()->GetLink()->GetScopedName()); - msgs::Geometry *geometry = visual_.mutable_geometry(); - geometry->set_type(msgs::Geometry::BOX); - msgs::Set(geometry->mutable_box()->mutable_size(), - ignition::math::Vector3d(depth_, width_, height_)); - visual_.mutable_material()->mutable_script()->set_name("Astrobee/Flashlight"); - msgs::Set(visual_.mutable_pose(), pose_); - visual_.set_is_static(false); - visual_.set_visible(true); - visual_.set_cast_shadows(false); - visual_.set_transparency(1.0); - pub_visual_->Publish(visual_); - - // Create the gazebo light - light_.set_name(GetFrame("flashlight_front_light", "_")); - light_.set_type(msgs::Light::SPOT); - light_.set_attenuation_constant(1.0); - light_.set_attenuation_linear(0.02); - light_.set_attenuation_quadratic(0.0); - light_.set_range(10); - light_.set_cast_shadows(false); - light_.set_spot_inner_angle(0.6); - light_.set_spot_outer_angle(2.2); - light_.set_spot_falloff(1.0); - msgs::Set(light_.mutable_diffuse(), common::Color(0.5, 0.5, 0.5, 1)); - msgs::Set(light_.mutable_specular(), common::Color(0.1, 0.1, 0.1, 1)); - #if GAZEBO_MAJOR_VERSION > 7 - msgs::Set(light_.mutable_pose(), pose_ + - GetModel()->GetLink()->WorldPose()); - #else - msgs::Set(light_.mutable_pose(), pose_ + - GetModel()->GetLink()->GetWorldPose().Ign()); - #endif - pub_factory_->Publish(light_); - - // Modify the new entity to be only visible in the GUI - update_ = event::Events::ConnectWorldUpdateBegin(std::bind( - &GazeboModelPluginFlashlightFront::WorldUpdateBegin, this)); - - // Success - return true; - } - - // Called when the laser needs to be toggled - bool ToggleCallback(ff_hw_msgs::SetFlashlight::Request &req, - ff_hw_msgs::SetFlashlight::Response &res) { - // Update the alpha channel in rviz - marker_.header.stamp = ros::Time::now(); - marker_.color.a = static_cast(req.brightness) / 200.0; - visualization_msgs::MarkerArray msg; - msg.markers.push_back(marker_); - pub_rviz_.publish(msg); - - // Update Gazebo visual - visual_.set_transparency(1.0 - marker_.color.a); - pub_visual_->Publish(visual_); - - // Update the gazebo light - light_.set_attenuation_constant(1.0 - marker_.color.a); - pub_light_->Publish(light_); - - // Print response - res.success = true; - res.status_message = "Flashlight toggled successfully"; - return true; - } - - // Called when a new entity is created - void WorldUpdateBegin() { - #if GAZEBO_MAJOR_VERSION > 7 - msgs::Set(light_.mutable_pose(), pose_ + - GetModel()->GetLink()->WorldPose()); - #else - msgs::Set(light_.mutable_pose(), pose_ + - GetModel()->GetLink()->GetWorldPose().Ign()); - #endif - pub_light_->Publish(light_); - } - - private: - double rate_, width_, height_, depth_; - transport::NodePtr gz_; - transport::PublisherPtr pub_visual_, pub_factory_, pub_light_; - event::ConnectionPtr update_; - ros::Timer timer_; - ros::ServiceServer srv_; - ros::Publisher pub_rviz_; - visualization_msgs::Marker marker_; - msgs::Light light_; - msgs::Visual visual_; - ignition::math::Pose3d pose_; -}; - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginFlashlightFront ) - -} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_llp_cpu_mem_monitor/gazebo_model_plugin_llp_cpu_mem_monitor.cc b/simulation/src/gazebo_model_plugin_llp_cpu_mem_monitor/gazebo_model_plugin_llp_cpu_mem_monitor.cc new file mode 100644 index 0000000000..351c0e13bd --- /dev/null +++ b/simulation/src/gazebo_model_plugin_llp_cpu_mem_monitor/gazebo_model_plugin_llp_cpu_mem_monitor.cc @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// ROS includes +#include + +// Gazebo includes +#include + +// STL includes +#include + +namespace gazebo { + +class GazeboModelPluginLlpCpuMemMonitor : public FreeFlyerModelPlugin { + public: + GazeboModelPluginLlpCpuMemMonitor() : FreeFlyerModelPlugin( + "llp_cpu_mem_monitor", "llp_cpu_mem_monitor", true) {} + + ~GazeboModelPluginLlpCpuMemMonitor() {} + + protected: + // Called when the plugin is loaded into the simulator + void LoadCallback(ros::NodeHandle *nh, + physics::ModelPtr model, sdf::ElementPtr sdf) {} + + // Manage the extrinsics based on the sensor type + bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf) { + return true; + } +}; + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginLlpCpuMemMonitor) + +} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_llp_cpu_monitor/gazebo_model_plugin_llp_cpu_monitor.cc b/simulation/src/gazebo_model_plugin_llp_cpu_monitor/gazebo_model_plugin_llp_cpu_monitor.cc deleted file mode 100644 index 0dbb3531ab..0000000000 --- a/simulation/src/gazebo_model_plugin_llp_cpu_monitor/gazebo_model_plugin_llp_cpu_monitor.cc +++ /dev/null @@ -1,51 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// ROS includes -#include - -// Gazebo includes -#include - -// STL includes -#include - -namespace gazebo { - -class GazeboModelPluginLlpCpuMonitor : public FreeFlyerModelPlugin { - public: - GazeboModelPluginLlpCpuMonitor() : FreeFlyerModelPlugin( - "llp_cpu_monitor", "llp_cpu_monitor", true) {} - - ~GazeboModelPluginLlpCpuMonitor() {} - - protected: - // Called when the plugin is loaded into the simulator - void LoadCallback(ros::NodeHandle *nh, - physics::ModelPtr model, sdf::ElementPtr sdf) {} - - // Manage the extrinsics based on the sensor type - bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf) { - return true; - } -}; - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginLlpCpuMonitor) - -} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_mlp_cpu_mem_monitor/gazebo_model_plugin_mlp_cpu_mem_monitor.cc b/simulation/src/gazebo_model_plugin_mlp_cpu_mem_monitor/gazebo_model_plugin_mlp_cpu_mem_monitor.cc new file mode 100644 index 0000000000..37b8cab4ad --- /dev/null +++ b/simulation/src/gazebo_model_plugin_mlp_cpu_mem_monitor/gazebo_model_plugin_mlp_cpu_mem_monitor.cc @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// ROS includes +#include + +// Gazebo includes +#include + +// STL includes +#include + +namespace gazebo { + +class GazeboModelPluginMlpCpuMemMonitor : public FreeFlyerModelPlugin { + public: + GazeboModelPluginMlpCpuMemMonitor() : FreeFlyerModelPlugin( + "mlp_cpu_mem_monitor", "mlp_cpu_mem_monitor", true) {} + + ~GazeboModelPluginMlpCpuMemMonitor() {} + + protected: + // Called when the plugin is loaded into the simulator + void LoadCallback(ros::NodeHandle *nh, + physics::ModelPtr model, sdf::ElementPtr sdf) {} + + // Manage the extrinsics based on the sensor type + bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf) { + return true; + } +}; + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginMlpCpuMemMonitor) + +} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_mlp_cpu_monitor/gazebo_model_plugin_mlp_cpu_monitor.cc b/simulation/src/gazebo_model_plugin_mlp_cpu_monitor/gazebo_model_plugin_mlp_cpu_monitor.cc deleted file mode 100644 index eb82089cb5..0000000000 --- a/simulation/src/gazebo_model_plugin_mlp_cpu_monitor/gazebo_model_plugin_mlp_cpu_monitor.cc +++ /dev/null @@ -1,51 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// ROS includes -#include - -// Gazebo includes -#include - -// STL includes -#include - -namespace gazebo { - -class GazeboModelPluginMlpCpuMonitor : public FreeFlyerModelPlugin { - public: - GazeboModelPluginMlpCpuMonitor() : FreeFlyerModelPlugin( - "mlp_cpu_monitor", "mlp_cpu_monitor", true) {} - - ~GazeboModelPluginMlpCpuMonitor() {} - - protected: - // Called when the plugin is loaded into the simulator - void LoadCallback(ros::NodeHandle *nh, - physics::ModelPtr model, sdf::ElementPtr sdf) {} - - // Manage the extrinsics based on the sensor type - bool ExtrinsicsCallback(geometry_msgs::TransformStamped const* tf) { - return true; - } -}; - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboModelPluginMlpCpuMonitor) - -} // namespace gazebo diff --git a/simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc b/simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc index ba13fc4741..25fb0889ce 100644 --- a/simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc +++ b/simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc @@ -34,9 +34,10 @@ // FSW messahes #include #include +#include // Autocode inclide -#include +#include // Gazebo includes #include @@ -177,13 +178,16 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { null_command_.goals.push_back(null_goal); null_command_.goals.push_back(null_goal); + // Set timeout + watchdog_period_ = ros::Duration(20.0/control_rate_hz_); + // Telemetry publisher pub_telemetry_ = nh->advertise( TOPIC_HARDWARE_PMC_TELEMETRY, 1); // State publisher as a latched topic pub_state_ = nh->advertise( - TOPIC_HARDWARE_PMC_STATE, 1, true); + TOPIC_HARDWARE_PMC_STATE, 1); // Subscibe to PMC commands sub_command_ = nh->subscribe(TOPIC_HARDWARE_PMC_COMMAND, 5, @@ -195,9 +199,13 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { &GazeboModelPluginPmc::FamCallback, this); // Create a watchdog timer to ensure the PMC commands are set - timer_watchdog_ = nh->createTimer(ros::Duration(20.0/control_rate_hz_), + timer_watchdog_ = nh->createTimer(watchdog_period_, &GazeboModelPluginPmc::WatchdogCallback, this, false, true); + // Update PMC watchdog timer timeout + update_timeout_srv_ = nh->advertiseService( + SERVICE_HARDWARE_PMC_TIMEOUT, &GazeboModelPluginPmc::IdlingTimeoutService, this); + // Create a watchdog timer to ensure the PMC commands are set timer_command_ = nh->createTimer(ros::Duration(1.0/control_rate_hz_), &GazeboModelPluginPmc::CommandTimerCallback, this, false, true); @@ -235,6 +243,10 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { ROS_FATAL("PMC Actuator: state_telemetry_scale not specified!"); return false; } + if (!config_params.GetPosReal("max_timeout", &max_timeout_)) { + ROS_FATAL("PMC Actuator: minimum control rate not specified!"); + return false; + } // TODO(asymingt) This is being phased out. state_tol_rads_per_sec_ = 0.1; // Success @@ -297,16 +309,15 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { // Set the impeller and nozzle values to those given in the message for (size_t i = 0; i < NUMBER_OF_PMCS; i++) { // Take care of ramping in a more responsible way - if (blowers_.states_[i].impeller_cmd < msg.goals[i].motor_speed) + if (pmc_.ImpellerCmd(i) < msg.goals[i].motor_speed) fsm_.Update(GOAL_RAMP_UP); - if (blowers_.states_[i].impeller_cmd > msg.goals[i].motor_speed) + if (pmc_.ImpellerCmd(i) > msg.goals[i].motor_speed) fsm_.Update(GOAL_RAMP_DOWN); // Set the motor speed - blowers_.states_[i].impeller_cmd = msg.goals[i].motor_speed; + pmc_.SetImpellerCmd(i, msg.goals[i].motor_speed); // Set the nozzles for (size_t j = 0; j < NUMBER_OF_NOZZLES; j++) - blowers_.states_[i].servo_cmd[j] - = static_cast (msg.goals[i].nozzle_positions[j]); + pmc_.SetServoCmd(i, j, static_cast (msg.goals[i].nozzle_positions[j])); } } @@ -314,36 +325,36 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { void CommandTimerCallback(ros::TimerEvent const& event) { // Step the blower model #if GAZEBO_MAJOR_VERSION > 7 - blowers_.SetAngularVelocity( + pmc_.SetAngularVelocity( GetLink()->RelativeAngularVel().X(), GetLink()->RelativeAngularVel().Y(), GetLink()->RelativeAngularVel().Z()); #else - blowers_.SetAngularVelocity( + pmc_.SetAngularVelocity( GetLink()->GetRelativeAngularVel().x, GetLink()->GetRelativeAngularVel().y, GetLink()->GetRelativeAngularVel().z); #endif - blowers_.SetBatteryVoltage(14.0); - blowers_.Step(); + pmc_.SetBatteryVoltage(14.0); + pmc_.Step(); // Calculate the force and torque on the platform #if GAZEBO_MAJOR_VERSION > 7 force_ = ignition::math::Vector3d(0, 0, 0); torque_ = ignition::math::Vector3d(0, 0, 0); for (size_t i = 0; i < NUMBER_OF_PMCS; i++) { - force_ += ignition::math::Vector3d(blowers_.states_[i].force_B[0], - blowers_.states_[i].force_B[1], blowers_.states_[i].force_B[2]); - torque_ += ignition::math::Vector3d(blowers_.states_[i].torque_B[0], - blowers_.states_[i].torque_B[1], blowers_.states_[i].torque_B[2]); + auto t = pmc_.Force(); + force_ += ignition::math::Vector3d(t[0], t[1], t[2]); + t = pmc_.Torque(); + torque_ += ignition::math::Vector3d(t[0], t[1], t[2]); } #else force_ = math::Vector3(0, 0, 0); torque_ = math::Vector3(0, 0, 0); for (size_t i = 0; i < NUMBER_OF_PMCS; i++) { - force_ += math::Vector3(blowers_.states_[i].force_B[0], - blowers_.states_[i].force_B[1], blowers_.states_[i].force_B[2]); - torque_ += math::Vector3(blowers_.states_[i].torque_B[0], - blowers_.states_[i].torque_B[1], blowers_.states_[i].torque_B[2]); + auto t = pmc_.Force(); + force_ += math::Vector3(t[0], t[1], t[2]); + t = pmc_.Torque(); + torque_ += math::Vector3(t[0], t[1], t[2]); } #endif // Publish telemetry @@ -358,7 +369,7 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { // Check if we need to change state based on the motor speed for (size_t i = 0; i < NUMBER_OF_PMCS; i++) { // Convert and check range - double rpm = round(blowers_.states_[i].motor_speed * RADS_PER_SEC_TO_RPM); + double rpm = round(pmc_.MotorSpeed(i) * RADS_PER_SEC_TO_RPM); if (rpm < 0 || rpm > 255) rpm = 0; // Populate as much telemetry as possible from the blower model @@ -369,8 +380,8 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { // Determine the current state based on the different between the // commanded and telemetry motor speeds, scaled appropriately static double crps, trps; - crps = state_command_scale_ * blowers_.states_[i].impeller_cmd; - trps = blowers_.states_[i].motor_speed; // Comes in rads/sec! + crps = state_command_scale_ * pmc_.ImpellerCmd(i); + trps = pmc_.MotorSpeed(i); // Comes in rads/sec! // ROS_INFO_STREAM("PMC delta C " << crps << ":: T " << trps); if (fabs(crps - trps) < state_tol_rads_per_sec_) { switch (i) { @@ -397,6 +408,23 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { return true; } + // Update Minimum Control Frequency (and cutoff time) + bool IdlingTimeoutService(ff_msgs::SetFloat::Request &req, + ff_msgs::SetFloat::Response &res) { // NOLINT + double new_timeout = req.data; + // Check if the new rate is within the safe and default limits + if (new_timeout <= max_timeout_ && new_timeout >= (20.0/control_rate_hz_)) { + watchdog_period_ = ros::Duration(new_timeout); + timer_.setPeriod(watchdog_period_); + ROS_INFO("PMC idling timeout updated."); + res.success = true; + } else { + ROS_INFO("Selected timeout is not within the safe timeout bounds."); + res.success = false; + } + return true; + } + // Called on each sensor update event void WorldUpdateCallback() { if (bypass_blower_model_) { @@ -424,7 +452,9 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { ros::Publisher pub_telemetry_, pub_state_; // State/telemetry pubs ros::Subscriber sub_command_; // Command subscriber ros::Subscriber sub_fam_; // Fam command subscriber + ros::ServiceServer update_timeout_srv_; ros::Timer timer_command_, timer_watchdog_; // Timers + ros::Duration watchdog_period_; #if GAZEBO_MAJOR_VERSION > 7 ignition::math::Vector3d force_; // Current body-frame force ignition::math::Vector3d torque_; // Current body-frame torque @@ -433,7 +463,7 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { math::Vector3 torque_; // Current body-frame torque #endif geometry_msgs::Wrench wrench_; // Used when bypassing PMC - gnc_autocode::GncBlowersAutocode blowers_; // Autocode blower iface + pmc::PMCSim pmc_; ff_hw_msgs::PmcCommand null_command_; // PMC null command event::ConnectionPtr update_; // Update event from gazeo ff_hw_msgs::PmcTelemetry telemetry_vector_; // Telemetry @@ -443,6 +473,7 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin { double state_telemetry_scale_; // Telemetry scale double state_tol_rads_per_sec_; // RPM tolerance double control_rate_hz_; // Control rate + double max_timeout_; // Maximum timeout allowed for PMC std::string frame_id_; // Frame }; diff --git a/simulation/src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc b/simulation/src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc index 7f80ac5102..a3b6ae4d9e 100644 --- a/simulation/src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc +++ b/simulation/src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc @@ -23,7 +23,7 @@ #include // FSW nodelet -#include +#include #include // Services diff --git a/simulation/src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc b/simulation/src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc index 4611308bb3..6042363f20 100644 --- a/simulation/src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc +++ b/simulation/src/gazebo_model_plugin_truth/gazebo_model_plugin_truth.cc @@ -112,11 +112,17 @@ class GazeboModelPluginTruth : public FreeFlyerModelPlugin { // Called on every discrete time tick in the simulated world void TimerCallback(ros::TimerEvent const& event) { - msg_.header.stamp = ros::Time::now(); - + msg_.header.stamp = event.current_real; + // If the rate is higher than the sim time, prevent repeated timestamps + bool publish_tf = true; + if (msg_.header.stamp == last_time_) { + publish_tf = false; + } else { + last_time_ = msg_.header.stamp; + } #if GAZEBO_MAJOR_VERSION > 7 - if (tf_) { + if (tf_ && publish_tf) { static tf2_ros::TransformBroadcaster br; msg_.transform.translation.x = GetModel()->WorldPose().Pos().X(); msg_.transform.translation.y = GetModel()->WorldPose().Pos().Y(); @@ -145,13 +151,13 @@ class GazeboModelPluginTruth : public FreeFlyerModelPlugin { ros_truth_twist_.twist.linear.x = GetModel()->WorldLinearVel().X(); ros_truth_twist_.twist.linear.y = GetModel()->WorldLinearVel().Y(); ros_truth_twist_.twist.linear.z = GetModel()->WorldLinearVel().Z(); - ros_truth_twist_.twist.angular.x = GetModel()->WorldAngularVel().X(); - ros_truth_twist_.twist.angular.y = GetModel()->WorldAngularVel().Y(); - ros_truth_twist_.twist.angular.z = GetModel()->WorldAngularVel().Z(); + ros_truth_twist_.twist.angular.x = GetModel()->RelativeAngularVel().X(); + ros_truth_twist_.twist.angular.y = GetModel()->RelativeAngularVel().Y(); + ros_truth_twist_.twist.angular.z = GetModel()->RelativeAngularVel().Z(); pub_truth_twist_.publish(ros_truth_twist_); } #else - if (tf_) { + if (tf_ && publish_tf) { static tf2_ros::TransformBroadcaster br; msg_.transform.translation.x = GetModel()->GetWorldPose().pos.x; msg_.transform.translation.y = GetModel()->GetWorldPose().pos.y; @@ -198,6 +204,7 @@ class GazeboModelPluginTruth : public FreeFlyerModelPlugin { ros::Publisher pub_truth_pose_; ros::Publisher pub_truth_twist_; ros::Timer timer_; + ros::Time last_time_; }; // Register this plugin with the simulator diff --git a/simulation/src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc b/simulation/src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc index 93fed365ba..738503c9eb 100644 --- a/simulation/src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc +++ b/simulation/src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc @@ -24,6 +24,7 @@ // FSW includes #include +#include // FSW messages #include @@ -41,16 +42,13 @@ namespace gazebo { class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { public: - GazeboSensorPluginARTags() : - FreeFlyerSensorPlugin("marker_tracking", "dock_cam", true), - active_(true) {} + GazeboSensorPluginARTags() : FreeFlyerSensorPlugin("marker_tracking", "dock_cam", true), active_(true) {} ~GazeboSensorPluginARTags() {} protected: // Called when plugin is loaded into gazebo - void LoadCallback(ros::NodeHandle *nh, - sensors::SensorPtr sensor, sdf::ElementPtr sdf) { + void LoadCallback(ros::NodeHandle* nh, sensors::SensorPtr sensor, sdf::ElementPtr sdf) { // Get a link to the parent sensor sensor_ = std::dynamic_pointer_cast(sensor); if (!sensor_) { @@ -59,42 +57,43 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { } // Check that we have a mono camera - if (sensor_->Camera()->ImageFormat() != "L8") - ROS_FATAL_STREAM("Camera format must be L8"); + if (sensor_->Camera()->ImageFormat() != "L8") ROS_FATAL_STREAM("Camera format must be L8"); // Build the nav cam Camera Model config_.AddFile("cameras.config"); + config_.AddFile("geometry.config"); config_.AddFile("dock_markers_specs.config"); config_.AddFile("dock_markers_world.config"); config_.AddFile("simulation/ar_tags.config"); if (!config_.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; + ROS_ERROR("Failed to read config files."); + return; } - if (!config_.GetReal("drawing_width", &width_)) - NODELET_ERROR("Could not read the drawing_width parameter."); + if (!config_.GetReal("drawing_width", &width_)) NODELET_ERROR("Could not read the drawing_width parameter."); - if (!config_.GetReal("drawing_height", &height_)) - NODELET_ERROR("Could not read the drawing_height parameter."); + if (!config_.GetReal("drawing_height", &height_)) NODELET_ERROR("Could not read the drawing_height parameter."); - if (!config_.GetReal("rate", &rate_)) - NODELET_ERROR("Could not read the rate parameter."); + if (!config_.GetReal("rate", &rate_)) NODELET_ERROR("Could not read the rate parameter."); - if (!config_.GetReal("delay_camera", &delay_camera_)) - NODELET_ERROR("Could not read the delay_camera parameter."); + if (!config_.GetReal("delay_camera", &delay_camera_)) NODELET_ERROR("Could not read the delay_camera parameter."); if (!config_.GetReal("delay_features", &delay_features_)) NODELET_ERROR("Could not read the delay_features parameter."); - if (!config_.GetUInt("num_samp", &num_samp_)) - NODELET_ERROR("Could not read the num_samp parameter."); + if (!config_.GetUInt("num_samp", &num_samp_)) NODELET_ERROR("Could not read the num_samp parameter."); + + if (!config_.GetUInt("num_features", &num_features_)) NODELET_ERROR("Could not read the num_features parameter."); - if (!config_.GetUInt("num_features", &num_features_)) - NODELET_ERROR("Could not read the num_features parameter."); + if (!config_.GetTable("markers_world", &markers_)) NODELET_ERROR("Could not read the markers_world parameter."); - if (!config_.GetTable("markers_world", &markers_)) - NODELET_ERROR("Could not read the markers_world parameter."); + Eigen::Vector3d world_t_dock; + Eigen::Quaterniond world_Q_dock; + if (!msg_conversions::config_read_transform(&config_, "world_dock_transform", &world_t_dock, &world_Q_dock)) + NODELET_ERROR("Could not read world_T_dock transform."); + world_T_dock_ = Eigen::Isometry3d::Identity(); + world_T_dock_.translation() = world_t_dock; + world_T_dock_.linear() = world_Q_dock.toRotationMatrix(); for (int marker_i = 0; marker_i < markers_.GetSize(); marker_i++) { config_reader::ConfigReader::Table current_marker; @@ -112,12 +111,10 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { } // Create a publisher for the registration messages - pub_reg_ = nh->advertise( - TOPIC_LOCALIZATION_AR_REGISTRATION, 1); + pub_reg_ = nh->advertise(TOPIC_LOCALIZATION_AR_REGISTRATION, 1); // Create a publisher for the feature messages - pub_feat_ = nh->advertise( - TOPIC_LOCALIZATION_AR_FEATURES, 1); + pub_feat_ = nh->advertise(TOPIC_LOCALIZATION_AR_FEATURES, 1); // Only do this once msg_feat_.header.frame_id = std::string(FRAME_NAME_DOCK); @@ -125,55 +122,26 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { } // Only send measurements when extrinsics are available - void OnExtrinsicsReceived(ros::NodeHandle *nh) { + void OnExtrinsicsReceived(ros::NodeHandle* nh) { // Enable mapped landmarks - srv_enable_ = nh->advertiseService(SERVICE_LOCALIZATION_AR_ENABLE, - &GazeboSensorPluginARTags::EnableService, this); + srv_enable_ = nh->advertiseService(SERVICE_LOCALIZATION_AR_ENABLE, &GazeboSensorPluginARTags::EnableService, this); // Timer triggers registration - timer_registration_ = nh->createTimer(ros::Duration(ros::Rate(rate_)), - &GazeboSensorPluginARTags::SendRegistration, this, false, true); + timer_registration_ = + nh->createTimer(ros::Duration(ros::Rate(rate_)), &GazeboSensorPluginARTags::SendRegistration, this, false, true); // Timer triggers features - timer_features_ = nh->createTimer(ros::Duration(0.8 / rate_), - &GazeboSensorPluginARTags::SendFeatures, this, true, false); + timer_features_ = + nh->createTimer(ros::Duration(0.8 / rate_), &GazeboSensorPluginARTags::SendFeatures, this, true, false); } // Enable or disable the feature timer - bool EnableService(ff_msgs::SetBool::Request & req, - ff_msgs::SetBool::Response & res) { + bool EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res) { active_ = req.enable; res.success = true; return true; } - // Manage the extrinsics based on the sensor type - bool GetDockLocation(Eigen::Affine3d & wTd) { - // Create a buffer and listener for TF2 transforms - static tf2_ros::Buffer buffer; - static tf2_ros::TransformListener listener(buffer); - // Get extrinsics from framestore - try { - // Lookup the transform for this sensor - geometry_msgs::TransformStamped tf = buffer.lookupTransform( - "world", "dock/body", ros::Time(0)); - // Handle the transform for all sensor types - wTd = ( - Eigen::Translation3d( - tf.transform.translation.x, - tf.transform.translation.y, - tf.transform.translation.z) * - Eigen::Quaterniond( - tf.transform.rotation.w, - tf.transform.rotation.x, - tf.transform.rotation.y, - tf.transform.rotation.z)); - } catch (tf2::TransformException &ex) { - return false; - } - return true; - } - // Called when featured must be sent void SendRegistration(ros::TimerEvent const& event) { if (!active_) return; @@ -191,58 +159,36 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { // Copy over the camera id to the feature message msg_feat_.camera_id = msg_reg_.camera_id; - // In order to get meaningful data we need a dock location - Eigen::Affine3d wTd; - if (!GetDockLocation(wTd)) { - timer_features_.stop(); - return; - } - // Handle the transform for all sensor types - Eigen::Affine3d wTb = ( - #if GAZEBO_MAJOR_VERSION > 7 - Eigen::Translation3d( - GetModel()->WorldPose().Pos().X(), - GetModel()->WorldPose().Pos().Y(), - GetModel()->WorldPose().Pos().Z()) * - Eigen::Quaterniond( - GetModel()->WorldPose().Rot().W(), - GetModel()->WorldPose().Rot().X(), - GetModel()->WorldPose().Rot().Y(), - GetModel()->WorldPose().Rot().Z())); - #else - Eigen::Translation3d( - GetModel()->GetWorldPose().pos.x, - GetModel()->GetWorldPose().pos.y, - GetModel()->GetWorldPose().pos.z) * - Eigen::Quaterniond( - GetModel()->GetWorldPose().rot.w, - GetModel()->GetWorldPose().rot.x, - GetModel()->GetWorldPose().rot.y, - GetModel()->GetWorldPose().rot.z)); - #endif - Eigen::Affine3d bTs = ( - Eigen::Translation3d( - sensor_->Pose().Pos().X(), - sensor_->Pose().Pos().Y(), - sensor_->Pose().Pos().Z()) * - Eigen::Quaterniond( - sensor_->Pose().Rot().W(), - sensor_->Pose().Rot().X(), - sensor_->Pose().Rot().Y(), - sensor_->Pose().Rot().Z())); - Eigen::Affine3d dTs = wTd.inverse() * wTb * bTs; + Eigen::Isometry3d world_T_body = ( +#if GAZEBO_MAJOR_VERSION > 7 + Eigen::Translation3d(GetModel()->WorldPose().Pos().X(), GetModel()->WorldPose().Pos().Y(), + GetModel()->WorldPose().Pos().Z()) * + Eigen::Quaterniond(GetModel()->WorldPose().Rot().W(), GetModel()->WorldPose().Rot().X(), + GetModel()->WorldPose().Rot().Y(), GetModel()->WorldPose().Rot().Z())); +#else + Eigen::Translation3d(GetModel()->GetWorldPose().pos.x, GetModel()->GetWorldPose().pos.y, + GetModel()->GetWorldPose().pos.z) * + Eigen::Quaterniond(GetModel()->GetWorldPose().rot.w, GetModel()->GetWorldPose().rot.x, + GetModel()->GetWorldPose().rot.y, GetModel()->GetWorldPose().rot.z)); +#endif + msg_feat_.header.stamp = ros::Time::now(); + + Eigen::Isometry3d body_T_dock_cam = + (Eigen::Translation3d(sensor_->Pose().Pos().X(), sensor_->Pose().Pos().Y(), sensor_->Pose().Pos().Z()) * + Eigen::Quaterniond(sensor_->Pose().Rot().W(), sensor_->Pose().Rot().X(), sensor_->Pose().Rot().Y(), + sensor_->Pose().Rot().Z())); + Eigen::Isometry3d dock_T_dock_cam = world_T_dock_.inverse() * world_T_body * body_T_dock_cam; // Initialize the camera paremeters static camera::CameraParameters cam_params(&config_, "dock_cam"); - static camera::CameraModel camera(Eigen::Vector3d(0, 0, 0), - Eigen::Matrix3d::Identity(), cam_params); + static camera::CameraModel camera(Eigen::Vector3d(0, 0, 0), Eigen::Matrix3d::Identity(), cam_params); // Assemble the feature message - msg_feat_.pose.position.x = dTs.translation().x(); - msg_feat_.pose.position.y = dTs.translation().y(); - msg_feat_.pose.position.z = dTs.translation().z(); - Eigen::Quaterniond q(dTs.rotation()); + msg_feat_.pose.position.x = dock_T_dock_cam.translation().x(); + msg_feat_.pose.position.y = dock_T_dock_cam.translation().y(); + msg_feat_.pose.position.z = dock_T_dock_cam.translation().z(); + Eigen::Quaterniond q(dock_T_dock_cam.rotation()); msg_feat_.pose.orientation.w = q.w(); msg_feat_.pose.orientation.x = q.x(); msg_feat_.pose.orientation.y = q.y(); @@ -256,11 +202,10 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { // using PnP on the points themselves. However, it's easier to just pull // this information from the simulation ground truth. Eigen::Vector3d pt_d = marker_positions_[i]; - Eigen::Vector3d pt_c = dTs.inverse() * pt_d; + Eigen::Vector3d pt_c = dock_T_dock_cam.inverse() * pt_d; // Check if the feature is in the field of view - if (!camera.IsInFov(pt_c)) - continue; + if (!camera.IsInFov(pt_c)) continue; // Get the image plane coordinates Eigen::Vector2d uv = camera.ImageCoordinates(pt_c); @@ -279,7 +224,6 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { // Send features void SendFeatures(ros::TimerEvent const& event) { if (!active_) return; - msg_feat_.header.stamp = ros::Time::now(); pub_feat_.publish(msg_feat_); } @@ -301,8 +245,9 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { unsigned int num_samp_; config_reader::ConfigReader::Table markers_; std::vector marker_positions_; + Eigen::Isometry3d world_T_dock_; }; GZ_REGISTER_SENSOR_PLUGIN(GazeboSensorPluginARTags) -} // namespace gazebo +} // namespace gazebo diff --git a/simulation/src/gazebo_sensor_plugin_haz_cam/gazebo_sensor_plugin_haz_cam.cc b/simulation/src/gazebo_sensor_plugin_haz_cam/gazebo_sensor_plugin_haz_cam.cc index fc87cfbf62..e1e9399974 100644 --- a/simulation/src/gazebo_sensor_plugin_haz_cam/gazebo_sensor_plugin_haz_cam.cc +++ b/simulation/src/gazebo_sensor_plugin_haz_cam/gazebo_sensor_plugin_haz_cam.cc @@ -260,6 +260,11 @@ class GazeboSensorPluginHazCam : public FreeFlyerSensorPlugin { // seems to be not fresh. The ImageCallback() above publishes the // correct intensity on a separate topic. const unsigned char * image_data = camera_->ImageData(); + + if (!image_data) { + // first frame of image data may not be received + return; + } for (size_t i = 0; i < num_floats / num_channels_; i++) cloud_data[num_channels_ * i + num_channels_ - 1] = static_cast(image_data[i]); diff --git a/simulation/src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc b/simulation/src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc index bc3401e78e..def7683515 100644 --- a/simulation/src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc +++ b/simulation/src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc @@ -192,6 +192,8 @@ class GazeboSensorPluginOpticalFlow : public FreeFlyerSensorPlugin { sensor_->Pose().Rot().Z())); Eigen::Affine3d wTs = wTb * bTs; + msg_feat_.header.stamp = ros::Time::now(); + // Initialize the camera paremeters static camera::CameraParameters cam_params(&config_, "nav_cam"); static camera::CameraModel camera(Eigen::Vector3d(0, 0, 0), @@ -206,7 +208,6 @@ class GazeboSensorPluginOpticalFlow : public FreeFlyerSensorPlugin { // Send a registration pulse void SendFeatures(ros::TimerEvent const& event) { if (!active_) return; - msg_feat_.header.stamp = ros::Time::now(); pub_feat_.publish(msg_feat_); } diff --git a/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc b/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc deleted file mode 100644 index 4527062942..0000000000 --- a/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc +++ /dev/null @@ -1,282 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * https://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace gazebo { - -class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin { - public: - GazeboSensorPluginSciCam() : - FreeFlyerSensorPlugin("sci_cam", "sci_cam", true), - continuousPictureTaking_(false), takeSinglePicture_(false), rate_(0.0) {} - - ~GazeboSensorPluginSciCam() { - if (update_) { - #if GAZEBO_MAJOR_VERSION > 7 - update_.reset(); - #else - sensor_->DisconnectUpdated(update_); - #endif - } - } - - protected: - // Called when plugin is loaded into gazebo - void LoadCallback(ros::NodeHandle *nh, - sensors::SensorPtr sensor, sdf::ElementPtr sdf) { - // Get a link to the parent sensor - sensor_ = std::dynamic_pointer_cast(sensor); - if (!sensor_) { - gzerr << "GazeboSensorPluginSciCam requires a parent camera sensor.\n"; - return; - } - - // Check that we have a color camera - if (sensor_->Camera()->ImageFormat() != "B8G8R8") - ROS_FATAL_STREAM("Camera format must be B8G8R8"); - - // Set image constants - sci_cam_image_msg_.is_bigendian = false; - sci_cam_image_msg_.header.frame_id = GetFrame(); - sci_cam_image_msg_.encoding = sensor_msgs::image_encodings::BGR8; - - // Create subscriber to DDS commands though which the sci cam will be controlled - dds_cmd_sub_ = nh->subscribe(TOPIC_COMMUNICATIONS_DDS_COMMAND, 10, - &GazeboSensorPluginSciCam::DdsCmdCallback, this); - - // Create publishers for sci cam image, pose, and camera info - pub_sci_cam_image_ = nh->advertise(TOPIC_HARDWARE_SCI_CAM, 2, - boost::bind(&GazeboSensorPluginSciCam::ToggleCallback, this), - boost::bind(&GazeboSensorPluginSciCam::ToggleCallback, this)); - pub_sci_cam_pose_ = nh->advertise(TOPIC_SCI_CAM_SIM_POSE, 10); - pub_sci_cam_info_ = nh->advertise(TOPIC_SCI_CAM_SIM_INFO, 10); - - // Read configuration - config_reader::ConfigReader config; - config.AddFile("simulation/simulation.config"); - if (!config.ReadFiles()) { - ROS_FATAL("Failed to read simulation config file."); - return; - } - bool dos = true; - if (!config.GetBool("disable_cameras_on_speedup", &dos)) - ROS_FATAL("Could not read the disable_cameras_on_speedup parameter."); - if (!config.GetReal("sci_cam_rate", &rate_)) - ROS_FATAL("Could not read the sci_cam_rate parameter."); - - bool cp; - if (config.GetBool("sci_cam_continuous_picture_taking", &cp)) { - continuousPictureTaking_ = cp; - } - config.Close(); - - // If we have a sped up simulation and we need to disable the camera - double simulation_speed = 1.0; - if (nh->getParam("/simulation_speed", simulation_speed)) - if (simulation_speed > 1.0 && dos) rate_ = 0.0; - - // Toggle if the camera is active or not - ToggleCallback(); - } - - // Only send measurements when extrinsics are available - void OnExtrinsicsReceived(ros::NodeHandle *nh) { - // Connect to the camera update event. - update_ = sensor_->ConnectUpdated( - std::bind(&GazeboSensorPluginSciCam::ImageCallback, this)); - } - - // Turn camera on or off based on topic subscription - void ToggleCallback() { - if (pub_sci_cam_image_.getNumSubscribers() > 0 && rate_ > 0) { - sensor_->SetUpdateRate(rate_); - sensor_->SetActive(true); - } else { - sensor_->SetUpdateRate(0.0001); - sensor_->SetActive(false); - } - } - - // Out of string: "{"name": "turnOnContinuousPictureTaking"}" - // collect the part in the second set of quotes. - // Some honest json parsing could be used here. - std::string parseJsonStr(std::string const& json_str) { - size_t start = 0; - size_t colon_pos = json_str.find(":", start); - if (colon_pos == std::string::npos) { - return ""; - } - size_t quote1_pos = json_str.find("\"", colon_pos + 1); - if (quote1_pos == std::string::npos) { - return ""; - } - size_t quote2_pos = json_str.find("\"", quote1_pos + 1); - - if (quote2_pos == std::string::npos) { - return ""; - } - - std::string parsed = json_str.substr(quote1_pos + 1, quote2_pos - quote1_pos - 1); - - return parsed; - } - - // Called when a dds command is received. Process only guest science - // sci cam control commands. - void DdsCmdCallback(ff_msgs::CommandStamped const& cmd) { - // Process only guest science commands - if (cmd.cmd_name != ff_msgs::CommandConstants::CMD_NAME_CUSTOM_GUEST_SCIENCE) { - // Only process custom sci cam commands - return; - } - - if (cmd.args.size() != 2) { - // Custom sci cam commands have two arguments - return; - } - - if (cmd.args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd.args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING ) { - return; - } - - std::string app_name = cmd.args[0].s.data(); - - // Process only sci cam commands - if (app_name != "gov.nasa.arc.irg.astrobee.sci_cam_image") { - return; - } - - std::string json_str = cmd.args[1].s.data(); - ROS_INFO_STREAM("Received command: " << json_str); - - std::string action = parseJsonStr(json_str); - if (action == "") - return; - - // Record the desired intention. Use a lock. - { - const std::lock_guard lock(sci_cam_image_lock); - if (action == "takeSinglePicture") { - takeSinglePicture_ = true; - continuousPictureTaking_ = false; - } else if (action == "turnOnContinuousPictureTaking") { - takeSinglePicture_ = false; - continuousPictureTaking_ = true; - } else if (action == "turnOffContinuousPictureTaking") { - takeSinglePicture_ = false; - continuousPictureTaking_ = false; - } else { - ROS_FATAL_STREAM("Unknown sci_cam command: " << action); - } - } - - return; - } - - // Called when a new image must be rendered - void ImageCallback() { - // Quickly record the current time and current pose before doing other computations - ros::Time curr_time = ros::Time::now(); - - // Publish the sci cam pose - #if GAZEBO_MAJOR_VERSION > 7 - Eigen::Affine3d sensor_to_world = SensorToWorld(GetModel()->WorldPose(), sensor_->Pose()); - #else - Eigen::Affine3d sensor_to_world = SensorToWorld(GetModel()->GetWorldPose(), sensor_->Pose()); - #endif - sci_cam_pose_msg_.header.frame_id = GetFrame(); - - sci_cam_pose_msg_.header.stamp = curr_time; // it is very important to get the time right - sci_cam_pose_msg_.pose.position.x = sensor_to_world.translation().x(); - sci_cam_pose_msg_.pose.position.y = sensor_to_world.translation().y(); - sci_cam_pose_msg_.pose.position.z = sensor_to_world.translation().z(); - Eigen::Quaterniond q(sensor_to_world.rotation()); - sci_cam_pose_msg_.pose.orientation.w = q.w(); - sci_cam_pose_msg_.pose.orientation.x = q.x(); - sci_cam_pose_msg_.pose.orientation.y = q.y(); - sci_cam_pose_msg_.pose.orientation.z = q.z(); - pub_sci_cam_pose_.publish(sci_cam_pose_msg_); - - // Publish the sci cam intrinsics - sci_cam_info_msg_.header.frame_id = GetFrame(); - sci_cam_info_msg_.header.stamp = curr_time; // it is very important to get the time right - FillCameraInfo(sensor_->Camera(), sci_cam_info_msg_); // fill in from the camera pointer - pub_sci_cam_info_.publish(sci_cam_info_msg_); - - // Do not publish unless specifically told to - if (!continuousPictureTaking_ && !takeSinglePicture_) { - return; - } - - // Publish the sci cam image - // Record not the current time, but the time when the image was acquired - sci_cam_image_msg_.header.stamp.sec = sensor_->LastMeasurementTime().sec; - sci_cam_image_msg_.header.stamp.nsec = sensor_->LastMeasurementTime().nsec; - sci_cam_image_msg_.height = sensor_->ImageHeight(); - sci_cam_image_msg_.width = sensor_->ImageWidth(); - sci_cam_image_msg_.step = sci_cam_image_msg_.width * 3; - sci_cam_image_msg_.data.resize(sci_cam_image_msg_.step * sci_cam_image_msg_.height); - const uint8_t* data_start = reinterpret_cast(sensor_->ImageData()); - std::copy(data_start, data_start + sci_cam_image_msg_.step * sci_cam_image_msg_.height, - sci_cam_image_msg_.data.begin()); - pub_sci_cam_image_.publish(sci_cam_image_msg_); - - if (takeSinglePicture_) { - // Done taking a single picture. Use a lock to change this flag. - const std::lock_guard lock(sci_cam_image_lock); - takeSinglePicture_ = false; - } - - return; - } - - private: - sensor_msgs::Image sci_cam_image_msg_; - geometry_msgs::PoseStamped sci_cam_pose_msg_; - sensor_msgs::CameraInfo sci_cam_info_msg_; - - bool continuousPictureTaking_; - bool takeSinglePicture_; - std::mutex sci_cam_image_lock; - - ros::Publisher pub_sci_cam_image_; - ros::Publisher pub_sci_cam_pose_; - ros::Publisher pub_sci_cam_info_; - ros::Subscriber dds_cmd_sub_; - std::shared_ptr sensor_; - event::ConnectionPtr update_; - double rate_; -}; - -GZ_REGISTER_SENSOR_PLUGIN(GazeboSensorPluginSciCam) - -} // namespace gazebo diff --git a/simulation/src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc b/simulation/src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc index f51b7dd849..8ab7c1fd8b 100644 --- a/simulation/src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc +++ b/simulation/src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc @@ -163,6 +163,8 @@ class GazeboSensorPluginSparseMap : public FreeFlyerSensorPlugin { Eigen::Affine3d sensor_to_world = SensorToWorld(GetModel()->GetWorldPose(), sensor_->Pose()); #endif + msg_feat_.header.stamp = ros::Time::now(); + // Initialize the camera parameters static camera::CameraParameters cam_params(&config_, "nav_cam"); static camera::CameraModel camera(Eigen::Vector3d(0, 0, 0), @@ -185,12 +187,13 @@ class GazeboSensorPluginSparseMap : public FreeFlyerSensorPlugin { GetWorld()->Physics()->InitForThread(); boost::unique_lock lock(*( GetWorld()->Physics()->GetPhysicsUpdateMutex())); + std::string physics = boost::any_cast(GetWorld()->Physics()->GetParam("type")); #else GetWorld()->GetPhysicsEngine()->InitForThread(); boost::unique_lock lock(*( GetWorld()->GetPhysicsEngine()->GetPhysicsUpdateMutex())); + std::string physics = boost::any_cast(GetWorld()->GetPhysicsEngine()->GetParam("type")); #endif - // Create a new ray in the world size_t i = 0; for (; i < num_samp_ && msg_feat_.landmarks.size() < num_features_; i++) { @@ -211,17 +214,24 @@ class GazeboSensorPluginSparseMap : public FreeFlyerSensorPlugin { // Collision detection double dist; - std::string entity; - - // Set the start anf end points of the ray - shape_->SetPoints( - ignition::math::Vector3d(n_w.x(), n_w.y(), n_w.z()), - ignition::math::Vector3d(f_w.x(), f_w.y(), f_w.z())); - shape_->GetIntersection(dist, entity); - - // If we don't have an entity then we didnt collide - if (entity.empty()) - continue; + if (physics != "dart") { + std::string entity; + + // Set the start anf end points of the ray + shape_->SetPoints( + ignition::math::Vector3d(n_w.x(), n_w.y(), n_w.z()), + ignition::math::Vector3d(f_w.x(), f_w.y(), f_w.z())); + shape_->GetIntersection(dist, entity); + + // If we don't have an entity then we didnt collide + if (entity.empty()) + continue; + } else { + // The ray functionality does not work for dart physics engine + // Here we just fake some random distance values + // https://answers.gazebosim.org/question/16241/ray-sensor-doesnt-work-properly-with-dart/ + dist = std::max(near_clip_, std::min(static_cast(rand() % 1000) / 500, far_clip_)); // NOLINT + } // Get the landmark coordinate Eigen::Vector3d p_w = n_w + dist * (f_w - n_w).normalized(); @@ -241,7 +251,6 @@ class GazeboSensorPluginSparseMap : public FreeFlyerSensorPlugin { // Send off the features void SendFeatures(ros::TimerEvent const& event) { if (!active_) return; - msg_feat_.header.stamp = ros::Time::now(); pub_feat_.publish(msg_feat_); } diff --git a/simulation/src/gazebo_system_plugin_client/gazebo_system_plugin_client.cc b/simulation/src/gazebo_system_plugin_client/gazebo_system_plugin_client.cc index d58a0f7b84..c5ee0365b4 100644 --- a/simulation/src/gazebo_system_plugin_client/gazebo_system_plugin_client.cc +++ b/simulation/src/gazebo_system_plugin_client/gazebo_system_plugin_client.cc @@ -50,7 +50,7 @@ class SystemPluginClient : public SystemPlugin { if (!scene || !scene->Initialized()) return; for (uint32_t l = 0; l < scene->LightCount(); l++) { - #if GAZEBO_MAJOR_VERSION > 8 + #if GAZEBO_MAJOR_VERSION > 9 rendering::LightPtr light = scene->LightByIndex(l); #else rendering::LightPtr light = scene->GetLight(l); diff --git a/simulation/worlds/iss.world b/simulation/worlds/iss.world index 247e9e45d9..fbc8785120 100644 --- a/simulation/worlds/iss.world +++ b/simulation/worlds/iss.world @@ -1,7 +1,13 @@ - + + 0.008 + 1 + 125 + + + 0.008 1 125 @@ -42,7 +48,7 @@ model://astrobee_dock - 9.82 -9.50 4.41 0 0 0 + 9.5868938 -10.0604698 4.2955872 0 0 0 diff --git a/submodules/android b/submodules/android index 04d92486ca..ad16e76730 160000 --- a/submodules/android +++ b/submodules/android @@ -1 +1 @@ -Subproject commit 04d92486cad31ef0b6c1323eb9f4e5fc9dab6518 +Subproject commit ad16e76730c664a0cc890bd40010973edec2acdf diff --git a/submodules/common b/submodules/common deleted file mode 160000 index 99d4d1a9a7..0000000000 --- a/submodules/common +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 99d4d1a9a77a98f12cf4771cbdddb788e9a24d32 diff --git a/submodules/platform b/submodules/platform index 36ae14df25..e77a160800 160000 --- a/submodules/platform +++ b/submodules/platform @@ -1 +1 @@ -Subproject commit 36ae14df25a79000c91bd6510f0cee9edddb0677 +Subproject commit e77a160800eb8a935ece6e3d02762a06a7a265a7 diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt deleted file mode 100644 index c570e9f306..0000000000 --- a/tools/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -add_subdirectory(ekf_bag) -add_subdirectory(rviz_visualizer) -add_subdirectory(visualeyez) -add_subdirectory(simulator) - -if (FFMPEG_FOUND) - add_subdirectory(ekf_video) -endif (FFMPEG_FOUND) diff --git a/tools/bag_processing/CMakeLists.txt b/tools/bag_processing/CMakeLists.txt new file mode 100644 index 0000000000..af0f1fa0dc --- /dev/null +++ b/tools/bag_processing/CMakeLists.txt @@ -0,0 +1,46 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(bag_processing) + +if (USE_ROS) + ## Compile as C++14, supported in ROS Kinetic and newer + add_compile_options(-std=c++14) + + ## Find catkin macros and libraries + find_package(catkin2 REQUIRED COMPONENTS + rosbag + ) + + catkin_package( + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS rosbag + ) + + if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(test/test_fix_all.test) + endif() +endif (USE_ROS) + +find_package(catkin REQUIRED COMPONENTS rospy) +catkin_python_setup() + +if (CATKIN_ENABLE_TESTING) + catkin_add_nosetests(test/test_pixel_utils.py) +endif() diff --git a/tools/bag_processing/package.xml b/tools/bag_processing/package.xml new file mode 100644 index 0000000000..94507c10c4 --- /dev/null +++ b/tools/bag_processing/package.xml @@ -0,0 +1,19 @@ + + bag_processing + 1.0.0 + + The bag processing package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + rosbag + rosbag + diff --git a/tools/bag_processing/readme.md b/tools/bag_processing/readme.md new file mode 100644 index 0000000000..f84b19a130 --- /dev/null +++ b/tools/bag_processing/readme.md @@ -0,0 +1,102 @@ +\page bag_processing Bag Processing + +# Package Overview +The bag processing package provides several helper tools for handling bagfiles. + +# Usage Instructions +For each script, run `rosrun bag_processing script_name.py -h` for further details and +usage instructions. + +# Scripts +## apply_histogram_equalization_to_images +Applies either CLAHE or standard histogram equalization to images in a bagfile. + +## check_bag_for_gaps +Prints gaps above provided maximum time for a bagfile and topic using either message header times or bag receive times. +Also prints stats for the time differences for messages with the provided topic in the bagfile. + +## clock_skew +Analyze clock skew LLP->MLP and HLP->MLP. Inputs an `ars_default.bag` +containing the `/mgt/sys_monitor/time_sync` topic. + +## csv_join +Join two CSV files by timestamp. (After creating the CSV files by exporting a message topic from a bag.) + +## get_msg_stats +Prints stats for a numeric field of a topic in a bagfile. + +## rosbag_debayer +Creates a new bagfile with grayscale and color images for the provided bags using their bayer encoded images. +If no bags are provided, runs conversion for each bag in the current directory. + +## rosbag_detect_bad_topics +Detect bad topics that the `rosbag` API can't handle. (The main problem so +far has been `rosjava` messages that have incomplete message definition +dependency information that causes `rosbag` to raise an exception when it +tries to deserialize the message.) + +## rosbag_fix_all +Master script to apply all passes of processing needed to fix our legacy +bag files. The actual processing steps are found in +`Makefile.rosbag_fix_all`. The strategy is explained in [1]. + +[1] https://github.com/nasa/astrobee/blob/develop/doc/general_documentation/maintaining_telemetry.md + +## rosbag_merge +Merges bagfiles with given prefix in the current directory. Creates merged +bagfiles for each provided bag prefix or for each prefix in the current directory +if none are provided. + +## rosbag_rewrite_types +Tool that applies rules to fix bag files, as specified in +`rosbag_rewrite_types_rules.json`. It performs two types of fix: + +1. Fix incomplete message definition metadata, as written by `rosjava` + nodes. Which topics to fix is specified in the + `fix_message_definition_topic_patterns` field of the rules file. + +2. For message types whose definition changed in a way that makes + migration infeasible, changes the message type name to be the + "frozen" type name, so that migration is not needed. Which message + types to fix is specified in the `rename_types` field of the rules + file. + +This script is normally invoked by `rosbag_fix_all`. + +## rosbag_sample +Takes a minimal sample from each message in a bag for compatibility +testing. If the full bags are multiple gigabytes in size and are not +needed for basic backward compatibility testing. + +## rosbag_splice +Interactively splices a bagfile at selected timestamps to create multiple +smaller bagfiles, which when combined span the original bagfile. Iterates +through the bagfile images to allow the user to select splice points. + +## rosbag_topic_filter +Filter `rosbag` messages based on topic. Like a subset of `rosbag filter` +functionality, but more robust to incomplete message definitions because +it doesn't need to deserialize the messages to filter on topic. + +## rosbag_trim +Creates a new bagfile with trimmed start and end times from a provided +bagfile. + +## rosbag_verify +Verify correctness of a ROS bag, as output by rosbag_fix_all.py. This +performs some simple checks that are not part of the standard "rosbag +check". + +# Utilities + +## utilities/bmr_renumber_enum +This is not a standalone script. It is a library that provides utility +functions to be used in \*.bmr bag migration rules to help with migrating +legacy messages that contain an enumerated field where the label +numbering has changed. + +# Resources + +Contains the decoding files for the haz cam extended topics. These were +generated with data from the robot checkout activities and ISAAC activities +using the scripts in \ref picoflexx_python diff --git a/tools/bag_processing/resources/bsharp_haz_xyz_coeff.npy b/tools/bag_processing/resources/bsharp_haz_xyz_coeff.npy new file mode 100755 index 0000000000..4f73bc0779 Binary files /dev/null and b/tools/bag_processing/resources/bsharp_haz_xyz_coeff.npy differ diff --git a/tools/bag_processing/resources/bumble_haz_xyz_coeff.npy b/tools/bag_processing/resources/bumble_haz_xyz_coeff.npy new file mode 100755 index 0000000000..3042c96d18 Binary files /dev/null and b/tools/bag_processing/resources/bumble_haz_xyz_coeff.npy differ diff --git a/tools/bag_processing/resources/queen_haz_xyz_coeff.npy b/tools/bag_processing/resources/queen_haz_xyz_coeff.npy new file mode 100755 index 0000000000..b2c462c40d Binary files /dev/null and b/tools/bag_processing/resources/queen_haz_xyz_coeff.npy differ diff --git a/tools/bag_processing/resources/wannabee_haz_xyz_coeff.npy b/tools/bag_processing/resources/wannabee_haz_xyz_coeff.npy new file mode 100755 index 0000000000..f91c18d4e2 Binary files /dev/null and b/tools/bag_processing/resources/wannabee_haz_xyz_coeff.npy differ diff --git a/tools/bag_processing/scripts/Makefile.rosbag_fix_all b/tools/bag_processing/scripts/Makefile.rosbag_fix_all new file mode 100644 index 0000000000..a6d3330cf0 --- /dev/null +++ b/tools/bag_processing/scripts/Makefile.rosbag_fix_all @@ -0,0 +1,117 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# This Makefile is designed to be used by the rosbag_fix_all.py script. + +# The paths to scripts and configuration files should work as long as +# the relevant ROS packages (ff_msgs, bag_processing, optionally +# isaac_msgs) are installed and the necessary devel/setup.bash files +# have been sourced. + +# As a fallback, you can override the relevant variables (REWRITE_TYPES, +# FF_MSGS_BMR, ISAAC_MSGS_BMR) in your shell environment before running +# rosbag_fix_all.py. + +ROS_VERSION=$(shell rosversion -d) + +# Find script executables +ifeq (,${REWRITE_TYPES}) + REWRITE_TYPES := $(shell catkin_find --first-only bag_processing scripts/rosbag_rewrite_types.py || echo FAIL) + ifneq (,$(findstring FAIL,$(REWRITE_TYPES))) + $(error could not find path to rosbag_rewrite_types.py) + endif +endif +ifeq (,${ROSBAG_VERIFY}) + ROSBAG_VERIFY := $(shell catkin_find --first-only bag_processing scripts/rosbag_verify.py || echo FAIL) + ifneq (,$(findstring FAIL,$(ROSBAG_VERIFY))) + $(error could not find path to rosbag_verify.py) + endif +endif +ifeq (,${ROSBAG_DEBAYER}) + ROSBAG_DEBAYER := $(shell catkin_find --first-only bag_processing scripts/rosbag_debayer.py || echo FAIL) + ifneq (,$(findstring FAIL,$(ROSBAG_DEBAYER))) + $(error could not find path to rosbag_debayer.py) + endif +endif +ifeq (,${ROSBAG_SPLIT_DEPTH}) + ROSBAG_SPLIT_DEPTH := $(shell catkin_find --first-only pico_driver scripts/pico_split_extended.py || echo FAIL) + ifneq (,$(findstring FAIL,$(ROSBAG_SPLIT_DEPTH))) + $(error could not find path to pico_split_extended.py) + endif +endif +ifeq (,${ROSBAG_FILTER}) + ROSBAG_FILTER := $(shell catkin_find --first-only bag_processing scripts/rosbag_topic_filter.py || echo FAIL) + ifneq (,$(findstring FAIL,$(ROSBAG_FILTER))) + $(error could not find path to rosbag_topic_filter.py) + endif +endif + +# Find rosbag migration definitions +ifeq (,${FF_MSGS_BMR}) + FF_MSGS_BMR := $(shell catkin_find --first-only ff_msgs bmr || echo FAIL) + ifneq (,$(findstring FAIL,$(FF_MSGS_BMR))) + $(error could not find path to ff_msgs package bmr folder) + endif +endif +BMR = $(sort $(wildcard ${FF_MSGS_BMR}/*.bmr) $(wildcard ${FF_MSGS_BMR}/${ROS_VERSION}/*.bmr)) +REWRITE_TYPES_ARGS += -r ${FF_MSGS_BMR}/rosbag_rewrite_types_rules.json + +ifeq (,${ISAAC_MSGS_BMR}) + ISAAC_MSGS_BMR = $(shell catkin_find --first-only isaac_msgs bmr 2> /dev/null) +endif +ifneq (,${ISAAC_MSGS_BMR}) + # If isaac_msgs is available, apply extra rules. Shouldn't hurt non-ISAAC bags. + BMR += $(sort $(wildcard ${ISAAC_MSGS_BMR}/*.bmr)) + REWRITE_TYPES_ARGS += -r ${ISAAC_MSGS_BMR}/rosbag_rewrite_types_rules.json +endif + +# Fix rosjava scicam bug +%.rewrite_types.bag: %.bag + ${REWRITE_TYPES} -v ${REWRITE_TYPES_ARGS} $< -o $@ + +# Fix old message definitions +%.migrate_old.bag: %.rewrite_types.bag + rosbag fix $< $@ ${BMR} + rm $< # explicitly delete right away to save disk space + +migrate_check1-%: %.migrate_old.bag + rosbag check $< # test: post-migration consistency + +migrate_check2-%: %.migrate_old.bag + # compare with original bag to make sure data is there + ${ROSBAG_VERIFY} ${ROSBAG_VERIFY_ARGS} $< $(<:.migrate_old.bag=.bag) + +# Debayer the nav cam +%.debayer.bag: %.migrate_old.bag migrate_check1-% migrate_check2-% + ${ROSBAG_DEBAYER} ${ROSBAG_DEBAYER_ARGS} $< -o $@ + if [ -f $< ]; then rm $< ; fi # explicitly delete right away to save disk space + +# Decode the haz cam into point cloud + image +%.depth_split.bag: %.debayer.bag + ${ROSBAG_SPLIT_DEPTH} ${ROSBAG_SPLIT_DEPTH_ARGS} $< -o $@ + if [ -f $< ]; then rm $< ; fi # explicitly delete right away to save disk space + +# Filter the rosbag topics +%.fix_all.bag: %.depth_split.bag +ifeq (,${ROSBAG_FILTER_ARGS}) + mv $< $@ # skip no-op filtering +else + ${ROSBAG_FILTER} ${ROSBAG_FILTER_ARGS} -o $@ $< +endif + if [ -f $< ]; then rm $< ; fi # explicitly delete right away to save disk space + +.PRECIOUS: %.rewrite_types.bag %.migrate_old.bag %.debayer.bag %.depth_split.bag diff --git a/gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_ref.rsp b/tools/bag_processing/scripts/__init__.py similarity index 100% rename from gnc/matlab/code_generation/ctl_controller0_ert_rtw/ctl_controller0_ref.rsp rename to tools/bag_processing/scripts/__init__.py diff --git a/tools/bag_processing/scripts/analyze_bad_pixels b/tools/bag_processing/scripts/analyze_bad_pixels new file mode 100755 index 0000000000..f3b04698bb --- /dev/null +++ b/tools/bag_processing/scripts/analyze_bad_pixels @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2024, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +"Executable wrapper for analyze_bad_pixels module." + +from bag_processing import analyze_bad_pixels + +analyze_bad_pixels.main() diff --git a/tools/bag_processing/scripts/apply_histogram_equalization_to_images.py b/tools/bag_processing/scripts/apply_histogram_equalization_to_images.py new file mode 100755 index 0000000000..560e542f1a --- /dev/null +++ b/tools/bag_processing/scripts/apply_histogram_equalization_to_images.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Applies histogram equalization to images in the provided bagfile and saves the results to a new bagfile. +Either uses normal histogram equalization or CLAHE adaptive equalization. +""" + +import argparse +import os +import sys + +import cv2 +import rosbag +import rospy +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image + +import utilities.utilities + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile with grayscale images.") + parser.add_argument( + "-t", + "--topic", + default="/mgt/img_sampler/nav_cam/image_record", + help="Image topic in bagfile.", + ) + parser.add_argument( + "-n", + "--non-adaptive", + dest="adaptive", + action="store_false", + help="Changes equalization approach from adaptive (CLAHE) to non-adaptive (normal histrogram equalization). Default approach is CLAHE.", + ) + parser.add_argument( + "-s", + "--save-all-topics", + dest="save_all_topics", + action="store_true", + help="Save all topics from input bagfile to output bagfile.", + ) + parser.set_defaults(adaptive=True) + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + + output_bag_name = ( + os.path.splitext(args.bagfile)[0] + "_clahe.bag" + if args.adaptive + else os.path.splitext(args.bagfile)[0] + "_hist_equalized.bag" + ) + output_bag = rosbag.Bag(output_bag_name, "w") + bridge = CvBridge() + clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8)) + topics = None if args.save_all_topics else [args.topic] + + with rosbag.Bag(args.bagfile, "r") as bag: + for topic, msg, t in bag.read_messages(topics): + if topic == args.topic: + try: + image = bridge.imgmsg_to_cv2(msg, "mono8") + except (CvBridgeError) as e: + print(e) + equalized_image = ( + clahe.apply(image) if args.adaptive else cv2.equalizeHist(image) + ) + equalized_image_msg = bridge.cv2_to_imgmsg( + equalized_image, encoding="mono8" + ) + equalized_image_msg.header = msg.header + output_bag.write(args.topic, equalized_image_msg, t) + else: + output_bag.write(topic, msg, t) + + output_bag.close() diff --git a/tools/bag_processing/scripts/check_bag_for_gaps.py b/tools/bag_processing/scripts/check_bag_for_gaps.py new file mode 100755 index 0000000000..7f42d98635 --- /dev/null +++ b/tools/bag_processing/scripts/check_bag_for_gaps.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Checks a bagfile for messages with gaps larger than the provided maximum time difference for the given topic. +Prints timestamps for found gaps along with min/mean/max/stddev for the time differences. +""" + +import argparse +import os +import sys + +import rosbag + +import utilities.utilities + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile.") + parser.add_argument("topic", help="Topic to check.") + parser.add_argument( + "-m", + "--max-time-diff", + type=float, + default=0.5, + help="Maximum time difference for a gap, time differences above this will be counted as gaps.", + ) + # Use header or received time + parser.add_argument( + "-r", + "--use-receive-time", + action="store_true", + help="Use receive time instead of header time.", + ) + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + + utilities.utilities.get_topic_rates( + args.bagfile, args.topic, args.max_time_diff, not args.use_receive_time, True + ) diff --git a/tools/bag_processing/scripts/clock_skew.py b/tools/bag_processing/scripts/clock_skew.py new file mode 100755 index 0000000000..5122ef80b9 --- /dev/null +++ b/tools/bag_processing/scripts/clock_skew.py @@ -0,0 +1,230 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Analyze clock skew LLP->MLP and HLP->MLP. Inputs an 'ars_default' type bag +containing the /mgt/sys_monitor/time_sync topic. +""" + +from __future__ import print_function + +import matplotlib + +matplotlib.use("Agg") + +import argparse + +import numpy as np +import rosbag +import scipy.signal +from matplotlib import pyplot as plt +from numpy.polynomial.polynomial import Polynomial + +REMOTE_PROCESSORS = ( + "LLP", + "HLP", +) +REMOTE_ID = {name: i for i, name in enumerate(REMOTE_PROCESSORS)} +OUTLIER_PERCENTILE = 99.0 + + +def dt64(t): + """ + Convert rospy.Time to np.datetime64. + """ + return np.datetime64(t.to_nsec(), "ns") + + +def read_time_sync(in_bag_path): + """ + Read time sync data from @in_bag_path. Return a numpy array with named + fields. + """ + topic = "/mgt/sys_monitor/time_sync" + with rosbag.Bag(in_bag_path, "r") as in_bag: + msg_count = in_bag.get_type_and_topic_info().topics[topic].message_count + d = np.empty( + shape=(msg_count,), + dtype=[ + ("remote_processor", np.ubyte), + ("mlp_time", "datetime64[ns]"), + ("remote_time", "datetime64[ns]"), + ], + ) + for i, (topic, msg, t) in enumerate(in_bag.read_messages([topic])): + remote_id = REMOTE_ID[msg.remote_processor] + d[i] = (remote_id, dt64(msg.mlp_time), dt64(msg.remote_time)) + return d + + +def seconds_from_time_delta(td): + """ + Convert np.timedelta64 to floating point seconds. + """ + return td / np.timedelta64(1, "s") + + +def plot_skew(out_path, title, t, delta, subsets, fit_tf): + """ + Plot measured delta vs. time. @subsets lets you specify different + parts of the data set to plot in different colors and label the + legend properly. If @fit_tf is not None, plot the linear fit as + well. + """ + fig = plt.figure() + + for label, ind in subsets: + plt.plot(t[ind], delta[ind] * 1e3, ".") + labels = [label.capitalize() for label, ind in subsets] + + if fit_tf is not None: + plt.plot(t, (fit_tf(t) - t) * 1e3, "-") + labels.append("Linear fit") + + plt.grid() + plt.legend(labels) + plt.title(title) + plt.xlabel("Time (s)") + plt.ylabel("Time delta (ms)") + + plt.tight_layout() + fig.set_size_inches((15, 5)) + plt.savefig(out_path) + plt.close() + print("Saved figure to:", out_path) + + +def model_processor(d, remote_name): + """ + Analyze the clock skew data for @remote_name. + """ + print("== %s ==" % remote_name) + + remote_id = REMOTE_ID[remote_name] + # filter entries for given processor + d = d[(d["remote_processor"] == remote_id).nonzero()] + + t0 = d["remote_time"][0] + remote_time = seconds_from_time_delta(d["remote_time"] - t0) + mlp_time = seconds_from_time_delta(d["mlp_time"] - t0) + delta = mlp_time - remote_time + + # hack to get index array for all indices of an array + all_indices = (delta == delta).nonzero() + + # select a small subset of low-delta messages, using delta as a proxy for + # comm latency. these will be used for the model fit. + low_delta = scipy.signal.argrelextrema(delta, np.less, order=20) + + # reject outliers in "zoom" plots for better y axis scaling + thresh = np.percentile(delta, OUTLIER_PERCENTILE) + non_outlier = (delta <= thresh).nonzero() + + fit_tf = Polynomial.fit(remote_time[low_delta], mlp_time[low_delta], deg=1) + b = fit_tf(0) + m = fit_tf(1) - b + mlp_model = fit_tf(remote_time) + resid = mlp_time - mlp_model + + subsets = ( + ("all", all_indices), + ("non-outlier", non_outlier), + ("low-delta", low_delta), + ) + + print("Model form: MLP = m * %s + b" % remote_name) + print(" Time offset b (ms): %.3f" % (b * 1e3)) + print(" Drift rate m - 1 (ppm): %.3f" % ((m - 1) * 1e6)) + print(" Model referenced to start time:", t0) + + for label, ind in subsets: + print("Over %s samples:" % label) + print(" Residual max (ms): %.3f" % (np.max(np.abs(resid[ind])) * 1e3)) + print(" Residual RMS (ms): %.3f" % (np.std(resid[ind]) * 1e3)) + + if 1: + top10_thresh = np.sort(resid)[-10] + top10_keep = (resid >= top10_thresh).nonzero() + top10 = d[top10_keep] + resid_top10 = resid[top10_keep] + print("Top 10 residuals:") + for i, rec in enumerate(top10): + resid_val = resid_top10[i] + print( + " %s %s %s %s resid %.1f ms" + % ( + remote_name, + rec["remote_time"], + "MLP", + rec["mlp_time"], + resid_val * 1e3, + ) + ) + + for plot_type in ("raw", "residual"): + if plot_type == "raw": + y = delta + plot_tf = fit_tf + plot_title = "Clock skew MLP - %s [%s]" % (remote_name, plot_type) + else: + y = resid + plot_tf = None + plot_title = "Clock skew MLP - corrected(%s) [%s]" % ( + remote_name, + plot_type, + ) + + out_path = "clock_skew_%s_%s.png" % (remote_name, plot_type) + plot_skew(out_path, plot_title, remote_time, y, subsets, plot_tf) + + out_path_zoom = "clock_skew_%s_%s_zoom.png" % (remote_name, plot_type) + zoom_subsets = subsets[1:] + plot_skew( + out_path_zoom, plot_title + " (zoom)", remote_time, y, zoom_subsets, plot_tf + ) + print() + + +def clock_skew(in_bag_path): + print("Non-outliers are points below percentile:", OUTLIER_PERCENTILE) + print( + "Low-delta samples are neigborhood minima with respect to mlp_time - remote_time." + ) + print( + "To minimize impact of variable comm latency, model is fit to low-delta samples." + ) + print() + + d = read_time_sync(in_bag_path) + for remote_name in REMOTE_PROCESSORS: + model_processor(d, remote_name) + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "in_bag", + help="Path to input bag for analysis. Normally ends in 'ars_default.bag'.", + ) + args = parser.parse_args() + clock_skew(args.in_bag) + + +if __name__ == "__main__": + main() diff --git a/tools/bag_processing/scripts/csv_join.py b/tools/bag_processing/scripts/csv_join.py new file mode 100755 index 0000000000..98cc2f3f71 --- /dev/null +++ b/tools/bag_processing/scripts/csv_join.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Join two CSV files (call them A and B) by timestamp. + +A typical use case would be to join sensor messages with robot pose +messages, so the sensor+pose output can be used to generate a +scatter plot showing how the sensor value varies with position. + +Each input CSV file is assumed to be the output of a "rostopic echo -b +in.bag -p " command, where the message type for that topic +includes the standard ROS message header. + +The joined output CSV file has the following properties: + +- Each row contains columns from both inputs A+B and represents their + joined values "at the same time", based on the ROS header.stamp field. + +- The timestamp ('time' column) of each output row is drawn from the + header.stamp field of input A. This is called a "left join" in database + terminology. + +- The 'time' column from each input, which is the time when the bag + recorder received the message, is ignored in favor of the + 'header.stamp' field set by the publishing node. + +- In general, inputs A and B are asynchronous, so there may be no input + B row with the same timestamp as the output row. Instead: + + - The input B row with the nearest timestamp is used. + + - If there is no input B row within the time delta tolerance, the + output row is dropped. + +- The output header column names have the names of the input files + prepended so you can tell which columns came from which input. You may + want to rename the input files before running this script if you care + about the column name prefix. +""" + +from __future__ import print_function + +import argparse +import logging +import os + +import pandas as pd + + +def read_csv(in_path): + df = pd.read_csv(in_path) + + in_name = os.path.splitext(os.path.basename(in_path))[0] + + # HACK: This specifically deals with the strange format of /hw/wifi message + # by promoting the header embedded inside the signals0 field to the top + # level. It should have no effect on other messages. + df.columns = df.columns.str.replace(".signals0.header", ".header") + + df.columns = df.columns.str.replace("field.", "") + df.columns = df.columns.str.replace(".", "_") + # df["header_stamp"] = pd.to_datetime(df["header_stamp"], unit="ns") + df.reindex(df["header_stamp"]) + df = df.drop(columns=["%time"]) + df.columns = [in_name + "__" + c for c in df.columns] + return in_name, df + + +def csv_join(csv_a, csv_b, out_path, verbose=False): + if os.path.exists(out_path): + logging.critical("not overwriting existing file %s", out_path) + return + + a_name, a = read_csv(csv_a) + b_name, b = read_csv(csv_b) + out = pd.merge_asof( + a, + b, + left_index=True, + right_index=True, + direction="nearest", + tolerance=int(1e9), # ns + ) + out.insert(0, "%time", out[a_name + "__header_stamp"]) + out.to_csv(out_path, index=False) + logging.info("wrote output to %s", out_path) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "-v", + "--verbose", + help="print more debug info", + action="store_true", + ) + + parser.add_argument("csv_a", help="input A CSV filename") + parser.add_argument("csv_b", help="input B CSV filename") + parser.add_argument("csv_out", help="output joined CSV filename") + + args = parser.parse_args() + level = logging.DEBUG if args.verbose else logging.INFO + logging.basicConfig(level=level, format="%(message)s") + + csv_join(args.csv_a, args.csv_b, args.csv_out, args.verbose) diff --git a/tools/bag_processing/scripts/get_msg_stats.py b/tools/bag_processing/scripts/get_msg_stats.py new file mode 100755 index 0000000000..eced70b9f3 --- /dev/null +++ b/tools/bag_processing/scripts/get_msg_stats.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Generates mean, min, max, and stddev stats for provided topic and its field +in a bagfile. +""" + +import argparse +import os +import sys + +import numpy as np +import rosbag + + +def get_stats(bagfile, topic, field, use_size=False): + with rosbag.Bag(bagfile, "r") as bag: + vals = [] + for _, msg, _ in bag.read_messages([topic]): + val = getattr(msg, field) + if use_size: + vals.append(len(val)) + else: + vals.append(val) + + mean_val = np.mean(vals) + min_val = np.min(vals) + max_val = np.max(vals) + stddev_val = np.std(vals) + print(("Mean " + field + ": " + str(mean_val))) + print(("Min " + field + ": " + str(min_val))) + print(("Max " + field + ": " + str(max_val))) + print(("Stddev " + field + ": " + str(stddev_val))) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile containing desired topic.") + parser.add_argument("topic", help="Topic in the bagfile to get stats for.") + parser.add_argument( + "field", + help="Field of the message published on the provided topic to get stats for.", + ) + parser.add_argument( + "--use-size", + "-s", + default=False, + action="store_true", + help="Get stats for the size of the provided field.", + ) + + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + + get_stats(args.bagfile, args.topic, args.field, args.use_size) diff --git a/tools/bag_processing/scripts/rosbag_debayer.py b/tools/bag_processing/scripts/rosbag_debayer.py new file mode 100755 index 0000000000..04ea6b89d5 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_debayer.py @@ -0,0 +1,283 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Converts bayer encoded images from the provided bagfile to grayscale and color images in a new bagfile. +""" + +import argparse +import glob +import os +import pathlib +import sys +import time +from typing import Dict, List, Optional + +import cv2 +import rosbag +from cv_bridge import CvBridge, CvBridgeError + +from bag_processing import pixel_utils as pu + +# pylint: disable=c-extension-no-member # pylint can't find cv2 members +# pylint: disable=line-too-long # let black handle this + + +def get_correctors( + inbag_paths: List[str], + list_cam: List[str], + bayer_image_topic: str, + no_correct: bool, + load_corrector: Optional[str], + generate_corrector: str, +) -> Dict[str, Optional[pu.BadPixelCorrector]]: + "Return loaded or generated correctors for cameras in `list_cam`." + out: Dict[str, Optional[pu.BadPixelCorrector]] = {} + if no_correct: + return out + corrector_class, accum_class = pu.BadPixelCorrector.get_classes(generate_corrector) + for cam in list_cam: + if load_corrector is not None: + # Load pre-configured BadPixelCorrector + corrector_path = pathlib.Path(load_corrector.format(cam=cam)) + print(f"Loading corrector for {cam} from {corrector_path}") + corrector: Optional[pu.BadPixelCorrector] = pu.BadPixelCorrector.load( + corrector_path + ) + else: + # Generate BadPixelCorrector of specified type + t0 = time.time() + print( + f"Generating {generate_corrector} for {cam} by analyzing images... ", + end="", + ) + sys.stdout.flush() + topic = bayer_image_topic.format(cam=cam) + images = pu.ImageSourceBagPaths(inbag_paths, topic=topic) + stats = accum_class.get_image_stats_parallel(images) + if stats is None: + corrector = None + else: + note = f"bags={inbag_paths} topic='{topic}'" + corrector = corrector_class.from_image_stats(stats, note) + t1 = time.time() + print(f"done in {t1 - t0:.1f}s") + if corrector is None: + print(f"Can't generate corrector for {cam}, not enough images") + else: + corrector.save( + pathlib.Path(f"{corrector.__class__.__name__}_{cam}_cam.json") + ) + out[cam] = corrector + return out + + +def convert_bayer( + bagfile, + output_bag_name, + list_cam, + bayer_image_topic, + gray_image_topic, + color_image_topic, + save_all_topics=False, + correctors: Optional[Dict[str, Optional[pu.BadPixelCorrector]]] = None, +): + bridge = CvBridge() + topics = dict((bayer_image_topic.format(cam=cam), cam) for cam in list_cam) + output_bag = rosbag.Bag(output_bag_name, "w") + topics_bag = [] if save_all_topics else topics + + with rosbag.Bag(bagfile, "r") as bag: + for topic, msg, t in bag.read_messages(topics_bag): + if topic in topics: + try: + bayer_image = bridge.imgmsg_to_cv2(msg, msg.encoding) + except CvBridgeError as exc: + print(exc) + continue + + cam = topics[topic] + if correctors: + corrector = correctors[cam] + if corrector is not None: + bayer_image = corrector(bayer_image) + + # Check if we should save greyscale image + if gray_image_topic != "": + # Technically this color conversion should be using cv2.COLOR_BAYER_GB2GRAY to + # match the true Bayer convention of the Astrobee NavCam/DockCam. Using + # cv2.COLOR_BAYER_GR2GRAY swaps the R/B channels of the Bayer pattern. A color + # channel swap has a subtle effect on grayscale output because different color + # channels have different weighting factors when outputting luminance calibrated + # for human perception. However, for the purposes of this script, the key + # requirement is to exactly replicate the (similarly erroneous) onboard debayer + # conversion performed in the FSW is_camera ROS node, so localization features + # will be the same regardless of which tool is used to do the conversion. + # https://github.com/nasa/astrobee/blob/develop/hardware/is_camera/src/camera.cc#L522 + # https://docs.opencv.org/3.4/de/d25/imgproc_color_conversions.html + gray_image = cv2.cvtColor(bayer_image, cv2.COLOR_BAYER_GR2GRAY) + gray_image_msg = bridge.cv2_to_imgmsg(gray_image, encoding="mono8") + gray_image_msg.header = msg.header + output_bag.write( + gray_image_topic.format(cam=cam), + gray_image_msg, + t, + ) + # Check if we should save color image + if color_image_topic != "": + # Here we are using the correct Bayer convention because we want the color image + # to look right and we have no need to replicate legacy onboard debayering. + color_image = cv2.cvtColor(bayer_image, cv2.COLOR_BAYER_GB2BGR) + color_image_msg = bridge.cv2_to_imgmsg(color_image, encoding="bgr8") + color_image_msg.header = msg.header + output_bag.write( + color_image_topic.format(cam=cam), + color_image_msg, + t, + ) + if save_all_topics: + output_bag.write(topic, msg, t) + output_bag.close() + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument( + "inbag", + nargs="+", + help="List of bags to convert. If none provided, all bags in the current directory are used.", + ) + parser.add_argument( + "-o", "--output", help="path for output bag", default="debayer_{inbag}" + ) + parser.add_argument( + "-l", + "--list-cam", + default=["nav"], + help="Cameras to be converted, default nav, can add dock.", + nargs="+", + type=str, + ) + parser.add_argument( + "-b", + "--bayer-image-topic", + default="/hw/cam_{cam}_bayer", + help="Bayer image topic name.", + ) + parser.add_argument( + "--no-gray", + dest="disable_gray", + action="store_true", + help="Disable grayscale conversion.", + ) + parser.add_argument( + "-g", + "--gray-image-topic", + default="/mgt/img_sampler/{cam}_cam/image_record", + help="Output gray image topic.", + ) + parser.add_argument( + "--no-color", + dest="disable_color", + action="store_true", + help="Disable color conversion.", + ) + parser.add_argument( + "-c", + "--color-image-topic", + default="/hw/cam_{cam}/image_color", + help="Output color image topic.", + ) + parser.add_argument( + "-s", + "--save-all-topics", + dest="save_all_topics", + action="store_true", + help="Save all topics from input bagfile to output bagfile.", + ) + parser.add_argument( + "--no-correct", + action="store_true", + default=False, + help="Skip bad pixel correction", + ) + parser.add_argument( + "--load-corrector", + default=None, + help="Load pre-configured bad pixel correctors using path template with '{cam}', e.g. 'corrector_{cam}_cam.json'", + ) + parser.add_argument( + "--generate-corrector", + choices=("BiasCorrector", "NeighborMeanCorrector"), + default="BiasCorrector", + help="Generate bad pixel corrector of specified type, if needed", + ) + parser.add_argument( + "-n", + dest="do_nothing", + action="store_true", + help="Option to not debayer anything and write output", + ) + args = parser.parse_args() + + if args.disable_gray: + args.gray_image_topic = "" + if args.disable_color: + args.color_image_topic = "" + + inbag_paths = args.inbag if args.inbag is not None else glob.glob("*.bag") + + if not args.do_nothing: + correctors = get_correctors( + inbag_paths=inbag_paths, + list_cam=args.list_cam, + bayer_image_topic=args.bayer_image_topic, + no_correct=args.no_correct, + load_corrector=args.load_corrector, + generate_corrector=args.generate_corrector, + ) + + for inbag_path in inbag_paths: + # Check if input bag exists + if not os.path.isfile(inbag_path): + print(("Bag file " + inbag_path + " does not exist.")) + sys.exit(1) + output_bag_name = args.output.format(inbag=inbag_path) + + # Check if output bag already exists + if os.path.exists(output_bag_name): + parser.error("not replacing existing file %s" % output_bag_name) + if not args.do_nothing: + # Conver bayer topic to black/white and color + convert_bayer( + inbag_path, + output_bag_name, + args.list_cam, + args.bayer_image_topic, + args.gray_image_topic, + args.color_image_topic, + args.save_all_topics, + correctors, + ) + else: + os.rename(inbag_path, output_bag_name) + + +if __name__ == "__main__": + main() diff --git a/tools/bag_processing/scripts/rosbag_detect_bad_topics.py b/tools/bag_processing/scripts/rosbag_detect_bad_topics.py new file mode 100755 index 0000000000..8162dd697d --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_detect_bad_topics.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Detect bad topics that the rosbag API can't handle. (The main problem +so far has been rosjava messages that have incomplete message definition +dependency information that causes rosbag to raise an exception when +it tries to parse the message.) +""" + +from __future__ import print_function + +import argparse +import logging + +import rosbag + + +def detect_bad_topic(inbag_path): + with rosbag.Bag(inbag_path, "r") as inbag: + logging.info("== phase 1: extract topic list on first pass ") + all_topics = inbag.get_type_and_topic_info()[1].keys() + logging.info(" %s topics found", len(all_topics)) + maybe_bad_topics = set(all_topics) + + logging.info( + "\n== phase 2: quickly identify as many good topics as possible in one pass" + ) + try: + for topic, msg, t in inbag.read_messages(maybe_bad_topics, raw=True): + if topic in maybe_bad_topics: + logging.info("good: %s", topic) + maybe_bad_topics.remove(topic) + except: + pass + logging.info("%s remaining topics:", len(maybe_bad_topics)) + logging.info(" %s\n", "\n ".join(sorted(maybe_bad_topics))) + + logging.info( + "\n== phase 3: check remaining topics one at a time, one pass for each topic" + ) + bad_topics = [] + for check_topic in sorted(maybe_bad_topics): + logging.info("checking: %s", check_topic) + try: + for topic, msg, t in inbag.read_messages([check_topic], raw=True): + logging.info(" good") + break + except: + logging.info(" bad") + bad_topics.append(check_topic) + + logging.info("\n== check complete, final bad topics ==") + print("\n".join(bad_topics)) + logging.info("") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "-v", + "--verbose", + help="Print debug info", + default=False, + action="store_true", + ) + parser.add_argument("inbag", help="Input bag") + + args = parser.parse_args() + level = logging.INFO if args.verbose else logging.WARN + logging.basicConfig(level=level, format="%(message)s") + detect_bad_topic(args.inbag) diff --git a/tools/bag_processing/scripts/rosbag_extract_images.py b/tools/bag_processing/scripts/rosbag_extract_images.py new file mode 100755 index 0000000000..0cda04a550 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_extract_images.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Read images on specified topic from bag and write them to specified output folder in PNG format. +Output images will be named according to ROS message timestamp. +""" + +import argparse +import glob +import itertools +import pathlib +from typing import Iterable, List, Optional, Tuple + +import cv2 +import numpy as np +import rosbag +import rospy +import sensor_msgs.msg +from cv_bridge import CvBridge + +# pylint: disable=c-extension-no-member # because pylint can't find cv2 members + +# White balance and contrast-enhance RGB image to improve appearance, assuming it was taken by NavCam +# on ISS. The contrast enhancement in particular might need to be tuned depending on exposure settings. +WHITE_BALANCE_LEVELS = np.array([1.0000, 0.5250, 0.6818]) +CONTRAST_ENHANCE = 1.8 + + +def message_source( + bagfiles: List[str], topic: str +) -> Iterable[Tuple[sensor_msgs.msg.Image, rospy.rostime.Time]]: + "Return a generator that reads `bagfiles` on `topic` and yields (message, timestamp) pairs." + for bagfile in bagfiles: + with rosbag.Bag(bagfile, "r") as bag: + print(f"Reading images from: {bagfile}") + for _, msg, t in bag.read_messages([topic]): + yield msg, t + + +def rosbag_extract_images( + bagfiles: List[str], + output_dir: pathlib.Path, + topic: str, + head: Optional[int], + ratio: int, +): + "Main driver for extracting images." + bridge = CvBridge() + messages = message_source(bagfiles, topic) + if not (head is None and ratio == 1): + msg = f"Extracting 1 out of every {ratio} images" + if head is not None: + msg += f" for the first {head} images" + print(msg) + messages = itertools.islice(messages, 0, head, ratio) + + count = 0 + for msg, t in messages: + image = bridge.imgmsg_to_cv2(msg, msg.encoding) # type: ignore # mypy doesn't have msg definition + if msg.encoding == "bgr8": # type: ignore + adjust = WHITE_BALANCE_LEVELS * CONTRAST_ENHANCE + image = np.clip(image * adjust[np.newaxis, np.newaxis, :], 0, 255).astype( + np.uint8 + ) + cv2.imwrite(str(output_dir / str(t)) + ".png", image) + count += 1 + + print(f"Wrote {count} images to: {output_dir}") + + +class CustomFormatter( + argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter +): + "Formatter with multiple mixins." + + +def main(): + "Parse command-line args and call rosbag_extract_images()" + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "inbag", + nargs="+", + default=[], + help="bags to extract images from", + ) + parser.add_argument( + "-o", + "--output", + help="folder for writing output images", + default="rosbag_extract_images_output", + ) + parser.add_argument( + "-t", + "--topic", + default="/hw/cam_nav_bayer", + help="image topic name.", + ) + parser.add_argument( + "--head", + type=int, + default=None, + help="process first HEAD images only", + ) + parser.add_argument( + "--ratio", + type=int, + default=1, + help="process 1 out of every RATIO images", + ) + args = parser.parse_args() + + output = pathlib.Path(args.output) + if output.exists(): + parser.error("not overwriting --output folder {output}") + output.mkdir(parents=True) + + inbag_paths = args.inbag if args.inbag else glob.glob("*.bag") + + rosbag_extract_images( + inbag_paths, + output_dir=output, + topic=args.topic, + head=args.head, + ratio=args.ratio, + ) + + +if __name__ == "__main__": + main() diff --git a/tools/bag_processing/scripts/rosbag_fix_all.py b/tools/bag_processing/scripts/rosbag_fix_all.py new file mode 100755 index 0000000000..734916db91 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_fix_all.py @@ -0,0 +1,216 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Master script to apply all passes of processing needed to fix our legacy bag +files. The actual processing steps are found in Makefile.rosbag_fix_all. + +Fixed bag files will be grouped by folder at the end and each group +will be merged. To suppress this, use --no-merge. +""" + +from __future__ import print_function + +import argparse +import logging +import os +import sys + + +def dosys(cmd): + logging.info(cmd) + ret = os.system(cmd) + if ret != 0: + logging.warning("Command failed with return value %s\n" % ret) + return ret + + +def rosbag_fix_all( + inbag_paths_in, jobs, debayer, decode_haz, filter_args, no_merge, deserialize=False +): + this_folder = os.path.dirname(os.path.realpath(__file__)) + makefile = os.path.join(this_folder, "Makefile.rosbag_fix_all") + inbag_paths_in = [p for p in inbag_paths_in if p.endswith(".bag")] + + # Skip processed bags to start in the correct stage in the Makefile + inbag_paths = [p for p in inbag_paths_in if not ".rewrite_types.bag" in p] + inbag_paths = [p for p in inbag_paths if not ".migrate_old.bag" in p] + inbag_paths = [p for p in inbag_paths if not ".debayer.bag" in p] + inbag_paths = [p for p in inbag_paths if not ".depth_split.bag" in p] + inbag_paths = [p for p in inbag_paths if not ".fix_all.bag" in p] + + skip_count = len(inbag_paths_in) - len(inbag_paths) + if skip_count: + logging.info( + "Not trying to fix %d files that already end in .fix_all.bag", skip_count + ) + + outbag_paths = [os.path.splitext(p)[0] + ".fix_all.bag" for p in inbag_paths] + outbag_paths_str = " ".join(outbag_paths) + + # Rosbag migrate message definitions + rewrite_types_args = "-v" + + # Rosbag verify bags + rosbag_verify_args = "-v" + if deserialize: + rosbag_verify_args += " -d" + + # Rosbag convert debayer + if debayer: + rosbag_debayer_args = "-s" + else: + rosbag_debayer_args = "-n" + + # Rosbag decode haz cam + if decode_haz == "": + rosbag_pico_split_extended_args = "-n" + else: + output_stream = os.popen("catkin_find --first-only bag_processing resources") + coeff_path = ( + output_stream.read().rstrip() + "/" + decode_haz + "_haz_xyz_coeff.npy" + ) + # Assert that coefficient file exists + if os.path.exists(coeff_path): + rosbag_pico_split_extended_args = "-s --in_npy " + coeff_path + else: + logging.warning( + "Depth cam robot coefficient file " + coeff_path + " does not exist." + ) + return 1 + + # Rosbag filter + rosbag_filter_args = filter_args + + # Call entire pipeline on all the files + # "1>&2": redirect stdout to stderr to see make's command echo in rostest output + ret1 = dosys( + 'REWRITE_TYPES_ARGS="%s" ROSBAG_VERIFY_ARGS="%s" ROSBAG_DEBAYER_ARGS="%s" ROSBAG_SPLIT_DEPTH_ARGS="%s" ROSBAG_FILTER_ARGS="%s" make -f%s -j%s %s 1>&2' + % ( + rewrite_types_args, + rosbag_verify_args, + rosbag_debayer_args, + rosbag_pico_split_extended_args, + rosbag_filter_args, + makefile, + jobs, + outbag_paths_str, + ) + ) + + logging.info("") + logging.info("====================") + if ret1 == 0: + logging.info("Fixed bags:") + for outbag_path in outbag_paths: + logging.info(" %s", outbag_path) + else: + logging.warning("Not all bags were fixed successfully (see errors above).") + logging.warning( + "If you want to try again, you may need to clean the intermediate results." + ) + return ret1 + + if no_merge: + return ret1 + + # Group fixed bags by folder and merge each group. + inbag_folders_in = list( + set([os.path.dirname(os.path.realpath(p)) for p in inbag_paths_in]) + ) + rosbag_merge = ( + os.popen("catkin_find --first-only bag_processing scripts/rosbag_merge.py") + .read() + .rstrip() + ) + merge_errors = 0 + for inbag_folder_in in inbag_folders_in: + ret = dosys( + "%s -d %s --input-bag-suffix .fix_all.bag" % (rosbag_merge, inbag_folder_in) + ) + if ret != 0: + merge_errors += 1 + + return 0 if (merge_errors == 0) else 1 + + +class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): + pass + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "-j", + "--jobs", + help="specifies the number of jobs to run simultaneously", + type=int, + default=1, + ) + parser.add_argument( + "-d", + "--deserialize", + help="perform deserialization check on output bag (can be slow for large bags)", + default=False, + action="store_true", + ) + parser.add_argument( + "--debayer", + help="debayer the nav or dock cam", + default=False, + action="store_true", + ) + parser.add_argument( + "--decode-haz", + help="decode the extended haz cam topic into points and amplitude_int, argument is robot name", + default="", + type=str, + ) + parser.add_argument( + "--filter", + help='filter the bagfile. Use with "" quotes. Example: --filter "--accept /loc/* --reject /loc/ml/features"', + default="", + type=str, + ) + parser.add_argument( + "--no-merge", + help="skip merging bag files grouped by directory", + default=False, + action="store_true", + ) + parser.add_argument("inbag", nargs="+", help="input bag") + + args = parser.parse_args() + logging.basicConfig(level=logging.INFO, format="%(message)s") + + ret = rosbag_fix_all( + args.inbag, + args.jobs, + args.debayer, + args.decode_haz, + args.filter, + args.no_merge, + deserialize=args.deserialize, + ) + + # suppress confusing ROS message at exit + logging.getLogger().setLevel(logging.WARN) + + sys.exit(0 if ret == 0 else 1) diff --git a/tools/bag_processing/scripts/rosbag_merge.py b/tools/bag_processing/scripts/rosbag_merge.py new file mode 100755 index 0000000000..4877b96ff0 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_merge.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Merges bagfiles with given prefix in the current working directory. +""" + + +import argparse +import os +import re +import string +import subprocess +import sys + +import localization_common.utilities as lu + + +# https://stackoverflow.com/a/4836734 +def natural_sort(l): + convert = lambda text: int(text) if text.isdigit() else text.lower() + alphanum_key = lambda key: [convert(c) for c in re.split("([0-9]+)", key)] + return sorted(l, key=alphanum_key) + + +def merge_bag(input_bag_prefix, input_bag_suffix, merged_bag, only_loc_topics=False): + # Find bagfiles with bag prefix in current directory, fail if none found + bag_names = [ + bag + for bag in os.listdir(".") + if os.path.isfile(bag) + and bag.startswith(input_bag_prefix) + and bag.endswith(input_bag_suffix) + ] + if len(bag_names) == 0: + print("No bag files found") + sys.exit() + elif len(bag_names) == 1: + print("Only one, nothing to merge") + return + else: + print(("Found " + str(len(bag_names)) + " bag files.")) + + merged_bag_name = "" + if not merged_bag: + merged_bag_name = input_bag_prefix + ".merged.bag" + + sorted_bag_names = natural_sort(bag_names) + merge_bags_command = ( + "rosrun localization_node merge_bags " + + " ".join(sorted_bag_names) + + " -output_bag " + + str(merged_bag_name) + ) + + if only_loc_topics: + merge_bags_command += ( + " -save_topics" + + " '/hw/imu" + + " /loc/of/features" + + " /loc/ml/features" + + " /loc/ar/features" + + " /mgt/img_sampler/nav_cam/image_record" + + " /graph_loc/state" + + " /gnc/ekf" + + " /sparse_mapping/pose" + + " /mob/flight_mode" + + " /beh/inspection/feedback" + + " /beh/inspection/goal" + + " /beh/inspection/result'" + ) + + lu.run_command_and_save_output(merge_bags_command) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument( + "input_bag_prefix", + nargs="*", + help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", + ) + parser.add_argument( + "--input-bag-suffix", + default=".bag", + help="Filter suffix for bagfiles to merge. Bags should all be in the current working directory.", + ) + parser.add_argument( + "--merged-bag", + default="", + help="Output merged bag. By default this is merged_prefix.bag where prefix is the provided bag prefix.", + ) + parser.add_argument( + "-d", + "--directory", + default=".", + help="Directory to where to find the bags", + ) + parser.add_argument( + "--only-loc-topics", + dest="only_loc_topics", + action="store_true", + help="Only save loc topics to output merged bag.", + ) + args = parser.parse_args() + os.chdir(args.directory) + + bag_names = args.input_bag_prefix + if not bag_names: + # Find bagfiles with bag prefix in current directory, fail if none found + bag_names = [ + bag[: -len(args.input_bag_suffix)].rstrip(string.digits) + for bag in os.listdir(".") + if os.path.isfile(bag) and bag.endswith(args.input_bag_suffix) + ] + # Remove duplicates + bag_names = sorted(set(bag_names)) + if len(bag_names) == 0: + print("No bag files found") + sys.exit() + else: + print(("Found " + str(len(bag_names)) + " bag file prefixes.")) + + print(bag_names) + for bag_name in bag_names: + merge_bag( + bag_name, args.input_bag_suffix, args.merged_bag, args.only_loc_topics + ) diff --git a/tools/bag_processing/scripts/rosbag_rewrite_types.py b/tools/bag_processing/scripts/rosbag_rewrite_types.py new file mode 100755 index 0000000000..04c204279f --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_rewrite_types.py @@ -0,0 +1,337 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Tool that applies rules to fix bag files, as specified in +rosbag_rewrite_types_rules.json. It performs two types of fix: + +1. Fix incomplete message definition metadata, as written by rosjava + nodes. Which topics to fix is specified in the + fix_message_definition_topic_patterns field of the rules file. + +2. For message types whose definition changed in a way that makes + migration infeasible, changes the message type name to be the + "frozen" type name, so that migration is not needed. Which message + types to fix is specified in the rename_types field of the rules + file. + +These fixes should be run before "rosbag fix" bag migration. The overall +strategy is explained in [1]. + +[1] https://github.com/nasa/astrobee/blob/develop/doc/general_documentation/maintaining_telemetry.md +""" + +from __future__ import print_function + +import argparse +import collections +import fnmatch +import json +import logging +import os +import re +import tempfile + +import genpy +import rosbag +import roslib + +# See the following sources for many of the genpy/rosbag/roslib patterns used +# here: +# http://docs.ros.org/en/melodic/api/rosbag/html/python/rosbag.migration-pysrc.html +# https://github.com/gavanderhoorn/rosbag_fixer + + +INTERNAL_MESSAGE_DEF_REGEX = re.compile(r"^MSG: ", re.MULTILINE) + + +def dosys(cmd): + logging.info(cmd) + ret = os.system(cmd) + if ret != 0: + logging.warning("Command failed with return value %s\n", ret) + return ret + + +def get_rule_entry(rule): + key = (rule["old_type"], rule["old_type_md5sum"]) + + new_type = rule["new_type"] + new_pytype = genpy.message.get_message_class(new_type) + if rule["new_type_md5sum"] != new_pytype._md5sum: + logging.warning("warning: md5sum mismatch for: %s", new_type) + logging.warning(" migration rule md5sum: %s", rule["new_type_md5sum"]) + logging.warning(" latest md5sum: %s", new_pytype._md5sum) + logging.warning(" [a changed message definition could cause data corruption]") + + val = { + "type": new_type, + "md5sum": rule["new_type_md5sum"], + "message_definition": new_pytype._full_text, + "pytype": new_pytype, + } + + return (key, val) + + +def get_rules(rules_files): + fix_topic_patterns = [] + rename_lookup = {} + for rules_file in rules_files: + with open(rules_file, "r") as rules_stream: + rules_info = json.load(rules_stream) + + for k, rules in rules_info.items(): + if k == "fix_message_definition_topic_patterns": + fix_topic_patterns += rules + elif k == "rename_types": + rename_lookup.update(dict((get_rule_entry(r) for r in rules))) + + return (fix_topic_patterns, rename_lookup) + + +def topic_matcher(topic, topic_patterns): + return any((fnmatch.fnmatch(topic, p) for p in topic_patterns)) + + +def reindex(inbag_path): + tmp_folder = tempfile.mkdtemp() + dosys("rosbag reindex --output-dir=%s %s " % (tmp_folder, inbag_path)) + reindexed_path = os.path.join(tmp_folder, os.path.basename(inbag_path)) + return reindexed_path, tmp_folder + + +def reindex_if_needed(inbag_path): + try: + with rosbag.Bag(inbag_path, "r") as inbag: + pass + return inbag_path, None + except rosbag.bag.ROSBagUnindexedException: + logging.warning( + "WARNING: input bag %s was unindexed, reindexing automatically", inbag_path + ) + return reindex(inbag_path) + + +def fix_message_definitions(inbag, fix_topic_patterns, verbose=False): + """ + Modifies inbag metadata (in memory, not on disk). Changes will take + effect when inbag's messages are subsequently written to outbag. + """ + + if not fix_topic_patterns: + return + + fixed_topics = set() + for conn in inbag._get_connections(): + if not topic_matcher(conn.topic, fix_topic_patterns): + continue + if INTERNAL_MESSAGE_DEF_REGEX.search(conn.msg_def): + logging.info( + "Connection's message definition would be fixed based on message topic '%s', but leaving it alone because it already appears to properly incorporate internal message definitions." + % conn.topic + ) + # Note that "fixing" a message definition means forcing it to be + # deserialized according to the current schema for that message + # type, which could be wrong if the schema has changed since the + # message was recorded. The usual problem with rosjava message + # definitions is that they don't incorporate the definitions of + # referenced field types. So we do a superficial check for + # that problem and bail out if it comes up negative. + # + # A real-world example of where this check can be important: the + # /mgt/cpu_monitor/state message can be published on both the MLP + # and the HLP (different publishers). If on the HLP, the message + # definition will be broken and we need to fix it. If on the MLP, + # the message definition will not be broken. The schema for that + # message has also changed, so fixing non-broken MLP messages from + # before the schema change could break them! + continue + pytype = roslib.message.get_message_class(conn.datatype) + if pytype is None: + raise ValueError("Message class '%s' not found." % conn.datatype) + conn.header["message_definition"] = pytype._full_text + conn.msg_def = pytype._full_text + fixed_topics.add(conn.topic) + + if verbose: + logging.info("Fixed message definitions for topics:") + if fixed_topics: + for topic in sorted(fixed_topics): + logging.info(" %s", topic) + else: + logging.info(" [none]") + + +def rename_types(inbag, outbag_path, rename_lookup, verbose=False): + # rewrite connection headers + renamed_types = set() + for conn in inbag._get_connections(): + hdr = conn.header + rename_key = (hdr["type"].decode("utf-8"), hdr["md5sum"].decode("utf-8")) + tgt = rename_lookup.get(rename_key) + if tgt is not None: + renamed_types.add(rename_key) + # rewrite the necessary fields of the connection header + for k in ("type", "md5sum", "message_definition"): + hdr[k] = tgt[k].encode("utf-8") + + # summarize rewrite rules that were applied + if verbose: + # summary of renamed messages + migrated = sorted(renamed_types) + logging.info("Renamed types in bag:") + if not migrated: + logging.info(" [no matching types found in bag]") + for key in migrated: + old_type, old_type_md5sum = key + tgt = rename_lookup[key] + logging.info( + " %s %s -> %s %s", + old_type, + old_type_md5sum[:8], + tgt["type"], + tgt["md5sum"][:8], + ) + + # while copying messages we need to rename types so they are mapped to the right connection + do_meter = False # unhelpful in CI test logs + with rosbag.Bag(outbag_path, "w", options=inbag.options) as outbag: + if do_meter: + meter = rosbag.rosbag_main.ProgressMeter( + outbag_path, inbag._uncompressed_size + ) + total_bytes = 0 + + for topic, raw_msg, t in inbag.read_messages(raw=True): + msg_type, serialized_bytes, md5sum, pos, pytype = raw_msg + + rename_key = (msg_type, md5sum) + tgt = rename_lookup.get(rename_key) + if tgt is not None: + out_msg = ( + tgt["type"], + serialized_bytes, + tgt["md5sum"], + pos, + tgt["pytype"], + ) + else: + out_msg = raw_msg + + outbag.write(topic, out_msg, t, raw=True) + + if do_meter: + total_bytes += len(serialized_bytes) + meter.step(total_bytes) + + if do_meter: + meter.finish() + + +def rewrite_msg_types1( + inbag_path_in, outbag_path, rules, verbose=False, no_reindex=False +): + inbag_path, tmp_folder = reindex_if_needed(inbag_path_in) + + fix_topic_patterns, rename_lookup = rules + with rosbag.Bag(inbag_path, "r") as inbag: + fix_message_definitions(inbag, fix_topic_patterns, verbose) + rename_types(inbag, outbag_path, rename_lookup, verbose) + + if tmp_folder: + dosys("rm -r %s" % tmp_folder) + + cmd = "rosbag reindex -q %s" % outbag_path + if no_reindex: + logging.info("%s probably needs to be re-indexed. Run: %s\n", outbag_path, cmd) + else: + dosys(cmd) + dosys("rm %s" % (os.path.splitext(outbag_path)[0] + ".orig.bag")) + + +def rewrite_msg_types( + inbag_paths, outbag_path_pattern, rules_files, verbose=False, no_reindex=False +): + rules = get_rules(rules_files) + + for inbag_path in inbag_paths: + inbag_name = os.path.splitext(inbag_path)[0] + outbag_path = outbag_path_pattern.format(inbag=inbag_name) + if os.path.exists(outbag_path): + logging.error("Not overwriting existing file %s", outbag_path) + continue + rewrite_msg_types1( + inbag_path, outbag_path, rules, verbose=verbose, no_reindex=no_reindex + ) + + +class CustomFormatter( + argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter +): + pass + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "-v", "--verbose", help="print debug info", default=False, action="store_true" + ) + parser.add_argument( + "-o", + "--output", + help="path for output bag", + default="{inbag}.rewrite_types.bag", + ) + parser.add_argument( + "-r", + "--rules", + nargs="?", + help="path to input rules file (specify multiple times for multiple files)", + default=[], + action="append", + ) + parser.add_argument( + "-n", + "--no-reindex", + action="store_true", + help="suppress 'rosbag reindex' call", + default=False, + ) + parser.add_argument("inbag", nargs="+", help="input bag") + + args = parser.parse_args() + + level = logging.DEBUG if args.verbose else logging.WARN + logging.basicConfig(level=level, format="%(message)s") + + if not args.rules: + parser.error("-r argument is required") + + rewrite_msg_types( + args.inbag, + args.output, + args.rules, + verbose=args.verbose, + no_reindex=args.no_reindex, + ) + + # suppress confusing ROS message at exit + logging.getLogger().setLevel(logging.WARN) diff --git a/tools/bag_processing/scripts/rosbag_sample.py b/tools/bag_processing/scripts/rosbag_sample.py new file mode 100755 index 0000000000..ecfddbc824 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_sample.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Write an overview bag that contains one message on each unique topic, collected +from the specified input bags. The resulting overview bag allows for quick +backward compatibility testing with the latest ROS packages. +""" + +from __future__ import print_function + +import argparse +import collections +import fnmatch +import json +import logging +import os + +import genpy +import rosbag +import roslib + +import rosbag_rewrite_types as rrt + + +def sample_bags(inbag_paths, outbag_path, rules_files, verbose=False): + if os.path.exists(outbag_path): + logging.critical("not overwriting existing file %s", outbag_path) + return + + fix_topic_patterns, _ = rrt.get_rules(rules_files) + + sampled = set() + with rosbag.Bag(outbag_path, "w") as outbag: + for inbag_path in inbag_paths: + if verbose: + logging.info("scanning bag: %s", inbag_path) + with rosbag.Bag(inbag_path, "r") as inbag: + rrt.fix_message_definitions(inbag, fix_topic_patterns) + for topic, msg, t in inbag.read_messages(raw=True): + if topic in sampled: + continue + sampled.add(topic) + if verbose: + logging.info("found new topic: %s", topic) + outbag.write(topic, msg, t, raw=True) + logging.info("found %s topics in %s bags", len(sampled), len(inbag_paths)) + logging.info("wrote one sample message on each topic to %s", outbag_path) + + +class CustomFormatter( + argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter +): + pass + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "-v", + "--verbose", + help="print more debug info", + default=False, + action="store_true", + ) + parser.add_argument( + "-o", + "--output", + help="path for output bag", + default="sample_bags.bag", + ) + parser.add_argument( + "-r", + "--rules", + nargs="?", + help="path to input rules file (specify multiple times for multiple files)", + default=[], + action="append", + ) + parser.add_argument("inbag", nargs="+", help="input bag") + + args = parser.parse_args() + + level = logging.DEBUG if args.verbose else logging.INFO + logging.basicConfig(level=level, format="%(message)s") + + sample_bags(args.inbag, args.output, args.rules, verbose=args.verbose) + + # suppress confusing ROS message at exit + logging.getLogger().setLevel(logging.WARN) diff --git a/tools/bag_processing/scripts/rosbag_splice.py b/tools/bag_processing/scripts/rosbag_splice.py new file mode 100755 index 0000000000..bca0c00fd1 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_splice.py @@ -0,0 +1,296 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Splices a bagfile at selected timestamps to create multiple smaller bagfiles which when combined +span the original bagfile. Uses wasd controls to iterate through the images. +Iterate through the images one at a time using the d and a keys. +Iterate more quickly (skipping 10 images at a time) using the w and s keys. +Create a splice point using the current image's timestamp using the space bar. +Undo adding last splice point using the u key. +Print current splice points and intervals using the p key. +Generate spliced bags using all selected splice points by pressing the enter key at +any time. To exit without splicing, use the escape or q keys. +""" + +import argparse +import os +import sys + +import cv2 +import rosbag +import rospy +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image + +import utilities.utilities + + +def print_info(splice_timestamps, bag_start_time, bag_end_time): + print(str(len(splice_timestamps)) + " splice timestamps selected.") + print("Splicing timestamps: " + str(splice_timestamps)) + print("Splicing intervals: ") + starting_splice_percent = 0 + for i in range(len(splice_timestamps) + 1): + ending_splice_percent = ( + (splice_timestamps[i] - bag_start_time) + / float(bag_end_time - bag_start_time) + if i < len(splice_timestamps) + else 1 + ) + print( + str(i) + + ": " + + "{0:.2f}".format(starting_splice_percent * 100) + + "% to " + + "{0:.2f}".format(ending_splice_percent * 100) + + "%" + ) + starting_splice_percent = ending_splice_percent + + +def sort_and_remove_repeats(splice_timestamps): + # Remove repeats + splice_timestamps = list(set(splice_timestamps)) + splice_timestamps.sort() + return splice_timestamps + + +def show_image_with_message( + image, window, title, message, origin=(50, 450), font_size=4, timeout=750 +): + color_image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) + splice_image = cv2.putText( + color_image, + message, + origin, + cv2.FONT_HERSHEY_SIMPLEX, + font_size, + (0, 0, 255), + 10, + ) + cv2.imshow(window, splice_image) + cv2.setWindowTitle(window, title) + key = cv2.waitKey(timeout) + + +def make_spliced_bag(bagfile, start_time, end_time, index): + spliced_bagfile = os.path.splitext(bagfile)[0] + "_" + str(index) + ".bag" + with rosbag.Bag(bagfile, "r") as bag: + with rosbag.Bag(spliced_bagfile, "w") as spliced_bag: + for topic, msg, t in bag.read_messages( + None, rospy.Time(start_time), rospy.Time(end_time) + ): + spliced_bag.write(topic, msg, t) + + +def splice_bag(bagfile, splice_timestamps): + if not splice_timestamps: + print("No timestamps provided, not splicing.") + return + start_and_end_times = [] + with rosbag.Bag(bagfile, "r") as bag: + # Subtract extra time from bag start time to ensure first message included + start_time = bag.get_start_time() - 1.0 + for i in range(len(splice_timestamps) + 1): + # Add extra time to end time to ensure final message included + end_time = ( + splice_timestamps[i] + if i < len(splice_timestamps) + else bag.get_end_time() + 1.0 + ) + make_spliced_bag(bagfile, start_time, end_time, i) + # Add slight extra time so new start time doesn't overlap with previous end time + start_time = end_time + 1e-4 + + +def select_splice_timestamps_and_splice_bag(bagfile, image_topic): + splice_timestamps = [] + bridge = CvBridge() + with rosbag.Bag(bagfile, "r") as bag: + msg_tuples = [] + print("Reading msgs...") + for topic, msg, t in bag.read_messages([image_topic]): + msg_tuples.append((msg, t)) + if not msg_tuples: + print("No messages found for topic: " + image_topic) + sys.exit() + i = 0 + num_msgs = len(msg_tuples) + window = "image" + cv2.namedWindow(window) + cv2.moveWindow(window, 40, 30) + + # Define the legend text and its position + legend_text = "d: next | a: previous | w: next 10 | s: previous 10 | q: quit | p: print | space: split | u: remove splice| enter: splice" + position = (10, 30) # Position (x, y) of the text on the image + + # Define font, scale, color, and thickness + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 0.67 + color = (255, 255, 255) # White color in BGR + thickness = 2 + + print_string = True + while True: + msg = (msg_tuples[i])[0] + timestamp = ((msg_tuples[i])[1]).to_sec() + progress = i / float(num_msgs) * 100 + msg_info_string = ( + "{0:.2f}".format(progress) + + "%, Image " + + str(i) + + "/" + + str(num_msgs) + + ", t: " + + str(timestamp) + ) + if print_string: + print(msg_info_string) + print_string = False + try: + image = bridge.imgmsg_to_cv2(msg, msg.encoding) + except (CvBridgeError) as e: + print(e) + # Add the legend text to the image + cv2.putText( + image, + legend_text, + position, + font, + font_scale, + color, + thickness, + cv2.LINE_AA, + ) + cv2.imshow(window, image) + cv2.setWindowTitle(window, msg_info_string) + key = cv2.waitKey(0) + # Have to use wasd instead of arrow keys due to opencv/QT bug where + # arrow presses aren't registered when the user clicks of the screen then back to the screen + if key == ord("d"): + i += 1 + print_string = True + elif key == ord("a"): + i -= 1 + print_string = True + if key == ord("w"): + i += 10 + print_string = True + elif key == ord("s"): + i -= 10 + print_string = True + + elif key == ord("q") or key == 27: # Escape key + print("Manually closing program, no splice operation applied.") + exit(0) + elif key == ord("p"): + print_info(splice_timestamps, bag.get_start_time(), bag.get_end_time()) + show_image_with_message( + image, + window, + "Printed splice intervals", + "Printed splice intervals", + (130, 450), + 3, + ) + elif key == 32: # Space bar + print("Splice timestamp selected, t: " + str(timestamp)) + splice_timestamps.append(timestamp) + splice_timestamps = sort_and_remove_repeats(splice_timestamps) + print_info(splice_timestamps, bag.get_start_time(), bag.get_end_time()) + show_image_with_message( + image, + window, + "Splice t selected! " + msg_info_string, + "Added splice point", + ) + elif key == ord("u"): + if not splice_timestamps: + message = "No splice timestamps added." + print(message) + show_image_with_message( + image, window, message, message, (80, 450), 2.4 + ) + else: + print( + "Removed last added splice point at timestamp: " + + str(timestamp) + ) + splice_timestamps.pop() + print_info( + splice_timestamps, bag.get_start_time(), bag.get_end_time() + ) + show_image_with_message( + image, + window, + "Removed last splice point at timestamp " + str(timestamp), + "Removed last splice point", + (40, 450), + 3, + ) + + elif key == 13 or key == 10: # Enter key + if not splice_timestamps: + message = "No splice timestamps added." + print(message) + show_image_with_message( + image, window, message, message, (80, 450), 2.4 + ) + else: + print("Splicing bag using selected timestamps.") + print_info( + splice_timestamps, bag.get_start_time(), bag.get_end_time() + ) + show_image_with_message( + image, + window, + "Creating spliced bags", + "Creating spliced bags", + (20, 450), + 3.5, + ) + splice_bag(bagfile, splice_timestamps) + return + + if i < 0: + i = 0 + if i >= num_msgs: + i = num_msgs - 1 + show_image_with_message( + image, window, "End of bag", "End of bag", (350, 450), 3 + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile.") + parser.add_argument( + "-i", + "--image-topic", + default="/hw/cam_nav", + help="Image topic name.", + ) + + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + + select_splice_timestamps_and_splice_bag(args.bagfile, args.image_topic) diff --git a/tools/bag_processing/scripts/rosbag_topic_filter.py b/tools/bag_processing/scripts/rosbag_topic_filter.py new file mode 100755 index 0000000000..e892be43e9 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_topic_filter.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Filter rosbag messages based on topic. Like a subset of 'rosbag filter' functionality, +but more robust to malformed bags because it doesn't need to parse the messages +to filter on topic. + +Topic filtering is controlled by an accept list and a reject list, as follows: +- By default, all messages are accepted. +- However, if the accept list is non-empty, only topics that match one of the patterns + in the accept list are accepted. +- If the reject list is non-empty, topics that match one of the reject patterns will be + rejected, even if they match an accept pattern. + +The patterns in the accept and reject list can be simple strings, or you can use +Python fnmatch Unix shell-style wildcards, like "/gs/*" to match any topic that starts +with "/gs/". Note that the "/" character is not treated as special in this context. +Remember to quote patterns that include wildcards so they are not expanded by your shell. + +Specify multiple --accept or --reject arguments on the command line to add multiple +patterns to the accept or reject list. + +Examples: + rosbag_topic_filter.py in.bag -r "/gs/*" -r /hw/cam_sci/compressed out.bag + Filters out rosjava messages produced by Guest Science and SciCamImage APKs + that have incomplete message definitions that break many rosbag API operations, + including the command-line utilities 'rosbag filter' and 'rosbag check'. + rosbag_topic_filter.py in.bag -a /mgt/sys_monitor/time_sync out.bag + Keep only a specific message of interest for data analysis so downstream script + can run much faster on a smaller bag. +""" + +from __future__ import print_function + +import argparse +import collections +import fnmatch +import os +import re +import sys + +import rosbag + +# check if pattern contains a character that is special for fnmatch +FNMATCH_SPECIAL_REGEX = re.compile(r"[\*\?\[]") + + +def is_special(fnmatch_pattern): + return FNMATCH_SPECIAL_REGEX.search(fnmatch_pattern) is not None + + +def topic_filter(verbose, inbag_path, accept_patterns, reject_patterns, outbag_path): + with rosbag.Bag(inbag_path, "r") as inbag: + if ( + accept_patterns + and not reject_patterns + and all([not is_special(p) for p in accept_patterns]) + ): + # if the accept patterns are just a list of plain topics and there + # are no reject patterns, there's no need to do an extra slow pass + # through the whole bag just to extract what topics it contains + accept_topics = accept_patterns + else: + all_topics = inbag.get_type_and_topic_info()[1].keys() + accept_topics = [] + for topic in all_topics: + do_accept = True + if accept_patterns: + do_accept = any( + [fnmatch.fnmatch(topic, p) for p in accept_patterns] + ) + if reject_patterns: + do_accept = do_accept and not any( + [fnmatch.fnmatch(topic, p) for p in reject_patterns] + ) + if do_accept: + accept_topics.append(topic) + + for p in accept_patterns: + if not fnmatch.filter(all_topics, p): + print("warning: no topics in bag match accept pattern '%s'" % p) + for p in reject_patterns: + if not fnmatch.filter(all_topics, p): + print("warning: no topics in bag match reject pattern '%s'" % p) + + if verbose: + print("accept topics:\n %s" % "\n ".join(sorted(accept_topics))) + + msg_count = collections.Counter() + with rosbag.Bag(outbag_path, "w") as outbag: + if accept_topics: + for topic, msg, t in inbag.read_messages(accept_topics): + outbag.write(topic, msg, t) + msg_count[topic] += 1 + else: + # don't call read_messages() with an empty accept_topics list: + # turns out, it would iterate through all messages instead of + # no messages + pass + + for topic in accept_topics: + if msg_count[topic] == 0: + print("warning: no messages on topic '%s' in bag" % topic) + if sum([c for c in msg_count.values()]) == 0: + print("warning: output bag does not contain any messages") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("inbag", nargs="+", help="Input bagfile with bayer images.") + parser.add_argument( + "-o", "--output", help="path for output bag", default="filtered_{inbag}" + ) + parser.add_argument( + "-a", + "--accept", + nargs="?", + help="Add topic pattern to accept list", + default=[], + action="append", + ) + parser.add_argument( + "-r", + "--reject", + nargs="?", + help="Add topic pattern to reject list", + default=[], + action="append", + ) + parser.add_argument("-v", "--verbose", help="Print debug info", action="store_true") + + args = parser.parse_args() + + for inbag_path in args.inbag: + # Check if input bag exists + if not os.path.isfile(inbag_path): + print(("Bag file " + inbag_path + " does not exist.")) + sys.exit() + + output_bag_name = os.path.join( + os.path.dirname(inbag_path), + args.output.format(inbag=os.path.basename(inbag_path)), + ) + + # Check if output bag already exists + if os.path.exists(output_bag_name): + parser.error("not replacing existing file %s" % output_bag_name) + + topic_filter( + args.verbose, inbag_path, args.accept, args.reject, output_bag_name + ) diff --git a/tools/bag_processing/scripts/rosbag_trim.py b/tools/bag_processing/scripts/rosbag_trim.py new file mode 100755 index 0000000000..6598a4ed54 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_trim.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Creates a new bagfile with trimmed start and end times from a provided bagfile. +""" + +import argparse +import os +import sys + +import rosbag + +import utilities.utilities + + +def trim_bag(bag_name, start_time_to_trim, end_time_to_trim): + with rosbag.Bag(bag_name, "r") as bag: + start_time = bag.get_start_time() + new_start_time = start_time + start_time_to_trim + end_time = bag.get_end_time() + new_end_time = end_time - end_time_to_trim + output_bag_name = os.path.splitext(bag_name)[0] + "_trimmed.bag" + run_command = ( + "rosbag filter " + + bag_name + + " " + + output_bag_name + + ' "t.secs >= ' + + str(new_start_time) + + " and t.secs <= " + + str(new_end_time) + + '"' + ) + os.system(run_command) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile.") + parser.add_argument( + "-s", + "--start-time-to-trim", + type=float, + default=0, + help="Duration of time to trim from the beginning of the bagfile in seconds.", + ) + parser.add_argument( + "-e", + "--end-time-to-trim", + type=float, + default=0, + help="Duration of time to trim from the end of the bagfile in seconds.", + ) + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + + trim_bag(args.bagfile, args.start_time_to_trim, args.end_time_to_trim) diff --git a/tools/bag_processing/scripts/rosbag_verify.py b/tools/bag_processing/scripts/rosbag_verify.py new file mode 100755 index 0000000000..10fe8ba360 --- /dev/null +++ b/tools/bag_processing/scripts/rosbag_verify.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +Verify correctness of a ROS bag, as output by rosbag_fix_all.py. This +performs some simple checks that are not part of the standard "rosbag +check". +""" + +from __future__ import print_function + +import argparse +import json +import logging +import os +import sys +import traceback as tb + +import rosbag + + +def get_message_counts(bag_path): + try: + with rosbag.Bag(bag_path, "r") as bag: + types, topics = bag.get_type_and_topic_info() + return {topic: info.message_count for topic, info in topics.items()} + except: + logging.warning("WARNING: Caught exception during message count check") + logging.debug(" Exception was: %s", get_exception_text()) + return None + + +def message_counts_match(bag_path1, bag_path2): + c1 = get_message_counts(bag_path1) + c2 = get_message_counts(bag_path2) + if c2 is None: + logging.info( + "INCONCLUSIVE: Message count comparison: The unfixed bag was too broken to support checking its message counts. (But the fixed bag is likely ok.)" + ) + return True + elif c1 == c2: + num_topics = len(c1) + num_messages = sum([mc for mc in c1.values()]) + logging.info("PASS: Input bags have identical message counts on each topic") + logging.debug( + " Each file has a total of %d messages on %d topics", + num_messages, + num_topics, + ) + return True + else: + logging.warning("FAIL: Input bags do not have identical message counts") + logging.debug( + "Bag message count info:\n%s", + json.dumps({bag_path1: c1, bag_path2: c2}, indent=4, sort_keys=True), + ) + return False + + +def get_exception_text(): + return tb.format_exception_only(*sys.exc_info()[:2])[0].strip() + + +def valid_deserialization(bag_path): + """ + Force deserialization of messages, which can catch some binary + corruption that is not detected by rosbag check. + """ + try: + with rosbag.Bag(bag_path, "r") as bag: + for topic, msg, t in bag.read_messages(): + pass + except: + logging.warning("FAIL: Caught exception during message deserialization check") + logging.debug(" Exception was: %s", get_exception_text()) + return False + + logging.info("PASS: Successfully deserialized messages") + return True + + +def verify(fixed_path, orig_path, verbose=False, deserialize=False): + result = True + result &= message_counts_match(fixed_path, orig_path) + if deserialize: + result &= valid_deserialization(fixed_path) + + if result: + logging.info("PASS: rosbag_verify: All tests passed") + return 0 + else: + logging.warning("FAIL: rosbag_verify: Some tests failed; check output above") + return 1 + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "-v", + "--verbose", + help="print more debug info", + default=False, + action="store_true", + ) + parser.add_argument( + "-d", + "--deserialize", + help="perform deserialization check (can be very slow on large bags)", + default=False, + action="store_true", + ) + parser.add_argument("fixed_bag", help="Path to fixed bag to verify") + parser.add_argument("orig_bag", help="Path to original bag to check against") + + args = parser.parse_args() + + level = logging.DEBUG if args.verbose else logging.INFO + logging.basicConfig(level=level, format="%(message)s") + + ret = verify( + args.fixed_bag, + args.orig_bag, + verbose=args.verbose, + deserialize=args.deserialize, + ) + + # suppress confusing ROS message at exit + logging.getLogger().setLevel(logging.WARN) + + sys.exit(ret) diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_ref.rsp b/tools/bag_processing/scripts/utilities/__init__.py similarity index 100% rename from gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_ref.rsp rename to tools/bag_processing/scripts/utilities/__init__.py diff --git a/tools/bag_processing/scripts/utilities/bmr_renumber_enum.py b/tools/bag_processing/scripts/utilities/bmr_renumber_enum.py new file mode 100755 index 0000000000..3650c35d2f --- /dev/null +++ b/tools/bag_processing/scripts/utilities/bmr_renumber_enum.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python3 +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +""" +This file provides utility functions to be used in *.bmr bag migration +rules to help with migrating legacy messages that contain an enumerated +field where the label numbering has changed. + +We should stop renumbering enumerated types in message definitions! See +[1]. + +Example usage is in [2]. + +[1] https://github.com/nasa/astrobee/blob/develop/doc/general_documentation/maintaining_telemetry.md +[2] https://github.com/nasa/astrobee/blob/develop/communications/ff_msgs/bmr/2021_08_25_MotionResult_renumber_enum.bmr +""" + +from __future__ import print_function + +import logging +import re + +ENUM_REGEX = re.compile( + r"^\s* (?P\w+) \s* (?P