Anpassen der EulerAngles.cpp an den Softwarestand SOAPP 2.0

libraries/EulerAngles/EulerAngles.cpp

@@ -7,6 +7,16 @@
 // Lizenz:  CC-BY-SA  (siehe Wikipedia: Creative Commons)
 //
 
+//###########################################################################
+//                        Anpassungschronik
+//                        Stand: 12.02.2024
+
+  //Nachrüstung vom 04.10.2023 -> Einfügen der "angepassten" Berechnungnen der Eulerwinkel
+
+//###########################################################################
+ 
+
+
 #include "EulerAngles.h"
 
 
@@ -23,8 +33,10 @@ EulerAngles::EulerAngles()
 // Anwenderfunktionen
 // ---------------------------------------------------------------------------
 //
+
 float EulerAngles::getRollFromGravity(float accY, float accZ)
 {
+  
   if(accZ < 0)
   {
     if(accZ > - minTanDenom)
@@ -35,6 +47,7 @@ float EulerAngles::getRollFromGravity(float accY, float accZ)
     if(accZ < minTanDenom)
       accZ = minTanDenom;
   }
+  
   return(atan2(accY,accZ));
 }
 
@@ -50,4 +63,157 @@ float EulerAngles::getPitchFromGravity(float accX, float accY, float accZ)
     denom = - denom;
 
   return(atan2(- accX, denom));
+
+}
+
+//#########################################################################
+//     Nachrüstung/ Erweiterung der Eulerfunktionen Stand: 04.10.2023     #
+  float EulerAngles::getNewRollFromGravity(float accX, float accY, float accZ)  {
+
+    float Phi;
+
+    //"Ausfiltern" zu großer Beschleunigungswerte
+    if(accX < 0)  {
+      if(accX > - minTanDenom)  {
+        accX = - minTanDenom;
+      }
+    }
+    else  {
+      if(accX < minTanDenom)  {
+        accX = minTanDenom;
+      }
+    }
+
+    if(accY < 0)  {
+      if(accY > - minTanDenom)  {
+        accY = - minTanDenom;
+      }
+    }
+    else  {
+      if(accY < minTanDenom)  {
+        accY = minTanDenom;
+      }
+    }
+
+    if(accZ < 0)  {
+      if(accZ > - minTanDenom)  {
+        accZ = - minTanDenom;
+      }
+    }
+    else  {
+      if(accZ < minTanDenom)  {
+        accZ = minTanDenom;
+      }
+    }
+
+    Phi=atan2(accY,(accZ));
+
+    return(Phi);
+  }
+
+  float EulerAngles::getNewPitchFromGravity(float accX, float accY, float accZ) {
+
+    float Phi;
+    float Theta;
+
+    float accZ2;
+
+    //"Ausfiltern" zu großer Beschleunigungswerte
+    if(accX < 0)  {
+      if(accX > - minTanDenom)  {
+        accX = - minTanDenom;
+      }
+    }
+    else  {
+      if(accX < minTanDenom)  {
+        accX = minTanDenom;
+      }
+    }
+
+    if(accY < 0)  {
+      if(accY > - minTanDenom)  {
+        accY = - minTanDenom;
       }
+    }
+    else  {
+      if(accY < minTanDenom)  {
+        accY = minTanDenom;
+      }
+    }
+
+    if(accZ < 0)  {
+      if(accZ > - minTanDenom)  {
+        accZ = - minTanDenom;
+      }
+    }
+    else  {
+      if(accZ < minTanDenom)  {
+        accZ = minTanDenom;
+      }
+    }
+
+    Phi=getNewRollFromGravity(accX,accY,accZ);
+
+    accZ2=accY*sin(Phi)+accZ*cos(Phi);
+
+    Theta=atan(-accX/accZ2);
+
+    return(Theta);
+  }
+
+  float EulerAngles::getNewYawFromMagnetAndGravity(float magX, float magY, float magZ, float accX, float accY, float accZ)  {
+
+        //"Ausfiltern" zu großer Beschleunigungswerte
+    if(accX < 0)  {
+      if(accX > - minTanDenom)  {
+        accX = - minTanDenom;
+      }
+    }
+    else  {
+      if(accX < minTanDenom)  {
+        accX = minTanDenom;
+      }
+    }
+
+    if(accY < 0)  {
+      if(accY > - minTanDenom)  {
+        accY = - minTanDenom;
+      }
+    }
+    else  {
+      if(accY < minTanDenom)  {
+        accY = minTanDenom;
+      }
+    }
+
+    if(accZ < 0)  {
+      if(accZ > - minTanDenom)  {
+        accZ = - minTanDenom;
+      }
+    }
+    else  {
+      if(accZ < minTanDenom)  {
+        accZ = minTanDenom;
+      }
+    }
+
+    float Phi;
+    float Theta;
+    float Psi;
+
+    float magX2, magY2, magZ2;
+
+    Phi=getNewRollFromGravity(accX, accY, accZ);
+    Theta=getNewPitchFromGravity(accX, accY, accZ);
+
+    magY2=magZ*sin(Phi)-magY*cos(Phi);
+    magZ2=magY*sin(Phi)+magZ*cos(Phi);
+    magX2=magX*cos(Theta)+magZ2*sin(Theta);
+
+    Psi=atan2(magY2,magX2);
+  
+    return(Psi);
+  }
+//           Ende Nachrüstung/ Erweiterung der Eulerfunktionen            #
+//#########################################################################
+     
\ No newline at end of file