ping360: Only use heading when running in full scam mode

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
bluerobotics · Feb 7, 2020 · 04d7bd5 · 04d7bd5
1 parent ea03b8b
commit 04d7bd5
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/src/sensor/ping360.h b/src/sensor/ping360.h
@@ -150,7 +150,13 @@ class Ping360 : public PingSensor {
      *
      * @return uint16_t
      */
-    uint16_t angle() { return (_angle + angle_offset() + static_cast<int>(_heading)) % _angularResolutionGrad; }
+    uint16_t angle()
+    {
+        // Only use heading correction if running in full scam mode (sector size == resolution)
+        const int angle
+            = _angle + angle_offset() + (_sectorSize == _angularResolutionGrad ? static_cast<int>(_heading) : 0);
+        return angle % _angularResolutionGrad;
+    }
     Q_PROPERTY(int angle READ angle NOTIFY angleChanged)
 
     /**