[sensorfw] Properly handle angle rollover in built-in compass support…

…. JB#46853 Fix rotation direction. Initialize all variables in constructor. Remove unused variables.
sailfishos · Dec 2, 2020 · 4de48d2 · 4de48d2
1 parent 23abb4a
commit 4de48d2
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 9 deletions.
diff --git a/chains/compasschain/compassfilter.cpp b/chains/compasschain/compassfilter.cpp
@@ -26,11 +26,12 @@
 #define DEGREES_TO_RADIANS 0.017453292
 #define GRAVITY_EARTH 9.80665f
 #define FILTER_FACTOR 0.24f
-#define LIST_COUNT 10
 
 CompassFilter::CompassFilter() :
         magDataSink(this, &CompassFilter::magDataAvailable),
         accelSink(this, &CompassFilter::accelDataAvailable),
+        magX(0), magY(0), magZ(0),
+        oldMagX(0), oldMagY(0), oldMagZ(0),
         level(0),
         oldHeading(0)
 {
@@ -60,7 +61,7 @@ void CompassFilter::accelDataAvailable(unsigned, const AccelerationData *data)
     // the x/y are switched as compass expects it in aero coordinates
     qreal Gx = data->y_ * .001f; //convert to g
     qreal Gy = data->x_ * .001f;
-    qreal Gz = -data->z_ * .001f;
+    qreal Gz = data->z_ * .001f;
 
     qreal divisor = qSqrt(Gx * Gx + Gy * Gy + Gz * Gz);
     qreal normalizedGx = Gx / divisor;
@@ -104,11 +105,19 @@ void CompassFilter::accelDataAvailable(unsigned, const AccelerationData *data)
     /* calculate yaw = ecompass angle psi (-180deg, 180deg) */
     Psi = (qAtan2(-fBfy, fBfx) * RADIANS_TO_DEGREES); /* Equation 7 */
 
-    qreal heading = Psi * FILTER_FACTOR + oldHeading * (1.0 - FILTER_FACTOR);
+    qreal heading;
+    if (Psi < -90.0f && oldHeading > 90.0f) {
+        heading = (Psi + 360.0f) * FILTER_FACTOR + oldHeading * (1.0 - FILTER_FACTOR);
+    } else if (Psi > 90.0f && oldHeading < -90.0f) {
+        heading = Psi * FILTER_FACTOR + (oldHeading + 360.0f) * (1.0 - FILTER_FACTOR);
+    } else {
+        heading = Psi * FILTER_FACTOR + oldHeading * (1.0 - FILTER_FACTOR);
+    }
 
     CompassData compassData; //north angle
     compassData.timestamp_ = data->timestamp_;
     compassData.degrees_ = (int)(heading + 360) % 360;
+    compassData.rawDegrees_ = compassData.degrees_;
     compassData.level_ = level;
     magSource.propagate(1, &compassData);
     oldHeading = heading;

diff --git a/chains/compasschain/compassfilter.h b/chains/compasschain/compassfilter.h
@@ -48,8 +48,6 @@ class CompassFilter : public QObject, public FilterBase
     void magDataAvailable(unsigned, const CalibratedMagneticFieldData*);
     void accelDataAvailable(unsigned, const AccelerationData*);
 
-    CalibratedMagneticFieldData magData;
-
     qreal magX;
     qreal magY;
     qreal magZ;
@@ -59,10 +57,6 @@ class CompassFilter : public QObject, public FilterBase
 
     int level;
     qreal oldHeading;
-    QList <int> averagingBuffer;
-    QList <const CalibratedMagneticFieldData *> magAvgBuffer;
-    QList <const AccelerationData *> accelAvgBuffer;
-//    MagAvgBuffer magAvgBuffer;
 };
 
 #endif