Кэфы курса

Author: DaShakuev

docker/run.sh

@@ -5,7 +5,7 @@ set -e
 IMAGE_NAME="stingray_core"
 CONTAINER_NAME="stingray_core"
 
-xhost +si:localuser:root
+#xhost +si:localuser:root
 
 
 if [ "$(docker ps -a -q -f name=^/${CONTAINER_NAME}$)" ]; then

run_rov.sh

@@ -12,7 +12,6 @@ colcon build --packages-select \
     serial_driver \
     io_context \
     asio_cmake_module \
-    ms5837_pressure_sensor \
     vectornav_msgs \
     vectornav \
     stingray_core_launch

src/stingray_core_control/params/controllers.yaml

@@ -45,12 +45,12 @@ stingray_core_control_node:
     controllers.pitch.out_sat: 1.0
     controllers.pitch.ap_K: 1.0
     controllers.pitch.ap_T: 0.2
-    controllers.yaw.K_p: 1.2
-    controllers.yaw.K_1: 1.0
-    controllers.yaw.K_2: 0.3
-    controllers.yaw.K_i: 0.05
-    controllers.yaw.I_min: -0.3
-    controllers.yaw.I_max: 0.3
-    controllers.yaw.out_sat: 0.8
+    controllers.yaw.K_p: 0.6
+    controllers.yaw.K_1: 4.4
+    controllers.yaw.K_2: 1.4
+    controllers.yaw.K_i: 0.6
+    controllers.yaw.I_min: -0.01
+    controllers.yaw.I_max: 0.01
+    controllers.yaw.out_sat: 200.0
     controllers.yaw.ap_K: 1.0
-    controllers.yaw.ap_T: 0.15
+    controllers.yaw.ap_T: 0.0

src/stingray_core_control/params/stingray_core_control_node.yaml

@@ -5,7 +5,7 @@ stingray_core_control_node:
     topic_loop_flags: /control/loop_flags
     topic_imu_angular: /vectornav/raw/common/yawpitchroll
     topic_imu_linear_accel: /vectornav/imu_accel
-    topic_imu_angular_rate: /vectornav/imu_rate
+    topic_imu_angular_rate: /vectornav/imu
     thrusters:
     - depth_front_left
     - depth_front_right
@@ -93,3 +93,4 @@ stingray_core_control_node:
     - ap_T
     flag_setup_feedback_speed: false
     debug_publish: true
+    use_sim_time: false

src/stingray_core_control/stingray_core_control/controllers.py

@@ -98,6 +98,27 @@ def set_debug_hook(self, hook):
         """hook(dict) — вызывается, если debug включён"""
         self.debug_hook = hook
 
+    def apply_params(self, **kwargs):
+        """
+        Обновляет параметры контроллера "на лету".
+        Не трогает внутренние состояния, если явно не указано reset=True.
+        """
+        reset_required = False
+
+        for k, v in kwargs.items():
+            if not hasattr(self, k):
+                continue
+
+            # если меняем динамические параметры — нужен reset
+            if k in ("K_i", "ap_T", "ap_K"):
+                reset_required = True
+
+            setattr(self, k, float(v) if isinstance(v, (int, float)) else v)
+
+        if reset_required:
+            self.reset()
+
+
 
     @abstractmethod
     def update(self, setpoint: float, measurement: float, measurement_rate: float, dt: float) -> float:
@@ -115,7 +136,19 @@ def update(self, setpoint: float, measurement: float, measurement_rate: float, d
 # Angular controllers
 # -----------------------
 class YawController(BaseController):
-    def update(self, setpoint: float, measurement: float, measurement_rate: float, dt: float, flag_setup_feedback_speed: bool) -> float:
+    def update(
+        self,
+        setpoint: float,
+        measurement: float,
+        measurement_rate: float,
+        dt: float,
+        flag_setup_feedback_speed: bool,
+        param_update: dict | None = None
+    ) -> float:
+
+        if param_update:
+            self.apply_params(**param_update)
+
         setspeed = setpoint * flag_setup_feedback_speed
         if flag_setup_feedback_speed:
             output_pi = 0.0
@@ -137,8 +170,9 @@ def update(self, setpoint: float, measurement: float, measurement_rate: float, d
             output_pi = res_p + res_i
 
         # 6) aperiodic filter from rate (wz) and scale
-        ap = self.aperiodic_step(measurement_rate, dt)
+        ap = self.aperiodic_step(57.3 * measurement_rate, dt)
         feedback_speed = ap * self.K_2
+        # feedback_speed = 57.3 *measurement_rate * self.K_2
 
         # 7) final output (PI minus speed feedback) 
         error_speed = output_pi + setspeed - feedback_speed 
@@ -152,7 +186,7 @@ def update(self, setpoint: float, measurement: float, measurement_rate: float, d
         if self.debug_hook is not None:
             self.debug_hook({
                 "err_position": err_position,
-                "output_pi": output_pi,
+                "output_pi": 57.3 *measurement_rate,
                 "feedback_speed": feedback_speed,
                 "error_speed": error_speed,
                 "out": out,

src/stingray_core_control/stingray_core_control/stingray_core_control_node.py

@@ -374,7 +374,21 @@ def compute_pitch(self):
 
     def compute_yaw(self):
         dt = self.last_dt
-        return self.yaw_ctrl.update(self.impact_yaw, self.imu_yaw, self.imu_rate_z, dt, self.flag_setup_feedback_speed)
+        return self.yaw_ctrl.update(
+            self.impact_yaw,
+            self.imu_yaw,
+            self.imu_rate_z,
+            dt,
+            self.flag_setup_feedback_speed,
+            param_update={
+                "K_p": self.controllers["yaw"].K_p,
+                "K_i": self.controllers["yaw"].K_i,
+                "K_1": self.controllers["yaw"].K_1,
+                "K_2": self.controllers["yaw"].K_2,
+                "ap_T": self.controllers["yaw"].ap_T,
+                "ap_K": self.controllers["yaw"].ap_K,
+                "out_sat": self.controllers["yaw"].out_sat,
+            })
 
 
 

src/stingray_core_launch/launch/run_rov.launch.py

@@ -35,6 +35,6 @@ def generate_launch_description():
     return LaunchDescription([
         thruster_link,
         core_control,
-        ms5837,
+        # ms5837,
         vectornav
     ])