diff --git a/.gitignore b/.gitignore index 39b2fb34..8e4253e0 100644 --- a/.gitignore +++ b/.gitignore @@ -79,3 +79,4 @@ develop_ws .codex/ .mimo/ CLAUDE.md +rmcs_ws/src/odin_ros_driver/ diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 387b384e..e15d2c84 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 387b384e9765735a5425abd4e79564ed3310748c +Subproject commit e15d2c84a00acfde5a6966fd851500e5a2a8608d diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 04bb643e..30e9a2ce 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -35,8 +35,8 @@ rmcs_executor: auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 3000.0 - gain: 8.0 + exposure_us: 2000.0 + gain: 10.0 framerate: 120.0 invert_image: false rls_tau_sec: 10.0 @@ -45,11 +45,12 @@ auto_aim_capturer: auto_aim_recorder: ros__parameters: - output_path: "/tmp/autoaim/records" + output_path: "/autoaim/records" queue_depth: 16 flush_every_n_frames: 64 - max_duration_seconds: 0 - max_videos_size_gb: 0.0 + max_duration_seconds: 600 + max_videos_size_gb: 500.0 + auto_record: false auto_aim_component: ros__parameters: @@ -57,29 +58,29 @@ auto_aim_component: # 将强行绑定为对应阵营哨兵,仅供调试,严禁比赛启用。 # 留空或填 unknow 表示禁用。 dangerous_fallback: "" - manual_shoot: true + manual_shoot: false enable_rune: false camera_translation: [0.10238, 0.0, 0.05286] fire_control: bullet_speed: 22.5 - shoot_delay: 0.05 - offset_yaw: +1.3 #越大越左 - offset_pitch: +1.7 #越大越下 - attack_window: 80.0 + shoot_delay: 0.07 + offset_yaw: +1.5 #越大越左 + offset_pitch: +3.5 #越大越下 + attack_window: 120.0 degraded_angle_speed: 12.0 window_redundancy: 0.8 window_hysteresis: 0.2 is_lazy_gimbal: false attack_preaim: false require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 + yaw_tolerance: 0.14 + pitch_tolerance: 0.08 rune_idle_duration: 0.4 rune_shoot_duration: 0.2 px4_vision_bridge: ros__parameters: - source_topic: /odin1/odometry_highfreq + source_topic: /odin1/odometry system_id: 1 # 与 PX4 MAV_SYS_ID 一致 component_id: 197 # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY max_send_rate_hz: 50.0 @@ -116,6 +117,7 @@ tf_broadcaster: flight_hardware: ros__parameters: board_serial: "AF-7C58-5458-E731-9F74-1F9C-CAFD-30AF-9C09" + vt13_board_serial: "AF-79CF-C275-34AA-F714-8CC6-E363-5B59-FB4E" yaw_motor_zero_point: 11720 pitch_motor_zero_point: 18578 @@ -128,14 +130,14 @@ gimbal_controller: upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° lower_limit: 0.7 # 0.7 rad ≈ 40.1° yaw_lower_limit: 0.1745 - yaw_upper_limit: 2.5708 + yaw_upper_limit: 4.0708 yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity kp: 15.0 - ki: 0.0 + ki: 0.01 kd: 0.0 yaw_velocity_pid_controller: @@ -161,8 +163,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 620.0 - - 620.0 + - 585.0 + - 585.0 friction_soft_start_stop_time: 1.0 heat_controller: diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp index 2951f53f..b6bf346e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp @@ -31,6 +31,7 @@ class PidCalculator { double update(double err) { if (!std::isfinite(err)) { + reset(); return nan; } else { double control = kp * err; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp index 83c1b2e3..33ae9bef 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp @@ -84,7 +84,7 @@ class Bmi088Ekf { ekf_state_time_ = accel_sample_time; const auto correction = ekf_.prepare_correction(pending_accel_sample_->accel_g); - if (!correction || correction->chi_square() >= 3.0) + if (!correction || correction->chi_square() >= 16.0) break; if (!ekf_.correct(*correction)) break; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 75480707..91f02967 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -22,6 +23,8 @@ #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" #include "hardware/device/remote_control.hpp" +#include "hardware/device/vt13.hpp" +#include "hardware/vtm-link/ladar_package_transmit.hpp" #include "librmcs/board/rmcs_board_lite.hpp" namespace rmcs_core::hardware { @@ -98,16 +101,22 @@ class Flight board_ = std::make_unique( *this, get_parameter("board_serial").as_string()); + + vt13_board_ = + std::make_unique(*this, get_parameter("vt13_board_serial").as_string()); } void update() override { update_motors(); update_imu(); dr16_.update_status(); + vt13_board_->update(); remote_control_->update(); } void command_update() { + vt13_board_->command_update(); + auto builder = board_->start_transmit(); builder .can_transmit( @@ -243,6 +252,37 @@ class Flight } private: + struct Vt13Board final : librmcs::board::RmcsBoardLite::Callback { + explicit Vt13Board(Flight& flight, std::string_view board_serial) + : ladar_transmit_( + *flight.command_component_, std::chrono::milliseconds{200}, + [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, + {.uart_data = std::span{buffer, size}}); + }, + flight.get_logger()) { + board_ = std::make_unique(*this, board_serial); + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + flight.remote_control_->register_vt13(&vt13_); + } + + void update() { vt13_.update_status(); } + + void command_update() { ladar_transmit_.command_update(); } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) { + vt13_.store_status(data.uart_data); + } + } + + device::Vt13 vt13_; + vtm::LadarPackageTransmit ladar_transmit_; + std::unique_ptr board_; + }; + class FlightCommand : public rmcs_executor::Component { public: explicit FlightCommand(Flight& flight) @@ -259,6 +299,7 @@ class Flight std::shared_ptr> status_service_; std::unique_ptr board_; + std::unique_ptr vt13_board_; device::LkMotor gimbal_yaw_motor_{*this, *command_component_, "/gimbal/yaw"}; device::LkMotor gimbal_pitch_motor_{*this, *command_component_, "/gimbal/pitch"}; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp b/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp new file mode 100644 index 00000000..798dc665 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "referee/frame.hpp" + +namespace rmcs_core::hardware::vtm { + +class LadarPackageTransmit { +public: + using UartWriter = std::function; + using LidarMsgBroadcast = std::array; + + LadarPackageTransmit( + rmcs_executor::Component& component, std::chrono::milliseconds interval, + UartWriter uart_writer, rclcpp::Logger logger, + const std::string& lidar_msg_broadcast_name = + "/referee/multi_robot_communication/lidar_msg_broadcast") + : uart_writer_(std::move(uart_writer)) + , interval_(interval) + , logger_(std::move(logger)) { + component.register_input(lidar_msg_broadcast_name, lidar_msg_broadcast_, false); + } + + void command_update() { + if (!lidar_msg_broadcast_.ready()) + return; + + auto now = std::chrono::steady_clock::now(); + if (now < next_publish_time_) + return; + + publish_single_packet(*lidar_msg_broadcast_); + next_publish_time_ = now + interval_; + } + +private: + static constexpr uint16_t kLadarCmdId = 0x0310; + static constexpr size_t kLadarDataSize = 118; + static constexpr size_t kLadarPacketSize = 300; + + void publish_single_packet(const LidarMsgBroadcast& lidar_msg_broadcast) { + static constexpr size_t kHeaderSize = sizeof(referee::FrameHeader); + static constexpr size_t kCmdIdSize = sizeof(uint16_t); + static constexpr size_t kCrc16Size = sizeof(uint16_t); + static constexpr size_t kFrameSize = + kHeaderSize + kCmdIdSize + kLadarPacketSize + kCrc16Size; + + referee::Frame frame; + frame.header.sof = referee::sof_value; + frame.header.data_length = kLadarPacketSize; + frame.header.sequence = sequence_++; + frame.header.crc8 = 0; + frame.body.command_id = kLadarCmdId; + std::memcpy(frame.body.data, lidar_msg_broadcast.data(), kLadarDataSize); + std::memset(frame.body.data + kLadarDataSize, 0, kLadarPacketSize - kLadarDataSize); + + rmcs_utility::dji_crc::append_crc8(frame.header); + rmcs_utility::dji_crc::append_crc16(&frame, kFrameSize); + + uart_writer_(reinterpret_cast(&frame), kFrameSize); + RCLCPP_DEBUG(logger_, "uart sent ladar packet"); + } + + rmcs_executor::Component::InputInterface lidar_msg_broadcast_; + UartWriter uart_writer_; + std::chrono::milliseconds interval_; + std::chrono::steady_clock::time_point next_publish_time_ = + std::chrono::steady_clock::time_point::min(); + uint8_t sequence_{0}; + rclcpp::Logger logger_; +}; + +} // namespace rmcs_core::hardware::vtm diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c0a7db62..3f11a3af 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -103,6 +104,9 @@ class Status register_output("/referee/map_command/event/source", map_command_event_source_, 0); register_output("/referee/map_command/event/timestamp", map_command_event_timestamp_, 0.0); register_output("/referee/map_command/event/sequence", map_command_event_sequence_, 0); + register_output( + "/referee/multi_robot_communication/lidar_msg_broadcast", lidar_msg_broadcast_, + LidarMsgBroadcast{}); robot_status_watchdog_.reset(5'000); } @@ -182,6 +186,8 @@ class Status update_sentry_info(); else if (command_id == 0x0303) update_map_command(); + else if (command_id == 0x0301) + update_robot_interaction_data(); } void update_game_status() { @@ -326,6 +332,28 @@ class Status *map_command_event_timestamp_ = *map_command_received_timestamp_; *map_command_event_sequence_ += 1; } + + void update_robot_interaction_data() { + if (frame_.header.data_length < sizeof(RobotInteractionData)) { + RCLCPP_WARN( + logger_, "Robot interaction data length invalid: %u", + static_cast(frame_.header.data_length)); + return; + } + + RobotInteractionData data; + std::memcpy(&data, frame_.body.data, sizeof(data)); + + if (data.sender_id != 9 && data.sender_id != 109) + return; + if (data.data_cmd_id < 0x0200 || data.data_cmd_id > 0x02ff) + return; + + LidarMsgBroadcast lidar_msg{}; + std::memcpy(lidar_msg.data(), data.user_data, lidar_msg.size()); + *lidar_msg_broadcast_ = lidar_msg; + } + // When referee system loses connection unexpectedly, // use these indicators make sure the robot safe. // Muzzle: Cooling priority with level 1 @@ -403,6 +431,9 @@ class Status OutputInterface map_command_event_sequence_; MapCommand last_map_command_{}; bool has_last_map_command_ = false; + + using LidarMsgBroadcast = std::array; + OutputInterface lidar_msg_broadcast_; }; } // namespace rmcs_core::referee diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index ad5e2161..1f2f0282 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -158,4 +158,11 @@ struct __attribute__((packed)) SentryInfo { }; static_assert(sizeof(SentryInfo) == 14); +struct __attribute__((packed)) RobotInteractionData { + uint16_t data_cmd_id; + uint16_t sender_id; + uint16_t receiver_id; + uint8_t user_data[112]; +}; + } // namespace rmcs_core::referee::status