Revision 2:771eed5f655a, committed 2021-05-31

Comitter:

cocorlow

Date:

Mon May 31 07:27:20 2021 +0000

Parent:

1:e8b22e2be051

Commit message:

rpy

Changed in this revision

diff -r e8b22e2be051 -r 771eed5f655a HAPS_EKF.cpp
--- a/HAPS_EKF.cpp	Mon May 31 07:15:13 2021 +0000
+++ b/HAPS_EKF.cpp	Mon May 31 07:27:20 2021 +0000
@@ -148,23 +148,23 @@
     Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
 }
 
-void HAPS_EKF::computeAngles(Vector3& rot, Vector3& rot_g, Vector3 rot_align)
+void HAPS_EKF::computeAngles(Vector3& rpy, Vector3& rpy_g, Vector3 rpy_align)
 {
     float q0 = qhat.getNumber( 1, 1 );
     float q1 = qhat.getNumber( 2, 1 ); 
     float q2 = qhat.getNumber( 3, 1 ); 
     float q3 = qhat.getNumber( 4, 1 ); 
-    rot.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rot_align.x;
-    rot.y = asinf(-2.0f * (q1*q3 - q0*q2))-rot_align.y;
-    rot.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+    rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
+    rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
+    rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
     
     q0 = qhat_gyro.getNumber( 1, 1 );
     q1 = qhat_gyro.getNumber( 2, 1 ); 
     q2 = qhat_gyro.getNumber( 3, 1 ); 
     q3 = qhat_gyro.getNumber( 4, 1 ); 
-    rot_g.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rot_align.x;
-    rot_g.y = asinf(-2.0f * (q1*q3 - q0*q2))-rot_align.y;
-    rot_g.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+    rpy_g.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
+    rpy_g.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
+    rpy_g.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
     
 }
 

diff -r e8b22e2be051 -r 771eed5f655a HAPS_EKF.hpp
--- a/HAPS_EKF.hpp	Mon May 31 07:15:13 2021 +0000
+++ b/HAPS_EKF.hpp	Mon May 31 07:27:20 2021 +0000
@@ -21,7 +21,7 @@
     HAPS_EKF();
     void updateBetweenMeasures(Vector3 gyro, float att_dt);
     void updateAcrossMeasures(Vector3 _v, Vector3 _u, Matrix& R);
-    void computeAngles(Vector3& rot, Vector3& rot_g, Vector3 rot_align);
+    void computeAngles(Vector3& rpy, Vector3& rpy_g, Vector3 rpy_align);
     void triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn);
     Vector3 calcMagRef(Vector3 m);
     Vector3 calcDynAcc(Vector3 LPacc, Vector3 accref);