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

Replace old lowpass with BiQuad for Vbat and Servo

Browse source
This commit is contained in:
borisbstyle 2016-02-03 14:33:43 +01:00
parent eebacc2542
commit 27772ba56a
7 changed files with 23 additions and 170 deletions
Download patch file Download diff file Expand all files Collapse all files

13
src/main/flight/mixer.c
View file

@ -19,6 +19,7 @@
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "debug.h"
@ -27,6 +28,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "drivers/system.h"
#include "drivers/pwm_output.h"
@ -34,6 +36,7 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "rx/rx.h"
@ -48,7 +51,6 @@
#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"
@ -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[] = {
@ -924,6 +925,8 @@ void filterServos(void)
{
#ifdef USE_SERVOS
int16_t servoIdx;
static bool servoFilterIsSet;
biquad_t servoFitlerState[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
@ -931,7 +934,11 @@ 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);
if (!servoFilterIsSet) {
BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFitlerState[servoIdx], targetLooptime);
servoFilterIsSet = true;
}
servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFitlerState[servoIdx]));
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);