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

Filter Cleanup CF coding style// Remove Old pt1 for acc etc // F1 slower acc update

This commit is contained in:
borisbstyle 2016-01-17 22:33:30 +01:00
parent e15ee54513
commit a105af1225
11 changed files with 52 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,37 +30,22 @@
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_GAIN 6.0f /* gain in db */
#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, uint8_t f_cut, float dT) {
// Pre calculate and store RC
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
}
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
return filter->state;
}
/* sets up a biquad Filter */
biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate)
{
float samplingRate;
samplingRate = 1 / (targetLooptime * 0.000001f);
biquad_t *newState;
if (!refreshRate) {
samplingRate = 1 / (targetLooptime * 0.000001f);
} else {
samplingRate = refreshRate;
}
float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2;
newState = malloc(sizeof(biquad_t));
if (newState == NULL)
return NULL;
/* setup variables */
omega = 2 * M_PI_FLOAT * (float) filterCutFreq / samplingRate;
sn = sinf(omega);
@ -84,8 +69,6 @@ biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
/* zero initial samples */
newState->x1 = newState->x2 = 0;
newState->y1 = newState->y2 = 0;
return newState;
}
/* Computes a biquad_t filter on a sample */

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

@ -15,20 +15,11 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#define FILTER_TAPS 14
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 a0, a1, a2, a3, a4;
float x1, x2, y1, y2;
} biquad_t;
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
float applyBiQuadFilter(float sample, biquad_t *state);
biquad_t *BiQuadNewLpf(uint8_t filterCutFreq);
void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate);

6
src/main/config/config.c
View File

@ -493,7 +493,7 @@ static void resetConf(void)
resetRollAndPitchTrims(&currentProfile->accelerometerTrims);
currentProfile->mag_declination = 0;
currentProfile->acc_cut_hz = 15;
currentProfile->acc_lpf_hz = 20;
currentProfile->accz_lpf_cutoff = 5.0f;
currentProfile->accDeadband.xy = 40;
currentProfile->accDeadband.z = 40;
@ -748,7 +748,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, &currentProfile->pidProfile.gyro_lpf_hz);
useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_lpf_hz);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
@ -778,7 +778,7 @@ void activateConfig(void)
imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
imuRuntimeConfig.acc_cut_hz = currentProfile->acc_cut_hz;
imuRuntimeConfig.acc_cut_hz = currentProfile->acc_lpf_hz;
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;
imuRuntimeConfig.small_angle = masterConfig.small_angle;

2
src/main/config/config_profile.h
View File

@ -28,7 +28,7 @@ typedef struct profile_s {
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
// sensor-related stuff
uint8_t acc_cut_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t acc_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
float accz_lpf_cutoff; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
accDeadband_t accDeadband;

12
src/main/flight/imu.c
View File

@ -377,7 +377,8 @@ static bool isMagnetometerHealthy(void)
static void imuCalculateEstimatedAttitude(void)
{
static filterStatePt1_t accLPFState[3];
static biquad_t accLPFState[3];
static bool accStateIsSet;
static uint32_t previousIMUUpdateTime;
float rawYawError = 0;
int32_t axis;
@ -391,8 +392,13 @@ static void imuCalculateEstimatedAttitude(void)
// Smooth and use only valid accelerometer readings
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz > 0) {
accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
if (imuRuntimeConfig->acc_cut_hz) {
if (accStateIsSet) {
accSmooth[axis] = lrintf(applyBiQuadFilter((float) accADC[axis], &accLPFState[axis]));
} else {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(imuRuntimeConfig->acc_cut_hz, &accLPFState[axis], 1000);
accStateIsSet = true;
}
} else {
accSmooth[axis] = accADC[axis];
}

10
src/main/flight/pid.c
View File

@ -113,7 +113,7 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static airModePlus_t airModePlusAxisState[3];
static biquad_t *deltaBiQuadState[3];
static biquad_t deltaBiQuadState[3];
static bool deltaStateIsSet;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
@ -128,7 +128,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
deltaStateIsSet = true;
}
@ -215,7 +215,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
lastError[axis] = RateError;
if (deltaStateIsSet) {
delta = applyBiQuadFilter(delta, deltaBiQuadState[axis]);
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
@ -259,7 +259,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int8_t horizonLevelStrength = 100;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
deltaStateIsSet = true;
}
@ -348,7 +348,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
lastError[axis] = RateError;
if (deltaStateIsSet) {
delta = lrintf(applyBiQuadFilter((float) delta, deltaBiQuadState[axis]));
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis]));
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference

2
src/main/io/serial_cli.c
View File

@ -602,7 +602,7 @@ const clivalue_t valueTable[] = {
#endif
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, .config.minmax = { 0, ACC_MAX } },
{ "acc_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_cut_hz, .config.minmax = { 0, 200 } },
{ "acc_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_hz, .config.minmax = { 0, 200 } },
{ "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, .config.minmax = { 0, 100 } },
{ "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, .config.minmax = { 0, 100 } },
{ "accz_lpf_cutoff", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].accz_lpf_cutoff, .config.minmax = { 1, 20 } },

10
src/main/main.c
View File

@ -549,15 +549,21 @@ int main(void) {
setTaskEnabled(TASK_GYROPID, true);
if(sensors(SENSOR_ACC)) {
uint32_t accTargetLooptime = 0;
setTaskEnabled(TASK_ACCEL, true);
switch(targetLooptime) {
case(500):
rescheduleTask(TASK_ACCEL, 10000);
accTargetLooptime = 10000;
break;
default:
case(1000):
rescheduleTask(TASK_ACCEL, 1000);
#ifdef STM32F10X
accTargetLooptime = 3000;
#else
accTargetLooptime = 1000;
#endif
}
rescheduleTask(TASK_ACCEL, accTargetLooptime);
}
setTaskEnabled(TASK_SERIAL, true);
setTaskEnabled(TASK_BEEPER, true);

20
src/main/mw.c
View File

@ -121,8 +121,6 @@ extern uint32_t currentTime;
extern uint8_t PIDweight[3];
static bool isRXDataNew;
static filterStatePt1_t filteredCycleTimeState;
uint16_t filteredCycleTime;
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
@ -179,10 +177,21 @@ 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(1, &filteredCycleTimeState, 0);
filterIsSet = true;
}
filteredCycleTime = applyBiQuadFilter((float) cycleTime, &filteredCycleTimeState);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
if (isRXDataNew) {
@ -657,13 +666,6 @@ void taskMainPidLoop(void)
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)targetLooptime * 0.000001f;
// Calculate average cycle time and average jitter
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
#if defined JITTER_DEBUG
debug[JITTER_DEBUG] = cycleTime - filteredCycleTime;
#endif
imuUpdateGyroAndAttitude();
annexCode();

10
src/main/sensors/gyro.c
View File

@ -41,7 +41,7 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static biquad_t *gyroBiQuadState[3];
static biquad_t gyroBiQuadState[3];
static bool gyroFilterStateIsSet;
static uint8_t gyroLpfCutFreq;
int axis;
@ -49,15 +49,15 @@ int axis;
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz)
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz)
{
gyroConfig = gyroConfigToUse;
gyroLpfCutFreq = *gyro_lpf_hz;
gyroLpfCutFreq = gyro_lpf_hz;
}
void initGyroFilterCoefficients(void) {
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
for (axis = 0; axis < 3; axis++) gyroBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(gyroLpfCutFreq);
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroBiQuadState[axis], 0);
gyroFilterStateIsSet = true;
}
}
@ -143,7 +143,7 @@ void gyroUpdate(void)
if (!gyroFilterStateIsSet) {
initGyroFilterCoefficients();
} else {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], gyroBiQuadState[axis]));
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]));
}
}

2
src/main/sensors/gyro.h
View File

@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz);
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);