Skip to content

Commit 98463d1

Browse files
aul12aul12
committed
#45: Started with tests for mode handler
1 parent d9fce8f commit 98463d1

File tree

5 files changed

+218
-26
lines changed

5 files changed

+218
-26
lines changed

‎Src/Application/mode_handler.c

Lines changed: 31 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -10,20 +10,26 @@
1010
#include "error_handler.h"
1111

1212
enum {
13-
IMU_TIMEOUT = 6, // 6 * 16.384 \approx 100ms
14-
FLIGHTCOMPUTER_TIMOUT = 12, // 12 * 16.384 \approx 200ms
15-
REMOTE_TIMEOUT = 12, // 12 * 16.384 \approx 200ms
13+
IMU_TIMEOUT = (uint16_t) (100 / 16.384),
14+
FLIGHTCOMPUTER_TIMOUT = (uint16_t) ((2 * 100) / 16.384),
15+
REMOTE_TIMEOUT = (uint16_t) (200 / 16.384),
1616
};
1717

18+
static uint8_t imu_timeout_counter;
19+
static uint8_t flightcomputer_timeout_counter;
20+
static uint8_t remote_timeout_counter;
21+
22+
void mode_handler_init(void) {
23+
imu_timeout_counter = IMU_TIMEOUT;
24+
flightcomputer_timeout_counter = FLIGHTCOMPUTER_TIMOUT;
25+
remote_timeout_counter = REMOTE_TIMEOUT;
26+
}
27+
1828
mode_handler_mode_t mode_handler_handle(imu_data_t *imu_data, remote_data_t *remote_data,
1929
flightcomputer_setpoint_t *flightcomputer_setpoint) {
2030
/*
2131
* Timeout Handling
2232
*/
23-
static uint8_t imu_timeout_counter = 0;
24-
static uint8_t flightcomputer_timeout_counter = 0;
25-
static uint8_t remote_timeout_counter = 0;
26-
2733
if (!imu_data_available()) {
2834
imu_timeout_counter += 1;
2935
} else {
@@ -33,7 +39,6 @@ mode_handler_mode_t mode_handler_handle(imu_data_t *imu_data, remote_data_t *rem
3339
if (imu_timeout_counter >= IMU_TIMEOUT) {
3440
imu_timeout_counter = IMU_TIMEOUT;
3541
imu_active = false;
36-
error_handler_handle_warning(MODE_HANDLER, MODE_HANDLER_ERROR_NO_IMU_DATA);
3742
}
3843
*imu_data = imu_get_latest_data();
3944

@@ -46,7 +51,6 @@ mode_handler_mode_t mode_handler_handle(imu_data_t *imu_data, remote_data_t *rem
4651
if (remote_timeout_counter >= REMOTE_TIMEOUT) {
4752
remote_timeout_counter = REMOTE_TIMEOUT;
4853
remote_active = false;
49-
// error_handler_handle_warning(MODE_HANDLER, MODE_HANDLER_ERROR_NO_REMOTE_DATA);
5054
}
5155
*remote_data = remote_get_data();
5256

@@ -59,7 +63,6 @@ mode_handler_mode_t mode_handler_handle(imu_data_t *imu_data, remote_data_t *rem
5963
if (flightcomputer_timeout_counter >= FLIGHTCOMPUTER_TIMOUT) {
6064
flightcomputer_timeout_counter = FLIGHTCOMPUTER_TIMOUT;
6165
flightcomputer_active = false;
62-
// error_handler_handle_warning(MODE_HANDLER, MODE_HANDLER_ERROR_NO_FCP_DATA);
6366
}
6467
*flightcomputer_setpoint = flightcomputer_get_setpoint();
6568

@@ -70,19 +73,28 @@ mode_handler_mode_t mode_handler_handle(imu_data_t *imu_data, remote_data_t *rem
7073
remote_active = remote_active && remote_data->remote_ok;
7174

7275
/*
73-
* Setpoint selection
76+
* Signal warning if not active
7477
*/
75-
if (imu_active && remote_active && flightcomputer_active) {
76-
if (!remote_data->override_active && remote_data->is_armed) {
77-
return MODE_FLIGHTCOMPUTER;
78-
}
78+
if (!flightcomputer_active) {
79+
error_handler_handle_warning(MODE_HANDLER, MODE_HANDLER_ERROR_NO_FCP_DATA);
80+
}
81+
if (!remote_active) {
82+
error_handler_handle_warning(MODE_HANDLER, MODE_HANDLER_ERROR_NO_REMOTE_DATA);
83+
}
84+
if (!imu_active) {
85+
error_handler_handle_warning(MODE_HANDLER, MODE_HANDLER_ERROR_NO_IMU_DATA);
86+
}
87+
7988

80-
if (remote_data->override_active) {
81-
return MODE_REMOTE;
82-
}
89+
/*
90+
* Setpoint selection
91+
*/
92+
if (imu_active && remote_active && flightcomputer_active && !remote_data->override_active &&
93+
remote_data->is_armed) {
94+
return MODE_FLIGHTCOMPUTER;
8395
}
8496

85-
if (!flightcomputer_active && remote_active) {
97+
if (remote_active) {
8698
return MODE_REMOTE;
8799
}
88100

‎Src/Application/mode_handler.h

Lines changed: 24 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,30 @@ enum {
2727
typedef enum { MODE_FLIGHTCOMPUTER, MODE_REMOTE, MODE_STABILISED_FAILSAVE, MODE_FAILSAVE } mode_handler_mode_t;
2828

2929
/**
30-
* Query all components for new data, check the timeouts and whether the data is ok.
31-
* The mode is then returned based on the availability.
30+
* Initialize the mode handler by setting the timeouts for all devices to true.
31+
*/
32+
void mode_handler_init(void);
33+
34+
/**
35+
* @brief Query all components for new data, check the timeouts and whether the data is ok, the mode is then returned
36+
* based on the availability.
37+
*
38+
* First the timeout is checked for all devices, a device is set to not have a timeout if [device]_data_available()
39+
* return true at least once in the last [device]_TIMEOUT calls.
40+
* Next the integrity of the data is checked, a device is set to be active if it has no timeout and
41+
* [device]_get_data().[device]_ok. For every non-active device error_handle_warning(MODE_HANDLER,
42+
* MODE_HANDLER_ERROR_NO_[device]_DATA) is called.
43+
*
44+
* Depending on the active-status of all devices and the remote.override_active and remote.armed the mode is decided
45+
* according to the following decision table:
46+
*
47+
* | IMU | Remote | FCP | Override | Armed | Mode |
48+
* | --- | ------ | --- | -------- | ----- | -------------------- |
49+
* | Y | Y | Y | N | Y | FCP |
50+
* | | Y | | | | Remote |
51+
* | Y | N | | | | Stabilisied Failsave |
52+
* | N | N | | | | Failsave |
53+
*
3254
*
3355
* @param imu_data out-parameter, contains the last valid imu measurement
3456
* @param remote_data out-parameter, contains the last valid remote package

‎Src/main.c

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -116,6 +116,7 @@ int main(void) {
116116
remote_init();
117117
flightcomputer_init();
118118
actuators_init();
119+
mode_handler_init();
119120

120121
imu_start_sampling();
121122

‎Tests/LowLevel/Application/mode_handler.cpp

Lines changed: 157 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,164 @@
1+
#include <Mock/Application/error_handler.hpp>
2+
#include <Mock/Components/flightcomputer.hpp>
3+
#include <Mock/Components/imu.hpp>
4+
#include <Mock/Components/remote.hpp>
15
#include <gtest/gtest.h>
26

37
extern "C" {
48
#include <Application/mode_handler.h>
59
}
610

7-
TEST(TEST_NAME, init) {
11+
TEST(TEST_NAME, all_ok) {
12+
auto imuHandle = mock::imu.getHandle();
13+
auto flightcomputerHandle = mock::flightcomputer.getHandle();
14+
auto remoteHandle = mock::remote.getHandle();
15+
auto errorHandlerHandle = mock::error_handler.getHandle();
16+
17+
imuHandle.overrideFunc<imu_data_available>([]() { return true; });
18+
imuHandle.overrideFunc<imu_get_latest_data>([]() { return imu_data_t{.imu_ok = true}; });
19+
remoteHandle.overrideFunc<remote_data_available>([]() { return true; });
20+
remoteHandle.overrideFunc<remote_get_data>(
21+
[]() { return remote_data_t{.is_armed = true, .override_active = false, .remote_ok = true}; });
22+
flightcomputerHandle.overrideFunc<flightcomputer_setpoint_available>([]() { return true; });
23+
flightcomputerHandle.overrideFunc<flightcomputer_get_setpoint>([]() { return flightcomputer_setpoint_t{}; });
24+
25+
mode_handler_init();
26+
27+
imu_data_t imuData;
28+
remote_data_t remoteData;
29+
flightcomputer_setpoint_t flightcomputerSetpoint;
30+
31+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_FLIGHTCOMPUTER);
32+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
33+
MODE_HANDLER_ERROR_NO_IMU_DATA));
34+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
35+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
36+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
37+
MODE_HANDLER_ERROR_NO_FCP_DATA));
38+
}
39+
40+
TEST(TEST_NAME, imu_initial_nok) {
41+
auto imuHandle = mock::imu.getHandle();
42+
auto flightcomputerHandle = mock::flightcomputer.getHandle();
43+
auto remoteHandle = mock::remote.getHandle();
44+
auto errorHandlerHandle = mock::error_handler.getHandle();
45+
46+
imuHandle.overrideFunc<imu_data_available>([]() { return false; });
47+
imuHandle.overrideFunc<imu_get_latest_data>([]() { return imu_data_t{.imu_ok = true}; });
48+
remoteHandle.overrideFunc<remote_data_available>([]() { return true; });
49+
remoteHandle.overrideFunc<remote_get_data>(
50+
[]() { return remote_data_t{.is_armed = true, .override_active = false, .remote_ok = true}; });
51+
flightcomputerHandle.overrideFunc<flightcomputer_setpoint_available>([]() { return true; });
52+
flightcomputerHandle.overrideFunc<flightcomputer_get_setpoint>([]() { return flightcomputer_setpoint_t{}; });
53+
54+
mode_handler_init();
55+
56+
imu_data_t imuData;
57+
remote_data_t remoteData;
58+
flightcomputer_setpoint_t flightcomputerSetpoint;
59+
60+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_REMOTE);
61+
EXPECT_TRUE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
62+
MODE_HANDLER_ERROR_NO_IMU_DATA));
63+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
64+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
65+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
66+
MODE_HANDLER_ERROR_NO_FCP_DATA));
67+
}
68+
69+
TEST(TEST_NAME, imu_timeout) {
70+
auto imuHandle = mock::imu.getHandle();
71+
auto flightcomputerHandle = mock::flightcomputer.getHandle();
72+
auto remoteHandle = mock::remote.getHandle();
73+
auto errorHandlerHandle = mock::error_handler.getHandle();
74+
75+
std::size_t frameCounter = 0;
76+
imuHandle.overrideFunc<imu_data_available>([&frameCounter]() {
77+
frameCounter += 1;
78+
return not(2 <= frameCounter and frameCounter <= 7);
79+
});
80+
imuHandle.overrideFunc<imu_get_latest_data>([]() { return imu_data_t{.imu_ok = true}; });
81+
remoteHandle.overrideFunc<remote_data_available>([]() { return true; });
82+
remoteHandle.overrideFunc<remote_get_data>(
83+
[]() { return remote_data_t{.is_armed = true, .override_active = false, .remote_ok = true}; });
84+
flightcomputerHandle.overrideFunc<flightcomputer_setpoint_available>([]() { return true; });
85+
flightcomputerHandle.overrideFunc<flightcomputer_get_setpoint>([]() { return flightcomputer_setpoint_t{}; });
86+
87+
mode_handler_init();
88+
89+
imu_data_t imuData;
90+
remote_data_t remoteData;
91+
flightcomputer_setpoint_t flightcomputerSetpoint;
92+
93+
// 1 - data available
94+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_FLIGHTCOMPUTER);
95+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
96+
MODE_HANDLER_ERROR_NO_IMU_DATA));
97+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
98+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
99+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
100+
MODE_HANDLER_ERROR_NO_FCP_DATA));
101+
102+
// 2 - no data available (1. frame)
103+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_FLIGHTCOMPUTER);
104+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
105+
MODE_HANDLER_ERROR_NO_IMU_DATA));
106+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
107+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
108+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
109+
MODE_HANDLER_ERROR_NO_FCP_DATA));
110+
111+
// 3 - no data available (2. frame)
112+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_FLIGHTCOMPUTER);
113+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
114+
MODE_HANDLER_ERROR_NO_IMU_DATA));
115+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
116+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
117+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
118+
MODE_HANDLER_ERROR_NO_FCP_DATA));
119+
120+
// 4 - no data available (3. frame)
121+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_FLIGHTCOMPUTER);
122+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
123+
MODE_HANDLER_ERROR_NO_IMU_DATA));
124+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
125+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
126+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
127+
MODE_HANDLER_ERROR_NO_FCP_DATA));
128+
129+
// 5 - no data available (4. frame)
130+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_FLIGHTCOMPUTER);
131+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
132+
MODE_HANDLER_ERROR_NO_IMU_DATA));
133+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
134+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
135+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
136+
MODE_HANDLER_ERROR_NO_FCP_DATA));
137+
138+
// 6 - no data available (5. frame)
139+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_FLIGHTCOMPUTER);
140+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
141+
MODE_HANDLER_ERROR_NO_IMU_DATA));
142+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
143+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
144+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
145+
MODE_HANDLER_ERROR_NO_FCP_DATA));
146+
147+
// 7 - no data available (6. frame) -> timeout
148+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_REMOTE);
149+
EXPECT_TRUE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
150+
MODE_HANDLER_ERROR_NO_IMU_DATA));
151+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
152+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
153+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
154+
MODE_HANDLER_ERROR_NO_FCP_DATA));
155+
156+
// 8 - data available
157+
EXPECT_EQ(mode_handler_handle(&imuData, &remoteData, &flightcomputerSetpoint), MODE_FLIGHTCOMPUTER);
158+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
159+
MODE_HANDLER_ERROR_NO_IMU_DATA));
160+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
161+
MODE_HANDLER_ERROR_NO_REMOTE_DATA));
162+
EXPECT_FALSE(errorHandlerHandle.functionGotCalled<error_handler_handle_warning>(MODE_HANDLER,
163+
MODE_HANDLER_ERROR_NO_FCP_DATA));
8164
}

‎Tests/LowLevel/Mock/Lib/Handler.hpp

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -52,11 +52,12 @@ namespace mock {
5252
*/
5353
template<auto func>
5454
auto functionGotCalled(auto... args) {
55-
auto funcInfo = getFunctionInfo<func>();
55+
auto &funcInfo = getFunctionInfo<func>();
5656
if constexpr (sizeof...(args) > 0) {
57-
static_assert(sizeof...(args) == decltype(funcInfo)::NUM_ARGS);
58-
for (const auto &call : funcInfo.calls) {
59-
if (util::argsMatch(call, args...)) {
57+
static_assert(sizeof...(args) == std::remove_reference_t<decltype(funcInfo)>::NUM_ARGS);
58+
for (auto it = funcInfo.calls.begin(); it != funcInfo.calls.end(); ++it) {
59+
if (util::argsMatch(*it, args...)) {
60+
funcInfo.calls.erase(it);
6061
return true;
6162
}
6263
}

0 commit comments

Comments
 (0)