From 8e6570754cc48c9bc4001472390b99491fd8f377 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 16 Mar 2015 22:56:14 +0100 Subject: [PATCH] Add the first unit test for the mixer. See #16 --- src/main/drivers/pwm_mapping.c | 6 ++ src/main/drivers/pwm_output.h | 5 -- src/main/flight/mixer.c | 38 +++++----- src/test/Makefile | 25 +++++++ src/test/unit/flight_mixer_unittest.cc | 125 +++++++++++++++++++++++++++++++++ 5 files changed, 177 insertions(+), 22 deletions(-) create mode 100644 src/test/unit/flight_mixer_unittest.cc diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 533d98074..760aec33c 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -28,6 +28,12 @@ #include "pwm_output.h" #include "pwm_rx.h" #include "pwm_mapping.h" + +void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t idlePulse); +void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); + /* Configuration maps diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 40689e9f9..9139fdef3 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -17,12 +17,7 @@ #pragma once -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t idlePulse); - void pwmWriteMotor(uint8_t index, uint16_t value); void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); -void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); void pwmWriteServo(uint8_t index, uint16_t value); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 42dcc4ca4..3f665079e 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -27,8 +27,6 @@ #include "common/maths.h" #include "drivers/system.h" -#include "drivers/gpio.h" -#include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" #include "drivers/sensor.h" @@ -78,7 +76,7 @@ static mixerMode_e currentMixerMode; static gimbalConfig_t *gimbalConfig; int16_t servo[MAX_SUPPORTED_SERVOS]; static int useServo; -static uint8_t servoCount; +STATIC_UNIT_TESTED uint8_t servoCount; static servoParam_t *servoConf; static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; #endif @@ -404,6 +402,25 @@ void mixerResetMotors(void) } #ifdef USE_SERVOS + +STATIC_UNIT_TESTED void forwardAuxChannelsToServos(void) +{ + // offset servos based off number already used in mixer types + // airplane and servo_tilt together can't be used + int8_t firstServo = servoCount - AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; + + // start forwarding from this channel + uint8_t channelOffset = AUX1; + + int8_t servoOffset; + for (servoOffset = 0; servoOffset < AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; servoOffset++) { + if (firstServo + servoOffset < 0) { + continue; // there are not enough servos to forward all the AUX channels. + } + pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]); + } +} + static void updateGimbalServos(void) { pwmWriteServo(0, servo[0]); @@ -653,20 +670,7 @@ void mixTable(void) // forward AUX1-4 to servo outputs (not constrained) if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) { - // offset servos based off number already used in mixer types - // airplane and servo_tilt together can't be used - int8_t firstServo = servoCount - AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; - - // start forwarding from this channel - uint8_t channelOffset = AUX1; - - int8_t servoOffset; - for (servoOffset = 0; servoOffset < AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; servoOffset++) { - if (firstServo + servoOffset < 0) { - continue; // there are not enough servos to forward all the AUX channels. - } - pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]); - } + forwardAuxChannelsToServos(); } #endif diff --git a/src/test/Makefile b/src/test/Makefile index 6575c48a1..f60ee6cc8 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -46,6 +46,7 @@ CXX_FLAGS = $(COMMON_FLAGS) \ TESTS = \ battery_unittest \ flight_imu_unittest \ + flight_mixer_unittest \ altitude_hold_unittest \ maths_unittest \ gps_conversion_unittest \ @@ -364,6 +365,30 @@ lowpass_unittest : \ $(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 $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@ + +$(OBJECT_DIR)/flight_mixer_unittest.o : \ + $(TEST_DIR)/flight_mixer_unittest.cc \ + $(USER_DIR)/flight/mixer.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_mixer_unittest.cc -o $@ + +flight_mixer_unittest : \ + $(OBJECT_DIR)/flight/mixer.o \ + $(OBJECT_DIR)/flight_mixer_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 \ diff --git a/src/test/unit/flight_mixer_unittest.cc b/src/test/unit/flight_mixer_unittest.cc new file mode 100644 index 000000000..386db1d2b --- /dev/null +++ b/src/test/unit/flight_mixer_unittest.cc @@ -0,0 +1,125 @@ +/* + * 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 "common/axis.h" + #include "common/maths.h" + + #include "drivers/sensor.h" + #include "drivers/accgyro.h" + + #include "sensors/sensors.h" + #include "sensors/acceleration.h" + + #include "rx/rx.h" + #include "flight/pid.h" + #include "flight/imu.h" + #include "flight/mixer.h" + #include "flight/lowpass.h" + + #include "io/rc_controls.h" + + extern uint8_t servoCount; + void forwardAuxChannelsToServos(void); + +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +typedef struct motor_s { + uint16_t value; +} motor_t; + +typedef struct servo_s { + uint16_t value; +} servo_t; + +motor_t motors[MAX_SUPPORTED_MOTORS]; +servo_t servos[MAX_SUPPORTED_SERVOS]; + +uint8_t lastOneShotUpdateMotorCount; + + +TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithNoServos) +{ + // given + memset(&motors, 0, sizeof(motors)); + memset(&servos, 0, sizeof(servos)); + servoCount = 0; + + rcData[AUX1] = 1500; + rcData[AUX2] = 1500; + rcData[AUX3] = 1500; + rcData[AUX4] = 1500; + + // when + forwardAuxChannelsToServos(); + + // then + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + EXPECT_EQ(servos[i].value, 0); + } +} + +// STUBS + +extern "C" { +rollAndPitchInclination_t inclination; +rxRuntimeConfig_t rxRuntimeConfig; + +int16_t axisPID[XYZ_AXIS_COUNT]; +int16_t rcCommand[4]; +int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + +uint32_t rcModeActivationMask; +int16_t debug[4]; + +uint8_t stateFlags; +uint16_t flightModeFlags; +uint8_t armingFlags; + +void delay(uint32_t) {} + +bool feature(uint32_t) { + return true; +} + +int32_t lowpassFixed(lowpass_t *, int32_t, int16_t) { + return 0; +} + +void pwmWriteMotor(uint8_t index, uint16_t value) { + motors[index].value = value; +} + +void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { + lastOneShotUpdateMotorCount = motorCount; +} + +void pwmWriteServo(uint8_t index, uint16_t value) { + servos[index].value = value; +} + +} -- 2.11.4.GIT