mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 21:35:44 +03:00
Fixed point passes initial unit test
This commit is contained in:
Joel Fuster
parent
11a9725605
commit
de7fa3f31a
3 changed files with 114 additions and 30 deletions
|
|
@ -53,6 +53,8 @@
|
|||
|
||||
#define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4
|
||||
|
||||
#define MIXER_DEBUG
|
||||
|
||||
#include "drivers/system.h"
|
||||
extern int16_t debug[4];
|
||||
|
||||
|
|
@ -672,24 +674,95 @@ bool isMixerUsingServos(void)
|
|||
|
||||
#endif
|
||||
|
||||
static void generate_lowpass_coeffs2(int16_t freq, float *b, float *a)
|
||||
static 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.4142f*omega + 1.0f);
|
||||
float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
|
||||
|
||||
b[0] = scaling * omega*omega;
|
||||
b[1] = 2.0f * b[0];
|
||||
b[2] = b[0];
|
||||
|
||||
// First assumed to be 1.0
|
||||
a[0] = scaling * (2.0f * omega*omega - 2.0f);
|
||||
a[1] = scaling * (omega*omega - 1.4142f * omega + 1.0f);
|
||||
|
||||
#ifdef UNIT_TEST
|
||||
printf("lowpass cutoff: %f, omega: %f\n", freqf, omega);
|
||||
printf("b: %f %f %f, a: %f %f\n", b[2], b[1], b[0], a[1], a[0]);
|
||||
#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;
|
||||
}
|
||||
|
||||
static float lowpass(lowpass_t *filter, float in, int16_t freq)
|
||||
|
|
@ -704,50 +777,53 @@ static float lowpass(lowpass_t *filter, float in, int16_t freq)
|
|||
|
||||
// Initialize if needed
|
||||
if (!filter->init) {
|
||||
generate_lowpass_coeffs2(freq, filter->b, filter->a);
|
||||
filter->freq = freq;
|
||||
//filter->b = &lowpass_table[filter->freqIdx][1];
|
||||
//filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF];
|
||||
generate_lowpass_coeffs2(freq, filter);
|
||||
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
filter->x[coefIdx] = in;
|
||||
filter->y[coefIdx] = in;
|
||||
filter->xf[coefIdx] = in;
|
||||
filter->yf[coefIdx] = in;
|
||||
}
|
||||
filter->init = true;
|
||||
}
|
||||
|
||||
// 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->xf[coefIdx] = filter->xf[coefIdx-1];
|
||||
filter->yf[coefIdx] = filter->yf[coefIdx-1];
|
||||
}
|
||||
filter->x[0] = in;
|
||||
filter->xf[0] = in;
|
||||
|
||||
// Accumulate result
|
||||
out = filter->x[0] * filter->b[0];
|
||||
out = filter->xf[0] * filter->b[0];
|
||||
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
out += filter->x[coefIdx] * filter->b[coefIdx];
|
||||
out -= filter->y[coefIdx] * filter->a[coefIdx-1];
|
||||
out += filter->xf[coefIdx] * filter->bf[coefIdx];
|
||||
out -= filter->yf[coefIdx] * filter->af[coefIdx];
|
||||
}
|
||||
filter->y[0] = out;
|
||||
filter->yf[0] = out;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
void filterServos(void)
|
||||
{
|
||||
int16_t servoIdx;
|
||||
|
||||
#if defined(MIXER_DEBUG)
|
||||
uint32_t startTime = micros();
|
||||
#endif
|
||||
|
||||
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(&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
|
||||
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(MIXER_DEBUG)
|
||||
debug[0] = (int16_t)(micros() - startTime);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -93,14 +93,22 @@ typedef struct servoParam_t {
|
|||
} 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 b[LOWPASS_NUM_COEF];
|
||||
float a[LOWPASS_NUM_COEF-1];
|
||||
float x[LOWPASS_NUM_COEF];
|
||||
float y[LOWPASS_NUM_COEF];
|
||||
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;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -148,7 +148,7 @@ gps_conversion_unittest : $(OBJECT_DIR)/flight/gps_conversion.o $(OBJECT_DIR)/gp
|
|||
$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||
|
||||
|
||||
$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(USER_DIR)/flight/lowpass_table.h $(GTEST_HEADERS)
|
||||
$(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 $@
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue