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