1  #ifndef _MAVLINK_CONVERSIONS_H_


2  #define _MAVLINK_CONVERSIONS_H_


3 


4  /* enable math defines on Windows */


5  #ifdef _MSC_VER


6  #ifndef _USE_MATH_DEFINES


7  #define _USE_MATH_DEFINES


8  #endif


9  #endif


10  #include <math.h>


11 


12  #ifndef M_PI_2


13  #define M_PI_2 ((float)asin(1))


14  #endif


15 


16  /**


17  * @file mavlink_conversions.h


18  *


19  * These conversion functions follow the NASA rotation standards definition file


20  * available online.


21  *


22  * Their intent is to lower the barrier for MAVLink adopters to use gimballock free


23  * (both rotation matrices, sometimes called DCM, and quaternions are gimballock free)


24  * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the


25  * protocol as widely as possible.


26  *


27  * @author James Goppert


28  * @author Thomas Gubler <thomasgubler@gmail.com>


29  */


30 


31 


32  /**


33  * Converts a quaternion to a rotation matrix


34  *


35  * @param quaternion a [w, x, y, z] ordered quaternion (nullrotation being 1 0 0 0)


36  * @param dcm a 3x3 rotation matrix


37  */


38  MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])


39  {


40  double a = quaternion[0];


41  double b = quaternion[1];


42  double c = quaternion[2];


43  double d = quaternion[3];


44  double aSq = a * a;


45  double bSq = b * b;


46  double cSq = c * c;


47  double dSq = d * d;


48  dcm[0][0] = aSq + bSq  cSq  dSq;


49  dcm[0][1] = 2 * (b * c  a * d);


50  dcm[0][2] = 2 * (a * c + b * d);


51  dcm[1][0] = 2 * (b * c + a * d);


52  dcm[1][1] = aSq  bSq + cSq  dSq;


53  dcm[1][2] = 2 * (c * d  a * b);


54  dcm[2][0] = 2 * (b * d  a * c);


55  dcm[2][1] = 2 * (a * b + c * d);


56  dcm[2][2] = aSq  bSq  cSq + dSq;


57  }


58 


59 


60  /**


61  * Converts a rotation matrix to euler angles


62  *


63  * @param dcm a 3x3 rotation matrix


64  * @param roll the roll angle in radians


65  * @param pitch the pitch angle in radians


66  * @param yaw the yaw angle in radians


67  */


68  MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)


69  {


70  float phi, theta, psi;


71  theta = asin(dcm[2][0]);


72 


73  if (fabsf(theta  (float)M_PI_2) < 1.0e3f) {


74  phi = 0.0f;


75  psi = (atan2f(dcm[1][2]  dcm[0][1],


76  dcm[0][2] + dcm[1][1]) + phi);


77 


78  } else if (fabsf(theta + (float)M_PI_2) < 1.0e3f) {


79  phi = 0.0f;


80  psi = atan2f(dcm[1][2]  dcm[0][1],


81  dcm[0][2] + dcm[1][1]  phi);


82 


83  } else {


84  phi = atan2f(dcm[2][1], dcm[2][2]);


85  psi = atan2f(dcm[1][0], dcm[0][0]);


86  }


87 


88  *roll = phi;


89  *pitch = theta;


90  *yaw = psi;


91  }


92 


93 


94  /**


95  * Converts a quaternion to euler angles


96  *


97  * @param quaternion a [w, x, y, z] ordered quaternion (nullrotation being 1 0 0 0)


98  * @param roll the roll angle in radians


99  * @param pitch the pitch angle in radians


100  * @param yaw the yaw angle in radians


101  */


102  MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)


103  {


104  float dcm[3][3];


105  mavlink_quaternion_to_dcm(quaternion, dcm);


106  mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw);


107  }


108 


109 


110  /**


111  * Converts euler angles to a quaternion


112  *


113  * @param roll the roll angle in radians


114  * @param pitch the pitch angle in radians


115  * @param yaw the yaw angle in radians


116  * @param quaternion a [w, x, y, z] ordered quaternion (nullrotation being 1 0 0 0)


117  */


118  MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])


119  {


120  float cosPhi_2 = cosf(roll / 2);


121  float sinPhi_2 = sinf(roll / 2);


122  float cosTheta_2 = cosf(pitch / 2);


123  float sinTheta_2 = sinf(pitch / 2);


124  float cosPsi_2 = cosf(yaw / 2);


125  float sinPsi_2 = sinf(yaw / 2);


126  quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +


127  sinPhi_2 * sinTheta_2 * sinPsi_2);


128  quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 


129  cosPhi_2 * sinTheta_2 * sinPsi_2);


130  quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +


131  sinPhi_2 * cosTheta_2 * sinPsi_2);


132  quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 


133  sinPhi_2 * sinTheta_2 * cosPsi_2);


134  }


135 


136 


137  /**


138  * Converts a rotation matrix to a quaternion


139  * Reference:


140  *  Shoemake, Quaternions,


141  * http://www.cs.ucr.edu/~vbz/resources/quatut.pdf


142  *


143  * @param dcm a 3x3 rotation matrix


144  * @param quaternion a [w, x, y, z] ordered quaternion (nullrotation being 1 0 0 0)


145  */


146  MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])


147  {


148  float tr = dcm[0][0] + dcm[1][1] + dcm[2][2];


149  if (tr > 0.0f) {


150  float s = sqrtf(tr + 1.0f);


151  quaternion[0] = s * 0.5f;


152  s = 0.5f / s;


153  quaternion[1] = (dcm[2][1]  dcm[1][2]) * s;


154  quaternion[2] = (dcm[0][2]  dcm[2][0]) * s;


155  quaternion[3] = (dcm[1][0]  dcm[0][1]) * s;


156  } else {


157  /* Find maximum diagonal element in dcm


158  * store index in dcm_i */


159  int dcm_i = 0;


160  int i;


161  for (i = 1; i < 3; i++) {


162  if (dcm[i][i] > dcm[dcm_i][dcm_i]) {


163  dcm_i = i;


164  }


165  }


166 


167  int dcm_j = (dcm_i + 1) % 3;


168  int dcm_k = (dcm_i + 2) % 3;


169 


170  float s = sqrtf((dcm[dcm_i][dcm_i]  dcm[dcm_j][dcm_j] 


171  dcm[dcm_k][dcm_k]) + 1.0f);


172  quaternion[dcm_i + 1] = s * 0.5f;


173  s = 0.5f / s;


174  quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;


175  quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;


176  quaternion[0] = (dcm[dcm_k][dcm_j]  dcm[dcm_j][dcm_k]) * s;


177  }


178  }


179 


180 


181  /**


182  * Converts euler angles to a rotation matrix


183  *


184  * @param roll the roll angle in radians


185  * @param pitch the pitch angle in radians


186  * @param yaw the yaw angle in radians


187  * @param dcm a 3x3 rotation matrix


188  */


189  MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])


190  {


191  float cosPhi = cosf(roll);


192  float sinPhi = sinf(roll);


193  float cosThe = cosf(pitch);


194  float sinThe = sinf(pitch);


195  float cosPsi = cosf(yaw);


196  float sinPsi = sinf(yaw);


197 


198  dcm[0][0] = cosThe * cosPsi;


199  dcm[0][1] = cosPhi * sinPsi + sinPhi * sinThe * cosPsi;


200  dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;


201 


202  dcm[1][0] = cosThe * sinPsi;


203  dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;


204  dcm[1][2] = sinPhi * cosPsi + cosPhi * sinThe * sinPsi;


205 


206  dcm[2][0] = sinThe;


207  dcm[2][1] = sinPhi * cosThe;


208  dcm[2][2] = cosPhi * cosThe;


209  }


210 


211  #endif

