You may want to investigate a velocity level PID control loop based off a gyro rate. This should kill two birds with one stone (arc turns and lock to heading.. kind of)
Your rate control could come to the following
Code:
left = speed + calculated_turn_rate;
right = speed - calculated_turn_rate;