Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Reduce angle drift by adding level correction to the estimator Leverage a priori knowledge that the drone's average attitude is level. Explanation: https://t.me/opensourcequadcopter/158.

Oleg Kalachevbd270db4930e
diff --git a/flix/estimate.ino b/flix/estimate.ino
index e07bd07..e66cf38 100644
--- a/flix/estimate.ino
+++ b/flix/estimate.ino
@@ -13,11 +13,13 @@ Quaternion attitude; // estimated attitude
bool landed;
float accWeight = 0.003;
+float levelWeight = 0.0002;
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
void estimate() {
applyGyro();
applyAcc();
+ applyLevel();
}
void applyGyro() {
@@ -42,3 +44,12 @@ void applyAcc() {
// apply correction
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
}
+
+void applyLevel() {
+ if (landed) return;
+
+ // assume the pilot keeps the drone more or less level in flight
+ Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
+ Vector correction = Vector::rotationVectorBetween(Vector(0, 0, 1), up) * levelWeight;
+ attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
+}
diff --git a/flix/parameters.ino b/flix/parameters.ino
index 3117a53..4fc0063 100644
--- a/flix/parameters.ino
+++ b/flix/parameters.ino
@@ -73,6 +73,7 @@ Parameter parameters[] = {
{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
// estimate
{"EST_ACC_WEIGHT", &accWeight},
+ {"EST_LVL_WEIGHT", &levelWeight},
{"EST_RATES_LPF_A", &ratesFilter.alpha},
// motors
{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 899245f..ddac951 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -27,6 +27,7 @@ void step();
void computeLoopRate();
void applyGyro();
void applyAcc();
+void applyLevel();
void control();
void interpretControls();
void controlAttitude();