#45: Added application mode fcp test

ToolboxPlane · Jan 1, 2023 · b7044b9 · b7044b9
1 parent d4f10d1
commit b7044b9
Show file tree

Hide file tree

Showing 2 changed files with 55 additions and 1 deletion.
diff --git 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
@@ -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,14 +36,15 @@ 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();
     auto imuHandle = mock::imu.getHandle();
     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>());
 }