mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 06:15:16 +03:00
Merge remote-tracking branch 'upstream/master' into lowpass
This commit is contained in:
commit
4f0af41e79
122 changed files with 5252 additions and 1649 deletions
|
@ -23,13 +23,23 @@ USER_INCLUDE_DIR = $(USER_DIR)
|
|||
|
||||
OBJECT_DIR = ../../obj/test
|
||||
|
||||
# Flags passed to the preprocessor.
|
||||
# Set Google Test's header directory as a system directory, such that
|
||||
# the compiler doesn't generate warnings in Google Test headers.
|
||||
CPPFLAGS += -isystem $(GTEST_DIR)/inc
|
||||
COMMON_FLAGS = \
|
||||
-g \
|
||||
-Wall \
|
||||
-Wextra \
|
||||
-pthread \
|
||||
-ggdb3 \
|
||||
-O0 \
|
||||
-DUNIT_TEST \
|
||||
-isystem $(GTEST_DIR)/inc
|
||||
|
||||
# Flags passed to the C compiler.
|
||||
C_FLAGS = $(COMMON_FLAGS) \
|
||||
-std=gnu99
|
||||
|
||||
# Flags passed to the C++ compiler.
|
||||
CXXFLAGS += -g -Wall -Wextra -pthread -ggdb3 -O0 -DUNIT_TEST
|
||||
CXX_FLAGS = $(COMMON_FLAGS) \
|
||||
-std=gnu++98
|
||||
|
||||
# All tests produced by this Makefile. Remember to add new tests you
|
||||
# created to the list.
|
||||
|
@ -37,6 +47,7 @@ TESTS = \
|
|||
battery_unittest \
|
||||
flight_imu_unittest \
|
||||
altitude_hold_unittest \
|
||||
maths_unittest \
|
||||
gps_conversion_unittest \
|
||||
telemetry_hott_unittest \
|
||||
rc_controls_unittest \
|
||||
|
@ -67,12 +78,12 @@ GTEST_SRCS_ = $(GTEST_DIR)/src/*.cc $(GTEST_DIR)/inc/gtest/*.h $(GTEST_HEADERS)
|
|||
# compiles fast and for ordinary users its source rarely changes.
|
||||
$(OBJECT_DIR)/gtest-all.o : $(GTEST_SRCS_)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) -I$(GTEST_DIR) $(CXXFLAGS) -c \
|
||||
$(CXX) $(CXX_FLAGS) -I$(GTEST_DIR) -c \
|
||||
$(GTEST_DIR)/src/gtest-all.cc -o $@
|
||||
|
||||
$(OBJECT_DIR)/gtest_main.o : $(GTEST_SRCS_)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) -I$(GTEST_DIR) $(CXXFLAGS) -c \
|
||||
$(CXX) $(CXX_FLAGS) -I$(GTEST_DIR) -c \
|
||||
$(GTEST_DIR)/src/gtest_main.cc -o $@
|
||||
|
||||
$(OBJECT_DIR)/gtest.a : $(OBJECT_DIR)/gtest-all.o
|
||||
|
@ -88,119 +99,226 @@ $(OBJECT_DIR)/gtest_main.a : $(OBJECT_DIR)/gtest-all.o $(OBJECT_DIR)/gtest_main.
|
|||
# includes in test dir must override includes in user dir
|
||||
TEST_INCLUDE_DIRS := $(TEST_DIR) \
|
||||
$(USER_INCLUDE_DIR)
|
||||
|
||||
|
||||
TEST_CFLAGS = $(addprefix -I,$(TEST_INCLUDE_DIRS))
|
||||
|
||||
$(OBJECT_DIR)/common/maths.o : \
|
||||
$(USER_DIR)/common/maths.c \
|
||||
$(USER_DIR)/common/maths.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@
|
||||
|
||||
|
||||
$(OBJECT_DIR)/sensors/battery.o : $(USER_DIR)/sensors/battery.c $(USER_DIR)/sensors/battery.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/battery.c -o $@
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/battery.c -o $@
|
||||
|
||||
$(OBJECT_DIR)/battery_unittest.o : \
|
||||
$(TEST_DIR)/battery_unittest.cc \
|
||||
$(USER_DIR)/sensors/battery.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/battery_unittest.o : $(TEST_DIR)/battery_unittest.cc \
|
||||
$(USER_DIR)/sensors/battery.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/battery_unittest.cc -o $@
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/battery_unittest.cc -o $@
|
||||
|
||||
battery_unittest : $(OBJECT_DIR)/sensors/battery.o $(OBJECT_DIR)/battery_unittest.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
battery_unittest : \
|
||||
$(OBJECT_DIR)/sensors/battery.o \
|
||||
$(OBJECT_DIR)/common/maths.o \
|
||||
$(OBJECT_DIR)/battery_unittest.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
$(OBJECT_DIR)/flight/imu.o : \
|
||||
$(USER_DIR)/flight/imu.c \
|
||||
$(USER_DIR)/flight/imu.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/flight/imu.o : $(USER_DIR)/flight/imu.c $(USER_DIR)/flight/imu.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/imu.c -o $@
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/imu.c -o $@
|
||||
|
||||
$(OBJECT_DIR)/flight_imu_unittest.o : \
|
||||
$(TEST_DIR)/flight_imu_unittest.cc \
|
||||
$(USER_DIR)/flight/imu.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/flight_imu_unittest.o : $(TEST_DIR)/flight_imu_unittest.cc \
|
||||
$(USER_DIR)/flight/imu.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -o $@
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -o $@
|
||||
|
||||
flight_imu_unittest : $(OBJECT_DIR)/flight/imu.o $(OBJECT_DIR)/flight/altitudehold.o $(OBJECT_DIR)/flight_imu_unittest.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
flight_imu_unittest : \
|
||||
$(OBJECT_DIR)/flight/imu.o \
|
||||
$(OBJECT_DIR)/flight/altitudehold.o \
|
||||
$(OBJECT_DIR)/flight_imu_unittest.o \
|
||||
$(OBJECT_DIR)/common/maths.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
$(OBJECT_DIR)/maths_unittest.o : \
|
||||
$(TEST_DIR)/maths_unittest.cc \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/flight/altitudehold.o : $(USER_DIR)/flight/altitudehold.c $(USER_DIR)/flight/altitudehold.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/maths_unittest.cc -o $@
|
||||
|
||||
maths_unittest : \
|
||||
$(OBJECT_DIR)/maths_unittest.o \
|
||||
$(OBJECT_DIR)/common/maths.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
|
||||
|
||||
$(OBJECT_DIR)/flight/altitudehold.o : \
|
||||
$(USER_DIR)/flight/altitudehold.c \
|
||||
$(USER_DIR)/flight/altitudehold.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/altitude_hold_unittest.o : $(TEST_DIR)/altitude_hold_unittest.cc \
|
||||
$(USER_DIR)/flight/altitudehold.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/altitude_hold_unittest.cc -o $@
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@
|
||||
|
||||
altitude_hold_unittest : $(OBJECT_DIR)/flight/altitudehold.o $(OBJECT_DIR)/altitude_hold_unittest.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
$(OBJECT_DIR)/altitude_hold_unittest.o : \
|
||||
$(TEST_DIR)/altitude_hold_unittest.cc \
|
||||
$(USER_DIR)/flight/altitudehold.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
|
||||
|
||||
$(OBJECT_DIR)/flight/gps_conversion.o : $(USER_DIR)/flight/gps_conversion.c $(USER_DIR)/flight/gps_conversion.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/altitude_hold_unittest.cc -o $@
|
||||
|
||||
altitude_hold_unittest : \
|
||||
$(OBJECT_DIR)/flight/altitudehold.o \
|
||||
$(OBJECT_DIR)/altitude_hold_unittest.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
|
||||
$(OBJECT_DIR)/flight/gps_conversion.o : \
|
||||
$(USER_DIR)/flight/gps_conversion.c \
|
||||
$(USER_DIR)/flight/gps_conversion.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/gps_conversion_unittest.o : $(TEST_DIR)/gps_conversion_unittest.cc \
|
||||
$(USER_DIR)/flight/gps_conversion.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@
|
||||
|
||||
gps_conversion_unittest : $(OBJECT_DIR)/flight/gps_conversion.o $(OBJECT_DIR)/gps_conversion_unittest.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
$(OBJECT_DIR)/gps_conversion_unittest.o : \
|
||||
$(TEST_DIR)/gps_conversion_unittest.cc \
|
||||
$(USER_DIR)/flight/gps_conversion.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
|
||||
|
||||
$(OBJECT_DIR)/telemetry/hott.o : $(USER_DIR)/telemetry/hott.c $(USER_DIR)/telemetry/hott.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/hott.c -o $@
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@
|
||||
|
||||
gps_conversion_unittest : \
|
||||
$(OBJECT_DIR)/flight/gps_conversion.o \
|
||||
$(OBJECT_DIR)/gps_conversion_unittest.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
|
||||
|
||||
$(OBJECT_DIR)/telemetry/hott.o : \
|
||||
$(USER_DIR)/telemetry/hott.c \
|
||||
$(USER_DIR)/telemetry/hott.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/telemetry_hott_unittest.o : $(TEST_DIR)/telemetry_hott_unittest.cc \
|
||||
$(USER_DIR)/telemetry/hott.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_hott_unittest.cc -o $@
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/hott.c -o $@
|
||||
|
||||
telemetry_hott_unittest :$(OBJECT_DIR)/telemetry/hott.o $(OBJECT_DIR)/telemetry_hott_unittest.o $(OBJECT_DIR)/flight/gps_conversion.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
|
||||
|
||||
$(OBJECT_DIR)/io/rc_controls.o : $(USER_DIR)/io/rc_controls.c $(USER_DIR)/io/rc_controls.h $(GTEST_HEADERS)
|
||||
$(OBJECT_DIR)/telemetry_hott_unittest.o : \
|
||||
$(TEST_DIR)/telemetry_hott_unittest.cc \
|
||||
$(USER_DIR)/telemetry/hott.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/rc_controls.c -o $@
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_hott_unittest.cc -o $@
|
||||
|
||||
telemetry_hott_unittest : \
|
||||
$(OBJECT_DIR)/telemetry/hott.o \
|
||||
$(OBJECT_DIR)/telemetry_hott_unittest.o \
|
||||
$(OBJECT_DIR)/flight/gps_conversion.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
|
||||
|
||||
$(OBJECT_DIR)/io/rc_controls.o : \
|
||||
$(USER_DIR)/io/rc_controls.c \
|
||||
$(USER_DIR)/io/rc_controls.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/rc_controls_unittest.o : $(TEST_DIR)/rc_controls_unittest.cc \
|
||||
$(USER_DIR)/io/rc_controls.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/rc_controls.c -o $@
|
||||
|
||||
rc_controls_unittest :$(OBJECT_DIR)/io/rc_controls.o $(OBJECT_DIR)/rc_controls_unittest.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
$(OBJECT_DIR)/rc_controls_unittest.o : \
|
||||
$(TEST_DIR)/rc_controls_unittest.cc \
|
||||
$(USER_DIR)/io/rc_controls.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
|
||||
$(OBJECT_DIR)/io/ledstrip.o : $(USER_DIR)/io/ledstrip.c $(USER_DIR)/io/ledstrip.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@
|
||||
|
||||
$(OBJECT_DIR)/ledstrip_unittest.o : $(TEST_DIR)/ledstrip_unittest.cc \
|
||||
$(USER_DIR)/io/ledstrip.h $(GTEST_HEADERS)
|
||||
rc_controls_unittest : \
|
||||
$(OBJECT_DIR)/common/maths.o \
|
||||
$(OBJECT_DIR)/io/rc_controls.o \
|
||||
$(OBJECT_DIR)/rc_controls_unittest.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
|
||||
$(OBJECT_DIR)/io/ledstrip.o : \
|
||||
$(USER_DIR)/io/ledstrip.c \
|
||||
$(USER_DIR)/io/ledstrip.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@
|
||||
|
||||
ledstrip_unittest :$(OBJECT_DIR)/io/ledstrip.o $(OBJECT_DIR)/ledstrip_unittest.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
$(OBJECT_DIR)/ledstrip_unittest.o : \
|
||||
$(TEST_DIR)/ledstrip_unittest.cc \
|
||||
$(USER_DIR)/io/ledstrip.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
|
||||
|
||||
$(OBJECT_DIR)/drivers/light_ws2811strip.o : $(USER_DIR)/drivers/light_ws2811strip.c $(USER_DIR)/drivers/light_ws2811strip.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/light_ws2811strip.c -o $@
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@
|
||||
|
||||
ledstrip_unittest : \
|
||||
$(OBJECT_DIR)/io/ledstrip.o \
|
||||
$(OBJECT_DIR)/ledstrip_unittest.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
|
||||
|
||||
$(OBJECT_DIR)/drivers/light_ws2811strip.o : \
|
||||
$(USER_DIR)/drivers/light_ws2811strip.c \
|
||||
$(USER_DIR)/drivers/light_ws2811strip.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
$(OBJECT_DIR)/ws2811_unittest.o : $(TEST_DIR)/ws2811_unittest.cc \
|
||||
$(USER_DIR)/drivers/light_ws2811strip.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ws2811_unittest.cc -o $@
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/light_ws2811strip.c -o $@
|
||||
|
||||
ws2811_unittest :$(OBJECT_DIR)/drivers/light_ws2811strip.o $(OBJECT_DIR)/ws2811_unittest.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
$(OBJECT_DIR)/ws2811_unittest.o : \
|
||||
$(TEST_DIR)/ws2811_unittest.cc \
|
||||
$(USER_DIR)/drivers/light_ws2811strip.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ws2811_unittest.cc -o $@
|
||||
|
||||
ws2811_unittest : \
|
||||
$(OBJECT_DIR)/drivers/light_ws2811strip.o \
|
||||
$(OBJECT_DIR)/ws2811_unittest.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
|
@ -213,3 +331,8 @@ $(OBJECT_DIR)/mixer_unittest.o : $(TEST_DIR)/mixer_unittest.cc $(USER_DIR)/fligh
|
|||
mixer_unittest : $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/mixer_unittest.o $(OBJECT_DIR)/gtest_main.a
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
test: $(TESTS)
|
||||
set -e && for test in $(TESTS) ; do \
|
||||
$(OBJECT_DIR)/$$test; \
|
||||
done
|
||||
|
||||
|
|
|
@ -27,6 +27,7 @@ extern "C" {
|
|||
#include "flight/flight.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
|
@ -53,6 +54,7 @@ extern "C" {
|
|||
|
||||
extern "C" {
|
||||
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
|
||||
uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations);
|
||||
}
|
||||
|
||||
typedef struct inclinationExpectation_s {
|
||||
|
@ -89,6 +91,36 @@ TEST(AltitudeHoldTest, IsThrustFacingDownwards)
|
|||
}
|
||||
}
|
||||
|
||||
typedef struct inclinationAngleExpectations_s {
|
||||
rollAndPitchInclination_t inclination;
|
||||
uint16_t expected_angle;
|
||||
} inclinationAngleExpectations_t;
|
||||
|
||||
TEST(AltitudeHoldTest, TestCalculateTiltAngle)
|
||||
{
|
||||
inclinationAngleExpectations_t inclinationAngleExpectations[] = {
|
||||
{ {0, 0}, 0},
|
||||
{ {1, 0}, 1},
|
||||
{ {0, 1}, 1},
|
||||
{ {0, -1}, 1},
|
||||
{ {-1, 0}, 1},
|
||||
{ {-1, -2}, 2},
|
||||
{ {-2, -1}, 2},
|
||||
{ {1, 2}, 2},
|
||||
{ {2, 1}, 2}
|
||||
};
|
||||
|
||||
rollAndPitchInclination_t inclination = {0, 0};
|
||||
uint16_t tilt_angle = calculateTiltAngle(&inclination);
|
||||
EXPECT_EQ(tilt_angle, 0);
|
||||
|
||||
for (uint8_t i = 0; i < 9; i++) {
|
||||
inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i];
|
||||
uint16_t result = calculateTiltAngle(&expectation->inclination);
|
||||
EXPECT_EQ(expectation->expected_angle, result);
|
||||
}
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
|
|
|
@ -73,6 +73,9 @@ TEST(BatteryTest, BatteryADCToVoltage)
|
|||
|
||||
extern "C" {
|
||||
|
||||
uint8_t armingFlags = 0;
|
||||
int16_t rcCommand[4] = {0,0,0,0};
|
||||
|
||||
uint16_t adcGetChannel(uint8_t channel)
|
||||
{
|
||||
UNUSED(channel);
|
||||
|
|
|
@ -24,10 +24,13 @@
|
|||
|
||||
extern "C" {
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "flight/flight.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/compass.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/acceleration.h"
|
||||
|
@ -46,10 +49,23 @@ extern "C" {
|
|||
#define UPWARDS_THRUST false
|
||||
|
||||
|
||||
TEST(FlightImuTest, Placeholder)
|
||||
TEST(FlightImuTest, TestCalculateHeading)
|
||||
{
|
||||
// TODO test things
|
||||
EXPECT_EQ(true, true);
|
||||
//TODO: Add test cases using the Z dimension.
|
||||
t_fp_vector north = {.A={1.0f, 0.0f, 0.0f}};
|
||||
EXPECT_EQ(calculateHeading(&north), 0);
|
||||
|
||||
t_fp_vector east = {.A={0.0f, 1.0f, 0.0f}};
|
||||
EXPECT_EQ(calculateHeading(&east), 90);
|
||||
|
||||
t_fp_vector south = {.A={-1.0f, 0.0f, 0.0f}};
|
||||
EXPECT_EQ(calculateHeading(&south), 180);
|
||||
|
||||
t_fp_vector west = {.A={0.0f, -1.0f, 0.0f}};
|
||||
EXPECT_EQ(calculateHeading(&west), 270);
|
||||
|
||||
t_fp_vector north_east = {.A={1.0f, 1.0f, 0.0f}};
|
||||
EXPECT_EQ(calculateHeading(&north_east), 45);
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
@ -83,18 +99,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
|
|||
UNUSED(rollAndPitchTrims);
|
||||
}
|
||||
|
||||
int32_t applyDeadband(int32_t, int32_t) { return 0; }
|
||||
|
||||
uint32_t micros(void) { return 0; }
|
||||
bool isBaroCalibrationComplete(void) { return true; }
|
||||
void performBaroCalibrationCycle(void) {}
|
||||
int32_t baroCalculateAltitude(void) { return 0; }
|
||||
int constrain(int amt, int low, int high)
|
||||
{
|
||||
UNUSED(amt);
|
||||
UNUSED(low);
|
||||
UNUSED(high);
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -18,7 +18,10 @@
|
|||
#include <stdint.h>
|
||||
|
||||
#include <limits.h>
|
||||
#include "flight/gps_conversion.h"
|
||||
|
||||
extern "C" {
|
||||
#include "flight/gps_conversion.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
@ -33,7 +36,7 @@ TEST(GpsConversionTest, GPSCoordToDegrees_BadString)
|
|||
}
|
||||
|
||||
typedef struct gpsConversionExpectation_s {
|
||||
char *coord;
|
||||
const char *coord;
|
||||
uint32_t degrees;
|
||||
} gpsConversionExpectation_t;
|
||||
|
||||
|
|
|
@ -19,87 +19,85 @@
|
|||
|
||||
#include <limits.h>
|
||||
|
||||
#include "build_config.h"
|
||||
extern "C" {
|
||||
#include "build_config.h"
|
||||
|
||||
#include "common/color.h"
|
||||
#include "common/axis.h"
|
||||
#include "flight/flight.h"
|
||||
#include "common/color.h"
|
||||
#include "common/axis.h"
|
||||
#include "flight/flight.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "config/runtime_config.h"
|
||||
#include "config/config.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "config/runtime_config.h"
|
||||
#include "config/config.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "drivers/light_ws2811strip.h"
|
||||
#include "io/ledstrip.h"
|
||||
#include "io/rc_controls.h"
|
||||
|
||||
#include "drivers/light_ws2811strip.h"
|
||||
#include "io/ledstrip.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
extern ledConfig_t *ledConfigs;
|
||||
extern uint8_t highestYValueForNorth;
|
||||
extern uint8_t lowestYValueForSouth;
|
||||
extern uint8_t highestXValueForWest;
|
||||
extern uint8_t lowestXValueForEast;
|
||||
extern uint8_t ledGridWidth;
|
||||
extern uint8_t ledGridHeight;
|
||||
extern "C" {
|
||||
extern ledConfig_t *ledConfigs;
|
||||
extern uint8_t highestYValueForNorth;
|
||||
extern uint8_t lowestYValueForSouth;
|
||||
extern uint8_t highestXValueForWest;
|
||||
extern uint8_t lowestXValueForEast;
|
||||
extern uint8_t ledGridWidth;
|
||||
extern uint8_t ledGridHeight;
|
||||
|
||||
void determineLedStripDimensions(void);
|
||||
void determineOrientationLimits(void);
|
||||
void determineLedStripDimensions(void);
|
||||
void determineOrientationLimits(void);
|
||||
|
||||
ledConfig_t systemLedConfigs[MAX_LED_STRIP_LENGTH];
|
||||
ledConfig_t systemLedConfigs[MAX_LED_STRIP_LENGTH];
|
||||
}
|
||||
|
||||
TEST(LedStripTest, parseLedStripConfig)
|
||||
{
|
||||
/*
|
||||
* 0..5 - rear right cluster, 0..2 rear 3..5 right
|
||||
* 6..11 - front right cluster, 6..8 rear, 9..11 front
|
||||
* 12..15 - front center cluster
|
||||
* 16..21 - front left cluster, 16..18 front, 19..21 rear
|
||||
* 22..27 - rear left cluster, 22..24 left, 25..27 rear
|
||||
*/
|
||||
|
||||
// given
|
||||
static const ledConfig_t expectedLedStripConfig[WS2811_LED_STRIP_LENGTH] = {
|
||||
{ CALCULATE_LED_XY( 9, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY(10, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY(11, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY(11, 11), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY(10, 10), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 9, 9), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 9, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
|
||||
{ CALCULATE_LED_XY(10, 5), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY(11, 4), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY(12, 3), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY(12, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY(11, 1), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY(10, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY(10, 5), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY(11, 4), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY(12, 3), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY(12, 2), 0, LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY(11, 1), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY(10, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
|
||||
{ CALCULATE_LED_XY( 7, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 6, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 5, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 4, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 6, 0), 1, LED_DIRECTION_NORTH | LED_FUNCTION_COLOR | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 5, 0), 1, LED_DIRECTION_NORTH | LED_FUNCTION_COLOR | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 4, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
|
||||
{ CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 1, 1), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 4), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 2, 5), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 3), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 2, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
|
||||
{ CALCULATE_LED_XY( 2, 9), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 1, 10), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 2, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 2, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
|
||||
{ 0, 0 },
|
||||
{ 0, 0 },
|
||||
{ 0, 0 },
|
||||
{ 0, 0 },
|
||||
{ CALCULATE_LED_XY( 7, 7), 14, LED_FUNCTION_THRUST_RING },
|
||||
{ CALCULATE_LED_XY( 8, 7), 15, LED_FUNCTION_THRUST_RING },
|
||||
{ CALCULATE_LED_XY( 8, 8), 14, LED_FUNCTION_THRUST_RING },
|
||||
{ CALCULATE_LED_XY( 7, 8), 15, LED_FUNCTION_THRUST_RING },
|
||||
|
||||
{ 0, 0, 0 },
|
||||
{ 0, 0, 0 },
|
||||
};
|
||||
|
||||
// and
|
||||
|
@ -107,42 +105,46 @@ TEST(LedStripTest, parseLedStripConfig)
|
|||
// Spider quad
|
||||
|
||||
// right rear cluster
|
||||
"9,9:S:FW",
|
||||
"10,10:S:FW",
|
||||
"11,11:S:IA",
|
||||
"11,11:E:IA",
|
||||
"10,10:E:F",
|
||||
"9,9:E:F",
|
||||
"9,9:S:FW:0",
|
||||
"10,10:S:FW:0",
|
||||
"11,11:S:IA:0",
|
||||
"11,11:E:IA:0",
|
||||
"10,10:E:F:0",
|
||||
|
||||
// right front cluster
|
||||
"10,5:S:F",
|
||||
"11,4:S:F",
|
||||
"12,3:S:IA",
|
||||
"12,2:N:IA",
|
||||
"11,1:N:F",
|
||||
"10,0:N:F",
|
||||
"10,5:S:F:0",
|
||||
"11,4:S:F:0",
|
||||
"12,3:S:IA:0",
|
||||
"12,2:N:IA:0",
|
||||
"11,1:N:F:0",
|
||||
"10,0:N:F:0",
|
||||
|
||||
// center front cluster
|
||||
"7,0:N:FW",
|
||||
"6,0:N:FW",
|
||||
"5,0:N:FW",
|
||||
"4,0:N:FW",
|
||||
"7,0:N:FW:0",
|
||||
"6,0:N:CW:1",
|
||||
"5,0:N:CW:1",
|
||||
"4,0:N:FW:0",
|
||||
|
||||
// left front cluster
|
||||
"2,0:N:F",
|
||||
"1,1:N:F",
|
||||
"0,2:N:IA",
|
||||
"0,3:W:IA",
|
||||
"1,4:W:F",
|
||||
"2,5:W:F",
|
||||
"2,0:N:F:0",
|
||||
"1,1:N:F:0",
|
||||
"0,2:N:IA:0",
|
||||
"0,3:W:IA:0",
|
||||
"1,4:W:F:0",
|
||||
"2,5:W:F:0",
|
||||
|
||||
// left rear cluster
|
||||
"2,9:W:F",
|
||||
"1,10:W:F",
|
||||
"0,11:W:IA",
|
||||
"0,11:S:IA",
|
||||
"1,10:S:FW",
|
||||
"2,9:S:FW"
|
||||
"1,10:W:F:0",
|
||||
"0,11:W:IA:0",
|
||||
"0,11:S:IA:0",
|
||||
"1,10:S:FW:0",
|
||||
"2,9:S:FW:0",
|
||||
|
||||
// thrust ring
|
||||
"7,7::R:14",
|
||||
"8,7::R:15",
|
||||
"8,8::R:14",
|
||||
"7,8::R:15"
|
||||
};
|
||||
// and
|
||||
memset(&systemLedConfigs, 0, sizeof(systemLedConfigs));
|
||||
|
@ -158,7 +160,8 @@ TEST(LedStripTest, parseLedStripConfig)
|
|||
|
||||
// then
|
||||
EXPECT_EQ(true, ok);
|
||||
EXPECT_EQ(28, ledCount);
|
||||
EXPECT_EQ(30, ledCount);
|
||||
EXPECT_EQ(4, ledsInRingCount);
|
||||
|
||||
|
||||
// and
|
||||
|
@ -167,6 +170,7 @@ TEST(LedStripTest, parseLedStripConfig)
|
|||
|
||||
EXPECT_EQ(expectedLedStripConfig[index].xy, ledConfigs[index].xy);
|
||||
EXPECT_EQ(expectedLedStripConfig[index].flags, ledConfigs[index].flags);
|
||||
EXPECT_EQ(expectedLedStripConfig[index].color, ledConfigs[index].color);
|
||||
}
|
||||
|
||||
// then
|
||||
|
@ -188,14 +192,14 @@ TEST(LedStripTest, smallestGridWithCenter)
|
|||
|
||||
// and
|
||||
static const ledConfig_t testLedConfigs[] = {
|
||||
{ CALCULATE_LED_XY( 2, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 2, 1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }
|
||||
{ CALCULATE_LED_XY( 2, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 2, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 2), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }
|
||||
};
|
||||
memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs));
|
||||
|
||||
|
@ -224,10 +228,10 @@ TEST(LedStripTest, smallestGrid)
|
|||
|
||||
// and
|
||||
static const ledConfig_t testLedConfigs[] = {
|
||||
{ CALCULATE_LED_XY( 1, 1), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 1), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
};
|
||||
memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs));
|
||||
|
||||
|
@ -249,45 +253,45 @@ TEST(LedStripTest, smallestGrid)
|
|||
}
|
||||
|
||||
/*
|
||||
{ CALCULATE_LED_XY( 1, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 1, 14), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE },
|
||||
|
||||
{ CALCULATE_LED_XY( 0, 13), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 12), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 13), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 12), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
|
||||
{ CALCULATE_LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 10), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 9), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 8), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 7), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 6), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 5), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 4), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 9), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 6), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 0, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 0, 3), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE },
|
||||
|
||||
{ CALCULATE_LED_XY( 0, 2), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
|
||||
{ CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 3, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 3, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
|
||||
{ CALCULATE_LED_XY( 4, 1), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 4, 2), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 4, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 4, 2), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
|
||||
{ CALCULATE_LED_XY( 4, 3), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 4), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 5), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 6), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 4, 7), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 4, 8), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 4, 9), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 10), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 11), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 3), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 4), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 5), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 6), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 4, 7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 4, 8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
|
||||
{ CALCULATE_LED_XY( 4, 9), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
{ CALCULATE_LED_XY( 4, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE },
|
||||
|
||||
{ CALCULATE_LED_XY( 4, 12), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 4, 13), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 4, 12), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 4, 13), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
|
||||
{ CALCULATE_LED_XY( 3, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
{ CALCULATE_LED_XY( 3, 14), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
|
||||
|
||||
*/
|
||||
|
||||
|
@ -312,7 +316,7 @@ TEST(ColorTest, parseColor)
|
|||
{ 333, 22, 1 }
|
||||
};
|
||||
|
||||
char *testColors[TEST_COLOR_COUNT] = {
|
||||
const char *testColors[TEST_COLOR_COUNT] = {
|
||||
"0,0,0",
|
||||
"1,1,1",
|
||||
"359,255,255",
|
||||
|
@ -336,12 +340,19 @@ TEST(ColorTest, parseColor)
|
|||
}
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
|
||||
uint8_t armingFlags = 0;
|
||||
uint16_t flightModeFlags = 0;
|
||||
int16_t rcCommand[4];
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint32_t rcModeActivationMask;
|
||||
|
||||
batteryState_e calculateBatteryState(void) {
|
||||
return BATTERY_OK;
|
||||
}
|
||||
|
||||
void ws2811LedStripInit(void) {}
|
||||
void ws2811UpdateStrip(void) {}
|
||||
|
||||
void setLedValue(uint16_t index, const uint8_t value) {
|
||||
|
@ -399,3 +410,5 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
|
|||
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
147
src/test/unit/maths_unittest.cc
Normal file
147
src/test/unit/maths_unittest.cc
Normal file
|
@ -0,0 +1,147 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
#define BARO
|
||||
|
||||
extern "C" {
|
||||
#include "common/maths.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
TEST(MathsUnittest, TestConstrain)
|
||||
{
|
||||
// Within bounds
|
||||
EXPECT_EQ(constrain(0, 0, 0), 0);
|
||||
EXPECT_EQ(constrain(1, 1, 1), 1);
|
||||
EXPECT_EQ(constrain(1, 0, 2), 1);
|
||||
|
||||
// Equal to bottom bound.
|
||||
EXPECT_EQ(constrain(1, 1, 2), 1);
|
||||
// Equal to top bound.
|
||||
EXPECT_EQ(constrain(1, 0, 1), 1);
|
||||
|
||||
// Equal to both bottom and top bound.
|
||||
EXPECT_EQ(constrain(1, 1, 1), 1);
|
||||
|
||||
// Above top bound.
|
||||
EXPECT_EQ(constrain(2, 0, 1), 1);
|
||||
// Below bottom bound.
|
||||
EXPECT_EQ(constrain(0, 1, 2), 1);
|
||||
}
|
||||
|
||||
TEST(MathsUnittest, TestConstrainNegatives)
|
||||
{
|
||||
// Within bounds.
|
||||
EXPECT_EQ(constrain(-1, -1, -1), -1);
|
||||
EXPECT_EQ(constrain(-1, -2, 0), -1);
|
||||
|
||||
// Equal to bottom bound.
|
||||
EXPECT_EQ(constrain(-1, -1, 0), -1);
|
||||
// Equal to top bound.
|
||||
EXPECT_EQ(constrain(-1, -2, -1), -1);
|
||||
|
||||
// Equal to both bottom and top bound.
|
||||
EXPECT_EQ(constrain(-1, -1, -1), -1);
|
||||
|
||||
// Above top bound.
|
||||
EXPECT_EQ(constrain(-1, -3, -2), -2);
|
||||
// Below bottom bound.
|
||||
EXPECT_EQ(constrain(-3, -2, -1), -2);
|
||||
}
|
||||
|
||||
TEST(MathsUnittest, TestConstrainf)
|
||||
{
|
||||
// Within bounds.
|
||||
EXPECT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f);
|
||||
|
||||
// Equal to bottom bound.
|
||||
EXPECT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f);
|
||||
// Equal to top bound.
|
||||
EXPECT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f);
|
||||
|
||||
// Equal to both bottom and top bound.
|
||||
EXPECT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f);
|
||||
|
||||
// Above top bound.
|
||||
EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
|
||||
// Below bottom bound.
|
||||
EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
|
||||
|
||||
// Above bouth bounds.
|
||||
EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
|
||||
// Below bouth bounds.
|
||||
EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
|
||||
}
|
||||
|
||||
TEST(MathsUnittest, TestDegreesToRadians)
|
||||
{
|
||||
EXPECT_EQ(degreesToRadians(0), 0.0f);
|
||||
EXPECT_EQ(degreesToRadians(90), 0.5f * M_PIf);
|
||||
EXPECT_EQ(degreesToRadians(180), M_PIf);
|
||||
EXPECT_EQ(degreesToRadians(-180), - M_PIf);
|
||||
}
|
||||
|
||||
TEST(MathsUnittest, TestApplyDeadband)
|
||||
{
|
||||
EXPECT_EQ(applyDeadband(0, 0), 0);
|
||||
EXPECT_EQ(applyDeadband(1, 0), 1);
|
||||
EXPECT_EQ(applyDeadband(-1, 0), -1);
|
||||
|
||||
EXPECT_EQ(applyDeadband(0, 10), 0);
|
||||
EXPECT_EQ(applyDeadband(1, 10), 0);
|
||||
EXPECT_EQ(applyDeadband(10, 10), 0);
|
||||
|
||||
EXPECT_EQ(applyDeadband(11, 10), 1);
|
||||
EXPECT_EQ(applyDeadband(-11, 10), -1);
|
||||
}
|
||||
|
||||
void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b)
|
||||
{
|
||||
EXPECT_EQ(a->X, b->X);
|
||||
EXPECT_EQ(a->Y, b->Y);
|
||||
EXPECT_EQ(a->Z, b->Z);
|
||||
}
|
||||
|
||||
TEST(MathsUnittest, TestRotateVectorWithNoAngle)
|
||||
{
|
||||
fp_vector vector = {1.0f, 0.0f, 0.0f};
|
||||
fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}};
|
||||
|
||||
rotateV(&vector, &euler_angles);
|
||||
fp_vector expected_result = {1.0f, 0.0f, 0.0f};
|
||||
|
||||
expectVectorsAreEqual(&vector, &expected_result);
|
||||
}
|
||||
|
||||
TEST(MathsUnittest, TestRotateVectorAroundAxis)
|
||||
{
|
||||
// Rotate a vector <1, 0, 0> around an each axis x y and z.
|
||||
fp_vector vector = {1.0f, 0.0f, 0.0f};
|
||||
fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}};
|
||||
|
||||
rotateV(&vector, &euler_angles);
|
||||
fp_vector expected_result = {1.0f, 0.0f, 0.0f};
|
||||
|
||||
expectVectorsAreEqual(&vector, &expected_result);
|
||||
}
|
|
@ -17,6 +17,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define MAG
|
||||
#define BARO
|
||||
#define GPS
|
||||
#define TELEMETRY
|
||||
|
@ -24,3 +25,4 @@
|
|||
|
||||
#define SERIAL_PORT_COUNT 4
|
||||
|
||||
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6
|
||||
|
|
|
@ -17,7 +17,10 @@
|
|||
#include <stdint.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
extern "C" {
|
||||
#include "platform.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/axis.h"
|
||||
#include "flight/flight.h"
|
||||
|
||||
|
@ -29,15 +32,7 @@ extern "C" {
|
|||
#include "gtest/gtest.h"
|
||||
|
||||
extern "C" {
|
||||
int constrain(int amt, int low, int high)
|
||||
{
|
||||
if (amt < low)
|
||||
return low;
|
||||
else if (amt > high)
|
||||
return high;
|
||||
else
|
||||
return amt;
|
||||
}
|
||||
extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile);
|
||||
}
|
||||
|
||||
TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde)
|
||||
|
@ -479,6 +474,215 @@ TEST(RcControlsTest, processRcRateProfileAdjustments)
|
|||
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
|
||||
}
|
||||
|
||||
static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidYawPAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_YAW_P,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidYawIAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_YAW_I,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidYawDAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_YAW_D,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
TEST(RcControlsTest, processPIDIncreasePidController0)
|
||||
{
|
||||
// given
|
||||
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
|
||||
memset(&modeActivationConditions, 0, sizeof (modeActivationConditions));
|
||||
|
||||
escAndServoConfig_t escAndServoConfig;
|
||||
memset(&escAndServoConfig, 0, sizeof (escAndServoConfig));
|
||||
|
||||
pidProfile_t pidProfile;
|
||||
memset(&pidProfile, 0, sizeof (pidProfile));
|
||||
pidProfile.pidController = 0;
|
||||
pidProfile.P8[PIDPITCH] = 0;
|
||||
pidProfile.P8[PIDROLL] = 5;
|
||||
pidProfile.P8[YAW] = 7;
|
||||
pidProfile.I8[PIDPITCH] = 10;
|
||||
pidProfile.I8[PIDROLL] = 15;
|
||||
pidProfile.I8[YAW] = 17;
|
||||
pidProfile.D8[PIDPITCH] = 20;
|
||||
pidProfile.D8[PIDROLL] = 25;
|
||||
pidProfile.D8[YAW] = 27;
|
||||
|
||||
// and
|
||||
controlRateConfig_t controlRateConfig;
|
||||
memset(&controlRateConfig, 0, sizeof (controlRateConfig));
|
||||
|
||||
// and
|
||||
memset(&rxConfig, 0, sizeof (rxConfig));
|
||||
rxConfig.mincheck = DEFAULT_MIN_CHECK;
|
||||
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
|
||||
rxConfig.midrc = 1500;
|
||||
|
||||
adjustmentStateMask = 0;
|
||||
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
|
||||
|
||||
configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
|
||||
configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
|
||||
configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
|
||||
configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig);
|
||||
configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig);
|
||||
configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig);
|
||||
|
||||
// and
|
||||
uint8_t index;
|
||||
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
|
||||
rcData[index] = PWM_RANGE_MIDDLE;
|
||||
}
|
||||
|
||||
// and
|
||||
resetCallCounters();
|
||||
resetMillis();
|
||||
|
||||
// and
|
||||
rcData[AUX1] = PWM_RANGE_MAX;
|
||||
rcData[AUX2] = PWM_RANGE_MAX;
|
||||
rcData[AUX3] = PWM_RANGE_MAX;
|
||||
|
||||
// and
|
||||
uint8_t expectedAdjustmentStateMask =
|
||||
(1 << 0) |
|
||||
(1 << 1) |
|
||||
(1 << 2) |
|
||||
(1 << 3) |
|
||||
(1 << 4) |
|
||||
(1 << 5);
|
||||
|
||||
// when
|
||||
useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
|
||||
processRcAdjustments(&controlRateConfig, &rxConfig);
|
||||
|
||||
// then
|
||||
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6);
|
||||
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
|
||||
|
||||
// and
|
||||
EXPECT_EQ(1, pidProfile.P8[PIDPITCH]);
|
||||
EXPECT_EQ(6, pidProfile.P8[PIDROLL]);
|
||||
EXPECT_EQ(8, pidProfile.P8[YAW]);
|
||||
EXPECT_EQ(11, pidProfile.I8[PIDPITCH]);
|
||||
EXPECT_EQ(16, pidProfile.I8[PIDROLL]);
|
||||
EXPECT_EQ(18, pidProfile.I8[YAW]);
|
||||
EXPECT_EQ(21, pidProfile.D8[PIDPITCH]);
|
||||
EXPECT_EQ(26, pidProfile.D8[PIDROLL]);
|
||||
EXPECT_EQ(28, pidProfile.D8[YAW]);
|
||||
}
|
||||
|
||||
TEST(RcControlsTest, processPIDIncreasePidController2)
|
||||
{
|
||||
// given
|
||||
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
|
||||
memset(&modeActivationConditions, 0, sizeof (modeActivationConditions));
|
||||
|
||||
escAndServoConfig_t escAndServoConfig;
|
||||
memset(&escAndServoConfig, 0, sizeof (escAndServoConfig));
|
||||
|
||||
pidProfile_t pidProfile;
|
||||
memset(&pidProfile, 0, sizeof (pidProfile));
|
||||
pidProfile.pidController = 2;
|
||||
pidProfile.P_f[PIDPITCH] = 0.0f;
|
||||
pidProfile.P_f[PIDROLL] = 5.0f;
|
||||
pidProfile.P_f[PIDYAW] = 7.0f;
|
||||
pidProfile.I_f[PIDPITCH] = 10.0f;
|
||||
pidProfile.I_f[PIDROLL] = 15.0f;
|
||||
pidProfile.I_f[PIDYAW] = 17.0f;
|
||||
pidProfile.D_f[PIDPITCH] = 20.0f;
|
||||
pidProfile.D_f[PIDROLL] = 25.0f;
|
||||
pidProfile.D_f[PIDYAW] = 27.0f;
|
||||
|
||||
// and
|
||||
controlRateConfig_t controlRateConfig;
|
||||
memset(&controlRateConfig, 0, sizeof (controlRateConfig));
|
||||
|
||||
// and
|
||||
memset(&rxConfig, 0, sizeof (rxConfig));
|
||||
rxConfig.mincheck = DEFAULT_MIN_CHECK;
|
||||
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
|
||||
rxConfig.midrc = 1500;
|
||||
|
||||
adjustmentStateMask = 0;
|
||||
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
|
||||
|
||||
configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
|
||||
configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
|
||||
configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
|
||||
configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig);
|
||||
configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig);
|
||||
configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig);
|
||||
|
||||
// and
|
||||
uint8_t index;
|
||||
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
|
||||
rcData[index] = PWM_RANGE_MIDDLE;
|
||||
}
|
||||
|
||||
// and
|
||||
resetCallCounters();
|
||||
resetMillis();
|
||||
|
||||
// and
|
||||
rcData[AUX1] = PWM_RANGE_MAX;
|
||||
rcData[AUX2] = PWM_RANGE_MAX;
|
||||
rcData[AUX3] = PWM_RANGE_MAX;
|
||||
|
||||
// and
|
||||
uint8_t expectedAdjustmentStateMask =
|
||||
(1 << 0) |
|
||||
(1 << 1) |
|
||||
(1 << 2) |
|
||||
(1 << 3) |
|
||||
(1 << 4) |
|
||||
(1 << 5);
|
||||
|
||||
// when
|
||||
useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
|
||||
processRcAdjustments(&controlRateConfig, &rxConfig);
|
||||
|
||||
// then
|
||||
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6);
|
||||
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
|
||||
|
||||
// and
|
||||
EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]);
|
||||
EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]);
|
||||
EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]);
|
||||
EXPECT_EQ(10.01f, pidProfile.I_f[PIDPITCH]);
|
||||
EXPECT_EQ(15.01f, pidProfile.I_f[PIDROLL]);
|
||||
EXPECT_EQ(17.01f, pidProfile.I_f[PIDYAW]);
|
||||
EXPECT_EQ(20.001f, pidProfile.D_f[PIDPITCH]);
|
||||
EXPECT_EQ(25.001f, pidProfile.D_f[PIDROLL]);
|
||||
EXPECT_EQ(27.001f, pidProfile.D_f[PIDYAW]);
|
||||
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
void saveConfigAndNotify(void) {}
|
||||
void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {}
|
||||
|
@ -494,6 +698,8 @@ void mwDisarm(void) {}
|
|||
uint8_t getCurrentControlRateProfile(void) {
|
||||
return 0;
|
||||
}
|
||||
void GPS_reset_home_position(void) {}
|
||||
void baroSetCalibrationCycles(uint16_t) {}
|
||||
|
||||
uint8_t armingFlags = 0;
|
||||
int16_t heading;
|
||||
|
|
|
@ -19,23 +19,27 @@
|
|||
|
||||
#include <limits.h>
|
||||
|
||||
#include "build_config.h"
|
||||
extern "C" {
|
||||
#include "build_config.h"
|
||||
|
||||
#include "common/color.h"
|
||||
#include "common/color.h"
|
||||
|
||||
#include "drivers/light_ws2811strip.h"
|
||||
#include "drivers/light_ws2811strip.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
extern "C" {
|
||||
STATIC_UNIT_TESTED extern uint16_t dmaBufferOffset;
|
||||
|
||||
STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color);
|
||||
STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue);
|
||||
}
|
||||
|
||||
TEST(WS2812, updateDMABuffer) {
|
||||
// given
|
||||
rgbColor24bpp_t color1 = {0xFF,0xAA,0x55};
|
||||
rgbColor24bpp_t color1 = { .raw = {0xFF,0xAA,0x55} };
|
||||
|
||||
// and
|
||||
dmaBufferOffset = 0;
|
||||
|
@ -86,6 +90,7 @@ TEST(WS2812, updateDMABuffer) {
|
|||
byteIndex++;
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
rgbColor24bpp_t* hsvToRgb24(const hsvColor_t *c) {
|
||||
UNUSED(c);
|
||||
return NULL;
|
||||
|
@ -93,3 +98,4 @@ rgbColor24bpp_t* hsvToRgb24(const hsvColor_t *c) {
|
|||
|
||||
void ws2811LedStripHardwareInit(void) {}
|
||||
void ws2811LedStripDMAEnable(void) {}
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue