Hi Eliot, thank you for your answer
I indeed need to be in RC Override control mode to control the outputs individually and achieve my goal.
All remarks hereafter will be relative to ArduSub 4.5.0, as there are no failsafes at all for RC Override in 4.1.2.
You will find attached the script that I use for testing, with screenshots of the configuration for the relevant parameters. When the script is interrupted, all outputs keep their latest command for 3 seconds (FS_PILOT_TIMEOUT
parameter), then outputs 1 to 6 go back to neutral value 1500 while the others keep their latest received command (including thrusters 7 and 8).
The parameter SERVO_RC_FS_MSK
does not seem to affect this behaviour in any way.
from pymavlink import mavutil
class Controller:
def __init__(self):
self.conn = mavutil.mavlink_connection("udpin:0.0.0.0:14550")
self.conn.wait_heartbeat()
self._mcu_is_connected = True
self._verbose = True
self.set_rc_configuration()
def read_param(self, param_name, timeout=1, retries=3):
param_dict = None
while self._mcu_is_connected and param_dict is None and retries > -1:
retries -= 1
self.conn.mav.param_request_read_send(
self.conn.target_system, self.conn.target_component,
param_name.encode('utf8'),
-1
)
param_dict = self.conn.recv_match(type='PARAM_VALUE',
blocking=True,
timeout=timeout)
if param_dict is not None:
return param_dict.to_dict()["param_value"]
return None
def set_param(self, param_name, new_value, retries=3):
self.conn.mav.param_set_send(
self.conn.target_system, self.conn.target_component,
param_name.encode(),
new_value,
mavutil.mavlink.MAV_PARAM_TYPE_REAL32
)
current_value = None
while self._mcu_is_connected and retries > 0 \
and (current_value := self.read_param(param_name, retries=1)) != new_value:
retries -= 1
self.conn.mav.param_set_send(
self.conn.target_system, self.conn.target_component,
param_name.encode(),
new_value,
mavutil.mavlink.MAV_PARAM_TYPE_REAL32
)
if current_value is not None and current_value == new_value:
return True
return False
def set_rc_configuration(self):
"""
0 : Disabled
1 : RC Pass Through
33-40 : Motor 1-8
"""
for i in range(16): # First, deactivate all to avoid bad RC commands set when reassigning functions
if self._verbose:
print(f"Value before change SERVO{i + 1}_FUNCTION: {self.read_param(f'SERVO{i + 1}_FUNCTION')}")
self.set_param(f'SERVO{i + 1}_FUNCTION', 0.0)
# Set all commands to initial values
self.conn.mav.rc_channels_override_send(
self.conn.target_system,
self.conn.target_component,
*([1500] * 16))
for i in range(16): # Configure all PWM outputs as RC-Pass-Through
self.set_param(f'SERVO{i + 1}_FUNCTION', 1.0)
if self._verbose:
print(f"Value after change SERVO{i + 1}_FUNCTION: {self.read_param(f'SERVO{i + 1}_FUNCTION')}")
def send_pwm_commands(self, commands: list[int]):
self.conn.mav.rc_channels_override_send(
self.conn.target_system,
self.conn.target_component,
*commands)
if __name__ == "__main__":
import signal
import time
signal.signal(signal.SIGINT, signal.default_int_handler)
try:
controller = Controller()
direction = 1
cmd = 0
while True:
# Oscillate back and forth, with little amplitude, alternate PWM
if abs(cmd) > 100:
direction = - direction
cmd += direction * 2
main_commands = [1500 + cmd * (2 * (i % 2) - 1) for i in range(8)]
aux_commands = [1500] * 8
aux_commands[1] = 1000 # I currently have lights here
controller.send_pwm_commands(main_commands + aux_commands)
time.sleep(0.1)
except KeyboardInterrupt:
print("User interrupted program.")
except Exception as e:
print(f"Exception occured:\n{e}")
# To stop thrusters at the end of tests, uncomment this
main_commands = [1500] * 8
controller.send_pwm_commands(main_commands + aux_commands)
print("Program terminated")
Parameters Configuration