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

**URL:** <https://discuss.bluerobotics.com/t/pixhawk-heading-fluctuates-even-when-vehicle-is-stationary-causing-unnecessary-pid-output/20992>\
**Category:** ArduSub\
**Tags:** ardusub, pymavlink\
**Created:** [July 16, 2025, 9:35pm UTC](https://discuss.bluerobotics.com/t/pixhawk-heading-fluctuates-even-when-vehicle-is-stationary-causing-unnecessary-pid-output/20992 "2025-07-16T21:35:24Z")\
**Posts on this page:** 4\
**Page:** 1

<div class="post-metadata">

**Author:** ![beyza](https://avatars.discourse-cdn.com/v4/letter/b/b4bc9f/32.png) [@beyza](https://discuss.bluerobotics.com/u/beyza)\
**Post date:** [July 16, 2025, 9:35pm UTC](https://discuss.bluerobotics.com/t/pixhawk-heading-fluctuates-even-when-vehicle-is-stationary-causing-unnecessary-pid-output/20992/1 "2025-07-16T21:35:24Z")

</div>

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:

```auto
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:

```python
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!

---

<div class="post-metadata">

**Author:** ![tony-white](https://sea1.discourse-cdn.com/flex019/user_avatar/discuss.bluerobotics.com/tony-white/32/18385_2.png) [@tony-white](https://discuss.bluerobotics.com/u/tony-white)\
**Post date:** [July 17, 2025, 5:35am UTC](https://discuss.bluerobotics.com/t/pixhawk-heading-fluctuates-even-when-vehicle-is-stationary-causing-unnecessary-pid-output/20992/2 "2025-07-17T05:35:01Z")

</div>

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](https://blueos.cloud/docs/latest/usage/advanced/#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.)

---

<div class="post-metadata">

**Author:** ![beyza](https://avatars.discourse-cdn.com/v4/letter/b/b4bc9f/32.png) [@beyza](https://discuss.bluerobotics.com/u/beyza)\
**Post date:** [July 17, 2025, 7:49am UTC](https://discuss.bluerobotics.com/t/pixhawk-heading-fluctuates-even-when-vehicle-is-stationary-causing-unnecessary-pid-output/20992/3 "2025-07-17T07:49:01Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![EliotBR](https://sea1.discourse-cdn.com/flex019/user_avatar/discuss.bluerobotics.com/eliotbr/32/6937_2.png) [@EliotBR](https://discuss.bluerobotics.com/u/EliotBR)\
**Post date:** [July 18, 2025, 12:02am UTC](https://discuss.bluerobotics.com/t/pixhawk-heading-fluctuates-even-when-vehicle-is-stationary-causing-unnecessary-pid-output/20992/4 "2025-07-18T00:02:08Z")

</div>

A post was merged into an existing topic: [Depth Hold Mode Does Not Hold at 1 Meter How to Set Target Depth Autonomously?](https://discuss.bluerobotics.com/t/depth-hold-mode-does-not-hold-at-1-meter-how-to-set-target-depth-autonomously/20991/3)