From b7044b97d3506aa16e857efff8a0af0488ba211f Mon Sep 17 00:00:00 2001
From: Paul Nykiel <paul.nykiel@gmail.com>
Date: Sun, 1 Jan 2023 20:51:10 +0100
Subject: [PATCH] #45: Added application mode fcp test

---
 Src/Application/application.h              |  1 +
 Tests/LowLevel/Application/application.cpp | 55 +++++++++++++++++++++-
 2 files changed, 55 insertions(+), 1 deletion(-)

diff --git a/Src/Application/application.h b/Src/Application/application.h
index e16e462..cd4d98e 100644
--- a/Src/Application/application.h
+++ b/Src/Application/application.h
@@ -33,6 +33,7 @@
  *      * In failsave mode: set all three commands to 0
  *  * Pass the actuator commands to the actuators
  *  * Every FLIGHTCOMPUTER_SEND_PERIOD frame: pass the current data to flightcomputer_send
+ *  * Call imu_start_sampling
  */
 void application_init(void);
 
diff --git a/Tests/LowLevel/Application/application.cpp b/Tests/LowLevel/Application/application.cpp
index 8bc1053..52d9ea9 100644
--- a/Tests/LowLevel/Application/application.cpp
+++ b/Tests/LowLevel/Application/application.cpp
@@ -1,4 +1,5 @@
 #include <Mock/actuators.hpp>
+#include <Mock/controller.hpp>
 #include <Mock/error_handler.hpp>
 #include <Mock/flightcomputer.hpp>
 #include <Mock/imu.hpp>
@@ -35,7 +36,7 @@ TEST(TEST_NAME, init) {
     EXPECT_TRUE(systemHandle.functionGotCalled<system_post_init>());
 }
 
-TEST(TEST_NAME, timer) {
+TEST(TEST_NAME, timer_mode_fcp) {
     auto actuatorsHandle = mock::actuators.getHandle();
     auto errorHandlerHandle = mock::error_handler.getHandle();
     auto flighcomputerHandle = mock::flightcomputer.getHandle();
@@ -43,6 +44,7 @@ TEST(TEST_NAME, timer) {
     auto modeHandlerHandle = mock::mode_handler.getHandle();
     auto remoteHandlerHandle = mock::remote.getHandle();
     auto systemHandle = mock::system.getHandle();
+    auto controllerHandle = mock::controller.getHandle();
 
     system_timer_16_384ms_callback timer_callback = nullptr;
     systemHandle.overrideFunc<system_pre_init>(
@@ -62,4 +64,55 @@ TEST(TEST_NAME, timer) {
     EXPECT_TRUE(systemHandle.functionGotCalled<system_post_init>());
 
     // Actual test
+    modeHandlerHandle.overrideFunc<mode_handler_handle>(
+            [](imu_data_t *imu_data, remote_data_t *remote_data, flightcomputer_setpoint_t *flightcomputer_setpoint) {
+                imu_data->heading_mul_16 = 17;
+                imu_data->pitch_mul_16 = 18;
+                imu_data->roll_mul_16 = 19;
+                imu_data->d_heading_mul_16 = 20;
+                imu_data->d_pitch_mul_16 = 21;
+                imu_data->d_roll_mul_16 = 22;
+                imu_data->acc_x_mul_100 = 23;
+                imu_data->acc_y_mul_100 = 24;
+                imu_data->acc_z_mul_100 = 25;
+                imu_data->imu_ok = true;
+                remote_data->throttle_mixed = 26;
+                remote_data->elevon_left_mixed = 27;
+                remote_data->elevon_right_mixed = 28;
+                remote_data->remote_ok = true;
+                remote_data->override_active = false;
+                remote_data->is_armed = true;
+                flightcomputer_setpoint->roll = 29;
+                flightcomputer_setpoint->pitch = 30;
+                flightcomputer_setpoint->motor = 31;
+                return MODE_FLIGHTCOMPUTER;
+            });
+
+    controllerHandle.overrideFunc<controller_update>(
+            [](const imu_data_t *imu_data, int16_t roll_setpoint, int16_t pitch_setpoint) {
+                EXPECT_EQ(imu_data->heading_mul_16, 17);
+                EXPECT_EQ(imu_data->pitch_mul_16, 18);
+                EXPECT_EQ(imu_data->roll_mul_16, 19);
+                EXPECT_EQ(imu_data->d_heading_mul_16, 20);
+                EXPECT_EQ(imu_data->d_pitch_mul_16, 21);
+                EXPECT_EQ(imu_data->d_roll_mul_16, 22);
+                EXPECT_EQ(imu_data->acc_x_mul_100, 23);
+                EXPECT_EQ(imu_data->acc_y_mul_100, 24);
+                EXPECT_EQ(imu_data->acc_z_mul_100, 25);
+                EXPECT_EQ(imu_data->imu_ok, true);
+                EXPECT_EQ(roll_setpoint, 29);
+                EXPECT_EQ(pitch_setpoint, 30);
+                return controller_result_t{.elevon_left = 32, .elevon_right = 33};
+            });
+
+    actuatorsHandle.overrideFunc<actuators_set>([](const actuator_cmd_t *actuator_cmd) {
+        EXPECT_EQ(actuator_cmd->elevon_left, 32);
+        EXPECT_EQ(actuator_cmd->elevon_right, 33);
+        EXPECT_EQ(actuator_cmd->motor, 31);
+    });
+    timer_callback();
+    EXPECT_TRUE(modeHandlerHandle.functionGotCalled<mode_handler_handle>());
+    EXPECT_TRUE(controllerHandle.functionGotCalled<controller_update>());
+    EXPECT_TRUE(actuatorsHandle.functionGotCalled<actuators_set>());
+    EXPECT_TRUE(imuHandle.functionGotCalled<imu_start_sampling>());
 }