Split filter into separate file for future reuse
This commit is contained in:
Joel Fuster
parent
4f0af41e79
commit
ec946ea7d5
1
Makefile
1
Makefile
|
|
@ -206,6 +206,7 @@ COMMON_SRC = build_config.c \
|
|||
flight/flight.c \
|
||||
flight/imu.c \
|
||||
flight/mixer.c \
|
||||
flight/lowpass.c \
|
||||
drivers/bus_i2c_soft.c \
|
||||
drivers/serial.c \
|
||||
drivers/sound_beeper.c \
|
||||
|
|
|
|||
|
|
@ -0,0 +1,115 @@
|
|||
/*
|
||||
* 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 <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "flight/lowpass.h"
|
||||
|
||||
void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter)
|
||||
{
|
||||
float fixedScaler;
|
||||
int i;
|
||||
|
||||
// generates coefficients for a 2nd-order butterworth low-pass filter
|
||||
float freqf = (float)freq*0.001f;
|
||||
float omega = tanf((float)M_PI*freqf/2.0f);
|
||||
float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
|
||||
|
||||
|
||||
#ifdef UNIT_TEST
|
||||
printf("lowpass cutoff: %f, omega: %f\n", freqf, omega);
|
||||
#endif
|
||||
|
||||
filter->bf[0] = scaling * omega*omega;
|
||||
filter->bf[1] = 2.0f * filter->bf[0];
|
||||
filter->bf[2] = filter->bf[0];
|
||||
|
||||
filter->af[0] = 1.0f;
|
||||
filter->af[1] = scaling * (2.0f * omega*omega - 2.0f);
|
||||
filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f);
|
||||
|
||||
// Scale for fixed-point
|
||||
filter->input_bias = 1500; // Typical servo range is 1500 +/- 500
|
||||
filter->input_shift = 16;
|
||||
filter->coeff_shift = 24;
|
||||
fixedScaler = (float)(1ULL << filter->coeff_shift);
|
||||
for (i = 0; i < LOWPASS_NUM_COEF; i++) {
|
||||
filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler);
|
||||
filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler);
|
||||
#ifdef UNIT_TEST
|
||||
printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i,
|
||||
filter->bf[i], filter->af[i], filter->b[i], filter->a[i]);
|
||||
#endif
|
||||
}
|
||||
|
||||
filter->freq = freq;
|
||||
}
|
||||
|
||||
int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq)
|
||||
{
|
||||
int16_t coefIdx;
|
||||
int64_t out;
|
||||
int32_t in_s;
|
||||
|
||||
// Check to see if cutoff frequency changed
|
||||
if (freq != filter->freq) {
|
||||
filter->init = false;
|
||||
}
|
||||
|
||||
// Initialize if needed
|
||||
if (!filter->init) {
|
||||
generateLowpassCoeffs2(freq, filter);
|
||||
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift;
|
||||
filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift;
|
||||
}
|
||||
filter->init = true;
|
||||
}
|
||||
|
||||
// Unbias input and scale
|
||||
in_s = (in - filter->input_bias) << filter->input_shift;
|
||||
|
||||
// Delays
|
||||
for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
|
||||
filter->x[coefIdx] = filter->x[coefIdx-1];
|
||||
filter->y[coefIdx] = filter->y[coefIdx-1];
|
||||
}
|
||||
filter->x[0] = in_s;
|
||||
|
||||
// Accumulate result
|
||||
out = filter->x[0] * filter->b[0];
|
||||
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
out -= filter->y[coefIdx] * filter->a[coefIdx];
|
||||
out += filter->x[coefIdx] * filter->b[coefIdx];
|
||||
}
|
||||
|
||||
// Scale output by coefficient shift
|
||||
out >>= filter->coeff_shift;
|
||||
filter->y[0] = (int32_t)out;
|
||||
|
||||
// Scale output by input shift and round
|
||||
out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift;
|
||||
|
||||
// Reapply bias
|
||||
out += filter->input_bias;
|
||||
|
||||
return (int32_t)out;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#define LOWPASS_NUM_COEF 3
|
||||
#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f))
|
||||
|
||||
typedef struct lowpass_t {
|
||||
bool init;
|
||||
int16_t freq; // Normalized freq in 1/1000ths
|
||||
float bf[LOWPASS_NUM_COEF];
|
||||
float af[LOWPASS_NUM_COEF];
|
||||
int64_t b[LOWPASS_NUM_COEF];
|
||||
int64_t a[LOWPASS_NUM_COEF];
|
||||
int16_t coeff_shift;
|
||||
int16_t input_shift;
|
||||
int32_t input_bias;
|
||||
float xf[LOWPASS_NUM_COEF];
|
||||
float yf[LOWPASS_NUM_COEF];
|
||||
int32_t x[LOWPASS_NUM_COEF];
|
||||
int32_t y[LOWPASS_NUM_COEF];
|
||||
} lowpass_t;
|
||||
|
||||
void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter);
|
||||
int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq);
|
||||
|
|
@ -18,11 +18,6 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifdef UNIT_TEST
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
@ -30,12 +25,11 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#ifndef UNIT_TEST
|
||||
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
#endif
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "io/gimbal.h"
|
||||
|
|
@ -44,6 +38,7 @@
|
|||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/flight.h"
|
||||
#include "flight/lowpass.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
#include "config/config.h"
|
||||
|
|
@ -55,19 +50,16 @@
|
|||
|
||||
//#define MIXER_DEBUG
|
||||
|
||||
#include "drivers/system.h"
|
||||
extern int16_t debug[4];
|
||||
|
||||
#ifndef UNIT_TEST
|
||||
uint8_t motorCount = 0;
|
||||
static int useServo;
|
||||
static uint8_t servoCount;
|
||||
#endif
|
||||
|
||||
int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
static int useServo;
|
||||
|
||||
static uint8_t servoCount;
|
||||
|
||||
static servoParam_t *servoConf;
|
||||
static mixerConfig_t *mixerConfig;
|
||||
|
|
@ -77,10 +69,8 @@ static airplaneConfig_t *airplaneConfig;
|
|||
static rxConfig_t *rxConfig;
|
||||
static gimbalConfig_t *gimbalConfig;
|
||||
|
||||
#ifndef UNIT_TEST
|
||||
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
||||
static mixerMode_e currentMixerMode;
|
||||
#endif
|
||||
static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
static const motorMixer_t mixerQuadX[] = {
|
||||
|
|
@ -244,7 +234,6 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
|
|||
gimbalConfig = gimbalConfigToUse;
|
||||
}
|
||||
|
||||
#ifndef UNIT_TEST
|
||||
int16_t determineServoMiddleOrForwardFromChannel(int nr)
|
||||
{
|
||||
uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel;
|
||||
|
|
@ -524,7 +513,7 @@ void mixTable(void)
|
|||
int16_t maxMotor;
|
||||
uint32_t i;
|
||||
|
||||
// paranoia: give all servos a default command; prevents drift on unused servos with lowpass enabled
|
||||
// paranoia: give all servos a default command
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
servo[i] = DEFAULT_SERVO_MIDDLE;
|
||||
}
|
||||
|
|
@ -676,99 +665,6 @@ bool isMixerUsingServos(void)
|
|||
return useServo;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter)
|
||||
{
|
||||
float fixedScaler;
|
||||
int i;
|
||||
|
||||
// generates coefficients for a 2nd-order butterworth low-pass filter
|
||||
float freqf = (float)freq*0.001f;
|
||||
float omega = tanf(M_PI*freqf/2.0f);
|
||||
float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
|
||||
|
||||
|
||||
#ifdef UNIT_TEST
|
||||
printf("lowpass cutoff: %f, omega: %f\n", freqf, omega);
|
||||
#endif
|
||||
|
||||
filter->bf[0] = scaling * omega*omega;
|
||||
filter->bf[1] = 2.0f * filter->bf[0];
|
||||
filter->bf[2] = filter->bf[0];
|
||||
|
||||
filter->af[0] = 1.0f;
|
||||
filter->af[1] = scaling * (2.0f * omega*omega - 2.0f);
|
||||
filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f);
|
||||
|
||||
// Scale for fixed-point
|
||||
filter->input_bias = 1500; // Typical servo range is 1500 +/- 500
|
||||
filter->input_shift = 16;
|
||||
filter->coeff_shift = 24;
|
||||
fixedScaler = (float)(1ULL << filter->coeff_shift);
|
||||
for (i = 0; i < LOWPASS_NUM_COEF; i++) {
|
||||
filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler);
|
||||
filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler);
|
||||
#ifdef UNIT_TEST
|
||||
printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i,
|
||||
filter->bf[i], filter->af[i], filter->b[i], filter->a[i]);
|
||||
#endif
|
||||
}
|
||||
|
||||
filter->freq = freq;
|
||||
}
|
||||
|
||||
static int32_t lowpass_fixed(lowpass_t *filter, int32_t in, int16_t freq)
|
||||
{
|
||||
int16_t coefIdx;
|
||||
int64_t out;
|
||||
int32_t in_s;
|
||||
|
||||
// Check to see if cutoff frequency changed
|
||||
if (freq != filter->freq) {
|
||||
filter->init = false;
|
||||
}
|
||||
|
||||
// Initialize if needed
|
||||
if (!filter->init) {
|
||||
generate_lowpass_coeffs2(freq, filter);
|
||||
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift;
|
||||
filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift;
|
||||
}
|
||||
filter->init = true;
|
||||
}
|
||||
|
||||
// Unbias input and scale
|
||||
in_s = (in - filter->input_bias) << filter->input_shift;
|
||||
|
||||
// Delays
|
||||
for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
|
||||
filter->x[coefIdx] = filter->x[coefIdx-1];
|
||||
filter->y[coefIdx] = filter->y[coefIdx-1];
|
||||
}
|
||||
filter->x[0] = in_s;
|
||||
|
||||
// Accumulate result
|
||||
out = filter->x[0] * filter->b[0];
|
||||
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
out -= filter->y[coefIdx] * filter->a[coefIdx];
|
||||
out += filter->x[coefIdx] * filter->b[coefIdx];
|
||||
}
|
||||
|
||||
// Scale output by coefficient shift
|
||||
out >>= filter->coeff_shift;
|
||||
filter->y[0] = (int32_t)out;
|
||||
|
||||
// Scale output by input shift and round
|
||||
out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift;
|
||||
|
||||
// Reapply bias
|
||||
out += filter->input_bias;
|
||||
|
||||
return (int32_t)out;
|
||||
}
|
||||
|
||||
void filterServos(void)
|
||||
{
|
||||
int16_t servoIdx;
|
||||
|
|
@ -779,7 +675,7 @@ void filterServos(void)
|
|||
|
||||
if (mixerConfig->servo_lowpass_enable) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
|
||||
servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
|
||||
|
||||
// Sanity check
|
||||
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
|
||||
|
|
|
|||
|
|
@ -92,26 +92,6 @@ typedef struct servoParam_t {
|
|||
int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
|
||||
} servoParam_t;
|
||||
|
||||
#define LOWPASS_NUM_COEF 3
|
||||
#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f))
|
||||
|
||||
typedef struct lowpass_t {
|
||||
bool init;
|
||||
int16_t freq; // Normalized freq in 1/1000ths
|
||||
float bf[LOWPASS_NUM_COEF];
|
||||
float af[LOWPASS_NUM_COEF];
|
||||
int64_t b[LOWPASS_NUM_COEF];
|
||||
int64_t a[LOWPASS_NUM_COEF];
|
||||
int16_t coeff_shift;
|
||||
int16_t input_shift;
|
||||
int32_t input_bias;
|
||||
float xf[LOWPASS_NUM_COEF];
|
||||
float yf[LOWPASS_NUM_COEF];
|
||||
int32_t x[LOWPASS_NUM_COEF];
|
||||
int32_t y[LOWPASS_NUM_COEF];
|
||||
} lowpass_t;
|
||||
|
||||
|
||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
|
|
|
|||
|
|
@ -715,6 +715,7 @@ void loop(void)
|
|||
);
|
||||
|
||||
mixTable();
|
||||
filterServos();
|
||||
writeServos();
|
||||
writeMotors();
|
||||
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ TESTS = \
|
|||
rc_controls_unittest \
|
||||
ledstrip_unittest \
|
||||
ws2811_unittest \
|
||||
mixer_unittest
|
||||
lowpass_unittest
|
||||
|
||||
# All Google Test headers. Usually you shouldn't change this
|
||||
# definition.
|
||||
|
|
@ -320,16 +320,30 @@ ws2811_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) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@
|
||||
|
||||
$(OBJECT_DIR)/mixer_unittest.o : $(TEST_DIR)/mixer_unittest.cc $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/mixer_unittest.cc -o $@
|
||||
$(OBJECT_DIR)/flight/lowpass.o : \
|
||||
$(USER_DIR)/flight/lowpass.c \
|
||||
$(USER_DIR)/flight/lowpass.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/lowpass.c -o $@
|
||||
|
||||
$(OBJECT_DIR)/lowpass_unittest.o : \
|
||||
$(TEST_DIR)/lowpass_unittest.cc \
|
||||
$(USER_DIR)/flight/lowpass.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/lowpass_unittest.cc -o $@
|
||||
|
||||
lowpass_unittest : \
|
||||
$(OBJECT_DIR)/flight/lowpass.o \
|
||||
$(OBJECT_DIR)/lowpass_unittest.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
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 \
|
||||
|
|
|
|||
|
|
@ -15,29 +15,19 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <stdint.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
extern "C" {
|
||||
#include "flight/mixer.h"
|
||||
#include "rx/rx.h"
|
||||
#include "io/gimbal.h"
|
||||
#include "io/escservo.h"
|
||||
extern void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse);
|
||||
extern void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter);
|
||||
#include "flight/lowpass.h"
|
||||
}
|
||||
|
||||
uint32_t debug[4];
|
||||
static int16_t servoRef[MAX_SUPPORTED_SERVOS];
|
||||
static int16_t referenceOut[MAX_SUPPORTED_SERVOS];
|
||||
static uint16_t freq;
|
||||
static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
|
||||
static servoParam_t servoConfig[MAX_SUPPORTED_SERVOS];
|
||||
static lowpass_t lowpassFilterReference;
|
||||
static lowpass_t lowpassFilter;
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
static float lowpass_ref(lowpass_t *filter, float in, int16_t freq)
|
||||
static float lowpassRef(lowpass_t *filter, float in, int16_t freq)
|
||||
{
|
||||
int16_t coefIdx;
|
||||
float out;
|
||||
|
|
@ -49,7 +39,7 @@ static float lowpass_ref(lowpass_t *filter, float in, int16_t freq)
|
|||
|
||||
// Initialize if needed
|
||||
if (!filter->init) {
|
||||
generate_lowpass_coeffs2(freq, filter);
|
||||
generateLowpassCoeffs2(freq, filter);
|
||||
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
filter->xf[coefIdx] = in;
|
||||
filter->yf[coefIdx] = in;
|
||||
|
|
@ -75,24 +65,14 @@ static float lowpass_ref(lowpass_t *filter, float in, int16_t freq)
|
|||
return out;
|
||||
}
|
||||
|
||||
static void filterServosReference(void)
|
||||
{
|
||||
int16_t servoIdx;
|
||||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
// Round to nearest
|
||||
referenceOut[servoIdx] = (int16_t)(lowpass_ref(&lowpassFilters[servoIdx], (float)servoRef[servoIdx], freq) + 0.5f);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST(MixerTest, ServoLowpassFilter)
|
||||
TEST(LowpassTest, Lowpass)
|
||||
{
|
||||
int16_t servoCmds[3000];
|
||||
int16_t expectedOut[3000];
|
||||
uint8_t servoIdx;
|
||||
int16_t referenceOut;
|
||||
int16_t filterOut;
|
||||
uint16_t sampleIdx;
|
||||
static mixerConfig_t mixerConfig;
|
||||
int16_t freq;
|
||||
|
||||
uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t);
|
||||
|
||||
|
|
@ -115,38 +95,26 @@ TEST(MixerTest, ServoLowpassFilter)
|
|||
}
|
||||
}
|
||||
|
||||
// Set mixer configuration
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
servoConfig[servoIdx].min = 0;
|
||||
servoConfig[servoIdx].max = 3000;
|
||||
}
|
||||
|
||||
// Test all frequencies
|
||||
for (freq = 10; freq <= 400; freq++)
|
||||
{
|
||||
for (freq = 10; freq <= 400; freq++) {
|
||||
printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
|
||||
|
||||
mixerConfig.servo_lowpass_enable = 1;
|
||||
mixerConfig.servo_lowpass_freq = freq;
|
||||
mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL);
|
||||
|
||||
// Run tests
|
||||
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
servo[servoIdx] = servoCmds[sampleIdx];
|
||||
servoRef[servoIdx] = servoCmds[sampleIdx];
|
||||
}
|
||||
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++)
|
||||
{
|
||||
// Filter under test
|
||||
filterOut = (int16_t)lowpassFixed(&lowpassFilter, servoCmds[sampleIdx], freq);
|
||||
|
||||
filterServos();
|
||||
filterServosReference();
|
||||
// Floating-point reference
|
||||
referenceOut = (int16_t)(lowpassRef(&lowpassFilterReference, (float)servoCmds[sampleIdx], freq) + 0.5f);
|
||||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
if (expectedOut[sampleIdx] >= 0) {
|
||||
EXPECT_EQ(servo[servoIdx], expectedOut[sampleIdx]);
|
||||
}
|
||||
EXPECT_LE(servo[servoIdx], referenceOut[servoIdx] + 1);
|
||||
EXPECT_GE(servo[servoIdx], referenceOut[servoIdx] - 1);
|
||||
if (expectedOut[sampleIdx] >= 0) {
|
||||
EXPECT_EQ(filterOut, expectedOut[sampleIdx]);
|
||||
}
|
||||
// Some tolerance
|
||||
// TODO adjust precision to remove the need for this?
|
||||
EXPECT_LE(filterOut, referenceOut + 1);
|
||||
EXPECT_GE(filterOut, referenceOut - 1);
|
||||
} // for each sample
|
||||
} // for each freq
|
||||
}
|
||||
|
|
@ -155,21 +123,6 @@ TEST(MixerTest, ServoLowpassFilter)
|
|||
|
||||
extern "C" {
|
||||
|
||||
void delay(uint32_t ms)
|
||||
{
|
||||
UNUSED(ms);
|
||||
return;
|
||||
}
|
||||
|
||||
int constrain(int amt, int low, int high)
|
||||
{
|
||||
return (amt > high ? high : (amt < low ? low : amt));
|
||||
}
|
||||
|
||||
uint32_t micros()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue