From ff3ff8d8b2ee7530e5e819a7f99cb7f0add6238f Mon Sep 17 00:00:00 2001
From: Kyle Isom <kyle@imap.cc>
Date: Tue, 6 Aug 2019 23:42:51 -0700
Subject: [PATCH] Add Euler angles to Madgwick and update docs.

---
 docs/sphinx/index.rst            |  2 +-
 docs/sphinx/overview.rst         |  3 ++-
 include/wrmath/filter/madgwick.h | 10 ++++++++++
 test/madgwick_test.cc            |  9 ++++++++-
 update-docs                      |  3 +--
 5 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/docs/sphinx/index.rst b/docs/sphinx/index.rst
index c1dcea6..2d5130f 100644
--- a/docs/sphinx/index.rst
+++ b/docs/sphinx/index.rst
@@ -10,7 +10,7 @@ libwrmath: WNTRMUTE ROBOTICS' maths library
    :maxdepth: 3
    :caption: Contents:
 
-   api
+   overview
    vector
    quaternion
    resources
diff --git a/docs/sphinx/overview.rst b/docs/sphinx/overview.rst
index 4d803af..b42f7db 100644
--- a/docs/sphinx/overview.rst
+++ b/docs/sphinx/overview.rst
@@ -15,7 +15,8 @@ Coordinate systems
 
 The library uses a left-hand coordinate system where +x is north, +y is
 east, and +z is up. Accordingly, where Euler angles are concerned, the
-ZYX (yaw / pitch / roll) axes are used.
+ZYX (yaw / pitch / roll) axes are used. The code follows the convention
+of using ψ, θ, ϕ to represent yaw, pitch, and roll.
 
 Important classes
 -----------------
diff --git a/include/wrmath/filter/madgwick.h b/include/wrmath/filter/madgwick.h
index 11fdff7..618c485 100644
--- a/include/wrmath/filter/madgwick.h
+++ b/include/wrmath/filter/madgwick.h
@@ -91,6 +91,16 @@ public:
 		this->updateFrame(this->sensorFrame + q, delta);
 	}
 
+
+	/// Retrieve a vector of the Euler angles in ZYX orientation.
+	///
+	/// \return A vector of Euler angles as <ψ, θ, ϕ>.
+	geom::Vector<T, 3>
+	euler()
+	{
+		return this->sensorFrame.euler();
+	}
+
 private:
 	T			deltaT;
 	geom::Quaternion<T>	previousSensorFrame;
diff --git a/test/madgwick_test.cc b/test/madgwick_test.cc
index 45c9c5a..eeb190a 100644
--- a/test/madgwick_test.cc
+++ b/test/madgwick_test.cc
@@ -3,6 +3,7 @@
 #include <gtest/gtest.h>
 #include <wrmath/geom/vector.h>
 #include <wrmath/geom/quaternion.h>
+#include <wrmath/math.h>
 #include <wrmath/filter/madgwick.h>
 
 using namespace std;
@@ -12,9 +13,10 @@ using namespace wr;
 TEST(MadgwickFilter, SimpleAngularOrientation)
 {
 	filter::Madgwickd	mf;
-	geom::Vector3d		gyro {0.17453292519943295, 0.0, 0.0}; // 10° X rotation.
+	geom::Vector3d		gyro {0.174533, 0.0, 0.0}; // 10° X rotation.
 	geom::Quaterniond	frame20Deg {0.984808, 0.173648, 0, 0}; // 20° final orientation.
 	double 			delta = 0.00917; // assume 109 updates per second, as per the paper.
+	double			twentyDegrees = math::DegreesToRadiansD(20.0);
 
 	// The paper specifies a minimum of 109 IMU readings to stabilize; for
 	// two seconds, that means 218 updates.
@@ -23,6 +25,11 @@ TEST(MadgwickFilter, SimpleAngularOrientation)
 	}
 
 	EXPECT_EQ(mf.orientation(), frame20Deg);
+
+	auto			euler = mf.euler();
+	EXPECT_NEAR(euler[0], twentyDegrees, 0.01);
+	EXPECT_NEAR(euler[1], 0.0, 0.01);
+	EXPECT_NEAR(euler[2], 0.0, 0.01);
 }
 
 
diff --git a/update-docs b/update-docs
index ccde62d..c63ee77 100755
--- a/update-docs
+++ b/update-docs
@@ -8,5 +8,4 @@ else
 	HOST="freeside.wntrmute.net:"
 fi
 
-cd docs && doxygen && rsync -auv html/ ${HOST}sites/wntrmute-dev/wrmath/
-cd sphinx && make html && rsync -auv _build/html/ ${HOST}sites/wntrmute-dev/sphinx/
+cd docs/sphinx && make html && rsync -auv _build/html/ ${HOST}sites/wrmath/