Revert "Smooth thrusters"

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", "UP040"]
+ignore = ["D100", "D101", "D102", "D103", "D104", "D107", "T201", "FIX002", "TD003", "TD002", "TRY003", "EM101", "EM102", "RET504", "D211", "COM812", "ISC001", "ERA001", "S602", "S603", "D205"]
 
 [tool.ruff.lint.pydocstyle]
 convention = "numpy"

src/surface/flight_control/flight_control/multiplexer.py

@@ -1,3 +1,4 @@
+from collections.abc import Callable
 from typing import Final
 
 import rclpy
@@ -6,19 +7,14 @@
 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: Final = 0.65
+SPEED_THROTTLE = 0.85
 
 # Joystick curve
-JOYSTICK_EXPONENT: Final = 3
+JOYSTICK_EXPONENT = 3
 
 # Range of values Pixhawk takes
 # In microseconds
@@ -31,100 +27,25 @@
 
 EXTENSIONS_CODE: Final = 0b00000011
 
-NEXT_INSTR_FRAC: Final = 0.05
-PREV_INSTR_FRAC: Final = 1 - NEXT_INSTR_FRAC
+# Channels for RC command
+MAX_CHANNEL = 8
+MIN_CHANNEL = 1
+
+FORWARD_CHANNEL = 4  # X
+THROTTLE_CHANNEL = 2  # Z (vertical)
+LATERAL_CHANNEL = 5  # Y (left & right)
+PITCH_CHANNEL = 0  # Pitch
+YAW_CHANNEL = 3  # Yaw
+ROLL_CHANNEL = 1  # Roll
 
 
 def joystick_map(raw: float) -> float:
-    """
-    Convert the provided joystick position to a
-    float in [-1.0, 1.0] for use in a PixhawkInstruction.
-
-    Parameters
-    ----------
-    raw : float
-        The joystick position to convert
-
-    Returns
-    -------
-    float
-        A float in [-1.0, 1.0] to act as a PixhawkInstruction dimension
-    """
     mapped = abs(raw) ** JOYSTICK_EXPONENT
     if raw < 0:
         mapped *= -1
     return mapped
 
 
-def manual_control_map(value: float) -> float:
-    """
-    Convert the provided float in [-1.0, 1.0] to a ManualControl dimension.
-
-    Parameters
-    ----------
-    value : float
-        The float in [-1.0, 1.0] to convert to a ManualControl dimension
-
-    Returns
-    -------
-    float
-        The resulting ManualControl dimension
-    """
-    return RANGE_SPEED * value + ZERO_SPEED
-
-
-def smooth_value(prev_value: float, next_value: float) -> float:
-    """
-    Get a value that interpolates prev_value & next_value.
-
-    Parameters
-    ----------
-    prev_value : float
-        The previous value, affects the result based on PREV_INSTR_FRAC
-    next_value : float
-        The next value, affects the result based on NEXT_INSTR_FRAC
-
-    Returns
-    -------
-    float
-        The resulting value between prev_value & next_value
-    """
-    return PREV_INSTR_FRAC * prev_value + NEXT_INSTR_FRAC * next_value
-
-
-def to_manual_control(msg: PixhawkInstruction) -> ManualControl:
-    """
-    Convert the provided PixhawkInstruction to a ManualControl message.
-
-    Parameters
-    ----------
-    msg : PixhawkInstruction
-        The PixhawkInstruction to convert
-
-    Returns
-    -------
-    ManualControl
-        The resulting ManualControl message
-    """
-    mc_msg = ManualControl()
-
-    # Maps to PWM
-    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
-
-    mc_msg.x = mapped_msg.forward
-    mc_msg.z = mapped_msg.vertical
-    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 mc_msg
-
-
 class MultiplexerNode(Node):
     def __init__(self) -> None:
         super().__init__('multiplexer', parameter_overrides=[])
@@ -146,24 +67,35 @@ def __init__(self) -> None:
             ManualControl, 'mavros/manual_control/send', QoSPresetProfiles.DEFAULT.value
         )
 
-        self.previous_instruction_tuple: tuple[float, ...] = pixhawk_instruction_to_tuple(
-            PixhawkInstruction()
-        )
-
-    def smooth_pixhawk_instruction(self, msg: PixhawkInstruction) -> PixhawkInstruction:
-        instruction_tuple = pixhawk_instruction_to_tuple(msg)
-
-        smoothed_tuple = tuple(
-            smooth_value(previous_value, value)
-            for (previous_value, value) in zip(
-                self.previous_instruction_tuple, instruction_tuple, strict=True
-            )
-        )
-        smoothed_instruction = tuple_to_pixhawk_instruction(smoothed_tuple, msg.author)
-
-        self.previous_instruction_tuple = smoothed_tuple
-
-        return smoothed_instruction
+    @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 = 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_manual_control(msg: PixhawkInstruction) -> ManualControl:
+        """Convert this PixhawkInstruction to an rc_msg with the appropriate channels array."""
+        mc_msg = ManualControl()
+
+        # Maps to PWM
+        MultiplexerNode.apply(msg, lambda value: (RANGE_SPEED * value) + ZERO_SPEED)
+
+        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 mc_msg
 
     def state_control(
         self, req: AutonomousFlight.Request, res: AutonomousFlight.Response
@@ -179,7 +111,7 @@ def control_callback(self, msg: PixhawkInstruction) -> None:
         ):
             # Smooth out adjustments
             # TODO: look into maybe doing inheritance on a PixhawkInstruction
-            msg = apply_function(msg, joystick_map)
+            MultiplexerNode.apply(msg, joystick_map)
         elif (
             msg.author == PixhawkInstruction.KEYBOARD_CONTROL
             and self.state == AutonomousFlight.Request.STOP
@@ -191,8 +123,7 @@ def control_callback(self, msg: PixhawkInstruction) -> None:
         else:
             return
 
-        smoothed_instruction = self.smooth_pixhawk_instruction(msg)
-        self.mc_pub.publish(to_manual_control(smoothed_instruction))
+        self.mc_pub.publish(self.to_manual_control(msg))
 
 
 def main() -> None:

src/surface/flight_control/test/test_manual_control.py

@@ -5,7 +5,7 @@
     Z_RANGE_SPEED,
     Z_ZERO_SPEED,
     ZERO_SPEED,
-    to_manual_control,
+    MultiplexerNode,
 )
 
 from rov_msgs.msg import PixhawkInstruction
@@ -31,14 +31,14 @@ def test_joystick_profiles() -> None:
         roll=0.92,
     )
 
-    msg = to_manual_control(instruction)
+    msg = MultiplexerNode.to_manual_control(instruction)
 
     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.s == ZERO_SPEED + RANGE_SPEED * 0.34
-    assert msg.r == ZERO_SPEED + RANGE_SPEED * -0.6
-    assert msg.t == ZERO_SPEED + 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)