Replace override_rc_in with manual_control

.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 = [email protected]:CWRUbotix/ros2_video_streamer.git
+[submodule "src/pi/mavros"]
+	path = src/pi/mavros
+	url = https://github.com/mavlink/mavros.git

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,7 +1,7 @@
 from collections.abc import Callable
 
 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
@@ -17,8 +17,8 @@
 
 # Range of values Pixhawk takes
 # In microseconds
-ZERO_SPEED = 1500
-MAX_RANGE_SPEED = 400
+ZERO_SPEED = 0
+MAX_RANGE_SPEED = 2000
 RANGE_SPEED = MAX_RANGE_SPEED * SPEED_THROTTLE
 
 # Channels for RC command
@@ -58,33 +58,36 @@ def __init__(self) -> None:
         )
 
         self.rc_pub = self.create_publisher(
-            OverrideRCIn, 'mavros/rc/override', QoSPresetProfiles.DEFAULT.value
+            ManualControl, 'mavros/manual_control/send', QoSPresetProfiles.DEFAULT.value
         )
 
     @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.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()
+        rc_msg = ManualControl()
 
         # Maps to PWM
         MultiplexerNode.apply(msg, lambda value: int(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
+        rc_msg.x = float(msg.forward)
+        rc_msg.z = (
+            float(msg.vertical) * (1000 * SPEED_THROTTLE) + 500
+        )  # To account for different z limits
+        rc_msg.y = float(msg.lateral)
+        rc_msg.r = float(msg.yaw)
+        rc_msg.enabled_extensions = 0b00000011
+        rc_msg.s = float(msg.pitch)
+        rc_msg.t = float(msg.roll)
 
         return rc_msg
 
@@ -113,7 +116,7 @@ def control_callback(self, msg: PixhawkInstruction) -> None:
         else:
             return
 
-        self.rc_pub.publish(msg=self.to_override_rc_in(msg))
+        self.rc_pub.publish(msg=self.to_manual_control(msg))
 
 
 def main() -> None:

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,8 @@
 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,
+    SPEED_THROTTLE,
     ZERO_SPEED,
     MultiplexerNode,
 )
@@ -35,14 +30,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 == (500 + 1000 * SPEED_THROTTLE)
+    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)