mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 09:39:33 +00:00
Transfer gyro low pass filter to estimate.ino
Separate raw gyro data and filtered rates to different variables
This commit is contained in:
parent
24e8569905
commit
41a9a95747
@ -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.
|
||||
|
||||
|
@ -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);
|
||||
|
@ -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();
|
||||
}
|
||||
|
@ -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]
|
||||
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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
|
||||
|
Loading…
x
Reference in New Issue
Block a user