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

Remove lowpass.c // replace by pt1 

unittests fix
This commit is contained in:
borisbstyle 2016-02-09 23:23:33 +01:00
parent 19d1a92c4f
commit 83d8a8441c
11 changed files with 40 additions and 338 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -291,7 +291,6 @@ COMMON_SRC = build_config.c \
flight/pid.c \
flight/imu.c \
flight/mixer.c \
flight/lowpass.c \
drivers/bus_i2c_soft.c \
drivers/serial.c \
drivers/sound_beeper.c \

13
src/main/common/filter.c
View File

@ -29,6 +29,19 @@
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) {
// Pre calculate and store RC
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
}
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
return filter->state;
}
/* sets up a biquad Filter */
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
{

7
src/main/common/filter.h
View File

@ -15,6 +15,12 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
typedef struct filterStatePt1_s {
float state;
float RC;
float constdT;
} filterStatePt1_t;
/* this holds the data required to update samples thru a filter */
typedef struct biquad_s {
float b0, b1, b2, a1, a2;
@ -22,4 +28,5 @@ typedef struct biquad_s {
} biquad_t;
float applyBiQuadFilter(float sample, biquad_t *state);
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);

118
src/main/flight/lowpass.c
View File

@ -1,118 +0,0 @@
/*
* 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 "common/maths.h"
#include "flight/lowpass.h"
//#define DEBUG_LOWPASS
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 = tan_approx((float)M_PI*freqf/2.0f);
float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
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);
#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
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;
}

41
src/main/flight/lowpass.h
View File

@ -1,41 +0,0 @@
/*
* 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_s {
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);

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

@ -28,6 +28,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "drivers/system.h"
#include "drivers/pwm_output.h"
@ -35,6 +36,7 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "rx/rx.h"
@ -50,12 +52,12 @@
#include "flight/failsafe.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/lowpass.h"
#include "config/runtime_config.h"
#include "config/config.h"
uint8_t motorCount;
extern float dT;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@ -77,7 +79,6 @@ int16_t servo[MAX_SUPPORTED_SERVOS];
static int useServo;
STATIC_UNIT_TESTED uint8_t servoCount;
static servoParam_t *servoConf;
static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
#endif
static const motorMixer_t mixerQuadX[] = {
@ -923,6 +924,7 @@ void filterServos(void)
{
#ifdef USE_SERVOS
int16_t servoIdx;
static filterStatePt1_t servoFitlerState[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
@ -930,8 +932,7 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
servo[servoIdx] = filterApplyPt1(servo[servoIdx], &servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, dT);
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}

20
src/main/mw.c
View File

@ -119,7 +119,8 @@ extern uint32_t currentTime;
extern uint8_t PIDweight[3];
extern bool antiWindupProtection;
static filterStatePt1_t filteredCycleTimeState;
uint16_t filteredCycleTime;
static bool isRXDataNew;
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
@ -177,21 +178,10 @@ void filterRc(void){
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
uint16_t rxRefreshRate;
static biquad_t filteredCycleTimeState;
static bool filterIsSet;
uint16_t filteredCycleTime;
// Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate);
/* Initialize cycletime filter */
if (!filterIsSet) {
BiQuadNewLpf(0.5f, &filteredCycleTimeState, targetLooptime);
filterIsSet = true;
}
filteredCycleTime = applyBiQuadFilter((float) cycleTime, &filteredCycleTimeState);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
if (isRXDataNew) {
@ -647,6 +637,12 @@ void taskMainPidLoop(void)
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)targetLooptime * 0.000001f;
// Calculate average cycle time and average jitter
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 0.5f, dT);
debug[0] = cycleTime;
debug[1] = cycleTime - filteredCycleTime;
imuUpdateGyroAndAttitude();
annexCode();

13
src/main/sensors/battery.c
View File

@ -19,8 +19,10 @@
#include "stdint.h"
#include "platform.h"
#include "debug.h"
#include "common/maths.h"
#include "common/filter.h"
#include "drivers/adc.h"
#include "drivers/system.h"
@ -33,11 +35,10 @@
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "flight/lowpass.h"
#include "io/beeper.h"
#define VBATT_PRESENT_THRESHOLD_MV 10
#define VBATT_LPF_FREQ 10
#define VBATT_LPF_FREQ 1.0f
// Battery monitoring stuff
uint8_t batteryCellCount = 3; // cell count
@ -52,7 +53,6 @@ int32_t amperage = 0; // amperage read by current sensor in centia
int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start
static batteryState_e batteryState;
static lowpass_t lowpassFilter;
uint16_t batteryAdcToVoltage(uint16_t src)
{
@ -64,12 +64,13 @@ uint16_t batteryAdcToVoltage(uint16_t src)
static void updateBatteryVoltage(void)
{
uint16_t vbatSample;
uint16_t vbatFiltered;
static filterStatePt1_t vbatFilterState;
// store the battery voltage with some other recent battery voltage readings
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
vbatFiltered = (uint16_t)lowpassFixed(&lowpassFilter, vbatSample, VBATT_LPF_FREQ);
vbat = batteryAdcToVoltage(vbatFiltered);
float delta = micros() * 0.000001f;
vbatSample = filterApplyPt1(vbatSample, &vbatFilterState, VBATT_LPF_FREQ, delta);
vbat = batteryAdcToVoltage(vbatSample);
}
#define VBATTERY_STABLE_DELAY 40

23
src/test/Makefile
View File

@ -332,29 +332,6 @@ $(OBJECT_DIR)/ws2811_unittest : \
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(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 $@
$(OBJECT_DIR)/lowpass_unittest : \
$(OBJECT_DIR)/flight/lowpass.o \
$(OBJECT_DIR)/lowpass_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/flight/mixer.o : \
$(USER_DIR)/flight/mixer.c \
$(USER_DIR)/flight/mixer.h \

2
src/test/unit/battery_unittest.cc
View File

@ -22,9 +22,7 @@
extern "C" {
#include "sensors/battery.h"
#include "io/rc_controls.h"
#include "flight/lowpass.h"
#include "io/beeper.h"
}

131
src/test/unit/lowpass_unittest.cc
View File

@ -1,131 +0,0 @@
/*
* 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 <stdint.h>
#include <limits.h>
//#define DEBUG_LOWPASS
extern "C" {
#include "flight/lowpass.h"
}
static lowpass_t lowpassFilterReference;
static lowpass_t lowpassFilter;
#include "unittest_macros.h"
#include "gtest/gtest.h"
static float lowpassRef(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) {
generateLowpassCoeffs2(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;
}
TEST(LowpassTest, Lowpass)
{
int16_t servoCmds[3000];
int16_t expectedOut[3000];
int16_t referenceOut;
int16_t filterOut;
uint16_t sampleIdx;
int16_t freq;
uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t);
// generate inputs and expecteds
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
if (sampleIdx < 1000) {
servoCmds[sampleIdx] = 500;
} else if (sampleIdx >= 1000 && sampleIdx < 2000) {
servoCmds[sampleIdx] = 2500;
} else {
servoCmds[sampleIdx] = 1500;
}
if ((sampleIdx >= 900 && sampleIdx < 1000) ||
(sampleIdx >= 1900 && sampleIdx < 2000)||
(sampleIdx >= 2900 && sampleIdx < 3000)) {
expectedOut[sampleIdx] = servoCmds[sampleIdx];
} else {
expectedOut[sampleIdx] = -1;
}
}
// Test all frequencies
for (freq = 10; freq <= 400; freq++) {
#ifdef DEBUG_LOWPASS
printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
#endif
// Run tests
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++)
{
// Filter under test
filterOut = (int16_t)lowpassFixed(&lowpassFilter, servoCmds[sampleIdx], freq);
// Floating-point reference
referenceOut = (int16_t)(lowpassRef(&lowpassFilterReference, (float)servoCmds[sampleIdx], freq) + 0.5f);
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
}
// STUBS
extern "C" {
}