mirror of
https://github.com/okalachev/flix.git
synced 2026-01-10 04:57:17 +00:00
Compare commits
2 Commits
f1dc4a0400
...
0547ea548b
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
0547ea548b | ||
|
|
c02dba6812 |
@@ -8,8 +8,8 @@
|
|||||||
#include "lpf.h"
|
#include "lpf.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
#define WEIGHT_ACC 0.003
|
float accWeight = 0.003;
|
||||||
#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
||||||
|
|
||||||
void estimate() {
|
void estimate() {
|
||||||
applyGyro();
|
applyGyro();
|
||||||
@@ -18,7 +18,6 @@ void estimate() {
|
|||||||
|
|
||||||
void applyGyro() {
|
void applyGyro() {
|
||||||
// filter gyro to get angular rates
|
// filter gyro to get angular rates
|
||||||
static LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
|
|
||||||
rates = ratesFilter.update(gyro);
|
rates = ratesFilter.update(gyro);
|
||||||
|
|
||||||
// apply rates to attitude
|
// apply rates to attitude
|
||||||
@@ -34,7 +33,7 @@ void applyAcc() {
|
|||||||
|
|
||||||
// calculate accelerometer correction
|
// calculate accelerometer correction
|
||||||
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
|
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
|
||||||
Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
|
Vector correction = Vector::rotationVectorBetween(acc, up) * accWeight;
|
||||||
|
|
||||||
// apply correction
|
// apply correction
|
||||||
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
||||||
|
|||||||
@@ -53,8 +53,8 @@ void calibrateGyroOnce() {
|
|||||||
static Delay landedDelay(2);
|
static Delay landedDelay(2);
|
||||||
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
|
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
|
||||||
|
|
||||||
static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
|
static LowPassFilter<Vector> gyroBiasFilter(0.001);
|
||||||
gyroBias = gyroCalibrationFilter.update(gyro);
|
gyroBias = gyroBiasFilter.update(gyro);
|
||||||
}
|
}
|
||||||
|
|
||||||
void calibrateAccel() {
|
void calibrateAccel() {
|
||||||
|
|||||||
@@ -49,6 +49,9 @@ Parameter parameters[] = {
|
|||||||
{"IMU_ACC_SCALE_X", &accScale.x},
|
{"IMU_ACC_SCALE_X", &accScale.x},
|
||||||
{"IMU_ACC_SCALE_Y", &accScale.y},
|
{"IMU_ACC_SCALE_Y", &accScale.y},
|
||||||
{"IMU_ACC_SCALE_Z", &accScale.z},
|
{"IMU_ACC_SCALE_Z", &accScale.z},
|
||||||
|
// estimate
|
||||||
|
{"EST_ACC_WEIGHT", &accWeight},
|
||||||
|
{"EST_RATES_LPF_A", &ratesFilter.alpha},
|
||||||
// rc
|
// rc
|
||||||
{"RC_ZERO_0", &channelZero[0]},
|
{"RC_ZERO_0", &channelZero[0]},
|
||||||
{"RC_ZERO_1", &channelZero[1]},
|
{"RC_ZERO_1", &channelZero[1]},
|
||||||
|
|||||||
Reference in New Issue
Block a user