26uc/flight

.gitignore

@@ -79,3 +79,4 @@ develop_ws
 .codex/
 .mimo/
 CLAUDE.md
+rmcs_ws/src/odin_ros_driver/

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,41 +45,42 @@ 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:
     # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时
     # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。
     # 留空或填 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:

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;

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;

rmcs_ws/src/rmcs_core/src/hardware/flight.cpp

@@ -1,3 +1,4 @@
+#include <chrono>
 #include <cstddef>
 #include <memory>
 #include <new>
@@ -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<librmcs::board::RmcsBoardLite>(
             *this, get_parameter("board_serial").as_string());
+
+        vt13_board_ =
+            std::make_unique<Vt13Board>(*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<const std::byte>{buffer, size}});
+                  },
+                  flight.get_logger()) {
+            board_ = std::make_unique<librmcs::board::RmcsBoardLite>(*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<librmcs::board::RmcsBoardLite> board_;
+    };
+
     class FlightCommand : public rmcs_executor::Component {
     public:
         explicit FlightCommand(Flight& flight)
@@ -259,6 +299,7 @@ class Flight
     std::shared_ptr<rclcpp::Service<std_srvs::srv::Trigger>> status_service_;
 
     std::unique_ptr<librmcs::board::RmcsBoardLite> board_;
+    std::unique_ptr<Vt13Board> vt13_board_;
 
     device::LkMotor gimbal_yaw_motor_{*this, *command_component_, "/gimbal/yaw"};
     device::LkMotor gimbal_pitch_motor_{*this, *command_component_, "/gimbal/pitch"};

rmcs_ws/src/rmcs_core/src/hardware/vtm-link/ladar_package_transmit.hpp

@@ -0,0 +1,85 @@
+#pragma once
+
+#include <array>
+#include <chrono>
+#include <cstddef>
+#include <cstdint>
+#include <cstring>
+#include <functional>
+#include <string>
+#include <utility>
+
+#include <rclcpp/logging.hpp>
+#include <rmcs_executor/component.hpp>
+#include <rmcs_utility/crc/dji_crc.hpp>
+
+#include "referee/frame.hpp"
+
+namespace rmcs_core::hardware::vtm {
+
+class LadarPackageTransmit {
+public:
+    using UartWriter = std::function<void(const std::byte*, size_t)>;
+    using LidarMsgBroadcast = std::array<std::uint8_t, 118>;
+
+    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<const std::byte*>(&frame), kFrameSize);
+        RCLCPP_DEBUG(logger_, "uart sent ladar packet");
+    }
+
+    rmcs_executor::Component::InputInterface<LidarMsgBroadcast> 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

rmcs_ws/src/rmcs_core/src/referee/status.cpp

@@ -1,3 +1,4 @@
+#include <array>
 #include <cstdint>
 #include <cstring>
 #include <eigen3/Eigen/Eigen>
@@ -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<unsigned>(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<uint64_t> map_command_event_sequence_;
     MapCommand last_map_command_{};
     bool has_last_map_command_ = false;
+
+    using LidarMsgBroadcast = std::array<std::uint8_t, 118>;
+    OutputInterface<LidarMsgBroadcast> lidar_msg_broadcast_;
 };
 
 } // namespace rmcs_core::referee

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