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