Add getRoll, getPitch, setRoll, setPitch methods

Add methods to Quaternion for consistency with getYaw and setYaw

flix/estimate.ino

@@ -11,8 +11,6 @@
 #define WEIGHT_ACC 0.003
 #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
-
 void estimate() {
 	applyGyro();
 	applyAcc();
@@ -20,6 +18,7 @@ void estimate() {
 
 void applyGyro() {
 	// filter gyro to get angular rates
+	static LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
 	rates = ratesFilter.update(gyro);
 
 	// apply rates to attitude

flix/quaternion.h

@@ -116,29 +116,31 @@ public:
 		return euler;
 	}
 
+	float getRoll() const {
+		return toEuler().x;
+	}
+
+	float getPitch() const {
+		return toEuler().y;
+	}
+
 	float getYaw() const {
-		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122
-		float yaw;
-		float sqx = x * x;
-		float sqy = y * y;
-		float sqz = z * z;
-		float sqw = w * w;
-		double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
-		if (sarg <= -0.99999) {
-			yaw = -2 * atan2(y, x);
-		} else if (sarg >= 0.99999) {
-			yaw = 2 * atan2(y, x);
-		} else {
-			yaw = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);
-		}
-		return yaw;
+		return toEuler().z;
+	}
+
+	void setRoll(float roll) {
+		Vector euler = toEuler();
+		*this = Quaternion::fromEuler(Vector(roll, euler.y, euler.z));
+	}
+
+	void setPitch(float pitch) {
+		Vector euler = toEuler();
+		*this = Quaternion::fromEuler(Vector(euler.x, pitch, euler.z));
 	}
 
 	void setYaw(float yaw) {
-		// TODO: optimize?
 		Vector euler = toEuler();
-		euler.z = yaw;
-		(*this) = Quaternion::fromEuler(euler);
+		*this = Quaternion::fromEuler(Vector(euler.x, euler.y, yaw));
 	}
 
 	Quaternion operator * (const Quaternion& q) const {