diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 754e872..8ca6ab1 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -2,6 +2,8 @@ idf_component_register( SRCS "bbot.c" "bmi160.c" + "fixedpoint.c" + "mahony_filter.c" "motors.c" INCLUDE_DIRS "." diff --git a/main/bbot.c b/main/bbot.c index babf929..e8e9383 100644 --- a/main/bbot.c +++ b/main/bbot.c @@ -1,4 +1,6 @@ #include +#include +#include #include "driver/gpio.h" #include "driver/i2c.h" @@ -6,10 +8,13 @@ #include "esp_log.h" #include "freertos/FreeRTOS.h" #include "freertos/queue.h" +#include "freertos/semphr.h" #include "freertos/task.h" #include "motors.h" #include "bmi160.h" +#include "fixedpoint.h" +#include "mahony_filter.h" #define BBOT_DRDY_INPUT_PIN GPIO_NUM_1 @@ -18,8 +23,77 @@ #define BBOT_I2C_SDA_IO GPIO_NUM_2 #define BBOT_I2C_FREQ_HZ 100000 +#define BBOT_FILTER_FRAC_BITS 12 +#define BBOT_FILTER_KP FIXED_FROM_INT(2, BBOT_FILTER_FRAC_BITS) +#define BBOT_FILTER_KI 0 +#define BBOT_PROCESS_TASK_STACK_SIZE 4096 +#define BBOT_PROCESS_TASK_PRIORITY 5 +#define BBOT_PRINT_PERIOD_MS 20 + static bmi160_t imu; static QueueHandle_t imu_queue; +static SemaphoreHandle_t imu_state_mutex; + +typedef struct +{ + mahony_filter_t filter; + bmi160_value_t sample; + int32_t yaw; + int32_t pitch; + int32_t roll; + uint32_t last_time; + bool has_sample; + bool has_orientation; +} imu_state_t; + +static imu_state_t imu_state; + +static void imu_process_task(void *arg) +{ + QueueHandle_t queue = (QueueHandle_t)arg; + + for (;;) + { + bmi160_value_t sample; + + if (xQueueReceive(queue, &sample, portMAX_DELAY) != pdTRUE) + { + continue; + } + + if (xSemaphoreTake(imu_state_mutex, portMAX_DELAY) != pdTRUE) + { + continue; + } + + if (imu_state.has_sample) + { + const uint32_t dt_ticks = sample.time - imu_state.last_time; + const int32_t dt = FIXED_FROM_RATIO((int32_t)dt_ticks, 100, BBOT_FILTER_FRAC_BITS); + + mahony_filter_update_imu( + &imu_state.filter, + sample.gyr.x, + sample.gyr.y, + sample.gyr.z, + sample.acc.x, + sample.acc.y, + sample.acc.z, + dt); + mahony_filter_get_yaw_pitch_roll( + &imu_state.filter, + &imu_state.yaw, + &imu_state.pitch, + &imu_state.roll); + imu_state.has_orientation = true; + } + + imu_state.sample = sample; + imu_state.last_time = sample.time; + imu_state.has_sample = true; + xSemaphoreGive(imu_state_mutex); + } +} static esp_err_t init_i2c() { @@ -39,16 +113,80 @@ static esp_err_t init_i2c() void app_main(void) { + imu_state_mutex = xSemaphoreCreateMutex(); + if (imu_state_mutex == NULL) + { + ESP_ERROR_CHECK(ESP_ERR_NO_MEM); + } + + mahony_filter_init(&imu_state.filter, BBOT_FILTER_FRAC_BITS); + mahony_filter_set_gains(&imu_state.filter, BBOT_FILTER_KP, BBOT_FILTER_KI); + ESP_ERROR_CHECK(init_motors()); ESP_ERROR_CHECK(init_i2c()); ESP_ERROR_CHECK(imu_init(&imu, BBOT_I2C_PORT, BBOT_DRDY_INPUT_PIN, &imu_queue)); + if (xTaskCreate( + imu_process_task, + "imu_process", + BBOT_PROCESS_TASK_STACK_SIZE, + imu_queue, + BBOT_PROCESS_TASK_PRIORITY, + NULL) != pdPASS) + { + ESP_ERROR_CHECK(ESP_ERR_NO_MEM); + } while (1) { - bmi160_value_t v; - while (xQueueReceive(imu_queue, &v, pdMS_TO_TICKS(1000)) == pdTRUE) + bmi160_value_t sample; + int32_t q0; + int32_t q1; + int32_t q2; + int32_t q3; + int32_t yaw; + int32_t pitch; + int32_t roll; + bool has_sample; + bool has_orientation; + + if (xSemaphoreTake(imu_state_mutex, pdMS_TO_TICKS(50)) == pdTRUE) { - ESP_LOGI("[]", "%d %d %d %d %d %d %d", v.time, v.acc.x, v.acc.y, v.acc.z, v.gyr.x, v.gyr.y, v.gyr.z); + sample = imu_state.sample; + mahony_filter_get_quaternion(&imu_state.filter, &q0, &q1, &q2, &q3); + yaw = imu_state.yaw; + pitch = imu_state.pitch; + roll = imu_state.roll; + has_sample = imu_state.has_sample; + has_orientation = imu_state.has_orientation; + xSemaphoreGive(imu_state_mutex); } + else + { + vTaskDelay(pdMS_TO_TICKS(BBOT_PRINT_PERIOD_MS)); + continue; + } + + if (has_sample && has_orientation) + { + ESP_LOGI( + "[]", + "%" PRIu32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32, + sample.time, + sample.acc.x, + sample.acc.y, + sample.acc.z, + sample.gyr.x, + sample.gyr.y, + sample.gyr.z, + q0, + q1, + q2, + q3, + yaw, + pitch, + roll); + } + + vTaskDelay(pdMS_TO_TICKS(BBOT_PRINT_PERIOD_MS)); } } diff --git a/main/bmi160.c b/main/bmi160.c index 61489e6..0349a5b 100644 --- a/main/bmi160.c +++ b/main/bmi160.c @@ -236,9 +236,9 @@ esp_err_t imu_read(const bmi160_t *dev, bmi160_value_t *value) 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); + value->gyr.x = FIXED_MUL(FIXED_FROM_RATIO((int32_t)gyr_x * 5, 82, 12), FIXED_FROM_RATIO(31416, 1800000, 12), 12); // Scale to rad/s (pi/180 = 0.017453) + value->gyr.y = FIXED_MUL(FIXED_FROM_RATIO((int32_t)gyr_y * 5, 82, 12), FIXED_FROM_RATIO(31416, 1800000, 12), 12); // Scale to rad/s (pi/180 = 0.017453) + value->gyr.z = FIXED_MUL(FIXED_FROM_RATIO((int32_t)gyr_z * 5, 82, 12), FIXED_FROM_RATIO(31416, 1800000, 12), 12); // Scale to rad/s (pi/180 = 0.017453) uint8_t time[3]; ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_SENSORTIME, time, BMI160_SIZE_SENSORTIME)); diff --git a/main/fixedpoint.c b/main/fixedpoint.c new file mode 100644 index 0000000..e93f15d --- /dev/null +++ b/main/fixedpoint.c @@ -0,0 +1,156 @@ +#include "fixedpoint.h" + +#include +#include + +static const int32_t s_fixed_atan_table_q30[] = { + 843314857, 497837829, 263043836, 133525159, + 67021687, 33543516, 16775851, 8388437, + 4194283, 2097149, 1048575, 524288, + 262144, 131072, 65536, 32768, +}; + +static uint32_t fixed_isqrt_u64(uint64_t value) +{ + uint64_t bit = (uint64_t)1 << 62; + uint64_t result = 0; + + while (bit > value) + { + bit >>= 2; + } + + while (bit != 0) + { + if (value >= result + bit) + { + value -= result + bit; + result = (result >> 1) + bit; + } + else + { + result >>= 1; + } + + bit >>= 2; + } + + return (uint32_t)result; +} + +int32_t fixed_sqrt(int32_t value, uint8_t frac_bits) +{ + uint64_t scaled_value; + uint32_t root; + + if (value <= 0) + { + return 0; + } + + scaled_value = ((uint64_t)(uint32_t)value) << frac_bits; + root = fixed_isqrt_u64(scaled_value); + if (root > (uint32_t)INT32_MAX) + { + return INT32_MAX; + } + + return (int32_t)root; +} + +int32_t fixed_inv_sqrt(int32_t value, uint8_t frac_bits) +{ + int32_t root; + + if (value <= 0) + { + return 0; + } + + root = fixed_sqrt(value, frac_bits); + return FIXED_DIV(FIXED_ONE(frac_bits), root, frac_bits); +} + +int32_t fixed_atan2(int32_t y, int32_t x, uint8_t frac_bits) +{ + int64_t x_q30; + int64_t y_q30; + int64_t angle_q30 = 0; + int shift; + + if (x == 0) + { + if (y > 0) + { + return FIXED_HALF_PI(frac_bits); + } + if (y < 0) + { + return -FIXED_HALF_PI(frac_bits); + } + return 0; + } + + if (x < 0) + { + if (y >= 0) + { + return fixed_atan2(-y, -x, frac_bits) + FIXED_PI(frac_bits); + } + return fixed_atan2(-y, -x, frac_bits) - FIXED_PI(frac_bits); + } + + shift = 30 - frac_bits; + if (shift >= 0) + { + x_q30 = (int64_t)x << shift; + y_q30 = (int64_t)y << shift; + } + else + { + x_q30 = (int64_t)x >> (-shift); + y_q30 = (int64_t)y >> (-shift); + } + + for (size_t i = 0; i < sizeof(s_fixed_atan_table_q30) / sizeof(s_fixed_atan_table_q30[0]); ++i) + { + int64_t x_next; + int64_t y_next; + + if (y_q30 > 0) + { + x_next = x_q30 + (y_q30 >> i); + y_next = y_q30 - (x_q30 >> i); + angle_q30 += s_fixed_atan_table_q30[i]; + } + else + { + x_next = x_q30 - (y_q30 >> i); + y_next = y_q30 + (x_q30 >> i); + angle_q30 -= s_fixed_atan_table_q30[i]; + } + + x_q30 = x_next; + y_q30 = y_next; + } + + if (shift >= 0) + { + return (int32_t)(angle_q30 >> shift); + } + return (int32_t)(angle_q30 << (-shift)); +} + +int32_t fixed_asin(int32_t value, uint8_t frac_bits) +{ + const int32_t one = FIXED_ONE(frac_bits); + const int32_t clamped = FIXED_CLAMP(value, -one, one); + const int32_t one_minus_square = one - FIXED_MUL(clamped, clamped, frac_bits); + + if (one_minus_square <= 0) + { + return clamped >= 0 ? FIXED_HALF_PI(frac_bits) : -FIXED_HALF_PI(frac_bits); + } + + return fixed_atan2(clamped, fixed_sqrt(one_minus_square, frac_bits), frac_bits); +} diff --git a/main/fixedpoint.h b/main/fixedpoint.h index e4f7fc8..8383aa5 100644 --- a/main/fixedpoint.h +++ b/main/fixedpoint.h @@ -20,6 +20,8 @@ #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_PI(frac_bits) FIXED_FROM_RATIO(355, 113, frac_bits) +#define FIXED_HALF_PI(frac_bits) (FIXED_PI(frac_bits) >> 1) #define FIXED_FROM_INT(value, frac_bits) ((value) << (frac_bits)) #define FIXED_TO_INT(value, frac_bits) ((value) >> (frac_bits)) @@ -51,3 +53,8 @@ #define FIXED_FROM_RATIO_T(type, numerator, denominator, frac_bits) \ ((denominator) == 0 ? (type)0 : (type)(((int64_t)(numerator) << (frac_bits)) / (denominator))) + +int32_t fixed_sqrt(int32_t value, uint8_t frac_bits); +int32_t fixed_inv_sqrt(int32_t value, uint8_t frac_bits); +int32_t fixed_atan2(int32_t y, int32_t x, uint8_t frac_bits); +int32_t fixed_asin(int32_t value, uint8_t frac_bits); diff --git a/main/mahony_filter.c b/main/mahony_filter.c new file mode 100644 index 0000000..5fbeb2c --- /dev/null +++ b/main/mahony_filter.c @@ -0,0 +1,245 @@ +#include "mahony_filter.h" + +#include + +#include "fixedpoint.h" + +static void mahony_filter_normalize_quaternion(mahony_filter_t *filter) +{ + const uint8_t frac_bits = filter->frac_bits; + const int32_t norm_sq = + FIXED_MUL(filter->q0, filter->q0, frac_bits) + + FIXED_MUL(filter->q1, filter->q1, frac_bits) + + FIXED_MUL(filter->q2, filter->q2, frac_bits) + + FIXED_MUL(filter->q3, filter->q3, frac_bits); + const int32_t inv_norm = fixed_inv_sqrt(norm_sq, frac_bits); + + if (inv_norm == 0) + { + mahony_filter_reset(filter); + return; + } + + filter->q0 = FIXED_MUL(filter->q0, inv_norm, frac_bits); + filter->q1 = FIXED_MUL(filter->q1, inv_norm, frac_bits); + filter->q2 = FIXED_MUL(filter->q2, inv_norm, frac_bits); + filter->q3 = FIXED_MUL(filter->q3, inv_norm, frac_bits); +} + +void mahony_filter_init(mahony_filter_t *filter, uint8_t frac_bits) +{ + if (filter == NULL) + { + return; + } + + filter->frac_bits = frac_bits; + filter->kp = 0; + filter->ki = 0; + mahony_filter_reset(filter); +} + +void mahony_filter_reset(mahony_filter_t *filter) +{ + if (filter == NULL) + { + return; + } + + filter->integral_x = 0; + filter->integral_y = 0; + filter->integral_z = 0; + filter->q0 = FIXED_ONE(filter->frac_bits); + filter->q1 = 0; + filter->q2 = 0; + filter->q3 = 0; +} + +void mahony_filter_set_gains(mahony_filter_t *filter, int32_t kp, int32_t ki) +{ + if (filter == NULL) + { + return; + } + + filter->kp = kp; + filter->ki = ki; +} + +void mahony_filter_update_imu( + mahony_filter_t *filter, + int32_t gx, + int32_t gy, + int32_t gz, + int32_t ax, + int32_t ay, + int32_t az, + int32_t dt) +{ + uint8_t frac_bits; + int32_t half; + int32_t accel_norm_sq; + int32_t q0; + int32_t q1; + int32_t q2; + int32_t q3; + + if (filter == NULL || dt <= 0) + { + return; + } + + frac_bits = filter->frac_bits; + half = FIXED_HALF(frac_bits); + accel_norm_sq = + FIXED_MUL(ax, ax, frac_bits) + + FIXED_MUL(ay, ay, frac_bits) + + FIXED_MUL(az, az, frac_bits); + + q0 = filter->q0; + q1 = filter->q1; + q2 = filter->q2; + q3 = filter->q3; + + if (accel_norm_sq > 0) + { + const int32_t accel_inv_norm = fixed_inv_sqrt(accel_norm_sq, frac_bits); + + if (accel_inv_norm != 0) + { + const int32_t ax_n = FIXED_MUL(ax, accel_inv_norm, frac_bits); + const int32_t ay_n = FIXED_MUL(ay, accel_inv_norm, frac_bits); + const int32_t az_n = FIXED_MUL(az, accel_inv_norm, frac_bits); + const int32_t vx = FIXED_MUL(FIXED_FROM_INT(2, frac_bits), (FIXED_MUL(q1, q3, frac_bits) - FIXED_MUL(q0, q2, frac_bits)), frac_bits); + const int32_t vy = FIXED_MUL(FIXED_FROM_INT(2, frac_bits), (FIXED_MUL(q0, q1, frac_bits) + FIXED_MUL(q2, q3, frac_bits)), frac_bits); + const int32_t vz = + FIXED_MUL(q0, q0, frac_bits) - + FIXED_MUL(q1, q1, frac_bits) - + FIXED_MUL(q2, q2, frac_bits) + + FIXED_MUL(q3, q3, frac_bits); + const int32_t ex = FIXED_MUL(ay_n, vz, frac_bits) - FIXED_MUL(az_n, vy, frac_bits); + const int32_t ey = FIXED_MUL(az_n, vx, frac_bits) - FIXED_MUL(ax_n, vz, frac_bits); + const int32_t ez = FIXED_MUL(ax_n, vy, frac_bits) - FIXED_MUL(ay_n, vx, frac_bits); + + if (filter->ki != 0) + { + filter->integral_x += FIXED_MUL(FIXED_MUL(filter->ki, ex, frac_bits), dt, frac_bits); + filter->integral_y += FIXED_MUL(FIXED_MUL(filter->ki, ey, frac_bits), dt, frac_bits); + filter->integral_z += FIXED_MUL(FIXED_MUL(filter->ki, ez, frac_bits), dt, frac_bits); + } + else + { + filter->integral_x = 0; + filter->integral_y = 0; + filter->integral_z = 0; + } + + gx += FIXED_MUL(filter->kp, ex, frac_bits) + filter->integral_x; + gy += FIXED_MUL(filter->kp, ey, frac_bits) + filter->integral_y; + gz += FIXED_MUL(filter->kp, ez, frac_bits) + filter->integral_z; + } + } + + { + const int32_t half_dt = FIXED_MUL(half, dt, frac_bits); + const int32_t q_dot0 = -FIXED_MUL(q1, gx, frac_bits) - FIXED_MUL(q2, gy, frac_bits) - FIXED_MUL(q3, gz, frac_bits); + const int32_t q_dot1 = FIXED_MUL(q0, gx, frac_bits) + FIXED_MUL(q2, gz, frac_bits) - FIXED_MUL(q3, gy, frac_bits); + const int32_t q_dot2 = FIXED_MUL(q0, gy, frac_bits) - FIXED_MUL(q1, gz, frac_bits) + FIXED_MUL(q3, gx, frac_bits); + const int32_t q_dot3 = FIXED_MUL(q0, gz, frac_bits) + FIXED_MUL(q1, gy, frac_bits) - FIXED_MUL(q2, gx, frac_bits); + + filter->q0 += FIXED_MUL(q_dot0, half_dt, frac_bits); + filter->q1 += FIXED_MUL(q_dot1, half_dt, frac_bits); + filter->q2 += FIXED_MUL(q_dot2, half_dt, frac_bits); + filter->q3 += FIXED_MUL(q_dot3, half_dt, frac_bits); + } + + mahony_filter_normalize_quaternion(filter); +} + +void mahony_filter_get_quaternion( + const mahony_filter_t *filter, + int32_t *q0, + int32_t *q1, + int32_t *q2, + int32_t *q3) +{ + if (filter == NULL) + { + return; + } + + if (q0 != NULL) + { + *q0 = filter->q0; + } + if (q1 != NULL) + { + *q1 = filter->q1; + } + if (q2 != NULL) + { + *q2 = filter->q2; + } + if (q3 != NULL) + { + *q3 = filter->q3; + } +} + +void mahony_filter_get_yaw_pitch_roll( + const mahony_filter_t *filter, + int32_t *yaw, + int32_t *pitch, + int32_t *roll) +{ + uint8_t frac_bits; + int32_t two; + int32_t q0q0; + int32_t q1q1; + int32_t q2q2; + int32_t q3q3; + int32_t yaw_num; + int32_t yaw_den; + int32_t pitch_arg; + int32_t roll_num; + int32_t roll_den; + + if (filter == NULL) + { + return; + } + + frac_bits = filter->frac_bits; + two = FIXED_FROM_INT(2, frac_bits); + q0q0 = FIXED_MUL(filter->q0, filter->q0, frac_bits); + q1q1 = FIXED_MUL(filter->q1, filter->q1, frac_bits); + q2q2 = FIXED_MUL(filter->q2, filter->q2, frac_bits); + q3q3 = FIXED_MUL(filter->q3, filter->q3, frac_bits); + yaw_num = FIXED_MUL( + two, + FIXED_MUL(filter->q0, filter->q3, frac_bits) + FIXED_MUL(filter->q1, filter->q2, frac_bits), + frac_bits); + yaw_den = q0q0 + q1q1 - q2q2 - q3q3; + pitch_arg = FIXED_MUL( + two, + FIXED_MUL(filter->q0, filter->q2, frac_bits) - FIXED_MUL(filter->q3, filter->q1, frac_bits), + frac_bits); + roll_num = FIXED_MUL( + two, + FIXED_MUL(filter->q0, filter->q1, frac_bits) + FIXED_MUL(filter->q2, filter->q3, frac_bits), + frac_bits); + roll_den = q0q0 - q1q1 - q2q2 + q3q3; + + if (yaw != NULL) + { + *yaw = fixed_atan2(yaw_num, yaw_den, frac_bits); + } + if (pitch != NULL) + { + *pitch = fixed_asin(pitch_arg, frac_bits); + } + if (roll != NULL) + { + *roll = fixed_atan2(roll_num, roll_den, frac_bits); + } +} diff --git a/main/mahony_filter.h b/main/mahony_filter.h new file mode 100644 index 0000000..890f69e --- /dev/null +++ b/main/mahony_filter.h @@ -0,0 +1,41 @@ +#pragma once + +#include + +typedef struct +{ + uint8_t frac_bits; + int32_t kp; + int32_t ki; + int32_t integral_x; + int32_t integral_y; + int32_t integral_z; + int32_t q0; + int32_t q1; + int32_t q2; + int32_t q3; +} mahony_filter_t; + +void mahony_filter_init(mahony_filter_t *filter, uint8_t frac_bits); +void mahony_filter_reset(mahony_filter_t *filter); +void mahony_filter_set_gains(mahony_filter_t *filter, int32_t kp, int32_t ki); +void mahony_filter_update_imu( + mahony_filter_t *filter, + int32_t gx, + int32_t gy, + int32_t gz, + int32_t ax, + int32_t ay, + int32_t az, + int32_t dt); +void mahony_filter_get_quaternion( + const mahony_filter_t *filter, + int32_t *q0, + int32_t *q1, + int32_t *q2, + int32_t *q3); +void mahony_filter_get_yaw_pitch_roll( + const mahony_filter_t *filter, + int32_t *yaw, + int32_t *pitch, + int32_t *roll); diff --git a/plot.py b/plot.py index dfe9243..5f16dbc 100644 --- a/plot.py +++ b/plot.py @@ -238,6 +238,32 @@ def draw_grid( surface.blit(font.render(title, True, TEXT), (rect.left + 8, rect.top + 8)) +def draw_graph_info( + surface: pygame.Surface, + rect: pygame.Rect, + font: pygame.font.Font, + graph_streams: list[dict], + latest_values: dict[str, float | None], +) -> None: + line_height = font.get_height() + 6 + y = rect.top + 8 + for stream in graph_streams: + color = color_tuple(stream["color"]) + label = stream.get("label", stream["key"]) + latest_value = latest_values.get(stream["key"]) + value_text = "--" if latest_value is None else f"{latest_value:.3f}" + pygame.draw.line( + surface, + color, + (rect.left, y + 8), + (rect.left + 24, y + 8), + 3, + ) + text = font.render(f"{label}: {value_text}", True, TEXT) + surface.blit(text, (rect.left + 32, y)) + y += line_height + + def draw_trace( surface: pygame.Surface, rect: pygame.Rect, @@ -348,16 +374,20 @@ def main() -> int: width, height = screen.get_size() now = time.monotonic() graph_count = len(graphs) - plot_width = max(100, width - 100) + left_margin = 70 + right_margin = 20 + info_width = min(280, max(180, width // 4)) + plot_width = max(100, width - left_margin - right_margin - info_width - 16) available_height = max(100, height - 120 - (graph_count - 1) * 30) panel_height = max(100, available_height // graph_count) screen.fill(BACKGROUND) - latest_values: dict[str, int | None] = {} + latest_values: dict[str, float | None] = {} top = 40 for graph in graphs: - rect = pygame.Rect(70, top, plot_width, panel_height) + rect = pygame.Rect(left_margin, top, plot_width, panel_height) + info_rect = pygame.Rect(rect.right + 16, top, info_width, panel_height) 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) @@ -381,39 +411,16 @@ def main() -> int: y_max, color_tuple(stream["color"]), ) + draw_graph_info(screen, info_rect, small_font, graph_streams, latest_values) top = rect.bottom + 30 header = f"port={args.port} baud={args.baudrate} zoom={view_span:.1f}s" - for graph in graphs: - graph_parts = [] - 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:.3f}") - if graph_parts: - header += " | " + " ".join(graph_parts) screen.blit(font.render(header, True, TEXT), (10, 8)) screen.blit( small_font.render("mouse wheel: zoom horizontal axis", True, AXIS), (10, height - 24), ) - legend_x = 10 - legend_y = 34 - for graph in graphs: - for stream in graph["streams"]: - color = color_tuple(stream["color"]) - label = stream.get("label", stream["key"]) - pygame.draw.line( - screen, - color, - (legend_x, legend_y + 9), - (legend_x + 24, legend_y + 9), - 3, - ) - screen.blit(small_font.render(label, True, TEXT), (legend_x + 30, legend_y)) - legend_x += max(90, 42 + len(label) * 10) - if error_message: error_surface = font.render(error_message, True, ERROR) screen.blit(error_surface, (10, height - 52)) diff --git a/plot_config.json b/plot_config.json index 4d7f240..10cb4ce 100644 --- a/plot_config.json +++ b/plot_config.json @@ -4,7 +4,22 @@ "sources": [ { "tag": "IMU", - "fields": ["time", "acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"] + "fields": [ + "time", + "acc_x", + "acc_y", + "acc_z", + "gyr_x", + "gyr_y", + "gyr_z", + "q0", + "q1", + "q2", + "q3", + "yaw", + "pitch", + "roll" + ] } ], "graphs": [ @@ -23,6 +38,23 @@ { "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 } ] + }, + { + "title": "Quaternion", + "streams": [ + { "key": "q0", "label": "Q0", "color": [230, 90, 90], "interpreter": "fixed", "frac_bits": 12 }, + { "key": "q1", "label": "Q1", "color": [90, 170, 255], "interpreter": "fixed", "frac_bits": 12 }, + { "key": "q2", "label": "Q2", "color": [80, 220, 140], "interpreter": "fixed", "frac_bits": 12 }, + { "key": "q3", "label": "Q3", "color": [255, 200, 60], "interpreter": "fixed", "frac_bits": 12 } + ] + }, + { + "title": "Yaw Pitch Roll", + "streams": [ + { "key": "yaw", "label": "Yaw", "color": [230, 90, 90], "interpreter": "fixed", "frac_bits": 12 }, + { "key": "pitch", "label": "Pitch", "color": [90, 170, 255], "interpreter": "fixed", "frac_bits": 12 }, + { "key": "roll", "label": "Roll", "color": [80, 220, 140], "interpreter": "fixed", "frac_bits": 12 } + ] } ] }