hm/c_library_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

autogenerated headers for rev https://github.com/mavlink/mavlink/tree/83b3c23332bab1b320b9674e604d90b3fc482f75

Browse Source
This commit is contained in:
PX4BuildBot
2014-12-29 11:10:46 +00:00
parent 7d0aa0205e
commit 42f1a37397
34 changed files with 1326 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavlink_conversions.h
+22 -26
View File
@@ -45,13 +45,13 @@ MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float d
double cSq = c * c;
double dSq = d * d;
dcm[0][0] = aSq + bSq - cSq - dSq;
dcm[0][1] = 2.0 * (b * c - a * d);
dcm[0][2] = 2.0 * (a * c + b * d);
dcm[1][0] = 2.0 * (b * c + a * d);
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.0 * (c * d - a * b);
dcm[2][0] = 2.0 * (b * d - a * c);
dcm[2][1] = 2.0 * (a * b + c * d);
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;
}
@@ -116,12 +116,12 @@ MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float
*/
MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
{
double cosPhi_2 = cos((double)roll / 2.0);
double sinPhi_2 = sin((double)roll / 2.0);
double cosTheta_2 = cos((double)pitch / 2.0);
double sinTheta_2 = sin((double)pitch / 2.0);
double cosPsi_2 = cos((double)yaw / 2.0);
double sinPsi_2 = sin((double)yaw / 2.0);
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 -
@@ -141,14 +141,10 @@ MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float y
*/
MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
{
quaternion[0] = (0.5 * sqrt(1.0 +
(double)(dcm[0][0] + dcm[1][1] + dcm[2][2])));
quaternion[1] = (0.5 * sqrt(1.0 +
(double)(dcm[0][0] - dcm[1][1] - dcm[2][2])));
quaternion[2] = (0.5 * sqrt(1.0 +
(double)(-dcm[0][0] + dcm[1][1] - dcm[2][2])));
quaternion[3] = (0.5 * sqrt(1.0 +
(double)(-dcm[0][0] - dcm[1][1] + dcm[2][2])));
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]);
}
@@ -162,12 +158,12 @@ MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quate
*/
MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
{
double cosPhi = cos(roll);
double sinPhi = sin(roll);
double cosThe = cos(pitch);
double sinThe = sin(pitch);
double cosPsi = cos(yaw);
double sinPsi = sin(yaw);
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;