From f8889d41f90faea409b64104d716aff9c0078fc7 Mon Sep 17 00:00:00 2001 From: Bunando Date: Sat, 25 Jan 2025 19:22:07 +0000 Subject: [PATCH] Manual Control servo control --- src/pi/manipulators/manipulators/valve_manipulator_node.py | 6 ++++++ src/rov_msgs/msg/PixhawkInstruction.msg | 1 + .../flight_control/flight_control/manual_control_node.py | 4 ++++ src/surface/flight_control/flight_control/multiplexer.py | 3 ++- .../flight_control/pixhawk_instruction_utils.py | 5 +++-- 5 files changed, 16 insertions(+), 3 deletions(-) diff --git a/src/pi/manipulators/manipulators/valve_manipulator_node.py b/src/pi/manipulators/manipulators/valve_manipulator_node.py index 3dbcdb44..965b6b7e 100644 --- a/src/pi/manipulators/manipulators/valve_manipulator_node.py +++ b/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: diff --git a/src/rov_msgs/msg/PixhawkInstruction.msg b/src/rov_msgs/msg/PixhawkInstruction.msg index 48260f63..19afb96b 100644 --- a/src/rov_msgs/msg/PixhawkInstruction.msg +++ b/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 diff --git a/src/surface/flight_control/flight_control/manual_control_node.py b/src/surface/flight_control/flight_control/manual_control_node.py index f32c3fbf..d13c4c27 100644 --- a/src/surface/flight_control/flight_control/manual_control_node.py +++ b/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) diff --git a/src/surface/flight_control/flight_control/multiplexer.py b/src/surface/flight_control/flight_control/multiplexer.py index 7ea3e16d..266d4348 100644 --- a/src/surface/flight_control/flight_control/multiplexer.py +++ b/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 diff --git a/src/surface/flight_control/flight_control/pixhawk_instruction_utils.py b/src/surface/flight_control/flight_control/pixhawk_instruction_utils.py index 19a09803..6ba0ca81 100644 --- a/src/surface/flight_control/flight_control/pixhawk_instruction_utils.py +++ b/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], )