diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index b4e5b46f..1217b6b1 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.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 }} diff --git a/.gitmodules b/.gitmodules index feec4278..ee8e7889 100644 --- a/.gitmodules +++ b/.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 diff --git a/src/pi/mavros b/src/pi/mavros new file mode 160000 index 00000000..b4909572 --- /dev/null +++ b/src/pi/mavros @@ -0,0 +1 @@ +Subproject commit b49095727a6ff160e1e913b90a4ce50e383e8863 diff --git a/src/pi/pixhawk_communication/launch/mavros_launch.py b/src/pi/pixhawk_communication/launch/mavros_launch.py index 8d5d6e8e..d6ed782d 100644 --- a/src/pi/pixhawk_communication/launch/mavros_launch.py +++ b/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'), ], diff --git a/src/surface/flight_control/flight_control/multiplexer.py b/src/surface/flight_control/flight_control/multiplexer.py index df52e0ea..edd22167 100644 --- a/src/surface/flight_control/flight_control/multiplexer.py +++ b/src/surface/flight_control/flight_control/multiplexer.py @@ -1,7 +1,8 @@ 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 @@ -17,9 +18,14 @@ # 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 @@ -57,36 +63,39 @@ 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 ) @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() + mc_msg = ManualControl() # Maps to PWM - MultiplexerNode.apply(msg, lambda value: int(RANGE_SPEED * value) + 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 = msg.forward + mc_msg.z = ( + Z_RANGE_SPEED * msg.vertical + ) + Z_ZERO_SPEED # To account for different z limits + mc_msg.y = msg.lateral + mc_msg.r = msg.yaw + mc_msg.enabled_extensions = EXTENSIONS_CODE + mc_msg.s = msg.pitch + mc_msg.t = msg.roll - return rc_msg + return mc_msg def state_control( self, req: AutonomousFlight.Request, res: AutonomousFlight.Response @@ -113,7 +122,7 @@ def control_callback(self, msg: PixhawkInstruction) -> None: else: return - self.rc_pub.publish(msg=self.to_override_rc_in(msg)) + self.mc_pub.publish(self.to_manual_control(msg)) def main() -> None: diff --git a/src/surface/flight_control/launch/flight_control_launch.py b/src/surface/flight_control/launch/flight_control_launch.py index 973fed3a..a6ce2bd0 100644 --- a/src/surface/flight_control/launch/flight_control_launch.py +++ b/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', ) diff --git a/src/surface/flight_control/test/test_manual_control.py b/src/surface/flight_control/test/test_manual_control.py index 340a4878..c4666584 100644 --- a/src/surface/flight_control/test/test_manual_control.py +++ b/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)