Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Improve soft filtering function for reuse

Browse source 
Reorder serial.c *_cut_hz parameters

Remove unnecessary dT calculation in luxfloat

Restructured filter

filter.h fix

Luxfloat remove internal dT

Void function for gyro fillter
This commit is contained in:
borisbstyle 2015-06-30 11:05:13 +02:00
parent 637fd64f36
commit fdcfe71b73
9 changed files with 86 additions and 68 deletions
Download patch file Download diff file Expand all files Collapse all files

35
src/main/mw.c
View file

@ -25,6 +25,7 @@
#include "common/maths.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -72,8 +73,6 @@
#include "flight/failsafe.h"
#include "flight/autotune.h"
#include "flight/navigation.h"
#include "flight/filter.h"
#include "config/runtime_config.h"
#include "config/config.h"
@ -96,6 +95,7 @@ enum {
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
float dT;
int16_t magHold;
int16_t headFreeModeHold;
@ -681,6 +681,27 @@ void processRx(void)
}
// Gyro Low Pass
void filterGyro(void) {
int axis;
static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (masterConfig.looptime > 0) {
// Static dT calculation based on configured looptime
if (!gyroADCState[axis].constdT) {
gyroADCState[axis].constdT = (float)masterConfig.looptime * 0.000001f;
}
gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, gyroADCState[axis].constdT);
}
else {
gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dT);
}
}
}
void loop(void)
{
static uint32_t loopTime;
@ -736,14 +757,10 @@ void loop(void)
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
// Gyro Low Pass
if (currentProfile->pidProfile.gyro_cut_hz) {
int axis;
static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
dT = (float)cycleTime * 0.000001f;
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz);
}
if (currentProfile->pidProfile.gyro_cut_hz) {
filterGyro();
}
annexCode();