Smooth thrusters

.github/workflows/industrial_ci_action.yml

@@ -27,6 +27,8 @@ jobs:
       CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci)
     steps:
       - uses: actions/checkout@v4 # clone target repository
+        with:
+          submodules: recursive
       - uses: actions/cache@v4 # fetch/store the directory used by ccache before/after the ci run
         with:
           path: ${{ env.CCACHE_DIR }}

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "src/surface/ros2_video_streamer"]
 	path = src/surface/ros2_video_streamer
 	url = git@github.com:CWRUbotix/ros2_video_streamer.git
+[submodule "src/pi/mavros"]
+	path = src/pi/mavros
+	url = https://github.com/mavlink/mavros.git

pyproject.toml

@@ -24,7 +24,7 @@ line-length = 100
 flake8-quotes.inline-quotes='single'
 extend-select = ["ALL"]
 fixable = ["ALL"]
-ignore = ["D100", "D101", "D102", "D103", "D104", "D107", "T201", "FIX002", "TD003", "TD002", "TRY003", "EM101", "EM102", "RET504", "D211", "COM812", "ISC001", "ERA001", "S602", "S603", "D205"]
+ignore = ["D100", "D101", "D102", "D103", "D104", "D107", "T201", "FIX002", "TD003", "TD002", "TRY003", "EM101", "EM102", "RET504", "D211", "COM812", "ISC001", "ERA001", "S602", "S603", "D205", "UP040"]
 
 [tool.ruff.lint.pydocstyle]
 convention = "numpy"

src/pi/pixhawk_communication/launch/mavros_launch.py

@@ -26,12 +26,12 @@ def generate_launch_description() -> LaunchDescription:
             # receive a signal from a GCS.
             {'system_id': 255},
             # plugin_allowlist allows which mavros nodes get launched. The default is all of them.
-            {'plugin_allowlist': ['sys_status', 'rc_io', 'command']},
+            {'plugin_allowlist': ['sys_status', 'rc_io', 'command', 'manual_control']},
             {'fcu_url': '/dev/ttyPixhawk'},
         ],
         remappings=[
             ('/pi/mavros/state', '/tether/mavros/state'),
-            ('/pi/mavros/rc/override', '/tether/mavros/rc/override'),
+            ('/pi/mavros/manual_control/send', '/tether/mavros/manual_control/send'),
             ('/pi/mavros/cmd/arming', '/tether/mavros/cmd/arming'),
             ('/pi/mavros/cmd/command', '/tether/mavros/cmd/command'),
         ],

src/surface/flight_control/flight_control/multiplexer.py

@@ -1,25 +1,35 @@
-from collections.abc import Callable
+from typing import Final
 
 import rclpy
-from mavros_msgs.msg import OverrideRCIn
+from mavros_msgs.msg import ManualControl
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.qos import QoSPresetProfiles
 
+from flight_control.pixhawk_instruction_utils import (
+    apply_function,
+    pixhawk_instruction_to_tuple,
+    tuple_to_pixhawk_instruction,
+)
 from rov_msgs.msg import PixhawkInstruction
 from rov_msgs.srv import AutonomousFlight
 
 # Brown out protection
-SPEED_THROTTLE = 0.85
+SPEED_THROTTLE = 0.65
 
 # Joystick curve
 JOYSTICK_EXPONENT = 3
 
 # Range of values Pixhawk takes
 # In microseconds
-ZERO_SPEED = 1500
-MAX_RANGE_SPEED = 400
-RANGE_SPEED = MAX_RANGE_SPEED * SPEED_THROTTLE
+ZERO_SPEED: Final = 0
+Z_ZERO_SPEED: Final = 500
+MAX_RANGE_SPEED: Final = 2000
+Z_MAX_RANGE_SPEED: Final = 1000
+RANGE_SPEED: Final = MAX_RANGE_SPEED * SPEED_THROTTLE
+Z_RANGE_SPEED: Final = Z_MAX_RANGE_SPEED * SPEED_THROTTLE
+
+EXTENSIONS_CODE: Final = 0b00000011
 
 # Channels for RC command
 MAX_CHANNEL = 8
@@ -32,6 +42,9 @@
 YAW_CHANNEL = 3  # Yaw
 ROLL_CHANNEL = 1  # Roll
 
+NEXT_INSTR_FRAC: Final[float] = 0.05
+PREV_INSTR_FRAC: Final[float] = 1 - NEXT_INSTR_FRAC
+
 
 def joystick_map(raw: float) -> float:
     mapped = abs(raw) ** JOYSTICK_EXPONENT
@@ -40,6 +53,10 @@ def joystick_map(raw: float) -> float:
     return mapped
 
 
+def manual_control_map(value: float) -> float:
+    return RANGE_SPEED * value + ZERO_SPEED
+
+
 class MultiplexerNode(Node):
     def __init__(self) -> None:
         super().__init__('multiplexer', parameter_overrides=[])
@@ -57,36 +74,69 @@ def __init__(self) -> None:
             QoSPresetProfiles.DEFAULT.value,
         )
 
-        self.rc_pub = self.create_publisher(
-            OverrideRCIn, 'mavros/rc/override', QoSPresetProfiles.DEFAULT.value
+        self.mc_pub = self.create_publisher(
+            ManualControl, 'mavros/manual_control/send', QoSPresetProfiles.DEFAULT.value
         )
 
+        self.previous_instruction = PixhawkInstruction(author=PixhawkInstruction.MANUAL_CONTROL)
+
     @staticmethod
-    def apply(msg: PixhawkInstruction, function_to_apply: Callable[[float], float]) -> None:
-        """Apply a function to each dimension of this PixhawkInstruction."""
-        msg.forward = function_to_apply(msg.forward)
-        msg.vertical = function_to_apply(msg.vertical)
-        msg.lateral = function_to_apply(msg.lateral)
-        msg.pitch = function_to_apply(msg.pitch)
-        msg.yaw = function_to_apply(msg.yaw)
-        msg.roll = function_to_apply(msg.roll)
+    def smooth_value(prev_value: float, next_value: float) -> float:
+        return PREV_INSTR_FRAC * prev_value + NEXT_INSTR_FRAC * next_value
+
+    def smooth_pixhawk_instruction(self, msg: PixhawkInstruction) -> PixhawkInstruction:
+        instruction_tuple = pixhawk_instruction_to_tuple(msg)
+        previous_instruction_tuple = pixhawk_instruction_to_tuple(self.previous_instruction)
+
+        instruction_tuple = tuple(
+            MultiplexerNode.smooth_value(previous_value, value)
+            for (previous_value, value) in zip(
+                previous_instruction_tuple, instruction_tuple, strict=True
+            )
+        )
+        smoothed_instruction = tuple_to_pixhawk_instruction(instruction_tuple, msg.author)
+
+        self.previous_instruction = smoothed_instruction
+
+        return smoothed_instruction
+
+    # def apply(msg: PixhawkInstruction, function_to_apply: Callable[[float], float]) -> None:
+    #     """Apply a function to each dimension of this PixhawkInstruction."""
+    #     msg.forward = function_to_apply(msg.forward)
+    #     msg.vertical = msg.vertical
+    #     msg.lateral = function_to_apply(msg.lateral)
+    #     msg.pitch = function_to_apply(msg.pitch)
+    #     msg.yaw = function_to_apply(msg.yaw)
+    #     msg.roll = function_to_apply(msg.roll)
 
     @staticmethod
-    def to_override_rc_in(msg: PixhawkInstruction) -> OverrideRCIn:
+    def to_manual_control(msg: PixhawkInstruction) -> ManualControl:
         """Convert this PixhawkInstruction to an rc_msg with the appropriate channels array."""
-        rc_msg = OverrideRCIn()
+        mc_msg = ManualControl()
 
         # Maps to PWM
-        MultiplexerNode.apply(msg, lambda value: int(RANGE_SPEED * value) + ZERO_SPEED)
+        # instruction_tuple = pixhawk_instruction_to_tuple(msg)
+        # instruction_tuple = tuple(manual_control_map(value) for value in instruction_tuple)
+        # mapped_msg = tuple_to_pixhawk_instruction(instruction_tuple)
+        mapped_msg = apply_function(msg, manual_control_map)
+
+        # To account for different z limits
+        mapped_msg.vertical = Z_RANGE_SPEED * msg.vertical + Z_ZERO_SPEED
+
+        # MultiplexerNode.apply(msg, lambda value: (RANGE_SPEED * value) + ZERO_SPEED)
 
-        rc_msg.channels[FORWARD_CHANNEL] = msg.forward
-        rc_msg.channels[THROTTLE_CHANNEL] = msg.vertical
-        rc_msg.channels[LATERAL_CHANNEL] = msg.lateral
-        rc_msg.channels[PITCH_CHANNEL] = msg.pitch
-        rc_msg.channels[YAW_CHANNEL] = msg.yaw
-        rc_msg.channels[ROLL_CHANNEL] = msg.roll
+        mc_msg.x = mapped_msg.forward
+        mc_msg.z = mapped_msg.vertical
+        # (
+        #     Z_RANGE_SPEED * mapped_msg.vertical
+        # ) + Z_ZERO_SPEED  # To account for different z limits
+        mc_msg.y = mapped_msg.lateral
+        mc_msg.r = mapped_msg.yaw
+        mc_msg.enabled_extensions = EXTENSIONS_CODE
+        mc_msg.s = mapped_msg.pitch
+        mc_msg.t = mapped_msg.roll
 
-        return rc_msg
+        return mc_msg
 
     def state_control(
         self, req: AutonomousFlight.Request, res: AutonomousFlight.Response
@@ -102,7 +152,10 @@ def control_callback(self, msg: PixhawkInstruction) -> None:
         ):
             # Smooth out adjustments
             # TODO: look into maybe doing inheritance on a PixhawkInstruction
-            MultiplexerNode.apply(msg, joystick_map)
+            # instruction_tuple = pixhawk_instruction_to_tuple(msg)
+            # instruction_tuple = tuple(joystick_map(value) for value in instruction_tuple)
+            # msg = tuple_to_pixhawk_instruction(instruction_tuple)
+            msg = apply_function(msg, joystick_map)
         elif (
             msg.author == PixhawkInstruction.KEYBOARD_CONTROL
             and self.state == AutonomousFlight.Request.STOP
@@ -113,7 +166,8 @@ def control_callback(self, msg: PixhawkInstruction) -> None:
         else:
             return
 
-        self.rc_pub.publish(msg=self.to_override_rc_in(msg))
+        smoothed_instruction = self.smooth_pixhawk_instruction(msg)
+        self.mc_pub.publish(self.to_manual_control(smoothed_instruction))
 
 
 def main() -> None:

src/surface/flight_control/flight_control/pixhawk_instruction_utils.py

@@ -0,0 +1,29 @@
+from collections.abc import Callable
+
+from rov_msgs.msg import PixhawkInstruction
+
+
+def pixhawk_instruction_to_tuple(msg: PixhawkInstruction) -> tuple[float, ...]:
+    return (msg.forward, msg.vertical, msg.lateral, msg.pitch, msg.yaw, msg.roll)
+
+
+def tuple_to_pixhawk_instruction(
+    instruction_tuple: tuple[float, ...], author: int = PixhawkInstruction.MANUAL_CONTROL
+) -> PixhawkInstruction:
+    return PixhawkInstruction(
+        forward=instruction_tuple[0],
+        lateral=instruction_tuple[1],
+        vertical=instruction_tuple[2],
+        roll=instruction_tuple[3],
+        pitch=instruction_tuple[4],
+        yaw=instruction_tuple[5],
+        author=author,
+    )
+
+
+def apply_function(
+    msg: PixhawkInstruction, function_to_apply: Callable[[float], float]
+) -> PixhawkInstruction:
+    instruction_tuple = pixhawk_instruction_to_tuple(msg)
+    instruction_tuple = tuple(function_to_apply(value) for value in instruction_tuple)
+    return tuple_to_pixhawk_instruction(instruction_tuple, msg.author)

src/surface/flight_control/launch/flight_control_launch.py

@@ -35,7 +35,7 @@ def generate_launch_description() -> LaunchDescription:
     multiplexer_node = Node(
         package='flight_control',
         executable='multiplexer_node',
-        remappings=[('/surface/mavros/rc/override', '/tether/mavros/rc/override')],
+        remappings=[('/surface/mavros/manual_control/send', '/tether/mavros/manual_control/send')],
         emulate_tty=True,
         output='screen',
     )

src/surface/flight_control/test/test_manual_control.py

@@ -1,13 +1,9 @@
 import rclpy
 from flight_control.manual_control_node import ManualControlNode
 from flight_control.multiplexer import (
-    FORWARD_CHANNEL,
-    LATERAL_CHANNEL,
-    PITCH_CHANNEL,
     RANGE_SPEED,
-    ROLL_CHANNEL,
-    THROTTLE_CHANNEL,
-    YAW_CHANNEL,
+    Z_RANGE_SPEED,
+    Z_ZERO_SPEED,
     ZERO_SPEED,
     MultiplexerNode,
 )
@@ -35,14 +31,14 @@ def test_joystick_profiles() -> None:
         roll=0.92,
     )
 
-    msg = MultiplexerNode.to_override_rc_in(instruction)
+    msg = MultiplexerNode.to_manual_control(instruction)
 
-    assert msg.channels[FORWARD_CHANNEL] == ZERO_SPEED
-    assert msg.channels[THROTTLE_CHANNEL] == (ZERO_SPEED + RANGE_SPEED)
-    assert msg.channels[LATERAL_CHANNEL] == (ZERO_SPEED - RANGE_SPEED)
+    assert msg.x == ZERO_SPEED
+    assert msg.z == (Z_ZERO_SPEED + Z_RANGE_SPEED)
+    assert msg.y == (ZERO_SPEED - RANGE_SPEED)
 
     # 1539 1378
 
-    assert msg.channels[PITCH_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * 0.34)
-    assert msg.channels[YAW_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * -0.6)
-    assert msg.channels[ROLL_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * 0.92)
+    assert msg.s == ZERO_SPEED + int(RANGE_SPEED * 0.34)
+    assert msg.r == ZERO_SPEED + int(RANGE_SPEED * -0.6)
+    assert msg.t == ZERO_SPEED + int(RANGE_SPEED * 0.92)