rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

More unit tests

This commit is contained in:
Joel Fuster 2015-01-04 22:10:20 -05:00
parent de7fa3f31a
commit dc3271255c
2 changed files with 83 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/main/flight/mixer.c
View File

@ -674,7 +674,7 @@ bool isMixerUsingServos(void)
#endif
static void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter)
void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter)
{
float fixedScaler;
int i;
@ -765,45 +765,6 @@ static int32_t lowpass_fixed(lowpass_t *filter, int32_t in, int16_t freq)
return (int32_t)out;
}
static float lowpass(lowpass_t *filter, float in, int16_t freq)
{
int16_t coefIdx;
float out;
// 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->xf[coefIdx] = in;
filter->yf[coefIdx] = in;
}
filter->init = true;
}
// Delays
for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
filter->xf[coefIdx] = filter->xf[coefIdx-1];
filter->yf[coefIdx] = filter->yf[coefIdx-1];
}
filter->xf[0] = in;
// Accumulate result
out = filter->xf[0] * filter->b[0];
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
out += filter->xf[coefIdx] * filter->bf[coefIdx];
out -= filter->yf[coefIdx] * filter->af[coefIdx];
}
filter->yf[0] = out;
return out;
}
void filterServos(void)
{
int16_t servoIdx;
@ -814,8 +775,6 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
// Round to nearest
//servo[servoIdx] = (int16_t)(lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx) + 0.5f);
servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx);
// Sanity check

103
src/test/unit/mixer_unittest.cc
View File

@ -24,29 +24,82 @@ extern "C" {
#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);
}
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];
#include "unittest_macros.h"
#include "gtest/gtest.h"
static float lowpass_ref(lowpass_t *filter, float in, int16_t freq)
{
int16_t coefIdx;
float out;
// 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->xf[coefIdx] = in;
filter->yf[coefIdx] = in;
}
filter->init = true;
}
// Delays
for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
filter->xf[coefIdx] = filter->xf[coefIdx-1];
filter->yf[coefIdx] = filter->yf[coefIdx-1];
}
filter->xf[0] = in;
// Accumulate result
out = filter->xf[0] * filter->bf[0];
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
out += filter->xf[coefIdx] * filter->bf[coefIdx];
out -= filter->yf[coefIdx] * filter->af[coefIdx];
}
filter->yf[0] = out;
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)
{
int16_t servoCmds[3000];
int16_t expectedOut[3000];
const int16_t margin = 0;
uint8_t servoIdx;
uint16_t sampleIdx;
static mixerConfig_t mixerConfig;
static servoParam_t servoConfig[MAX_SUPPORTED_SERVOS];
uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t);
// generate inputs and expecteds
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
if (sampleIdx < 1000) {
servoCmds[sampleIdx] = 501;
servoCmds[sampleIdx] = 500;
} else if (sampleIdx >= 1000 && sampleIdx < 2000) {
servoCmds[sampleIdx] = 2500;
} else {
@ -60,34 +113,42 @@ TEST(MixerTest, ServoLowpassFilter)
} else {
expectedOut[sampleIdx] = -1;
}
}
// Set mixer configuration
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servoConfig[servoIdx].min = 0;
servoConfig[servoIdx].max = 2500;
servoConfig[servoIdx].max = 3000;
}
mixerConfig.servo_lowpass_enable = 1;
mixerConfig.servo_lowpass_freq_idx = 400; // 10 to 400
mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL);
// Test all frequencies
for (freq = 10; freq <= 400; freq++)
{
printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
// Run tests
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = servoCmds[sampleIdx];
}
mixerConfig.servo_lowpass_enable = 1;
mixerConfig.servo_lowpass_freq_idx = freq;
mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL);
filterServos();
if (expectedOut[sampleIdx] >= 0) {
// Run tests
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
EXPECT_LE(servo[servoIdx], expectedOut[sampleIdx] + margin);
EXPECT_GE(servo[servoIdx], expectedOut[sampleIdx] - margin);
servo[servoIdx] = servoCmds[sampleIdx];
servoRef[servoIdx] = servoCmds[sampleIdx];
}
}
}
filterServos();
filterServosReference();
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);
}
} // for each sample
} // for each freq
}
// STUBS