Transfer gyro low pass filter to estimate.ino

Separate raw gyro data and filtered rates to different variables

docs/firmware.md

@@ -8,9 +8,10 @@ The main loop is running at 1000 Hz. All the dataflow is happening through globa
 
 * `t` *(float)* — current step time, *s*.
 * `dt` *(float)* — time delta between the current and previous steps, *s*.
-* `rates` *(Vector)* — angular rates from the gyroscope, *rad/s*.
+* `gyro` *(Vector)* — data from the gyroscope, *rad/s*.
 * `acc` *(Vector)* — acceleration data from the accelerometer, *m/s<sup>2</sup>*.
-* `attitude` *(Quaternion)* — current estimated attitude (orientation) of drone.
+* `rates` *(Vector)* — filtered angular rates, *rad/s*.
+* `attitude` *(Quaternion)* — estimated attitude (orientation) of drone.
 * `controls` *(float[])* — user control inputs from the RC, normalized to [-1, 1] range.
 * `motors` *(float[])* — motor outputs, normalized to [-1, 1] range; reverse rotation is possible.
 

flix/control.ino

@@ -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 - rates;
-	Vector error = ratesTarget - ratesFiltered;
 
 	// Calculate desired torque, where 0 - no torque, 1 - maximum possible torque
 	torqueTarget.x = rollRatePID.update(error.x, dt);

flix/estimate.ino

@@ -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();
 }

flix/flix.ino

@@ -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]
 

flix/imu.ino

@@ -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);
+		IMU.getGyro(gyro.x, gyro.y, gyro.z);
-		gyroBias = gyroBias + rates;
+		gyroBias = gyroBias + gyro;
 	}
 	gyroBias = gyroBias / samples;
 

flix/mavlink.ino

@@ -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);
 	}

gazebo/flix.h

@@ -26,6 +26,7 @@ float motors[4];
 int16_t channels[16]; // raw rc channels
 float controls[RC_CHANNELS];
 Vector acc;
+Vector gyro;
 Vector rates;
 Quaternion attitude;
 

gazebo/simulator.cpp

@@ -64,7 +64,7 @@ public:
 		step();
 
 		// read imu
-		rates = flu2frd(imu->AngularVelocity());
+		gyro = flu2frd(imu->AngularVelocity());
 		acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
 
 		// read rc