How to stabilize heading?

Hi, I am working on my small boat with an APMrover and a pixfalcon.
My boat has two paddles on both side and they can be individually controlled as a tank.

Compared to a ground vehicle, heading of a boat is not stable due to flow of water and wind.

I am planning to add new mode to stabilize the heading like a copter during loiter mode or auto mode.

Do anybody have an idea how to modify codes?

Have a look on the Wiki for the skid steering modes.

Thanks, Grant.