Normalized values in Q12

main/bmi160.c

@@ -9,6 +9,8 @@
 #include "freertos/queue.h"
 #include "freertos/task.h"
 
+#include "fixedpoint.h"
+
 #define BMI160_MAX_WRITE_LEN 32
 #define BMI160_SAMPLE_QUEUE_LEN 16
 #define BMI160_QUEUE_MIN_FREE_SLOTS 1
@@ -222,12 +224,21 @@ esp_err_t imu_read(const bmi160_t *dev, bmi160_value_t *value)
 
     uint8_t data[20];
     ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));
-    value->acc.x = data[14] | (data[15] << 8);
+
-    value->acc.y = data[16] | (data[17] << 8);
+    int16_t acc_x = data[14] | (data[15] << 8);     // unbiased raw
-    value->acc.z = data[18] | (data[19] << 8);
+    int16_t acc_y = data[16] | (data[17] << 8);     // unbiased raw
-    value->gyr.x = data[8] | (data[9] << 8);
+    int16_t acc_z = data[18] | (data[19] << 8);     // unbiased raw
-    value->gyr.y = data[10] | (data[11] << 8);
+    int16_t gyr_x = data[8] | (data[9] << 8);       // unbiased raw
-    value->gyr.z = data[12] | (data[13] << 8);
+    int16_t gyr_y = data[10] | (data[11] << 8);     // unbiased raw
+    int16_t gyr_z = data[12] | (data[13] << 8);     // unbiased raw
+
+    value->acc.x = FIXED_FROM_RATIO(acc_x, 2048, 12);
+    value->acc.y = FIXED_FROM_RATIO(acc_y, 2048, 12);
+    value->acc.z = FIXED_FROM_RATIO(acc_z, 2048, 12);
+
+    value->gyr.x = FIXED_FROM_RATIO((int32_t)gyr_x * 5, 82, 12);
+    value->gyr.y = FIXED_FROM_RATIO((int32_t)gyr_y * 5, 82, 12);
+    value->gyr.z = FIXED_FROM_RATIO((int32_t)gyr_z * 5, 82, 12);
 
     uint8_t time[3];
     ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_SENSORTIME, time, BMI160_SIZE_SENSORTIME));

main/bmi160.h

@@ -52,9 +52,9 @@ typedef struct
 {
     struct
     {
-        int16_t x;
+        int32_t x; // Fixed point Q.12
-        int16_t y;
+        int32_t y; // Fixed point Q.12
-        int16_t z;
+        int32_t z; // Fixed point Q.12
     } acc, gyr;
     uint32_t time;
 } bmi160_value_t;

main/fixedpoint.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include <stdint.h>
+
+/*
+ * Generic fixed-point helpers.
+ *
+ * Example for Q4.12 using int16_t:
+ *   typedef int16_t q4_12_t;
+ *   #define Q4_12_FRAC_BITS 12
+ *   q4_12_t a = FIXED_FROM_INT(3, Q4_12_FRAC_BITS);
+ *   q4_12_t b = FIXED_FROM_RATIO_T(int16_t, 1, 2, Q4_12_FRAC_BITS);
+ *   q4_12_t c = FIXED_MUL_T(int16_t, a, b, Q4_12_FRAC_BITS);
+ *
+ * Example for Q16.16 using int32_t:
+ *   typedef int32_t q16_16_t;
+ *   #define Q16_16_FRAC_BITS 16
+ */
+
+#define FIXED_ONE(frac_bits) ((int32_t)1 << (frac_bits))
+#define FIXED_HALF(frac_bits) (FIXED_ONE(frac_bits) >> 1)
+#define FIXED_FRACTION_MASK(frac_bits) (FIXED_ONE(frac_bits) - 1)
+
+#define FIXED_FROM_INT(value, frac_bits) ((value) << (frac_bits))
+#define FIXED_TO_INT(value, frac_bits) ((value) >> (frac_bits))
+#define FIXED_FRACTION(value, frac_bits) ((value) & FIXED_FRACTION_MASK(frac_bits))
+
+#define FIXED_ABS(value) ((value) < 0 ? -(value) : (value))
+#define FIXED_MIN(a, b) ((a) < (b) ? (a) : (b))
+#define FIXED_MAX(a, b) ((a) > (b) ? (a) : (b))
+#define FIXED_CLAMP(value, min_value, max_value) \
+    ((value) < (min_value) ? (min_value) : ((value) > (max_value) ? (max_value) : (value)))
+
+#define FIXED_MUL(a, b, frac_bits) \
+    ((int32_t)(((int64_t)(a) * (int64_t)(b)) >> (frac_bits)))
+
+#define FIXED_DIV(numerator, denominator, frac_bits) \
+    ((denominator) == 0 ? 0 : (int32_t)(((int64_t)(numerator) << (frac_bits)) / (denominator)))
+
+#define FIXED_FROM_RATIO(numerator, denominator, frac_bits) \
+    ((denominator) == 0 ? 0 : (int32_t)(((int64_t)(numerator) << (frac_bits)) / (denominator)))
+
+#define FIXED_LERP(a, b, t, frac_bits) \
+    ((a) + FIXED_MUL((b) - (a), (t), (frac_bits)))
+
+#define FIXED_MUL_T(type, a, b, frac_bits) \
+    ((type)(((int64_t)(a) * (int64_t)(b)) >> (frac_bits)))
+
+#define FIXED_DIV_T(type, numerator, denominator, frac_bits) \
+    ((denominator) == 0 ? (type)0 : (type)(((int64_t)(numerator) << (frac_bits)) / (denominator)))
+
+#define FIXED_FROM_RATIO_T(type, numerator, denominator, frac_bits) \
+    ((denominator) == 0 ? (type)0 : (type)(((int64_t)(numerator) << (frac_bits)) / (denominator)))

plot.py

@@ -114,11 +114,31 @@ def validate_config(config: dict) -> tuple[dict[str, list[str]], list[dict], lis
             key = stream.get("key")
             if not isinstance(key, str) or not key:
                 raise SystemExit("Each stream must have a non-empty string `key`.")
+            interpreter = stream.get("interpreter", "int")
+            if interpreter not in ("int", "fixed"):
+                raise SystemExit(f"Unsupported interpreter for stream `{key}`: {interpreter}")
+            if interpreter == "fixed":
+                frac_bits = stream.get("frac_bits")
+                if not isinstance(frac_bits, int) or frac_bits < 0:
+                    raise SystemExit(f"Fixed-point stream `{key}` requires integer `frac_bits`.")
             stream_keys.append(key)
 
     return tag_to_fields, graphs, stream_keys
 
 
+def interpret_value(raw_value: int, stream: dict) -> float:
+    interpreter = stream.get("interpreter", "int")
+    if interpreter == "int":
+        return float(raw_value)
+    if interpreter == "fixed":
+        frac_bits = stream.get("frac_bits")
+        if not isinstance(frac_bits, int) or frac_bits < 0:
+            raise SystemExit("Fixed-point streams require a non-negative integer `frac_bits`.")
+        return raw_value / float(1 << frac_bits)
+
+    raise SystemExit(f"Unsupported interpreter: {interpreter}")
+
+
 def serial_reader(port: str, baudrate: int, tag_to_fields: dict[str, list[str]], output: Queue) -> None:
     try:
         with serial.Serial(port, baudrate=baudrate, timeout=1) as ser:
@@ -162,7 +182,8 @@ def serial_reader(port: str, baudrate: int, tag_to_fields: dict[str, list[str]],
 
 def drain_queue(
     queue: Queue,
-    sample_sets: dict[str, deque[tuple[float, int]]],
+    sample_sets: dict[str, deque[tuple[float, float]]],
+    stream_defs: dict[str, dict],
 ) -> str | None:
     error_message = None
     while True:
@@ -177,7 +198,9 @@ def drain_queue(
 
         stream_name, timestamp, value = item
         if stream_name in sample_sets:
-            sample_sets[stream_name].append((timestamp, value))
+            sample_sets[stream_name].append(
+                (timestamp, interpret_value(value, stream_defs[stream_name]))
+            )
 
 
 def draw_grid(
@@ -195,6 +218,11 @@ def draw_grid(
         y = rect.top + round(fraction * rect.height)
         pygame.draw.line(surface, GRID, (rect.left, y), (rect.right, y), 1)
         value = y_max - fraction * (y_max - y_min)
+        if abs(y_max - y_min) <= 4.0:
+            label = f"{value:.2f}".rstrip("0").rstrip(".")
+        elif abs(y_max - y_min) <= 40.0:
+            label = f"{value:.1f}".rstrip("0").rstrip(".")
+        else:
             label = f"{value:.0f}"
         text = font.render(label, True, AXIS)
         surface.blit(text, (10, y - text.get_height() // 2))
@@ -213,13 +241,13 @@ def draw_grid(
 def draw_trace(
     surface: pygame.Surface,
     rect: pygame.Rect,
-    samples: deque[tuple[float, int]],
+    samples: deque[tuple[float, float]],
     view_span: float,
     now: float,
     y_min: float,
     y_max: float,
     color: tuple[int, int, int],
-) -> int | None:
+) -> float | None:
     visible_points = []
     latest_value = None
     y_span = max(y_max - y_min, 1e-6)
@@ -242,7 +270,7 @@ def draw_trace(
 
 
 def visible_range(
-    sample_sets: dict[str, deque[tuple[float, int]]],
+    sample_sets: dict[str, deque[tuple[float, float]]],
     stream_names: list[str],
     now: float,
     view_span: float,
@@ -274,6 +302,11 @@ def main() -> int:
     args = parse_args()
     config = load_config(args.config)
     tag_to_fields, graphs, stream_keys = validate_config(config)
+    stream_map = {
+        stream["key"]: stream
+        for graph in graphs
+        for stream in graph["streams"]
+    }
     sample_sets = {key: deque(maxlen=args.max_samples) for key in stream_keys}
     queue: Queue = Queue()
 
@@ -308,7 +341,7 @@ def main() -> int:
                 elif event.button == 5:
                     view_span = zoom(view_span, -1, initial_span)
 
-        queued_error = drain_queue(queue, sample_sets)
+        queued_error = drain_queue(queue, sample_sets, stream_map)
         if queued_error is not None:
             error_message = queued_error
 
@@ -325,8 +358,8 @@ def main() -> int:
         top = 40
         for graph in graphs:
             rect = pygame.Rect(70, top, plot_width, panel_height)
-            stream_defs = graph["streams"]
+            graph_streams = graph["streams"]
-            stream_names = [stream["key"] for stream in stream_defs]
+            stream_names = [stream["key"] for stream in graph_streams]
             y_min, y_max = visible_range(sample_sets, stream_names, now, view_span)
             draw_grid(
                 screen,
@@ -337,7 +370,7 @@ def main() -> int:
                 y_max,
                 graph.get("title", "Graph"),
             )
-            for stream in stream_defs:
+            for stream in graph_streams:
                 latest_values[stream["key"]] = draw_trace(
                     screen,
                     rect,
@@ -356,7 +389,7 @@ def main() -> int:
             for stream in graph["streams"]:
                 latest_value = latest_values.get(stream["key"])
                 if latest_value is not None:
-                    graph_parts.append(f"{stream.get('label', stream['key'])}={latest_value}")
+                    graph_parts.append(f"{stream.get('label', stream['key'])}={latest_value:.3f}")
             if graph_parts:
                 header += " | " + " ".join(graph_parts)
         screen.blit(font.render(header, True, TEXT), (10, 8))

plot_config.json

@@ -11,17 +11,17 @@
     {
       "title": "Accelerometer",
       "streams": [
-        { "key": "acc_x", "label": "ACC X", "color": [90, 170, 255] },
+        { "key": "acc_x", "label": "ACC X", "color": [90, 170, 255], "interpreter": "fixed", "frac_bits": 12 },
-        { "key": "acc_y", "label": "ACC Y", "color": [80, 220, 140] },
+        { "key": "acc_y", "label": "ACC Y", "color": [80, 220, 140], "interpreter": "fixed", "frac_bits": 12 },
-        { "key": "acc_z", "label": "ACC Z", "color": [255, 200, 60] }
+        { "key": "acc_z", "label": "ACC Z", "color": [255, 200, 60], "interpreter": "fixed", "frac_bits": 12 }
       ]
     },
     {
       "title": "Gyroscope",
       "streams": [
-        { "key": "gyr_x", "label": "GYR X", "color": [90, 170, 255] },
+        { "key": "gyr_x", "label": "GYR X", "color": [90, 170, 255], "interpreter": "fixed", "frac_bits": 12 },
-        { "key": "gyr_y", "label": "GYR Y", "color": [80, 220, 140] },
+        { "key": "gyr_y", "label": "GYR Y", "color": [80, 220, 140], "interpreter": "fixed", "frac_bits": 12 },
-        { "key": "gyr_z", "label": "GYR Z", "color": [225, 200, 60] }
+        { "key": "gyr_z", "label": "GYR Z", "color": [225, 200, 60], "interpreter": "fixed", "frac_bits": 12 }
       ]
     }
   ]