aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-02-17 15:41:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Transfer gyro low pass filter to estimate.ino

Browse Source 
Separate raw gyro data and filtered rates to different variables
This commit is contained in:
Oleg Kalachev
2024-04-20 14:52:01 +03:00
parent 24e8569905
commit 41a9a95747
8 changed files with 21 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
flix/control.ino
View File

@@ -32,7 +32,6 @@
#define YAWRATE_MAX radians(360)
#define MAX_TILT radians(30)
#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
enum { MANUAL, ACRO, STAB, USER } mode = STAB;
@@ -46,8 +45,6 @@ PID rollPID(ROLL_P, ROLL_I, ROLL_D);
PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
PID yawPID(YAW_P, 0, 0);
LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
Quaternion attitudeTarget;
Vector ratesTarget;
Vector torqueTarget;
@@ -138,8 +135,7 @@ void controlRate() {
return;
}
Vector ratesFiltered = ratesFilter.update(rates);
Vector error = ratesTarget - ratesFiltered;
Vector error = ratesTarget - rates;
// Calculate desired torque, where 0 - no torque, 1 - maximum possible torque
torqueTarget.x = rollRatePID.update(error.x, dt);