Failsafe configuration while overriding RC with pymavlink

Hi BlueRobotics,

For some project, I need to control individually the outputs of a Navigator.

Using PyMavlink, I can compute everything on a “surface computer” and send individual PWM commands to each of them. I use connection.mav.rc_channels_override_send() to achieve this.

I updated the firmware to ArduSub 4.5.0, and noticed that fail-safes now exist for RC-configured outputs. However they seem to act only over channels 1-6, while I would like to see all outputs go back to neutral when connection is lost.

There is a SERVO_RC_FS_MSK parameter that seems to aim at configuring just that, but I cannot figure out how to works. I hoped that setting it to 65535 would change the behaviour to what I want, but apparently not. Is this feature actually implemented in ArduSub ?

I am also unsure if this would apply to direct PWM override, as the ArduPilot documentation states that it is a mask for scaled passthru output channels.

In short, is it currently possible to configure fail-safes for RC overriden inputs ? And if not, will it become possible with a next release of ArduSub ?

Hi @Matth, welcome to the forum :slight_smile:

There’s some relevant discussion of applicable failsafes in this thread.

Note that failsafes that cause the autopilot to disarm will affect outputs designated as motors, but that may be counter to your desire to control outputs individually.

That is new in the latest version, and was ported over automatically from other firmwares, so not something we’ve meaningfully tested with. I looked briefly at the code and ArduSub does at least use the library which defines the parameter, but I’m unclear on whether it actually makes use of it, and don’t currently have the time to test.

It’s also possible that failsafe only applies for real RC inputs, not RC overrides. If you describe what values you’ve been using for the related parameters and what control inputs you’ve been sending we can try to point out issues with your approach, or try to replicate it and more clearly determine how specific functionalities work.

Hi Eliot, thank you for your answer :slightly_smiling_face:

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