From 8337cdc542b3ceddd78babddc438adcb398bd549 Mon Sep 17 00:00:00 2001
From: Yugesh kc <uges.kk@gmail.com>
Date: Tue, 5 Jan 2021 13:12:47 +0900
Subject: [PATCH 1/3] bno080x_heading_example

Using rotation vectors to find the heading value
---
 examples/bno08x_find_heading.py | 54 +++++++++++++++++++++++++++++++++
 1 file changed, 54 insertions(+)
 create mode 100644 examples/bno08x_find_heading.py

diff --git a/examples/bno08x_find_heading.py b/examples/bno08x_find_heading.py
new file mode 100644
index 0000000..bcbf829
--- /dev/null
+++ b/examples/bno08x_find_heading.py
@@ -0,0 +1,54 @@
+import time
+from math import atan2, sqrt, pi
+from board import SCL, SDA
+from busio import I2C
+from adafruit_bno08x import (
+    BNO_REPORT_STEP_COUNTER,
+    BNO_REPORT_ROTATION_VECTOR,
+    BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR
+)
+from adafruit_bno08x.i2c import BNO08X_I2C
+
+i2c = I2C(SCL, SDA, frequency=800000)
+bno = BNO08X_I2C(i2c)
+bno.enable_feature(BNO_REPORT_STEP_COUNTER)
+bno.enable_feature(BNO_REPORT_ROTATION_VECTOR)
+bno.enable_feature(BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR)
+
+# quat_real, quat_i, quat_j, quat_k
+
+
+def find_heading(dqw, dqx, dqy, dqz):
+    norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz)
+    dqw = dqw/norm
+    dqx = dqx/norm
+    dqy = dqy/norm
+    dqz = dqz/norm
+
+    ysqr = dqy * dqy
+
+    t3 = +2.0 * (dqw * dqz + dqx * dqy)
+    t4 = +1.0 - 2.0 * (ysqr + dqz * dqz)
+    yaw_raw = atan2(t3, t4)
+    yaw = yaw_raw * 180.0 / pi
+    if yaw > 0:
+        yaw = 360 - yaw
+    else:
+        yaw = abs(yaw)
+    return yaw  # heading in 360 clockwise
+
+
+while True:
+    quat_i, quat_j, quat_k, quat_real = bno.quaternion
+    heading = find_heading(quat_real, quat_i, quat_j, quat_k)
+    print("Heading using rotation vector:", heading)
+
+    # the geomagnetic sensor is unstable
+    # Heading is calculated using geomagnetic vector
+    geo_quat_i, geo_quat_j, geo_quat_k, geo_quat_real = bno.geomagnetic_quaternion
+    heading_geo = find_heading(
+        geo_quat_real, geo_quat_i, geo_quat_j, geo_quat_k)
+    print("Heading using geomagnetic rotation vector:", heading_geo)
+    print("")
+    time.sleep(0.1)
+

From 67f9f50d7361a318c21f3e04d420135f2dde7bd8 Mon Sep 17 00:00:00 2001
From: yugyesh <uges.kk@gmail.com>
Date: Wed, 17 Mar 2021 11:48:40 +0900
Subject: [PATCH 2/3] Add licence

---
 examples/bno08x_find_heading.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/examples/bno08x_find_heading.py b/examples/bno08x_find_heading.py
index bcbf829..5cd2df1 100644
--- a/examples/bno08x_find_heading.py
+++ b/examples/bno08x_find_heading.py
@@ -1,3 +1,6 @@
+# SPDX-FileCopyrightText: 2021 KC YUGESH
+# SPDX-License-Identifier: Unlicense
+
 import time
 from math import atan2, sqrt, pi
 from board import SCL, SDA
@@ -51,4 +54,3 @@ def find_heading(dqw, dqx, dqy, dqz):
     print("Heading using geomagnetic rotation vector:", heading_geo)
     print("")
     time.sleep(0.1)
-

From cb460d5993dca53107924e9b72350628a57a5bb2 Mon Sep 17 00:00:00 2001
From: yugyesh <uges.kk@gmail.com>
Date: Wed, 17 Mar 2021 11:49:33 +0900
Subject: [PATCH 3/3] refactor: Format code using black

---
 examples/bno08x_find_heading.py | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/examples/bno08x_find_heading.py b/examples/bno08x_find_heading.py
index 5cd2df1..b156ace 100644
--- a/examples/bno08x_find_heading.py
+++ b/examples/bno08x_find_heading.py
@@ -8,7 +8,7 @@
 from adafruit_bno08x import (
     BNO_REPORT_STEP_COUNTER,
     BNO_REPORT_ROTATION_VECTOR,
-    BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR
+    BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR,
 )
 from adafruit_bno08x.i2c import BNO08X_I2C
 
@@ -22,11 +22,11 @@
 
 
 def find_heading(dqw, dqx, dqy, dqz):
-    norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz)
-    dqw = dqw/norm
-    dqx = dqx/norm
-    dqy = dqy/norm
-    dqz = dqz/norm
+    norm = sqrt(dqw * dqw + dqx * dqx + dqy * dqy + dqz * dqz)
+    dqw = dqw / norm
+    dqx = dqx / norm
+    dqy = dqy / norm
+    dqz = dqz / norm
 
     ysqr = dqy * dqy
 
@@ -49,8 +49,7 @@ def find_heading(dqw, dqx, dqy, dqz):
     # the geomagnetic sensor is unstable
     # Heading is calculated using geomagnetic vector
     geo_quat_i, geo_quat_j, geo_quat_k, geo_quat_real = bno.geomagnetic_quaternion
-    heading_geo = find_heading(
-        geo_quat_real, geo_quat_i, geo_quat_j, geo_quat_k)
+    heading_geo = find_heading(geo_quat_real, geo_quat_i, geo_quat_j, geo_quat_k)
     print("Heading using geomagnetic rotation vector:", heading_geo)
     print("")
     time.sleep(0.1)