mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 17:49:33 +00:00
Implement low pass filter in separate module
This commit is contained in:
parent
a04d713157
commit
7cabdc5e62
@ -6,6 +6,7 @@
|
|||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
#include "quaternion.h"
|
#include "quaternion.h"
|
||||||
#include "pid.h"
|
#include "pid.h"
|
||||||
|
#include "lpf.h"
|
||||||
|
|
||||||
#define PITCHRATE_P 0.05
|
#define PITCHRATE_P 0.05
|
||||||
#define PITCHRATE_I 0.2
|
#define PITCHRATE_I 0.2
|
||||||
@ -31,17 +32,20 @@
|
|||||||
#define YAWRATE_MAX 360 * DEG_TO_RAD
|
#define YAWRATE_MAX 360 * DEG_TO_RAD
|
||||||
#define MAX_TILT 30 * DEG_TO_RAD
|
#define MAX_TILT 30 * DEG_TO_RAD
|
||||||
|
|
||||||
|
#define RATES_LFP_ALPHA 0.8
|
||||||
|
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
||||||
|
|
||||||
enum { MANUAL, ACRO, STAB } mode = STAB;
|
enum { MANUAL, ACRO, STAB } mode = STAB;
|
||||||
bool armed = false;
|
bool armed = false;
|
||||||
|
|
||||||
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM);
|
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
|
||||||
PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM);
|
PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
|
||||||
PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
|
PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
|
||||||
PID rollPID(ROLL_P, ROLL_I, ROLL_D);
|
PID rollPID(ROLL_P, ROLL_I, ROLL_D);
|
||||||
PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
|
PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
|
||||||
PID yawPID(YAW_P, 0, 0);
|
PID yawPID(YAW_P, 0, 0);
|
||||||
|
|
||||||
Vector ratesFiltered;
|
LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
|
||||||
|
|
||||||
Quaternion attitudeTarget;
|
Quaternion attitudeTarget;
|
||||||
Vector ratesTarget;
|
Vector ratesTarget;
|
||||||
@ -206,7 +210,7 @@ void controlRate()
|
|||||||
// collective thrust is throttle * 4
|
// collective thrust is throttle * 4
|
||||||
thrustTarget = controls[RC_CHANNEL_THROTTLE]; // WARNING:
|
thrustTarget = controls[RC_CHANNEL_THROTTLE]; // WARNING:
|
||||||
|
|
||||||
ratesFiltered = rates * 0.8 + ratesFiltered * 0.2; // cut-off frequency 40 Hz
|
Vector ratesFiltered = ratesFilter.update(rates);
|
||||||
|
|
||||||
torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
|
torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
|
||||||
torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);
|
torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);
|
||||||
|
42
flix/lpf.h
Normal file
42
flix/lpf.h
Normal file
@ -0,0 +1,42 @@
|
|||||||
|
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||||
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
|
// Low pass filter implementation
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
template <typename T> // Using template to make the filter usable for scalar and vector values
|
||||||
|
class LowPassFilter
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
float alpha; // smoothing constant, 1 means filter disabled
|
||||||
|
T output;
|
||||||
|
|
||||||
|
LowPassFilter(float alpha): alpha(alpha) {};
|
||||||
|
|
||||||
|
T update(const T input)
|
||||||
|
{
|
||||||
|
if (alpha == 1) { // filter disabled
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!initialized) {
|
||||||
|
output = input;
|
||||||
|
initialized = true;
|
||||||
|
}
|
||||||
|
return output = output * (1 - alpha) + input * alpha;
|
||||||
|
}
|
||||||
|
|
||||||
|
void setCutOffFrequency(float cutOffFreq, float dt)
|
||||||
|
{
|
||||||
|
alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
void reset()
|
||||||
|
{
|
||||||
|
initialized = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool initialized = false;
|
||||||
|
};
|
13
flix/pid.h
13
flix/pid.h
@ -5,6 +5,8 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "lpf.h"
|
||||||
|
|
||||||
class PID
|
class PID
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
@ -16,15 +18,20 @@ public:
|
|||||||
float derivative = 0;
|
float derivative = 0;
|
||||||
float integral = 0;
|
float integral = 0;
|
||||||
|
|
||||||
PID(float p, float i, float d, float windup = 0) : p(p), i(i), d(d), windup(windup) {};
|
LowPassFilter<float> lpf; // low pass filter for derivative term
|
||||||
|
|
||||||
|
PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {};
|
||||||
|
|
||||||
float update(float error, float dt)
|
float update(float error, float dt)
|
||||||
{
|
{
|
||||||
integral += error * dt;
|
integral += error * dt;
|
||||||
|
|
||||||
if (isfinite(prevError) && dt > 0) {
|
if (isfinite(prevError) && dt > 0) {
|
||||||
// calculate derivative if both dt and prevError are valid
|
// calculate derivative if both dt and prevError are valid
|
||||||
float _derivative = (error - prevError) / dt;
|
derivative = (error - prevError) / dt;
|
||||||
derivative = derivative * 0.8 + 0.2 * _derivative; // lpf WARNING:
|
|
||||||
|
// apply low pass filter to derivative
|
||||||
|
derivative = lpf.update(derivative);
|
||||||
}
|
}
|
||||||
|
|
||||||
prevError = error;
|
prevError = error;
|
||||||
|
Loading…
x
Reference in New Issue
Block a user