From 7a5224a57400ecaeb57cd8a2275375cebbfec4d0 Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Tue, 24 Mar 2026 12:25:01 +0000 Subject: [PATCH] feat(sensor): Add timestamp difference handling for IMU data processing --- core/include/librmcs/data/datas.hpp | 2 + core/src/protocol/deserializer.cpp | 4 ++ core/src/protocol/protocol.hpp | 6 ++- core/src/protocol/serializer.hpp | 3 ++ firmware/c_board/app/src/gpio/gpio.cpp | 9 +++- firmware/c_board/app/src/spi/bmi088/accel.hpp | 53 ++++++++++++++++--- firmware/c_board/app/src/spi/bmi088/gyro.hpp | 53 ++++++++++++++++--- firmware/rmcs_board/src/gpio/gpio.cpp | 9 +++- firmware/rmcs_board/src/spi/bmi088/accel.hpp | 53 ++++++++++++++++--- firmware/rmcs_board/src/spi/bmi088/gyro.hpp | 53 ++++++++++++++++--- 10 files changed, 215 insertions(+), 30 deletions(-) diff --git a/core/include/librmcs/data/datas.hpp b/core/include/librmcs/data/datas.hpp index afa015c..086bfd8 100644 --- a/core/include/librmcs/data/datas.hpp +++ b/core/include/librmcs/data/datas.hpp @@ -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 { diff --git a/core/src/protocol/deserializer.cpp b/core/src/protocol/deserializer.cpp index 657377c..9abb38c 100644 --- a/core/src/protocol/deserializer.cpp +++ b/core/src/protocol/deserializer.cpp @@ -267,6 +267,8 @@ coroutine::LifoTask Deserializer::process_imu_field(FieldId) { data_view.x = payload.get(); data_view.y = payload.get(); data_view.z = payload.get(); + data_view.timestamp_diff_quarter_us = + payload.get(); consume_peeked(); callback_.accelerometer_deserialized_callback(data_view); break; @@ -280,6 +282,8 @@ coroutine::LifoTask Deserializer::process_imu_field(FieldId) { data_view.x = payload.get(); data_view.y = payload.get(); data_view.z = payload.get(); + data_view.timestamp_diff_quarter_us = + payload.get(); consume_peeked(); callback_.gyroscope_deserialized_callback(data_view); break; diff --git a/core/src/protocol/protocol.hpp b/core/src/protocol/protocol.hpp index ba64224..05d0678 100644 --- a/core/src/protocol/protocol.hpp +++ b/core/src/protocol/protocol.hpp @@ -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 diff --git a/core/src/protocol/serializer.hpp b/core/src/protocol/serializer.hpp index 911349a..efbbd67 100644 --- a/core/src/protocol/serializer.hpp +++ b/core/src/protocol/serializer.hpp @@ -304,6 +304,8 @@ class Serializer { payload.set(view.x); payload.set(view.y); payload.set(view.z); + payload.set( + view.timestamp_diff_quarter_us); utility::assert_debug(cursor == dst.data() + dst.size()); return SerializeResult::kSuccess; @@ -329,6 +331,7 @@ class Serializer { payload.set(view.x); payload.set(view.y); payload.set(view.z); + payload.set(view.timestamp_diff_quarter_us); utility::assert_debug(cursor == dst.data() + dst.size()); return SerializeResult::kSuccess; diff --git a/firmware/c_board/app/src/gpio/gpio.cpp b/firmware/c_board/app/src/gpio/gpio.cpp index 54ae676..025bc60 100644 --- a/firmware/c_board/app/src/gpio/gpio.cpp +++ b/firmware/c_board/app/src/gpio/gpio.cpp @@ -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); } diff --git a/firmware/c_board/app/src/spi/bmi088/accel.hpp b/firmware/c_board/app/src/spi/bmi088/accel.hpp index e7fed92..4f6bb08 100644 --- a/firmware/c_board/app/src/spi/bmi088/accel.hpp +++ b/firmware/c_board/app/src/spi/bmi088/accel.hpp @@ -3,6 +3,7 @@ #include // IWYU pragma: keep (https://github.com/llvm/llvm-project/issues/68213) #include #include +#include #include @@ -53,6 +54,8 @@ class Accelerometer final public: using Lazy = utility::Lazy; + 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, @@ -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::max()) + return std::numeric_limits::max(); + return static_cast(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); diff --git a/firmware/c_board/app/src/spi/bmi088/gyro.hpp b/firmware/c_board/app/src/spi/bmi088/gyro.hpp index 0b78243..fb027a7 100644 --- a/firmware/c_board/app/src/spi/bmi088/gyro.hpp +++ b/firmware/c_board/app/src/spi/bmi088/gyro.hpp @@ -3,6 +3,7 @@ #include // IWYU pragma: keep (https://github.com/llvm/llvm-project/issues/68213) #include #include +#include #include @@ -45,6 +46,8 @@ class Gyroscope final public: using Lazy = utility::Lazy; + static constexpr uint16_t kFirstFrameTimestampDiffQuarterUs = 1'000'000 / 2000 * 4; + enum class Range : uint8_t { k2000 = 0x00, k1000 = 0x01, @@ -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::max()) + return std::numeric_limits::max(); + return static_cast(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); diff --git a/firmware/rmcs_board/src/gpio/gpio.cpp b/firmware/rmcs_board/src/gpio/gpio.cpp index bcdb4cd..9a1bffa 100644 --- a/firmware/rmcs_board/src/gpio/gpio.cpp +++ b/firmware/rmcs_board/src/gpio/gpio.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -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(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(mchtmr_get_count(HPM_MCHTMR) / 6U); + spi::bmi088::accelerometer->data_ready_callback(capture_timestamp_quarter_us); } } diff --git a/firmware/rmcs_board/src/spi/bmi088/accel.hpp b/firmware/rmcs_board/src/spi/bmi088/accel.hpp index 0ef1be4..2ebc3c3 100644 --- a/firmware/rmcs_board/src/spi/bmi088/accel.hpp +++ b/firmware/rmcs_board/src/spi/bmi088/accel.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -53,6 +54,8 @@ class Accelerometer final public: using Lazy = utility::Lazy; + 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, @@ -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::max()) + return std::numeric_limits::max(); + return static_cast(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( diff --git a/firmware/rmcs_board/src/spi/bmi088/gyro.hpp b/firmware/rmcs_board/src/spi/bmi088/gyro.hpp index 2c5e3da..e2946ee 100644 --- a/firmware/rmcs_board/src/spi/bmi088/gyro.hpp +++ b/firmware/rmcs_board/src/spi/bmi088/gyro.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -45,6 +46,8 @@ class Gyroscope final public: using Lazy = utility::Lazy; + static constexpr uint16_t kFirstFrameTimestampDiffQuarterUs = 1'000'000 / 2000 * 4; + enum class DataRange : uint8_t { k2000 = 0x00, k1000 = 0x01, @@ -98,22 +101,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(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_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::max()) + return std::numeric_limits::max(); + return static_cast(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 Gyroscope::Lazy gyroscope(