Emaxx Navigation Group
Mavlink Messages for Emaxx Nav Board
Dependents: Madpulse_Speed_Control_temp Madpulse_Speed_Control_Students
Diff: mavlink_conversions.h
- Revision:
- 1:7e9af9a921f7
- Parent:
- 0:bb2cacd02294
diff -r bb2cacd02294 -r 7e9af9a921f7 mavlink_conversions.h
--- a/mavlink_conversions.h Fri Jan 20 13:20:58 2017 +0000
+++ b/mavlink_conversions.h Fri Jan 20 13:27:11 2017 +0000
@@ -1,212 +1,212 @@
-#ifndef _MAVLINK_CONVERSIONS_H_
-#define _MAVLINK_CONVERSIONS_H_
-
-/* enable math defines on Windows */
-#ifdef _MSC_VER
-#ifndef _USE_MATH_DEFINES
-#define _USE_MATH_DEFINES
-#endif
-#endif
-#include <math.h>
-
-#ifndef M_PI_2
- #define M_PI_2 ((float)asin(1))
-#endif
-
-/**
- * @file mavlink_conversions.h
- *
- * These conversion functions follow the NASA rotation standards definition file
- * available online.
- *
- * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
- * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
- * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
- * protocol as widely as possible.
- *
- * @author James Goppert
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-
-/**
- * Converts a quaternion to a rotation matrix
- *
- * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
- * @param dcm a 3x3 rotation matrix
- */
-MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])
-{
- double a = quaternion[0];
- double b = quaternion[1];
- double c = quaternion[2];
- double d = quaternion[3];
- double aSq = a * a;
- double bSq = b * b;
- double cSq = c * c;
- double dSq = d * d;
- dcm[0][0] = aSq + bSq - cSq - dSq;
- dcm[0][1] = 2 * (b * c - a * d);
- dcm[0][2] = 2 * (a * c + b * d);
- dcm[1][0] = 2 * (b * c + a * d);
- dcm[1][1] = aSq - bSq + cSq - dSq;
- dcm[1][2] = 2 * (c * d - a * b);
- dcm[2][0] = 2 * (b * d - a * c);
- dcm[2][1] = 2 * (a * b + c * d);
- dcm[2][2] = aSq - bSq - cSq + dSq;
-}
-
-
-/**
- * Converts a rotation matrix to euler angles
- *
- * @param dcm a 3x3 rotation matrix
- * @param roll the roll angle in radians
- * @param pitch the pitch angle in radians
- * @param yaw the yaw angle in radians
- */
-MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)
-{
- float phi, theta, psi;
- theta = asin(-dcm[2][0]);
-
- if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
- phi = 0.0f;
- psi = (atan2f(dcm[1][2] - dcm[0][1],
- dcm[0][2] + dcm[1][1]) + phi);
-
- } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
- phi = 0.0f;
- psi = atan2f(dcm[1][2] - dcm[0][1],
- dcm[0][2] + dcm[1][1] - phi);
-
- } else {
- phi = atan2f(dcm[2][1], dcm[2][2]);
- psi = atan2f(dcm[1][0], dcm[0][0]);
- }
-
- *roll = phi;
- *pitch = theta;
- *yaw = psi;
-}
-
-
-/**
- * Converts a quaternion to euler angles
- *
- * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
- * @param roll the roll angle in radians
- * @param pitch the pitch angle in radians
- * @param yaw the yaw angle in radians
- */
-MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)
-{
- float dcm[3][3];
- mavlink_quaternion_to_dcm(quaternion, dcm);
- mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw);
-}
-
-
-/**
- * Converts euler angles to a quaternion
- *
- * @param roll the roll angle in radians
- * @param pitch the pitch angle in radians
- * @param yaw the yaw angle in radians
- * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
- */
-MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
-{
- float cosPhi_2 = cosf(roll / 2);
- float sinPhi_2 = sinf(roll / 2);
- float cosTheta_2 = cosf(pitch / 2);
- float sinTheta_2 = sinf(pitch / 2);
- float cosPsi_2 = cosf(yaw / 2);
- float sinPsi_2 = sinf(yaw / 2);
- quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
- sinPhi_2 * sinTheta_2 * sinPsi_2);
- quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
- cosPhi_2 * sinTheta_2 * sinPsi_2);
- quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
- sinPhi_2 * cosTheta_2 * sinPsi_2);
- quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
- sinPhi_2 * sinTheta_2 * cosPsi_2);
-}
-
-
-/**
- * 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])
-{
- 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;
- int i;
- for (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;
- }
-}
-
-
-/**
- * Converts euler angles to a rotation matrix
- *
- * @param roll the roll angle in radians
- * @param pitch the pitch angle in radians
- * @param yaw the yaw angle in radians
- * @param dcm a 3x3 rotation matrix
- */
-MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
-{
- float cosPhi = cosf(roll);
- float sinPhi = sinf(roll);
- float cosThe = cosf(pitch);
- float sinThe = sinf(pitch);
- float cosPsi = cosf(yaw);
- float sinPsi = sinf(yaw);
-
- dcm[0][0] = cosThe * cosPsi;
- dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
- dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
-
- dcm[1][0] = cosThe * sinPsi;
- dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
- dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
-
- dcm[2][0] = -sinThe;
- dcm[2][1] = sinPhi * cosThe;
- dcm[2][2] = cosPhi * cosThe;
-}
-
-#endif
+#ifndef _MAVLINK_CONVERSIONS_H_
+#define _MAVLINK_CONVERSIONS_H_
+
+/* enable math defines on Windows */
+#ifdef _MSC_VER
+#ifndef _USE_MATH_DEFINES
+#define _USE_MATH_DEFINES
+#endif
+#endif
+#include <math.h>
+
+#ifndef M_PI_2
+ #define M_PI_2 ((float)asin(1))
+#endif
+
+/**
+ * @file mavlink_conversions.h
+ *
+ * These conversion functions follow the NASA rotation standards definition file
+ * available online.
+ *
+ * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
+ * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
+ * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
+ * protocol as widely as possible.
+ *
+ * @author James Goppert
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+/**
+ * Converts a quaternion to a rotation matrix
+ *
+ * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
+ * @param dcm a 3x3 rotation matrix
+ */
+MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])
+{
+ double a = quaternion[0];
+ double b = quaternion[1];
+ double c = quaternion[2];
+ double d = quaternion[3];
+ double aSq = a * a;
+ double bSq = b * b;
+ double cSq = c * c;
+ double dSq = d * d;
+ dcm[0][0] = aSq + bSq - cSq - dSq;
+ dcm[0][1] = 2 * (b * c - a * d);
+ dcm[0][2] = 2 * (a * c + b * d);
+ dcm[1][0] = 2 * (b * c + a * d);
+ dcm[1][1] = aSq - bSq + cSq - dSq;
+ dcm[1][2] = 2 * (c * d - a * b);
+ dcm[2][0] = 2 * (b * d - a * c);
+ dcm[2][1] = 2 * (a * b + c * d);
+ dcm[2][2] = aSq - bSq - cSq + dSq;
+}
+
+
+/**
+ * Converts a rotation matrix to euler angles
+ *
+ * @param dcm a 3x3 rotation matrix
+ * @param roll the roll angle in radians
+ * @param pitch the pitch angle in radians
+ * @param yaw the yaw angle in radians
+ */
+MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)
+{
+ float phi, theta, psi;
+ theta = asin(-dcm[2][0]);
+
+ // if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
+ // phi = 0.0f;
+ // psi = (atan2f(dcm[1][2] - dcm[0][1],
+ // dcm[0][2] + dcm[1][1]) + phi);
+//
+// } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
+ // phi = 0.0f;
+ // psi = atan2f(dcm[1][2] - dcm[0][1],
+ // dcm[0][2] + dcm[1][1] - phi);
+//
+ // } else {
+ phi = atan2f(dcm[2][1], dcm[2][2]);
+ psi = atan2f(dcm[1][0], dcm[0][0]);
+ // }
+//
+ *roll = phi;
+ *pitch = theta;
+ *yaw = psi;
+}
+
+/**
+ * Converts a quaternion to euler angles
+ *
+ * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
+ * @param roll the roll angle in radians
+ * @param pitch the pitch angle in radians
+ * @param yaw the yaw angle in radians
+ */
+MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)
+{
+ float dcm[3][3];
+ mavlink_quaternion_to_dcm(quaternion, dcm);
+ mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw);
+}
+
+
+/**
+ * Converts euler angles to a quaternion
+ *
+ * @param roll the roll angle in radians
+ * @param pitch the pitch angle in radians
+ * @param yaw the yaw angle in radians
+ * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
+ */
+MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
+{
+ float cosPhi_2 = cosf(roll / 2);
+ float sinPhi_2 = sinf(roll / 2);
+ float cosTheta_2 = cosf(pitch / 2);
+ float sinTheta_2 = sinf(pitch / 2);
+ float cosPsi_2 = cosf(yaw / 2);
+ float sinPsi_2 = sinf(yaw / 2);
+ quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
+ sinPhi_2 * sinTheta_2 * sinPsi_2);
+ quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
+ cosPhi_2 * sinTheta_2 * sinPsi_2);
+ quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
+ sinPhi_2 * cosTheta_2 * sinPsi_2);
+ quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
+ sinPhi_2 * sinTheta_2 * cosPsi_2);
+}
+
+
+/**
+ * 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])
+{
+ 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;
+ int i;
+ for (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;
+ }
+}
+
+
+/**
+ * Converts euler angles to a rotation matrix
+ *
+ * @param roll the roll angle in radians
+ * @param pitch the pitch angle in radians
+ * @param yaw the yaw angle in radians
+ * @param dcm a 3x3 rotation matrix
+ */
+MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
+{
+ float cosPhi = cosf(roll);
+ float sinPhi = sinf(roll);
+ float cosThe = cosf(pitch);
+ float sinThe = sinf(pitch);
+ float cosPsi = cosf(yaw);
+ float sinPsi = sinf(yaw);
+
+ dcm[0][0] = cosThe * cosPsi;
+ dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
+ dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
+
+ dcm[1][0] = cosThe * sinPsi;
+ dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
+ dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
+
+ dcm[2][0] = -sinThe;
+ dcm[2][1] = sinPhi * cosThe;
+ dcm[2][2] = cosPhi * cosThe;
+}
+
+#endif
+