add pitch

Author: shakuevda

src/stingray_core_control/stingray_core_control/controllers.py

@@ -186,7 +186,7 @@ def update(
         if self.debug_hook is not None:
             self.debug_hook({
                 "err_position": err_position,
-                "output_pi": 57.3 *measurement_rate,
+                "output_pi": output_pi,
                 "feedback_speed": feedback_speed,
                 "measurement_rate": 57.3 *measurement_rate,
                 "out": out,
@@ -195,33 +195,132 @@ def update(
         return out
 
 class PitchController(BaseController):
-    def update(self, setpoint: float, measurement: float, measurement_rate: float, dt: float) -> float:
-        # same structure as yaw but error normalized for angles
-        err = normalize_angle_deg(setpoint - measurement)
-        stage = err * self.K_1
-        res_p = stage * self.K_p
-        res_i = self.trapezoidal_integrate(stage * self.K_i, dt)
-        output_pi = res_p + res_i
+    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)
+
+        dt = max(min(dt, 0.05), 1e-3)
+
+        # --- позиционный контур ---
+        if flag_setup_feedback_speed:
+            err_position = 0.0
+            output_pi = 0.0
+            setspeed = setpoint
+        else:
+            err_position = normalize_angle_deg(setpoint - measurement)
+            stage = err_position * self.K_1
+            res_p = stage * self.K_p
+            res_i = self.trapezoidal_integrate(stage * self.K_i, dt)
+            output_pi = res_p + res_i
+            setspeed = 0.0
+
+        # --- ОС по скорости ---
         ap = self.aperiodic_step(measurement_rate, dt)
         feedback_speed = ap * self.K_2
-        out = output_pi - feedback_speed
+
+        error_speed = output_pi + setspeed - feedback_speed
+
         if self.out_sat is not None:
-            out = saturation(out, self.out_sat, -self.out_sat)
+            out = saturation(error_speed, self.out_sat, -self.out_sat)
+        else:
+            out = error_speed
+
+        # --- DEBUG ---
+        if self.debug_hook is not None:
+            self.debug_hook({
+                "err_position": err_position,
+                "output_pi": output_pi,
+                "measurement_rate": measurement_rate,
+                "feedback_speed": feedback_speed,
+                "error_speed": error_speed,
+                "out": out,
+            })
+
         return out
 
 class RollController(BaseController):
-    def update(self, setpoint: float, measurement: float, measurement_rate: float, dt: float) -> float:
-        # same as pitch
-        err = normalize_angle_deg(setpoint - measurement)
-        stage = err * self.K_1
-        res_p = stage * self.K_p
-        res_i = self.trapezoidal_integrate(stage * self.K_i, dt)
-        output_pi = res_p + res_i
+    def __init__(
+        self,
+        *args,
+        grav_bias: float = 0.0,
+        grav_gain: float = 0.0,
+        grav_offset_deg: float = 0.0,
+        **kwargs
+    ):
+        super().__init__(*args, **kwargs)
+        self.grav_bias = grav_bias
+        self.grav_gain = grav_gain
+        self.grav_offset_deg = grav_offset_deg
+
+    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)
+
+        dt = max(min(dt, 0.05), 1e-3)
+
+        # -------- режим настройки скорости --------
+        if flag_setup_feedback_speed:
+            output_pi = 0.0
+            err_position = 0.0
+            setspeed = setpoint
+        else:
+            # -------- позиционный контур --------
+            err_position = normalize_angle_deg(setpoint - measurement)
+            stage = err_position * self.K_1
+            res_p = stage * self.K_p
+            res_i = self.trapezoidal_integrate(stage * self.K_i, dt)
+            output_pi = res_p + res_i
+            setspeed = 0.0
+
+        # -------- гравитационная компенсация --------
+        grav = (
+            self.grav_bias
+            + math.sin(math.radians(measurement - self.grav_offset_deg))
+            * self.grav_gain
+        )
+
+        # -------- ОС по скорости --------
         ap = self.aperiodic_step(measurement_rate, dt)
         feedback_speed = ap * self.K_2
-        out = output_pi - feedback_speed
+
+        # -------- итог --------
+        error_speed = output_pi + grav + setspeed - feedback_speed
+
         if self.out_sat is not None:
-            out = saturation(out, self.out_sat, -self.out_sat)
+            out = saturation(error_speed, self.out_sat, -self.out_sat)
+        else:
+            out = error_speed
+
+        if self.debug_hook is not None:
+            self.debug_hook({
+                "err_position": err_position,
+                "output_pi": output_pi,
+                "measurement_rate": measurement_rate,
+                "feedback_speed": feedback_speed,
+                "grav": grav,
+                "error_speed": error_speed,
+                "out": out,
+            })
+
+
         return out
 
 # -----------------------

src/stingray_core_control/stingray_core_control/stingray_core_control_node.py

@@ -223,7 +223,18 @@ def __init__(self):
             ap_T=self.controllers['heave'].ap_T
         )
 
-        # self.pitch_ctrl = PitchController(Kp=1.0, K_stage=1.0, out_sat=100.0, ap_K=1.0, ap_T=0.1)
+        self.pitch_ctrl = PitchController(
+            K_p=self.controllers['heave'].K_p,
+            K_1=self.controllers['heave'].K_1,
+            K_2=self.controllers['heave'].K_2,
+            K_i=self.controllers['heave'].K_i,
+            I_max=self.controllers['heave'].I_max,
+            I_min=self.controllers['heave'].I_min,
+            out_sat=self.controllers['heave'].out_sat,
+            ap_K=self.controllers['heave'].ap_K,
+            ap_T=self.controllers['heave'].ap_T
+        )
+
         # self.roll_ctrl = RollController(Kp=1.0, K_stage=1.0, out_sat=100.0, ap_K=1.0, ap_T=0.1)
         # self.surge_ctrl = SurgeController(Kp=1.0, out_sat=100.0)
         # self.sway_ctrl = SwayController(Kp=1.0, out_sat=100.0)
@@ -392,11 +403,25 @@ def compute_heave(self):
 
     def compute_roll(self):
         dt = self.last_dt
-        return self.roll_ctrl.update(self.impact_roll, self.imu_roll, self.imu_rate_x, dt)
+        # return self.roll_ctrl.update(self.impact_roll, self.imu_roll, self.imu_rate_x, dt)
 
     def compute_pitch(self):
         dt = self.last_dt
-        return self.pitch_ctrl.update(self.impact_pitch, self.imu_pitch, self.imu_rate_y, dt)
+        return self.pitch_ctrl.update(
+            self.impact_pitch,
+            self.imu_pitch,
+            self.imu_rate_y,
+            dt,
+            self.flag_setup_feedback_speed,
+            param_update={
+                "K_p": self.controllers["pitch"].K_p,
+                "K_i": self.controllers["pitch"].K_i,
+                "K_1": self.controllers["pitch"].K_1,
+                "K_2": self.controllers["pitch"].K_2,
+                "ap_T": self.controllers["pitch"].ap_T,
+                "ap_K": self.controllers["pitch"].ap_K,
+                "out_sat": self.controllers["pitch"].out_sat,
+            })
 
     def compute_yaw(self):
         dt = self.last_dt