Merge pull request #1 from wsribunma/control

Fix stabilized mode + add control input type on GUI

Author: akmetko

cubs2_control/cubs2_control/autolevel_controller.py

@@ -43,15 +43,24 @@ class AutolevelController:
     mode: float = input_var(desc='mode: 0=manual, 1=stabilized')
 
     # === Parameters ===
-    # Outer loop: angle control (slow, 1-3 Hz)
-    Kp_phi: float = param(2.5, desc='P gain roll angle (per rad)')
-    Kp_theta: float = param(2.0, desc='P gain pitch angle (per rad)')
-
-    # Inner loop: rate damping (fast, 15-25 rad/s)
-    Kp_p: float = param(0.6, desc='P gain roll rate (per rad/s)')
-    Ki_p: float = param(0.0, desc='I gain roll rate')
-    Kp_q: float = param(0.6, desc='P gain pitch rate (per rad/s)')
-    Ki_q: float = param(0.0, desc='I gain pitch rate')
+    # Rate command sensitivity (how much stick deflection causes rate change)
+    # Stick-to-rate scaling (rad/s per full stick)
+    stick_rate_scale_phi: float = param(3.5 * np.pi, desc='stick to roll rate (rad/s)')
+    stick_rate_scale_theta: float = param(2.0 * np.pi, desc='stick to pitch rate (rad/s)')
+    
+    # Outer loop: auto-level gains (smooth return to level when stick centered)
+    Kp_phi: float = param(3.0, desc='P gain roll auto-level (per rad error)')
+    Kp_theta: float = param(2.0, desc='P gain pitch auto-level (per rad error)')
+
+    # Inner loop: rate tracking (gyro-based, fast stabilization)
+    Kp_p: float = param(0.5, desc='P gain roll rate tracking')
+    Ki_p: float = param(0.15, desc='I gain roll rate tracking')
+    Kp_q: float = param(0.3, desc='P gain pitch rate tracking')
+    Ki_q: float = param(0.1, desc='I gain pitch rate tracking')
+    
+    # Manual mode gyro damping (gentle - helps with oscillations)
+    Kd_p_manual: float = param(0.15, desc='damping roll rate in manual mode')
+    Kd_q_manual: float = param(0.15, desc='damping pitch rate in manual mode')
 
     # Yaw damping (passive)
     Kp_r: float = param(0.3, desc='P gain yaw rate damping')
@@ -65,10 +74,6 @@ class AutolevelController:
     trim_elevator: float = param(0.0, desc='elevator trim offset (rad)')
     trim_rudder: float = param(0.0, desc='rudder trim offset (rad)')
 
-    # Stick to attitude mapping
-    stick_to_phi: float = param(np.deg2rad(45), desc='aileron stick to roll ref (rad)')
-    stick_to_theta: float = param(np.deg2rad(20), desc='elevator stick to pitch ref (rad)')
-
     # Limits
     phi_max: float = param(np.deg2rad(50), desc='max bank angle (rad)')
     theta_max: float = param(np.deg2rad(30), desc='max pitch angle (rad)')
@@ -131,54 +136,89 @@ def autolevel_controller() -> Model:
     # Airspeed from velocity
     speed_meas = cy.norm_2(m.vel.sym)
 
-    # In stabilized mode, map stick inputs to attitude references
-    phi_ref_from_stick = m.ail_manual.sym * m.stick_to_phi.sym
-    theta_ref_from_stick = m.elev_manual.sym * m.stick_to_theta.sym
-
-    # Saturate reference angles to max limits
-    phi_ref_sat = _saturate(phi_ref_from_stick, -m.phi_max.sym, m.phi_max.sym)
-    theta_ref_sat = _saturate(theta_ref_from_stick, -m.theta_max.sym, m.theta_max.sym)
-
-    # Outer loop: angle errors -> rate commands
-    e_phi = phi_ref_sat - phi
-    e_theta = theta_ref_sat - theta
-
-    p_cmd = m.Kp_phi.sym * e_phi
-    q_cmd = m.Kp_theta.sym * e_theta
-
-    # Inner loop: rate errors
-    e_p = p_cmd - p_meas
-    e_q = q_cmd - q_meas
-
-    # ODEs: Integral states (only accumulate in stabilized mode)
+    # SAFE/AS3X Style Stabilization:
+    # In stabilized mode, stick commands RATE; autopilot auto-levels when stick centered
+    # This is rate-based stabilization + automatic leveling, not attitude hold
+    
+    # Stick input to desired rate commands (pilot commands rate directly)
+    p_cmd_stick = m.ail_manual.sym * m.stick_rate_scale_phi.sym
+    q_cmd_stick = m.elev_manual.sym * m.stick_rate_scale_theta.sym
+    
+    # Auto-level: when stick is centered, command returns to level attitude
+    # Proportional feedback from ACTUAL attitude error to drive back to level (0°)
+    # This ensures wings-level return regardless of current bank/pitch
+    p_cmd_level = -m.Kp_phi.sym * phi  # Roll error: always drives to phi=0 (wings-level)
+    q_cmd_level = -m.Kp_theta.sym * theta  # Pitch error: always drives to theta=0 (level)
+    
+    # Apply angle limits to STICK commands in stabilized mode (only limit pilot input)
+    # If near max bank angle, reduce stick authority to prevent exceeding limit
+    # Margin factor: 0.9 = start limiting at 90% of max angle
+    angle_margin = 0.9
+    phi_saturation_factor = cy.fmin(1.0, (m.phi_max.sym - cy.fabs(phi)) / (m.phi_max.sym * (1.0 - angle_margin)))
+    theta_saturation_factor = cy.fmin(1.0, (m.theta_max.sym - cy.fabs(theta)) / (m.theta_max.sym * (1.0 - angle_margin)))
+    
+    # Limit stick-commanded rates by saturation factor (soft limit on pilot input)
+    p_cmd_stick_limited = p_cmd_stick * phi_saturation_factor
+    q_cmd_stick_limited = q_cmd_stick * theta_saturation_factor
+    
+    # In stabilized mode: blend LIMITED stick rate commands with auto-level
+    # Auto-level is NEVER saturated - it always tries to level wings
+    # In manual mode: stick directly controls surfaces
+    p_cmd = p_cmd_stick_limited * (1.0 - m.mode.sym) + (p_cmd_stick_limited + p_cmd_level) * m.mode.sym
+    q_cmd = q_cmd_stick_limited * (1.0 - m.mode.sym) + (q_cmd_stick_limited + q_cmd_level) * m.mode.sym
+
+    # Inner loop: rate tracking (gyro-based rate damping)
+    # Always active in stabilized mode to track commanded rates
+    e_p = (p_cmd - p_meas) * m.mode.sym  # Rate error (only in stabilized)
+    e_q = (q_cmd - q_meas) * m.mode.sym  # Rate error (only in stabilized)
+    
+    # In manual mode, provide some gyro damping to improve handling
+    e_p_manual = -m.Kp_p.sym * p_meas  # Damping proportional to rate
+    e_q_manual = -m.Kp_q.sym * q_meas  # Damping proportional to rate
+
+    # ODEs: Integral states for rate tracking (only in stabilized mode)
     model.ode(m.i_p, e_p * m.mode.sym)
     model.ode(m.i_q, e_q * m.mode.sym)
 
-    # Stabilized mode control outputs
+    # Stabilized mode control outputs (rate-based)
+    # PID rate controller to track commanded rates
+    ail_attitude_fb = -1.5 * phi  # Direct roll error feedback (only for roll leveling)
+    
     ail_stabilized = _saturate(
-        m.Kp_p.sym * e_p + m.Ki_p.sym * m.i_p.sym,
+        m.Kp_p.sym * e_p + m.Ki_p.sym * m.i_p.sym + ail_attitude_fb,
         m.ail_min.sym, m.ail_max.sym
     )
     elev_stabilized = _saturate(
         m.Kp_q.sym * e_q + m.Ki_q.sym * m.i_q.sym,
         m.elev_min.sym, m.elev_max.sym
     )
+    
+    # Manual mode outputs (direct pass-through + gentle gyro damping)
+    ail_manual_out = m.ail_manual.sym + e_p_manual
+    elev_manual_out = m.elev_manual.sym + e_q_manual
     rud_stabilized = _saturate(-m.Kp_r.sym * r_meas, m.rud_min.sym, m.rud_max.sym)
     e_speed = m.speed_ref.sym - speed_meas
     thr_stabilized = _saturate(m.Kp_speed.sym * e_speed, m.thr_min.sym, m.thr_max.sym)
 
-    # Mode switch: 0 = manual (pass-through), 1 = stabilized (autopilot)
-    # Apply trim offsets to all outputs
+    # Mode switch: 0 = manual (direct + damping), 1 = stabilized (rate-based auto-level)
+    # Aileron/Elevator: switch between manual and stabilized
     ail_out = (
-        m.ail_manual.sym * (1.0 - m.mode.sym) + ail_stabilized * m.mode.sym
+        _saturate(ail_manual_out, m.ail_min.sym, m.ail_max.sym) * (1.0 - m.mode.sym) +
+        ail_stabilized * m.mode.sym
     ) + m.trim_aileron.sym
+    
     elev_out = (
-        m.elev_manual.sym * (1.0 - m.mode.sym) + elev_stabilized * m.mode.sym
+        _saturate(elev_manual_out, m.elev_min.sym, m.elev_max.sym) * (1.0 - m.mode.sym) +
+        elev_stabilized * m.mode.sym
     ) + m.trim_elevator.sym
+    
+    # Rudder: yaw damping in both modes
     rud_out = (
         m.rud_manual.sym * (1.0 - m.mode.sym) + rud_stabilized * m.mode.sym
     ) + m.trim_rudder.sym
-    thr_out = m.thr_manual.sym * (1.0 - m.mode.sym) + thr_stabilized * m.mode.sym
+    
+    # Throttle: pilot always controls throttle directly
+    thr_out = m.thr_manual.sym
 
     # Define outputs
     model.output(m.ail, ail_out)

cubs2_control/cubs2_control/gamepad_control.py

@@ -159,11 +159,17 @@ def __init__(self):
         self.sub_joy = self.create_subscription(
             Joy, '/joy', self.joy_callback, qos_profile)
 
+        # Subscribe to external control messages (from RViz dropdown, etc.)
+        # to sync mode changes from other sources
+        self.sub_control = self.create_subscription(
+            AircraftControl, '/control', self.control_callback, 10)
+
         # Current state
         self.aileron = 0.0
         self.elevator = 0.0
         self.throttle = self.throttle_default
         self.rudder = 0.0
+        self.mode = 0  # 0 = manual, 1 = stabilized
 
         # Trim values (applied as offsets to stick inputs)
         self.trim_aileron = 0.0
@@ -217,6 +223,8 @@ def __init__(self):
             f'  Button {self.btn_trim_rud_left} (X): Trim rudder left')
         self.get_logger().info(
             f'  Button {self.btn_trim_rud_right} (Y): Trim rudder right')
+        self.get_logger().info(
+            f'  Button {self.btn_right_bumper}: Toggle flight mode (manual/stabilized)')
         dpad_type = (
             'buttons' if self.dpad_is_buttons else f'axes ({
                 self.axis_dpad_h}, {
@@ -509,6 +517,12 @@ def get_axis(index: int) -> float:
                 else:
                     self.trim_hold_count[self.btn_dpad_right] = 0
 
+            # Button: Toggle flight mode (manual/stabilized)
+            if self._button_pressed(msg, self.btn_right_bumper):
+                self.mode = 1 - self.mode  # Toggle between 0 and 1
+                mode_name = 'STABILIZED' if self.mode == 1 else 'MANUAL'
+                self.get_logger().info(f'Flight mode changed to: {mode_name}')
+
             # Button: Exit node
             if self._button_pressed(msg, self.btn_exit):
                 self.get_logger().info('Exit button pressed, shutting down...')
@@ -583,6 +597,20 @@ def get_axis(index: int) -> float:
         # Publish control messages
         self.publish_controls()
 
+    def control_callback(self, msg: AircraftControl):
+        """
+        Listen for external control messages (e.g., from RViz dropdown).
+        
+        Syncs mode changes from other sources so both gamepad and dropdown
+        control the same mode.
+        """
+        # Update mode if it changed externally (e.g., from RViz dropdown)
+        external_mode = int(msg.mode)
+        if external_mode != self.mode:
+            self.mode = external_mode
+            mode_name = 'STABILIZED' if self.mode == 1 else 'MANUAL'
+            self.get_logger().info(f'Flight mode synced to: {mode_name}')
+
     def publish_controls(self):
         """Publish current control state as AircraftControl message."""
         msg = AircraftControl()
@@ -591,6 +619,7 @@ def publish_controls(self):
         msg.elevator = float(self.elevator)
         msg.throttle = float(self.throttle)
         msg.rudder = float(self.rudder)
+        msg.mode = int(self.mode)
         self.pub_control.publish(msg)
 
 

cubs2_control/cubs2_control/wings_level_controller.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+# Copyright 2025 CogniPilot Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+"""Wings-level inner loop stabilization controller."""
+from beartype import beartype
+from cyecca.dynamics.explicit import explicit
+from cyecca.dynamics.explicit import input_var
+from cyecca.dynamics.explicit import Model
+from cyecca.dynamics.explicit import output_var
+from cyecca.dynamics.explicit import param
+import cyecca.sym as cy
+import numpy as np
+
+
+@explicit
+class WingsLevelController:
+    """Inner loop wings-level stabilization - keeps aircraft wings level."""
+
+    # === Inputs ===
+    q: float = input_var(4, desc='quaternion [w,x,y,z]')
+    omega: float = input_var(3, desc='angular velocity body frame (rad/s)')
+    ail_manual: float = input_var(desc='manual aileron (rad)')
+    elev_manual: float = input_var(desc='manual elevator (rad)')
+    rud_manual: float = input_var(desc='manual rudder (rad)')
+    thr_manual: float = input_var(desc='manual throttle')
+
+    # === Parameters ===
+    # Roll stabilization (P control on roll angle error)
+    Kp_phi: float = param(2.0, desc='P gain roll angle control')
+    
+    # Manual mode gyro damping (gentle - helps with oscillations)
+    Kp_p: float = param(0.15, desc='damping roll rate in manual mode')
+    Kp_q: float = param(0.15, desc='damping pitch rate in manual mode')
+
+    # Yaw damping (passive)
+    Kp_r: float = param(0.3, desc='P gain yaw rate damping')
+
+    # Trim offsets
+    trim_aileron: float = param(0.0, desc='aileron trim offset (rad)')
+    trim_elevator: float = param(0.0, desc='elevator trim offset (rad)')
+    trim_rudder: float = param(0.0, desc='rudder trim offset (rad)')
+
+    # Limits
+    ail_min: float = param(-0.5, desc='ail min (rad)')
+    ail_max: float = param(0.5, desc='ail max (rad)')
+    elev_min: float = param(-0.5, desc='elev min (rad)')
+    elev_max: float = param(0.5, desc='elev max (rad)')
+    rud_min: float = param(-0.5, desc='rud min (rad)')
+    rud_max: float = param(0.5, desc='rud max (rad)')
+    thr_min: float = param(0.0, desc='thr min')
+    thr_max: float = param(1.0, desc='thr max')
+
+    # === Outputs ===
+    ail: float = output_var(desc='aileron (rad)')
+    elev: float = output_var(desc='elevator (rad)')
+    rud: float = output_var(desc='rudder (rad)')
+    thr: float = output_var(desc='throttle')
+
+
+def _saturate(val, low, high):
+    """Saturate value between low and high."""
+    return cy.fmin(cy.fmax(val, low), high)
+
+
+@beartype
+def wings_level_controller() -> Model:
+    """
+    Create wings-level inner loop stabilization controller.
+    
+    Keeps aircraft wings level by applying proportional feedback on roll angle.
+    Pitch and throttle are stick pass-through.
+
+    Returns
+    -------
+    Model
+        Wings-level controller model
+
+    """
+    model = Model(WingsLevelController)
+    m = model.v  # Unified namespace
+
+    # Extract quaternion and compute Euler angles
+    qw = m.q.sym[0]
+    qx = m.q.sym[1]
+    qy = m.q.sym[2]
+    qz = m.q.sym[3]
+
+    # Roll (phi) - aerospace ZYX sequence
+    phi = cy.atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy))
+
+    # Extract angular rates
+    p_meas = m.omega.sym[0]
+    q_meas = m.omega.sym[1]
+    r_meas = m.omega.sym[2]
+
+    # Roll angle error from level (phi = 0 is wings level)
+    phi_error = phi
+
+    # Aileron: P controller on roll angle error to bring wings to level
+    ail_stabilized = _saturate(
+        -m.Kp_phi.sym * phi_error,  # Negative feedback: positive phi error → negative aileron
+        m.ail_min.sym, m.ail_max.sym
+    )
+
+    # Elevator: stick pass-through with rate damping
+    e_q_manual = -m.Kp_q.sym * q_meas  # Gentle gyro damping
+    elev_manual_out = m.elev_manual.sym + e_q_manual
+    elev_out = _saturate(elev_manual_out, m.elev_min.sym, m.elev_max.sym) + m.trim_elevator.sym
+
+    # Rudder: passive yaw damping
+    rud_stabilized = _saturate(-m.Kp_r.sym * r_meas, m.rud_min.sym, m.rud_max.sym)
+
+    # Throttle: pilot always controls throttle directly
+    thr_out = m.thr_manual.sym
+
+    # Define outputs
+    model.output(m.ail, ail_stabilized + m.trim_aileron.sym)
+    model.output(m.elev, elev_out)
+    model.output(m.rud, rud_stabilized + m.trim_rudder.sym)
+    model.output(m.thr, thr_out)
+
+    model.build()
+    return model
+
+
+__all__ = [
+    'wings_level_controller',
+    'WingsLevelController',
+]