diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml index 1f453480..5ef17bf2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little-six-friction.yaml @@ -8,7 +8,6 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::Hero -> referee_ui_hero - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - rmcs_core::referee::Command -> referee_command - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller @@ -38,6 +37,7 @@ rmcs_executor: - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs::AutoAimComponent -> auto_aim_component + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 1183c9b1..8efa589b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -31,7 +31,9 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" #include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -58,6 +60,8 @@ class DeformableInfantryOmniB tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + remote_control_ = std::make_unique(*this); + bottom_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); top_board_ = std::make_unique( @@ -79,6 +83,7 @@ class DeformableInfantryOmniB void update() override { bottom_board_->update(); top_board_->update(); + remote_control_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -121,7 +126,8 @@ class DeformableInfantryOmniB explicit TopBoard( DeformableInfantryOmniB& status, Component& command, const std::string& serial_filter = {}) - : tf_{status.tf_} + : status_{status} + , tf_{status.tf_} , bmi088_{device::Bmi088Ekf::Config{ .body_to_sensor = Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} @@ -163,8 +169,11 @@ class DeformableInfantryOmniB .capture_timestamp = true, .pull = librmcs::data::GpioPull::kUp, }); - } + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + status_.remote_control_->register_vt13(&vt13_); + } ~TopBoard() override = default; [[nodiscard]] auto gimbal_yaw_velocity() const -> double { @@ -177,6 +186,7 @@ class DeformableInfantryOmniB } void update() { + vt13_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); @@ -243,6 +253,11 @@ class DeformableInfantryOmniB } } + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) + vt13_.store_status(data.uart_data); + } + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); @@ -277,6 +292,7 @@ class DeformableInfantryOmniB auto status() const -> std::vector { return monitor_.text(); } + DeformableInfantryOmniB& status_; OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; OutputInterface gimbal_pitch_velocity_bmi088_; @@ -286,6 +302,7 @@ class DeformableInfantryOmniB device::Bmi088Ekf bmi088_; device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; @@ -371,8 +388,9 @@ class DeformableInfantryOmniB auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); - } + status_.remote_control_->register_dr16(&dr16_); + } void update() { imu_.update_status(); *chassis_yaw_velocity_imu_ = imu_.gz(); @@ -584,7 +602,7 @@ class DeformableInfantryOmniB device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{status_}; + device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -844,6 +862,7 @@ class DeformableInfantryOmniB std::unique_ptr bottom_board_; std::unique_ptr top_board_; + std::unique_ptr remote_control_; std::shared_ptr command_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 6e9242e2..83c40c97 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -32,6 +32,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" #include "hardware/util/status_monitor.hpp" @@ -59,6 +60,8 @@ class DeformableInfantryOmni tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); + remote_control_ = std::make_unique(*this); + bottom_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); top_board_ = std::make_unique( @@ -80,6 +83,7 @@ class DeformableInfantryOmni void update() override { bottom_board_->update(); top_board_->update(); + remote_control_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -193,6 +197,8 @@ class DeformableInfantryOmni auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); + + status_.remote_control_->register_dr16(&dr16_); } void update() { @@ -406,7 +412,7 @@ class DeformableInfantryOmni device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{status_}; + device::Dr16 dr16_{}; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -848,6 +854,7 @@ class DeformableInfantryOmni std::unique_ptr bottom_board_; std::unique_ptr top_board_; + std::unique_ptr remote_control_; std::shared_ptr command_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 75152b17..ba0ec4fa 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -6,11 +6,9 @@ #include #include +#include #include -#include -#include -#include #include #include #include @@ -19,32 +17,7 @@ namespace rmcs_core::hardware::device { class Dr16 { public: - explicit Dr16(rmcs_executor::Component& component) { - component.register_output( - "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); - component.register_output( - "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); - - component.register_output( - "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); - component.register_output( - "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); - - component.register_output( - "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); - component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_); - - component.register_output("/remote/mouse", mouse_output_); - std::memset(&*mouse_output_, 0, sizeof(*mouse_output_)); - component.register_output("/remote/keyboard", keyboard_output_); - std::memset(&*keyboard_output_, 0, sizeof(*keyboard_output_)); - - component.register_output("/remote/rotary_knob", rotary_knob_output_); - - // Simulate the rotary knob as a switch, with anti-shake algorithm. - component.register_output( - "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); - } + Dr16() = default; void store_status(const std::byte* uart_data, size_t uart_data_length) { if (uart_data_length != 6 + 8 + 4) @@ -73,9 +46,17 @@ class Dr16 { std::memcpy(&part3, uart_data, 4); uart_data += 4; data_part3_.store(part3, std::memory_order::relaxed); + + last_remote_control_received_at_ = Clock::now(); + valid_ = true; } void update_status() { + const auto now = Clock::now(); + refresh_validity(now); + if (!valid_) + return; + auto part1 alignas(uint64_t) = std::bit_cast(data_part1_.load(std::memory_order::relaxed)); @@ -110,19 +91,6 @@ class Dr16 { keyboard_ = part3.keyboard; rotary_knob_ = channel_to_double(part3.rotary_knob); - *joystick_right_output_ = joystick_right(); - *joystick_left_output_ = joystick_left(); - - *switch_right_output_ = switch_right(); - *switch_left_output_ = switch_left(); - - *mouse_velocity_output_ = mouse_velocity(); - *mouse_wheel_output_ = mouse_wheel(); - - *mouse_output_ = mouse(); - *keyboard_output_ = keyboard(); - - *rotary_knob_output_ = rotary_knob(); update_rotary_knob_switch(); } @@ -182,6 +150,10 @@ class Dr16 { rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } + rmcs_msgs::Switch rotary_knob_switch() const { return rotary_knob_switch_; } + + bool valid() const noexcept { return valid_; } + double rotary_knob() const { return rotary_knob_; } double mouse_wheel() const { return mouse_wheel_; } @@ -193,7 +165,7 @@ class Dr16 { constexpr double divider = 0.7, anti_shake_shift = 0.05; double upper_divider = divider, lower_divider = -divider; - auto& switch_value = *rotary_knob_switch_output_; + auto switch_value = rotary_knob_switch_; if (switch_value == rmcs_msgs::Switch::UP) upper_divider -= anti_shake_shift, lower_divider -= anti_shake_shift; else if (switch_value == rmcs_msgs::Switch::MIDDLE) @@ -201,7 +173,7 @@ class Dr16 { else if (switch_value == rmcs_msgs::Switch::DOWN) upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; - const auto knob_value = -*rotary_knob_output_; + const auto knob_value = -rotary_knob_; if (knob_value > upper_divider) { switch_value = rmcs_msgs::Switch::UP; } else if (knob_value < lower_divider) { @@ -209,6 +181,33 @@ class Dr16 { } else { switch_value = rmcs_msgs::Switch::MIDDLE; } + rotary_knob_switch_ = switch_value; + } + + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + + void refresh_validity(const TimePoint now) { + if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + return; + + reset_remote_control_state(); + valid_ = false; + } + + void reset_remote_control_state() { + joystick_right_ = Vector::zero(); + joystick_left_ = Vector::zero(); + switch_right_ = Switch::kUnknown; + switch_left_ = Switch::kUnknown; + mouse_velocity_ = Vector::zero(); + mouse_wheel_ = 0.0; + mouse_ = Mouse::zero(); + keyboard_ = Keyboard::zero(); + rotary_knob_ = 0.0; + rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; } struct [[gnu::packed]] Dr16DataPart1 { @@ -270,27 +269,15 @@ class Dr16 { Switch switch_left_ = Switch::kUnknown; Vector mouse_velocity_ = Vector::zero(); + double mouse_wheel_ = 0.0; Mouse mouse_ = Mouse::zero(); Keyboard keyboard_ = Keyboard::zero(); double rotary_knob_ = 0.0; - double mouse_wheel_ = 0.0; - - rmcs_executor::Component::OutputInterface joystick_right_output_; - rmcs_executor::Component::OutputInterface joystick_left_output_; - - rmcs_executor::Component::OutputInterface switch_right_output_; - rmcs_executor::Component::OutputInterface switch_left_output_; - - rmcs_executor::Component::OutputInterface mouse_velocity_output_; - rmcs_executor::Component::OutputInterface mouse_wheel_output_; - - rmcs_executor::Component::OutputInterface mouse_output_; - rmcs_executor::Component::OutputInterface keyboard_output_; - - rmcs_executor::Component::OutputInterface rotary_knob_output_; - rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; + rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + TimePoint last_remote_control_received_at_ = TimePoint::min(); + bool valid_ = false; }; } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp new file mode 100644 index 00000000..c61b874e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -0,0 +1,165 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "hardware/device/dr16.hpp" +#include "hardware/device/vt13.hpp" + +namespace rmcs_core::hardware::device { + +/* +遥控输入仲裁: +- vt13 valid S挡:vt13主控 | 比赛用 +- vt13 valid C挡:等同于dr16双下 | 疯车救车 +- 其他情况:dr16主控;dr16无效则进入空安全态 +- 旋钮始终来自 dr16,dr16 无效则清零 +*/ + +class RemoteControl { +public: + explicit RemoteControl(rmcs_executor::Component& component) { + component.register_output( + "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); + component.register_output( + "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); + + component.register_output( + "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); + component.register_output( + "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output("/remote/rotary_knob", rotary_knob_output_, 0.0); + component.register_output( + "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output( + "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); + component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_, 0.0); + + component.register_output("/remote/mouse", mouse_output_, rmcs_msgs::Mouse::zero()); + component.register_output( + "/remote/keyboard", keyboard_output_, rmcs_msgs::Keyboard::zero()); + } + + void register_dr16(Dr16* dr16) { dr16_ = dr16; } + void register_vt13(Vt13* vt13) { vt13_ = vt13; } + + void update() { + const auto control_source = select_control_source(); + const auto snapshot = build_snapshot(control_source); + + *joystick_right_output_ = snapshot.joystick_right; + *joystick_left_output_ = snapshot.joystick_left; + + *switch_right_output_ = snapshot.switch_right; + *switch_left_output_ = snapshot.switch_left; + + *mouse_velocity_output_ = snapshot.mouse_velocity; + *mouse_wheel_output_ = snapshot.mouse_wheel; + + *mouse_output_ = snapshot.mouse; + *keyboard_output_ = snapshot.keyboard; + + if (dr16_ && dr16_->valid()) { + *rotary_knob_output_ = dr16_->rotary_knob(); + *rotary_knob_switch_output_ = dr16_->rotary_knob_switch(); + } else { + *rotary_knob_output_ = 0.0; + *rotary_knob_switch_output_ = rmcs_msgs::Switch::UNKNOWN; + } + } + +private: + enum class ControlSource { + kDr16, + kVt13Sport, + kCineSafe, + kInvalidSafe, + }; + + struct Snapshot { + Eigen::Vector2d joystick_right = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_left = Eigen::Vector2d::Zero(); + + rmcs_msgs::Switch switch_right = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch switch_left = rmcs_msgs::Switch::UNKNOWN; + + Eigen::Vector2d mouse_velocity = Eigen::Vector2d::Zero(); + double mouse_wheel = 0.0; + + rmcs_msgs::Mouse mouse = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard = rmcs_msgs::Keyboard::zero(); + }; + + ControlSource select_control_source() const { + if (vt13_ && vt13_->valid()) { + switch (vt13_->mode_switch()) { + case Vt13::ModeSwitch::kSport: return ControlSource::kVt13Sport; + case Vt13::ModeSwitch::kCine: return ControlSource::kCineSafe; + case Vt13::ModeSwitch::kNormal: + case Vt13::ModeSwitch::kUnknown: break; + } + } + + return (dr16_ && dr16_->valid()) ? ControlSource::kDr16 : ControlSource::kInvalidSafe; + } + + Snapshot build_snapshot(ControlSource source) const { + Snapshot snapshot{}; + switch (source) { + case ControlSource::kDr16: + snapshot.joystick_right = dr16_->joystick_right(); + snapshot.joystick_left = dr16_->joystick_left(); + snapshot.switch_right = dr16_->switch_right(); + snapshot.switch_left = dr16_->switch_left(); + snapshot.mouse_velocity = dr16_->mouse_velocity(); + snapshot.mouse_wheel = dr16_->mouse_wheel(); + snapshot.mouse = dr16_->mouse(); + snapshot.keyboard = dr16_->keyboard(); + break; + case ControlSource::kVt13Sport: + snapshot.joystick_right = vt13_->joystick_right(); + snapshot.joystick_left = vt13_->joystick_left(); + snapshot.switch_right = rmcs_msgs::Switch::MIDDLE; + snapshot.switch_left = rmcs_msgs::Switch::MIDDLE; + snapshot.mouse_velocity = vt13_->mouse_velocity(); + snapshot.mouse_wheel = vt13_->mouse_wheel(); + snapshot.mouse = vt13_->mouse(); + snapshot.keyboard = vt13_->keyboard(); + break; + case ControlSource::kCineSafe: + snapshot.switch_right = rmcs_msgs::Switch::DOWN; + snapshot.switch_left = rmcs_msgs::Switch::DOWN; + break; + case ControlSource::kInvalidSafe: break; + } + + return snapshot; + } + + Dr16* dr16_{nullptr}; + Vt13* vt13_{nullptr}; + + rmcs_executor::Component::OutputInterface joystick_right_output_; + rmcs_executor::Component::OutputInterface joystick_left_output_; + + rmcs_executor::Component::OutputInterface switch_right_output_; + rmcs_executor::Component::OutputInterface switch_left_output_; + + rmcs_executor::Component::OutputInterface rotary_knob_output_; + rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; + + rmcs_executor::Component::OutputInterface mouse_velocity_output_; + rmcs_executor::Component::OutputInterface mouse_wheel_output_; + + rmcs_executor::Component::OutputInterface mouse_output_; + rmcs_executor::Component::OutputInterface keyboard_output_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp new file mode 100644 index 00000000..619fe458 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -0,0 +1,386 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class Vt13 { +public: + enum class ModeSwitch : uint8_t { + kUnknown = 0, + kCine = 1, + kNormal = 2, + kSport = 3, + }; + + Vt13() = default; + + void store_status(std::span uart_data) { + store_calls_.fetch_add(1, std::memory_order_relaxed); + received_bytes_.fetch_add(uart_data.size(), std::memory_order_relaxed); + + const auto written = data_buffer_.emplace_back_n( + [iter = uart_data.cbegin()](std::byte* storage) mutable noexcept { + *storage = *iter++; + }, + uart_data.size()); + if (written != uart_data.size()) { + const auto dropped = uart_data.size() - written; + overflow_count_.fetch_add(1, std::memory_order_relaxed); + overflow_dropped_bytes_.fetch_add(dropped, std::memory_order_relaxed); + if (should_log_overflow()) { + RCLCPP_WARN( + logger_, "VT13 input buffer overflow: dropped %zu of %zu bytes", dropped, + uart_data.size()); + } + } + } + + void update_status() { + const auto now = Clock::now(); + auto readable = data_buffer_.readable(); + peak_readable_ = std::max(peak_readable_, readable); + + while (readable) { + ReadResult result = VerificationFailed{}; + + const std::byte front = *data_buffer_.peek_front(); + if (front == std::byte{0xa9}) + result = read_remote_control_data(readable, now); + else if (front == std::byte(0xa5)) + result = read_referee_style_data(readable, now); + else { + unknown_prefix_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 unknown prefix: front=0x%02x readable=%zu", + std::to_integer(front), readable); + } + } + + if (std::holds_alternative(result)) { + break; + } + if (std::holds_alternative(result)) { + verification_failures_++; + data_buffer_.pop_front([](std::byte&&) noexcept {}); + readable--; + continue; + } + if (std::holds_alternative(result)) { + readable -= std::get(result).read; + continue; + } + } + + refresh_validity(now); + maybe_log_statistics(now); + } + + ModeSwitch mode_switch() const noexcept { return mode_switch_; } + bool valid() const noexcept { return valid_; } + + const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } + const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } + + const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } + double mouse_wheel() const noexcept { return mouse_wheel_; } + + rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } + rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } + +private: + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + static constexpr auto kVerificationLogInterval = std::chrono::seconds(1); + static constexpr auto kOverflowLogInterval = std::chrono::seconds(1); + static constexpr auto kStatisticsLogInterval = std::chrono::seconds(5); + static constexpr std::size_t kRefereeFrameMaxSize = 256; + + struct Incomplete {}; + struct VerificationFailed {}; + struct Success { + std::size_t read; + }; + using ReadResult = std::variant; + + struct [[gnu::packed]] RemoteControlData { + static constexpr uint16_t kHeaderMagic = 0x53a9; + + uint16_t header; + + uint16_t joystick_channel0 : 11; + uint16_t joystick_channel1 : 11; + uint16_t joystick_channel2 : 11; + uint16_t joystick_channel3 : 11; + + uint8_t mode_switch : 2; + uint8_t pause_button : 1; + uint8_t left_custom_button : 1; + uint8_t right_custom_button : 1; + uint16_t dial : 11; + uint8_t trigger : 1; + uint8_t padding1 : 3; + + int16_t mouse_velocity_x; + int16_t mouse_velocity_y; + int16_t mouse_velocity_z; + uint8_t mouse_left : 2; + uint8_t mouse_right : 2; + uint8_t mouse_middle : 2; + uint8_t padding2 : 2; + + uint16_t keyboard; + + uint16_t crc16; + }; + + struct [[gnu::packed]] RefereeFrameHeader { + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc8; + }; + + ReadResult read_remote_control_data(const std::size_t readable, const TimePoint now) { + if (readable < sizeof(RemoteControlData)) + return Incomplete{}; + + RemoteControlData data; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&data)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RemoteControlData)); + + if (data.header != RemoteControlData::kHeaderMagic) { + remote_bad_header_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 remote control header invalid: header=0x%04x readable=%zu", + data.header, readable); + } + return VerificationFailed{}; + } + if (!rmcs_utility::dji_crc::verify_crc16(data)) { + remote_bad_crc_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 remote control crc16 invalid: readable=%zu", readable); + return VerificationFailed{}; + } + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, sizeof(RemoteControlData)); + + update_remote_control_data(data); + valid_ = true; + last_remote_control_received_at_ = now; + remote_success_count_++; + return Success{sizeof(RemoteControlData)}; + } + + void update_remote_control_data(const RemoteControlData& data) { + mode_switch_ = static_cast(data.mode_switch + 1); + + joystick_right_ = { + channel_to_double(static_cast(data.joystick_channel1)), + -channel_to_double(static_cast(data.joystick_channel0)), + }; + joystick_left_ = { + channel_to_double(static_cast(data.joystick_channel2)), + -channel_to_double(static_cast(data.joystick_channel3)), + }; + + mouse_velocity_ = { + -data.mouse_velocity_y / 32768.0, + -data.mouse_velocity_x / 32768.0, + }; + mouse_wheel_ = -static_cast(data.mouse_velocity_z) / 32768.0; + + mouse_ = { + .left = static_cast(data.mouse_left), + .right = static_cast(data.mouse_right), + }; + keyboard_ = std::bit_cast(data.keyboard); + } + + ReadResult read_referee_style_data(const std::size_t readable, const TimePoint now) { + if (readable < sizeof(RefereeFrameHeader)) + return Incomplete{}; + + RefereeFrameHeader header; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&header)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RefereeFrameHeader)); + + if (!rmcs_utility::dji_crc::verify_crc8(header)) { + referee_bad_crc8_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 referee header crc8 invalid: readable=%zu", readable); + return VerificationFailed{}; + } + + const std::size_t total_frame_size = + sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; + if (total_frame_size > kRefereeFrameMaxSize) { + referee_oversize_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 referee frame oversized: data_length=%u total=%zu readable=%zu", + header.data_length, total_frame_size, readable); + } + return VerificationFailed{}; + } + if (readable < total_frame_size) + return Incomplete{}; + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, total_frame_size); + referee_discarded_count_++; + return Success{total_frame_size}; + } + + bool should_log_verification_failure(const TimePoint now) { + if (last_verification_log_time_ != TimePoint::min() + && now - last_verification_log_time_ < kVerificationLogInterval) + return false; + last_verification_log_time_ = now; + return true; + } + + bool should_log_overflow() { + const auto now = Clock::now(); + if (last_overflow_log_time_ != TimePoint::min() + && now - last_overflow_log_time_ < kOverflowLogInterval) + return false; + + last_overflow_log_time_ = now; + return true; + } + + void refresh_validity(const TimePoint now) { + if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + return; + + reset_remote_control_state(); + valid_ = false; + } + + void maybe_log_statistics(const TimePoint now) { + if (last_statistics_log_time_ == TimePoint::min()) { + last_statistics_log_time_ = now; + return; + } + + const auto elapsed = now - last_statistics_log_time_; + if (elapsed < kStatisticsLogInterval) + return; + + const auto readable = data_buffer_.readable(); + const auto store_calls = store_calls_.exchange(0, std::memory_order_relaxed); + const auto received_bytes = received_bytes_.exchange(0, std::memory_order_relaxed); + const auto overflow_count = overflow_count_.exchange(0, std::memory_order_relaxed); + const auto overflow_dropped_bytes = + overflow_dropped_bytes_.exchange(0, std::memory_order_relaxed); + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + + /* RCLCPP_INFO( + logger_, + "VT13 stats: rx=%.1f Hz %.1f B/s remote_ok=%zu verify_fail=%zu remote_bad_header=%zu " + "remote_bad_crc=%zu referee_discarded=%zu referee_bad_crc8=%zu referee_oversize=%zu " + "unknown_prefix=%zu overflow=%llu dropped=%llu readable=%zu peak=%zu valid=%s", + static_cast(store_calls) / elapsed_seconds, + static_cast(received_bytes) / elapsed_seconds, remote_success_count_, + verification_failures_, remote_bad_header_count_, remote_bad_crc_count_, + referee_discarded_count_, referee_bad_crc8_count_, referee_oversize_count_, + unknown_prefix_count_, static_cast(overflow_count), + static_cast(overflow_dropped_bytes), readable, peak_readable_, + valid_ ? "true" : "false"); + */ + + remote_success_count_ = 0; + verification_failures_ = 0; + remote_bad_header_count_ = 0; + remote_bad_crc_count_ = 0; + referee_discarded_count_ = 0; + referee_bad_crc8_count_ = 0; + referee_oversize_count_ = 0; + unknown_prefix_count_ = 0; + peak_readable_ = readable; + last_statistics_log_time_ = now; + } + + void reset_remote_control_state() { + mode_switch_ = ModeSwitch::kUnknown; + joystick_left_ = Eigen::Vector2d::Zero(); + joystick_right_ = Eigen::Vector2d::Zero(); + mouse_velocity_ = Eigen::Vector2d::Zero(); + mouse_wheel_ = 0; + mouse_ = rmcs_msgs::Mouse::zero(); + keyboard_ = rmcs_msgs::Keyboard::zero(); + } + + static double channel_to_double(int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + } + + rclcpp::Logger logger_ = rclcpp::get_logger("vt13"); + rmcs_utility::RingBuffer data_buffer_{1024}; + + std::atomic store_calls_{0}; + std::atomic received_bytes_{0}; + std::atomic overflow_count_{0}; + std::atomic overflow_dropped_bytes_{0}; + + TimePoint last_remote_control_received_at_ = TimePoint::min(); + TimePoint last_verification_log_time_ = TimePoint::min(); + TimePoint last_overflow_log_time_ = TimePoint::min(); + TimePoint last_statistics_log_time_ = TimePoint::min(); + + bool valid_ = false; + std::size_t peak_readable_ = 0; + std::size_t remote_success_count_ = 0; + std::size_t verification_failures_ = 0; + std::size_t remote_bad_header_count_ = 0; + std::size_t remote_bad_crc_count_ = 0; + std::size_t referee_discarded_count_ = 0; + std::size_t referee_bad_crc8_count_ = 0; + std::size_t referee_oversize_count_ = 0; + std::size_t unknown_prefix_count_ = 0; + + ModeSwitch mode_switch_ = ModeSwitch::kUnknown; + + Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); + + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); + double mouse_wheel_ = 0; + + rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index e0595c16..d1c5b214 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -19,6 +19,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "librmcs/board/rmcs_board_lite.hpp" namespace rmcs_core::hardware { @@ -78,6 +79,9 @@ class Flight return size; }; + remote_control_ = std::make_unique(*this); + remote_control_->register_dr16(&dr16_); + status_service_ = create_service( "/rmcs/service/robot_status", [this]( @@ -94,6 +98,7 @@ class Flight update_motors(); update_imu(); dr16_.update_status(); + remote_control_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -251,7 +256,8 @@ class Flight device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; - device::Dr16 dr16_{*this}; + device::Dr16 dr16_; + std::unique_ptr remote_control_; device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; OutputInterface gimbal_yaw_velocity_imu_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 62420a43..b9589e4c 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -26,6 +26,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" namespace rmcs_core::hardware { @@ -53,7 +54,8 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{*this} { + , dr16_{} + { for (auto& motor : chassis_wheel_motors_) motor.configure( @@ -132,6 +134,9 @@ class OmniInfantry Spec::kUarts.kUart1, {.uart_data = std::span{buffer, size}}); return size; }; + + remote_control_ = std::make_unique(*this); + remote_control_->register_dr16(&dr16_); } OmniInfantry(const OmniInfantry&) = delete; @@ -145,6 +150,7 @@ class OmniInfantry update_motors(); update_imu(); dr16_.update_status(); + remote_control_->update(); supercap_.update_status(); } @@ -350,6 +356,7 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; + std::unique_ptr remote_control_; device::Bmi088Ekf bmi088_; device::BoardClockLifter board_clock_lifter_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index dd92f1a4..4233d7c7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -19,6 +19,7 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" #include "hardware/util/status_monitor.hpp" @@ -53,6 +54,8 @@ class Sentry status_service_callback(response); }); + remote_control_ = std::make_unique(*this); + gimbal_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_gimbal_board").as_string()); @@ -68,6 +71,7 @@ class Sentry void update() override { gimbal_board_->update(); chassis_board_->update(); + remote_control_->update(); using namespace rmcs_description; *camera_transform_ = fast_tf::lookup_transform(*tf_); @@ -280,7 +284,7 @@ class Sentry Sentry& sentry, rmcs_executor::Component& sentry_command, std::string_view board_serial = {}) : tf_(sentry.tf_) - , dr16_(sentry) + , dr16_{} , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") , chassis_wheel_motors_( {sentry, sentry_command, "/chassis/left_front_wheel"}, @@ -358,6 +362,8 @@ class Sentry .enable_multi_turn_angle()); board_ = std::make_unique(*this, board_serial); + + sentry.remote_control_->register_dr16(&dr16_); } auto status() const -> std::vector { return monitor_.text(); } @@ -625,6 +631,7 @@ class Sentry std::unique_ptr gimbal_board_; std::unique_ptr chassis_board_; + std::unique_ptr remote_control_; std::shared_ptr> status_service_; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp index f20588c2..b216fa6e 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little-six-friction.cpp @@ -36,7 +36,9 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -134,6 +136,8 @@ class SteeringHeroLittle gimbal_calibrate_subscription_callback(std::move(msg)); }); + remote_control_ = std::make_unique(*this); + top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); @@ -154,6 +158,7 @@ class SteeringHeroLittle void update() override { top_board_->update(); bottom_board_->update(); + remote_control_->update(); tf_->set_state( bottom_board_->gimbal_bottom_yaw_motor_.angle() @@ -308,6 +313,10 @@ class SteeringHeroLittle "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); board_ = std::make_unique(*this, board_serial); + + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + steering_hero.remote_control_->register_vt13(&vt13_); } void update() { @@ -316,6 +325,8 @@ class SteeringHeroLittle // can2_receive_rate_counter_.report_if_due(); // can3_receive_rate_counter_.report_if_due(); + vt13_.update_status(); + if (auto snapshot = bmi088_.snapshot()) { tf_->set_transform( snapshot->orientation.conjugate()); @@ -481,6 +492,12 @@ class SteeringHeroLittle } } + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) { + vt13_.store_status(data.uart_data); + } + } + void gpio_digital_read_result_callback( const Spec::Gpio& gpio, const View::GpioDigital& data) override { @@ -516,6 +533,9 @@ class SteeringHeroLittle return *gimbal_yaw_velocity_imu_; } + [[nodiscard]] device::Vt13& vt13() noexcept { return vt13_; } + [[nodiscard]] const device::Vt13& vt13() const noexcept { return vt13_; } + rclcpp::Logger logger_; // CanReceiveRateCounter can0_receive_rate_counter_; // CanReceiveRateCounter can1_receive_rate_counter_; @@ -529,6 +549,7 @@ class SteeringHeroLittle device::Bmi088Ekf bmi088_; device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; device::LkMotor gimbal_top_yaw_motor_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_friction_wheels_[6]; @@ -559,7 +580,7 @@ class SteeringHeroLittle // , can2_receive_rate_counter_(logger_, "bottom/can2") // , can3_receive_rate_counter_(logger_, "bottom/can3") , imu_(1000, 0.2, 0.0) - , dr16_(steering_hero) + , dr16_{} , supercap_(steering_hero, steering_hero_command) , chassis_steering_motors_( {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, @@ -669,6 +690,8 @@ class SteeringHeroLittle steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); board_ = std::make_unique(*this, board_serial); + + steering_hero.remote_control_->register_dr16(&dr16_); } void update() { @@ -866,6 +889,9 @@ class SteeringHeroLittle } } + [[nodiscard]] device::Dr16& dr16() noexcept { return dr16_; } + [[nodiscard]] const device::Dr16& dr16() const noexcept { return dr16_; } + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } @@ -915,6 +941,7 @@ class SteeringHeroLittle std::shared_ptr top_board_; std::shared_ptr bottom_board_; + std::unique_ptr remote_control_; }; } // namespace rmcs_core::hardware