diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 0831030422..fe2d99e65f 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -49,6 +49,7 @@ add_subdirectory(hysteresis) add_subdirectory(l1) add_subdirectory(landing_slope) add_subdirectory(led) +add_subdirectory(matrix) add_subdirectory(mathlib) add_subdirectory(mixer) add_subdirectory(mixer_module) diff --git a/src/lib/matrix/.clang-tidy b/src/lib/matrix/.clang-tidy deleted file mode 100644 index 2878339187..0000000000 --- a/src/lib/matrix/.clang-tidy +++ /dev/null @@ -1,9 +0,0 @@ -Checks: 'clang-diagnostic-*,clang-analyzer-*,*, - ,-cppcoreguidelines-pro-type-vararg - ,-cppcoreguidelines-pro-bounds-array-to-pointer-decay - ,-cppcoreguidelines-pro-bounds-constant-array-index - ,-cppcoreguidelines-pro-bounds-pointer-arithmetic - ' -WarningsAsErrors: '*' -HeaderFilterRegex: '*.h, *.hpp, *.hh, *.hxx' - diff --git a/src/lib/matrix/.github/workflows/test.yml b/src/lib/matrix/.github/workflows/test.yml deleted file mode 100644 index c368930777..0000000000 --- a/src/lib/matrix/.github/workflows/test.yml +++ /dev/null @@ -1,53 +0,0 @@ -name: Tests - -on: - push: - branches: - - 'master' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - strategy: - fail-fast: false # don't cancel if a job from the matrix fails - matrix: - compiler: - - { compiler: GNU, CC: gcc, CXX: g++, FORMAT: ON } - - { compiler: LLVM, CC: clang, CXX: clang++, FORMAT: OFF } - #flavor: [ Release, Coverage ] # TODO - flavor: [ Release ] - exclude: - - compiler: { compiler: LLVM, CC: clang, CXX: clang++ } - flavor: Coverage - - steps: - - uses: actions/checkout@v2 - with: - submodules: 'recursive' - - name: Install Dependencies - run: | - sudo apt install -y clang cmake g++ gcc lcov - - name: Fetch coveralls - if: matrix.flavor == 'Coverage' - run: | - pip install --user cpp-coveralls - - - name: Build & Run - run: | - cmake -DCMAKE_BUILD_TYPE=${{ matrix.flavor }} -DSUPPORT_STDIOSTREAM=ON -DTESTING=ON -DFORMAT=${{ matrix.compiler.FORMAT }} . - make check - - # TODO: enable -# - name: Coveralls build -# if: matrix.flavor == 'Coverage' -# run: | -# cpp-coveralls -i matrix -# - name: Coveralls -# if: matrix.flavor == 'Coverage' -# uses: coverallsapp/github-action@master -# with: -# github-token: ${{ secrets.GITHUB_TOKEN }} - diff --git a/src/lib/matrix/.gitignore b/src/lib/matrix/.gitignore deleted file mode 100644 index 1edf5f525d..0000000000 --- a/src/lib/matrix/.gitignore +++ /dev/null @@ -1,39 +0,0 @@ -*.orig -*.swp -*~ -astyle/ -build*/ -cmake_install.cmake -CMakeCache.txt -CMakeFiles/ -compile_commands.json -CPackConfig.cmake -CPackSourceConfig.cmake -CTestTestfile.cmake -Makefile -test/attitude -test/cmake_install.cmake -test/CMakeFiles/ -test/copyto -test/coverage.info -test/CTestTestfile.cmake -test/filter -test/hatvee -test/helper -test/integration -test/inverse -test/Makefile -test/matrixAssignment -test/matrixMult -test/matrixScalarMult -test/out/ -test/pseudoinverse -test/setIdentity -test/slice -test/squareMatrix -test/transpose -test/vector -test/vector2 -test/vector3 -test/vectorAssignment -Testing/ diff --git a/src/lib/matrix/.gitmodules b/src/lib/matrix/.gitmodules deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/lib/matrix/CMakeLists.txt b/src/lib/matrix/CMakeLists.txt index a899d67989..bdad9b4491 100644 --- a/src/lib/matrix/CMakeLists.txt +++ b/src/lib/matrix/CMakeLists.txt @@ -1,180 +1,36 @@ -cmake_minimum_required(VERSION 2.8) +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ -set(VERSION_MAJOR "1") -set(VERSION_MINOR "0") -set(VERSION_PATCH "2") - -project(matrix CXX) - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -if (NOT CMAKE_BUILD_TYPE) - if(TESTING) - set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type" FORCE) - else() - set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE) - endif() - message(STATUS "set build type to ${CMAKE_BUILD_TYPE}") +if(BUILD_TESTING) + add_subdirectory(test) endif() - -set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage") - -option(SUPPORT_STDIOSTREAM "If enabled provides support for << operator (as used with std::cout)" OFF) -option(TESTING "Enable testing" OFF) -option(FORMAT "Enable formatting" OFF) -option(COV_HTML "Display html for coverage" OFF) -option(ASAN "Enable address sanitizer" OFF) -option(UBSAN "Enable undefined behaviour sanitizer" OFF) - -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -include_directories(${CMAKE_SOURCE_DIR}) - -if(SUPPORT_STDIOSTREAM) - add_definitions(-DSUPPORT_STDIOSTREAM) -endif() - -if (("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") OR ("${CMAKE_CXX_COMPILER_ID}" MATCHES "AppleClang")) - set(CMAKE_CXX_FLAGS_COVERAGE - "--coverage -ftest-coverage -fdiagnostics-absolute-paths -O0 -fprofile-arcs -fno-inline-functions -fno-elide-constructors" - CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE) - set(CMAKE_EXE_LINKER_FLAGS_COVERAGE - "-ftest-coverage -fdiagnostics-absolute-paths" - CACHE STRING "Flags used for linking binaries during coverage builds" FORCE) -else() - set(CMAKE_CXX_FLAGS_COVERAGE - # Add when GCC 9 or later is available as part of the default toolchain: -fprofile-abs-path - "--coverage -fprofile-arcs -ftest-coverage -O0 -fno-default-inline -fno-inline -fno-inline-small-functions -fno-elide-constructors" - CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE) - set(CMAKE_EXE_LINKER_FLAGS_COVERAGE - "--coverage -ftest-coverage -lgcov" - CACHE STRING "Flags used for linking binaries during coverage builds" FORCE) -endif() -mark_as_advanced(CMAKE_CXX_FLAGS_COVERAGE CMAKE_C_FLAGS_COVERAGE CMAKE_EXE_LINKER_FLAGS_COVERAGE) - -add_compile_options( - -pedantic - -Wall - -Warray-bounds - -Wcast-align - -Wcast-qual - -Wconversion - -Wctor-dtor-privacy - -Wdisabled-optimization - -Werror - -Wextra - -Wfloat-equal - -Wformat-security - -Wformat=2 - -Winit-self - -Wlogical-op - -Wmissing-declarations - -Wmissing-include-dirs - -Wno-sign-compare - -Wno-unused - -Wno-unused-parameter - -Wnoexcept - -Wold-style-cast - -Woverloaded-virtual - -Wpointer-arith - -Wredundant-decls - -Wreorder - -Wshadow - -Wsign-conversion - -Wsign-promo - -Wstrict-null-sentinel - -Wswitch-default - -Wundef - -Wuninitialized - -Wunused-variable - ) - -# clang tolerate unknown gcc options -if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") - add_compile_options( - -Wno-error=unused-command-line-argument-hard-error-in-future - -Wno-unknown-warning-option - ) -else() - add_compile_options( - -Wstrict-overflow=5 - ) -endif() - -# santiziers (ASAN, UBSAN) -if(ASAN) - message(STATUS "address sanitizer enabled") - - # environment variables - # ASAN_OPTIONS=detect_stack_use_after_return=1 - # ASAN_OPTIONS=check_initialization_order=1 - - add_compile_options( - -fsanitize=address - -g3 - -O1 - -fno-omit-frame-pointer - ) - - set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address) - -elseif(UBSAN) - message(STATUS "undefined behaviour sanitizer enabled") - - add_compile_options( - -fsanitize=undefined - -fsanitize=integer - -g3 - -O1 - -fno-omit-frame-pointer - ) - - set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=undefined) -endif() - - -add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) - -if(TESTING) - enable_testing() - add_subdirectory(test) - add_dependencies(check test_build) - - add_custom_target(clang-tidy COMMAND clang-tidy -p . ${CMAKE_SOURCE_DIR}/test/*.cpp) - add_dependencies(clang-tidy test_build) -endif() - -if(FORMAT) - set(astyle_exe ${CMAKE_BINARY_DIR}/astyle/src/bin/astyle) - add_custom_command(OUTPUT ${astyle_exe} - COMMAND wget http://sourceforge.net/projects/astyle/files/astyle/astyle%202.06/astyle_2.06_linux.tar.gz -O /tmp/astyle.tar.gz - COMMAND tar -xvf /tmp/astyle.tar.gz - COMMAND cd astyle/src && make -f ../build/gcc/Makefile - WORKING_DIRECTORY ${CMAKE_BINARY_DIR} - ) - - add_custom_target(check_format - COMMAND scripts/format.sh ${astyle_exe} 0 - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - DEPENDS ${astyle_exe} - VERBATIM - ) - - add_custom_target(format - COMMAND scripts/format.sh ${astyle_exe} 1 - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - VERBATIM - DEPENDS ${astyle_exe} - ) - - add_dependencies(check check_format) -endif() - -set(CPACK_PACKAGE_VERSION_MAJOR ${VERSION_MAJOR}) -set(CPACK_PACKAGE_VERSION_MINOR ${VERSION_MINOR}) -set(CPACK_PACKAGE_VERSION_PATCH ${VERSION_PATCH}) -set(CPACK_PACKAGE_CONTACT "james.goppert@gmail.com") -include(CPack) - -# vim: set et fenc=utf-8 ft=cmake ff=unix sts=0 sw=4 ts=4 : diff --git a/src/lib/matrix/LICENSE b/src/lib/matrix/LICENSE deleted file mode 100644 index cc64b5a484..0000000000 --- a/src/lib/matrix/LICENSE +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2015, PX4 Pro Drone Autopilot -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 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. diff --git a/src/lib/matrix/README.md b/src/lib/matrix/README.md deleted file mode 100644 index 1cc31a5e2f..0000000000 --- a/src/lib/matrix/README.md +++ /dev/null @@ -1,146 +0,0 @@ -# matrix [![Build Status](https://travis-ci.org/PX4/Matrix.svg?branch=master)](https://travis-ci.org/PX4/Matrix) [![Coverage Status](https://coveralls.io/repos/PX4/Matrix/badge.svg?branch=master&service=github)](https://coveralls.io/github/PX4/Matrix?branch=master) - -A simple and efficient template based matrix library. - -## License -* (BSD) The Matrix library is licensed under a permissive 3-clause BSD license. Contributions must be made under the same license. - -## Features -* Compile time size checking. -* Euler angle (321), DCM, Quaternion conversion through constructors. -* High testing coverage. - -## Limitations -* No dynamically sized matrices. - -## Library Overview - -* matrix/math.hpp : Provides matrix math routines. - * Matrix (MxN) - * Square Matrix (MxM, has inverse) - * Vector (Mx1) - * Scalar (1x1) - * Quaternion - * Dcm - * Euler (Body 321) - * Axis Angle - -* matrix/filter.hpp : Provides filtering routines. - * kalman_correct - -* matrix/integrate.hpp : Provides integration routines. - * integrate_rk4 (Runge-Kutta 4th order) - -## Testing -The tests can be executed as following: -``` -mkdir build -cd build -cmake -DTESTING=ON .. -make check -``` - -## Formatting -The format can be checked as following: -``` -mkdir build -cd build -cmake -DFORMAT=ON .. -make check_format -``` - - -## Example - -See the test directory for detailed examples. Some simple examples are included below: - -```c++ - // define an euler angle (Body 3(yaw)-2(pitch)-1(roll) rotation) - float roll = 0.1f; - float pitch = 0.2f; - float yaw = 0.3f; - Eulerf euler(roll, pitch, yaw); - - // convert to quaternion from euler - Quatf q_nb(euler); - - // convert to DCM from quaternion - Dcmf dcm(q_nb); - - // you can assign a rotation instance that already exist to another rotation instance, e.g. - dcm = euler; - - // you can also directly create a DCM instance from euler angles like this - dcm = Eulerf(roll, pitch, yaw); - - // create an axis angle representation from euler angles - AxisAngle axis_angle = euler; - - // use axis angle to initialize a DCM - Dcmf dcm2(AxisAngle(1, 2, 3)); - - // use axis angle with axis/angle separated to init DCM - Dcmf dcm3(AxisAngle(Vector3f(1, 0, 0), 0.2)); - - // do some kalman filtering - const size_t n_x = 5; - const size_t n_y = 3; - - // define matrix sizes - SquareMatrix P; - Vector x; - Vector y; - Matrix C; - SquareMatrix R; - SquareMatrix S; - Matrix K; - - // define measurement matrix - C = zero(); // or C.setZero() - C(0,0) = 1; - C(1,1) = 2; - C(2,2) = 3; - - // set x to zero - x = zero(); // or x.setZero() - - // set P to identity * 0.01 - P = eye()*0.01; - - // set R to identity * 0.1 - R = eye()*0.1; - - // measurement - y(0) = 1; - y(1) = 2; - y(2) = 3; - - // innovation - r = y - C*x; - - // innovations variance - S = C*P*C.T() + R; - - // Kalman gain matrix - K = P*C.T()*S.I(); - // S.I() is the inverse, defined for SquareMatrix - - // correction - x += K*r; - - // slicing - float data[9] = {0, 2, 3, - 4, 5, 6, - 7, 8, 10 - }; - SquareMatrix A(data); - - // Slice a 3,3 matrix starting at row 1, col 0, - // with size 2 x 3, warning, no size checking - Matrix B(A.slice<2, 3>(1, 0)); - - // this results in: - // 4, 5, 6 - // 7, 8, 10 -``` - diff --git a/src/lib/matrix/doc/.gitignore b/src/lib/matrix/doc/.gitignore deleted file mode 100644 index 87620ac7e7..0000000000 --- a/src/lib/matrix/doc/.gitignore +++ /dev/null @@ -1 +0,0 @@ -.ipynb_checkpoints/ diff --git a/src/lib/matrix/doc/euler_gimbal_lock.ipynb b/src/lib/matrix/doc/euler_gimbal_lock.ipynb deleted file mode 100644 index 9131c84848..0000000000 --- a/src/lib/matrix/doc/euler_gimbal_lock.ipynb +++ /dev/null @@ -1,240 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": 4, - "metadata": { - "collapsed": true - }, - "outputs": [], - "source": [ - "from sympy import *\n", - "init_printing()" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "metadata": { - "collapsed": false - }, - "outputs": [ - { - "data": { - "image/png": "iVBORw0KGgoAAAANSUhEUgAAA6cAAABMBAMAAABjWdyfAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAv3aB7AAAAD3RSTlMAMquZdlQQ3SJEie/N\nZrv3ZvUrAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAPr0lEQVR4Ae1df4hlVR3/vp2d99783MkMqdVm\nNbGiYqdcKqzkUSPUYLSQErSgUyqEq45Q0VLG3pAgWFynP2JBqR3WjVQU1qxlFwpflEo11ET9IWn4\nyEUCS9dRQ618fc+559c993zv+d47d3Zn5B2Yd8+993O+38/3fO6vN/PZuzDd76/CoL1ZZqDV7/cB\npnfNXvFmqWhQB0zMzu5EUacGU/HmmoGnKVEvwDpvXpHFThCqMyC5yWKMYUBs2Ci4gTU0/5YO+JMd\n5/YYEA2PpoM4QscCYCSuBKFEbfcARp79fErgQsvD6TEgEt34mR3EGBOFxONt79iMB7H7oxtm5IYt\n4aOzGBJPF0dYOlArNxk3T58SdXweYPv8NvzENi4//Q8GRA5pvG5HMsZEIfF400s242N4QhyHf6cb\nuna70yuGxNPFEU6yWrnJuHn6lKhziP82bNsth21dkgvvgwHxRgAwxoQhX8mFkhvCYAcrqLdW4JV0\nk4DnGgOix0TThQtsJjpAZslIXA1CiXoCYOIYbMMbErbmsQwZtcKA5IYxxoQhV+VCyQ1hsIMdSwCe\nBPhfummvs8d0GRCNjaaDIGK4owNklozE1SCEqCO34gE+A9PpmQqPZsikKwxICmy/xYxmjCEgjqjx\neN+ZMhkPYe+r0FBn6riqx+wWnQgkni6KcEWtlRtBX4t63f0/hubRuUU496orETo6A3B4ET63KIYB\n3IM/zcvu3w03zt4HjStmu7gegOBWrwnswVU4/+FzP9nBXaEx8cwiphI1Hg/5t1/Y0fj9B2cvFuOQ\n+chrsPU10U/z5woJQFI03o2j9OMIjKVFrYWbr0OAvhK1/XPYnpyXwKlmD1pIY3IFYOdFFz0C8E5R\n4FP4c2B340RjH0x2bgG41YWMXv5hBcGF1yT27wB/6LRfxV2BsGTm0blLcITILJoSlYpnOEj+kztg\n/BTcmeC4fwFsWb3ofafh6JF5vKN0cZNfiIE0erhXQnApG5XOzgqFyLBXopLc7nlmns+NpG8rVKKO\nd+HaqW/id9a3H4dRrKe1G+A2gP/CW98tqvsC/izD8IutJWi8ePc8LOG6gdwL71cQXHhNYlHU59Lb\nmhljw5KZPwGXYzCRWTQlKhXPcBgR/FHUSfVA8HUk2oVt3aGVoR6eMuJo9AvRkMaHxF4JwaVsVDpL\nn0Jk2CtRKW6jyZYunxtF36lQibowg9cavPFsW9p/KsF6xhYBXpdb5KxO49X3Jdws7rGvt974NHYt\nZB9MJ7hLbNPtpm+I9iucUIFFUf8J8DLuzIcFMvNlgNxMWCUqFc9yEPyFqKhkB8efFDUh76GlidNY\nIv7kCtGQVE8BMY1Klx5romQKkWGvRAWCWytpvMrmRtK3FYqJE78mPNzBQwVvPNt6N/5RqIez33wF\nho6pUwXpjwhRrl0EeKP5QH/GhZyEO3GzqDDfJBZFfd6I6oUtyAwfx3gy7APLy99aXv41rlLxLAfB\nX4jaM6Iu7IaP4dChbjpxuUJQ1BQiT9KMqFQ6KyqJMOxHl5cf/8XyspgygtvYYhunXiRmcKMhpkIt\nqjlfPprA0ygRXiebr8lpkWfqIecAf+l7MIEcHAhcO58+Q+JWr0msFTUf1p6puczNhzAYZpZNnalk\nPM2hLfg7ouK1dWFRXiX2TuGk3RooxECkqAJiGplOzwqQCJe9OlNpbsP4yxEmN3Omah0MfdAValHx\nzga7H8Yz9gdLsGUF5BPNSfgJVifpP4WdZTyXx2bwWfKLAB/AdfHQk0Lga7gqIPkmsVZUO8aEJTM3\nr+xgPB1WiUrG0xyGBX9HVHwKGlscRaGu/w1Gk09BfiEGIkXNPCiR6Qx9CpFhr0QluUGrw+dG0rcV\nqstv+5cwmmyfgt8NH4fJBJ/9dwDsap8AJeql2DmwAvc2HoLxpT3z8A9ct5DRGVwVkHyTWLz2qsuv\nHSNnRYyhM8sHJR1WiUrG0xwkf7z26svvEbzudg4kmEg8KA0Jpn4hBiJFlRBEyUamM/RJhMteiyrm\nNsgNv5awuZH0bYVKVPjyrnuhfdmRxZEHP3sfZhDXgus+NY89Sf+32Gm+ay6B2+9/L9z1l78u4rqF\n/BnXQEDyTWDP6V9yTv/Ca/6zwx1jwtKZ5WOZDqtEJeNpDoJ/Y+fql3auvu2FnyKfQ0h8l/jKJZ/6\nxgVxvxADkaJKiMCLRqYz9EmEy16JSnKbEMcalxtJ31aoRZU1OB/7dF/Qbx/Ta+5SQ0aWRqYIiAuX\nfT1GHiuFYfGedOeKDatE9QPqeCSHVpIO2TvVxl8VHvDHi3UNSZ9+gxA9TKej6StElr0SVUfRS5P4\nLvh+dW46ilMhJao88kX2Pfgz2hE9v2nI+VdfM09A/CGgxzDC7oMnnbA/zIWSG3Q8koM8CRDaSiaO\nAxwNRdEQGBbHbhCih+l0NH2NyLAfWdEBMkuduPHo1c8SiTWkgJuGOBVSooobq2i3778Y4Ka0731q\nyCPCExOGeCPw6SpJNzHCvmPuPfGwOh7NQdz+sTWOHkmg2cVevilI+yOrHWh28/vtFp2Opq8RLPYq\n8Vi/fxqaXZvG6TG45SukRG338pGdLaLLgHgjWGNKhY2DDzoc9Iw7m0SXAVEj4uniCCc5I3ElCCUq\n3GGTTyS27/YYEBcu+4wxDIgNGwW3nUvfE3ac22NANDyajjNvOhgwEleCkKKazIPOppuBgaibTrI4\n4YGo8TnadIiBqJtOsjjhgajxOdp0CErUSibi4uovwN03p8+iE1NBaB0IJ/AGrUHQikyELaJSDa6o\n2zs2mPh+tCYPtAwVtznXgbCss72NU0OGl6BV6JNfsw6uqNNLNvlj+EuYNXmgZai4zbkOhGWd7W2c\nGjK8kFaxT37NOriiOqm3LuGvS2vzQMvAc/hZbA+viGgmMn7uY4PWIGhFJsKWUq0GQtSxpE4PtCR5\nAv8OXGwPr4gg/gYCG7QGQSsyEVbUajW4otbuM4aozbkOBCUq/p10jT5uObl11GBVSg06rUKf/Jp1\n0KLGfcae6Tpkk3a512KELmWVZricGZDaawjk9H3yTs64DoF4vl1didrsoYd7MuCBbvQw40QX/77i\n2b2tTfqeZ+aVwwahtt0iPN9oT6J93BTCOLOBRLhGb32mki5nUwPt47ZGaFsA9qj8DDO3cbgHaKU+\n+YzfW6UldbAEA/FSu7otU4lK+YytwXncs3sDuthaaK7tOlbkzITcLTzfKOpzpI+bQhhnNlCIkFWa\nY9JmGKFrryGQM/XJZ4pQaSkdHKd2IJ7UwUol3IQ3zGPE/acSaZdFmTq4ehItwMIDbQ3OCzOpzUfb\nvQ3EWpFxnNMom3PqeppGJIWwzmwKEbJKG++kdpvnayAhjhHaqYBmWKKGUM7UJ2+LaD4uvO+3id/I\nEDpYgqF4nlTwXYDzEgxG+IytqIc7Wbu3EHVB2KStFRnDOI20OUvTlhCVQlhnNoWAgFWadjlLQ1nj\nNH7lfxmzuoZ0XQN6+bp4zCIk28j8/BpCOZWh3RThJKV0MARD8aQOVipxyAmHfpvyQMsJEcZBc6Yq\n07W4/C6kNmltRXa4YZe0OcsJOVSE0M5sOkbAKm0O4ZzL2dRAQ6wROlNEDTWEciqfvFuESkvqYAiG\n4qU6mDK1mXuY8kBLpHhQwntqxu4tHpRSm7S1Iitm6YKyOaeXLmHSJhHamU0iQlbpnNvcEDQ10BBr\nhK69hmXPBI+0UhN8pgiVdpjSwRIMxEt1sGWqByXSAy2RwuDsm67BeKDlv17NeKAlwz3C8/18kY+b\nRGhnNpCIgFWaY9I+4BnSbQ1DPWWnVrObLsj88mpzKYJIhKkhkFP55N0iVFpSB+vUDsRL7epGKn2m\nkj5jiZQ+Y8/ubW3S0qMoIZkJIW3OckKESZtESJ9lISJgleaYtBlG6NprCORUPnm3CJWW1ME6tQPx\nUru6lUqdqZlScEU7hNO7b9DgbCC0FTkTdZ9eE6K2j+k1Z6kRpDMbFIJllS5Tg2OEdvjku5qhvIVU\nrEHPW7aIfC65RYMLCGqIIxUhqnYIM0zEBVbkDFNtc44boUlntvGCc6zSUKIGxwid4eyv1FGDoZUp\nws+k1jW4gKCGOFIRosp/AoWBiwzODCuyS3UySdfiRmjama1jsKzSJWoosnrXXYOmBawi8k5tl4/s\n5/3elKgHnbF6Kp1NosuAuCPaPWdNEXG2YLcOhBuRQZABcSPWwtDNaRR2s7h9F8zWgRK1konYZZPv\n32E3EfbwOhA2SU1eaScgMKza8RrcqSUmwuZ0wU/YzW4vD6FEdUcN+ptsBgaibjLBOHQHonJmaZNh\nBqJuMsE4dAeicmZpk2HKicqwFq9D/XGPd6n3Ya8Dw3hIxswxIPE8EhEXdXvHhhLfmiIWbwteSy/u\n8c5EF98fC/3RGfTZWGHMHAPCZB4XtaS1mJk3Aot7vDMBxucj/ugM+mysPFaLOZ7JPC6qE2jrEv6q\nv9ji7aDr6s5hoGIXePh92HXlryMOY+YYEC6TUqKOJVGLNzdvCdyJuPmZASmRcB2gjJljQLjE4qKW\nsxZz80ZwcQe1G0DYbVqF/mgXfTb6wr2ztheEl2FdLGrcWlwmFxPLcHB7xnLhMjicfY84M9d6wTyC\nIec7A1KVXaGozR5h8Sb821VJZMfdQrjArfnZN5ZPrqj3iFsbeDbkmV7zCQac7zTE+rar0i4UlbIW\nU/7tqiSy4+4mXODW/DzuGcvNC7+tDTwb8kyv+QQDzncS4vi2q9IOiWq9xftPBS3elH+7KonsuLiD\newEvt8H3iFsbeDbkmV7zCQac7yTE+rYrsw6JaoMR1mLKv20HrqVHOqiN+dk3lpv3iFsb+FoIrH2s\nTzDgfCchlLG8BKtCUWlrcdi/XSJvAZR0UBvzsznKlbHcvvDb2MAL4p+BXT7BgPO9ALJ3Kn2JbmWi\nhaIOU9Ziwr9dmUVmIOXxtuZnvB9ljOXiQSn6kvBMjnVe8QkGnO80xPq2q7IsFjX+KumqeQvG7aFc\n4Mb87BvLR3fo94gbC3VB/DOwyycYcL4XQMLG8hK0C0UlrcXSlpj3b5fIWwAlPd7W/OwZy1kvCS/I\nWP8uj2DI+U5CrG+7Kq9CUf2gxjfM9G/746ut70uHFZmfFQRoG3i11LWNYsychhT4trl0SomqfcNc\n/zaXRDFOO6gLzM8aQtvAi1Os+17GzGlIgW+bS7OUqNqlWvQqaW5iPm4ySbEF5mcNoW3g/HTrg2Q4\n3xm+bSa3cqKKv+PqpidSr6/bst1zQgdd4AwbuBPirHQZM8eAMKmXE7WN3x10e0J31n0Z90czTNbr\nzrI4AWPmGJDiHGZvOVHNsEFnI8+AELXfX93IFAfcSs1Aqy/+95HrZ2c/U2rYALyRZ2BidnYW/g9o\nKMBQ2em6pwAAAABJRU5ErkJggg==\n", - "text/latex": [ - "$$\\left[\\begin{matrix}\\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{2} \\right )} & - \\sin{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{3} \\right )} + \\sin{\\left (\\theta_{2} \\right )} \\sin{\\left (\\theta_{3} \\right )} \\cos{\\left (\\theta_{1} \\right )} & \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{3} \\right )} + \\sin{\\left (\\theta_{2} \\right )} \\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{3} \\right )}\\\\\\sin{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{2} \\right )} & \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{2} \\right )} \\sin{\\left (\\theta_{3} \\right )} + \\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{3} \\right )} & \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{2} \\right )} \\cos{\\left (\\theta_{3} \\right )} - \\sin{\\left (\\theta_{3} \\right )} \\cos{\\left (\\theta_{1} \\right )}\\\\- \\sin{\\left (\\theta_{2} \\right )} & \\sin{\\left (\\theta_{3} \\right )} \\cos{\\left (\\theta_{2} \\right )} & \\cos{\\left (\\theta_{2} \\right )} \\cos{\\left (\\theta_{3} \\right )}\\end{matrix}\\right]$$" - ], - "text/plain": [ - "⎡cos(θ₁)⋅cos(θ₂) -sin(θ₁)⋅cos(θ₃) + sin(θ₂)⋅sin(θ₃)⋅cos(θ₁) sin(θ₁)⋅sin(θ₃) \n", - "⎢ \n", - "⎢sin(θ₁)⋅cos(θ₂) sin(θ₁)⋅sin(θ₂)⋅sin(θ₃) + cos(θ₁)⋅cos(θ₃) sin(θ₁)⋅sin(θ₂)⋅\n", - "⎢ \n", - "⎣ -sin(θ₂) sin(θ₃)⋅cos(θ₂) cos\n", - "\n", - "+ sin(θ₂)⋅cos(θ₁)⋅cos(θ₃)⎤\n", - " ⎥\n", - "cos(θ₃) - sin(θ₃)⋅cos(θ₁)⎥\n", - " ⎥\n", - "(θ₂)⋅cos(θ₃) ⎦" - ] - }, - "execution_count": 16, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "th1, th2, th3 = symbols('theta_1, theta_2, theta_3')\n", - "dcm_321 = Matrix([[cos(th1)*cos(th2),\n", - " cos(th1)*sin(th2)*sin(th3) - sin(th1)*cos(th3),\n", - " cos(th1)*sin(th2)*cos(th3) + sin(th1)*sin(th3)],\n", - " [sin(th1)*cos(th2),\n", - " sin(th1)*sin(th2)*sin(th3) + cos(th1)*cos(th3),\n", - " sin(th1)*sin(th2)*cos(th3) - cos(th1)*sin(th3)],\n", - " [-sin(th2),\n", - " cos(th2)*sin(th3),\n", - " cos(th2)*cos(th3)]\n", - " ])\n", - "dcm_321" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "metadata": { - "collapsed": false - }, - "outputs": [ - { - "data": { - "image/png": "iVBORw0KGgoAAAANSUhEUgAAA1oAAABMBAMAAACG69u2AAAAMFBMVEX///8AAAAAAAAAAAAAAAAA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\nlOIAAAAASUVORK5CYII=\n", - "text/latex": [ - "$$\\left[\\begin{matrix}\\cos{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} & \\sin{\\left (\\phi \\right )} \\sin{\\left (\\theta \\right )} \\cos{\\left (\\psi \\right )} - \\sin{\\left (\\psi \\right )} \\cos{\\left (\\phi \\right )} & \\sin{\\left (\\phi \\right )} \\sin{\\left (\\psi \\right )} + \\sin{\\left (\\theta \\right )} \\cos{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )}\\\\\\sin{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} & \\sin{\\left (\\phi \\right )} \\sin{\\left (\\psi \\right )} \\sin{\\left (\\theta \\right )} + \\cos{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} & - \\sin{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} + \\sin{\\left (\\psi \\right )} \\sin{\\left (\\theta \\right )} \\cos{\\left (\\phi \\right )}\\\\- \\sin{\\left (\\theta \\right )} & \\sin{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )} & \\cos{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )}\\end{matrix}\\right]$$" - ], - "text/plain": [ - "⎡cos(ψ)⋅cos(θ) sin(φ)⋅sin(θ)⋅cos(ψ) - sin(ψ)⋅cos(φ) sin(φ)⋅sin(ψ) + sin(θ)⋅c\n", - "⎢ \n", - "⎢sin(ψ)⋅cos(θ) sin(φ)⋅sin(ψ)⋅sin(θ) + cos(φ)⋅cos(ψ) -sin(φ)⋅cos(ψ) + sin(ψ)⋅\n", - "⎢ \n", - "⎣ -sin(θ) sin(φ)⋅cos(θ) cos(φ)⋅cos(θ\n", - "\n", - "os(φ)⋅cos(ψ) ⎤\n", - " ⎥\n", - "sin(θ)⋅cos(φ)⎥\n", - " ⎥\n", - ") ⎦" - ] - }, - "execution_count": 18, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "phi, theta, psi = symbols('phi, theta, psi')\n", - "sub_eulers = {\n", - " th1: psi, th2: theta, th3: phi\n", - "}\n", - "dcm_321_euler = dcm_321.subs(sub_eulers)\n", - "dcm_321_euler" - ] - }, - { - "cell_type": "code", - "execution_count": 38, - "metadata": { - "collapsed": false - }, - "outputs": [ - { - "data": { - "image/png": "iVBORw0KGgoAAAANSUhEUgAAAOAAAABLCAMAAABeKHv+AAAAPFBMVEX///8AAAAAAAAAAAAAAAAA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\nRK5CYII=\n", - "text/latex": [ - "$$\\left[\\begin{matrix}0 & - \\sin{\\left (\\psi \\right )} & \\cos{\\left (\\psi \\right )}\\\\0 & \\cos{\\left (\\psi \\right )} & \\sin{\\left (\\psi \\right )}\\\\-1 & 0 & 0\\end{matrix}\\right]$$" - ], - "text/plain": [ - "⎡0 -sin(ψ) cos(ψ)⎤\n", - "⎢ ⎥\n", - "⎢0 cos(ψ) sin(ψ)⎥\n", - "⎢ ⎥\n", - "⎣-1 0 0 ⎦" - ] - }, - "execution_count": 38, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "dcm_pitch_up = dcm_321_euler.subs({theta:pi/2, phi:0})\n", - "dcm_pitch_up" - ] - }, - { - "cell_type": "code", - "execution_count": 39, - "metadata": { - "collapsed": false - }, - "outputs": [ - { - "data": { - "image/png": "iVBORw0KGgoAAAANSUhEUgAAAMUAAAAVBAMAAAAEKDfsAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAv3aB7AAAAD3RSTlMAZnaJVN0imavvMkQQ\nzbsZbzSbAAAACXBIWXMAAA7EAAAOxAGVKw4bAAADfUlEQVRIDcVVS4gUVxQ91VVdM139mWYWGvLB\n0iEhi4ilJo6QQJqQ4CLgNCi69AlukoUZxi9Z6JAsEtzYQYkhcdGKgQwmTEMSQTdT6MIPiE0yiwTF\ntLNQAjFOxC8K47n31fQ4XW22Xrivzjv3njrvUz0DPO84klpAUBYqAJx6qjZLWBXrL5KR/L/wW1od\nC+eatip0Oe6bI+ejRBUBvXWbtp6f35bMirGCcbbPxhUF4lGcpTqfiUpEzSS15Wxno84HUmzG2m1m\nIYGpFliVX2ZFYPslC9OtZL5OsTmj1AKOhVqqagmrcgxn55OUgndfxs7IVy3zcb1d+dUi3c1km50H\nEpUrZLFis38gxAt3v6xAwE+Hjr4eSlXCHQaOvrfUvzUa/Pnpks+EGuT6B0NEGdoTa5xd+SYKKwYa\n0ktCVGdG4OJVizn3NmA3sJF7UXA19O9RfiNmd6mMQgtZlEZRnMInhtQ16rxpv5J5DLzCOcM/iDHz\nvcGU9qoK0RC2YFkMp8mPvAnvW0zE6qHgb+A/uKaXRWQryB/mguhRqqGvTGo9sKWn6sSoAR9wzig2\nMVRfB4wfl15V+ZUdeBk5rrtqMxjYa8QDCm4Ct5E1ATeDXAPYNGXUo4m+kNR3QJyL+KphvpRzxiXC\n4A7QF0mvqgo4gAg95Kc1C1fNeF08LPhXPHIN/5Htxpm/7qtHq+2BCeOiELY9Jgg9tve1pNeuLH+P\nHqfbHsUqxn+MNyK0QD0o4nnLWfkGixpyVokHzwpv8azdGEi+sfY+dkqvquBW/QreAeQbY16KMHS8\n/gciCxKPbMiX8c69CL3lpzx459jDs/6Qz+TOeR+oHAImfpFeVaGn5cQe/ZI7z7Xw7pF4ByILEo9B\nvgPuKLzDKBluYnYfq0mfROhQj128C77f/wquGavjd+1VFfK1DF5iS8+wpv/FktO/ofdzo+CHmX1r\nH47CYU13mV+++O1g/4ML+x8cu/UNKTkfb/u116R8meciR3puZBX8basb0sup/gbXrO03xEWuRbJb\n/IxTQm9I1bKGVFAR3q9xOCGoI1R1TMmPOEp2iWDy/X+EXpiq6QYzsfBuyKEsqCNU9YaSKzhKdonc\nzMy00CWTKl4no3+KcJGo0M1DVZEoC02bgp8VfitV2Uomo6y4Oak6CVWFUhG3LuuU0lxsnoMJ0n8L\ngh2TMOlHW7WANeYTnXDuNRzecvUAAAAASUVORK5CYII=\n", - "text/latex": [ - "$$\\operatorname{atan_{2}}{\\left (\\sin{\\left (\\psi \\right )},\\cos{\\left (\\psi \\right )} \\right )}$$" - ], - "text/plain": [ - "atan2(sin(ψ), cos(ψ))" - ] - }, - "execution_count": 39, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "atan2(dcm_pitch_up[1,2], dcm_pitch_up[0,2])" - ] - }, - { - "cell_type": "code", - "execution_count": 40, - "metadata": { - "collapsed": false - }, - "outputs": [ - { - "data": { - "image/png": "iVBORw0KGgoAAAANSUhEUgAAAOAAAABLCAMAAABeKHv+AAAAPFBMVEX///8AAAAAAAAAAAAAAAAA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\nRK5CYII=\n", - "text/latex": [ - "$$\\left[\\begin{matrix}0 & - \\sin{\\left (\\psi \\right )} & \\cos{\\left (\\psi \\right )}\\\\0 & \\cos{\\left (\\psi \\right )} & \\sin{\\left (\\psi \\right )}\\\\-1 & 0 & 0\\end{matrix}\\right]$$" - ], - "text/plain": [ - "⎡0 -sin(ψ) cos(ψ)⎤\n", - "⎢ ⎥\n", - "⎢0 cos(ψ) sin(ψ)⎥\n", - "⎢ ⎥\n", - "⎣-1 0 0 ⎦" - ] - }, - "execution_count": 40, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "dcm_pitch_down = dcm_321_euler.subs({theta:pi/2, phi:0})\n", - "dcm_pitch_down" - ] - }, - { - "cell_type": "code", - "execution_count": 41, - "metadata": { - "collapsed": false - }, - "outputs": [ - { - "data": { - "image/png": "iVBORw0KGgoAAAANSUhEUgAAAMUAAAAVBAMAAAAEKDfsAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAv3aB7AAAAD3RSTlMAZnaJVN0imavvMkQQ\nzbsZbzSbAAAACXBIWXMAAA7EAAAOxAGVKw4bAAADfUlEQVRIDcVVS4gUVxQ91VVdM139mWYWGvLB\n0iEhi4ilJo6QQJqQ4CLgNCi69AlukoUZxi9Z6JAsEtzYQYkhcdGKgQwmTEMSQTdT6MIPiE0yiwTF\ntLNQAjFOxC8K47n31fQ4XW22Xrivzjv3njrvUz0DPO84klpAUBYqAJx6qjZLWBXrL5KR/L/wW1od\nC+eatip0Oe6bI+ejRBUBvXWbtp6f35bMirGCcbbPxhUF4lGcpTqfiUpEzSS15Wxno84HUmzG2m1m\nIYGpFliVX2ZFYPslC9OtZL5OsTmj1AKOhVqqagmrcgxn55OUgndfxs7IVy3zcb1d+dUi3c1km50H\nEpUrZLFis38gxAt3v6xAwE+Hjr4eSlXCHQaOvrfUvzUa/Pnpks+EGuT6B0NEGdoTa5xd+SYKKwYa\n0ktCVGdG4OJVizn3NmA3sJF7UXA19O9RfiNmd6mMQgtZlEZRnMInhtQ16rxpv5J5DLzCOcM/iDHz\nvcGU9qoK0RC2YFkMp8mPvAnvW0zE6qHgb+A/uKaXRWQryB/mguhRqqGvTGo9sKWn6sSoAR9wzig2\nMVRfB4wfl15V+ZUdeBk5rrtqMxjYa8QDCm4Ct5E1ATeDXAPYNGXUo4m+kNR3QJyL+KphvpRzxiXC\n4A7QF0mvqgo4gAg95Kc1C1fNeF08LPhXPHIN/5Htxpm/7qtHq+2BCeOiELY9Jgg9tve1pNeuLH+P\nHqfbHsUqxn+MNyK0QD0o4nnLWfkGixpyVokHzwpv8azdGEi+sfY+dkqvquBW/QreAeQbY16KMHS8\n/gciCxKPbMiX8c69CL3lpzx459jDs/6Qz+TOeR+oHAImfpFeVaGn5cQe/ZI7z7Xw7pF4ByILEo9B\nvgPuKLzDKBluYnYfq0mfROhQj128C77f/wquGavjd+1VFfK1DF5iS8+wpv/FktO/ofdzo+CHmX1r\nH47CYU13mV+++O1g/4ML+x8cu/UNKTkfb/u116R8meciR3puZBX8basb0sup/gbXrO03xEWuRbJb\n/IxTQm9I1bKGVFAR3q9xOCGoI1R1TMmPOEp2iWDy/X+EXpiq6QYzsfBuyKEsqCNU9YaSKzhKdonc\nzMy00CWTKl4no3+KcJGo0M1DVZEoC02bgp8VfitV2Uomo6y4Oak6CVWFUhG3LuuU0lxsnoMJ0n8L\ngh2TMOlHW7WANeYTnXDuNRzecvUAAAAASUVORK5CYII=\n", - "text/latex": [ - "$$\\operatorname{atan_{2}}{\\left (\\sin{\\left (\\psi \\right )},\\cos{\\left (\\psi \\right )} \\right )}$$" - ], - "text/plain": [ - "atan2(sin(ψ), cos(ψ))" - ] - }, - "execution_count": 41, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "dcm_pitch_down = dcm_321_euler.subs({theta:-pi/2, phi:0})\n", - "atan2(-dcm_pitch_down[1,2], -dcm_pitch_down[0,2])" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 2", - "language": "python", - "name": "python2" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 2 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython2", - "version": "2.7.6" - } - }, - "nbformat": 4, - "nbformat_minor": 0 -} diff --git a/src/lib/matrix/doc/nasa_rotation_def.pdf b/src/lib/matrix/doc/nasa_rotation_def.pdf deleted file mode 100644 index eb67a4bfcf..0000000000 Binary files a/src/lib/matrix/doc/nasa_rotation_def.pdf and /dev/null differ diff --git a/src/lib/matrix/scripts/format.sh b/src/lib/matrix/scripts/format.sh deleted file mode 100755 index feee98a76c..0000000000 --- a/src/lib/matrix/scripts/format.sh +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/bash -echo pwd:$PWD -astyle=$1 -format=$2 -format_wildcards=""" -./matrix/*.*pp -./test/*.*pp -""" - -#echo astyle: $astyle -#echo format: $format - -if [[ $format -eq 1 ]] -then - echo formatting - $astyle ${format_wildcards} -else - echo checking format... - $astyle --dry-run ${format_wildcards} | grep Formatted &>/dev/null - if [[ $? -eq 0 ]] - then - echo Error: need to format - echo "From cmake build directory run: 'make format'" - exit 1 - fi - echo no formatting needed - exit 0 -fi - -# vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : diff --git a/src/lib/matrix/test/CMakeLists.txt b/src/lib/matrix/test/CMakeLists.txt index 944bfc32a3..1eb71f9634 100644 --- a/src/lib/matrix/test/CMakeLists.txt +++ b/src/lib/matrix/test/CMakeLists.txt @@ -1,4 +1,9 @@ -include(gtest.cmake) +#include(gtest.cmake) + +add_compile_options( + -Wno-double-promotion + -Wno-float-equal +) set(tests setIdentity @@ -15,7 +20,6 @@ set(tests attitude filter integration - sparseVector squareMatrix helper hatvee @@ -24,44 +28,19 @@ set(tests upperRightTriangle dual pseudoInverse - ) +) add_custom_target(test_build) foreach(test_name ${tests}) - add_executable(${test_name} - ${test_name}.cpp) - add_test(test_${test_name} ${test_name}) - add_dependencies(test_build ${test_name}) + add_executable(test-matrix_${test_name} ${test_name}.cpp) + + add_test(NAME test-matrix_${test_name} + COMMAND test-matrix_${test_name} + WORKING_DIRECTORY ${PX4_BINARY_DIR} + ) + + add_dependencies(test_build test-matrix_${test_name}) + add_dependencies(test_results test-matrix_${test_name}) endforeach() -target_link_libraries(sparseVector gtest_main) -if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage") - - add_custom_target(coverage_build - COMMAND ${CMAKE_CTEST_COMMAND} - COMMAND lcov --capture --directory . --output-file coverage.info --rc lcov_branch_coverage=1 - COMMAND lcov --rc lcov_branch_coverage=1 --summary coverage.info - WORKING_DIRECTORY ${CMAKE_BUILD_DIR} - DEPENDS test_build - ) - - add_custom_target(coverage_html - COMMAND genhtml coverage.info --output-directory out --branch-coverage - COMMAND x-www-browser out/index.html - WORKING_DIRECTORY ${CMAKE_BUILD_DIR} - DEPENDS coverage_build - ) - - set(coverage_deps - coverage_build) - - if (COV_HTML) - list(APPEND coverage_deps coverage_html) - endif() - - add_custom_target(coverage - DEPENDS ${coverage_deps} - ) -endif() - -# vim: set et fenc=utf-8 ft=cmake ff=unix sts=0 sw=4 ts=4 : +px4_add_unit_gtest(SRC sparseVector.cpp) diff --git a/src/lib/matrix/test/CMakeLists.txt.in b/src/lib/matrix/test/CMakeLists.txt.in deleted file mode 100644 index a341cac5c5..0000000000 --- a/src/lib/matrix/test/CMakeLists.txt.in +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.4) - -project(googletest-download NONE) - -include(ExternalProject) -ExternalProject_Add(googletest - URL https://github.com/google/googletest/archive/e2239ee6043f73722e7aa812a459f54a28552929.zip - SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src" - BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build" - CONFIGURE_COMMAND "" - BUILD_COMMAND "" - INSTALL_COMMAND "" - TEST_COMMAND "" - # Wrap download, configure and build steps in a script to log output - LOG_DOWNLOAD ON - LOG_CONFIGURE ON - LOG_BUILD ON -) diff --git a/src/lib/matrix/test/gtest.cmake b/src/lib/matrix/test/gtest.cmake deleted file mode 100644 index f1c07b3576..0000000000 --- a/src/lib/matrix/test/gtest.cmake +++ /dev/null @@ -1,12 +0,0 @@ -set_directory_properties(PROPERTIES COMPILE_OPTIONS "") - -# Download and unpack googletest at configure time -configure_file(${CMAKE_CURRENT_LIST_DIR}/CMakeLists.txt.in googletest-download/CMakeLists.txt) -execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . RESULT_VARIABLE result1 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download) -execute_process(COMMAND ${CMAKE_COMMAND} --build . RESULT_VARIABLE result2 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download) -if(result1 OR result2) - message(FATAL_ERROR "Preparing googletest failed: ${result1} ${result2}") -endif() - -# Add googletest, defines gtest and gtest_main targets -add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src ${CMAKE_CURRENT_BINARY_DIR}/googletest-build EXCLUDE_FROM_ALL)