Pixhawk heading fluctuates even when vehicle is stationary, causing unnecessary PID output

[beyza (BEYZA NUR)]

Hi everyone,

I’m working on an underwater vehicle project using a Pixhawk running ArduSub firmware. I’m communicating with the Pixhawk via pymavlink and reading the heading value from the VFR_HUD message to keep the vehicle going straight with a simple PID controller.

The main problem is that even when the Pixhawk is completely stationary, the heading value keeps fluctuating slightly—about 1 or 2 degrees. This causes the PID output to constantly adjust and the motors to slightly correct unnecessarily. As a result, the vehicle makes small unnecessary turns instead of moving straight.

Here’s a sample output I get:

Heading: 346.0° | Error: -18.00 | PWM_LEFT: 1718 | PWM_RIGHT: 1682  
Heading: 345.0° | Error: -19.00 | PWM_LEFT: 1719 | PWM_RIGHT: 1681

I’ve implemented a small dead zone (±2°), but it’s still not enough. The vehicle reacts to every small drift, and this issue becomes more problematic when trying to move straight over longer distances underwater.

I’m unsure if this is caused by compass calibration, magnetic interference, or just the way heading works on ArduSub. Also, would using ATTITUDE.yaw instead of VFR_HUD.heading give more stable readings?

Here is the full code I’m running on the companion computer:

from pymavlink import mavutil
import time

time.sleep(3)

Kp = 1.0
Ki = 0.0
Kd = 0.3
integral = 0
prev_error = 0
prev_time = time.time()
heading_ref = None

def send_pwm(master, channel, pwm):
    master.mav.command_long_send(
        1, 1,
        mavutil.mavlink.MAV_CMD_DO_SET_SERVO,
        0,
        channel,
        pwm,
        0, 0, 0, 0, 0
    )

def arm_vehicle(master):
    master.mav.command_long_send(
        1, 1,
        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
        0,
        1, 0, 0, 0, 0, 0, 0
    )
    print("Vehicle armed.")
    time.sleep(2)

def clear_message_buffer(master):
    print("Clearing message buffer...")
    start_time = time.time()
    while time.time() - start_time < 1.0:
        msg = master.recv_match(blocking=False)
        if msg is None:
            break
    print("Buffer cleared.")

def get_fresh_heading(master):
    print("Waiting for fresh heading value...")
    clear_message_buffer(master)
    heading_values = []
    for i in range(10):
        msg = master.recv_match(type='VFR_HUD', blocking=True, timeout=2.0)
        if msg and msg.heading != 0:
            heading_values.append(msg.heading)
            print(f"Heading {i+1}: {msg.heading}°")
            time.sleep(0.1)
    if heading_values:
        return heading_values[-1]
    else:
        print("Heading data not received.")
        return None

def main():
    global heading_ref, integral, prev_error, prev_time
    print("Connecting to Pixhawk...")
    master = mavutil.mavlink_connection('/dev/ttyACM0', baud=57600)
    master.wait_heartbeat()
    print("Connected.")

    arm_vehicle(master)
    print("Waiting 5 seconds before capturing heading reference...")
    time.sleep(5)

    heading_ref = get_fresh_heading(master)
    if heading_ref is None:
        print("Reference heading not captured. Exiting.")
        return

    prev_time = time.time()
    print(f"Reference heading set: {heading_ref}°")
    print("Starting PID control...")

    while True:
        msg = master.recv_match(type='VFR_HUD', blocking=True)
        if not msg:
            continue

        heading = msg.heading
        error = heading - heading_ref
        if error > 180:
            error -= 360
        elif error < -180:
            error += 360

        if abs(error) < 2.0:
            error = 0
            integral = 0

        now = time.time()
        dt = now - prev_time
        prev_time = now

        integral += error * dt
        integral = max(min(integral, 100), -100)

        derivative = (error - prev_error) / dt
        prev_error = error

        pid = Kp * error + Ki * integral + Kd * derivative
        pid = max(min(pid, 200), -200)

        pwm_left = max(min(1700 - pid, 1900), 1100)
        pwm_right = max(min(1700 + pid, 1900), 1100)

        send_pwm(master, 1, pwm_right)
        send_pwm(master, 2, pwm_left)
        send_pwm(master, 3, pwm_right)
        send_pwm(master, 4, pwm_left)

        print(f"Heading: {heading:.1f}° | Error: {error:.2f} | PWM_LEFT: {pwm_left:.0f} | PWM_RIGHT: {pwm_right:.0f}")
        time.sleep(0.1)

if __name__ == '__main__':
    main()

If anyone knows how to solve this issue, I would really appreciate any advice.

Thanks in advance!

[tony-white]

Hi @beyza -
If you’re not happy with the output of the EKF, you could try calibrating your motion sensors again, particularly the compass and the Gyro, from within BlueOS (Vehicle Setup.) However, I would not expect better than +/-1 degree with a well calibrated system, after all these are low-cost MEM devices, similar to those used in your cellphone!

Those two endpoints will be reporting the same yaw value. It’s also worth noting that if you’d like a control loop to run outside your autopilot, it’s not likely to have low enough latency to do something like control heading - this is what stabilize (or alt hold) mode is for (holds heading automatically at setpoint.)

[beyza (BEYZA NUR)]

Hi Tonny, thank you for your detailed explanation and the information you shared.