Manual Control servo control

src/pi/manipulators/manipulators/valve_manipulator_node.py

@@ -4,12 +4,15 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import qos_profile_system_default
+from rclpy.qos import QoSPresetProfiles
+from mavros_msgs.msg import ManualControl
 
 from rov_msgs.msg import ValveManip
 
 # Configuration
 SERVO_PIN: Final = 12  # pin used to drive Valve Manip
 
+EXTENSIONS_CODE: Final = 0b00000100
 
 class ValveManipulator(Node):
     def __init__(self) -> None:
@@ -23,6 +26,9 @@ def __init__(self) -> None:
         self.curr_active = False
 
     def servo(self, width: int, freq: int = 50) -> None:
+        # mc_msg = ManualControl()
+        # mc_msg.enabled_extensions = EXTENSIONS_CODE
+        # mc_msg.aux1 = 1000
         lgpio.tx_servo(self.gpio_handle, SERVO_PIN, width, freq)
 
     def manip_callback(self, message: ValveManip) -> None:

src/rov_msgs/msg/PixhawkInstruction.msg

@@ -4,6 +4,7 @@ float32 lateral
 float32 pitch
 float32 yaw
 float32 roll
+float32 aux1
 
 uint8 MANUAL_CONTROL = 0
 uint8 KEYBOARD_CONTROL = 1

src/surface/flight_control/flight_control/manual_control_node.py

@@ -102,6 +102,7 @@ def __init__(self) -> None:
         self.valve_manip_state = False
 
     def controller_callback(self, msg: Joy) -> None:
+        self.get_logger().debug("Controller Callback")
         self.joystick_to_pixhawk(msg)
         self.valve_manip_callback(msg)
         self.manip_callback(msg)
@@ -111,6 +112,8 @@ def joystick_to_pixhawk(self, msg: Joy) -> None:
         axes: MutableSequence[float] = msg.axes
         buttons: MutableSequence[int] = msg.buttons
 
+        self.get_logger().debug("PixhawkInstruction creation")
+
         instruction = PixhawkInstruction(
             forward=float(axes[LJOYY]),  # Left Joystick Y
             lateral=-float(axes[LJOYX]),  # Left Joystick X
@@ -119,6 +122,7 @@ def joystick_to_pixhawk(self, msg: Joy) -> None:
             pitch=float(axes[RJOYY]),  # Right Joystick Y
             yaw=-float(axes[RJOYX]),  # Right Joystick X
             author=PixhawkInstruction.MANUAL_CONTROL,
+            aux1=1900
         )
 
         self.rc_pub.publish(instruction)

src/surface/flight_control/flight_control/multiplexer.py

@@ -29,7 +29,7 @@
 RANGE_SPEED: Final = MAX_RANGE_SPEED * SPEED_THROTTLE
 Z_RANGE_SPEED: Final = Z_MAX_RANGE_SPEED * SPEED_THROTTLE
 
-EXTENSIONS_CODE: Final = 0b00000011
+EXTENSIONS_CODE: Final = 0b00000111
 
 NEXT_INSTR_FRAC: Final = 0.05
 PREV_INSTR_FRAC: Final = 1 - NEXT_INSTR_FRAC
@@ -129,6 +129,7 @@ def to_manual_control(msg: PixhawkInstruction) -> ManualControl:
     mc_msg.enabled_extensions = EXTENSIONS_CODE
     mc_msg.s = mapped_msg.pitch
     mc_msg.t = mapped_msg.roll
+    mc_msg.aux1 = mapped_msg.aux1
 
     return mc_msg
 

src/surface/flight_control/flight_control/pixhawk_instruction_utils.py

@@ -5,7 +5,7 @@
 
 def pixhawk_instruction_to_tuple(
     msg: PixhawkInstruction,
-) -> tuple[float, float, float, float, float, float]:
+) -> tuple[float, float, float, float, float, float, float]:
     """
     Convert PixhawkInstruction to tuple of dimensions.
 
@@ -19,7 +19,7 @@ def pixhawk_instruction_to_tuple(
     tuple[float, float, float, float, float, float]
         Tuple of dimensions from the instruction
     """
-    return (msg.forward, msg.vertical, msg.lateral, msg.pitch, msg.yaw, msg.roll)
+    return (msg.forward, msg.vertical, msg.lateral, msg.pitch, msg.yaw, msg.roll, msg.aux1)
 
 
 def tuple_to_pixhawk_instruction(
@@ -48,6 +48,7 @@ def tuple_to_pixhawk_instruction(
         yaw=instruction_tuple[4],
         roll=instruction_tuple[5],
         author=author,
+        aux1=instruction_tuple[6],
     )