aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-08-17 17:16:10 +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);

9
flix/estimate.ino
View File

@@ -5,9 +5,13 @@
#include "quaternion.h"
#include "vector.h"
#include "lpf.h"
#define ONE_G 9.807f
#define WEIGHT_ACC 0.5f
#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
void estimate() {
applyGyro();
@@ -16,7 +20,10 @@ void estimate() {
}
void applyGyro() {
// applying gyro
// filter gyro to get angular rates
rates = ratesFilter.update(gyro);
// apply rates to attitude
attitude *= Quaternion::fromAngularRates(rates * dt);
attitude.normalize();
}

3
flix/flix.ino
View File

@@ -28,8 +28,9 @@ float dt; // time delta from previous step, s
float loopFreq; // loop frequency, Hz
int16_t channels[16]; // raw rc channels
float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
Vector rates; // angular rates, rad/s
Vector gyro; // gyroscope data
Vector acc; // accelerometer data, m/s/s
Vector rates; // filtered angular rates, rad/s
Quaternion attitude; // estimated attitude
float motors[4]; // normalized motors thrust in range [-1..1]

8
flix/imu.ino
View File

@@ -36,11 +36,11 @@ void configureIMU() {
void readIMU() {
IMU.waitForData();
IMU.getGyro(rates.x, rates.y, rates.z);
IMU.getGyro(gyro.x, gyro.y, gyro.z);
IMU.getAccel(acc.x, acc.y, acc.z);
// apply scale and bias
acc = (acc - accBias) / accScale;
rates = rates - gyroBias;
gyro = gyro - gyroBias;
}
void calibrateGyro() {
@@ -51,8 +51,8 @@ void calibrateGyro() {
gyroBias = Vector(0, 0, 0);
for (int i = 0; i < samples; i++) {
IMU.waitForData();
IMU.getGyro(rates.x, rates.y, rates.z);
gyroBias = gyroBias + rates;
IMU.getGyro(gyro.x, gyro.y, gyro.z);
gyroBias = gyroBias + gyro;
}
gyroBias = gyroBias / samples;

2
flix/mavlink.ino
View File

@@ -55,7 +55,7 @@ void sendMavlink() {
mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
acc.x * 1000, acc.y * 1000, acc.z * 1000,
rates.x * 1000, rates.y * 1000, rates.z * 1000,
gyro.x * 1000, gyro.y * 1000, gyro.z * 1000,
0, 0, 0, 0);
sendMessage(&msg);
}