From 1b6bd537ad04abdcd76815ce745487095d8a07cd Mon Sep 17 00:00:00 2001 From: Joppe Blondel Date: Fri, 20 Mar 2026 15:43:14 +0100 Subject: [PATCH] Normalized values in Q12 --- main/bmi160.c | 23 ++++++++++++++------ main/bmi160.h | 6 +++--- main/fixedpoint.h | 53 +++++++++++++++++++++++++++++++++++++++++++++ plot.py | 55 +++++++++++++++++++++++++++++++++++++---------- plot_config.json | 12 +++++------ 5 files changed, 123 insertions(+), 26 deletions(-) create mode 100644 main/fixedpoint.h diff --git a/main/bmi160.c b/main/bmi160.c index 03d7578..61489e6 100644 --- a/main/bmi160.c +++ b/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); - value->acc.z = data[18] | (data[19] << 8); - value->gyr.x = data[8] | (data[9] << 8); - value->gyr.y = data[10] | (data[11] << 8); - value->gyr.z = data[12] | (data[13] << 8); + + int16_t acc_x = data[14] | (data[15] << 8); // unbiased raw + int16_t acc_y = data[16] | (data[17] << 8); // unbiased raw + int16_t acc_z = data[18] | (data[19] << 8); // unbiased raw + int16_t gyr_x = data[8] | (data[9] << 8); // unbiased raw + 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)); diff --git a/main/bmi160.h b/main/bmi160.h index 1618551..c508866 100644 --- a/main/bmi160.h +++ b/main/bmi160.h @@ -52,9 +52,9 @@ typedef struct { struct { - int16_t x; - int16_t y; - int16_t z; + int32_t x; // Fixed point Q.12 + int32_t y; // Fixed point Q.12 + int32_t z; // Fixed point Q.12 } acc, gyr; uint32_t time; } bmi160_value_t; diff --git a/main/fixedpoint.h b/main/fixedpoint.h new file mode 100644 index 0000000..e4f7fc8 --- /dev/null +++ b/main/fixedpoint.h @@ -0,0 +1,53 @@ +#pragma once + +#include + +/* + * 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))) diff --git a/plot.py b/plot.py index 0dbb7e9..dfe9243 100644 --- a/plot.py +++ b/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,7 +218,12 @@ 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) - label = f"{value:.0f}" + 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"] - stream_names = [stream["key"] for stream in stream_defs] + graph_streams = graph["streams"] + 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)) diff --git a/plot_config.json b/plot_config.json index 43d96ba..4d7f240 100644 --- a/plot_config.json +++ b/plot_config.json @@ -11,17 +11,17 @@ { "title": "Accelerometer", "streams": [ - { "key": "acc_x", "label": "ACC X", "color": [90, 170, 255] }, - { "key": "acc_y", "label": "ACC Y", "color": [80, 220, 140] }, - { "key": "acc_z", "label": "ACC Z", "color": [255, 200, 60] } + { "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], "interpreter": "fixed", "frac_bits": 12 }, + { "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_y", "label": "GYR Y", "color": [80, 220, 140] }, - { "key": "gyr_z", "label": "GYR Z", "color": [225, 200, 60] } + { "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], "interpreter": "fixed", "frac_bits": 12 }, + { "key": "gyr_z", "label": "GYR Z", "color": [225, 200, 60], "interpreter": "fixed", "frac_bits": 12 } ] } ]