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