From fbc3a8e1ebe7bc127f1c081b1f1bbcd93a459948 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 19 Apr 2015 13:22:32 +0100 Subject: [PATCH] Correct rx channel min/max pulse checking so that it is inclusive to match the documentation. Add first unit tests for code in rx.c. --- src/main/drivers/gpio.h | 2 + src/main/drivers/timer.h | 5 + src/main/rx/msp.c | 4 +- src/main/rx/rx.c | 9 +- src/main/rx/rx.h | 2 + src/test/Makefile | 25 +++++ src/test/unit/platform.h | 18 ++++ src/test/unit/rx_rx_unittest.cc | 162 ++++++++++++++++++++++++++++++++ 8 files changed, 219 insertions(+), 8 deletions(-) create mode 100644 src/test/unit/rx_rx_unittest.cc diff --git a/src/main/drivers/gpio.h b/src/main/drivers/gpio.h index 0f6d00980..dd08eb4ac 100644 --- a/src/main/drivers/gpio.h +++ b/src/main/drivers/gpio.h @@ -106,10 +106,12 @@ typedef struct GPIO_Speed speed; } gpio_config_t; +#ifndef UNIT_TEST static inline void digitalHi(GPIO_TypeDef *p, uint16_t i) { p->BSRR = i; } static inline void digitalLo(GPIO_TypeDef *p, uint16_t i) { p->BRR = i; } static inline void digitalToggle(GPIO_TypeDef *p, uint16_t i) { p->ODR ^= i; } static inline uint16_t digitalIn(GPIO_TypeDef *p, uint16_t i) {return p->IDR & i; } +#endif void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config); void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc); diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index d8d78a447..8edbd88e5 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -33,6 +33,11 @@ typedef uint16_t timCCR_t; typedef uint16_t timCCER_t; typedef uint16_t timSR_t; typedef uint16_t timCNT_t; +#elif defined(UNIT_TEST) +typedef uint32_t timCCR_t; +typedef uint32_t timCCER_t; +typedef uint32_t timSR_t; +typedef uint32_t timCNT_t; #else # error "Unknown CPU defined" #endif diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index 853a7bd0a..105a30ec9 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -54,12 +54,10 @@ bool rxMspFrameComplete(void) return true; } -bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { UNUSED(rxConfig); rxRuntimeConfig->channelCount = 8; // Limited to 8 channels due to MSP_SET_RAW_RC command. if (callback) *callback = rxMspReadRawRC; - - return true; } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index ded1f80fa..480930af4 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -56,7 +56,7 @@ bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcRe bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); -bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); const char rcChannelLetters[] = "AERT12345678abcdefgh"; @@ -91,17 +91,16 @@ void useRxConfig(rxConfig_t *rxConfigToUse) rxConfig = rxConfigToUse; } -#define STICK_CHANNEL_COUNT 4 #define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels // pulse duration is in micro seconds (usec) -void rxCheckPulse(uint8_t channel, uint16_t pulseDuration) +STATIC_UNIT_TESTED void rxCheckPulse(uint8_t channel, uint16_t pulseDuration) { static uint8_t goodChannelMask = 0; if (channel < 4 && - pulseDuration > rxConfig->rx_min_usec && - pulseDuration < rxConfig->rx_max_usec + pulseDuration >= rxConfig->rx_min_usec && + pulseDuration <= rxConfig->rx_max_usec ) { // if signal is valid - mark channel as OK goodChannelMask |= (1 << channel); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 4ebcadd7a..56389dbcb 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -17,6 +17,8 @@ #pragma once +#define STICK_CHANNEL_COUNT 4 + #define PWM_RANGE_ZERO 0 // FIXME should all usages of this be changed to use PWM_RANGE_MIN? #define PWM_RANGE_MIN 1000 #define PWM_RANGE_MAX 2000 diff --git a/src/test/Makefile b/src/test/Makefile index c7154b1fa..5e099151e 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -53,6 +53,7 @@ TESTS = \ gps_conversion_unittest \ telemetry_hott_unittest \ rc_controls_unittest \ + rx_rx_unittest \ ledstrip_unittest \ ws2811_unittest \ encoding_unittest \ @@ -438,6 +439,30 @@ io_serial_unittest : \ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/rx/rx.o : \ + $(USER_DIR)/rx/rx.c \ + $(USER_DIR)/rx/rx.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/rx/rx.c -o $@ + +$(OBJECT_DIR)/rx_rx_unittest.o : \ + $(TEST_DIR)/rx_rx_unittest.cc \ + $(USER_DIR)/rx/rx.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rx_rx_unittest.cc -o $@ + +rx_rx_unittest : \ + $(OBJECT_DIR)/rx/rx.o \ + $(OBJECT_DIR)/rx_rx_unittest.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + test: $(TESTS) set -e && for test in $(TESTS) ; do \ $(OBJECT_DIR)/$$test; \ diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 3d0a20faf..7841b19f9 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -27,3 +27,21 @@ #define SERIAL_PORT_COUNT 4 #define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 + +typedef enum +{ + Mode_TEST = 0x0, +} GPIO_Mode; + +typedef struct +{ + void* test; +} GPIO_TypeDef; + +typedef struct +{ + void* test; +} TIM_TypeDef; + +typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; + diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc new file mode 100644 index 000000000..dff865065 --- /dev/null +++ b/src/test/unit/rx_rx_unittest.cc @@ -0,0 +1,162 @@ +/* + * 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 . + */ + +#include +#include + +#include + +extern "C" { + #include "platform.h" + + #include "rx/rx.h" + + void rxInit(rxConfig_t *rxConfig); + void rxCheckPulse(uint8_t channel, uint16_t pulseDuration); +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +enum { + COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED = 0, +}; +#define CALL_COUNT_ITEM_COUNT 1 + +static int callCounts[CALL_COUNT_ITEM_COUNT]; + +#define CALL_COUNTER(item) (callCounts[item]) + +void resetCallCounters(void) { + memset(&callCounts, 0, sizeof(callCounts)); +} + +typedef struct testData_s { + bool isPPMDataBeingReceived; +} testData_t; + +static testData_t testData; + +TEST(RxTest, TestFailsafeInformedOfValidData) +{ + // given + resetCallCounters(); + memset(&testData, 0, sizeof(testData)); + + // and + rxConfig_t rxConfig; + + memset(&rxConfig, 0, sizeof(rxConfig)); + rxConfig.rx_min_usec = 1000; + rxConfig.rx_max_usec = 2000; + + // when + rxInit(&rxConfig); + + for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { + rxCheckPulse(channelIndex, 1500); + } + + // then + EXPECT_EQ(1, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED)); +} + +TEST(RxTest, TestFailsafeNotInformedOfValidDataWhenStickChannelsAreBad) +{ + // given + memset(&testData, 0, sizeof(testData)); + + // and + rxConfig_t rxConfig; + + memset(&rxConfig, 0, sizeof(rxConfig)); + rxConfig.rx_min_usec = 1000; + rxConfig.rx_max_usec = 2000; + + // and + uint16_t channelPulses[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + memset(&channelPulses, 1500, sizeof(channelPulses)); + + // and + rxInit(&rxConfig); + + // and + + for (uint8_t stickChannelIndex = 0; stickChannelIndex < STICK_CHANNEL_COUNT; stickChannelIndex++) { + + // given + resetCallCounters(); + + for (uint8_t channelIndex = 0; channelIndex < STICK_CHANNEL_COUNT; channelIndex++) { + channelPulses[stickChannelIndex] = rxConfig.rx_min_usec; + } + channelPulses[stickChannelIndex] = rxConfig.rx_min_usec - 1; + + // when + for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { + rxCheckPulse(channelIndex, channelPulses[channelIndex]); + } + + // then + EXPECT_EQ(0, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED)); + + // given + resetCallCounters(); + + for (uint8_t channelIndex = 0; channelIndex < STICK_CHANNEL_COUNT; channelIndex++) { + channelPulses[stickChannelIndex] = rxConfig.rx_max_usec; + } + channelPulses[stickChannelIndex] = rxConfig.rx_max_usec + 1; + + // when + for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { + rxCheckPulse(channelIndex, channelPulses[channelIndex]); + } + + // then + EXPECT_EQ(0, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED)); + } +} + + +// STUBS + +extern "C" { + void failsafeOnRxCycleStarted() {} + void failsafeOnValidDataReceived() { + callCounts[COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED]++; + } + + bool feature(uint32_t mask) { + UNUSED(mask); + return false; + } + + bool isPPMDataBeingReceived(void) { + return testData.isPPMDataBeingReceived; + } + + void resetPPMDataReceivedState(void) {} + + bool rxMspFrameComplete(void) { return false; } + + void rxMspInit(rxConfig_t *, rxRuntimeConfig_t *, rcReadRawDataPtr *) {} + + void rxPwmInit(rxRuntimeConfig_t *, rcReadRawDataPtr *) {} + + +}