gyro: add gyroscope test/display application (#76)

* iio: switch to maximum sampling frequency, apply mount matrix

* mag: Apply mount matrix for magnetometer, too

* iio: allow separate path for gyro sensor

* gyro: fork from compass, get drawing back to work

* iio: add todo.

* gyro: gyro seems to work way better on pinephone

* iio: move scale conversion where it belongs

* gyro: rely of iio driver providing right scale of values

* iio: Turn down debugging, but I still get framerate drops

* gyro: add reset and calibration support, introduce vectors, display help image

internal_filesystem/apps/cz.ucw.pavel.gyro/META-INF/MANIFEST.JSON

@@ -0,0 +1,24 @@
+{
+"name": "Gyro",
+"publisher": "Pavel Machek",
+"short_description": "Gyro",
+"long_description": "Simple gyro app.",
+"icon_url": "https://apps.micropythonos.com/apps/cz.ucw.pavel.gyro/icons/cz.ucw.pavel.gyro_0.0.1_64x64.png",
+"download_url": "https://apps.micropythonos.com/apps/cz.ucw.pavel.gyro/mpks/cz.ucw.pavel.gyro_0.0.1.mpk",
+"fullname": "cz.ucw.pavel.gyro",
+"version": "0.0.1",
+"category": "utilities",
+"activities": [
+    {
+      "entrypoint": "assets/main.py",
+      "classname": "Main",
+      "intent_filters": [
+        {
+          "action": "main",
+          "category": "launcher"
+        }
+      ]
+    }
+  ]
+}
+

internal_filesystem/lib/mpos/imu/drivers/iio.py

@@ -13,12 +13,17 @@ class IIODriver(IMUDriverBase):
 
     accel_path: str
     mag_path: str
+    gyro_path: str
 
     def __init__(self):
         super().__init__()
         self.accel_path = self.find_iio_device_with_file("in_accel_x_raw")
+        self.ensure_sampling_frequency_max(self.accel_path)
         self.mag_path = self.find_iio_device_with_file("in_magn_x_raw")
-
+        self.ensure_sampling_frequency_max(self.mag_path)
+        self.gyro_path = self.find_iio_device_with_file("in_anglvel_x_raw")
+        self.ensure_sampling_frequency_max(self.gyro_path)
+        
     def _p(self, name: str):
         return self.accel_path + "/" + name
 
@@ -69,13 +74,100 @@ class IIODriver(IMUDriverBase):
         return None
 
     def _read_text(self, name: str) -> str:
-        print("Read: ", name)
+        if False:
+            print("Read: ", name)
         f = open(name, "r")
         try:
             return f.readline().strip()
         finally:
             f.close()
 
+    def _parse_available_freqs(self, text):
+        """
+        IIO typically uses either:
+          "12.5 25 50 100"
+        or
+          "0.5 1 2 4 8 16"
+
+        Returns list of floats.
+        """
+        out = []
+        for tok in text.replace(",", " ").split():
+            out.append(float(tok))
+        return out
+
+    def _format_freq_for_sysfs(self, f):
+        """
+        Kernel sysfs usually accepts either integer or decimal.
+        We'll keep it minimal:
+          - if f is whole number -> "100"
+          - else -> "12.5"
+        """
+        if int(f) == f:
+            return str(int(f))
+        # avoid scientific notation
+        s = ("%.6f" % f).rstrip("0").rstrip(".")
+        return s
+
+    def _try_set_via_sudo_tee(self, path, value_str):
+        """
+        Executes:
+          sh -c 'echo VALUE | sudo tee PATH'
+        Returns True if command returns 0.
+        """
+        cmd = "sh -c 'echo %s | sudo tee %s >/dev/null'" % (value_str, path)
+        rc = os.system(cmd)
+        return rc == 0
+
+    def ensure_sampling_frequency_max(self, dev_path):
+        """
+        dev_path: "/sys/bus/iio/devices/iio:deviceX"
+
+        Returns:
+          (changed: bool, max_freq: float or None, current: float or None)
+        """
+        sf = dev_path + "/sampling_frequency"
+        sfa = dev_path + "/sampling_frequency_available"
+
+        # read current
+        cur_s = self._read_text(sf)
+        cur = float(cur_s)
+
+        avail_s = self._read_text(sfa)
+        avail = self._parse_available_freqs(avail_s)
+
+        maxf = max(avail)
+
+        # already max (tolerate float fuzz)
+        if abs(cur - maxf) < 1e-6:
+            print("Already at max frequency")
+            return (False, maxf, cur)
+
+        max_str = self._format_freq_for_sysfs(maxf)
+
+        # Fallback: sudo tee
+        ok = self._try_set_via_sudo_tee(sf, max_str)
+        if not ok:
+            print("Can't switch to max frequency")
+            return (False, maxf, cur)
+
+        new_cur = float(self._read_text(sf))
+
+        return (True, maxf, new_cur)
+
+    def ensure_sampling_frequency_max_for_device_with_file(self, filename):
+        """
+        Convenience wrapper:
+          - finds iio device containing filename
+          - sets sampling_frequency to maximum
+        """
+        dev = self.find_iio_device_with_file(filename)
+        if dev is None:
+            return (None, False, None, None)
+
+        changed, maxf, cur = self.ensure_sampling_frequency_max(dev)
+        return (dev, changed, maxf, cur)
+
     def _read_float(self, name: str) -> float:
         return float(self._read_text(name))
 
@@ -93,6 +185,7 @@ class IIODriver(IMUDriverBase):
           - in_temp_input (already scaled, usually millidegree C)
           - in_temp_raw + in_temp_scale
         """
+        return 12.34
         if not self.accel_path:
             return None
 
@@ -102,6 +195,51 @@ class IIODriver(IMUDriverBase):
             return None
         return self._read_raw_scaled(raw_path, scale_path)
 
+    def _read_mount_matrix(self, p):
+        """
+        Reads IIO mount matrix from *mount_matrix
+
+        Format example:
+            "0, 1, 0; -1, 0, 0; 0, 0, 1"
+
+        Returns:
+            3x3 matrix as tuple of tuples (float)
+        """
+        path = p + "/" + "in_accel_mount_matrix"
+        if not self._exists(path):
+            # Strange, librem 5 has different filename
+            path = self.accel_path + "/" + "mount_matrix"
+            if not self._exists(path):
+                return None
+
+        text = self._read_text(path).strip()
+
+        rows = []
+        for row in text.split(";"):
+            rows.append(tuple(float(x.strip()) for x in row.split(",")))
+
+        if len(rows) != 3 or any(len(r) != 3 for r in rows):
+            raise ValueError("Invalid mount matrix format")
+
+        return tuple(rows)
+
+
+    def _apply_mount_matrix(self, ax, ay, az, p):
+        """
+        Applies IIO mount matrix to acceleration vector.
+
+        Returns rotated (ax, ay, az).
+        """
+        M = self._read_mount_matrix(p)
+        if M is None:
+            return (ax, ay, az)
+
+        x = M[0][0]*ax + M[0][1]*ay + M[0][2]*az
+        y = M[1][0]*ax + M[1][1]*ay + M[1][2]*az
+        z = M[2][0]*ax + M[2][1]*ay + M[2][2]*az
+
+        return (x, y, z)
+
     def _raw_acceleration_mps2(self):
         if not self.accel_path:
             return (0.0, 0.0, 0.0)
@@ -111,18 +249,19 @@ class IIODriver(IMUDriverBase):
         ay = self._read_raw_scaled(self.accel_path + "/" + "in_accel_y_raw", scale_name)
         az = self._read_raw_scaled(self.accel_path + "/" + "in_accel_z_raw", scale_name)
 
-        return (ax, ay, az)
+        return self._apply_mount_matrix(ax, ay, az, self.accel_path)
 
     def _raw_gyroscope_dps(self):
-        if not self.accel_path:
+        if not self.gyro_path:
             return (0.0, 0.0, 0.0)
-        scale_name = self.accel_path + "/" + "in_anglvel_scale"
+        scale_name = self.gyro_path + "/" + "in_anglvel_scale"
+        mul = 57.2957795
 
-        gx = self._read_raw_scaled(self.accel_path + "/" + "in_anglvel_x_raw", scale_name)
-        gy = self._read_raw_scaled(self.accel_path + "/" + "in_anglvel_y_raw", scale_name)
-        gz = self._read_raw_scaled(self.accel_path + "/" + "in_anglvel_z_raw", scale_name)
+        gx = mul * self._read_raw_scaled(self.gyro_path + "/" + "in_anglvel_x_raw", scale_name)
+        gy = mul * self._read_raw_scaled(self.gyro_path + "/" + "in_anglvel_y_raw", scale_name)
+        gz = mul * self._read_raw_scaled(self.gyro_path + "/" + "in_anglvel_z_raw", scale_name)
 
-        return (gx, gy, gz)
+        return self._apply_mount_matrix(gx, gy, gz, self.gyro_path)
 
     def read_acceleration(self):
         ax, ay, az = self._raw_acceleration_mps2()
@@ -145,4 +284,4 @@ class IIODriver(IMUDriverBase):
         gy = self._read_raw_scaled(self.mag_path + "/" + "in_magn_y_raw", self.mag_path + "/" + "in_magn_y_scale")
         gz = self._read_raw_scaled(self.mag_path + "/" + "in_magn_z_raw", self.mag_path + "/" + "in_magn_z_scale")        
 
-        return (gx, gy, gz)
+        return self._apply_mount_matrix(gx, gy, gz, self.mag_path)