Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
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.inoindex e07bd07..e66cf38 100644--- a/flix/estimate.ino+++ b/flix/estimate.ino@@ -13,11 +13,13 @@ Quaternion attitude; // estimated attitudebool landed;float accWeight = 0.003;+float levelWeight = 0.0002;LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hzvoid estimate() {applyGyro();applyAcc();+ applyLevel();}void applyGyro() {@@ -42,3 +44,12 @@ void applyAcc() {// apply correctionattitude = 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.inoindex 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.hindex 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();