diff --git a/ASLUAV/version.h b/ASLUAV/version.h
index c2643f1a2fdd7c9b74d8c74c6da4fefdd7cce0ee..b1bbceb0e536df33155e80fe1adcaad77802fd82 100644
--- a/ASLUAV/version.h
+++ b/ASLUAV/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:47:01 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:10:33 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/ardupilotmega/version.h b/ardupilotmega/version.h
index 7f2f06e640fae1714e26b23a588c4fae4ffe6b53..9c486d2d9f85a0ab8163798987ee79925500e7b1 100644
--- a/ardupilotmega/version.h
+++ b/ardupilotmega/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:46:12 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:09:14 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/autoquad/version.h b/autoquad/version.h
index 69959c0222dec6d1fe70d896e17b59e69436f3ea..431cbb2f1329158d55e2ac56164e3a47378f0c93 100644
--- a/autoquad/version.h
+++ b/autoquad/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:46:19 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:09:25 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/common/version.h b/common/version.h
index 3154d3a6129f516dff3535b6c070b335907cc1f1..8421ff184544eebdcb9cf63f1e9638887fa873f9 100644
--- a/common/version.h
+++ b/common/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:47:06 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:10:41 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/matrixpilot/version.h b/matrixpilot/version.h
index 371163a9bb7c06030b230b9216fa55fa5c930146..f8947cb68077040cde97ed1100474f3c36f5c701 100644
--- a/matrixpilot/version.h
+++ b/matrixpilot/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:46:25 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:09:36 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/mavlink_conversions.h b/mavlink_conversions.h
index fb6025b0de9b849b9701666e1739699c8483e9d8..217d4160804e2c44aac97ceea9ff4de20a8bab54 100644
--- a/mavlink_conversions.h
+++ b/mavlink_conversions.h
@@ -25,6 +25,7 @@
  * protocol as widely as possible.
  *
  * @author James Goppert
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 
@@ -135,16 +136,44 @@ MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float y
 
 /**
  * Converts a rotation matrix to a quaternion
+ * Reference:
+ *  - Shoemake, Quaternions,
+ *  http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
  *
  * @param dcm a 3x3 rotation matrix
  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
  */
 MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
 {
-    quaternion[0] = 0.5f * sqrtf(1 + dcm[0][0] + dcm[1][1] + dcm[2][2]);
-    quaternion[1] = 0.5f * sqrtf(1 + dcm[0][0] - dcm[1][1] - dcm[2][2]);
-    quaternion[2] = 0.5f * sqrtf(1 - dcm[0][0] + dcm[1][1] - dcm[2][2]);
-    quaternion[3] = 0.5f * sqrtf(1 - dcm[0][0] - dcm[1][1] + dcm[2][2]);
+    float tr = dcm[0][0] + dcm[1][1] + dcm[2][2];
+    if (tr > 0.0f) {
+        float s = sqrtf(tr + 1.0f);
+        quaternion[0] = s * 0.5f;
+        s = 0.5f / s;
+        quaternion[1] = (dcm[2][1] - dcm[1][2]) * s;
+        quaternion[2] = (dcm[0][2] - dcm[2][0]) * s;
+        quaternion[3] = (dcm[1][0] - dcm[0][1]) * s;
+    } else {
+        /* Find maximum diagonal element in dcm
+         * store index in dcm_i */
+        int dcm_i = 0;
+        for (int i = 1; i < 3; i++) {
+            if (dcm[i][i] > dcm[dcm_i][dcm_i]) {
+                dcm_i = i;
+            }
+        }
+
+        int dcm_j = (dcm_i + 1) % 3;
+        int dcm_k = (dcm_i + 2) % 3;
+
+        float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
+                    dcm[dcm_k][dcm_k]) + 1.0f);
+        quaternion[dcm_i + 1] = s * 0.5f;
+        s = 0.5f / s;
+        quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;
+        quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;
+        quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s;
+    }
 }
 
 
diff --git a/minimal/version.h b/minimal/version.h
index 014b311eef6ff4b252a5f368040ff0e836ce327d..7de82c08e912c41dd1a66d1f8bf5d0d6167672e3 100644
--- a/minimal/version.h
+++ b/minimal/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:46:33 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:09:48 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 9
  
diff --git a/pixhawk/version.h b/pixhawk/version.h
index 4c10b7794a91a26a4984a47a0d7e7b12387f41e9..bdb46320e97e6f58815c2ebb681b0419ae7b1dc4 100644
--- a/pixhawk/version.h
+++ b/pixhawk/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:46:34 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:09:50 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/slugs/version.h b/slugs/version.h
index 45e1fcf07e2a6e15c6c277cc59f6d5dc41c88c96..2eeea39a0cd2a298f99850df7b1286e104824bd3 100644
--- a/slugs/version.h
+++ b/slugs/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:46:41 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:10:01 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/test/version.h b/test/version.h
index a388a3b4c73404863fa4445fe3aaab94c08b62d7..06bdd14fa0548a0dd01f9f29a88daff16a15f512 100644
--- a/test/version.h
+++ b/test/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:46:47 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:10:12 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179
  
diff --git a/ualberta/version.h b/ualberta/version.h
index b2232db6ad9afa37837b1978c35742d62de5dd2f..b752b5af57cce82b5ca9eb203f43e4d0342659aa 100644
--- a/ualberta/version.h
+++ b/ualberta/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Thu Feb 12 07:46:48 2015"
+#define MAVLINK_BUILD_DATE "Mon Feb 23 22:10:13 2015"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255