diff --git a/core/include/librmcs/data/datas.hpp b/core/include/librmcs/data/datas.hpp index 3e7bd86..9f35358 100644 --- a/core/include/librmcs/data/datas.hpp +++ b/core/include/librmcs/data/datas.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -46,6 +47,7 @@ struct UartDataView { struct GpioDigitalDataView { bool high; + std::optional timestamp_quarter_us = std::nullopt; }; struct GpioAnalogDataView { @@ -63,6 +65,7 @@ struct GpioReadConfigView { bool asap = false; bool rising_edge = false; bool falling_edge = false; + bool capture_timestamp = false; GpioPull pull = GpioPull::kNone; [[nodiscard]] constexpr bool supported(const spec::GpioDescriptor& gpio) const noexcept { @@ -71,7 +74,8 @@ struct GpioReadConfigView { && ((!rising_edge && !falling_edge) || gpio.supports(spec::GpioCapability::kDigitalReadInterrupt)) && (pull != GpioPull::kUp || gpio.supports(spec::GpioCapability::kPullUp)) - && (pull != GpioPull::kDown || gpio.supports(spec::GpioCapability::kPullDown)); + && (pull != GpioPull::kDown || gpio.supports(spec::GpioCapability::kPullDown)) + && (!capture_timestamp || gpio.supports(spec::GpioCapability::kTimestampedDigitalRead)); } }; @@ -79,12 +83,19 @@ struct AccelerometerDataView { int16_t x; int16_t y; int16_t z; + uint32_t timestamp_quarter_us; }; struct GyroscopeDataView { int16_t x; int16_t y; int16_t z; + uint32_t timestamp_quarter_us; +}; + +struct TemperatureDataView { + uint16_t temperature; + uint32_t timestamp_quarter_us; }; class DataCallback { @@ -110,6 +121,8 @@ class DataCallback { virtual void accelerometer_receive_callback(const AccelerometerDataView& data) = 0; virtual void gyroscope_receive_callback(const GyroscopeDataView& data) = 0; + + virtual void temperature_receive_callback(const TemperatureDataView& data) = 0; }; } // namespace librmcs::data diff --git a/core/include/librmcs/spec/gpio.hpp b/core/include/librmcs/spec/gpio.hpp index b279f44..4dccaa7 100644 --- a/core/include/librmcs/spec/gpio.hpp +++ b/core/include/librmcs/spec/gpio.hpp @@ -14,6 +14,7 @@ enum class GpioCapability : std::uint8_t { kDigitalReadInterrupt = 1U << 4, kPullUp = 1U << 5, kPullDown = 1U << 6, + kTimestampedDigitalRead = 1U << 7, }; constexpr GpioCapability operator|(GpioCapability a, GpioCapability b) noexcept { @@ -31,7 +32,7 @@ constexpr GpioCapability operator~(GpioCapability a) noexcept { inline constexpr GpioCapability kDigitalCapabilities = GpioCapability::kDigitalWrite | GpioCapability::kDigitalReadOnce | GpioCapability::kDigitalReadPeriodic | GpioCapability::kDigitalReadInterrupt - | GpioCapability::kPullUp | GpioCapability::kPullDown; + | GpioCapability::kPullUp | GpioCapability::kPullDown | GpioCapability::kTimestampedDigitalRead; inline constexpr GpioCapability kPwmCapabilities = kDigitalCapabilities | GpioCapability::kAnalogWrite; diff --git a/core/src/protocol/deserializer.cpp b/core/src/protocol/deserializer.cpp index c3baf9b..0408fca 100644 --- a/core/src/protocol/deserializer.cpp +++ b/core/src/protocol/deserializer.cpp @@ -154,7 +154,7 @@ coroutine::LifoTask Deserializer::process_uart_field(FieldId field_id) { coroutine::LifoTask Deserializer::process_gpio_field(FieldId) { GpioHeader::PayloadEnum payload_type; std::uint8_t channel_index = 0; - data::GpioPull pull = data::GpioPull::kNone; + bool timestamped = false; { const auto* header_bytes = co_await peek_bytes(sizeof(GpioHeader)); if (!header_bytes) [[unlikely]] @@ -163,7 +163,7 @@ coroutine::LifoTask Deserializer::process_gpio_field(FieldId) { auto header = GpioHeader::CRef{header_bytes}; payload_type = header.get(); channel_index = header.get(); - pull = header.get(); + timestamped = header.get(); consume_peeked(); } @@ -176,6 +176,8 @@ coroutine::LifoTask Deserializer::process_gpio_field(FieldId) { break; } case GpioHeader::PayloadEnum::kAnalogWrite: { + if (timestamped) [[unlikely]] + co_return false; const auto* payload_bytes = co_await peek_bytes(sizeof(GpioAnalogPayload)); if (!payload_bytes) [[unlikely]] co_return false; @@ -199,8 +201,9 @@ coroutine::LifoTask Deserializer::process_gpio_field(FieldId) { data_view.asap = payload.get(); data_view.rising_edge = payload.get(); data_view.falling_edge = payload.get(); + data_view.capture_timestamp = timestamped; + data_view.pull = payload.get(); data_view.period_ms = payload.get(); - data_view.pull = pull; consume_peeked(); if (data_view.pull != data::GpioPull::kNone && data_view.pull != data::GpioPull::kUp @@ -210,6 +213,8 @@ coroutine::LifoTask Deserializer::process_gpio_field(FieldId) { if (payload_type == GpioHeader::PayloadEnum::kDigitalRead) { callback_.gpio_digital_read_config_deserialized_callback(channel_index, data_view); } else { + if (data_view.capture_timestamp || data_view.rising_edge || data_view.falling_edge) + co_return false; callback_.gpio_analog_read_config_deserialized_callback(channel_index, data_view); } break; @@ -218,10 +223,22 @@ coroutine::LifoTask Deserializer::process_gpio_field(FieldId) { case GpioHeader::PayloadEnum::kDigitalReadResultHigh: { data::GpioDigitalDataView data_view{}; data_view.high = payload_type == GpioHeader::PayloadEnum::kDigitalReadResultHigh; + if (timestamped) { + const auto* payload_bytes = + co_await peek_bytes(sizeof(GpioDigitalReadTimestampPayload)); + if (!payload_bytes) [[unlikely]] + co_return false; + auto payload = GpioDigitalReadTimestampPayload::CRef{payload_bytes}; + data_view.timestamp_quarter_us = + payload.get(); + consume_peeked(); + } callback_.gpio_digital_data_deserialized_callback(channel_index, data_view); break; } case GpioHeader::PayloadEnum::kAnalogReadResult: { + if (timestamped) [[unlikely]] + co_return false; const auto* payload_bytes = co_await peek_bytes(sizeof(GpioAnalogPayload)); if (!payload_bytes) [[unlikely]] co_return false; @@ -262,6 +279,7 @@ 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_quarter_us = payload.get(); consume_peeked(); callback_.accelerometer_deserialized_callback(data_view); break; @@ -275,10 +293,23 @@ 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_quarter_us = payload.get(); consume_peeked(); callback_.gyroscope_deserialized_callback(data_view); break; } + case ImuHeader::PayloadEnum::kTemperature: { + data::TemperatureDataView data_view{}; + const auto* payload_bytes = co_await peek_bytes(sizeof(ImuTemperaturePayload)); + if (!payload_bytes) [[unlikely]] + co_return false; + auto payload = ImuTemperaturePayload::CRef{payload_bytes}; + data_view.temperature = payload.get(); + data_view.timestamp_quarter_us = payload.get(); + consume_peeked(); + callback_.temperature_deserialized_callback(data_view); + break; + } default: co_return false; } co_return true; diff --git a/core/src/protocol/deserializer.hpp b/core/src/protocol/deserializer.hpp index 7ac65d7..ea0dc87 100644 --- a/core/src/protocol/deserializer.hpp +++ b/core/src/protocol/deserializer.hpp @@ -44,6 +44,8 @@ class DeserializeCallback { virtual void gyroscope_deserialized_callback(const data::GyroscopeDataView& data) = 0; + virtual void temperature_deserialized_callback(const data::TemperatureDataView& data) = 0; + virtual void error_callback() = 0; }; diff --git a/core/src/protocol/protocol.hpp b/core/src/protocol/protocol.hpp index 12cfd92..c8be741 100644 --- a/core/src/protocol/protocol.hpp +++ b/core/src/protocol/protocol.hpp @@ -96,14 +96,19 @@ struct GpioHeader : utility::Bitfield<2> { using PayloadType = utility::BitfieldMember<4, 4, PayloadEnum>; using ChannelIndex = utility::BitfieldMember<8, 6>; - using Pull = utility::BitfieldMember<14, 2, data::GpioPull>; + using Timestamped = utility::BitfieldMember<15, 1>; }; -struct GpioReadConfigPayload : utility::Bitfield<2> { +struct GpioReadConfigPayload : utility::Bitfield<3> { using Asap = utility::BitfieldMember<0, 1>; using RisingEdge = utility::BitfieldMember<1, 1>; using FallingEdge = utility::BitfieldMember<2, 1>; - using PeriodMs = utility::BitfieldMember<3, 13, uint16_t>; + using Pull = utility::BitfieldMember<4, 2, data::GpioPull>; + using PeriodMs = utility::BitfieldMember<8, 16, uint16_t>; +}; + +struct GpioDigitalReadTimestampPayload : utility::Bitfield<4> { + using TimestampQuarterUs = utility::BitfieldMember<0, 32, uint32_t>; }; struct GpioAnalogPayload : utility::Bitfield<2> { @@ -114,20 +119,28 @@ struct ImuHeader : utility::Bitfield<1> { enum class PayloadEnum : uint8_t { kAccelerometer = 0, kGyroscope = 1, + kTemperature = 2, }; using PayloadType = utility::BitfieldMember<4, 4, PayloadEnum>; }; -struct ImuAccelerometerPayload : utility::Bitfield<6> { +struct ImuAccelerometerPayload : utility::Bitfield<10> { 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 TimestampQuarterUs = utility::BitfieldMember<48, 32, uint32_t>; }; -struct ImuGyroscopePayload : utility::Bitfield<6> { +struct ImuGyroscopePayload : utility::Bitfield<10> { 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 TimestampQuarterUs = utility::BitfieldMember<48, 32, uint32_t>; +}; + +struct ImuTemperaturePayload : utility::Bitfield<6> { + using Temperature = utility::BitfieldMember<0, 16, uint16_t>; + using TimestampQuarterUs = utility::BitfieldMember<16, 32, uint32_t>; }; } // namespace librmcs::core::protocol diff --git a/core/src/protocol/serializer.hpp b/core/src/protocol/serializer.hpp index 16821ef..2301e3f 100644 --- a/core/src/protocol/serializer.hpp +++ b/core/src/protocol/serializer.hpp @@ -123,6 +123,8 @@ class Serializer { SerializeResult write_gpio_digital_data( uint8_t channel_index, const data::GpioDigitalDataView& view) noexcept { utility::assert_debug(channel_index < (1U << GpioHeader::ChannelIndex::kBitWidth)); + LIBRMCS_VERIFY_LIKELY( + !view.timestamp_quarter_us.has_value(), SerializeResult::kInvalidArgument); const auto payload_type = view.high ? GpioHeader::PayloadEnum::kDigitalWriteHigh : GpioHeader::PayloadEnum::kDigitalWriteLow; const std::size_t required = required_gpio_size(FieldId::kGpio, payload_type); @@ -139,7 +141,7 @@ class Serializer { cursor += sizeof(GpioHeader); header.set(payload_type); header.set(channel_index); - header.set(data::GpioPull::kNone); + header.set(false); utility::assert_debug(cursor == dst.data() + dst.size()); return SerializeResult::kSuccess; @@ -148,9 +150,6 @@ class Serializer { SerializeResult write_gpio_digital_read_config( uint8_t channel_index, const data::GpioReadConfigView& view) noexcept { utility::assert_debug(channel_index < (1U << GpioHeader::ChannelIndex::kBitWidth)); - LIBRMCS_VERIFY_LIKELY( - view.period_ms <= ((1U << 13) - 1U), SerializeResult::kInvalidArgument); - const std::size_t required = required_gpio_size(FieldId::kGpio, GpioHeader::PayloadEnum::kDigitalRead); LIBRMCS_VERIFY_LIKELY(required, SerializeResult::kInvalidArgument); @@ -166,13 +165,14 @@ class Serializer { cursor += sizeof(GpioHeader); header.set(GpioHeader::PayloadEnum::kDigitalRead); header.set(channel_index); - header.set(view.pull); + header.set(view.capture_timestamp); auto payload = GpioReadConfigPayload::Ref(cursor); cursor += sizeof(GpioReadConfigPayload); payload.set(view.asap); payload.set(view.rising_edge); payload.set(view.falling_edge); + payload.set(view.pull); payload.set(view.period_ms); utility::assert_debug(cursor == dst.data() + dst.size()); @@ -197,7 +197,7 @@ class Serializer { cursor += sizeof(GpioHeader); header.set(GpioHeader::PayloadEnum::kAnalogWrite); header.set(channel_index); - header.set(data::GpioPull::kNone); + header.set(false); auto payload = GpioAnalogPayload::Ref(cursor); cursor += sizeof(GpioAnalogPayload); @@ -210,10 +210,9 @@ class Serializer { SerializeResult write_gpio_analog_read_config( uint8_t channel_index, const data::GpioReadConfigView& view) noexcept { utility::assert_debug(channel_index < (1U << GpioHeader::ChannelIndex::kBitWidth)); - LIBRMCS_VERIFY_LIKELY( - view.period_ms <= ((1U << 13) - 1U), SerializeResult::kInvalidArgument); LIBRMCS_VERIFY_LIKELY( !view.falling_edge && !view.rising_edge, SerializeResult::kInvalidArgument); + LIBRMCS_VERIFY_LIKELY(!view.capture_timestamp, SerializeResult::kInvalidArgument); const std::size_t required = required_gpio_size(FieldId::kGpio, GpioHeader::PayloadEnum::kAnalogRead); @@ -230,13 +229,14 @@ class Serializer { cursor += sizeof(GpioHeader); header.set(GpioHeader::PayloadEnum::kAnalogRead); header.set(channel_index); - header.set(view.pull); + header.set(false); auto payload = GpioReadConfigPayload::Ref(cursor); cursor += sizeof(GpioReadConfigPayload); payload.set(view.asap); payload.set(false); payload.set(false); + payload.set(view.pull); payload.set(view.period_ms); utility::assert_debug(cursor == dst.data() + dst.size()); @@ -248,7 +248,8 @@ class Serializer { utility::assert_debug(channel_index < (1U << GpioHeader::ChannelIndex::kBitWidth)); const auto payload_type = view.high ? GpioHeader::PayloadEnum::kDigitalReadResultHigh : GpioHeader::PayloadEnum::kDigitalReadResultLow; - const std::size_t required = required_gpio_size(FieldId::kGpio, payload_type); + const std::size_t required = + required_gpio_size(FieldId::kGpio, payload_type, view.timestamp_quarter_us.has_value()); LIBRMCS_VERIFY_LIKELY(required, SerializeResult::kInvalidArgument); auto dst = buffer_.allocate(required); @@ -262,7 +263,14 @@ class Serializer { cursor += sizeof(GpioHeader); header.set(payload_type); header.set(channel_index); - header.set(data::GpioPull::kNone); + header.set(view.timestamp_quarter_us.has_value()); + + if (view.timestamp_quarter_us) { + auto payload = GpioDigitalReadTimestampPayload::Ref(cursor); + cursor += sizeof(GpioDigitalReadTimestampPayload); + payload.set( + *view.timestamp_quarter_us); + } utility::assert_debug(cursor == dst.data() + dst.size()); return SerializeResult::kSuccess; @@ -286,7 +294,7 @@ class Serializer { cursor += sizeof(GpioHeader); header.set(GpioHeader::PayloadEnum::kAnalogReadResult); header.set(channel_index); - header.set(data::GpioPull::kNone); + header.set(false); auto payload = GpioAnalogPayload::Ref(cursor); cursor += sizeof(GpioAnalogPayload); @@ -316,6 +324,7 @@ class Serializer { payload.set(view.x); payload.set(view.y); payload.set(view.z); + payload.set(view.timestamp_quarter_us); utility::assert_debug(cursor == dst.data() + dst.size()); return SerializeResult::kSuccess; @@ -341,6 +350,31 @@ class Serializer { payload.set(view.x); payload.set(view.y); payload.set(view.z); + payload.set(view.timestamp_quarter_us); + + utility::assert_debug(cursor == dst.data() + dst.size()); + return SerializeResult::kSuccess; + } + + SerializeResult write_imu_temperature(const data::TemperatureDataView& view) noexcept { + const std::size_t required = required_imu_size(FieldId::kImu, ImuPayload::kTemperature); + LIBRMCS_VERIFY_LIKELY(required, SerializeResult::kInvalidArgument); + + auto dst = buffer_.allocate(required); + LIBRMCS_VERIFY_LIKELY(!dst.empty(), SerializeResult::kBadAlloc); + utility::assert_debug(dst.size() == required); + std::byte* cursor = dst.data(); + + write_field_header(cursor, FieldId::kImu); + + auto header = ImuHeader::Ref(cursor); + cursor += sizeof(ImuHeader); + header.set(ImuHeader::PayloadEnum::kTemperature); + + auto payload = ImuTemperaturePayload::Ref(cursor); + cursor += sizeof(ImuTemperaturePayload); + payload.set(view.temperature); + payload.set(view.timestamp_quarter_us); utility::assert_debug(cursor == dst.data() + dst.size()); return SerializeResult::kSuccess; @@ -407,16 +441,18 @@ class Serializer { return total; } - static std::size_t - required_gpio_size(FieldId field_id, GpioHeader::PayloadEnum payload) noexcept { + static std::size_t required_gpio_size( + FieldId field_id, GpioHeader::PayloadEnum payload, bool timestamped = false) noexcept { const std::size_t field_header_bytes = required_field_header_size(field_id); const std::size_t gpio_header_bytes = sizeof(GpioHeader); std::size_t payload_bytes = 0; switch (payload) { case GpioHeader::PayloadEnum::kDigitalWriteLow: - case GpioHeader::PayloadEnum::kDigitalWriteHigh: + case GpioHeader::PayloadEnum::kDigitalWriteHigh: payload_bytes = 0; break; case GpioHeader::PayloadEnum::kDigitalReadResultLow: - case GpioHeader::PayloadEnum::kDigitalReadResultHigh: payload_bytes = 0; break; + case GpioHeader::PayloadEnum::kDigitalReadResultHigh: + payload_bytes = timestamped ? sizeof(GpioDigitalReadTimestampPayload) : 0; + break; case GpioHeader::PayloadEnum::kDigitalRead: case GpioHeader::PayloadEnum::kAnalogRead: payload_bytes = sizeof(GpioReadConfigPayload); @@ -434,14 +470,18 @@ class Serializer { return total; } - enum class ImuPayload : std::uint8_t { kAccelerometer = 0, kGyroscope = 1 }; + enum class ImuPayload : std::uint8_t { kAccelerometer = 0, kGyroscope = 1, kTemperature = 2 }; static std::size_t required_imu_size(FieldId field_id, ImuPayload payload) noexcept { const std::size_t field_header_bytes = required_field_header_size(field_id); const std::size_t imu_header_bytes = sizeof(ImuHeader); - const std::size_t payload_bytes = (payload == ImuPayload::kAccelerometer) - ? sizeof(ImuAccelerometerPayload) - : sizeof(ImuGyroscopePayload); + std::size_t payload_bytes = 0; + switch (payload) { + case ImuPayload::kAccelerometer: payload_bytes = sizeof(ImuAccelerometerPayload); break; + case ImuPayload::kGyroscope: payload_bytes = sizeof(ImuGyroscopePayload); break; + case ImuPayload::kTemperature: payload_bytes = sizeof(ImuTemperaturePayload); break; + default: return 0; + } const std::size_t total = (field_header_bytes + imu_header_bytes - 1) + payload_bytes; utility::assert_debug(total <= kProtocolBufferSize); diff --git a/firmware/c_board/app/src/app.cpp b/firmware/c_board/app/src/app.cpp index 9fcd87a..3703a5b 100644 --- a/firmware/c_board/app/src/app.cpp +++ b/firmware/c_board/app/src/app.cpp @@ -15,6 +15,7 @@ #include "firmware/c_board/app/src/led/led.hpp" #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/spi/bmi088/service.hpp" #include "firmware/c_board/app/src/spi/spi.hpp" #include "firmware/c_board/app/src/timer/timer.hpp" #include "firmware/c_board/app/src/uart/uart.hpp" @@ -79,6 +80,7 @@ App::App() { can::can2->try_transmit(); usb::vendor->try_transmit(); spi::spi1->update(); + spi::bmi088::service_pending_reads(); usb::vendor->try_transmit(); uart::uart1->try_transmit(); usb::vendor->try_transmit(); 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/gpio/gpio.hpp b/firmware/c_board/app/src/gpio/gpio.hpp index da4f425..96cd189 100644 --- a/firmware/c_board/app/src/gpio/gpio.hpp +++ b/firmware/c_board/app/src/gpio/gpio.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -58,8 +59,13 @@ class Gpio : private core::utility::Immovable { void handle_digital_read(uint8_t channel_index, const data::GpioReadConfigView& data) { configure_digital_input_mode(channel_index, data); - if (data.asap) - publish_digital_input_sample(channel_index); + if (data.asap) { + const auto& hardware = channel_hardware(channel_index); + const bool high = + HAL_GPIO_ReadPin(hardware.gpio_port, hardware.gpio_pin) == GPIO_PIN_SET; + const uint32_t timestamp_quarter_us = current_timestamp_quarter_us(); + publish_digital_input_sample(channel_index, high, timestamp_quarter_us); + } } void poll_periodic_input_samples() { @@ -72,7 +78,12 @@ class Gpio : private core::utility::Immovable { if (!timer::timer->check_reached(state.next_sample_deadline)) continue; - publish_digital_input_sample(static_cast(channel_index)); + const auto& hardware = channel_hardware(static_cast(channel_index)); + const bool high = + HAL_GPIO_ReadPin(hardware.gpio_port, hardware.gpio_pin) == GPIO_PIN_SET; + const uint32_t timestamp_quarter_us = current_timestamp_quarter_us(); + publish_digital_input_sample( + static_cast(channel_index), high, timestamp_quarter_us); state.next_sample_deadline = now + state.sample_period; } } @@ -86,7 +97,11 @@ class Gpio : private core::utility::Immovable { if (state.mode != GpioMode::kDigitalInput || (!state.rising_edge && !state.falling_edge)) return; - publish_digital_input_sample(channel_index); + const uint32_t timestamp_quarter_us = current_timestamp_quarter_us(); + const auto& hardware = channel_hardware(channel_index); + const bool high = HAL_GPIO_ReadPin(hardware.gpio_port, hardware.gpio_pin) == GPIO_PIN_SET; + + publish_digital_input_sample(channel_index, high, timestamp_quarter_us); } private: @@ -96,6 +111,7 @@ class Gpio : private core::utility::Immovable { GpioMode mode = GpioMode::kOutput; bool rising_edge = false; bool falling_edge = false; + bool capture_timestamp = false; data::GpioPull pull = data::GpioPull::kNone; timer::Timer::Duration sample_period = timer::Timer::Duration::zero(); timer::Timer::TimePoint next_sample_deadline; @@ -135,11 +151,13 @@ class Gpio : private core::utility::Immovable { if (state.mode != GpioMode::kDigitalInput // || state.rising_edge != rising_edge || state.falling_edge != falling_edge - || state.pull != pull || state.sample_period != sample_period) { + || state.capture_timestamp != data.capture_timestamp || state.pull != pull + || state.sample_period != sample_period) { state.mode = GpioMode::kDigitalInput; state.rising_edge = rising_edge; state.falling_edge = falling_edge; + state.capture_timestamp = data.capture_timestamp; state.pull = pull; state.sample_period = sample_period; state.next_sample_deadline = timer::timer->timepoint(); @@ -185,16 +203,23 @@ class Gpio : private core::utility::Immovable { HAL_GPIO_Init(hardware.gpio_port, &gpio_init); } - void publish_digital_input_sample(uint8_t channel_index) { - const auto& hardware = channel_hardware(channel_index); - const bool high = HAL_GPIO_ReadPin(hardware.gpio_port, hardware.gpio_pin) == GPIO_PIN_SET; + void publish_digital_input_sample( + uint8_t channel_index, bool high, uint32_t timestamp_quarter_us) { + const auto& state = channel_state(channel_index); + const std::optional timestamp_to_publish = + state.capture_timestamp ? std::optional{timestamp_quarter_us} : std::nullopt; auto& serializer = usb::get_serializer(); core::utility::assert_debug( - serializer.write_gpio_digital_read_result(channel_index, {.high = high}) + serializer.write_gpio_digital_read_result( + channel_index, {.high = high, .timestamp_quarter_us = timestamp_to_publish}) != core::protocol::Serializer::SerializeResult::kInvalidArgument); } + [[nodiscard]] static uint32_t current_timestamp_quarter_us() { + return timer::timer->timepoint().time_since_epoch().count(); + } + static uint8_t exti_line_from_pin(uint16_t gpio_pin) { core::utility::assert_debug(gpio_pin != 0); diff --git a/firmware/c_board/app/src/spi/bmi088/accel.hpp b/firmware/c_board/app/src/spi/bmi088/accel.hpp index e7fed92..88fb541 100644 --- a/firmware/c_board/app/src/spi/bmi088/accel.hpp +++ b/firmware/c_board/app/src/spi/bmi088/accel.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include // IWYU pragma: keep (https://github.com/llvm/llvm-project/issues/68213) #include #include @@ -12,6 +13,7 @@ #include "firmware/c_board/app/src/spi/spi.hpp" #include "firmware/c_board/app/src/timer/timer.hpp" #include "firmware/c_board/app/src/usb/vendor.hpp" +#include "firmware/c_board/app/src/utility/interrupt_lock.hpp" #include "firmware/c_board/app/src/utility/lazy.hpp" namespace librmcs::firmware::spi::bmi088 { @@ -105,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) { + const utility::InterruptLockGuard guard; + pending_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us; + has_pending_capture_timestamp_ = true; + } + + bool service_pending_read() { + const utility::InterruptLockGuard guard; + if (!has_pending_capture_timestamp_) + return false; + if (!read_async(RegisterAddress::kAccXLsb, 6)) + return false; + + active_capture_timestamp_quarter_us_.store( + pending_capture_timestamp_quarter_us_, std::memory_order_relaxed); + has_active_capture_timestamp_.store(true, std::memory_order_release); + has_pending_capture_timestamp_ = false; + return true; + } private: void transmit_receive_async_callback(size_t size) override { - if (size) [[likely]] { + uint32_t active_capture_timestamp_quarter_us = 0; + const bool has_active_capture_timestamp = + has_active_capture_timestamp_.exchange(false, std::memory_order_acquire); + if (has_active_capture_timestamp) [[likely]] { + active_capture_timestamp_quarter_us = + active_capture_timestamp_quarter_us_.load(std::memory_order_relaxed); + } + + core::utility::assert_debug(!size || has_active_capture_timestamp); + if (size && has_active_capture_timestamp) [[likely]] { auto& data = parse_rx_data(spi_.rx_buffer, size); - handle_uplink(usb::vendor->serializer(), data); + handle_uplink(usb::vendor->serializer(), data, active_capture_timestamp_quarter_us); } spi_.unlock(); } - static void handle_uplink(core::protocol::Serializer& serializer, Data& data) { + static void handle_uplink( + core::protocol::Serializer& serializer, Data& data, uint32_t capture_timestamp_quarter_us) { + const auto result = serializer.write_imu_accelerometer({ + .x = data.x, + .y = data.y, + .z = data.z, + .timestamp_quarter_us = capture_timestamp_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); } + + uint32_t pending_capture_timestamp_quarter_us_ = 0; + std::atomic active_capture_timestamp_quarter_us_{0}; + bool has_pending_capture_timestamp_ = false; + std::atomic has_active_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..3f19a6f 100644 --- a/firmware/c_board/app/src/spi/bmi088/gyro.hpp +++ b/firmware/c_board/app/src/spi/bmi088/gyro.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include // IWYU pragma: keep (https://github.com/llvm/llvm-project/issues/68213) #include #include @@ -12,6 +13,7 @@ #include "firmware/c_board/app/src/spi/spi.hpp" #include "firmware/c_board/app/src/timer/timer.hpp" #include "firmware/c_board/app/src/usb/vendor.hpp" +#include "firmware/c_board/app/src/utility/interrupt_lock.hpp" #include "firmware/c_board/app/src/utility/lazy.hpp" namespace librmcs::firmware::spi::bmi088 { @@ -100,22 +102,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) { + const utility::InterruptLockGuard guard; + pending_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us; + has_pending_capture_timestamp_ = true; + } + + bool service_pending_read() { + const utility::InterruptLockGuard guard; + if (!has_pending_capture_timestamp_) + return false; + if (!read_async(RegisterAddress::kRateXLsb, 6)) + return false; + + active_capture_timestamp_quarter_us_.store( + pending_capture_timestamp_quarter_us_, std::memory_order_relaxed); + has_active_capture_timestamp_.store(true, std::memory_order_release); + has_pending_capture_timestamp_ = false; + return true; + } private: void transmit_receive_async_callback(size_t size) override { - if (size) [[likely]] { + uint32_t active_capture_timestamp_quarter_us = 0; + const bool has_active_capture_timestamp = + has_active_capture_timestamp_.exchange(false, std::memory_order_acquire); + if (has_active_capture_timestamp) [[likely]] { + active_capture_timestamp_quarter_us = + active_capture_timestamp_quarter_us_.load(std::memory_order_relaxed); + } + + core::utility::assert_debug(!size || has_active_capture_timestamp); + if (size && has_active_capture_timestamp) [[likely]] { auto& data = parse_rx_data(spi_.rx_buffer, size); - handle_uplink(usb::vendor->serializer(), data); + handle_uplink(usb::vendor->serializer(), data, active_capture_timestamp_quarter_us); } spi_.unlock(); } - static void handle_uplink(core::protocol::Serializer& serializer, Data& data) { + static void handle_uplink( + core::protocol::Serializer& serializer, Data& data, uint32_t capture_timestamp_quarter_us) { + const auto result = serializer.write_imu_gyroscope({ + .x = data.x, + .y = data.y, + .z = data.z, + .timestamp_quarter_us = capture_timestamp_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); } + + uint32_t pending_capture_timestamp_quarter_us_ = 0; + std::atomic active_capture_timestamp_quarter_us_{0}; + bool has_pending_capture_timestamp_ = false; + std::atomic has_active_capture_timestamp_{false}; }; inline constinit Gyroscope::Lazy gyroscope(&spi1); diff --git a/firmware/c_board/app/src/spi/bmi088/service.hpp b/firmware/c_board/app/src/spi/bmi088/service.hpp new file mode 100644 index 0000000..10eb1f2 --- /dev/null +++ b/firmware/c_board/app/src/spi/bmi088/service.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "firmware/c_board/app/src/spi/bmi088/accel.hpp" +#include "firmware/c_board/app/src/spi/bmi088/gyro.hpp" + +namespace librmcs::firmware::spi::bmi088 { + +inline void service_pending_reads() { + if (gyroscope->service_pending_read()) + return; + accelerometer->service_pending_read(); +} + +} // namespace librmcs::firmware::spi::bmi088 diff --git a/firmware/c_board/app/src/usb/vendor.hpp b/firmware/c_board/app/src/usb/vendor.hpp index fb4c0cc..e882c3c 100644 --- a/firmware/c_board/app/src/usb/vendor.hpp +++ b/firmware/c_board/app/src/usb/vendor.hpp @@ -155,6 +155,10 @@ class Vendor (void)data; } + void temperature_deserialized_callback(const data::TemperatureDataView& data) override { + (void)data; + } + void error_callback() override { core::utility::assert_failed_always(); } core::protocol::Deserializer deserializer_{*this}; diff --git a/firmware/rmcs_board/app/src/app.cpp b/firmware/rmcs_board/app/src/app.cpp index f393095..6390c7c 100644 --- a/firmware/rmcs_board/app/src/app.cpp +++ b/firmware/rmcs_board/app/src/app.cpp @@ -9,7 +9,8 @@ #include "firmware/rmcs_board/app/src/led/led.hpp" #include "firmware/rmcs_board/app/src/spi/bmi088/accel.hpp" #include "firmware/rmcs_board/app/src/spi/bmi088/gyro.hpp" -#include "firmware/rmcs_board/app/src/timer/tick.hpp" +#include "firmware/rmcs_board/app/src/spi/bmi088/temperature.hpp" +#include "firmware/rmcs_board/app/src/timer/timer.hpp" #include "firmware/rmcs_board/app/src/uart/uart.hpp" #include "firmware/rmcs_board/app/src/usb/vendor.hpp" #include "firmware/rmcs_board/app/src/utility/boot_mailbox.hpp" @@ -42,8 +43,9 @@ App::App() { spi::bmi088::accelerometer.init(); spi::bmi088::gyroscope.init(); + spi::bmi088::temperature.init(); - timer::tick.init(); + timer::timer.init(); } // Non-static to ensure instantiation diff --git a/firmware/rmcs_board/app/src/gpio/gpio.hpp b/firmware/rmcs_board/app/src/gpio/gpio.hpp index ea7cb0a..04f5b0a 100644 --- a/firmware/rmcs_board/app/src/gpio/gpio.hpp +++ b/firmware/rmcs_board/app/src/gpio/gpio.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -19,6 +20,7 @@ #include "core/src/utility/assert.hpp" #include "core/src/utility/immovable.hpp" #include "firmware/rmcs_board/app/src/gpio/analog_gpio_pin.hpp" +#include "firmware/rmcs_board/app/src/timer/timer.hpp" #include "firmware/rmcs_board/app/src/usb/helper.hpp" #include "firmware/rmcs_board/app/src/utility/lazy.hpp" @@ -46,8 +48,12 @@ class Gpio : private core::utility::Immovable { void handle_digital_read(uint8_t channel_index, const data::GpioReadConfigView& data) { configure_digital_input_mode(channel_index, data); - if (data.asap) - publish_digital_input_sample(channel_index); + if (data.asap) { + const auto& hardware = channel_hardware(channel_index); + const bool high = hardware.read_pin(); + const uint32_t timestamp_quarter_us = timer::Timer::timestamp_quarter_us(); + publish_digital_input_sample(channel_index, high, timestamp_quarter_us); + } } void poll_periodic_input_samples() { @@ -61,12 +67,18 @@ class Gpio : private core::utility::Immovable { if (now < state.next_sample_tick) continue; - publish_digital_input_sample(static_cast(channel_index)); + const auto& hardware = channel_hardware(static_cast(channel_index)); + const bool high = hardware.read_pin(); + const uint32_t timestamp_quarter_us = timer::Timer::timestamp_quarter_us(); + publish_digital_input_sample( + static_cast(channel_index), high, timestamp_quarter_us); state.next_sample_tick = now + state.period_ticks; } } void handle_port_interrupt(uint32_t port_index) { + const uint32_t timestamp_quarter_us = timer::Timer::timestamp_quarter_us(); + for (std::size_t channel_index = 0; channel_index < board::spec::kGpioDescriptors.size(); ++channel_index) { const auto& hardware = channel_hardware(static_cast(channel_index)); @@ -80,7 +92,9 @@ class Gpio : private core::utility::Immovable { || (!state.rising_edge && !state.falling_edge)) continue; - publish_digital_input_sample(static_cast(channel_index)); + const bool high = hardware.read_pin(); + publish_digital_input_sample( + static_cast(channel_index), high, timestamp_quarter_us); } } @@ -96,6 +110,7 @@ class Gpio : private core::utility::Immovable { ChannelMode mode = ChannelMode::kUnconfigured; bool rising_edge = false; bool falling_edge = false; + bool capture_timestamp = false; data::GpioPull pull = data::GpioPull::kNone; uint64_t period_ticks = 0; uint64_t next_sample_tick = 0; @@ -144,6 +159,7 @@ class Gpio : private core::utility::Immovable { state.mode = mode; state.rising_edge = false; state.falling_edge = false; + state.capture_timestamp = false; state.pull = data::GpioPull::kNone; state.period_ticks = 0; state.next_sample_tick = 0; @@ -166,6 +182,7 @@ class Gpio : private core::utility::Immovable { state.mode = ChannelMode::kDigitalInput; state.rising_edge = data.rising_edge; state.falling_edge = data.falling_edge; + state.capture_timestamp = data.capture_timestamp; state.pull = data.pull; state.period_ticks = period_ticks; state.next_sample_tick = now_ticks(); @@ -235,6 +252,7 @@ class Gpio : private core::utility::Immovable { const ChannelState& state, const data::GpioReadConfigView& data, uint64_t period_ticks) { return state.mode != ChannelMode::kDigitalInput || state.rising_edge != data.rising_edge || state.falling_edge != data.falling_edge || state.pull != data.pull + || state.capture_timestamp != data.capture_timestamp || state.period_ticks != period_ticks; } @@ -284,13 +302,16 @@ class Gpio : private core::utility::Immovable { } } - static void publish_digital_input_sample(uint8_t channel_index) { - const auto& hardware = channel_hardware(channel_index); - const bool high = hardware.read_pin(); + void publish_digital_input_sample( + uint8_t channel_index, bool high, uint32_t timestamp_quarter_us) { + const auto& state = channel_state(channel_index); + const std::optional timestamp_to_publish = + state.capture_timestamp ? std::optional{timestamp_quarter_us} : std::nullopt; auto& serializer = usb::get_serializer(); core::utility::assert_debug( - serializer.write_gpio_digital_read_result(channel_index, {.high = high}) + serializer.write_gpio_digital_read_result( + channel_index, {.high = high, .timestamp_quarter_us = timestamp_to_publish}) != core::protocol::Serializer::SerializeResult::kInvalidArgument); } diff --git a/firmware/rmcs_board/app/src/spi/bmi088/accel.cpp b/firmware/rmcs_board/app/src/spi/bmi088/accel.cpp index 500483a..2245173 100644 --- a/firmware/rmcs_board/app/src/spi/bmi088/accel.cpp +++ b/firmware/rmcs_board/app/src/spi/bmi088/accel.cpp @@ -1,7 +1,13 @@ #include "firmware/rmcs_board/app/src/spi/bmi088/accel.hpp" +#include "firmware/rmcs_board/app/src/spi/bmi088/service.hpp" +#include "firmware/rmcs_board/app/src/timer/timer.hpp" + namespace librmcs::firmware::board { -void bmi088_accel_dataready_irq_handler() { spi::bmi088::accelerometer->data_ready_callback(); } +void bmi088_accel_dataready_irq_handler() { + spi::bmi088::accelerometer->data_ready_callback(timer::Timer::timestamp_quarter_us()); + spi::bmi088::service_pending_reads(); +} } // namespace librmcs::firmware::board diff --git a/firmware/rmcs_board/app/src/spi/bmi088/accel.hpp b/firmware/rmcs_board/app/src/spi/bmi088/accel.hpp index e2967d2..22f4bf1 100644 --- a/firmware/rmcs_board/app/src/spi/bmi088/accel.hpp +++ b/firmware/rmcs_board/app/src/spi/bmi088/accel.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -12,6 +13,7 @@ #include "firmware/rmcs_board/app/src/spi/bmi088/base.hpp" #include "firmware/rmcs_board/app/src/spi/spi.hpp" #include "firmware/rmcs_board/app/src/usb/vendor.hpp" +#include "firmware/rmcs_board/app/src/utility/interrupt_lock.hpp" #include "firmware/rmcs_board/app/src/utility/lazy.hpp" namespace librmcs::firmware::spi::bmi088 { @@ -104,22 +106,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) { + const utility::InterruptLockGuard guard; + pending_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us; + has_pending_capture_timestamp_ = true; + } + + bool service_pending_read() { + const utility::InterruptLockGuard guard; + if (!has_pending_capture_timestamp_) + return false; + if (!read_async(RegisterAddress::kAccXLsb, 6)) + return false; + + active_capture_timestamp_quarter_us_.store( + pending_capture_timestamp_quarter_us_, std::memory_order_relaxed); + has_active_capture_timestamp_.store(true, std::memory_order_release); + has_pending_capture_timestamp_ = false; + return true; + } private: void transmit_receive_async_callback(std::size_t size) override { - if (size) [[likely]] { + uint32_t active_capture_timestamp_quarter_us = 0; + const bool has_active_capture_timestamp = + has_active_capture_timestamp_.exchange(false, std::memory_order_acquire); + if (has_active_capture_timestamp) [[likely]] { + active_capture_timestamp_quarter_us = + active_capture_timestamp_quarter_us_.load(std::memory_order_relaxed); + } + + core::utility::assert_debug(!size || has_active_capture_timestamp); + if (size && has_active_capture_timestamp) [[likely]] { auto& data = parse_rx_data(spi_.rx_buffer, size); - handle_uplink(usb::vendor->serializer(), data); + handle_uplink(usb::vendor->serializer(), data, active_capture_timestamp_quarter_us); } spi_.unlock(); } - static void handle_uplink(core::protocol::Serializer& serializer, Data& data) { + static void handle_uplink( + core::protocol::Serializer& serializer, Data& data, uint32_t capture_timestamp_quarter_us) { + const auto result = serializer.write_imu_accelerometer({ + .x = data.x, + .y = data.y, + .z = data.z, + .timestamp_quarter_us = capture_timestamp_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); } + + uint32_t pending_capture_timestamp_quarter_us_ = 0; + std::atomic active_capture_timestamp_quarter_us_{0}; + bool has_pending_capture_timestamp_ = false; + std::atomic has_active_capture_timestamp_{false}; }; inline Accelerometer::Lazy accelerometer(&spi::spi_bmi088, board::kBmi088AccelChipSelectPin); diff --git a/firmware/rmcs_board/app/src/spi/bmi088/gyro.cpp b/firmware/rmcs_board/app/src/spi/bmi088/gyro.cpp index 5b5b5f9..8d3c796 100644 --- a/firmware/rmcs_board/app/src/spi/bmi088/gyro.cpp +++ b/firmware/rmcs_board/app/src/spi/bmi088/gyro.cpp @@ -1,7 +1,13 @@ #include "firmware/rmcs_board/app/src/spi/bmi088/gyro.hpp" +#include "firmware/rmcs_board/app/src/spi/bmi088/service.hpp" +#include "firmware/rmcs_board/app/src/timer/timer.hpp" + namespace librmcs::firmware::board { -void bmi088_gyro_dataready_irq_handler() { spi::bmi088::gyroscope->data_ready_callback(); } +void bmi088_gyro_dataready_irq_handler() { + spi::bmi088::gyroscope->data_ready_callback(timer::Timer::timestamp_quarter_us()); + spi::bmi088::service_pending_reads(); +} } // namespace librmcs::firmware::board diff --git a/firmware/rmcs_board/app/src/spi/bmi088/gyro.hpp b/firmware/rmcs_board/app/src/spi/bmi088/gyro.hpp index 37bf7ed..d929430 100644 --- a/firmware/rmcs_board/app/src/spi/bmi088/gyro.hpp +++ b/firmware/rmcs_board/app/src/spi/bmi088/gyro.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -12,6 +13,7 @@ #include "firmware/rmcs_board/app/src/spi/bmi088/base.hpp" #include "firmware/rmcs_board/app/src/spi/spi.hpp" #include "firmware/rmcs_board/app/src/usb/vendor.hpp" +#include "firmware/rmcs_board/app/src/utility/interrupt_lock.hpp" #include "firmware/rmcs_board/app/src/utility/lazy.hpp" namespace librmcs::firmware::spi::bmi088 { @@ -98,22 +100,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) { + const utility::InterruptLockGuard guard; + pending_capture_timestamp_quarter_us_ = capture_timestamp_quarter_us; + has_pending_capture_timestamp_ = true; + } + + bool service_pending_read() { + const utility::InterruptLockGuard guard; + if (!has_pending_capture_timestamp_) + return false; + if (!read_async(RegisterAddress::kRateXLsb, 6)) + return false; + + active_capture_timestamp_quarter_us_.store( + pending_capture_timestamp_quarter_us_, std::memory_order_relaxed); + has_active_capture_timestamp_.store(true, std::memory_order_release); + has_pending_capture_timestamp_ = false; + return true; + } private: void transmit_receive_async_callback(std::size_t size) override { - if (size) [[likely]] { + uint32_t active_capture_timestamp_quarter_us = 0; + const bool has_active_capture_timestamp = + has_active_capture_timestamp_.exchange(false, std::memory_order_acquire); + if (has_active_capture_timestamp) [[likely]] { + active_capture_timestamp_quarter_us = + active_capture_timestamp_quarter_us_.load(std::memory_order_relaxed); + } + + core::utility::assert_debug(!size || has_active_capture_timestamp); + if (size && has_active_capture_timestamp) [[likely]] { auto& data = parse_rx_data(spi_.rx_buffer, size); - handle_uplink(usb::vendor->serializer(), data); + handle_uplink(usb::vendor->serializer(), data, active_capture_timestamp_quarter_us); } spi_.unlock(); } - static void handle_uplink(core::protocol::Serializer& serializer, Data& data) { + static void handle_uplink( + core::protocol::Serializer& serializer, Data& data, uint32_t capture_timestamp_quarter_us) { + const auto result = serializer.write_imu_gyroscope({ + .x = data.x, + .y = data.y, + .z = data.z, + .timestamp_quarter_us = capture_timestamp_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); } + + uint32_t pending_capture_timestamp_quarter_us_ = 0; + std::atomic active_capture_timestamp_quarter_us_{0}; + bool has_pending_capture_timestamp_ = false; + std::atomic has_active_capture_timestamp_{false}; }; inline Gyroscope::Lazy gyroscope(&spi::spi_bmi088, board::kBmi088GyroChipSelectPin); diff --git a/firmware/rmcs_board/app/src/spi/bmi088/service.hpp b/firmware/rmcs_board/app/src/spi/bmi088/service.hpp new file mode 100644 index 0000000..9a8156e --- /dev/null +++ b/firmware/rmcs_board/app/src/spi/bmi088/service.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include "firmware/rmcs_board/app/src/spi/bmi088/accel.hpp" +#include "firmware/rmcs_board/app/src/spi/bmi088/gyro.hpp" +#include "firmware/rmcs_board/app/src/spi/bmi088/temperature.hpp" + +namespace librmcs::firmware::spi::bmi088 { + +inline void service_pending_reads() { + if (gyroscope->service_pending_read()) + return; + if (accelerometer->service_pending_read()) + return; + temperature->service_pending_read(); +} + +} // namespace librmcs::firmware::spi::bmi088 diff --git a/firmware/rmcs_board/app/src/spi/bmi088/temperature.cpp b/firmware/rmcs_board/app/src/spi/bmi088/temperature.cpp new file mode 100644 index 0000000..9e1f510 --- /dev/null +++ b/firmware/rmcs_board/app/src/spi/bmi088/temperature.cpp @@ -0,0 +1,32 @@ +#include "firmware/rmcs_board/app/src/spi/bmi088/temperature.hpp" + +#include + +#include +#include +#include + +#include "firmware/rmcs_board/app/src/spi/bmi088/service.hpp" + +namespace librmcs::firmware::board { + +namespace { + +auto* const kTemperatureTimer = HPM_GPTMR1; +constexpr uint8_t kTemperatureTimerChannel = 0U; + +} // namespace + +SDK_DECLARE_EXT_ISR_M(IRQn_GPTMR1, bmi088_temperature_timer_isr) +void bmi088_temperature_timer_isr() { + if (!gptmr_check_status(kTemperatureTimer, GPTMR_CH_RLD_STAT_MASK(kTemperatureTimerChannel))) { + return; + } + + gptmr_clear_status(kTemperatureTimer, GPTMR_CH_RLD_STAT_MASK(kTemperatureTimerChannel)); + + spi::bmi088::temperature->timer_callback(); + spi::bmi088::service_pending_reads(); +} + +} // namespace librmcs::firmware::board diff --git a/firmware/rmcs_board/app/src/spi/bmi088/temperature.hpp b/firmware/rmcs_board/app/src/spi/bmi088/temperature.hpp new file mode 100644 index 0000000..8c6b514 --- /dev/null +++ b/firmware/rmcs_board/app/src/spi/bmi088/temperature.hpp @@ -0,0 +1,176 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "board_app.hpp" +#include "core/src/protocol/serializer.hpp" +#include "core/src/utility/assert.hpp" +#include "firmware/rmcs_board/app/src/gpio/gpio_pin.hpp" +#include "firmware/rmcs_board/app/src/spi/bmi088/accel.hpp" +#include "firmware/rmcs_board/app/src/spi/bmi088/base.hpp" +#include "firmware/rmcs_board/app/src/spi/spi.hpp" +#include "firmware/rmcs_board/app/src/timer/timer.hpp" +#include "firmware/rmcs_board/app/src/usb/vendor.hpp" +#include "firmware/rmcs_board/app/src/utility/interrupt_lock.hpp" +#include "firmware/rmcs_board/app/src/utility/lazy.hpp" + +namespace librmcs::firmware::spi::bmi088 { + +class Temperature final + : public AccelerometerTraits + , private Bmi088Base { +public: + using Lazy = utility::Lazy; + + static constexpr uint32_t kProbeFrequencyHz = 977U; + static constexpr uint32_t kHeartbeatPeriodQuarterUs = 1'300U * 1'000U * 4U; + + explicit Temperature(Spi::Lazy* spi, const GpioPin& chip_select) + : Bmi088Base(spi, chip_select) { + gptmr_channel_config_t config; + gptmr_channel_get_default_config(kTimer, &config); + + clock_add_to_group(kTimerClockName, 0); + const uint32_t gptmr_frequency = clock_get_frequency(kTimerClockName); + core::utility::assert_always(gptmr_frequency != 0); + + config.reload = (gptmr_frequency + (kProbeFrequencyHz / 2U)) / kProbeFrequencyHz; + core::utility::assert_always(config.reload != 0); + + gptmr_channel_config(kTimer, kTimerChannel, &config, false); + gptmr_enable_irq(kTimer, GPTMR_CH_RLD_IRQ_MASK(kTimerChannel)); + intc_m_enable_irq_with_priority(kTimerIrq, 1); + gptmr_start_counter(kTimer, kTimerChannel); + } + + void timer_callback() { + const utility::InterruptLockGuard guard; + probe_pending_ = true; + } + + bool service_pending_read() { + const utility::InterruptLockGuard guard; + if (!probe_pending_) + return false; + if (!read_async(RegisterAddress::kTempMsb, kTemperatureReadSizeBytes)) + return false; + + // Temperature timestamps are anchored to SPI launch time, not probing-tick arrival. + active_probe_launch_timestamp_quarter_us_.store( + timer::Timer::timestamp_quarter_us(), std::memory_order_relaxed); + has_active_probe_launch_timestamp_.store(true, std::memory_order_release); + probe_pending_ = false; + return true; + } + +private: + static inline GPTMR_Type* const kTimer = HPM_GPTMR1; + static constexpr clock_name_t kTimerClockName = clock_gptmr1; + static constexpr uint32_t kTimerIrq = IRQn_GPTMR1; + static constexpr uint8_t kTimerChannel = 0U; + static constexpr std::size_t kTemperatureReadSizeBytes = 2; + + void transmit_receive_async_callback(std::size_t size) override { + uint32_t active_probe_launch_timestamp_quarter_us = 0; + const bool has_active_probe_launch_timestamp = + has_active_probe_launch_timestamp_.exchange(false, std::memory_order_acquire); + if (has_active_probe_launch_timestamp) [[likely]] { + active_probe_launch_timestamp_quarter_us = + active_probe_launch_timestamp_quarter_us_.load(std::memory_order_relaxed); + } + + core::utility::assert_debug(!size || has_active_probe_launch_timestamp); + if (size && has_active_probe_launch_timestamp) [[likely]] { + const uint16_t raw_temperature = parse_raw_temperature(spi_.rx_buffer, size); + handle_uplink( + usb::vendor->serializer(), raw_temperature, + active_probe_launch_timestamp_quarter_us); + } + spi_.unlock(); + } + + static uint32_t midpoint_timestamp_quarter_us(uint32_t start, uint32_t end) { + return start + ((end - start) / 2U); + } + + static uint16_t parse_raw_temperature(const std::byte* rx_buffer, std::size_t size) { + core::utility::assert_debug( + size == kTemperatureReadSizeBytes + AccelerometerTraits::kDummyBytes); + + const auto msb = std::to_integer(rx_buffer[AccelerometerTraits::kDummyBytes]); + const auto lsb = std::to_integer(rx_buffer[AccelerometerTraits::kDummyBytes + 1]); + return static_cast((static_cast(msb) << 8U) | lsb); + } + + void handle_uplink( + core::protocol::Serializer& serializer, uint16_t raw_temperature, + uint32_t probe_launch_timestamp_quarter_us) { + const bool observed_value_changed = + !has_last_observation_ || raw_temperature != last_observed_temperature_; + if (observed_value_changed) { + // When the observed temperature changes, estimate the update time as the midpoint + // between the previous and current probe launches. + current_value_timestamp_quarter_us_ = has_last_observation_ + ? midpoint_timestamp_quarter_us( + last_probe_launch_timestamp_quarter_us_, + probe_launch_timestamp_quarter_us) + : probe_launch_timestamp_quarter_us; + } + + const bool value_differs_from_last_report = + !has_last_report_ || raw_temperature != last_reported_temperature_; + if (should_report(value_differs_from_last_report, probe_launch_timestamp_quarter_us)) { + const uint32_t report_timestamp_quarter_us = value_differs_from_last_report + ? current_value_timestamp_quarter_us_ + : probe_launch_timestamp_quarter_us; + const auto result = serializer.write_imu_temperature({ + .temperature = raw_temperature, + .timestamp_quarter_us = report_timestamp_quarter_us, + }); + core::utility::assert_debug( + result != core::protocol::Serializer::SerializeResult::kInvalidArgument); + if (result == core::protocol::Serializer::SerializeResult::kSuccess) { + last_reported_temperature_ = raw_temperature; + last_reported_probe_launch_timestamp_quarter_us_ = + probe_launch_timestamp_quarter_us; + has_last_report_ = true; + } + } + + last_observed_temperature_ = raw_temperature; + last_probe_launch_timestamp_quarter_us_ = probe_launch_timestamp_quarter_us; + has_last_observation_ = true; + } + + bool should_report( + bool value_differs_from_last_report, uint32_t probe_launch_timestamp_quarter_us) const { + if (value_differs_from_last_report) + return true; + return has_last_report_ + && probe_launch_timestamp_quarter_us - last_reported_probe_launch_timestamp_quarter_us_ + >= kHeartbeatPeriodQuarterUs; + } + + bool probe_pending_ = false; + std::atomic active_probe_launch_timestamp_quarter_us_{0}; + uint32_t current_value_timestamp_quarter_us_ = 0; + uint32_t last_probe_launch_timestamp_quarter_us_ = 0; + uint32_t last_reported_probe_launch_timestamp_quarter_us_ = 0; + uint16_t last_observed_temperature_ = 0; + uint16_t last_reported_temperature_ = 0; + bool has_last_observation_ = false; + std::atomic has_active_probe_launch_timestamp_{false}; + bool has_last_report_ = false; +}; + +inline Temperature::Lazy temperature(&spi::spi_bmi088, board::kBmi088AccelChipSelectPin); + +} // namespace librmcs::firmware::spi::bmi088 diff --git a/firmware/rmcs_board/app/src/spi/spi.cpp b/firmware/rmcs_board/app/src/spi/spi.cpp index 9c51b40..5cf12b3 100644 --- a/firmware/rmcs_board/app/src/spi/spi.cpp +++ b/firmware/rmcs_board/app/src/spi/spi.cpp @@ -1,9 +1,13 @@ #include "firmware/rmcs_board/app/src/spi/spi.hpp" #include "board_app.hpp" +#include "firmware/rmcs_board/app/src/spi/bmi088/service.hpp" namespace librmcs::firmware::board { -void spi_bmi088_irq_handler() { spi::spi_bmi088->irq_handler(); } +void spi_bmi088_irq_handler() { + spi::spi_bmi088->irq_handler(); + spi::bmi088::service_pending_reads(); +} } // namespace librmcs::firmware::board diff --git a/firmware/rmcs_board/app/src/timer/tick.cpp b/firmware/rmcs_board/app/src/timer/tick.cpp deleted file mode 100644 index 21c7ddc..0000000 --- a/firmware/rmcs_board/app/src/timer/tick.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "firmware/rmcs_board/app/src/timer/tick.hpp" - -#include "board_app.hpp" - -namespace librmcs::firmware::board { - -void tick_clock_irq_handler() { timer::tick->irq_handler(); } - -} // namespace librmcs::firmware::board diff --git a/firmware/rmcs_board/app/src/timer/tick.hpp b/firmware/rmcs_board/app/src/timer/tick.hpp deleted file mode 100644 index 1f8ac26..0000000 --- a/firmware/rmcs_board/app/src/timer/tick.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include "board_app.hpp" -#include "firmware/rmcs_board/app/src/led/led.hpp" -#include "firmware/rmcs_board/app/src/utility/lazy.hpp" - -namespace librmcs::firmware::timer { - -class Tick { -public: - using Lazy = utility::Lazy; - - Tick() { - const uint32_t freq = board::init_tick_clock(); - - gptmr_channel_config_t config; - gptmr_channel_get_default_config(BOARD_TICK_CLOCK, &config); - config.reload = (freq + 500) / 1000U; // 1kHz - gptmr_channel_config(BOARD_TICK_CLOCK, 0, &config, false); - - gptmr_enable_irq(BOARD_TICK_CLOCK, GPTMR_CH_RLD_IRQ_MASK(0)); - intc_m_enable_irq_with_priority(IRQn_GPTMR1, 1); - - gptmr_start_counter(BOARD_TICK_CLOCK, 0); - } - - void irq_handler() { - if (gptmr_check_status(BOARD_TICK_CLOCK, GPTMR_CH_RLD_STAT_MASK(0))) { - gptmr_clear_status(BOARD_TICK_CLOCK, GPTMR_CH_RLD_STAT_MASK(0)); - tick_counter_ = tick_counter_ + 1; - led::led->update(tick_counter_); - } - } - -private: - uint32_t tick_counter_ = 0; -}; - -inline constinit Tick::Lazy tick; - -} // namespace librmcs::firmware::timer diff --git a/firmware/rmcs_board/app/src/timer/timer.cpp b/firmware/rmcs_board/app/src/timer/timer.cpp new file mode 100644 index 0000000..1afde6c --- /dev/null +++ b/firmware/rmcs_board/app/src/timer/timer.cpp @@ -0,0 +1,6 @@ +#include "firmware/rmcs_board/app/src/timer/timer.hpp" + +#include + +SDK_DECLARE_MCHTMR_ISR(tick_clock_isr) +void tick_clock_isr() { librmcs::firmware::timer::timer->irq_handler(); } diff --git a/firmware/rmcs_board/app/src/timer/timer.hpp b/firmware/rmcs_board/app/src/timer/timer.hpp new file mode 100644 index 0000000..ba643c9 --- /dev/null +++ b/firmware/rmcs_board/app/src/timer/timer.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include + +#include +#include +#include + +#include "core/src/utility/assert.hpp" +#include "firmware/rmcs_board/app/src/led/led.hpp" +#include "firmware/rmcs_board/app/src/utility/lazy.hpp" + +namespace librmcs::firmware::timer { + +class Timer { +public: + using Lazy = utility::Lazy; + + static constexpr uint64_t kTimerFrequencyHz = 4'000'000U; + + static constexpr uint32_t kTickFrequencyHz = 1'000U; + static constexpr uint64_t kTickPeriodTicks = + (static_cast(kTimerFrequencyHz) + (kTickFrequencyHz / 2U)) / kTickFrequencyHz; + + Timer() { + core::utility::assert_always(clock_get_frequency(clock_mchtmr0) == kTimerFrequencyHz); + + // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) + next_tick_compare_value_ = HPM_MCHTMR->MTIME + kTickPeriodTicks; + mchtmr_set_compare_value(HPM_MCHTMR, next_tick_compare_value_); + + enable_mchtmr_irq(); + } + + void irq_handler() { + tick_counter_ = tick_counter_ + 1; + led::led->update(tick_counter_); + + do { + next_tick_compare_value_ += kTickPeriodTicks; + } while (next_tick_compare_value_ <= HPM_MCHTMR->MTIME); + mchtmr_set_compare_value(HPM_MCHTMR, next_tick_compare_value_); + } + + static uint32_t timestamp_quarter_us() { + return *reinterpret_cast(&HPM_MCHTMR->MTIME); + } + + static uint64_t timestamp64_quarter_us() { return HPM_MCHTMR->MTIME; } + +private: + uint64_t next_tick_compare_value_ = 0; + uint32_t tick_counter_ = 0; +}; + +inline constinit Timer::Lazy timer; + +} // namespace librmcs::firmware::timer diff --git a/firmware/rmcs_board/app/src/usb/vendor.hpp b/firmware/rmcs_board/app/src/usb/vendor.hpp index 41ec672..55aa5f3 100644 --- a/firmware/rmcs_board/app/src/usb/vendor.hpp +++ b/firmware/rmcs_board/app/src/usb/vendor.hpp @@ -167,6 +167,10 @@ class Vendor (void)data; } + void temperature_deserialized_callback(const data::TemperatureDataView& data) override { + (void)data; + } + void error_callback() override { core::utility::assert_failed_always(); } core::protocol::Deserializer deserializer_{*this}; diff --git a/firmware/rmcs_board/boards/lite/app/board_app.cpp b/firmware/rmcs_board/boards/lite/app/board_app.cpp index 95de32e..55b8997 100644 --- a/firmware/rmcs_board/boards/lite/app/board_app.cpp +++ b/firmware/rmcs_board/boards/lite/app/board_app.cpp @@ -162,11 +162,6 @@ void init_gpio_pins() { kGpioHardwareDescriptors[1].configure_pioc_function(); } -uint32_t init_tick_clock() { - clock_add_to_group(clock_gptmr1, 0); - return clock_get_frequency(clock_gptmr1); -} - void init_user_button_and_switch_pins() { kUserButtonPin.configure_controller(); kUserButtonPin.configure_ioc_function(); @@ -201,9 +196,6 @@ void gpio_bmi088_int_isr() { SDK_DECLARE_EXT_ISR_M(IRQn_GPIO0_Y, gpio_y_isr) void gpio_y_isr() { gpio_irq_handler(GPIO_DI_GPIOY); } -SDK_DECLARE_EXT_ISR_M(IRQn_GPTMR1, tick_isr) -void tick_isr() { tick_clock_irq_handler(); } - SDK_DECLARE_EXT_ISR_M(BOARD_CAN0(IRQn_MCAN, ), can0_isr) void can0_isr() { can_irq_handler(0); } diff --git a/firmware/rmcs_board/boards/lite/app/board_app.hpp b/firmware/rmcs_board/boards/lite/app/board_app.hpp index 0f9beb8..389f70a 100644 --- a/firmware/rmcs_board/boards/lite/app/board_app.hpp +++ b/firmware/rmcs_board/boards/lite/app/board_app.hpp @@ -71,9 +71,4 @@ static_assert(board::spec::kGpioDescriptors.size() == std::size(board::kGpioHard void init_gpio_pins(); void gpio_irq_handler(uint32_t port_index); -#define BOARD_TICK_CLOCK HPM_GPTMR1 - -uint32_t init_tick_clock(); -void tick_clock_irq_handler(); - } // namespace librmcs::firmware::board diff --git a/firmware/rmcs_board/boards/lite/board.c b/firmware/rmcs_board/boards/lite/board.c index 4da3365..a16f2cb 100644 --- a/firmware/rmcs_board/boards/lite/board.c +++ b/firmware/rmcs_board/boards/lite/board.c @@ -125,8 +125,8 @@ static inline void board_init_clock(void) { clock_update_core_clock(); - /* Configure mchtmr to 24MHz */ - clock_set_source_divider(clock_mchtmr0, clk_src_osc24m, 1); + /* Configure mchtmr to 4MHz */ + clock_set_source_divider(clock_mchtmr0, clk_src_osc24m, 6); } static inline void board_init_usb_dp_dm_pins(void) { diff --git a/firmware/rmcs_board/boards/pro/app/board_app.cpp b/firmware/rmcs_board/boards/pro/app/board_app.cpp index c2bc600..23bca68 100644 --- a/firmware/rmcs_board/boards/pro/app/board_app.cpp +++ b/firmware/rmcs_board/boards/pro/app/board_app.cpp @@ -237,11 +237,6 @@ void init_gpio_pins() { // Intentionally empty: No PIOC pins need to be configured. } -uint32_t init_tick_clock() { - clock_add_to_group(clock_gptmr1, 0); - return clock_get_frequency(clock_gptmr1); -} - SDK_DECLARE_EXT_ISR_M(IRQn_GPIO0_A, gpio_a_isr) void gpio_a_isr() { gpio_irq_handler(GPIO_DI_GPIOA); } @@ -261,9 +256,6 @@ void gpio_bmi088_int_accel_isr() { gpio_irq_handler(GPIO_DI_GPIOY); } -SDK_DECLARE_EXT_ISR_M(IRQn_GPTMR1, tick_isr) -void tick_isr() { tick_clock_irq_handler(); } - SDK_DECLARE_EXT_ISR_M(BOARD_CAN0(IRQn_MCAN, ), can0_isr) void can0_isr() { can_irq_handler(0); } diff --git a/firmware/rmcs_board/boards/pro/app/board_app.hpp b/firmware/rmcs_board/boards/pro/app/board_app.hpp index f404782..0b985ff 100644 --- a/firmware/rmcs_board/boards/pro/app/board_app.hpp +++ b/firmware/rmcs_board/boards/pro/app/board_app.hpp @@ -86,9 +86,4 @@ static_assert(board::spec::kGpioDescriptors.size() == std::size(board::kGpioHard void init_gpio_pins(); void gpio_irq_handler(uint32_t port_index); -#define BOARD_TICK_CLOCK HPM_GPTMR1 - -uint32_t init_tick_clock(); -void tick_clock_irq_handler(); - } // namespace librmcs::firmware::board diff --git a/firmware/rmcs_board/boards/pro/board.c b/firmware/rmcs_board/boards/pro/board.c index 8958f3d..1b63170 100644 --- a/firmware/rmcs_board/boards/pro/board.c +++ b/firmware/rmcs_board/boards/pro/board.c @@ -121,8 +121,8 @@ static inline void board_init_clock(void) { clock_update_core_clock(); - /* Configure mchtmr to 24MHz */ - clock_set_source_divider(clock_mchtmr0, clk_src_osc24m, 1); + /* Configure mchtmr to 4MHz */ + clock_set_source_divider(clock_mchtmr0, clk_src_osc24m, 6); } static inline void board_init_usb_dp_dm_pins(void) { diff --git a/host/include/librmcs/agent/c_board.hpp b/host/include/librmcs/agent/c_board.hpp index a3b00cd..0611622 100644 --- a/host/include/librmcs/agent/c_board.hpp +++ b/host/include/librmcs/agent/c_board.hpp @@ -140,6 +140,9 @@ class CBoard : private data::DataCallback { void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { (void)data; } + void temperature_receive_callback(const librmcs::data::TemperatureDataView& data) override { + (void)data; + } host::protocol::Handler handler_; }; diff --git a/host/include/librmcs/agent/rmcs_board_lite.hpp b/host/include/librmcs/agent/rmcs_board_lite.hpp index a6dcccb..8b1ad20 100644 --- a/host/include/librmcs/agent/rmcs_board_lite.hpp +++ b/host/include/librmcs/agent/rmcs_board_lite.hpp @@ -156,6 +156,9 @@ class RmcsBoardLite : private data::DataCallback { void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { (void)data; } + void temperature_receive_callback(const librmcs::data::TemperatureDataView& data) override { + (void)data; + } host::protocol::Handler handler_; }; diff --git a/host/include/librmcs/agent/rmcs_board_pro.hpp b/host/include/librmcs/agent/rmcs_board_pro.hpp index 2059fee..2920af9 100644 --- a/host/include/librmcs/agent/rmcs_board_pro.hpp +++ b/host/include/librmcs/agent/rmcs_board_pro.hpp @@ -170,6 +170,9 @@ class RmcsBoardPro : private data::DataCallback { void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { (void)data; } + void temperature_receive_callback(const librmcs::data::TemperatureDataView& data) override { + (void)data; + } host::protocol::Handler handler_; }; diff --git a/host/include/librmcs/protocol/handler.hpp b/host/include/librmcs/protocol/handler.hpp index 12aafe3..bbd8c78 100644 --- a/host/include/librmcs/protocol/handler.hpp +++ b/host/include/librmcs/protocol/handler.hpp @@ -33,10 +33,6 @@ class LIBRMCS_API Handler { bool write_gpio_analog_data( uint8_t channel_index, const data::GpioAnalogDataView& view) noexcept; - bool write_imu_accelerometer(const data::AccelerometerDataView& view) noexcept; - - bool write_imu_gyroscope(const data::GyroscopeDataView& view) noexcept; - private: friend class Handler; diff --git a/host/src/protocol/handler.cpp b/host/src/protocol/handler.cpp index 5e739b9..3af7a65 100644 --- a/host/src/protocol/handler.cpp +++ b/host/src/protocol/handler.cpp @@ -80,6 +80,10 @@ class Handler::Impl : public core::protocol::DeserializeCallback { callback_.gyroscope_receive_callback(data); } + void temperature_deserialized_callback(const data::TemperatureDataView& data) override { + callback_.temperature_receive_callback(data); + } + void error_callback() override { logging::get_logger().error("Deserializer encountered an error while parsing input"); } @@ -201,16 +205,6 @@ bool Handler::PacketBuilder::write_gpio_analog_data( ->write_gpio_analog_data(channel_index, view); } -bool Handler::PacketBuilder::write_imu_accelerometer( - const data::AccelerometerDataView& view) noexcept { - return std::launder(reinterpret_cast(storage_)) - ->write_imu_accelerometer(view); -} - -bool Handler::PacketBuilder::write_imu_gyroscope(const data::GyroscopeDataView& view) noexcept { - return std::launder(reinterpret_cast(storage_))->write_imu_gyroscope(view); -} - Handler::Handler( uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, const agent::AdvancedOptions& options, data::DataCallback& callback)