diff options
| author | Dawsyn Schraiber <[email protected]> | 2024-05-09 01:20:17 -0400 |
|---|---|---|
| committer | GitHub <[email protected]> | 2024-05-09 01:20:17 -0400 |
| commit | 90c4d94b13472114daab71d3e368660224423c90 (patch) | |
| tree | 2a56c3780e6ba2f157ce15f2356134cff5035694 | |
| parent | d695dce1a7ea28433db8e893025d1ec66fb077b2 (diff) | |
| download | active-drag-system-90c4d94b13472114daab71d3e368660224423c90.tar.gz active-drag-system-90c4d94b13472114daab71d3e368660224423c90.tar.bz2 active-drag-system-90c4d94b13472114daab71d3e368660224423c90.zip | |
02/24/2024 Test Launch Version (BB Black) (#11)
* Adding a 90% completed, compilable but untested ADS
* Made basic changes to actuator & sensor. Also added motor class
* Removed unnecessary .cpp files
* Updated sensor & actuator classes, finished ads, added variable time step to kalman filter, set up all tests for future assertions
* Relocated 'main' to 'active-drag-system.cpp'. Added more info to README
* Removed main.cpp
* Added more details to README
* Changed some function parameters from pass-by-pointer to pass-by-reference. Also removed the std namespace
* Started writing the test cases
* Updated the .gitignore file
* Removed some files that should be gitignored
* Up to date with Jazz's pull request
* Test Launch Branch Created; PRU Servo Control with Test Program
* Added I2C device class and register IDs for MPL [INCOMPLETE SENSOR IMPLEMENTATION]
Needs actual data getting function implementation for both sensors and register IDs for BNO, will implement shortly.
* Partial implementation of MPL sensor
Added startup method, still needs fleshed out data getters and setters and finished I2C implementation. MOST LIKELY WILL HAVE COMPILATION ISSUES.
* *Hypothetically* complete MPL implementation
NEEDS HARDWARE TESTING
* IMU Header and init() method implementation
Needs like, all data handling still lol
* Hypothetically functional (Definitely won't compile)
* We ball?
---------
Co-authored-by: Jazz Jackson <[email protected]>
Co-authored-by: Cian Capacci <[email protected]>
50 files changed, 4128 insertions, 93 deletions
@@ -1,15 +1,22 @@ -Testing/*
-.DS_Store
-.vagrant/*
-.idea/
-
-CMakeFiles/
-cmake_install.cmake
-CMakeCache.txt
-Makefile
-
-src/CMakeFiles
-src/cmake_install.cmake
-src/Makefile
-
-build/*
+<<<<<<< HEAD +Testing/* +.DS_Store +.vagrant/* +.idea/ +./vscode +CMakeFiles/ +cmake_install.cmake +CMakeCache.txt +Makefile +src/CMakeFiles +src/cmake_install.cmake +src/Makefile +build/* +tests/output.txt +tests/loggerRun +test/output.txt +test/loggerRun +test/adsTest.cpp +docs/150_VirginiaTech_TechnicalReport.pdf +docs/Chen_Presentation_ADS.pdf +docs/TestLaunch2-Critical.txt diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..63e048a --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,22 @@ +{ + "configurations": [ + { + "name": "Win32", + "includePath": [ + "${workspaceFolder}/**", + "${workspaceFolder}/include/math" + ], + "defines": [ + "_DEBUG", + "UNICODE", + "_UNICODE" + ], + "windowsSdkVersion": "10.0.22621.0", + "compilerPath": "cl.exe", + "cStandard": "c17", + "cppStandard": "c++17", + "intelliSenseMode": "windows-msvc-x64" + } + ], + "version": 4 +}
\ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e3ed90..28e3426 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,10 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE) # Set the cross-compiler for ARM32 if(CMAKE_HOST_APPLE) message(STATUS "Running on an Apple system") - + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") + include(CTest) # Apple Settings # Musl has many targets, must select one set(CMAKE_C_COMPILER /opt/homebrew/bin/arm-linux-musleabihf-gcc) @@ -22,12 +25,12 @@ elseif(UNIX) endif() # Set the architecture and flags -set(CMAKE_C_FLAGS "-march=armv7-a+fp") -set(CMAKE_CXX_FLAGS "-march=armv7-a+fp") +# set(CMAKE_C_FLAGS "-march=armv7-a+fp") +# set(CMAKE_CXX_FLAGS "-march=armv7-a+fp") -project(Active-Drag-System CXX) +# project(Active-Drag-System CXX) -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/out") +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/out") add_subdirectory(src) @@ -46,4 +49,3 @@ if(BUILD_TESTING) set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) endif() - @@ -1,6 +1,10 @@ +# THIS IS THE BRANCH USED FOR SOUTH CAROLINA TEST LAUNCH 02/24/2024 + # Active Drag System (ADS) This is the main codebase for Rocketry at Virginia Tech's Active Drag System, also known colloquially as the ADS, for the 2023-2024 competition year. It runs primarily on a BeagleBone Black, and its goal is to autonomously control the ADS' deployment during flight. +`Eigen` Library, `cmake` and `Google Test` required for successful build. + ## BUILD ```shell vagrant up @@ -60,4 +64,4 @@ GPIO1_12 = (1 × 32) + 12 = GPIO 44. ```shell /sys/class/gpio/gpio44 = GPIO1_12 -```
\ No newline at end of file +``` diff --git a/docs/Rocketry-ADS-UML.pdf b/docs/Rocketry-ADS-UML.pdf Binary files differnew file mode 100644 index 0000000..41b54f4 --- /dev/null +++ b/docs/Rocketry-ADS-UML.pdf diff --git a/docs/Rocketry-ADS-UML.png b/docs/Rocketry-ADS-UML.png Binary files differnew file mode 100644 index 0000000..1a491e5 --- /dev/null +++ b/docs/Rocketry-ADS-UML.png diff --git a/docs/TestLaunch2-Critial.txt b/docs/TestLaunch2-Critial.txt new file mode 100644 index 0000000..35f03ed --- /dev/null +++ b/docs/TestLaunch2-Critial.txt @@ -0,0 +1,255 @@ +1898.572 -2.313 120.00 0.22 -0.59 9.45 0.00 -0.00 0.01 8.31 1.81 3.44
+1898.984 -2.313 120.00 0.24 -0.60 9.43 0.00 -0.00 0.01 8.31 1.81 3.44
+1899.397 -2.188 120.00 0.23 -0.59 9.43 0.00 -0.00 -0.00 8.31 1.81 3.44
+1899.808 -2.313 120.00 0.24 -0.56 9.44 0.00 -0.00 0.01 8.31 1.81 3.44
+1900.221 -2.313 120.00 0.27 -0.58 9.48 0.00 0.00 -0.00 8.31 1.81 3.44
+1900.634 -2.188 120.00 0.23 -0.57 9.45 0.00 -0.00 0.00 8.31 1.81 3.44
+1901.047 -2.313 120.00 0.24 -0.57 9.43 -0.00 -0.00 -0.00 8.31 1.81 3.44
+1901.459 -2.438 120.00 0.21 -0.59 9.46 -0.00 0.00 0.00 8.31 1.81 3.44
+1901.872 -2.625 120.00 0.23 -0.60 9.46 -0.00 0.00 0.00 8.31 1.81 3.44
+1902.283 -2.563 120.00 0.22 -0.60 9.42 -0.00 -0.00 0.00 8.31 1.81 3.44
+1902.696 -2.750 120.00 0.23 -0.60 9.44 -0.00 -0.01 0.00 8.31 1.81 3.44
+1903.106 -2.188 120.00 0.25 -0.57 9.46 0.00 -0.00 -0.00 8.31 1.81 3.44
+1903.521 -2.188 120.00 0.23 -0.56 9.44 -0.00 0.00 -0.01 8.31 1.81 3.44
+1903.932 -2.313 120.00 0.22 -0.59 9.46 0.00 0.00 -0.00 8.31 1.81 3.44
+1904.345 -2.000 120.00 0.25 -0.60 9.43 0.00 -0.00 0.01 8.31 1.81 3.44
+1904.755 -2.000 120.00 0.26 -0.56 9.44 0.00 0.00 0.01 8.31 1.81 3.44
+1905.159 -1.875 120.00 0.22 -0.56 9.43 0.00 -0.00 -0.00 8.31 1.81 3.44
+1905.571 -2.125 120.00 0.24 -0.56 9.44 -0.00 0.00 0.00 8.31 1.81 3.44
+1905.984 -1.938 120.00 0.24 -0.57 9.41 0.00 0.00 0.00 8.31 1.81 3.44
+1906.395 -1.813 120.00 0.23 -0.57 9.44 -0.00 0.00 0.00 8.31 1.81 3.44
+1906.806 -2.125 120.00 0.23 -0.61 9.46 0.00 0.00 0.00 8.31 1.81 3.44
+1907.209 -1.938 120.00 0.22 -0.59 9.45 -0.00 -0.00 0.00 8.31 1.81 3.44
+1907.622 -2.188 120.00 0.24 -0.60 9.47 -0.00 0.00 0.01 8.31 1.81 3.44
+1908.034 -2.250 120.00 0.21 -0.56 9.43 0.00 -0.00 0.01 8.31 1.81 3.44
+1908.448 -2.000 120.00 0.23 -0.61 9.46 -0.00 0.00 -0.00 8.31 1.81 3.44
+1908.849 -1.875 120.00 0.24 -0.57 9.47 0.00 0.00 0.00 8.25 1.81 3.44
+1909.261 -1.750 120.00 0.24 -0.58 9.43 0.00 0.00 0.00 8.25 1.81 3.44
+1909.672 -2.000 120.00 0.21 -0.59 9.45 0.00 -0.00 0.00 8.25 1.81 3.44
+1910.085 -2.000 120.00 0.24 -0.59 9.45 -0.00 0.00 0.00 8.25 1.81 3.44
+1910.497 -1.938 120.00 0.23 -0.60 9.45 0.00 -0.00 0.01 8.25 1.81 3.44
+1910.912 -2.125 120.00 0.22 -0.58 9.46 0.00 -0.00 0.00 8.31 1.81 3.44
+1911.323 -2.063 120.00 0.22 -0.56 9.47 0.00 -0.00 0.01 8.31 1.81 3.44
+1911.736 -2.000 120.00 0.22 -0.54 9.43 -0.00 0.00 0.00 8.31 1.81 3.44
+1912.148 -1.813 120.00 0.21 -0.57 9.42 0.00 0.00 -0.01 8.38 1.81 3.44
+1912.550 -1.938 120.00 0.23 -0.59 9.46 0.00 0.00 -0.01 8.31 1.81 3.44
+1912.962 -2.188 120.00 0.24 -0.59 9.44 0.00 -0.00 -0.00 8.31 1.81 3.44
+1913.377 -1.813 120.00 0.21 -0.59 9.46 -0.00 -0.00 0.01 8.31 1.81 3.44
+1913.789 -1.938 120.00 0.27 -0.58 9.45 0.00 -0.00 0.00 8.31 1.81 3.44
+1914.203 -1.750 120.00 0.23 -0.62 9.44 -0.00 0.00 0.01 8.31 1.81 3.44
+1914.615 -2.000 120.00 0.21 -0.56 9.44 -0.00 -0.00 -0.00 8.31 1.81 3.44
+1915.029 -1.625 120.00 0.23 -0.56 9.49 0.00 -0.00 0.01 8.31 1.81 3.44
+1915.442 -1.875 120.00 0.24 -0.56 9.42 -0.00 -0.00 -0.00 8.25 1.81 3.44
+1915.853 -1.750 120.00 0.23 -0.58 9.46 0.00 0.00 -0.01 8.25 1.81 3.44
+1916.256 -1.750 120.00 0.25 -0.57 9.44 0.00 -0.00 -0.01 8.25 1.81 3.44
+1916.669 -2.000 120.00 0.24 -0.59 9.45 0.00 -0.00 0.00 8.25 1.81 3.44
+1917.081 -1.750 120.00 0.25 -0.57 9.45 -0.00 -0.00 0.00 8.25 1.81 3.44
+1917.495 -1.750 120.00 0.25 -0.56 9.42 -0.00 0.00 -0.00 8.25 1.81 3.44
+1917.907 -1.563 120.00 0.21 -0.61 9.47 -0.00 0.00 -0.00 8.25 1.81 3.44
+1918.320 -1.500 120.00 0.24 -0.56 9.46 0.00 0.00 0.01 8.31 1.81 3.44
+1918.732 -1.313 120.00 0.23 -0.57 9.45 0.00 -0.00 0.01 8.25 1.81 3.44
+1919.145 -1.313 120.00 0.26 -0.54 9.43 -0.00 -0.00 -0.01 8.25 1.81 3.44
+1919.556 -1.313 120.00 0.23 -0.58 9.44 0.00 0.00 -0.01 8.31 1.81 3.44
+1919.971 -1.438 120.00 0.22 -0.59 9.48 -0.00 0.00 -0.00 8.38 1.81 3.44
+1920.382 -1.313 120.00 0.23 -0.57 9.46 0.00 0.00 -0.00 8.38 1.81 3.44
+1920.794 -1.250 120.00 0.22 -0.56 9.44 0.00 -0.00 0.00 8.38 1.81 3.44
+1921.206 -1.125 120.00 0.22 -0.58 9.44 0.00 0.00 0.01 8.38 1.81 3.44
+1921.610 -1.188 120.00 0.25 -0.58 9.45 0.00 -0.00 -0.00 8.38 1.81 3.44
+1922.022 -1.313 120.00 0.25 -0.59 9.47 -0.00 0.00 -0.00 8.38 1.81 3.44
+1922.434 -0.938 120.00 0.22 -0.55 9.47 0.00 -0.00 0.00 8.38 1.81 3.44
+1922.846 -1.188 120.00 0.24 -0.55 9.46 0.00 -0.00 0.01 8.38 1.81 3.44
+1923.258 -1.438 120.00 0.26 -0.56 9.42 -0.00 0.00 0.00 8.38 1.81 3.44
+1923.672 -1.063 120.00 0.26 -0.60 9.44 -0.00 -0.00 0.01 8.38 1.81 3.44
+1924.084 -1.375 120.00 0.24 -0.56 9.43 -0.00 -0.00 -0.01 8.38 1.81 3.44
+1924.496 -1.063 120.00 0.23 -0.58 9.44 0.00 0.00 0.00 8.38 1.81 3.44
+1924.908 -1.250 120.00 0.20 -0.60 9.44 0.00 -0.00 -0.00 8.38 1.81 3.44
+1925.321 -1.438 120.00 0.22 -0.61 9.45 0.00 0.00 -0.01 8.38 1.81 3.44
+1925.735 -1.625 120.00 0.18 -0.56 9.41 -0.00 0.00 -0.00 8.38 1.81 3.44
+1926.147 -1.188 120.00 0.27 -0.58 9.44 0.00 -0.00 -0.00 8.38 1.81 3.44
+1926.559 -1.438 120.00 0.26 -0.58 9.46 0.00 0.00 0.00 8.38 1.81 3.44
+1926.972 -1.313 120.00 0.27 -0.59 9.46 -0.01 -0.00 0.00 8.44 1.88 3.44
+1927.385 -1.750 120.00 0.22 -0.58 9.43 0.00 0.00 -0.01 8.44 1.88 3.44
+1927.798 -1.438 120.00 0.23 -0.56 9.45 0.00 -0.00 0.01 8.44 1.88 3.44
+1928.211 -1.438 120.00 0.21 -0.57 9.48 -0.00 0.00 -0.01 8.44 1.88 3.44
+1928.622 -1.500 120.00 0.25 -0.52 9.42 0.00 0.00 -0.01 8.44 1.88 3.44
+1929.026 -2.000 120.00 0.26 -0.59 9.44 0.00 -0.00 -0.01 8.44 1.88 3.44
+1929.438 -1.813 120.00 0.22 -0.60 9.44 0.00 -0.00 -0.00 8.50 1.88 3.44
+1929.850 -1.563 120.00 0.23 -0.61 9.44 0.00 0.00 0.00 8.50 1.88 3.44
+1930.262 -2.000 120.00 0.24 -0.60 9.46 0.00 0.00 0.01 8.50 1.88 3.44
+1930.677 -1.875 120.00 0.21 -0.59 9.48 0.00 -0.00 0.00 8.50 1.88 3.44
+1931.089 -1.500 120.00 0.22 -0.60 9.46 0.00 0.00 -0.01 8.50 1.88 3.44
+1931.501 -1.688 120.00 0.23 -0.58 9.44 -0.00 0.00 -0.01 8.50 1.88 3.44
+1931.913 -1.750 120.00 0.22 -0.61 9.43 0.00 0.00 -0.00 8.50 1.88 3.44
+1932.325 -2.000 120.00 0.23 -0.63 9.45 -0.00 0.00 -0.00 8.50 1.88 3.44
+1932.740 -1.750 120.00 0.15 -1.07 9.04 0.00 0.00 0.01 8.50 1.88 3.44
+1933.152 -1.813 120.00 0.31 -0.64 9.48 0.00 -0.00 -0.00 8.50 1.88 3.44
+1933.564 -2.000 120.00 0.24 -0.56 9.45 0.00 -0.00 0.00 8.50 1.88 3.44
+1933.977 -1.875 120.00 0.20 -0.63 9.47 0.00 -0.00 0.00 8.50 1.88 3.44
+1934.390 -1.875 120.00 0.25 -0.58 9.45 -0.00 0.00 -0.01 8.50 1.88 3.44
+1934.801 -2.000 120.00 0.25 -0.56 9.48 0.00 -0.00 -0.00 8.50 1.88 3.44
+1935.213 -1.750 120.00 0.24 -0.54 9.45 -0.00 -0.00 -0.00 8.50 1.88 3.44
+1935.627 -1.688 120.00 0.25 -0.58 9.43 0.00 0.00 0.00 8.50 1.88 3.44
+1936.040 -1.750 120.00 0.26 -0.56 9.44 -0.00 0.00 0.00 8.50 1.88 3.44
+1936.450 -1.938 120.00 0.29 -0.57 9.44 -0.00 0.00 -0.00 8.50 1.88 3.44
+1936.862 -2.000 120.00 0.24 -0.56 9.45 0.00 -0.00 -0.00 8.50 1.88 3.44
+1937.275 -1.938 120.00 0.24 -0.56 9.45 0.00 0.00 -0.00 8.50 1.88 3.44
+1937.687 -1.875 120.00 0.24 -0.60 9.44 -0.00 -0.00 -0.00 8.50 1.88 3.44
+1938.089 -1.875 120.00 0.22 -0.56 9.46 0.00 -0.00 -0.00 8.50 1.88 3.44
+1938.500 -1.875 120.00 0.23 -0.59 9.47 -0.00 0.00 0.00 8.50 1.88 3.44
+1938.913 -2.188 120.00 0.22 -0.60 9.45 -0.00 0.00 -0.00 8.50 1.88 3.44
+1939.325 -2.125 120.00 0.30 -0.56 9.50 -0.00 0.00 -0.01 8.50 1.88 3.44
+1939.739 -2.063 120.00 0.32 -0.50 9.42 0.00 0.00 0.00 8.56 1.88 3.44
+1940.149 -1.813 120.00 0.23 -0.59 9.41 -0.00 -0.00 0.01 8.56 1.88 3.44
+1940.563 -1.875 120.00 0.26 -0.51 9.49 -0.00 -0.00 0.00 8.56 1.88 3.44
+1940.975 -2.063 120.00 0.29 -0.51 9.41 -0.00 0.00 -0.00 8.56 1.88 3.44
+1941.387 -2.188 120.00 0.31 -0.51 9.46 0.00 0.00 0.00 8.50 1.88 3.44
+1941.800 -2.188 120.00 0.30 -0.53 9.43 -0.00 -0.00 -0.00 8.44 1.88 3.44
+1942.213 -2.313 120.00 0.29 -0.60 9.49 -0.00 0.00 0.01 8.38 1.81 3.44
+1942.624 -1.875 120.00 0.22 -0.54 9.54 -0.00 -0.00 0.00 8.38 1.81 3.44
+1943.038 -2.000 120.00 0.22 -0.54 9.47 0.00 -0.00 0.01 8.38 1.81 3.44
+1943.450 -1.875 120.00 0.25 -0.62 9.36 -0.00 -0.00 0.01 8.38 1.81 3.44
+1943.864 -1.688 120.00 0.19 -0.67 9.38 -0.00 0.00 0.01 8.31 1.81 3.44
+1944.275 -2.000 120.00 0.21 -0.51 9.53 -0.00 0.00 0.00 8.31 1.81 3.44
+1944.688 -1.750 120.00 0.30 -0.51 9.52 0.00 0.00 0.00 8.31 1.81 3.44
+1945.099 -1.625 120.00 0.29 -0.57 9.49 0.00 -0.00 0.00 8.25 1.81 3.44
+1945.513 -1.500 120.00 0.30 -0.54 9.46 -0.00 -0.00 0.01 8.12 1.81 3.50
+1945.925 -1.438 120.00 0.36 -0.66 9.39 -0.00 -0.00 -0.00 8.12 1.81 3.50
+1946.338 -1.625 120.00 0.16 -0.62 9.28 0.00 0.00 -0.00 8.12 1.81 3.50
+1946.749 -1.500 120.00 0.31 -0.42 9.83 0.00 0.00 0.01 8.12 1.81 3.50
+1947.164 -1.563 120.00 0.32 -0.45 9.53 -0.00 0.00 0.02 8.00 1.81 3.50
+1947.575 -1.500 120.00 0.38 -0.51 9.52 0.00 -0.00 -0.00 8.00 1.75 3.50
+1947.988 -0.688 120.00 0.15 -0.51 9.87 0.00 -0.00 -0.01 8.00 1.75 3.50
+1948.398 2.188 120.00 -0.35 1.03 32.08 0.00 0.00 0.28 6.00 1.62 3.44
+1948.810 9.250 120.00 -1.58 -0.62 36.21 -0.02 0.06 -0.15 5.50 1.50 3.25
+1949.222 22.688 120.00 -0.43 -0.28 34.87 -0.26 0.35 -0.37 12.19 -3.19 6.75
+1949.635 41.063 120.00 1.98 1.71 35.88 -0.42 0.27 -0.76 26.56 -8.38 16.50
+1950.046 56.563 120.00 -2.13 8.33 35.49 -0.02 0.09 -4.24 72.75 7.69 24.94
+1950.457 78.439 120.00 -2.34 4.88 34.23 -0.60 0.44 -5.97 209.19 7.75 -13.38
+1950.871 105.127 120.00 -8.42 4.96 36.94 -0.47 0.29 -5.85 340.06 -6.44 9.81
+1951.284 136.565 120.00 -5.20 9.33 34.35 -0.57 0.27 -5.73 114.94 20.38 9.75
+1951.695 172.753 120.00 -1.75 10.43 33.26 -0.42 0.53 -6.07 251.06 -4.94 -11.06
+1952.106 211.753 120.00 -1.06 15.10 31.32 -0.15 0.78 -6.60 32.44 -3.88 22.25
+1952.519 254.566 120.00 -3.26 22.65 23.92 -0.13 0.90 -9.20 214.56 5.06 -11.00
+1952.931 303.130 120.00 -6.78 15.66 10.96 -0.48 0.59 -8.72 71.25 10.94 21.31
+1953.344 351.693 120.00 -5.43 9.68 -2.99 -0.33 0.41 -6.96 248.06 -3.12 -14.88
+1953.755 399.069 120.00 -2.44 8.77 -3.14 -0.18 0.42 -6.18 35.19 -2.81 22.19
+1954.167 443.569 120.00 -0.05 8.89 -2.72 -0.04 0.43 -6.25 174.75 15.75 -7.81
+1954.581 488.195 120.00 0.52 8.67 -2.33 -0.11 0.45 -6.74 327.50 -19.00 7.94
+1954.993 530.946 120.00 -1.44 6.65 -2.89 -0.19 0.40 -6.36 119.19 20.75 10.69
+1955.395 570.634 120.00 -2.64 6.27 -2.32 -0.16 0.35 -5.94 259.06 -8.31 -14.56
+1955.805 606.697 120.00 -2.31 5.05 -1.80 -0.26 0.31 -5.46 24.56 -6.12 23.25
+1956.220 641.635 120.00 -2.01 4.01 -1.48 -0.23 0.23 -5.15 146.38 25.00 -0.06
+1956.632 674.948 120.00 -1.95 3.69 -1.66 -0.21 0.25 -4.93 263.44 -10.25 -16.38
+1957.045 703.198 120.00 -2.31 2.91 -1.11 -0.28 0.22 -4.70 12.12 -10.88 22.19
+1957.455 730.824 120.00 -2.18 2.42 -1.33 -0.24 0.13 -4.46 117.06 26.44 12.25
+1957.870 757.324 120.00 -1.36 1.56 -0.97 -0.25 0.12 -4.19 216.44 8.38 -23.19
+1958.282 780.512 120.00 -2.56 1.17 -1.07 -0.19 0.17 -3.91 309.94 -21.88 -2.81
+1958.694 804.012 120.00 -1.82 0.88 -0.76 -0.37 0.12 -3.57 34.25 -3.56 29.06
+1959.104 822.825 120.00 -1.99 0.70 -0.67 -0.27 0.02 -3.39 114.44 31.50 16.44
+1959.516 841.075 120.00 -1.15 0.77 -0.61 -0.23 0.03 -3.16 190.62 24.06 -24.31
+1959.929 858.326 120.00 -2.02 0.71 -0.61 -0.09 0.09 -2.98 260.31 -11.00 -28.62
+1960.343 872.201 120.00 -1.81 -0.14 -0.44 -0.27 0.25 -2.87 326.75 -32.62 5.19
+1960.753 884.889 120.00 -1.23 0.00 -0.43 -0.43 0.04 -2.73 29.50 -8.56 41.00
+1961.164 895.576 120.00 -1.35 -0.45 -0.33 -0.33 -0.12 -2.51 89.69 33.44 40.31
+1961.577 904.076 120.00 -0.67 -0.46 -0.34 -0.18 -0.17 -2.28 146.19 55.25 -9.56
+1961.990 912.701 120.00 -0.69 -0.23 -0.40 -0.01 -0.14 -2.11 197.81 30.12 -52.50
+1962.400 918.639 120.00 -1.28 -0.37 -0.28 0.08 0.04 -2.02 245.25 -8.56 -59.50
+1962.811 922.077 120.00 -1.63 -0.74 -0.23 -0.09 0.31 -2.00 288.88 -48.56 -46.62
+1963.225 924.139 120.00 -1.52 -0.84 -0.11 -0.44 0.33 -2.29 332.44 -63.38 41.75
+1963.639 924.702 120.00 -1.06 -1.05 -0.07 -0.67 -0.03 -2.59 24.44 -13.19 83.00
+1964.048 925.014 120.00 -0.53 -1.58 -0.57 -0.35 -0.41 -2.52 79.31 43.94 100.12
+1964.459 924.827 120.00 2.55 -0.85 8.47 -0.11 0.18 -2.50 131.44 74.00 -162.62
+1964.870 929.827 120.00 1.39 4.57 0.42 -0.78 -0.12 -1.64 194.62 23.00 -94.25
+1965.285 923.827 120.00 3.42 6.27 -1.06 -0.99 1.16 2.85 196.62 23.31 -67.31
+1965.696 917.889 120.00 6.35 -7.18 1.43 1.06 -0.26 7.92 72.31 29.94 15.56
+1966.106 910.264 120.00 -0.28 4.39 -0.87 0.93 -1.75 9.52 252.19 -13.00 -7.50
+1966.518 904.514 120.00 -7.76 -0.71 -1.33 -0.81 2.15 11.66 19.62 -45.56 14.38
+1966.932 893.701 120.00 0.09 -8.09 0.43 0.39 -0.77 15.43 42.44 -31.88 55.56
+1967.344 882.201 120.00 -5.64 4.65 0.37 -0.05 -0.37 20.07 339.62 -31.88 -3.69
+1967.755 877.451 120.00 -0.55 10.84 -1.17 1.95 0.78 18.63 263.88 -4.94 -50.31
+1968.167 868.888 120.00 12.21 4.23 -0.51 0.63 0.37 23.15 163.88 79.88 -16.00
+1968.580 848.075 120.00 -7.08 9.55 1.30 -0.18 -1.29 26.35 318.62 -52.06 -27.31
+1968.993 835.013 120.00 -2.13 -10.67 9.63 -0.82 0.40 15.20 172.00 12.88 24.12
+1969.404 827.075 120.00 -10.37 1.39 7.11 -0.18 -0.02 7.19 300.31 -23.00 -24.19
+1969.816 820.075 120.00 7.46 -7.93 6.36 -0.14 -1.05 1.74 201.75 39.12 17.88
+1970.229 810.075 120.00 2.82 4.08 5.78 0.41 0.18 0.92 291.75 31.69 -22.94
+1970.643 806.262 120.00 -1.15 -10.63 4.44 -1.12 0.87 1.94 233.88 14.94 48.62
+1971.053 794.700 120.00 2.78 -4.33 4.60 0.94 -1.21 -1.39 233.31 27.81 36.31
+1971.463 786.950 120.00 8.94 -0.16 4.55 -1.20 0.09 -2.09 261.44 52.31 16.12
+1971.877 780.887 120.00 8.25 2.24 6.15 -0.04 -0.11 -3.71 334.12 42.38 -42.44
+1972.289 771.324 120.00 -7.32 2.43 4.58 -0.21 0.22 -6.07 89.62 -50.62 -29.50
+1972.701 761.199 120.00 3.29 -6.83 3.56 -0.01 0.27 -6.98 239.25 28.50 53.19
+1973.111 751.511 120.00 -4.56 5.45 3.29 0.26 -0.15 -10.20 98.25 -47.38 -36.06
+1973.524 741.449 120.00 3.18 8.63 4.09 0.12 -0.15 -13.64 7.88 21.00 -57.19
+1973.936 732.011 120.00 8.93 1.91 3.12 -0.21 -0.65 -15.94 341.75 47.38 -40.88
+1974.348 722.199 120.00 8.18 6.51 3.20 0.01 -0.15 -18.15 10.31 35.50 -48.12
+1974.757 713.761 120.00 -9.03 9.44 4.25 0.44 -0.09 -19.88 84.19 -34.06 -59.00
+1975.170 704.323 120.00 -3.27 -1.36 2.78 0.90 -0.32 -21.34 213.62 -19.06 51.69
+1975.583 696.448 120.00 10.41 9.02 3.38 0.56 -1.06 -23.49 7.69 43.50 -49.31
+1975.995 684.260 120.00 -3.97 1.48 5.19 0.07 -0.87 -24.27 188.31 -39.12 45.12
+1976.406 674.385 120.00 1.66 14.80 2.38 -0.05 -0.87 -25.11 34.25 23.62 -57.75
+1976.819 664.385 120.00 3.42 -3.06 5.06 -0.14 -0.87 -25.35 241.19 -0.88 56.19
+1977.231 654.197 120.00 -5.94 10.18 2.88 0.23 -1.10 -25.24 110.81 -46.62 -49.56
+1977.644 645.072 120.00 9.47 6.95 4.42 0.09 -0.32 -25.00 327.75 53.44 11.62
+1978.055 634.135 120.00 -10.57 0.18 5.11 -0.02 -0.61 -23.42 157.06 -66.38 15.88
+1978.467 624.510 120.00 7.92 9.02 2.69 0.71 -0.64 -23.82 332.12 49.94 -17.31
+1978.878 614.697 120.00 -6.97 1.07 3.16 0.89 -0.54 -22.72 141.75 -60.06 -9.56
+1979.287 605.822 120.00 9.39 4.43 4.10 0.45 -0.82 -22.84 311.81 53.50 22.44
+1979.698 594.884 120.00 -9.20 6.59 3.33 0.17 -0.71 -22.13 81.00 -30.12 -57.88
+1980.109 585.384 120.00 3.49 -1.80 4.67 -0.09 -0.97 -22.55 229.06 -2.75 51.94
+1980.521 574.884 120.00 -2.01 12.91 2.54 -0.26 -1.07 -23.27 32.69 11.81 -61.25
+1980.934 567.196 120.00 3.65 -2.22 4.18 -0.62 -1.12 -22.86 210.88 -14.44 49.31
+1981.346 553.321 120.00 -1.22 16.89 2.20 -0.87 -0.73 -23.20 0.94 33.31 -56.94
+1981.759 545.571 120.00 -1.35 -0.52 6.29 -0.92 -0.21 -20.54 144.12 -44.56 2.31
+1982.170 533.446 120.00 9.24 -1.67 4.11 0.33 0.69 -19.42 222.56 5.44 60.31
+1982.583 526.571 120.00 1.23 8.51 2.32 1.25 0.10 -19.95 334.81 41.50 -15.19
+1982.995 513.383 120.00 -8.37 2.31 0.64 0.54 -1.87 -19.60 64.50 -33.25 -51.31
+1983.397 504.320 120.00 0.97 -2.07 2.73 -0.14 -1.19 -19.96 175.81 -33.50 19.81
+1983.807 494.133 120.00 15.46 2.71 1.52 -0.38 -0.47 -19.99 226.69 29.06 41.50
+1984.220 481.882 120.00 0.51 7.48 3.54 -0.72 0.42 -19.83 327.19 38.44 -0.94
+1984.632 475.320 120.00 -10.24 13.98 6.53 2.01 0.31 -14.37 350.12 7.69 -37.12
+1985.046 459.257 120.00 2.23 7.06 2.71 -2.92 -0.67 -15.96 1.06 30.44 -25.50
+1985.458 454.382 120.00 4.39 12.62 3.16 1.61 -2.85 -7.55 274.19 19.88 -6.94
+1985.872 436.694 120.00 6.99 -2.31 0.74 -1.51 2.83 6.69 261.94 16.69 31.19
+1986.284 434.882 120.00 2.83 -4.76 2.77 -1.29 -1.46 2.44 166.69 -23.38 -4.31
+1986.697 420.069 120.00 -0.37 -16.55 3.27 1.66 1.20 -0.20 99.88 -27.81 20.62
+1987.109 409.069 120.00 -0.32 0.77 4.67 -0.12 -1.27 -0.51 96.81 -24.50 -28.12
+1987.521 397.319 120.00 19.83 -5.74 4.49 -1.02 0.36 -7.74 142.94 12.62 32.50
+1987.933 386.506 120.00 -0.15 1.49 2.54 -0.89 0.18 -7.86 313.56 37.75 -10.69
+1988.346 380.256 120.00 0.90 -16.31 6.25 -0.66 0.43 -3.49 69.81 -20.44 11.38
+1988.758 363.818 120.00 4.80 -0.93 3.44 -0.73 1.22 -7.32 179.88 -2.88 39.12
+1989.170 358.943 120.00 -15.73 0.80 5.63 0.73 1.15 -4.98 316.88 2.94 -29.00
+1989.582 340.943 120.00 -2.38 2.13 5.90 0.07 -1.49 2.15 316.62 23.75 -36.00
+1989.995 337.380 120.00 3.52 8.10 2.86 2.16 1.11 4.65 236.56 45.31 2.31
+1990.406 321.942 120.00 8.09 -0.80 2.16 0.70 2.08 2.65 120.75 -8.69 15.44
+1990.818 316.567 120.00 -2.51 -3.11 4.54 -0.96 1.05 2.84 47.62 -32.00 -29.62
+1991.231 303.942 120.00 -9.31 -8.24 3.56 -0.35 -0.70 1.48 347.06 -20.94 -15.81
+1991.643 292.504 120.00 -3.67 0.31 7.84 0.81 0.09 1.12 344.62 0.50 -37.00
+1992.053 285.942 120.00 -2.17 14.13 3.41 0.71 1.06 5.37 243.25 26.12 -29.31
+1992.466 273.379 120.00 5.81 1.26 7.42 -0.22 1.14 7.56 115.25 -16.88 24.12
+1992.878 265.317 120.00 -11.95 7.46 4.45 0.58 1.00 9.86 276.44 14.88 -54.75
+1993.291 255.566 120.00 -0.67 -2.90 5.64 0.62 0.05 13.14 7.62 -26.94 -31.19
+1993.702 245.191 120.00 -8.67 -8.81 4.57 -0.79 0.61 16.25 16.94 -62.12 -19.75
+1994.113 235.004 120.00 -4.25 4.18 3.41 0.51 -0.62 18.61 334.31 -1.81 -43.19
+1994.527 228.628 120.00 -5.47 14.60 2.89 1.19 1.72 18.12 276.50 21.56 -65.62
+1994.939 214.316 120.00 2.24 7.38 3.62 -2.31 1.04 20.07 204.44 45.56 33.56
+1995.351 207.628 120.00 9.44 -10.74 3.69 -0.81 -1.83 18.29 113.44 -14.62 82.25
+1995.762 194.628 120.00 2.19 -1.20 2.85 2.03 0.38 19.57 55.44 -55.19 -6.75
+1996.175 186.753 120.00 -16.19 3.75 2.35 -1.13 2.27 17.15 356.06 -43.00 -98.44
+1996.588 177.565 120.00 0.38 1.68 5.04 0.43 -2.28 17.63 312.06 24.19 -68.50
+1997.000 167.440 120.00 10.25 13.77 0.91 0.82 2.12 18.60 245.69 68.62 -144.81
+1997.411 156.940 120.00 3.83 2.44 2.09 -1.43 1.46 19.03 175.00 11.56 65.44
+1997.823 148.315 120.00 -4.36 -16.21 3.57 -1.01 0.02 17.34 114.31 -37.06 129.62
+1998.236 138.190 120.00 -2.48 3.32 4.78 0.92 0.10 19.86 61.81 -63.50 -78.75
+1998.648 129.127 120.00 11.40 12.20 3.03 0.78 1.06 23.28 291.12 47.44 -139.75
+1999.059 118.877 120.00 -2.21 6.21 3.70 1.54 0.73 25.85 82.00 -78.56 -90.38
+1999.469 108.752 120.00 6.12 -2.59 1.76 0.70 -0.31 28.28 205.12 11.38 116.06
+1999.884 100.064 120.00 7.30 3.30 2.89 -1.25 0.63 29.83 260.38 66.25 91.94
+2000.296 90.376 120.00 15.85 5.45 3.09 0.15 0.63 32.77 287.81 64.38 172.75
+2000.706 80.564 120.00 1.56 3.41 3.18 -0.63 1.75 34.70 250.12 42.88 96.75
+2001.116 69.251 120.00 -11.55 12.91 5.31 -0.29 1.14 -35.42 193.88 -37.38 102.94
+2001.531 59.251 120.00 12.33 11.40 3.34 -0.45 0.74 -35.42 87.69 -48.25 -94.12
+2001.943 49.938 120.00 -1.70 20.14 2.93 -0.10 0.33 -35.42 340.38 63.31 -96.81
+2002.355 40.063 120.00 -1.09 8.30 3.00 -0.33 1.00 -35.42 261.00 25.25 103.44
+2002.765 30.688 120.00 11.02 12.45 3.78 -0.85 1.18 -35.42 158.81 -60.81 134.25
+2003.177 20.063 120.00 15.61 19.47 2.56 -0.45 1.24 -35.42 58.75 -15.50 -117.50
\ No newline at end of file diff --git a/include/actuationPlan.hpp b/include/actuationPlan.hpp new file mode 100644 index 0000000..8d1ef26 --- /dev/null +++ b/include/actuationPlan.hpp @@ -0,0 +1,36 @@ +#pragma once +#include <algorithm> +#include <ctime> +#include "surfaceFitModel.hpp" +#include "rocketUtils.hpp" +#include "sensorIMU.hpp" +#include "sensorAltimeter.hpp" + +class ActuationPlan { + + private: + SurfaceFitModel sFitModel; + + public: + + /** + * @brief Construct a new Actuation Plan object + * + */ + ActuationPlan(); + + /** + * @brief Construct a new Actuation Plan object + * + * @param sFitModel + */ + ActuationPlan(SurfaceFitModel sFitModel); + + /** + * @brief Run the Fin Actuation Plan. + * Adjusts the fin angle values depending on the current vehicle state during the launch + * + * @param rocket Provides current rocket status and hold updated fin angle value. + */ + void runPlan(Vehicle& rocket); +};
\ No newline at end of file diff --git a/include/actuator.hpp b/include/actuator.hpp new file mode 100644 index 0000000..e7c4120 --- /dev/null +++ b/include/actuator.hpp @@ -0,0 +1,30 @@ +#pragma once + + +class Actuator { + + private: + + + public: + + /** + * @brief Initialize the actuator. + * + * @param data Data for initializing the actuator + * + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init(void* data) = 0; + + /** + * @brief Pass data to the actuator. + * + * @param data Data to sent to the actuator + * + * @return true Actuator write Success + * @return false Actuator write Failure + */ + virtual bool writeData(void* data) = 0; +};
\ No newline at end of file diff --git a/include/ads.hpp b/include/ads.hpp new file mode 100644 index 0000000..b16c4fb --- /dev/null +++ b/include/ads.hpp @@ -0,0 +1,66 @@ +#pragma once +#include <iostream> +#include <exception> +#include <stdexcept> +#include <chrono> +#include <thread> +#include "kalmanfilter.hpp" +#include "logger.hpp" +#include "actuationPlan.hpp" +#include "rocketUtils.hpp" +#include "sensorIMU.hpp" +#include "sensorAltimeter.hpp" +#include "motor.hpp" +#include "eigen3/Eigen/Dense" + +using namespace Eigen; + +class ADS { + + private: + + KalmanFilter kf; + ActuationPlan plan; + + IMUSensor imu; + AltimeterSensor altimeter; + Motor motor; + Vehicle rocket; + + /** + * @brief Logs a summary of all pertinent current rocket data + * (e.g. Altitude, Velocity, Acceleration) + */ + virtual void logSummary(); + + /** + * @brief Performs a routine to calculate the average altitude + * while the vehicle is waiting on the pad. + */ + virtual void updateOnPadAltitude(); + + /** + * @brief Update the vehicle with the current sensor (IMU & Altimeter) readings + */ + virtual void updateSensorData(); + + /** + * @brief Update the rocket state based on its current telemetry readings. + * Also Log pertinent telemetry and rocket state data + */ + virtual void updateRocketState(); + + public: + + /** + * @brief Construct a new ADS object + * + * @param plan The Actuation Plan for the Rocket + */ + ADS(ActuationPlan plan); + + /** + * @brief Run the full active drag system from launch to landing. + */ + virtual void run(); +};
\ No newline at end of file diff --git a/include/kalmanfilter.hpp b/include/kalmanfilter.hpp new file mode 100644 index 0000000..2c8398c --- /dev/null +++ b/include/kalmanfilter.hpp @@ -0,0 +1,98 @@ +#pragma once +#include "eigen3/Eigen/Dense" +#include <iostream> +#include <cmath> + +using namespace Eigen; + +class KalmanFilter { + + private: + + VectorXf state_vector; // x + MatrixXf state_covariance; // P + + MatrixXf state_transition_M; // F + MatrixXf control_input_M; // B + MatrixXf measurement_M; // H + + MatrixXf process_noise_covariance; // Q + MatrixXf measurement_covariance; // R + + MatrixXf I; // Identity + + int n; // State Vector Dimension + int p; // Control Vector Dimension + int m; // Measurement Vector Dimension + + double dt; // timestep + + /** + * @brief Initialize all necessary matrices. + * + */ + void matrixInit(); + + /** + * @brief Update any existing variable elements in your State Transition + * & Control Input matrices. + * + */ + void updateMatrices(); + + /** + * @brief Predict current State Vector & State Covariance + * given the current control input. + * + * @param control_vec The control input to be applied to the + * previous State Vector + */ + void prediction(VectorXf control_vec); + + /** + * @brief Correct the State Vector & State Covariance predictions + * given a related current measurement. + * + * @param measurement Current measurement + */ + void update(VectorXf measurement); + + public: + + KalmanFilter(); + + /** + * @brief Construct a new Kalman Filter object + * Set the sizes of the Filter's user inputs + * + * @param state_dim State Vector Dimension. i.e. dim(x) + * @param control_dim Control/Input Vector Dimension. i.e. dim(u) + * @param measurement_dim Measurement Vector Dimension. i.e. dim(z) + * @param dt timestep + */ + KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt); + + + /** + * @brief Optional function to set a custom initial state for the Filter. + * If not called, State Vector & State Covariance are zero-initialized + * + * @param state_vec Initial State Vector + * @param state_cov Initial State Covariance + * + * @return Whether state initialization was successful + */ + bool setInitialState(VectorXf state_vec, MatrixXf state_cov); + + /** + * @brief Perform Kalman Filter operation with given control input vector + * and measurement. + * + * @param control current control command + * @param measurement current measurement + * @param dt timestep + * + * @return Filtered state vector + */ + VectorXf run(VectorXf control, VectorXf measurement, double _dt); +};
\ No newline at end of file diff --git a/include/logger.hpp b/include/logger.hpp new file mode 100644 index 0000000..90fa05a --- /dev/null +++ b/include/logger.hpp @@ -0,0 +1,83 @@ +#pragma once +#include <iostream> +#include <fstream> +#include <ctime> + +class Logger { + + private: + std::fstream file; + std::string filename; + time_t t; + tm* now; + bool file_open; + + std::string infoTag = "-> [INFO]: "; + std::string errorTag = "-> [ERROR]: "; + std::string months[12] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sept", "Oct", "Nov", "Dec"}; + std::string days[7] = {"Sun", "Mon", "Tues", "Wed", "Thur", "Frid", "Sat"}; + + /** + * @brief Create formatted current-date tag + * + * @return string + */ + std::string getDate(); + + /** + * @brief Create formatted current-time tag + * + * @return string + */ + std::string getTime(); + + + public: + + /** + * @brief + * + */ + static Logger& Get(); + + /** + * @brief Open a Log File for writing + * + * @param _filename Name of file to open + * @return true Successful Open + * @return false Unsuccessful Open + */ + bool openLog(std::string _filename); + + /** + * @brief Close the Log File + * + */ + void closeLog(); + + /** + * @brief Write data to Log file + * + * @param data Data to log + * @return true Data Successfully Logged + * @return false Data Logging Unsuccessful + */ + bool log(std::string data); + + /** + * @brief Write error data to Log file + * + * @param data Error Data to log + * @return true Data Successfully Logged + * @return false Data Logging Unsuccessful + */ + bool logErr(std::string data); + + /** + * @brief Print Log Data to Terminal + * + * @return true Successful Print + * @return false Unsuccessful Print + */ + bool printLog(); +};
\ No newline at end of file diff --git a/include/math/imumath.h b/include/math/imumath.h new file mode 100644 index 0000000..831df60 --- /dev/null +++ b/include/math/imumath.h @@ -0,0 +1,30 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen <[email protected]> +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +#ifndef IMUMATH_H +#define IMUMATH_H + +#include "matrix.h" +#include "quaternion.h" +#include "vector.h" + +#endif
\ No newline at end of file diff --git a/include/math/matrix.h b/include/math/matrix.h new file mode 100644 index 0000000..e71b2db --- /dev/null +++ b/include/math/matrix.h @@ -0,0 +1,185 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen <[email protected]> +// Bug fixes and cleanups by Gé Vissers ([email protected]) +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +#ifndef IMUMATH_MATRIX_HPP +#define IMUMATH_MATRIX_HPP + +#include <stdint.h> +#include <string.h> + +#include "vector.h" + +namespace imu { + +template <uint8_t N> class Matrix { +public: + Matrix() { memset(_cell_data, 0, N * N * sizeof(double)); } + + Matrix(const Matrix &m) { + for (int ij = 0; ij < N * N; ++ij) { + _cell_data[ij] = m._cell_data[ij]; + } + } + + ~Matrix() {} + + Matrix &operator=(const Matrix &m) { + for (int ij = 0; ij < N * N; ++ij) { + _cell_data[ij] = m._cell_data[ij]; + } + return *this; + } + + Vector<N> row_to_vector(int i) const { + Vector<N> ret; + for (int j = 0; j < N; j++) { + ret[j] = cell(i, j); + } + return ret; + } + + Vector<N> col_to_vector(int j) const { + Vector<N> ret; + for (int i = 0; i < N; i++) { + ret[i] = cell(i, j); + } + return ret; + } + + void vector_to_row(const Vector<N> &v, int i) { + for (int j = 0; j < N; j++) { + cell(i, j) = v[j]; + } + } + + void vector_to_col(const Vector<N> &v, int j) { + for (int i = 0; i < N; i++) { + cell(i, j) = v[i]; + } + } + + double operator()(int i, int j) const { return cell(i, j); } + double &operator()(int i, int j) { return cell(i, j); } + + double cell(int i, int j) const { return _cell_data[i * N + j]; } + double &cell(int i, int j) { return _cell_data[i * N + j]; } + + Matrix operator+(const Matrix &m) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij]; + } + return ret; + } + + Matrix operator-(const Matrix &m) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij]; + } + return ret; + } + + Matrix operator*(double scalar) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] * scalar; + } + return ret; + } + + Matrix operator*(const Matrix &m) const { + Matrix ret; + for (int i = 0; i < N; i++) { + Vector<N> row = row_to_vector(i); + for (int j = 0; j < N; j++) { + ret(i, j) = row.dot(m.col_to_vector(j)); + } + } + return ret; + } + + Matrix transpose() const { + Matrix ret; + for (int i = 0; i < N; i++) { + for (int j = 0; j < N; j++) { + ret(j, i) = cell(i, j); + } + } + return ret; + } + + Matrix<N - 1> minor_matrix(int row, int col) const { + Matrix<N - 1> ret; + for (int i = 0, im = 0; i < N; i++) { + if (i == row) + continue; + + for (int j = 0, jm = 0; j < N; j++) { + if (j != col) { + ret(im, jm++) = cell(i, j); + } + } + im++; + } + return ret; + } + + double determinant() const { + // specialization for N == 1 given below this class + double det = 0.0, sign = 1.0; + for (int i = 0; i < N; ++i, sign = -sign) + det += sign * cell(0, i) * minor_matrix(0, i).determinant(); + return det; + } + + Matrix invert() const { + Matrix ret; + double det = determinant(); + + for (int i = 0; i < N; i++) { + for (int j = 0; j < N; j++) { + ret(i, j) = minor_matrix(j, i).determinant() / det; + if ((i + j) % 2 == 1) + ret(i, j) = -ret(i, j); + } + } + return ret; + } + + double trace() const { + double tr = 0.0; + for (int i = 0; i < N; ++i) + tr += cell(i, i); + return tr; + } + +private: + double _cell_data[N * N]; +}; + +template <> inline double Matrix<1>::determinant() const { return cell(0, 0); } + +}; // namespace imu + +#endif
\ No newline at end of file diff --git a/include/math/quaternion.h b/include/math/quaternion.h new file mode 100644 index 0000000..c5b907a --- /dev/null +++ b/include/math/quaternion.h @@ -0,0 +1,214 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen <[email protected]> +// Bug fixes and cleanups by Gé Vissers ([email protected]) +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +#ifndef IMUMATH_QUATERNION_HPP +#define IMUMATH_QUATERNION_HPP + +#include <math.h> +#include <stdint.h> +#include <stdlib.h> +#include <string.h> + +#include "matrix.h" + +namespace imu { + +class Quaternion { +public: + Quaternion() : _w(1.0), _x(0.0), _y(0.0), _z(0.0) {} + + Quaternion(double w, double x, double y, double z) + : _w(w), _x(x), _y(y), _z(z) {} + + Quaternion(double w, Vector<3> vec) + : _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {} + + double &w() { return _w; } + double &x() { return _x; } + double &y() { return _y; } + double &z() { return _z; } + + double w() const { return _w; } + double x() const { return _x; } + double y() const { return _y; } + double z() const { return _z; } + + double magnitude() const { + return sqrt(_w * _w + _x * _x + _y * _y + _z * _z); + } + + void normalize() { + double mag = magnitude(); + *this = this->scale(1 / mag); + } + + Quaternion conjugate() const { return Quaternion(_w, -_x, -_y, -_z); } + + void fromAxisAngle(const Vector<3> &axis, double theta) { + _w = cos(theta / 2); + // only need to calculate sine of half theta once + double sht = sin(theta / 2); + _x = axis.x() * sht; + _y = axis.y() * sht; + _z = axis.z() * sht; + } + + void fromMatrix(const Matrix<3> &m) { + double tr = m.trace(); + + double S; + if (tr > 0) { + S = sqrt(tr + 1.0) * 2; + _w = 0.25 * S; + _x = (m(2, 1) - m(1, 2)) / S; + _y = (m(0, 2) - m(2, 0)) / S; + _z = (m(1, 0) - m(0, 1)) / S; + } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) { + S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; + _w = (m(2, 1) - m(1, 2)) / S; + _x = 0.25 * S; + _y = (m(0, 1) + m(1, 0)) / S; + _z = (m(0, 2) + m(2, 0)) / S; + } else if (m(1, 1) > m(2, 2)) { + S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; + _w = (m(0, 2) - m(2, 0)) / S; + _x = (m(0, 1) + m(1, 0)) / S; + _y = 0.25 * S; + _z = (m(1, 2) + m(2, 1)) / S; + } else { + S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; + _w = (m(1, 0) - m(0, 1)) / S; + _x = (m(0, 2) + m(2, 0)) / S; + _y = (m(1, 2) + m(2, 1)) / S; + _z = 0.25 * S; + } + } + + void toAxisAngle(Vector<3> &axis, double &angle) const { + double sqw = sqrt(1 - _w * _w); + if (sqw == 0) // it's a singularity and divide by zero, avoid + return; + + angle = 2 * acos(_w); + axis.x() = _x / sqw; + axis.y() = _y / sqw; + axis.z() = _z / sqw; + } + + Matrix<3> toMatrix() const { + Matrix<3> ret; + ret.cell(0, 0) = 1 - 2 * _y * _y - 2 * _z * _z; + ret.cell(0, 1) = 2 * _x * _y - 2 * _w * _z; + ret.cell(0, 2) = 2 * _x * _z + 2 * _w * _y; + + ret.cell(1, 0) = 2 * _x * _y + 2 * _w * _z; + ret.cell(1, 1) = 1 - 2 * _x * _x - 2 * _z * _z; + ret.cell(1, 2) = 2 * _y * _z - 2 * _w * _x; + + ret.cell(2, 0) = 2 * _x * _z - 2 * _w * _y; + ret.cell(2, 1) = 2 * _y * _z + 2 * _w * _x; + ret.cell(2, 2) = 1 - 2 * _x * _x - 2 * _y * _y; + return ret; + } + + // Returns euler angles that represent the quaternion. Angles are + // returned in rotation order and right-handed about the specified + // axes: + // + // v[0] is applied 1st about z (ie, roll) + // v[1] is applied 2nd about y (ie, pitch) + // v[2] is applied 3rd about x (ie, yaw) + // + // Note that this means result.x() is not a rotation about x; + // similarly for result.z(). + // + Vector<3> toEuler() const { + Vector<3> ret; + double sqw = _w * _w; + double sqx = _x * _x; + double sqy = _y * _y; + double sqz = _z * _z; + + ret.x() = atan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw)); + ret.y() = asin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw)); + ret.z() = atan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw)); + + return ret; + } + + Vector<3> toAngularVelocity(double dt) const { + Vector<3> ret; + Quaternion one(1.0, 0.0, 0.0, 0.0); + Quaternion delta = one - *this; + Quaternion r = (delta / dt); + r = r * 2; + r = r * one; + + ret.x() = r.x(); + ret.y() = r.y(); + ret.z() = r.z(); + return ret; + } + + Vector<3> rotateVector(const Vector<2> &v) const { + return rotateVector(Vector<3>(v.x(), v.y())); + } + + Vector<3> rotateVector(const Vector<3> &v) const { + Vector<3> qv(_x, _y, _z); + Vector<3> t = qv.cross(v) * 2.0; + return v + t * _w + qv.cross(t); + } + + Quaternion operator*(const Quaternion &q) const { + return Quaternion(_w * q._w - _x * q._x - _y * q._y - _z * q._z, + _w * q._x + _x * q._w + _y * q._z - _z * q._y, + _w * q._y - _x * q._z + _y * q._w + _z * q._x, + _w * q._z + _x * q._y - _y * q._x + _z * q._w); + } + + Quaternion operator+(const Quaternion &q) const { + return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z); + } + + Quaternion operator-(const Quaternion &q) const { + return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z); + } + + Quaternion operator/(double scalar) const { + return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar); + } + + Quaternion operator*(double scalar) const { return scale(scalar); } + + Quaternion scale(double scalar) const { + return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar); + } + +private: + double _w, _x, _y, _z; +}; + +} // namespace imu + +#endif
\ No newline at end of file diff --git a/include/math/vector.h b/include/math/vector.h new file mode 100644 index 0000000..10345c3 --- /dev/null +++ b/include/math/vector.h @@ -0,0 +1,184 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen <[email protected]> +// Bug fixes and cleanups by Gé Vissers ([email protected]) +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +#ifndef IMUMATH_VECTOR_HPP +#define IMUMATH_VECTOR_HPP + +#include <math.h> +#include <stdint.h> +#include <string.h> + +namespace imu { + +template <uint8_t N> class Vector { +public: + Vector() { memset(p_vec, 0, sizeof(double) * N); } + + Vector(double a) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + } + + Vector(double a, double b) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + } + + Vector(double a, double b, double c) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + p_vec[2] = c; + } + + Vector(double a, double b, double c, double d) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + p_vec[2] = c; + p_vec[3] = d; + } + + Vector(const Vector<N> &v) { + for (int x = 0; x < N; x++) + p_vec[x] = v.p_vec[x]; + } + + ~Vector() {} + + uint8_t n() { return N; } + + double magnitude() const { + double res = 0; + for (int i = 0; i < N; i++) + res += p_vec[i] * p_vec[i]; + + return sqrt(res); + } + + void normalize() { + double mag = magnitude(); + if (isnan(mag) || mag == 0.0) + return; + + for (int i = 0; i < N; i++) + p_vec[i] /= mag; + } + + double dot(const Vector &v) const { + double ret = 0; + for (int i = 0; i < N; i++) + ret += p_vec[i] * v.p_vec[i]; + + return ret; + } + + // The cross product is only valid for vectors with 3 dimensions, + // with the exception of higher dimensional stuff that is beyond + // the intended scope of this library. + // Only a definition for N==3 is given below this class, using + // cross() with another value for N will result in a link error. + Vector cross(const Vector &v) const; + + Vector scale(double scalar) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] * scalar; + return ret; + } + + Vector invert() const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = -p_vec[i]; + return ret; + } + + Vector &operator=(const Vector &v) { + for (int x = 0; x < N; x++) + p_vec[x] = v.p_vec[x]; + return *this; + } + + double &operator[](int n) { return p_vec[n]; } + + double operator[](int n) const { return p_vec[n]; } + + double &operator()(int n) { return p_vec[n]; } + + double operator()(int n) const { return p_vec[n]; } + + Vector operator+(const Vector &v) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] + v.p_vec[i]; + return ret; + } + + Vector operator-(const Vector &v) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] - v.p_vec[i]; + return ret; + } + + Vector operator*(double scalar) const { return scale(scalar); } + + Vector operator/(double scalar) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] / scalar; + return ret; + } + + void toDegrees() { + for (int i = 0; i < N; i++) + p_vec[i] *= 57.2957795131; // 180/pi + } + + void toRadians() { + for (int i = 0; i < N; i++) + p_vec[i] *= 0.01745329251; // pi/180 + } + + double &x() { return p_vec[0]; } + double &y() { return p_vec[1]; } + double &z() { return p_vec[2]; } + double x() const { return p_vec[0]; } + double y() const { return p_vec[1]; } + double z() const { return p_vec[2]; } + +private: + double p_vec[N]; +}; + +template <> inline Vector<3> Vector<3>::cross(const Vector &v) const { + return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1], + p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2], + p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]); +} + +} // namespace imu + +#endif
\ No newline at end of file diff --git a/include/motor.hpp b/include/motor.hpp new file mode 100644 index 0000000..cb95b6a --- /dev/null +++ b/include/motor.hpp @@ -0,0 +1,36 @@ +#pragma once +#include "actuator.hpp" +#include "logger.hpp" +#include "rocketUtils.hpp" + +class Motor : public Actuator { + + private: + + + public: + + /** + * @brief Construct a new Motor object + * + */ + Motor(); + + /** + * @brief Initialize the motor. + * + * @param data Data for initializing the motor + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init(void* data) override; + + /** + * @brief Write data to the motor. + * + * @param data Data to be writen to the motor + * @return true Motor write Success + * @return false Motor write Failure + */ + virtual bool writeData(void* data) override; +};
\ No newline at end of file diff --git a/include/rocketUtils.hpp b/include/rocketUtils.hpp new file mode 100644 index 0000000..65b9cea --- /dev/null +++ b/include/rocketUtils.hpp @@ -0,0 +1,121 @@ +#pragma once +#include <iostream> +#include <vector> +#include <ctime> +#include <vector> +#include <iomanip> +#include <sstream> +#include <fstream> + +// Deployment angle limits +#define MAX_ANGLE 100 +#define MIN_ANGLE 120 + +// Altimeter initialization count limit +#define COUNT_LIMIT 50 + +// Constants +#define G_0 9.8066 + +// Threshold limits +#define BOOST_ACCEL_THRESH 3 +#define BOOST_HEIGHT_THRESH 20 +#define GLIDE_ACCEL_THRESH 0.5 + +#define ALTI_DEPL_THRESHOLD 0.5 +#define RATE_ALTI_DEPL 1/50 +#define FSM_DONE_SURFACE_ALTITUDE 200 +#define APOGEE_FSM_CHANGE 3 + +#define INIT_DEPLOYMENT 0 + +#define TIME_BO 8 +#define TIME_APO 25 +#define TIME_END 120 + +#define PAD_PRESSURE 102250 + +#define DUTY_MAX 14.5 +#define DUTY_MIN 3 + +#define LAUNCH_DATE "4-15-2023" +#define LOG_FILENAME "DataLog_" LAUNCH_DATE ".txt" + +#define LED_GAP_TIME 0.5 +#define LED_ONE_PATH "/sys/class/leds/beaglebone:green:usr1" +#define LED_BRIGHTNESS_FILE "brightness" +#define LED_FILENAME LED_ONE_PATH LED_BRIGHTNESS_FILE + +#define TEST_MODE false + + +enum VehicleState {ON_PAD, BOOST, GLIDE, APOGEE, DONE}; +extern std::string state_for_log[5]; + +struct Vehicle { + + int status; + + std::vector<double> acceleration; + std::vector<double> linear_acceleration; + + double apogee_altitude; + double previous_altitude; + double current_altitude; + double filtered_altitude; + + double filtered_velocity; + + double deployment_angle; + + bool imuInitFail; + bool imuReadFail; + bool altiInitFail; + bool altiReadFail; + + double ON_PAD_altitude; + bool ON_PAD_fail; + + double duty_span; + + double dt; + + int led_brightness; + + time_t start_time; + time_t fail_time; // For failure termination + time_t liftoff_time; + time_t relog_time; + time_t deploy_time; // NOT INITIALIZED YET + time_t loop_time; + time_t led_time; +}; + +/** + * @brief Convert fin deployment percentage to fin rotation angle + * + * @param percentage Fin deployment percentage + * @return double + */ +double deploy_percentage_to_angle(double percentage); + +/** + * @brief Set the decimal precision of the given data, and return it + * as a formatted string with a prefix containing a relevant description of the data. + * + * @param prefix Identifying or clarifying information about the loggef data + * @param data Data to Log + * @param precision The decimal precision value for the data + * + * @return A string with the formatted data. + */ +std::string format_data(std::string prefix, double data, int precision); + +/** + * @brief Blink Beaglebone LED 1 + * + * @param vehicle Holds settings pertinent to the Beaglebone LED + * @return true Successful Blink + * @return false Unsuccessful Blink + */ +bool led_out(Vehicle *vehicle); diff --git a/include/sensorAltimeter.hpp b/include/sensorAltimeter.hpp new file mode 100644 index 0000000..aa3a3cd --- /dev/null +++ b/include/sensorAltimeter.hpp @@ -0,0 +1,165 @@ +#pragma once +#include "sensorI2C.hpp" +#include <string> + +/* +Header file for driver for MPL3115a2 breakout, based heavily on https://github.com/adafruit/Adafruit_MPL3115A2_Library/tree/master +Based on register declarations found https://cdn-shop.adafruit.com/datasheets/1893_datasheet.pdf +Designed using subclass for I2C handler for Beaglebone Black +*/ + +class AltimeterSensor : public sensorI2C { + + + private: + double internalTemperature = 0; + double internalAltitude = 0; + + //ENUMS COPIED DIRECTLY FROM ADAFRUIT IMPLEMENTATION + /** MPL3115A2 registers **/ + enum { + MPL3115A2_REGISTER_STATUS = (0x00), + + MPL3115A2_REGISTER_PRESSURE_MSB = (0x01), + MPL3115A2_REGISTER_PRESSURE_CSB = (0x02), + MPL3115A2_REGISTER_PRESSURE_LSB = (0x03), + + MPL3115A2_REGISTER_TEMP_MSB = (0x04), + MPL3115A2_REGISTER_TEMP_LSB = (0x05), + + MPL3115A2_REGISTER_DR_STATUS = (0x06), + + MPL3115A2_OUT_P_DELTA_MSB = (0x07), + MPL3115A2_OUT_P_DELTA_CSB = (0x08), + MPL3115A2_OUT_P_DELTA_LSB = (0x09), + + MPL3115A2_OUT_T_DELTA_MSB = (0x0A), + MPL3115A2_OUT_T_DELTA_LSB = (0x0B), + + MPL3115A2_WHOAMI = (0x0C), + //This is hard-coded in the device from the factory + MPL3115A2_WHOAMI_EXPECTED = (0xC4), + + MPL3115A2_BAR_IN_MSB = (0x14), + MPL3115A2_BAR_IN_LSB = (0x15), + + MPL3115A2_OFF_H = (0x2D), + }; + + /** MPL3115A2 status register BITS **/ + enum { + MPL3115A2_REGISTER_STATUS_TDR = 0x02, + MPL3115A2_REGISTER_STATUS_PDR = 0x04, + MPL3115A2_REGISTER_STATUS_PTDR = 0x08, + }; + + /** MPL3115A2 PT DATA register BITS **/ + enum { + MPL3115A2_PT_DATA_CFG = 0x13, + MPL3115A2_PT_DATA_CFG_TDEFE = 0x01, + MPL3115A2_PT_DATA_CFG_PDEFE = 0x02, + MPL3115A2_PT_DATA_CFG_DREM = 0x04, + }; + + /** MPL3115A2 control registers **/ + enum { + MPL3115A2_CTRL_REG1 = (0x26), + MPL3115A2_CTRL_REG2 = (0x27), + MPL3115A2_CTRL_REG3 = (0x28), + MPL3115A2_CTRL_REG4 = (0x29), + MPL3115A2_CTRL_REG5 = (0x2A), + }; + + /** MPL3115A2 control register BITS **/ + enum { + MPL3115A2_CTRL_REG1_SBYB = 0x01, + MPL3115A2_CTRL_REG1_OST = 0x02, + MPL3115A2_CTRL_REG1_RST = 0x04, + MPL3115A2_CTRL_REG1_RAW = 0x40, + MPL3115A2_CTRL_REG1_ALT = 0x80, + MPL3115A2_CTRL_REG1_BAR = 0x00, + }; + + /** MPL3115A2 oversample values **/ + enum { + MPL3115A2_CTRL_REG1_OS1 = 0x00, + MPL3115A2_CTRL_REG1_OS2 = 0x08, + MPL3115A2_CTRL_REG1_OS4 = 0x10, + MPL3115A2_CTRL_REG1_OS8 = 0x18, + MPL3115A2_CTRL_REG1_OS16 = 0x20, + MPL3115A2_CTRL_REG1_OS32 = 0x28, + MPL3115A2_CTRL_REG1_OS64 = 0x30, + MPL3115A2_CTRL_REG1_OS128 = 0x38, + }; + + /** MPL3115A2 measurement modes **/ + typedef enum { + MPL3115A2_BAROMETER = 0, + MPL3115A2_ALTIMETER, + } mpl3115a2_mode_t; + + /** MPL3115A2 measurement types **/ + typedef enum { + MPL3115A2_PRESSURE, + MPL3115A2_ALTITUDE, + MPL3115A2_TEMPERATURE, + } mpl3115a2_meas_t; + + //This never actually gets used, and I can't find anything in the datasheet about it?? + #define MPL3115A2_REGISTER_STARTCONVERSION (0x12) ///< start conversion + + //Store current operating mode, sent to device during startup procedure + //This is why an enum is used rather than raw #define statements + mpl3115a2_mode_t currentMode; + + //Struct for storing ctrl register contents, copied from adafruit implementation + typedef union { + struct { + uint8_t SBYB : 1; + uint8_t OST : 1; + uint8_t RST : 1; + uint8_t OS : 3; + uint8_t RAW : 1; + uint8_t ALT : 1; + } bit; + uint8_t reg; + } CTRL_REG_1_STRUCT; + //Create instance of this register config to use during device startup and operation + CTRL_REG_1_STRUCT ctrl_reg1; + + public: + + /** + * @brief Construct a new Altimeter Sensor object + * + */ + AltimeterSensor(std::string I2C_FILE); + + /** + * @brief Initialize the Altimeter + * + * @param data Data for initializing the sensor + * @return true Initialization Success + * @return false Initialization Failure + */ + bool init() override; + + //Data getters and setters + // double getPressure(); + double getAltitude(); + double getTemperature(); + double setSeaLevelPressure(double pressure); + + //Data and mode handlers + //Use altimeter mode by default as this is what rocket logger wants + void setMode(mpl3115a2_mode_t mode = MPL3115A2_ALTIMETER); + void requestOneShotReading(); + bool isNewDataAvailable(); + void updateCurrentDataBuffer(); + + +}; + + + + diff --git a/include/sensorI2C.hpp b/include/sensorI2C.hpp new file mode 100644 index 0000000..bd497a5 --- /dev/null +++ b/include/sensorI2C.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <sys/ioctl.h> +#include <linux/i2c.h> +#include <linux/i2c-dev.h> +#include <cstdint> +#include <string> + +class sensorI2C { + + + //Predominantly used for I2C handler functions; implement high-level functions in sensor classes + //Implemented as a combination of Jazz' implementation and Derek Malloy's code + public: + + //Initial single byte write, used at beginning of read operation + //Returns 0 if successful, -1 if not + int initialWrite(unsigned char registerAddress) { + unsigned char *convertedAddressBuffer = new unsigned char[1]; + convertedAddressBuffer[0] = registerAddress; + //Expect 1 byte response from 1 byte write + if (write(i2c_bus, convertedAddressBuffer, 1) != 1) { + fprintf(stderr, "ERROR DOING INITIAL READ TRANSACTION WRITE TO REGISTER %x FOR DEVICE %x\n", registerAddress, deviceAddress); + return 0; + } + return 1; + } + + int writeRegister(unsigned char registerAddress, unsigned char value) { + //initialWrite() not used here because it's easier to just pack it into one buffer for file writing + unsigned char writeBuffer[2]; + writeBuffer[0] = registerAddress; + writeBuffer[1] = value; + + //Expect 2 byte output + if (write(i2c_bus, writeBuffer, 2) != 2) { + //These error messages are kind of obtuse but I'd rather have too much information than not enough + fprintf(stderr, "ERROR WRITING %x TO REGISTER %x ON DEVICE %x\n", value, registerAddress, deviceAddress); + return -1; + } + return 0; + } + + //Could probably be uint8_t but Derek Malloy does it with unsigned chars and that's what worked during testing so I don't want to touch it + unsigned char readSingleRegister(unsigned char registerAddress) { + printf("reg addr: %X\n", registerAddress); + + initialWrite(registerAddress); + unsigned char* readBuffer = new unsigned char[1]; + if (read(i2c_bus, readBuffer, 1) != 1){ + fprintf(stderr, "FAILED TO READ VALUE FROM REGISTER %x ON DEVICE %x\n", registerAddress, deviceAddress); + return -1; + } + printf("readBuffer: %X\n", readBuffer[0]); + return readBuffer[0]; + } + + unsigned char* readMultipleRegisters(unsigned char startingRegisterAddress, int numberOfRegisters) { + initialWrite(startingRegisterAddress); + unsigned char* readBuffer = new unsigned char[numberOfRegisters]; + if (read(i2c_bus, readBuffer, numberOfRegisters) != numberOfRegisters) { + fprintf(stderr, "ERROR TRYING TO READ %d REGISTERS STARTING AT ADDRESS %x ON DEVICE %x\n", + numberOfRegisters, startingRegisterAddress, deviceAddress); + } + return readBuffer; + } + + + //Intakes device address and file + //Private because IT'S ASSUMED PROGRAMMER WILL CALL THIS METHOD DURING INIT() ROUTINE + int setupI2C(std::string I2C_FILE) { + // Open i2c driver file + i2c_bus = open("/dev/i2c-2", O_RDWR); + if(i2c_bus < 0){ + fprintf(stderr, "FAILED TO OPEN I2C BUS USING FILE %s\n", "/dev/i2c-2"); + close(i2c_bus); + return -1; + } + + // Identify slave device address (MODIFIED FROM INITIAL IMPLEMENTATION, USES INTERNAL deviceAddress INSTEAD OF PARAMETER) + if(ioctl(i2c_bus, I2C_SLAVE, deviceAddress) < 0){ + fprintf(stderr, "FAILED TO CONNECT TO DEVICE AT ADDRESS %x VIA I2C\n", deviceAddress); + close(i2c_bus); + return -1; + } + return 0; + } + + /** + * @brief Initialize the sensor. + * + * @param data Data for initializing the sensor + * + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init() = 0; + + unsigned char deviceAddress; + int i2c_bus; + std::string I2C_FILE; +}; diff --git a/include/sensorIMU.hpp b/include/sensorIMU.hpp new file mode 100644 index 0000000..caddbe4 --- /dev/null +++ b/include/sensorIMU.hpp @@ -0,0 +1,312 @@ +#pragma once +#include <vector> +#include "sensorI2C.hpp" +#include "logger.hpp" +#include "rocketUtils.hpp" +#include "imumath.h" +#include <string> + +struct IMUData { + std::vector<double> acceleration[3]; + std::vector<double> linear_acceleration[3]; +}; + +//Register addresses and data structs copied from Adafruit implementation +/** BNO055 Address A **/ +#define BNO055_ADDRESS_A (0x28) +/** BNO055 Address B **/ +#define BNO055_ADDRESS_B (0x29) +/** BNO055 ID **/ +#define BNO055_ID (0xA0) + +/** Offsets registers **/ +#define NUM_BNO055_OFFSET_REGISTERS (22) + +/** A structure to represent offsets **/ +typedef struct { + int16_t accel_offset_x; /**< x acceleration offset */ + int16_t accel_offset_y; /**< y acceleration offset */ + int16_t accel_offset_z; /**< z acceleration offset */ + + int16_t mag_offset_x; /**< x magnetometer offset */ + int16_t mag_offset_y; /**< y magnetometer offset */ + int16_t mag_offset_z; /**< z magnetometer offset */ + + int16_t gyro_offset_x; /**< x gyroscrope offset */ + int16_t gyro_offset_y; /**< y gyroscrope offset */ + int16_t gyro_offset_z; /**< z gyroscrope offset */ + + int16_t accel_radius; /**< acceleration radius */ + + int16_t mag_radius; /**< magnetometer radius */ +} adafruit_bno055_offsets_t; + +/** Operation mode settings **/ +typedef enum { + OPERATION_MODE_CONFIG = 0X00, + OPERATION_MODE_ACCONLY = 0X01, + OPERATION_MODE_MAGONLY = 0X02, + OPERATION_MODE_GYRONLY = 0X03, + OPERATION_MODE_ACCMAG = 0X04, + OPERATION_MODE_ACCGYRO = 0X05, + OPERATION_MODE_MAGGYRO = 0X06, + OPERATION_MODE_AMG = 0X07, + OPERATION_MODE_IMUPLUS = 0X08, + OPERATION_MODE_COMPASS = 0X09, + OPERATION_MODE_M4G = 0X0A, + OPERATION_MODE_NDOF_FMC_OFF = 0X0B, + OPERATION_MODE_NDOF = 0X0C +} adafruit_bno055_opmode_t; +class IMUSensor : public sensorI2C { + + private: + deviceAddress = BNO055_ADDRESS_A; + + adafruit_bno055_opmode_t default_mode = OPERATION_MODE_NDOF; + /** BNO055 Registers **/ + typedef enum { + /* Page id register definition */ + BNO055_PAGE_ID_ADDR = 0X07, + + /* PAGE0 REGISTER DEFINITION START*/ + BNO055_CHIP_ID_ADDR = 0x00, + BNO055_ACCEL_REV_ID_ADDR = 0x01, + BNO055_MAG_REV_ID_ADDR = 0x02, + BNO055_GYRO_REV_ID_ADDR = 0x03, + BNO055_SW_REV_ID_LSB_ADDR = 0x04, + BNO055_SW_REV_ID_MSB_ADDR = 0x05, + BNO055_BL_REV_ID_ADDR = 0X06, + + /* Accel data register */ + BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08, + BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09, + BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A, + BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B, + BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C, + BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D, + + /* Mag data register */ + BNO055_MAG_DATA_X_LSB_ADDR = 0X0E, + BNO055_MAG_DATA_X_MSB_ADDR = 0X0F, + BNO055_MAG_DATA_Y_LSB_ADDR = 0X10, + BNO055_MAG_DATA_Y_MSB_ADDR = 0X11, + BNO055_MAG_DATA_Z_LSB_ADDR = 0X12, + BNO055_MAG_DATA_Z_MSB_ADDR = 0X13, + + /* Gyro data registers */ + BNO055_GYRO_DATA_X_LSB_ADDR = 0X14, + BNO055_GYRO_DATA_X_MSB_ADDR = 0X15, + BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16, + BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17, + BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18, + BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19, + + /* Euler data registers */ + BNO055_EULER_H_LSB_ADDR = 0X1A, + BNO055_EULER_H_MSB_ADDR = 0X1B, + BNO055_EULER_R_LSB_ADDR = 0X1C, + BNO055_EULER_R_MSB_ADDR = 0X1D, + BNO055_EULER_P_LSB_ADDR = 0X1E, + BNO055_EULER_P_MSB_ADDR = 0X1F, + + /* Quaternion data registers */ + BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20, + BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21, + BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22, + BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23, + BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24, + BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25, + BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26, + BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27, + + /* Linear acceleration data registers */ + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28, + BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29, + BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A, + BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B, + BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C, + BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D, + + /* Gravity data registers */ + BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E, + BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F, + BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30, + BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31, + BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32, + BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33, + + /* Temperature data register */ + BNO055_TEMP_ADDR = 0X34, + + /* Status registers */ + BNO055_CALIB_STAT_ADDR = 0X35, + BNO055_SELFTEST_RESULT_ADDR = 0X36, + BNO055_INTR_STAT_ADDR = 0X37, + + BNO055_SYS_CLK_STAT_ADDR = 0X38, + BNO055_SYS_STAT_ADDR = 0X39, + BNO055_SYS_ERR_ADDR = 0X3A, + + /* Unit selection register */ + BNO055_UNIT_SEL_ADDR = 0X3B, + + /* Mode registers */ + BNO055_OPR_MODE_ADDR = 0X3D, + BNO055_PWR_MODE_ADDR = 0X3E, + + BNO055_SYS_TRIGGER_ADDR = 0X3F, + BNO055_TEMP_SOURCE_ADDR = 0X40, + + /* Axis remap registers */ + BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, + BNO055_AXIS_MAP_SIGN_ADDR = 0X42, + + /* SIC registers */ + BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43, + BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44, + BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45, + BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46, + BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47, + BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48, + BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49, + BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A, + BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B, + BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C, + BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D, + BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E, + BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F, + BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50, + BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51, + BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52, + BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53, + BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54, + + /* Accelerometer Offset registers */ + ACCEL_OFFSET_X_LSB_ADDR = 0X55, + ACCEL_OFFSET_X_MSB_ADDR = 0X56, + ACCEL_OFFSET_Y_LSB_ADDR = 0X57, + ACCEL_OFFSET_Y_MSB_ADDR = 0X58, + ACCEL_OFFSET_Z_LSB_ADDR = 0X59, + ACCEL_OFFSET_Z_MSB_ADDR = 0X5A, + + /* Magnetometer Offset registers */ + MAG_OFFSET_X_LSB_ADDR = 0X5B, + MAG_OFFSET_X_MSB_ADDR = 0X5C, + MAG_OFFSET_Y_LSB_ADDR = 0X5D, + MAG_OFFSET_Y_MSB_ADDR = 0X5E, + MAG_OFFSET_Z_LSB_ADDR = 0X5F, + MAG_OFFSET_Z_MSB_ADDR = 0X60, + + /* Gyroscope Offset register s*/ + GYRO_OFFSET_X_LSB_ADDR = 0X61, + GYRO_OFFSET_X_MSB_ADDR = 0X62, + GYRO_OFFSET_Y_LSB_ADDR = 0X63, + GYRO_OFFSET_Y_MSB_ADDR = 0X64, + GYRO_OFFSET_Z_LSB_ADDR = 0X65, + GYRO_OFFSET_Z_MSB_ADDR = 0X66, + + /* Radius registers */ + ACCEL_RADIUS_LSB_ADDR = 0X67, + ACCEL_RADIUS_MSB_ADDR = 0X68, + MAG_RADIUS_LSB_ADDR = 0X69, + MAG_RADIUS_MSB_ADDR = 0X6A + } adafruit_bno055_reg_t; + + /** BNO055 power settings */ + typedef enum { + POWER_MODE_NORMAL = 0X00, + POWER_MODE_LOWPOWER = 0X01, + POWER_MODE_SUSPEND = 0X02 + } adafruit_bno055_powermode_t; + + /** Remap settings **/ + typedef enum { + REMAP_CONFIG_P0 = 0x21, + REMAP_CONFIG_P1 = 0x24, // default + REMAP_CONFIG_P2 = 0x24, + REMAP_CONFIG_P3 = 0x21, + REMAP_CONFIG_P4 = 0x24, + REMAP_CONFIG_P5 = 0x21, + REMAP_CONFIG_P6 = 0x21, + REMAP_CONFIG_P7 = 0x24 + } adafruit_bno055_axis_remap_config_t; + + /** Remap Signs **/ + typedef enum { + REMAP_SIGN_P0 = 0x04, + REMAP_SIGN_P1 = 0x00, // default + REMAP_SIGN_P2 = 0x06, + REMAP_SIGN_P3 = 0x02, + REMAP_SIGN_P4 = 0x03, + REMAP_SIGN_P5 = 0x01, + REMAP_SIGN_P6 = 0x07, + REMAP_SIGN_P7 = 0x05 + } adafruit_bno055_axis_remap_sign_t; + + /** A structure to represent revisions **/ + typedef struct { + uint8_t accel_rev; /**< acceleration rev */ + uint8_t mag_rev; /**< magnetometer rev */ + uint8_t gyro_rev; /**< gyroscrope rev */ + uint16_t sw_rev; /**< SW rev */ + uint8_t bl_rev; /**< bootloader rev */ + } adafruit_bno055_rev_info_t; + + /** Vector Mappings **/ + typedef enum { + VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, + VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, + VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, + VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, + VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, + VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR + } adafruit_vector_type_t; + + int32_t _sensorID; + adafruit_bno055_opmode_t currentMode; + + public: + /** + * @brief Construct a new IMUSensor object + * + */ + IMUSensor(std::string I2C_FILE); + + /** + * @brief Inititlize the IMU + * + * @param data Data for initializing the sensor + * @return true Initialization Success + * @return false Initialization Failure + */ + bool init() override; + + //Data handlers, copied from adafruit implementation (not all of them) + void setModeTemp(adafruit_bno055_opmode_t mode); + void setModeHard(adafruit_bno055_opmode_t mode); + adafruit_bno055_opmode_t getMode(); + + imu::Vector<3> getVector(adafruit_vector_type_t vector_type); + imu::Quaternion getQuat(); + int8_t getTemp(); + + //Configuration and status getters/setters + void setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode); + void setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign); + void getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, + uint8_t *system_error); + void getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, + uint8_t *mag); + + /* Functions to deal with raw calibration data */ + bool getSensorOffsets(uint8_t *calibData); + bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type); + void setSensorOffsets(const uint8_t *calibData); + void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type); + bool isFullyCalibrated(); + + /* Power managments functions */ + void enterSuspendMode(); + void enterNormalMode(); + +};
\ No newline at end of file diff --git a/include/surfaceFitModel.hpp b/include/surfaceFitModel.hpp new file mode 100644 index 0000000..303633f --- /dev/null +++ b/include/surfaceFitModel.hpp @@ -0,0 +1,32 @@ +#pragma once +#include <cmath> +#include "eigen3/Eigen/Dense" + + +using namespace Eigen; + +#define X_DEGREE 4 // Highest x-degree of current Surface Fit Model +#define Y_DEGREE 3 // Highest y-degree of current Surface Fit Model + +class SurfaceFitModel { + + private: + MatrixXd p; // Polynomial values + + public: + + /** + * @brief Construct a new Surface Fit Model object + * + */ + SurfaceFitModel(); + + /** + * @brief + * + * @param x + * @param y + * @return double + */ + double getFit(double x, double y); +}; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ea820ce..0a50318 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -12,3 +12,38 @@ add_executable(pru2 target_link_options(pru1 PRIVATE -static) target_link_options(pru2 PRIVATE -static) +cmake_minimum_required(VERSION 3.16.3) + +include_directories( ../include ) + +# Set Variables +set(TARGET ActiveDragSystem) +set(SOURCES active-drag-system.cpp ads.cpp actuationPlan.cpp surfaceFitModel.cpp rocketUtils.cpp sensorIMU.cpp sensorAltimeter.cpp motor.cpp logger.cpp kalmanfilter.cpp) + + +# Create Executables & Link Dependencies +add_executable(${TARGET} ${SOURCES}) + +add_executable(${TARGET_B} ${SOURCES_B}) +target_link_libraries(${TARGET_B} PUBLIC gtest_main) +add_test(NAME ${TARGET_B} COMMAND ${TARGET_B}) + +add_executable(${TARGET_C} ${SOURCES_C}) +target_link_libraries(${TARGET_C} PUBLIC gtest_main) +add_test(NAME ${TARGET_C} COMMAND ${TARGET_C}) + +add_executable(${TARGET_D} ${SOURCES_D}) +target_link_libraries(${TARGET_D} PUBLIC gtest_main) +add_test(NAME ${TARGET_D} COMMAND ${TARGET_D}) + +add_executable(${TARGET_E} ${SOURCES_E}) +target_link_libraries(${TARGET_E} PUBLIC gtest_main) +add_test(NAME ${TARGET_E} COMMAND ${TARGET_E}) + +add_executable(${TARGET_F} ${SOURCES_F}) +target_link_libraries(${TARGET_F} PUBLIC gtest_main) +add_test(NAME ${TARGET_F} COMMAND ${TARGET_F}) + +add_executable(${TARGET_G} ${SOURCES_G}) +target_link_libraries(${TARGET_G} PUBLIC gtest_main) +add_test(NAME ${TARGET_G} COMMAND ${TARGET_G}) diff --git a/src/active_drag_system.cpp b/src/active_drag_system.cpp index dce4f86..7b63faa 100644 --- a/src/active_drag_system.cpp +++ b/src/active_drag_system.cpp @@ -1,32 +1,14 @@ -/* - * Author: Dawsyn Schraiber <[email protected]> - * Date: 09/02/2023 - * - * MIT License - * - * Copyright (c) 2023 Rocketry at Virginia Tech - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - #include <iostream> +#include <cstdlib> +#include "../include/surfaceFitModel.hpp" +#include "../include/actuationPlan.hpp" +#include "../include/ads.hpp" -int main(int argc, char *argv[]) { - std::cout << "ADS" << std::endl; +int main() { + + SurfaceFitModel sfm = SurfaceFitModel(); + ActuationPlan plan = ActuationPlan(sfm); + ADS ads = ADS(plan); + ads.run(); + return EXIT_SUCCESS; } diff --git a/src/actuationPlan.cpp b/src/actuationPlan.cpp new file mode 100644 index 0000000..a987478 --- /dev/null +++ b/src/actuationPlan.cpp @@ -0,0 +1,60 @@ +#include "../include/actuationPlan.hpp" + +ActuationPlan::ActuationPlan() {} + +ActuationPlan::ActuationPlan(SurfaceFitModel sFitModel) : sFitModel(sFitModel) { + +} + + +void ActuationPlan::runPlan(Vehicle& rocket) { + + + if (rocket.imuReadFail || rocket.altiReadFail) { + rocket.deployment_angle = deploy_percentage_to_angle(0); // No fin deployment + } + + rocket.fail_time = time(nullptr); + + // 2024 Mission--------------------------------------------------------------------- + if (rocket.status == GLIDE) { + + // Fin deployment based on current drag coefficient value + try { + double cd = sFitModel.getFit(rocket.filtered_velocity, rocket.filtered_altitude); + cd = std::min(std::max(0.0, cd), 100.0); + rocket.deployment_angle = deploy_percentage_to_angle(cd); + } + + // Full deployment during coasting + catch (...) { + rocket.deployment_angle = deploy_percentage_to_angle(0); + + if ((time(nullptr) - rocket.deploy_time) > 2 && (time(nullptr) - rocket.deploy_time) < 7) { + rocket.deployment_angle = deploy_percentage_to_angle(100); + } + } + } + + else if (rocket.status == APOGEE) { + + rocket.deployment_angle = deploy_percentage_to_angle(50); + } + + else { + + rocket.deploy_time = time(nullptr); + } + // End 2024 Mission------------------------------------------------------------------ +} + + + + + + + + + + + diff --git a/src/ads.cpp b/src/ads.cpp new file mode 100644 index 0000000..5484970 --- /dev/null +++ b/src/ads.cpp @@ -0,0 +1,286 @@ +#include "../include/ads.hpp" + + +// Private---------------------------------------------------------------------- +void ADS::logSummary() { + + std::string output_string = "" + state_for_log[rocket.status]; + + if (!rocket.altiInitFail && !rocket.altiReadFail) { + + output_string += format_data(" ", rocket.filtered_altitude, 3); + } + + output_string += format_data(" ", rocket.deployment_angle, 2); + + if (!rocket.imuInitFail && !rocket.imuReadFail) { + + output_string += format_data(" ", rocket.acceleration[2], 2); + output_string += format_data(" ", rocket.filtered_velocity, 2); + } + + Logger::Get().log(output_string); +} + + +void ADS::updateOnPadAltitude() { + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + double avg_alt = 0; + double alt_read_count = 0; + + while (alt_read_count < COUNT_LIMIT) { + + altimeter.getData(&rocket.current_altitude); + alt_read_count++; + avg_alt = (avg_alt * (alt_read_count - 1) + rocket.current_altitude) / alt_read_count; + } + + Logger::Get().log(format_data("pad altitude initialization complete - ", avg_alt, 3)); + rocket.ON_PAD_altitude = avg_alt; +} + + +void ADS::updateSensorData() { + + if (!rocket.imuInitFail) { + + try { + imu.getData((void*)&rocket); + } + + catch (...) { + std::exception_ptr e = std::current_exception(); + Logger::Get().logErr(e.__cxa_exception_type()->name()); + rocket.imuReadFail = true; + } + } + + rocket.previous_altitude = rocket.current_altitude; // Why was this placed here???? + + if (!rocket.altiInitFail) { + + try { + altimeter.getData((void*)&rocket.current_altitude); + if (rocket.ON_PAD_fail) { + rocket.ON_PAD_altitude = rocket.current_altitude; + rocket.ON_PAD_fail = false; + } + + rocket.altiReadFail = false; + } + + catch (...) { + std::exception_ptr e = std::current_exception(); + Logger::Get().logErr(e.__cxa_exception_type()->name()); + rocket.altiReadFail = true; + } + } +} + + +void ADS::updateRocketState() { + + // Filter sensor data + VectorXf control_input(1); + VectorXf measurement(1); + control_input << rocket.acceleration[2]; + measurement << rocket.current_altitude; + VectorXf filtered = kf.run(control_input, measurement, rocket.dt); + rocket.filtered_altitude = filtered(0); + rocket.filtered_velocity = filtered(1); + + if (rocket.apogee_altitude < rocket.filtered_altitude) { + rocket.apogee_altitude = rocket.filtered_altitude; + } + + // (VEHICLE ON PAD) + if (rocket.status == ON_PAD) { + + // If launch detected + if (rocket.acceleration[2] >= BOOST_ACCEL_THRESH * G_0 + && rocket.filtered_altitude >= BOOST_HEIGHT_THRESH + rocket.ON_PAD_altitude) { + Logger::Get().log(format_data("LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3)); + } + + if (TEST_MODE && time(nullptr) - rocket.start_time >= 15) { + Logger::Get().log(format_data("TEST LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3)); + } + + if (time(nullptr) - rocket.relog_time > 2*60*60 + && rocket.status == ON_PAD) { + std::cout << "OverWR Success" << std::endl; + } + } + + // (VEHICLE BOOSTING) + else if (rocket.status == BOOST) { + + if (rocket.acceleration[2] <= GLIDE_ACCEL_THRESH * G_0 + || time(nullptr) - rocket.liftoff_time >= TIME_BO) { + rocket.status = GLIDE; + } + + } + + // (VEHICLE IN GLIDE) + else if (rocket.status == GLIDE) { + + if (rocket.filtered_altitude < rocket.apogee_altitude - APOGEE_FSM_CHANGE + || time(nullptr) - rocket.liftoff_time >= TIME_BO + TIME_APO) { + rocket.status = APOGEE; + Logger::Get().log(format_data("APO: ", (double)(rocket.apogee_altitude), 2)); + } + } + + // (VEHICLE AT APOGEE) + else if (rocket.status == APOGEE) { + + if (rocket.filtered_altitude <= FSM_DONE_SURFACE_ALTITUDE + rocket.ON_PAD_altitude) { + rocket.status = DONE; + return; + } + } +} + + +// Public---------------------------------------------------------------------- +ADS::ADS(ActuationPlan plan) : plan(plan) { + + rocket.status = ON_PAD; + + rocket.apogee_altitude = 0; + rocket.previous_altitude = 0; + rocket.current_altitude = 0; + rocket.filtered_altitude = 0; + + rocket.filtered_velocity = 0; + + rocket.duty_span = DUTY_MAX - DUTY_MIN; + rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT); + + rocket.dt = 0.1; + + rocket.imuInitFail = false; + rocket.imuReadFail = false; + rocket.altiInitFail = false; + rocket.altiReadFail = false; + + rocket.ON_PAD_altitude = 0; + rocket.ON_PAD_fail = false; + + rocket.start_time = time(nullptr); + rocket.fail_time = rocket.start_time; + rocket.relog_time = rocket.start_time; + rocket.led_time = rocket.start_time; + + imu = IMUSensor(); + altimeter = AltimeterSensor(); + motor = Motor(); + kf = KalmanFilter(2, 1, 1, rocket.dt); + + Logger::Get().openLog(LOG_FILENAME); + + motor.init(&rocket); + + imu.init(nullptr); + altimeter.init(nullptr); + + if (TEST_MODE) { + + Logger::Get().log("TEST Record Start --"); + } +} + + + +void ADS::run() { + + if (!rocket.altiInitFail) { + try { + updateOnPadAltitude(); + } + + catch (...) { + std::exception_ptr e = std::current_exception(); + Logger::Get().logErr(e.__cxa_exception_type()->name()); + rocket.ON_PAD_fail = true; + } + } + + rocket.loop_time = time(nullptr); + while (rocket.status != DONE) { + + updateSensorData(); + + if (!rocket.imuInitFail && !rocket.altiInitFail) { + + updateRocketState(); + + // Run the Actuation Plan---------------------------------- + plan.runPlan(rocket); + + if (rocket.imuReadFail || rocket.altiReadFail) { + + if (rocket.imuReadFail) { + imu.init(nullptr); // Restart + Logger::Get().log("Altimeter reset attempt"); + } + + if (rocket.altiReadFail) { + altimeter.init(nullptr); // Restart + Logger::Get().log("IMU reset attempt"); + } + } + } + + // Altimeter or IMU setup failed. Attempt to reinitialize + else { + + if (time(nullptr) - rocket.fail_time >= TIME_END) { + rocket.status = DONE; + } + + if (rocket.altiInitFail || rocket.altiReadFail) { + imu.init(nullptr); // Restart + Logger::Get().log("Altimeter reset attempt"); + } + + if (rocket.imuInitFail || rocket.imuReadFail) { + altimeter.init(nullptr); // Restart + Logger::Get().log("IMU reset attempt"); + } + + rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT); + } + + // Actuate Servos + motor.writeData(&rocket); + + logSummary(); + + // Blink Beaglebone LED 1 + if (time(nullptr) - rocket.led_time > LED_GAP_TIME) { + led_out(&rocket); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + rocket.dt = time(nullptr) - rocket.loop_time; + rocket.loop_time = time(nullptr); + } + + Logger::Get().closeLog(); + std::cout << "Done" << std::endl; +} + + + + + + + + + + + diff --git a/src/kalmanfilter.cpp b/src/kalmanfilter.cpp new file mode 100644 index 0000000..735e8c8 --- /dev/null +++ b/src/kalmanfilter.cpp @@ -0,0 +1,106 @@ +#include "../include/kalmanfilter.hpp" + +// Private---------------------------------------------------------------------- +void KalmanFilter::matrixInit() { + + state_vector.setZero(n); + state_covariance.setZero(n, n); + state_transition_M = MatrixXf::Zero(n, n); + control_input_M = MatrixXf::Zero(n, p); + I = MatrixXf::Identity(n, n); + measurement_M.setIdentity(m, n); // Setup Measurement Matrix + process_noise_covariance = MatrixXf::Zero(n, n); + measurement_covariance = MatrixXf::Zero(m, m); + + // Setup State Transition Matrix + state_transition_M << 1.0, dt, + 0.0, 1.0; + + // Setup Control Input Matrix + control_input_M << 0.5 * std::pow(dt, 2), // (Linear Displacement Eq.) + dt; + + // Setup Process-Noise Covariance + process_noise_covariance(0,0) = 0.01; + process_noise_covariance(1,1) = 0.1; + + // Setup Measurement Covariance + measurement_covariance << 0.1; +} + + +void KalmanFilter::updateMatrices() { + + state_transition_M(0, 1) = dt; + control_input_M(0, 0) = 0.5 * std::pow(dt, 2); + control_input_M(1, 0) = dt; +} + + +void KalmanFilter::prediction(VectorXf control_vec) { + + state_vector = (state_transition_M * state_vector) + (control_input_M * control_vec); + state_covariance = (state_transition_M * (state_covariance * state_transition_M.transpose())) + process_noise_covariance; +} + +void KalmanFilter::update(VectorXf measurement) { + + // Innovation + VectorXf y = measurement - (measurement_M * state_vector); + + // Residual/Innovation Covariance + MatrixXf S = (measurement_M * (state_covariance * measurement_M.transpose())) + measurement_covariance; + + // Kalman Gain + MatrixXf K = (state_covariance * measurement_M.transpose()) * S.inverse(); + + // Update + state_vector = state_vector + (K * y); + state_covariance = (I - (K * measurement_M)) * state_covariance; +} + + + +// Public---------------------------------------------------------------------- +KalmanFilter::KalmanFilter() { + +} + + +KalmanFilter::KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt) + : n(state_dim), p(control_dim), m(measurement_dim), dt(dt) { + + matrixInit(); +} + +bool KalmanFilter::setInitialState(VectorXf state_vec, MatrixXf state_cov) { + + if (state_vec.size() != n || state_cov.rows() != n) { + std::cout << "Error: Max State & Covariance Dimension should be " << n << std::endl; + return false; + } + + state_vector = state_vec; + state_covariance = state_cov; + return true; +} + + + + +VectorXf KalmanFilter::run(VectorXf control, VectorXf measurement, double _dt) { + + if (control.size() != p || measurement.size() != m) { + std::cout << "Error: Control Vector Size should be "<< p + << " Measurement Vector Size should be " << m << std::endl; + return state_vector; + } + + dt = _dt; + updateMatrices(); + + prediction(control); + update(measurement); + + return state_vector; +}
\ No newline at end of file diff --git a/src/logger.cpp b/src/logger.cpp new file mode 100644 index 0000000..a857be8 --- /dev/null +++ b/src/logger.cpp @@ -0,0 +1,132 @@ +#include "../include/logger.hpp" + +// Private---------------------------------------------------------------------- +std::string Logger::getDate() { + + t = time(nullptr); + now = localtime(&t); + return "(" + days[now->tm_wday] + " " + months[now->tm_mon] + " " + + std::to_string(now->tm_mday) + " " + std::to_string(now->tm_year + 1900) + ")"; +} + +std::string Logger::getTime() { + + t = time(nullptr); + now = localtime(&t); + std::string hour = std::to_string(now->tm_hour); + std::string min = std::to_string(now->tm_min); + std::string sec = std::to_string(now->tm_sec); + //string hour = "0" + to_string(now->tm_hour); + + if (now->tm_hour < 10) { + hour = "0" + std::to_string(now->tm_hour); + } + + if (now->tm_min < 10) { + min = "0" + std::to_string(now->tm_min); + } + + if (now->tm_sec < 10) { + sec = "0" + std::to_string(now->tm_sec); + } + + return hour + ":" + min + + ":" + sec; +} + + +// Public---------------------------------------------------------------------- +Logger& Logger::Get() { + + static Logger loggerSingleton; + return loggerSingleton; +} + +//Logger Logger::loggerSingleton; + + +bool Logger::openLog(std::string _filename) { + + filename = _filename; + + if (file_open) { + return false; + } + + file.open(filename, std::ios::in | std::ios::out | std::ios::app); + + if (!file) { + return false; + } + + file_open = true; + std::string date = getDate(); + std::string timestamp = getTime(); + file << timestamp << infoTag << "Log Start---- " << date << std::endl; + + return true; +} + + +void Logger::closeLog() { + + std::string timestamp = getTime(); + file << timestamp << infoTag << "Log End----\n\n"; + + file.close(); + file_open = false; +} + + +bool Logger::log(std::string data) { + + if (!file) { + return false; + } + + if (!file_open) { + return false; + } + std::string timestamp = getTime(); + file << timestamp << infoTag << data << std::endl; + return true; +} + +bool Logger::logErr(std::string data) { + + if (!file) { + return false; + } + + if (!file_open) { + return false; + } + + std::string timestamp = getTime(); + file << timestamp << errorTag << data << std::endl; + return true; +} + + +bool Logger::printLog() { + + if (file.is_open()) { + std::cout << "Log still open. Please close Log." << std::endl; + return false; + } + + file.open(filename, std::ios::in); + + if (!file.is_open()) { + return false; + } + + std::string line; + while(getline(file, line)) { + std::cout << line << std::endl; + } + + file.close(); + + return true; +} diff --git a/src/motor.cpp b/src/motor.cpp new file mode 100644 index 0000000..84785a9 --- /dev/null +++ b/src/motor.cpp @@ -0,0 +1,46 @@ +#include "../include/motor.hpp" + + + +Motor::Motor() { + + +} + +bool Motor::init(void* data) { + + Vehicle *vehicle = (Vehicle *) data; + double duty = 100 - ((MIN_ANGLE / 180) * vehicle->duty_span + DUTY_MIN); + + // Initialize stuff + // ..... + // ..... + + + data = (void*) vehicle; // Is this necessary? + return true; +} + + +bool Motor::writeData(void* data) { + + Vehicle *vehicle = (Vehicle *) data; + double duty = 100 - ((vehicle->deployment_angle / 180) * vehicle->duty_span + DUTY_MIN); + + // Send the Data somewhere + // ..... Pin + // ..... Duty + // ..... PWM frequency Hz + // ..... Polarity + + + if (1 == 2) { + Logger::Get().logErr("Some type of Error"); + return false; + } + + data = (void*) vehicle; // Is this necessary? + return true; +} + + diff --git a/src/pru/AM335x_PRU.cmd b/src/pru/AM335x_PRU.cmd new file mode 100644 index 0000000..b62f044 --- /dev/null +++ b/src/pru/AM335x_PRU.cmd @@ -0,0 +1,86 @@ +/****************************************************************************/ +/* AM335x_PRU.cmd */ +/* Copyright (c) 2015 Texas Instruments Incorporated */ +/* */ +/* Description: This file is a linker command file that can be used for */ +/* linking PRU programs built with the C compiler and */ +/* the resulting .out file on an AM335x device. */ +/****************************************************************************/ + +-cr /* Link using C conventions */ + +/* Specify the System Memory Map */ +MEMORY +{ + PAGE 0: + PRU_IMEM : org = 0x00000000 len = 0x00002000 /* 8kB PRU0 Instruction RAM */ + + PAGE 1: + + /* RAM */ + + PRU_DMEM_0_1 : org = 0x00000000 len = 0x00002000 CREGISTER=24 /* 8kB PRU Data RAM 0_1 */ + PRU_DMEM_1_0 : org = 0x00002000 len = 0x00002000 CREGISTER=25 /* 8kB PRU Data RAM 1_0 */ + + PAGE 2: + PRU_SHAREDMEM : org = 0x00010000 len = 0x00003000 CREGISTER=28 /* 12kB Shared RAM */ + + DDR : org = 0x80000000 len = 0x00000100 CREGISTER=31 + L3OCMC : org = 0x40000000 len = 0x00010000 CREGISTER=30 + + + /* Peripherals */ + + PRU_CFG : org = 0x00026000 len = 0x00000044 CREGISTER=4 + PRU_ECAP : org = 0x00030000 len = 0x00000060 CREGISTER=3 + PRU_IEP : org = 0x0002E000 len = 0x0000031C CREGISTER=26 + PRU_INTC : org = 0x00020000 len = 0x00001504 CREGISTER=0 + PRU_UART : org = 0x00028000 len = 0x00000038 CREGISTER=7 + + DCAN0 : org = 0x481CC000 len = 0x000001E8 CREGISTER=14 + DCAN1 : org = 0x481D0000 len = 0x000001E8 CREGISTER=15 + DMTIMER2 : org = 0x48040000 len = 0x0000005C CREGISTER=1 + PWMSS0 : org = 0x48300000 len = 0x000002C4 CREGISTER=18 + PWMSS1 : org = 0x48302000 len = 0x000002C4 CREGISTER=19 + PWMSS2 : org = 0x48304000 len = 0x000002C4 CREGISTER=20 + GEMAC : org = 0x4A100000 len = 0x0000128C CREGISTER=9 + I2C1 : org = 0x4802A000 len = 0x000000D8 CREGISTER=2 + I2C2 : org = 0x4819C000 len = 0x000000D8 CREGISTER=17 + MBX0 : org = 0x480C8000 len = 0x00000140 CREGISTER=22 + MCASP0_DMA : org = 0x46000000 len = 0x00000100 CREGISTER=8 + MCSPI0 : org = 0x48030000 len = 0x000001A4 CREGISTER=6 + MCSPI1 : org = 0x481A0000 len = 0x000001A4 CREGISTER=16 + MMCHS0 : org = 0x48060000 len = 0x00000300 CREGISTER=5 + SPINLOCK : org = 0x480CA000 len = 0x00000880 CREGISTER=23 + TPCC : org = 0x49000000 len = 0x00001098 CREGISTER=29 + UART1 : org = 0x48022000 len = 0x00000088 CREGISTER=11 + UART2 : org = 0x48024000 len = 0x00000088 CREGISTER=12 + + RSVD10 : org = 0x48318000 len = 0x00000100 CREGISTER=10 + RSVD13 : org = 0x48310000 len = 0x00000100 CREGISTER=13 + RSVD21 : org = 0x00032400 len = 0x00000100 CREGISTER=21 + RSVD27 : org = 0x00032000 len = 0x00000100 CREGISTER=27 + +} + +/* Specify the sections allocation into memory */ +SECTIONS { + /* Forces _c_int00 to the start of PRU IRAM. Not necessary when loading + an ELF file, but useful when loading a binary */ + .text:_c_int00* > 0x0, PAGE 0 + + .text > PRU_IMEM, PAGE 0 + .stack > PRU_DMEM_0_1, PAGE 1 + .bss > PRU_DMEM_0_1, PAGE 1 + .cio > PRU_DMEM_0_1, PAGE 1 + .data > PRU_DMEM_0_1, PAGE 1 + .switch > PRU_DMEM_0_1, PAGE 1 + .sysmem > PRU_DMEM_0_1, PAGE 1 + .cinit > PRU_DMEM_0_1, PAGE 1 + .rodata > PRU_DMEM_0_1, PAGE 1 + .rofardata > PRU_DMEM_0_1, PAGE 1 + .farbss > PRU_DMEM_0_1, PAGE 1 + .fardata > PRU_DMEM_0_1, PAGE 1 + + .resource_table > PRU_DMEM_0_1, PAGE 1 +} diff --git a/src/pru/intc_map.h b/src/pru/intc_map.h new file mode 100644 index 0000000..eebf27b --- /dev/null +++ b/src/pru/intc_map.h @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2021 Texas Instruments Incorporated - http://www.ti.com/ + * + * + * 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 Texas Instruments Incorporated nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _INTC_MAP_0_H_ +#define _INTC_MAP_0_H_ + +/* + * ======== PRU INTC Map ======== + * + * Define the INTC mapping for interrupts going to the ICSS / ICSSG: + * ICSS Host interrupts 0, 1 + * ICSSG Host interrupts 0, 1, 10-19 + * + * Note that INTC interrupts going to the ARM Linux host should not be defined + * in this file (ICSS/ICSSG Host interrupts 2-9). + * + * The INTC configuration for interrupts going to the ARM host should be defined + * in the device tree node of the client driver, "interrupts" property. + * See Documentation/devicetree/bindings/interrupt-controller/ti,pruss-intc.yaml + * entry #interrupt-cells for more. + * + * For example, on ICSSG: + * + * &client_driver0 { + * interrupt-parent = <&icssg0_intc>; + * interrupts = <21 2 2>, <22 3 3>; + * interrupt-names = "interrupt_name1", "interrupt_name2"; + * }; + * + */ + +#include <stddef.h> +#include <rsc_types.h> + +/* + * .pru_irq_map is used by the RemoteProc driver during initialization. However, + * the map is NOT used by the PRU firmware. That means DATA_SECTION and RETAIN + * are required to prevent the PRU compiler from optimizing out .pru_irq_map. + */ +#pragma DATA_SECTION(my_irq_rsc, ".pru_irq_map") +#pragma RETAIN(my_irq_rsc) + +struct pru_irq_rsc my_irq_rsc = { + 0, /* type = 0 */ + 1, /* number of system events being mapped */ + { + {7, 1, 1}, /* {sysevt, channel, host interrupt} */ + }, +}; + +#endif /* _INTC_MAP_0_H_ */ diff --git a/src/pru/pru_pwm.c b/src/pru/pru_pwm.c new file mode 100644 index 0000000..796040b --- /dev/null +++ b/src/pru/pru_pwm.c @@ -0,0 +1,102 @@ +#include <stdint.h> +#include <pru_cfg.h> +#include <pru_intc.h> +#include <pru_iep.h> +#include "intc_map.h" +#include "resource_table_empty.h" + +#define CYCLES_PER_SECOND 200000000 +#define CYCLES_PER_PERIOD (CYCLES_PER_SECOND / 800) +#define DEFAULT_DUTY_CYCLE 0 +#define P8_12 (1 << 14) +#define P8_12_n ~(1 << 14) +#define PRU0_DRAM 0x00000 // Offset to DRAM +#define PRU_TIMER_PASSCODE 0x31138423 +#define BURN_TIME 2 // seconds for motor burn + +volatile uint32_t *pru0_dram = (uint32_t *) (PRU0_DRAM + 0x200); + + +volatile register uint32_t __R30; +volatile register uint32_t __R31; + + +void main(void) { + uint32_t duty_cycle = DEFAULT_DUTY_CYCLE; + uint32_t off_cycles = ((100 - duty_cycle) * CYCLES_PER_PERIOD / 100); + uint32_t on_cycles = ((100 - (100 - duty_cycle)) * CYCLES_PER_PERIOD / 100); + uint32_t off_count = off_cycles; + uint32_t on_count = on_cycles; + *pru0_dram = duty_cycle; + + /* Clear SYSCFG[STANDBY_INIT] to enable OCP master port */ + CT_CFG.SYSCFG_bit.STANDBY_INIT = 0; + + while (1) { + if (pru0_dram[1] == PRU_TIMER_PASSCODE) { + /* Disable counter */ + CT_IEP.TMR_GLB_CFG_bit.CNT_EN = 0; + + /* Reset Count register */ + CT_IEP.TMR_CNT = 0x0; + + /* Clear overflow status register */ + CT_IEP.TMR_GLB_STS_bit.CNT_OVF = 0x1; + + /* Set compare value */ + CT_IEP.TMR_CMP0 = (BURN_TIME * CYCLES_PER_SECOND); // 10 seconds @ 200MHz + + /* Clear compare status */ + CT_IEP.TMR_CMP_STS_bit.CMP_HIT = 0xFF; + + /* Disable compensation */ + CT_IEP.TMR_COMPEN_bit.COMPEN_CNT = 0x0; + + /* Enable CMP0 and reset on event */ + CT_IEP.TMR_CMP_CFG_bit.CMP0_RST_CNT_EN = 0x1; + CT_IEP.TMR_CMP_CFG_bit.CMP_EN = 0x1; + + /* Clear the status of all interrupts */ + CT_INTC.SECR0 = 0xFFFFFFFF; + CT_INTC.SECR1 = 0xFFFFFFFF; + + /* Enable counter */ + CT_IEP.TMR_GLB_CFG = 0x11; + break; + } + } + + /* Poll until R31.31 is set */ + do { + while ((__R31 & 0x80000000) == 0) { + } + /* Verify that the IEP is the source of the interrupt */ + } while ((CT_INTC.SECR0 & (1 << 7)) == 0); + + /* Disable counter */ + CT_IEP.TMR_GLB_CFG_bit.CNT_EN = 0x0; + + /* Disable Compare0 */ + CT_IEP.TMR_CMP_CFG = 0x0; + + /* Clear Compare status */ + CT_IEP.TMR_CMP_STS = 0xFF; + + /* Clear the status of the interrupt */ + CT_INTC.SECR0 = (1 << 7); + + while (1) { + if (on_count) { + __R30 |= P8_12; // Set the GPIO pin to 1 + on_count--; + __delay_cycles(3); + } else if (off_count) { + __R30 &= P8_12_n; // Clear the GPIO pin + off_count--; + } else { + duty_cycle = *pru0_dram; + off_count = ((100 - duty_cycle) * CYCLES_PER_PERIOD / 100); + on_count = ((100 - (100 - duty_cycle)) * CYCLES_PER_PERIOD / 100); + } + } +} diff --git a/src/pru/pwm_test.c b/src/pru/pwm_test.c new file mode 100644 index 0000000..df8bbf0 --- /dev/null +++ b/src/pru/pwm_test.c @@ -0,0 +1,80 @@ +/* + * + * pwm tester + * The on cycle and off cycles are stored in each PRU's Data memory + * + */ + +#include <stdio.h> +#include <stdint.h> +#include <fcntl.h> +#include <sys/mman.h> + +#define PRU_ADDR 0x4A300000 // Start of PRU memory Page 184 am335x TRM +#define PRU_LEN 0x80000 // Length of PRU memory +#define PRU0_DRAM 0x00000 // Offset to DRAM +#define PRU1_DRAM 0x02000 +#define PRU_SHAREDMEM 0x10000 // Offset to shared memory + +#define CYCLES_PER_SECOND 200000000 +#define CYCLES_PER_PERIOD (CYCLES_PER_SECOND / 800) + +uint32_t *pru0DRAM_32int_ptr; // Points to the start of local DRAM +uint32_t *pru1DRAM_32int_ptr; // Points to the start of local DRAM +uint32_t *prusharedMem_32int_ptr; // Points to the start of the shared memory + +/******************************************************************************* +* int start_pwm_count(int ch, int countOn, int countOff) +* +* Starts a pwm pulse on for countOn and off for countOff to a single channel (ch) +*******************************************************************************/ +int start_pwm_count(uint32_t duty_cycle) { + uint32_t *pruDRAM_32int_ptr = pru0DRAM_32int_ptr; + uint32_t old_duty_cycle = pruDRAM_32int_ptr[0]; + uint32_t old_count_on = (100 - (100 - old_duty_cycle)) * CYCLES_PER_PERIOD / 100; + uint32_t old_count_off = (100 - old_duty_cycle) * CYCLES_PER_PERIOD / 100; + uint32_t new_count_on = (100 - (100 - duty_cycle)) * CYCLES_PER_PERIOD / 100; + uint32_t new_count_off = (100 - duty_cycle) * CYCLES_PER_PERIOD / 100; + + printf("old:\n\tcountOn: %d, countOff: %d, count: %d\nnew:\n\tcountOn: %d, countOff: %d, count: %d\n", old_count_on, old_count_off, old_count_off + old_count_on, new_count_on, new_count_off, new_count_off + new_count_on); + // write to PRU shared memory + pruDRAM_32int_ptr[0] = duty_cycle; // On time + return 0; +} + +int main(int argc, char *argv[]) +{ + uint32_t *pru; // Points to start of PRU memory. + int fd; + printf("Servo tester\n"); + + fd = open ("/dev/mem", O_RDWR | O_SYNC); + if (fd == -1) { + printf ("ERROR: could not open /dev/mem.\n\n"); + return 1; + } + pru = mmap (0, PRU_LEN, PROT_READ | PROT_WRITE, MAP_SHARED, fd, PRU_ADDR); + if (pru == MAP_FAILED) { + printf ("ERROR: could not map memory.\n\n"); + return 1; + } + close(fd); + printf ("Using /dev/mem.\n"); + + pru0DRAM_32int_ptr = pru + PRU0_DRAM/4 + 0x200/4; // Points to 0x200 of PRU0 memory + pru1DRAM_32int_ptr = pru + PRU1_DRAM/4 + 0x200/4; // Points to 0x200 of PRU1 memory + prusharedMem_32int_ptr = pru + PRU_SHAREDMEM/4; // Points to start of shared memory + + uint32_t desired_duty_cycle = 0; + while (1) { + printf("Enter a duty cycle: "); + scanf("%d", &desired_duty_cycle); + start_pwm_count(desired_duty_cycle); + } + + if(munmap(pru, PRU_LEN)) { + printf("munmap failed\n"); + } else { + printf("munmap succeeded\n"); + } +} diff --git a/src/pru/resource_table_empty.h b/src/pru/resource_table_empty.h new file mode 100644 index 0000000..8e7743e --- /dev/null +++ b/src/pru/resource_table_empty.h @@ -0,0 +1,38 @@ +/* + * ======== resource_table_empty.h ======== + * + * Define the resource table entries for all PRU cores. This will be + * incorporated into corresponding base images, and used by the remoteproc + * on the host-side to allocated/reserve resources. Note the remoteproc + * driver requires that all PRU firmware be built with a resource table. + * + * This file contains an empty resource table. It can be used either as: + * + * 1) A template, or + * 2) As-is if a PRU application does not need to configure PRU_INTC + * or interact with the rpmsg driver + * + */ + +#ifndef _RSC_TABLE_PRU_H_ +#define _RSC_TABLE_PRU_H_ + +#include <stddef.h> +#include <rsc_types.h> + +struct my_resource_table { + struct resource_table base; + + uint32_t offset[1]; /* Should match 'num' in actual definition */ +}; + +#pragma DATA_SECTION(pru_remoteproc_ResourceTable, ".resource_table") +#pragma RETAIN(pru_remoteproc_ResourceTable) +struct my_resource_table pru_remoteproc_ResourceTable = { + 1, /* we're the first version that implements this */ + 0, /* number of entries in the table */ + 0, 0, /* reserved, must be zero */ + 0, /* offset[0] */ +}; + +#endif /* _RSC_TABLE_PRU_H_ */ diff --git a/src/pru1/sensors.cpp b/src/pru1/sensors.cpp deleted file mode 100644 index 508387e..0000000 --- a/src/pru1/sensors.cpp +++ /dev/null @@ -1,12 +0,0 @@ -/* - * Author: Gregory Wainer <[email protected]> - * Date: 10/14/2023 - * - */ - -#include <iostream> -#include <string> - -int main(int argc, char *argv[]) { - std::cout << "PRU1" << std::endl; -} diff --git a/src/pru2/servos.cpp b/src/pru2/servos.cpp deleted file mode 100644 index ba71ce2..0000000 --- a/src/pru2/servos.cpp +++ /dev/null @@ -1,11 +0,0 @@ -/* - * Author: Gregory Wainer <[email protected]> - * Date: 10/14/2023 - * - */ - -#include <iostream> - -int main(int argc, char* argv[]) { - std::cout << "PRU2" << std::endl; -} diff --git a/src/rocketUtils.cpp b/src/rocketUtils.cpp new file mode 100644 index 0000000..45fcfc3 --- /dev/null +++ b/src/rocketUtils.cpp @@ -0,0 +1,35 @@ +#include "../include/rocketUtils.hpp" + +double deploy_percentage_to_angle(double percentage) { + + return (MAX_ANGLE - MIN_ANGLE) / 100.0 * percentage + MIN_ANGLE; +} + + +std::string format_data(std::string prefix, double data, int precision) { + + std::stringstream stream; + stream << std::fixed << std::setprecision(precision) << data; + std::string s = stream.str(); + return prefix + s; +} + +bool led_out(Vehicle *vehicle) { + + std::ofstream file; + file.open(LED_FILENAME); + if (!file.is_open()) { + return false; + } + + file << std::to_string(vehicle->led_brightness); + file.close(); + + vehicle->led_time = time(nullptr); + vehicle->led_brightness = (vehicle->led_brightness + 1) % 2; + + return true; +} + +std::string state_for_log[5] = {"ON_PAD", "BOOST", "GLIDE", "APOGEE", "DONE"}; +
\ No newline at end of file diff --git a/src/sensorAltimeter.cpp b/src/sensorAltimeter.cpp new file mode 100644 index 0000000..8ec065d --- /dev/null +++ b/src/sensorAltimeter.cpp @@ -0,0 +1,115 @@ +#include "sensorAltimeter.hpp" + +AltimeterSensor::AltimeterSensor(std::string I2C_FILE_in) { + I2C_FILE = I2C_FILE_in; + deviceAddress = 0x60; +} + +//Startup routine copied from Adafruit library, as is most of the data getting methods +//Adaptation is largely editing for readability and porting from Adafruit_I2C to BBB I2C (sensorI2C.hpp implementation) +bool AltimeterSensor::init() { + + // Vehicle *vehicle = (Vehicle *) data; + // // Do Stuff + // data = (void*) vehicle; + + //Pass file string from parent to setup function, actual I2C bus gets stored internally. + setupI2C(I2C_FILE); + + // Check a register with a hard-coded value to see if comms are working + uint8_t whoami = readSingleRegister(MPL3115A2_WHOAMI); + if (whoami != 0xC4) { + fprintf(stderr, "MPL INITIALIZATION DID NOT PASS WHOAMI DEVICE CHECK!, got: %X, expected: 0xC4\n", whoami); + return false; + } + + //Send device dedicated reset byte to CTRL1 Register + writeRegister(MPL3115A2_CTRL_REG1, MPL3115A2_CTRL_REG1_RST); + //Wait for reset to wipe its way through device and reset appropriate bit of CTRL1 Register + while (readSingleRegister(MPL3115A2_CTRL_REG1) & MPL3115A2_CTRL_REG1_RST); + + //Set oversampling (?) and altitude mode by default + currentMode = MPL3115A2_ALTIMETER; + ctrl_reg1.reg = MPL3115A2_CTRL_REG1_OS128 | MPL3115A2_CTRL_REG1_ALT; + writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); + + //Configure data return types, I don't really understand this chunk but Adafruit does it this way so we will too I guess + writeRegister(MPL3115A2_PT_DATA_CFG, MPL3115A2_PT_DATA_CFG_TDEFE | + MPL3115A2_PT_DATA_CFG_PDEFE | + MPL3115A2_PT_DATA_CFG_DREM); + + return true; +} + +//EXPECTED THAT USER WILL NEVER SET MODE TO PRESSURE AFTER INITIAL CONFIGURATION +void AltimeterSensor::setMode(mpl3115a2_mode_t mode) { + ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1); + ctrl_reg1.bit.ALT = mode; + writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); + currentMode = mode; +} + +double AltimeterSensor::getAltitude() { + //Request new data reading + requestOneShotReading(); + //If new data is available, read it and store it to internal fields + if (isNewDataAvailable()) { + //Logger flag here for new data? + updateCurrentDataBuffer(); + } + //Return internal field, whether updated or not + return internalAltitude; +} + +double AltimeterSensor::getTemperature() { + //Request new data reading + requestOneShotReading(); + //If new data is available, read it and store it to internal fields + if (isNewDataAvailable()) { + //Logger flag here for new data? + updateCurrentDataBuffer(); + } + //Return internal field, whether updated or not + return internalTemperature; +} + +void AltimeterSensor::requestOneShotReading() { + //Request current status of oneshot reading + ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1); + //If oneshot is complete, proc a new one; if it isn't, do nothing. + //THIS PRODUCES DUPLICATE DATA IF READING REQUESTS FROM BB DON'T LINE UP WITH READING COMPLETION ON SENSOR. + if (!ctrl_reg1.bit.OST) { + // initiate one-shot measurement + ctrl_reg1.bit.OST = 1; + writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); + } +} + +bool AltimeterSensor::isNewDataAvailable() { + //Returns PTDR bit of status register, 1 if new data for Temp OR Alt/Pres is available + //There *are* registers available for exclusively temperature *or* pressure/altitude, but + //for simplicity's sake we'll use the combined one for now. + return ((readSingleRegister(MPL3115A2_REGISTER_STATUS) & MPL3115A2_REGISTER_STATUS_PTDR) != 0); +} + +//Adafruit returns specific field based on input parameter, this method updates all internal fields at once instead +void AltimeterSensor::updateCurrentDataBuffer() { + uint8_t buffer[5] = {MPL3115A2_REGISTER_PRESSURE_MSB, 0, 0, 0, 0}; + readMultipleRegisters(MPL3115A2_REGISTER_PRESSURE_MSB, 5); + + //Pressure is no longer used, assumed rocket is only logging altitude + // uint32_t pressure; + // pressure = uint32_t(buffer[0]) << 16 | uint32_t(buffer[1]) << 8 | + // uint32_t(buffer[2]); + // return double(pressure) / 6400.0; + + //Altitude Conversion + int32_t alt; + alt = uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 | + uint32_t(buffer[2]) << 8; + internalAltitude = double(alt) / 65536.0; + + int16_t t; + t = uint16_t(buffer[3]) << 8 | uint16_t(buffer[4]); + internalTemperature = double(t) / 256.0; +} diff --git a/src/sensorIMU.cpp b/src/sensorIMU.cpp new file mode 100644 index 0000000..941ea35 --- /dev/null +++ b/src/sensorIMU.cpp @@ -0,0 +1,385 @@ +#include "../include/sensorIMU.hpp" + +IMUSensor::IMUSensor(std::string I2C_FILE) { + this -> I2C_FILE = I2C_FILE; +} + +bool IMUSensor::init(void* data) { + + //I2C_File passed on object creation, stored in sensorI2C parent + setupI2C(I2C_FILE); + + //In the adafruit code there's a big step of waiting for timeout and connection stuff for up to a full second + //I don't do that here because the BBB takes like 17 years to boot so we'll just hope it goes faster than that + + //Sanity check for factory device ID + uint8_t id = readSingleRegister(BNO055_CHIP_ID_ADDR); + if (id != BNO055_ID) { + fprintf(stderr, "DEVICE ID DID NOT PASS SANITY CHECK FOR BNO IMU!"); + return false; + } + + //Set default operating mode of IMU into config from startup (will be set properly after config phase) + setModeHard(OPERATION_MODE_CONFIG); + + //Writes 1 to the system reset bit in the trigger register + writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x20); + //Wait for reset to complete by doing sanity check again + while (readSingleRegister(BNO055_CHIP_ID_ADDR) != BNO055_ID); + + //Set power mode for sensor + writeRegister(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL); + + //Sensor chip uses two "pages" to multiplex register values + //Page 0 contains the sensor data (not configuration), which is what we want + writeRegister(BNO055_PAGE_ID_ADDR, 0); + + //Genuinely no idea why Adafruit does this, ensuring all triggers are off before mode config I guess + writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x0); + + setModeTemp(default_mode); + + return true; +} + +//Sets mode so it can be undone for temporary changes, like operation setting +void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) { + currentMode = mode; + writeRegister(BNO055_OPR_MODE_ADDR, currentMode); +} + +//Sets mode *AND* internal state variable +void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) { + writeRegister(BNO055_OPR_MODE_ADDR, currentMode); +} + +adafruit_bno055_opmode_t IMUSensor::getMode() { + return (adafruit_bno055_opmode_t)readSingleRegister(BNO055_OPR_MODE_ADDR); +} + +imu::Vector<3> IMUSensor::getVector(adafruit_vector_type_t vector_type) { + imu::Vector<3> xyz; + uint8_t buffer[6] = readMultipleRegisters((adafruit_bno055_reg_t)vector_type, 6); + + int16_t x, y, z; + x = y = z = 0; + + /* Read vector data (6 bytes) */ + x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8); + y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8); + z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8); + + /*! + * Convert the value to an appropriate range (section 3.6.4) + * and assign the value to the Vector type + */ + switch (vector_type) { + case VECTOR_MAGNETOMETER: + /* 1uT = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_GYROSCOPE: + /* 1dps = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_EULER: + /* 1 degree = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_ACCELEROMETER: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + case VECTOR_LINEARACCEL: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + case VECTOR_GRAVITY: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + } + + return xyz; +} +imu::Quaternion IMUSensor::getQuat() { + uint8_t buffer[8] = readMultipleRegisters(BNO055_QUATERNION_DATA_W_LSB_ADDR, 8); + + int16_t x, y, z, w; + x = y = z = w = 0; + + //Bit shift data into the right places and store it + w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); + x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); + y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); + z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]); + + /*! + * Assign to Quaternion + * See + * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf + * 3.6.5.5 Orientation (Quaternion) + */ + const double scale = (1.0 / (1 << 14)); + imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z); + return quat; +} + +int8_t IMUSensor::getTemp() { + int8_t temp = (int8_t)(readSingleRegister(BNO055_TEMP_ADDR)); + return temp; +} + +void IMUSensor::setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode) { + //Put into proper config for mapping stuff + setModeTemp(OPERATION_MODE_CONFIG); + writeRegister(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode); + + //Return mode to operating mode + setModeTemp(currentMode); +} + +void IMUSensor::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) { + //See above method, pretty much the exact same + setModeTemp(OPERATION_MODE_CONFIG); + writeRegister(BNO055_AXIS_MAP_SIGN_ADDR, remapsign); + setModeTemp(currentMode); +} + +//This method is weird; it intakes several existing byte pointers to see what action it should take. Luckily, we shouldn't have to use it. +void IMUSensor::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) { + //Make sure IMU is on proper register page to get system status + writeRegister(BNO055_PAGE_ID_ADDR, 0); + + //If system status requested, read the status. + if (system_status != 0) *system_status = readSingleRegister(BNO055_SYS_STAT_ADDR); + //If self test result requested, pull the self test results. + if (self_test_result != 0) *self_test_result = readSingleRegister(BNO055_SELFTEST_RESULT_ADDR); + //Finally, if there's an error pull and stash it. + if (system_error != 0) *system_error = readSingleRegister(BNO055_SYS_ERR_ADDR); +} + +//Same as above method, byte pointers are fed into it as parameters that get populated by method. +void IMUSensor::getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag) { + uint8_t calData = readSingleRegister(BNO055_CALIB_STAT_ADDR); + if (sys != NULL) { + *sys = (calData >> 6) & 0x03; + } + if (gyro != NULL) { + *gyro = (calData >> 4) & 0x03; + } + if (accel != NULL) { + *accel = (calData >> 2) & 0x03; + } + if (mag != NULL) { + *mag = calData & 0x03; + } +} + +/* Functions to deal with raw calibration data */ +bool IMUSensor::getSensorOffsets(uint8_t *calibData) { + if (isFullyCalibrated()) { + setModeTemp(OPERATION_MODE_CONFIG); + + calibData = readMultipleRegisters(ACCEL_OFFSET_X_LSB_ADDR, NUM_BNO055_OFFSET_REGISTERS); + + setModeTemp(currentMode); + return true; + } + return false; +} + +//Fully populated offset getter using type of offset, not just calibration data +bool IMUSensor::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type) { + if (isFullyCalibrated()) { + setModeTemp(OPERATION_MODE_CONFIG); + + /* Accel offset range depends on the G-range: + +/-2g = +/- 2000 mg + +/-4g = +/- 4000 mg + +/-8g = +/- 8000 mg + +/-1§g = +/- 16000 mg */ + offsets_type.accel_offset_x = (readSingleRegister(ACCEL_OFFSET_X_MSB_ADDR) << 8) | + (readSingleRegister(ACCEL_OFFSET_X_LSB_ADDR)); + offsets_type.accel_offset_y = (readSingleRegister(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | + (readSingleRegister(ACCEL_OFFSET_Y_LSB_ADDR)); + offsets_type.accel_offset_z = (readSingleRegister(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | + (readSingleRegister(ACCEL_OFFSET_Z_LSB_ADDR)); + + /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */ + offsets_type.mag_offset_x = + (readSingleRegister(MAG_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_X_LSB_ADDR)); + offsets_type.mag_offset_y = + (readSingleRegister(MAG_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Y_LSB_ADDR)); + offsets_type.mag_offset_z = + (readSingleRegister(MAG_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Z_LSB_ADDR)); + + /* Gyro offset range depends on the DPS range: + 2000 dps = +/- 32000 LSB + 1000 dps = +/- 16000 LSB + 500 dps = +/- 8000 LSB + 250 dps = +/- 4000 LSB + 125 dps = +/- 2000 LSB + ... where 1 DPS = 16 LSB */ + offsets_type.gyro_offset_x = + (readSingleRegister(GYRO_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_X_LSB_ADDR)); + offsets_type.gyro_offset_y = + (readSingleRegister(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Y_LSB_ADDR)); + offsets_type.gyro_offset_z = + (readSingleRegister(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Z_LSB_ADDR)); + + /* Accelerometer radius = +/- 1000 LSB */ + offsets_type.accel_radius = + (readSingleRegister(ACCEL_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(ACCEL_RADIUS_LSB_ADDR)); + + /* Magnetometer radius = +/- 960 LSB */ + offsets_type.mag_radius = + (readSingleRegister(MAG_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(MAG_RADIUS_LSB_ADDR)); + + setModeTemp(currentMode); + return true; + } + return false; +} + +void IMUSensor::setSensorOffsets(const uint8_t *calibData) { + setModeTemp(OPERATION_MODE_CONFIG); + + /* Note: Configuration will take place only when user writes to the last + byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). + Therefore the last byte must be written whenever the user wants to + changes the configuration. */ + + /* A writeLen() would make this much cleaner */ + writeRegister(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]); + writeRegister(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]); + writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]); + writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]); + writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]); + writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]); + + writeRegister(MAG_OFFSET_X_LSB_ADDR, calibData[6]); + writeRegister(MAG_OFFSET_X_MSB_ADDR, calibData[7]); + writeRegister(MAG_OFFSET_Y_LSB_ADDR, calibData[8]); + writeRegister(MAG_OFFSET_Y_MSB_ADDR, calibData[9]); + writeRegister(MAG_OFFSET_Z_LSB_ADDR, calibData[10]); + writeRegister(MAG_OFFSET_Z_MSB_ADDR, calibData[11]); + + writeRegister(GYRO_OFFSET_X_LSB_ADDR, calibData[12]); + writeRegister(GYRO_OFFSET_X_MSB_ADDR, calibData[13]); + writeRegister(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]); + writeRegister(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]); + writeRegister(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]); + writeRegister(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]); + + writeRegister(ACCEL_RADIUS_LSB_ADDR, calibData[18]); + writeRegister(ACCEL_RADIUS_MSB_ADDR, calibData[19]); + + writeRegister(MAG_RADIUS_LSB_ADDR, calibData[20]); + writeRegister(MAG_RADIUS_MSB_ADDR, calibData[21]); + + setModeTemp(currentMode); +} + +void IMUSensor::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) { + setModeTemp(OPERATION_MODE_CONFIG); + + /* Note: Configuration will take place only when user writes to the last + byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). + Therefore the last byte must be written whenever the user wants to + changes the configuration. */ + + writeRegister(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF); + writeRegister(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF); + writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF); + writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF); + writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF); + writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF); + + writeRegister(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF); + writeRegister(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF); + writeRegister(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF); + writeRegister(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF); + writeRegister(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF); + writeRegister(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF); + + writeRegister(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF); + writeRegister(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF); + writeRegister(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF); + writeRegister(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF); + writeRegister(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF); + writeRegister(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF); + + writeRegister(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF); + writeRegister(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF); + + writeRegister(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF); + writeRegister(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF); + + setModeTemp(currentMode); + +} + +bool IMUSensor::isFullyCalibrated() { + uint8_t system, gyro, accel, mag; + getCalibration(&system, &gyro, &accel, &mag); + + switch (currentMode) { + case OPERATION_MODE_ACCONLY: + return (accel == 3); + case OPERATION_MODE_MAGONLY: + return (mag == 3); + case OPERATION_MODE_GYRONLY: + case OPERATION_MODE_M4G: /* No magnetometer calibration required. */ + return (gyro == 3); + case OPERATION_MODE_ACCMAG: + case OPERATION_MODE_COMPASS: + return (accel == 3 && mag == 3); + case OPERATION_MODE_ACCGYRO: + case OPERATION_MODE_IMUPLUS: + return (accel == 3 && gyro == 3); + case OPERATION_MODE_MAGGYRO: + return (mag == 3 && gyro == 3); + default: + return (system == 3 && gyro == 3 && accel == 3 && mag == 3); + } +} + +/* Power managments functions */ +void IMUSensor::enterSuspendMode() { + /* Switch to config mode (just in case since this is the default) */ + setModeTemp(OPERATION_MODE_CONFIG); + writeRegister(BNO055_PWR_MODE_ADDR, 0x02); + /* Set the requested operating mode (see section 3.3) */ + setModeTemp(currentMode); +} + +void IMUSensor::enterNormalMode() { + /* Switch to config mode (just in case since this is the default) */ + setModeTemp(OPERATION_MODE_CONFIG); + writeRegister(BNO055_PWR_MODE_ADDR, 0x00); + /* Set the requested operating mode (see section 3.3) */ + setModeTemp(modeback); +} + + + + + + + + + + diff --git a/src/surfaceFitModel.cpp b/src/surfaceFitModel.cpp new file mode 100644 index 0000000..d48da49 --- /dev/null +++ b/src/surfaceFitModel.cpp @@ -0,0 +1,40 @@ +#include "../include/surfaceFitModel.hpp" + +SurfaceFitModel::SurfaceFitModel() { + + p = MatrixXd::Zero(X_DEGREE + 1, Y_DEGREE + 1); + + p(0, 0) = -781536.384794701; + p(1, 0) = 8623.59011973048; + p(0, 1) = 643.65918253; + p(2, 0) = -34.3646691281487; + p(1, 1) = -5.46066535343611; + p(0, 2) = -0.177121900557321; + p(3, 0) = 0.0573287698655951; + p(2, 1) = 0.0150031142038895; + p(1, 2) = 0.00101871763126609; + p(0, 3) = 1.63862900553892e-05; + p(4, 0) = -3.21785828407871e-05; + p(3, 1) = -1.3161091180883e-05; + p(2, 2) = -1.42505256569339e-06; + p(1, 3) = -4.76209793830867e-08; +} + + +double SurfaceFitModel::getFit(double x, double y) { + + return p(0, 0) + p(1, 0) * x + p(0, 1) * y + p(2, 0) * pow(x, 2) + + p(1, 1) * x * y + p(0, 2) * pow(y, 2) + p(3, 0) * pow(x, 3) + + p(2, 1) * pow(x, 2) * y + p(1, 2) * x * pow(y, 2) + p(0, 3) * pow(y, 3) + + p(4, 0) * pow(x, 4) + p(3, 1) * pow(x, 3) * y + p(2, 2) * pow(x, 2) * pow(y, 2) + + p(1, 3) * x * pow(y, 3); +} + + + + + + + + + diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 9aef78e..13d4dc6 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,14 +1,70 @@ -enable_testing() +cmake_minimum_required(VERSION 3.16.3) -add_executable( - hello_test - hello_test.cc -) -target_link_libraries( - hello_test - GTest::gtest_main +project(ADSTests) + +# Set Variables +set(TARGET_A TESTkalmanFilter) +set(SOURCES_A kalmanFilterTest.cpp ../src/kalmanfilter.cpp) + +set(TARGET_B TESTsurfaceFitModel) +set(SOURCES_B surfaceFitModelTest.cpp ../src/surfaceFitModel.cpp) + +set(TARGET_C TESTactuationPlan) +set(SOURCES_C actuationPlanTest.cpp ../src/actuationPlan.cpp ../src/surfaceFitModel.cpp ../src/rocketUtils.cpp ../src/sensorIMU.cpp ../src/sensorAltimeter.cpp) + +# set(TARGET_D TESTads) +# set(SOURCES_D adsTest.cpp ../src/ads.cpp ../src/actuationPlan.cpp ../src/surfaceFitModel.cpp ../src/rocketUtils.cpp ../src/sensorIMU.cpp ../src/sensorAltimeter.cpp ../src/motor.cpp ../src/logger.cpp ../src/kalmanfilter.cpp) + +set(TARGET_E TESTmotorPlan) +set(SOURCES_E motorTest.cpp ../src/motor.cpp ../src/rocketUtils.cpp) + +set(TARGET_F TESTsensorAltimeterPlan) +set(SOURCES_F sensorAltimeterTest.cpp ../src/sensorAltimeter.cpp ../src/rocketUtils.cpp) + +set(TARGET_G TESTsensorIMUPlan) +set(SOURCES_G sensorIMUTest.cpp ../src/sensorIMU.cpp ../src/rocketUtils.cpp) + + +#Set-up Google Test +set(CMAKE_CXX_STANDARD 14) +include(FetchContent) +FetchContent_Declare( + googletest + URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip ) +# For Windows: Prevent overriding the parent project's compiler/linker settings +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) +FetchContent_MakeAvailable(googletest) + + + + +# Create Executables & Link Dependencies +add_executable(${TARGET_A} ${SOURCES_A}) +target_link_libraries(${TARGET_A} PUBLIC gtest_main) # Link GoogleTest's main() to Executable +# Tell CMake the Target is a Unit Test +add_test(NAME ${TARGET_A} COMMAND ${TARGET_A}) + +add_executable(${TARGET_B} ${SOURCES_B}) +target_link_libraries(${TARGET_B} PUBLIC gtest_main) +add_test(NAME ${TARGET_B} COMMAND ${TARGET_B}) + +add_executable(${TARGET_C} ${SOURCES_C}) +target_link_libraries(${TARGET_C} PUBLIC gtest_main) +add_test(NAME ${TARGET_C} COMMAND ${TARGET_C}) + +# add_executable(${TARGET_D} ${SOURCES_D}) +# target_link_libraries(${TARGET_D} PUBLIC gtest_main) +# add_test(NAME ${TARGET_D} COMMAND ${TARGET_D}) + +add_executable(${TARGET_E} ${SOURCES_E}) +target_link_libraries(${TARGET_E} PUBLIC gtest_main) +add_test(NAME ${TARGET_E} COMMAND ${TARGET_E}) -include(GoogleTest) -gtest_discover_tests(hello_test) +add_executable(${TARGET_F} ${SOURCES_F}) +target_link_libraries(${TARGET_F} PUBLIC gtest_main) +add_test(NAME ${TARGET_F} COMMAND ${TARGET_F}) +add_executable(${TARGET_G} ${SOURCES_G}) +target_link_libraries(${TARGET_G} PUBLIC gtest_main) +add_test(NAME ${TARGET_G} COMMAND ${TARGET_G})
\ No newline at end of file diff --git a/test/actuationPlanTest.cpp b/test/actuationPlanTest.cpp new file mode 100644 index 0000000..72410b0 --- /dev/null +++ b/test/actuationPlanTest.cpp @@ -0,0 +1,54 @@ +#include <iostream> +#include <gtest/gtest.h> +#include "../include/surfaceFitModel.hpp" +#include "../include/actuationPlan.hpp" +#include "../include/rocketUtils.hpp" + + +class ActuationPlanTest : public ::testing::Test { + + protected: + + ActuationPlanTest() { + + SurfaceFitModel sfm = SurfaceFitModel(); + plan = new ActuationPlan(sfm); + } + + //~ActuationPlanTest() {} + + ActuationPlan *plan; +}; + + +/** + * @brief Tests running the Actuation Plan + * + * **/ +TEST_F(ActuationPlanTest, runPlan) { + + Vehicle rocket; + + rocket.imuInitFail = false; + rocket.imuReadFail = false; + rocket.altiInitFail = false; + rocket.altiReadFail = false; + + // Test when Vehicle Status: Glide + rocket.fail_time = (time_t)(-1); + rocket.deploy_time = time(nullptr); + rocket.status = GLIDE; + rocket.filtered_altitude = 1; + rocket.filtered_velocity = 2; + plan->runPlan(rocket); + EXPECT_NEAR(rocket.deployment_angle, 120.0, 0.01); + EXPECT_NE(rocket.fail_time, (time_t)(-1)); + + // Test when Vehicle Status: Apogee + rocket.deploy_time = time(nullptr); + rocket.status = APOGEE; + rocket.filtered_altitude = 1; + rocket.filtered_velocity = 2; + plan->runPlan(rocket); + EXPECT_NEAR(rocket.deployment_angle, 110.0, 0.01); +}
\ No newline at end of file diff --git a/test/hello_test.cc b/test/hello_test.cc deleted file mode 100644 index 5a57e13..0000000 --- a/test/hello_test.cc +++ /dev/null @@ -1,9 +0,0 @@ -#include <gtest/gtest.h> - -// Demonstrate some basic assertions. -TEST(HelloTest, BasicAssertions) { - // Expect two strings not to be equal. - EXPECT_STRNE("hello", "world"); - // Expect equality. - EXPECT_EQ(7 * 6, 42); -} diff --git a/test/kalmanFilterTest.cpp b/test/kalmanFilterTest.cpp new file mode 100644 index 0000000..e3dc1e4 --- /dev/null +++ b/test/kalmanFilterTest.cpp @@ -0,0 +1,101 @@ +#include <iostream> +#include <gtest/gtest.h> +#include "eigen3/Eigen/Dense" +#include "../include/kalmanfilter.hpp" + +using namespace Eigen; + +class KalmanFilterTest : public ::testing::Test { + + protected: + + KalmanFilterTest() { + + kf = new KalmanFilter(2, 1, 1, 1); + } + + //~KalmanFilterTest() {} + + KalmanFilter *kf; +}; + + +/** + * @brief Test Setting the initial state x & P + * + * **/ +TEST_F(KalmanFilterTest, setInitialState) { + + VectorXf state_vec(2); + MatrixXf state_cov(2, 2); + state_vec << 1, 2; + state_cov << 1, 3, 4, 9; + + // Success Case + EXPECT_TRUE(kf->setInitialState(state_vec, state_cov)); + + // Failure Case + VectorXf state_vec2(4); + state_vec2 << 1, 2, 3, 4; + EXPECT_FALSE(kf->setInitialState(state_vec2, state_cov)); +} + +/** + * @brief Test a single iteration of the Kalman Filter + * + * **/ +TEST_F(KalmanFilterTest, run) { + + VectorXf control(1); + VectorXf measurement(1); + control << 1; + measurement << 1; + VectorXf res(1); + + res = kf->run(control, measurement, 1); + + EXPECT_NEAR(0.5454, res(0), 0.0001); + EXPECT_NEAR(1, res(1), 0.0001); +} + + +/** + * @brief Test run() when the time step value is changed between function calls. + * + */ +TEST_F(KalmanFilterTest, runChange) { + + VectorXf control(1); + VectorXf measurement(1); + control << 1; + measurement << 1; + VectorXf res(1); + + res = kf->run(control, measurement, 0.1); + EXPECT_NEAR(0.09545, res(0), 0.00001); + EXPECT_NEAR(0.1, res(1), 0.1); + + res = kf->run(control, measurement, 0.15); + EXPECT_NEAR(0.2761, res(0), 0.0001); + EXPECT_NEAR(0.3585, res(1), 0.0001); +} + + + + + + + + + + + + + + + + + + + + diff --git a/test/loggerTest.cpp b/test/loggerTest.cpp new file mode 100644 index 0000000..fab2f25 --- /dev/null +++ b/test/loggerTest.cpp @@ -0,0 +1,57 @@ +#include "../include/logger.hpp" + +class Two { + + public: + Two() { + + } + + void WritetoLog() { + Logger::Get().log("THIS IS A NEEEEEWWWW TEST!!!!!"); + Logger::Get().logErr("ERRORRRRRRRR"); + Logger::Get().printLog(); + } + + void TryPrint() { + Logger::Get().printLog(); + } +}; + +class One { + + public: + Two two; + One() { + two = Two(); + } + + void OpenLog() { + Logger::Get().openLog("output.txt"); + two.WritetoLog(); + Logger::Get().closeLog(); + two.TryPrint(); + } +}; + + + + +int main(int argc, char* argv[]) { + + // Logger::Get().openLog("output.txt"); + // Logger::Get().log("Testing Info aksdjflkas lksajfdlasjfaowe aslkdjf alskjf asodfj03945430 0349534 5039485 345"); + // Logger::Get().log("Testing Info Just some stuff"); + // Logger::Get().log("Testing something words words words"); + // Logger::Get().logErr("Testing Error lksdj fa 3459374"); + // Logger::Get().log("Testing blah blah blah blah blah lbal lab lbas labal abala"); + // Logger::Get().log("Testing Info aksdjflkas l11111111111111111111111111111111111111111485 345"); + // Logger::Get().logErr("Testing Error Some other type of error"); + // Logger::Get().logErr("Testing Error Another one"); + // Logger::Get().closeLog(); + // //log.printLog(); + + One one = One(); + one.OpenLog(); + +}
\ No newline at end of file diff --git a/test/motorTest.cpp b/test/motorTest.cpp new file mode 100644 index 0000000..c570826 --- /dev/null +++ b/test/motorTest.cpp @@ -0,0 +1,50 @@ +#include <iostream> +#include <vector> +#include <gtest/gtest.h> +#include "../include/motor.hpp" +#include "../include/rocketUtils.hpp" + +class MotorTest : public ::testing::Test { + + protected: + + MotorTest() { + + m1 = new Motor(); + } + + //~IMUSensorTest() {} + + Motor *m1; +}; + + +/** + * @brief Test a + * + * **/ +TEST_F(MotorTest, init) { + + Vehicle rocket; + + m1->init((void*)&rocket); + + // ASSERT Statements..... +} + + +/** + * @brief Test a + * + * **/ +TEST_F(MotorTest, writeData) { + + Vehicle rocket; + + m1->writeData((void*)&rocket); + + // ASSERT Statements..... +} + + +// TODO: FIGURE OUT WHY MAKING 'rocket' a POINTER CAUSES A SEGFAULT diff --git a/test/sensorAltimeterTest.cpp b/test/sensorAltimeterTest.cpp new file mode 100644 index 0000000..7ba9521 --- /dev/null +++ b/test/sensorAltimeterTest.cpp @@ -0,0 +1,50 @@ +#include <iostream> +#include <vector> +#include <gtest/gtest.h> +#include "../include/sensorAltimeter.hpp" +#include "../include/rocketUtils.hpp" + +class AltimeterSensorTest : public ::testing::Test { + + protected: + + AltimeterSensorTest() { + + alti = new AltimeterSensor(); + } + + //~IMUSensorTest() {} + + AltimeterSensor *alti; +}; + + +/** + * @brief Test a + * + * **/ +TEST_F(AltimeterSensorTest, init) { + + Vehicle rocket; + + alti->init((void*)&rocket); + + // ASSERT Statements..... +} + + +/** + * @brief Test a + * + * **/ +TEST_F(AltimeterSensorTest, getData) { + + Vehicle rocket; + + alti->getData((void*)&rocket.current_altitude); + + // ASSERT Statements..... +} + + +// TODO: FIGURE OUT WHY MAKING 'rocket' a POINTER CAUSES A SEGFAULT diff --git a/test/sensorIMUTest.cpp b/test/sensorIMUTest.cpp new file mode 100644 index 0000000..d16930e --- /dev/null +++ b/test/sensorIMUTest.cpp @@ -0,0 +1,50 @@ +#include <iostream> +#include <vector> +#include <gtest/gtest.h> +#include "../include/sensorIMU.hpp" +#include "../include/rocketUtils.hpp" + +class IMUSensorTest : public ::testing::Test { + + protected: + + IMUSensorTest() { + + imu = new IMUSensor(); + } + + //~IMUSensorTest() {} + + IMUSensor *imu; +}; + + +/** + * @brief Test a + * + * **/ +TEST_F(IMUSensorTest, init) { + + Vehicle rocket; + + imu->init((void*)&rocket); + + // ASSERT Statements..... +} + + +/** + * @brief Test a + * + * **/ +TEST_F(IMUSensorTest, getData) { + + Vehicle rocket; + + imu->getData((void*)&rocket); + + // ASSERT Statements..... +} + + +// TODO: FIGURE OUT WHY MAKING 'rocket' a POINTER CAUSES A SEGFAULT diff --git a/test/sensor_test.cpp b/test/sensor_test.cpp new file mode 100644 index 0000000..e4ab5ca --- /dev/null +++ b/test/sensor_test.cpp @@ -0,0 +1,10 @@ +#include <iostream> +#include "sensorI2C.hpp" + +int main (int argc, char** argv) { + AltimeterSensor altimeter = new AltimeterSensor("/dev/i2c-2"); + altimeter.init(); + for (int i = 0; i < 1000; i++) { + std::cout << "Altitude: " << altimeter.getAltitude() << std::endl; + } +} diff --git a/test/surfaceFitModelTest.cpp b/test/surfaceFitModelTest.cpp new file mode 100644 index 0000000..f0521b5 --- /dev/null +++ b/test/surfaceFitModelTest.cpp @@ -0,0 +1,43 @@ +#include <iostream> +#include <gtest/gtest.h> +#include "eigen3/Eigen/Dense" +#include "../include/surfaceFitModel.hpp" + +using namespace Eigen; + +class SurfaceFitModelTest : public ::testing::Test { + + protected: + + SurfaceFitModelTest() { + + surfFM = new SurfaceFitModel(); + } + + //~SurfaceFitModelTest() {} + + SurfaceFitModel *surfFM; +}; + + +/** + * @brief Test a + * + * **/ +TEST_F(SurfaceFitModelTest, getFit1) { + + double res = surfFM->getFit(1, 2); + + EXPECT_NEAR(res, -771671.3793209707, 0.01); +} + +/** + * @brief Test a + * + * **/ +TEST_F(SurfaceFitModelTest, getFit2) { + + double res = surfFM->getFit(33.3, 49); + + EXPECT_NEAR(res, -507325.4658735892, 0.01); +}
\ No newline at end of file |
