Skip to content

feat(sensor): Add timestamp difference handling for IMU data processing #38

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
qzhhhi wants to merge 1 commit into
base: main
Choose a base branch
from
Open

feat(sensor): Add timestamp difference handling for IMU data processing #38

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions core/include/librmcs/data/datas.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,12 +71,14 @@ struct AccelerometerDataView {
int16_t x;
int16_t y;
int16_t z;
uint16_t timestamp_diff_quarter_us;
};

struct GyroscopeDataView {
int16_t x;
int16_t y;
int16_t z;
uint16_t timestamp_diff_quarter_us;
};

class DataCallback {
Expand Down
4 changes: 4 additions & 0 deletions core/src/protocol/deserializer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -267,6 +267,8 @@ coroutine::LifoTask<bool> Deserializer::process_imu_field(FieldId) {
data_view.x = payload.get<ImuAccelerometerPayload::X>();
data_view.y = payload.get<ImuAccelerometerPayload::Y>();
data_view.z = payload.get<ImuAccelerometerPayload::Z>();
data_view.timestamp_diff_quarter_us =
payload.get<ImuAccelerometerPayload::TimestampDiffQuarterUs>();
consume_peeked();
callback_.accelerometer_deserialized_callback(data_view);
break;
Expand All @@ -280,6 +282,8 @@ coroutine::LifoTask<bool> Deserializer::process_imu_field(FieldId) {
data_view.x = payload.get<ImuGyroscopePayload::X>();
data_view.y = payload.get<ImuGyroscopePayload::Y>();
data_view.z = payload.get<ImuGyroscopePayload::Z>();
data_view.timestamp_diff_quarter_us =
payload.get<ImuGyroscopePayload::TimestampDiffQuarterUs>();
consume_peeked();
callback_.gyroscope_deserialized_callback(data_view);
break;
Expand Down
6 changes: 4 additions & 2 deletions core/src/protocol/protocol.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,16 +118,18 @@ struct ImuHeader : utility::Bitfield<1> {
using PayloadType = utility::BitfieldMember<4, 4, PayloadEnum>;
};

struct ImuAccelerometerPayload : utility::Bitfield<6> {
struct ImuAccelerometerPayload : utility::Bitfield<8> {
using X = utility::BitfieldMember<0, 16, int16_t>;
using Y = utility::BitfieldMember<16, 16, int16_t>;
using Z = utility::BitfieldMember<32, 16, int16_t>;
using TimestampDiffQuarterUs = utility::BitfieldMember<48, 16, uint16_t>;
};

struct ImuGyroscopePayload : utility::Bitfield<6> {
struct ImuGyroscopePayload : utility::Bitfield<8> {
using X = utility::BitfieldMember<0, 16, int16_t>;
using Y = utility::BitfieldMember<16, 16, int16_t>;
using Z = utility::BitfieldMember<32, 16, int16_t>;
using TimestampDiffQuarterUs = utility::BitfieldMember<48, 16, uint16_t>;
};

} // namespace librmcs::core::protocol
3 changes: 3 additions & 0 deletions core/src/protocol/serializer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -304,6 +304,8 @@ class Serializer {
payload.set<ImuAccelerometerPayload::X>(view.x);
payload.set<ImuAccelerometerPayload::Y>(view.y);
payload.set<ImuAccelerometerPayload::Z>(view.z);
payload.set<ImuAccelerometerPayload::TimestampDiffQuarterUs>(
view.timestamp_diff_quarter_us);

utility::assert_debug(cursor == dst.data() + dst.size());
return SerializeResult::kSuccess;
Expand All @@ -329,6 +331,7 @@ class Serializer {
payload.set<ImuGyroscopePayload::X>(view.x);
payload.set<ImuGyroscopePayload::Y>(view.y);
payload.set<ImuGyroscopePayload::Z>(view.z);
payload.set<ImuGyroscopePayload::TimestampDiffQuarterUs>(view.timestamp_diff_quarter_us);

utility::assert_debug(cursor == dst.data() + dst.size());
return SerializeResult::kSuccess;
Expand Down
9 changes: 7 additions & 2 deletions firmware/c_board/app/src/gpio/gpio.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,19 @@

#include "firmware/c_board/app/src/spi/bmi088/accel.hpp"
#include "firmware/c_board/app/src/spi/bmi088/gyro.hpp"
#include "firmware/c_board/app/src/timer/timer.hpp"

namespace librmcs::firmware::gpio {

extern "C" void HAL_GPIO_EXTI_Callback(uint16_t gpio_pin) {
if (gpio_pin == INT1_ACC_Pin) {
spi::bmi088::accelerometer->data_ready_callback();
const uint32_t capture_timestamp_quarter_us =
timer::timer->timepoint().time_since_epoch().count();
spi::bmi088::accelerometer->data_ready_callback(capture_timestamp_quarter_us);
} else if (gpio_pin == INT1_GYRO_Pin) {
spi::bmi088::gyroscope->data_ready_callback();
const uint32_t capture_timestamp_quarter_us =
timer::timer->timepoint().time_since_epoch().count();
spi::bmi088::gyroscope->data_ready_callback(capture_timestamp_quarter_us);
} else {
gpio::gpio->handle_input_edge_interrupt(gpio_pin);
}
Expand Down
53 changes: 47 additions & 6 deletions firmware/c_board/app/src/spi/bmi088/accel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <chrono> // IWYU pragma: keep (https://github.com/llvm/llvm-project/issues/68213)
#include <cstddef>
#include <cstdint>
#include <limits>

#include <main.h>

Expand Down Expand Up @@ -53,6 +54,8 @@ class Accelerometer final
public:
using Lazy = utility::Lazy<Accelerometer, Spi::Lazy*>;

static constexpr uint16_t kFirstFrameTimestampDiffQuarterUs = 1'000'000 / 1600 * 4;
Copy link
Copy Markdown

@coderabbitai coderabbitai bot Mar 24, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🟠 Major

可配置 ODR 与时间差编码范围不匹配。

kFirstFrameTimestampDiffQuarterUs 固定按 1600Hz 计算，所以任何非默认 data_rate 的首帧都会写错；另外 uint16_t quarter-us 的上限只有 16383.75us（约 61Hz），而 DataRate 还支持 k50Hz/k25Hz/k12Hz，这些模式的正常周期都会被长期压成 65535。要么把时间差字段改大/改单位，要么把可选 ODR 限制在可表示范围内。

Also applies to: 147-159

🤖 Prompt for AI Agents 
Verify each finding against the current code and only fix it if needed.

In `@firmware/c_board/app/src/spi/bmi088/accel.hpp` at line 57,
kFirstFrameTimestampDiffQuarterUs is defined as a fixed 1/1600-derived
quarter-microsecond value and stored in a uint16_t, which mismatches supported
DataRate values (e.g. k50Hz/k25Hz/k12Hz) and causes overflow/clamping for lower
ODRs; update the implementation so timestamp-diff encoding can represent all
DataRate periods by either (A) widening the field type (replace uint16_t with a
larger integer like uint32_t) or changing the unit (e.g. use microseconds
instead of quarter-µs) and recomputing kFirstFrameTimestampDiff... constants for
each DataRate, or (B) restrict/validate DataRate to only those representable by
the current quarter-us/uint16_t range; apply this change consistently wherever
kFirstFrameTimestampDiffQuarterUs and its related constants (lines referenced
147-159) are used and ensure DataRate-to-timestamp conversion functions handle
the new unit/size.


enum class Range : uint8_t { k3G = 0x00, k6G = 0x01, k12G = 0x02, k24G = 0x03 };
enum class DataRate : uint8_t {
k12Hz = 0x05,
Expand Down Expand Up @@ -105,22 +108,60 @@ class Accelerometer final
spi_.unlock();
}

void data_ready_callback() { read_async(RegisterAddress::kAccXLsb, 6); }
void data_ready_callback(uint32_t capture_timestamp_quarter_us) {
if (read_async(RegisterAddress::kAccXLsb, 6)) {
pending_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us;
has_pending_capture_timestamp_ = true;
}
}

private:
void transmit_receive_async_callback(size_t size) override {
if (size) [[likely]] {
core::utility::assert_debug(!size || has_pending_capture_timestamp_);
if (size && has_pending_capture_timestamp_) [[likely]] {
auto& data = parse_rx_data(spi_.rx_buffer, size);
handle_uplink(usb::vendor->serializer(), data);
handle_uplink(usb::vendor->serializer(), data, pending_capture_timestamp_quarter_us_);
}
has_pending_capture_timestamp_ = false;
spi_.unlock();
}

static void handle_uplink(core::protocol::Serializer& serializer, Data& data) {
void handle_uplink(
core::protocol::Serializer& serializer, Data& data, uint32_t capture_timestamp_quarter_us) {
const uint16_t timestamp_diff_quarter_us =
calculate_timestamp_diff_quarter_us(capture_timestamp_quarter_us);
const auto result = serializer.write_imu_accelerometer({
.x = data.x,
.y = data.y,
.z = data.z,
.timestamp_diff_quarter_us = timestamp_diff_quarter_us,
});
core::utility::assert_debug(
serializer.write_imu_accelerometer({.x = data.x, .y = data.y, .z = data.z})
!= core::protocol::Serializer::SerializeResult::kInvalidArgument);
result != core::protocol::Serializer::SerializeResult::kInvalidArgument);
if (result == core::protocol::Serializer::SerializeResult::kSuccess) {
last_success_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us;
has_last_success_capture_timestamp_ = true;
}
}

uint16_t calculate_timestamp_diff_quarter_us(uint32_t capture_timestamp_quarter_us) const {
if (!has_last_success_capture_timestamp_)
return kFirstFrameTimestampDiffQuarterUs;

return saturate_timestamp_diff_quarter_us(
capture_timestamp_quarter_us - last_success_capture_timestamp_quarter_us_);
}

static uint16_t saturate_timestamp_diff_quarter_us(uint32_t timestamp_diff_quarter_us) {
if (timestamp_diff_quarter_us > std::numeric_limits<uint16_t>::max())
return std::numeric_limits<uint16_t>::max();
return static_cast<uint16_t>(timestamp_diff_quarter_us);
}

uint32_t pending_capture_timestamp_quarter_us_ = 0;
uint32_t last_success_capture_timestamp_quarter_us_ = 0;
bool has_pending_capture_timestamp_ = false;
bool has_last_success_capture_timestamp_ = false;
};

inline constinit Accelerometer::Lazy accelerometer(&spi1);
Expand Down
53 changes: 47 additions & 6 deletions firmware/c_board/app/src/spi/bmi088/gyro.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <chrono> // IWYU pragma: keep (https://github.com/llvm/llvm-project/issues/68213)
#include <cstddef>
#include <cstdint>
#include <limits>

#include <main.h>

Expand Down Expand Up @@ -45,6 +46,8 @@ class Gyroscope final
public:
using Lazy = utility::Lazy<Gyroscope, Spi::Lazy*>;

static constexpr uint16_t kFirstFrameTimestampDiffQuarterUs = 1'000'000 / 2000 * 4;

enum class Range : uint8_t {
k2000 = 0x00,
k1000 = 0x01,
Expand Down Expand Up @@ -100,22 +103,60 @@ class Gyroscope final
spi_.unlock();
}

void data_ready_callback() { read_async(RegisterAddress::kRateXLsb, 6); }
void data_ready_callback(uint32_t capture_timestamp_quarter_us) {
if (read_async(RegisterAddress::kRateXLsb, 6)) {
pending_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us;
has_pending_capture_timestamp_ = true;
}
}

private:
void transmit_receive_async_callback(size_t size) override {
if (size) [[likely]] {
core::utility::assert_debug(!size || has_pending_capture_timestamp_);
if (size && has_pending_capture_timestamp_) [[likely]] {
auto& data = parse_rx_data(spi_.rx_buffer, size);
handle_uplink(usb::vendor->serializer(), data);
handle_uplink(usb::vendor->serializer(), data, pending_capture_timestamp_quarter_us_);
}
has_pending_capture_timestamp_ = false;
spi_.unlock();
}

static void handle_uplink(core::protocol::Serializer& serializer, Data& data) {
void handle_uplink(
core::protocol::Serializer& serializer, Data& data, uint32_t capture_timestamp_quarter_us) {
const uint16_t timestamp_diff_quarter_us =
calculate_timestamp_diff_quarter_us(capture_timestamp_quarter_us);
const auto result = serializer.write_imu_gyroscope({
.x = data.x,
.y = data.y,
.z = data.z,
.timestamp_diff_quarter_us = timestamp_diff_quarter_us,
});
core::utility::assert_debug(
serializer.write_imu_gyroscope({.x = data.x, .y = data.y, .z = data.z})
!= core::protocol::Serializer::SerializeResult::kInvalidArgument);
result != core::protocol::Serializer::SerializeResult::kInvalidArgument);
if (result == core::protocol::Serializer::SerializeResult::kSuccess) {
last_success_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us;
has_last_success_capture_timestamp_ = true;
}
}

uint16_t calculate_timestamp_diff_quarter_us(uint32_t capture_timestamp_quarter_us) const {
if (!has_last_success_capture_timestamp_)
return kFirstFrameTimestampDiffQuarterUs;

return saturate_timestamp_diff_quarter_us(
capture_timestamp_quarter_us - last_success_capture_timestamp_quarter_us_);
}

static uint16_t saturate_timestamp_diff_quarter_us(uint32_t timestamp_diff_quarter_us) {
if (timestamp_diff_quarter_us > std::numeric_limits<uint16_t>::max())
return std::numeric_limits<uint16_t>::max();
return static_cast<uint16_t>(timestamp_diff_quarter_us);
}

uint32_t pending_capture_timestamp_quarter_us_ = 0;
uint32_t last_success_capture_timestamp_quarter_us_ = 0;
bool has_pending_capture_timestamp_ = false;
bool has_last_success_capture_timestamp_ = false;
};

inline constinit Gyroscope::Lazy gyroscope(&spi1);
Expand Down
9 changes: 7 additions & 2 deletions firmware/rmcs_board/src/gpio/gpio.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <hpm_gpiom_soc_drv.h>
#include <hpm_ioc_regs.h>
#include <hpm_iomux.h>
#include <hpm_mchtmr_drv.h>
#include <hpm_pmic_iomux.h>
#include <hpm_soc.h>
#include <hpm_soc_irq.h>
Expand Down Expand Up @@ -58,14 +59,18 @@ void init_bmi088_interrupts() {
SDK_DECLARE_EXT_ISR_M(IRQn_GPIO0_B, gpio_bmi088_int_gyro_isr)
void gpio_bmi088_int_gyro_isr() {
if (gpio_check_clear_interrupt_flag(HPM_GPIO0, GPIO_DI_GPIOB, kBmi088IntGyroPin)) {
spi::bmi088::gyroscope->data_ready_callback();
const uint32_t capture_timestamp_quarter_us =
static_cast<uint32_t>(mchtmr_get_count(HPM_MCHTMR) / 6U);
spi::bmi088::gyroscope->data_ready_callback(capture_timestamp_quarter_us);
}
}

SDK_DECLARE_EXT_ISR_M(IRQn_GPIO0_Y, gpio_bmi088_int_accel_isr)
void gpio_bmi088_int_accel_isr() {
if (gpio_check_clear_interrupt_flag(HPM_GPIO0, GPIO_DI_GPIOY, kBmi088IntAccelPin)) {
spi::bmi088::accelerometer->data_ready_callback();
const uint32_t capture_timestamp_quarter_us =
static_cast<uint32_t>(mchtmr_get_count(HPM_MCHTMR) / 6U);
spi::bmi088::accelerometer->data_ready_callback(capture_timestamp_quarter_us);
}
}

Expand Down
53 changes: 47 additions & 6 deletions firmware/rmcs_board/src/spi/bmi088/accel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include <cstddef>
#include <cstdint>
#include <limits>

#include <board.h>
#include <hpm_gpio_regs.h>
Expand Down Expand Up @@ -53,6 +54,8 @@ class Accelerometer final
public:
using Lazy = utility::Lazy<Accelerometer, Spi::Lazy*, ChipSelectPin>;

static constexpr uint16_t kFirstFrameTimestampDiffQuarterUs = 1'000'000 / 1600 * 4;

enum class Range : uint8_t { k3G = 0x00, k6G = 0x01, k12G = 0x02, k24G = 0x03 };
enum class DataRate : uint8_t {
k12Hz = 0x05,
Expand Down Expand Up @@ -104,22 +107,60 @@ class Accelerometer final
spi_.unlock();
}

void data_ready_callback() { read_async(RegisterAddress::kAccXLsb, 6); }
void data_ready_callback(uint32_t capture_timestamp_quarter_us) {
if (read_async(RegisterAddress::kAccXLsb, 6)) {
pending_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us;
has_pending_capture_timestamp_ = true;
}
}

private:
void transmit_receive_async_callback(std::size_t size) override {
if (size) [[likely]] {
core::utility::assert_debug(!size || has_pending_capture_timestamp_);
if (size && has_pending_capture_timestamp_) [[likely]] {
auto& data = parse_rx_data(spi_.rx_buffer, size);
handle_uplink(usb::vendor->serializer(), data);
handle_uplink(usb::vendor->serializer(), data, pending_capture_timestamp_quarter_us_);
}
has_pending_capture_timestamp_ = false;
spi_.unlock();
}

static void handle_uplink(core::protocol::Serializer& serializer, Data& data) {
void handle_uplink(
core::protocol::Serializer& serializer, Data& data, uint32_t capture_timestamp_quarter_us) {
const uint16_t timestamp_diff_quarter_us =
calculate_timestamp_diff_quarter_us(capture_timestamp_quarter_us);
const auto result = serializer.write_imu_accelerometer({
.x = data.x,
.y = data.y,
.z = data.z,
.timestamp_diff_quarter_us = timestamp_diff_quarter_us,
});
core::utility::assert_debug(
serializer.write_imu_accelerometer({.x = data.x, .y = data.y, .z = data.z})
!= core::protocol::Serializer::SerializeResult::kInvalidArgument);
result != core::protocol::Serializer::SerializeResult::kInvalidArgument);
if (result == core::protocol::Serializer::SerializeResult::kSuccess) {
last_success_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us;
has_last_success_capture_timestamp_ = true;
}
}

uint16_t calculate_timestamp_diff_quarter_us(uint32_t capture_timestamp_quarter_us) const {
if (!has_last_success_capture_timestamp_)
return kFirstFrameTimestampDiffQuarterUs;

return saturate_timestamp_diff_quarter_us(
capture_timestamp_quarter_us - last_success_capture_timestamp_quarter_us_);
}

static uint16_t saturate_timestamp_diff_quarter_us(uint32_t timestamp_diff_quarter_us) {
if (timestamp_diff_quarter_us > std::numeric_limits<uint16_t>::max())
return std::numeric_limits<uint16_t>::max();
return static_cast<uint16_t>(timestamp_diff_quarter_us);
}

uint32_t pending_capture_timestamp_quarter_us_ = 0;
uint32_t last_success_capture_timestamp_quarter_us_ = 0;
bool has_pending_capture_timestamp_ = false;
bool has_last_success_capture_timestamp_ = false;
};

inline Accelerometer::Lazy accelerometer(
Expand Down
Loading
Loading