Use Navigator Library and Ardupilot at the same time

Blue Robotics Software
1

Hello,

I am trying to control my blue boat with my own custom back seat software by creating a BlueOS extension. I want to have ArduPilot (ArduRover 4.5.4) running for its state estimation (sending my backseat gps information, attitude data etc) while also using the navigator library to control the thrusters. I seem to be having a problem with ArduPilot fighting with the navigator library for control over the PWM pins. I can control both thrusters just fine using the navigator library but the second I turn the autopilot back on I only have control over one thruster (the lowest channel number being set by the navigator) all other pins are unresponsive.

Is there a way to totally bypass Ardupilot’s PWM control? I have every SERVOX_FUNCTION set to disable but that doesn’t seem to make a difference.

Here is the code I am trying to run (with the hopes of more than one pin being responsive to the navigator library)

import signal
import time
import os
from datetime import datetime, timedelta
import bluerobotics_navigator as navigator
from bluerobotics_navigator import PwmChannel

# Constants
PWM_FREQ_HZ = 50
MAX_VAL = 409.5  # at 50 Hz, corresponds to 2000 microseconds pulse width
MIN_VAL = 204.75  # at 50 Hz, corresponds to 1000 microseconds pulse width
CENTER_PWM = MIN_VAL + (MAX_VAL - MIN_VAL) / 2
RANGE_PWM = MAX_VAL - MIN_VAL

INCREMENTS = 150
STEP_SIZE = MAX_VAL / INCREMENTS

SERVO_SPAN = 154 * 2 / 3  # degrees - per datasheet should only operate around 110
SERVO_SWING = SERVO_SPAN / 2
SERVO_INCREMENT = SERVO_SPAN / INCREMENTS

ESC_CHANNELS = [PwmChannel.Ch1,PwmChannel.Ch2,PwmChannel.Ch3,PwmChannel.Ch4,PwmChannel.Ch5,PwmChannel.Ch6, PwmChannel.Ch7, PwmChannel.Ch8]

HOLD_TIME = 5  # seconds

def set_pin_pulse_width(pin_num, target):
    """
    Set the PWM pulse width for the specified pin.

    Args:
        pin_num (PwmChannel): The PWM channel number.
        target (float): Target percentage (-100 to 100).
    """
    pulse_width = CENTER_PWM + (target / 100.0) * RANGE_PWM / 2
    navigator.set_pwm_channel_value(pin_num, int(pulse_width))

def get_initialize():
    """Checks to see if board has been initialized"""

    if os.environ.get("CI") == "true":
        print("Not Initialized")
        return False
    else:
        print("Board already initialized")
        return True

def center_servos():
    """Center all servo channels."""
    for channel in ESC_CHANNELS:
        set_pin_pulse_width(channel, 0)

def initialize_esc():
    """Calibrates each esc: set max, set min, set center"""
    set_pin_pulse_width(PwmChannel.Ch10, 100)
    set_pin_pulse_width(PwmChannel.Ch12, 100)
    set_pin_pulse_width(PwmChannel.Ch14, 100)
    time.sleep(0.5)

    set_pin_pulse_width(PwmChannel.Ch10, -100)
    set_pin_pulse_width(PwmChannel.Ch12, -100)
    set_pin_pulse_width(PwmChannel.Ch14, -100)
    time.sleep(0.5)

    set_pin_pulse_width(PwmChannel.Ch10, 0)
    set_pin_pulse_width(PwmChannel.Ch12, 0)
    set_pin_pulse_width(PwmChannel.Ch14, 0)
    time.sleep(0.5)

def signal_handler(signum, frame):
    """
    Handle signal interrupt (e.g., Ctrl+C) by centering servos and disabling PWM.

    Args:
        signum (int): Signal number.
        frame (frame object): Current stack frame.
    """
    center_servos()
    time.sleep(0.5)
    navigator.set_pwm_enable(False)
    exit(signum)

def main():
    # Set up signal handling
    signal.signal(signal.SIGINT, signal_handler)

    # Initialize Navigator

    navigator.init()    
    navigator.set_pwm_freq_hz(PWM_FREQ_HZ)
    navigator.set_pwm_enable(True)
    print(f"PWM Chip State: {navigator.get_pwm_enable()}, waiting for a second")
    time.sleep(3)

    #Initialize ESCs
    print("Calibrating ESCs")
    initialize_esc()

    # Center SERVOs initially
    print("Centering SERVOs")
    center_servos()

    # Hold the center position for a while
    hold_until = datetime.now() + timedelta(seconds=HOLD_TIME)
    while datetime.now() < hold_until:
        time_left = (hold_until - datetime.now()).total_seconds()
        print(f"T - {time_left:.2f}")
        time.sleep(1)

    # PWM sweeping logic
    i = CENTER_PWM
    increasing = True  # Initial direction
    next_update = datetime.now()

    while True:
        if datetime.now() >= next_update:
            for channel in ESC_CHANNELS:
                set_pin_pulse_width(channel, (i - CENTER_PWM) * 100 / RANGE_PWM)

            servo_angle = ((i - CENTER_PWM) / RANGE_PWM) * SERVO_SWING
            print(f"Set {servo_angle:.2f}")

            if increasing:
                i += STEP_SIZE
                if i >= MAX_VAL:
                    i = MAX_VAL
                    increasing = False
            else:
                i -= STEP_SIZE
                if i <= MIN_VAL:
                    i = MIN_VAL
                    increasing = True

            next_update = datetime.now() + timedelta(milliseconds=50)

if __name__ == "__main__":
    main()
2

Hi @jwenger,

It’s generally ill-advised to simultaneously run multiple programs that think they’re responsible for the same hardware, because even if they’re not actively controlling it they may still do some configuration of and/or connection to it.

I’d recommend you create a program that uses MAVLink commands to control the thrusters, either via high level motion commands, or by setting the servo values directly using MAV_CMD_DO_SET_SERVO or RCINx passthroughs assigned to the relevant motor outputs, with control provided via RC_CHANNELS_OVERRIDE messages.