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]>

10 files changed, 481 insertions, 19 deletions

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