feat: Standard flight support

.script/template/entrypoint

@@ -1,5 +1,18 @@
 #!/usr/bin/bash
 
+USBFS_MEMORY_MB="${USBFS_MEMORY_MB:-2000}"
+USBFS_MEMORY_MB_PATH="/sys/module/usbcore/parameters/usbfs_memory_mb"
+
+if [ -w "$USBFS_MEMORY_MB_PATH" ]; then
+    if ! printf '%s\n' "$USBFS_MEMORY_MB" > "$USBFS_MEMORY_MB_PATH"; then
+        echo "Warning: failed to set $USBFS_MEMORY_MB_PATH to $USBFS_MEMORY_MB" >&2
+    fi
+elif [ -e "$USBFS_MEMORY_MB_PATH" ]; then
+    echo "Warning: $USBFS_MEMORY_MB_PATH is not writable, skipping usbfs_memory_mb setup" >&2
+else
+    echo "Warning: $USBFS_MEMORY_MB_PATH not found, skipping usbfs_memory_mb setup" >&2
+fi
+
 # Remove all files in /tmp
 rm -rf /tmp/*
 
@@ -14,4 +27,4 @@ if [ -f "/etc/avahi/enabled" ]; then
     service avahi-daemon start
 fi
 
-sleep infinity
+sleep infinity

rmcs_ws/src/rmcs_bringup/config/flight.yaml

@@ -0,0 +1,153 @@
+rmcs_executor:
+  ros__parameters:
+    update_rate: 1000.0
+    components:
+      - rmcs_core::hardware::Flight -> flight_hardware
+
+      - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller
+      - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller
+      - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller
+      - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller
+
+      - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller
+      - rmcs_core::controller::shooting::HeatController -> heat_controller
+      - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller
+      - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller
+      - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller
+      - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller
+
+      - rmcs_core::referee::command::Interaction -> referee_interaction
+      - rmcs_core::referee::Command -> referee_command
+      - rmcs_core::referee::Status -> referee_status
+      - rmcs_core::referee::command::interaction::Ui -> referee_ui
+      - rmcs_core::referee::app::ui::Flight -> referee_ui_flight
+
+      # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster
+      # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster
+
+      - rmcs::AutoAimComponent
+
+odin_ros_driver:
+  ros__parameters:
+    enabled: true
+    config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml"
+    node_name: host_sdk_sample
+    respawn: true
+    respawn_delay: 1.0
+
+value_broadcaster:
+  ros__parameters:
+    forward_list:
+      - /gimbal/pitch/angle
+      - /gimbal/pitch/velocity
+      - /gimbal/pitch/torque
+      - /gimbal/pitch/control_angle_error
+      - /gimbal/pitch/control_velocity
+      - /gimbal/pitch/velocity_imu
+
+      - /gimbal/yaw/angle
+      - /gimbal/yaw/velocity
+      - /gimbal/yaw/torque
+      - /gimbal/yaw/control_torque
+      - /gimbal/yaw/control_angle_error
+      - /gimbal/yaw/velocity_imu
+      - /gimbal/bullet_feeder/velocity
+
+tf_broadcaster:
+  ros__parameters:
+    tf: /tf
+
+flight_hardware:
+  ros__parameters:
+    board_serial: "AF-7C58-5458-E731-9F74-1F9C-CAFD-30AF-9C09"
+    yaw_motor_zero_point: 11720
+    pitch_motor_zero_point: 18578
+
+referee_status:
+  ros__parameters:
+    path: /dev/tty0
+
+gimbal_controller:
+  ros__parameters:
+    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_angle_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/yaw/control_angle_error
+    control: /gimbal/yaw/control_velocity
+    kp: 15.0
+    ki: 0.0
+    kd: 0.0
+
+yaw_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/yaw/velocity_imu
+    setpoint: /gimbal/yaw/control_velocity
+    control: /gimbal/yaw/control_torque
+    kp: 8.0
+    ki: 0.0
+    kd: 0.0
+
+pitch_angle_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/pitch/control_angle_error
+    control: /gimbal/pitch/control_velocity
+    kp: 10.0
+    ki: 0.0
+    kd: 0.0
+
+friction_wheel_controller:
+  ros__parameters:
+    friction_wheels:
+      - /gimbal/left_friction
+      - /gimbal/right_friction
+    friction_velocities:
+      - 620.0
+      - 620.0
+    friction_soft_start_stop_time: 1.0
+
+heat_controller:
+  ros__parameters:
+    heat_per_shot: 10000
+    reserved_heat: 15000
+
+bullet_feeder_controller:
+  ros__parameters:
+    bullets_per_feeder_turn: 8.0
+    shot_frequency: 20.0
+    safe_shot_frequency: 10.0
+    eject_frequency: 10.0
+    eject_time: 0.05
+    deep_eject_frequency: 5.0
+    deep_eject_time: 0.2
+    single_shot_max_stop_delay: 2.0
+
+left_friction_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/left_friction/velocity
+    setpoint: /gimbal/left_friction/control_velocity
+    control: /gimbal/left_friction/control_torque
+    kp: 0.003436926
+    ki: 0.00
+    kd: 0.009373434
+
+right_friction_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/right_friction/velocity
+    setpoint: /gimbal/right_friction/control_velocity
+    control: /gimbal/right_friction/control_torque
+    kp: 0.003436926
+    ki: 0.00
+    kd: 0.009373434
+
+bullet_feeder_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/bullet_feeder/velocity
+    setpoint: /gimbal/bullet_feeder/control_velocity
+    control: /gimbal/bullet_feeder/control_torque
+    kp: 0.583
+    ki: 0.0
+    kd: 0.0

rmcs_ws/src/rmcs_core/CMakeLists.txt

@@ -8,29 +8,30 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-packed-bitfield-compat")
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-O2 -Wall -Wextra -Wpedantic)
+    add_compile_options(-O2 -Wall -Wextra -Wpedantic)
 endif()
 
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
+
 include(FetchContent)
 set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE)
 FetchContent_Declare(
-  librmcs
-  URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0/librmcs-sdk-src-3.2.0.zip
-  URL_HASH SHA256=f81c3af7fbcf35727a8a7586200db8e9bf668ee0f448529de4bfd3eb7c36ed6f
-  DOWNLOAD_EXTRACT_TIMESTAMP TRUE
+    librmcs
+    URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0/librmcs-sdk-src-3.2.0.zip
+    URL_HASH SHA256=f81c3af7fbcf35727a8a7586200db8e9bf668ee0f448529de4bfd3eb7c36ed6f
+    DOWNLOAD_EXTRACT_TIMESTAMP TRUE
 )
 FetchContent_MakeAvailable(librmcs)
 
 file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS
-  ${PROJECT_SOURCE_DIR}/src/*.cpp
-  ${PROJECT_SOURCE_DIR}/src/*.c
+    ${PROJECT_SOURCE_DIR}/src/*.cpp
+    ${PROJECT_SOURCE_DIR}/src/*.c
 )
 
 ament_auto_add_library(
-  ${PROJECT_NAME} SHARED
-  ${PROJECT_SOURCE}
+    ${PROJECT_NAME} SHARED
+    ${PROJECT_SOURCE}
 )
 
 include_directories(${PROJECT_SOURCE_DIR}/include)

rmcs_ws/src/rmcs_core/plugins.xml

@@ -1,12 +1,13 @@
 <library path="rmcs_core">
-  <class type="rmcs_core::hardware::MecanumHero" base_class_type="rmcs_executor::Component" />
+  <class type="rmcs_core::hardware::Flight" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::SteeringHero" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::SteeringHeroLittle" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::OmniInfantry" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::Sentry" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::SteeringInfantry" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::DeformableInfantryV2" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::DeformableInfantryOmni" base_class_type="rmcs_executor::Component" />
+
   <class type="rmcs_core::controller::chassis::ChassisController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::chassis::ChassisPowerController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::chassis::HeroChassisController" base_class_type="rmcs_executor::Component" />
@@ -20,14 +21,14 @@
   <class type="rmcs_core::controller::chassis::DeformableChassis" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::chassis::DeformableJointController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::chassis::DeformableChassisController" base_class_type="rmcs_executor::Component" />
-  <class type="rmcs_core::controller::chassis::ChassisControllerV2" base_class_type="rmcs_executor::Component" />
+
   <class type="rmcs_core::controller::gimbal::SimpleGimbalController" base_class_type="rmcs_executor::Component" />
-  <class type="rmcs_core::controller::gimbal::OmniInfantryPlannerGimbalController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::gimbal::HeroGimbalController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::gimbal::DualYawController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::gimbal::DeformableInfantryGimbalController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::gimbal::EccentricDualYaw" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::gimbal::PlayerViewer" base_class_type="rmcs_executor::Component" />
+
   <class type="rmcs_core::controller::shooting::FrictionWheelController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::shooting::HeroFrictionWheelController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::shooting::HeatController" base_class_type="rmcs_executor::Component" />
@@ -36,18 +37,23 @@
   <class type="rmcs_core::controller::shooting::BulletFeederController42mm" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::shooting::PutterController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::shooting::ShootingRecorder" base_class_type="rmcs_executor::Component" />
+
   <class type="rmcs_core::controller::pid::PidController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::pid::ErrorPidController" base_class_type="rmcs_executor::Component" />
+  <class type="rmcs_core::controller::pid::FrictionWheelPidRecorder" base_class_type="rmcs_executor::Component" />
+
   <class type="rmcs_core::controller::adrc::AdrcController" base_class_type="rmcs_executor::Component" />
+
   <class type="rmcs_core::broadcaster::TfBroadcaster" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::broadcaster::ValueBroadcaster" base_class_type="rmcs_executor::Component" />
+
   <class type="rmcs_core::referee::Status" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::referee::Command" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::referee::command::Interaction" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::referee::command::interaction::Ui" base_class_type="rmcs_executor::Component" />
+
   <class type="rmcs_core::referee::app::ui::Infantry" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::referee::app::ui::DeformableInfantry" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::referee::app::ui::Hero" base_class_type="rmcs_executor::Component" />
-
-  <class type="rmcs_core::test::output::AngleOutput" base_class_type="rmcs_executor::Component" />
+  <class type="rmcs_core::referee::app::ui::Flight" base_class_type="rmcs_executor::Component" />
 </library>

rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp

@@ -20,20 +20,27 @@ class SimpleGimbalController
         : Node(
               get_component_name(),
               rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true))
-        , two_axis_gimbal_solver(
-              *this, get_parameter("upper_limit").as_double(),
-              get_parameter("lower_limit").as_double()) {
+        , two_axis_gimbal_solver{
+              *this,
+              get_parameter("upper_limit").as_double(),
+              get_parameter("lower_limit").as_double(),
+          } {
 
         register_input("/remote/joystick/left", joystick_left_);
         register_input("/remote/switch/right", switch_right_);
         register_input("/remote/switch/left", switch_left_);
         register_input("/remote/mouse/velocity", mouse_velocity_);
         register_input("/remote/mouse", mouse_);
 
-        register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false);
+        register_input("/auto_aim/should_control", auto_aim_should_control_, false);
+        register_input("/auto_aim/control_direction", auto_aim_control_direction_, false);
 
         register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_);
         register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_);
+
+        two_axis_gimbal_solver.enable_yaw_limit(
+            *this, get_parameter("yaw_upper_limit").as_double(),
+            get_parameter("yaw_lower_limit").as_double());
     }
 
     void update() override {
@@ -45,18 +52,22 @@ class SimpleGimbalController
     TwoAxisGimbalSolver::AngleError calculate_angle_error() {
         auto switch_right = *switch_right_;
         auto switch_left = *switch_left_;
-        auto mouse = *mouse_;
 
         using namespace rmcs_msgs;
         if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN)
             || (switch_left == Switch::DOWN && switch_right == Switch::DOWN))
             return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled());
 
-        if (auto_aim_control_direction_.ready() && (mouse.right || switch_right == Switch::UP)
-            && !auto_aim_control_direction_->isZero())
+        const auto auto_aim_requested = mouse_->right || switch_right == Switch::UP;
+        const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_;
+        const auto valid_control =
+            auto_aim_control_direction_.ready() && auto_aim_control_direction_->allFinite();
+
+        if (auto_aim_requested && should_control && valid_control) {
             return two_axis_gimbal_solver.update(
                 TwoAxisGimbalSolver::SetControlDirection(
                     OdomImu::DirectionVector(*auto_aim_control_direction_)));
+        }
 
         if (!two_axis_gimbal_solver.enabled())
             return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel());
@@ -67,7 +78,7 @@ class SimpleGimbalController
         double yaw_shift =
             joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y();
         double pitch_shift =
-            -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x();
+            -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x();
 
         return two_axis_gimbal_solver.update(
             TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift));
@@ -82,6 +93,7 @@ class SimpleGimbalController
     InputInterface<Eigen::Vector2d> mouse_velocity_;
     InputInterface<rmcs_msgs::Mouse> mouse_;
 
+    InputInterface<bool> auto_aim_should_control_;
     InputInterface<Eigen::Vector3d> auto_aim_control_direction_;
 
     TwoAxisGimbalSolver two_axis_gimbal_solver;
@@ -94,4 +106,4 @@ class SimpleGimbalController
 #include <pluginlib/class_list_macros.hpp>
 
 PLUGINLIB_EXPORT_CLASS(
-    rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component)
+    rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component)