joppe/BBOT
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Normalized values in Q12

Browse Source
This commit is contained in:
Joppe Blondel
2026-03-20 15:43:14 +01:00
parent 00e354c2b3
commit 1b6bd537ad
5 changed files with 123 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
main/bmi160.c
View File

@@ -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));

6
main/bmi160.h
View File

@@ -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;

53
main/fixedpoint.h Normal file
View File

@@ -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)))

53
plot.py
View File

@@ -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"]
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))

12
plot_config.json
View File

@@ -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 }
]
}
]