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

update comm c-files

Browse Source
This commit is contained in:
Matt
2018-05-04 18:56:54 +08:00
parent 5b488ca7bd
commit 4708771c91
5 changed files with 14 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavlink_conversions.h
+10 -10
View File
@@ -38,14 +38,14 @@
*/
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;
float a = quaternion[0];
float b = quaternion[1];
float c = quaternion[2];
float d = quaternion[3];
float aSq = a * a;
float bSq = b * b;
float cSq = c * c;
float 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);
@@ -69,7 +69,7 @@ MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float d
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]);
theta = asinf(-dcm[2][0]);
if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
phi = 0.0f;
@@ -152,7 +152,7 @@ MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quate
int i;
int dcm_j;
int dcm_k;
tr = dcm[0][0] + dcm[1][1] + dcm[2][2];
if (tr > 0.0f) {
s = sqrtf(tr + 1.0f);