Fix yaw locking out at -180

yaw drift compensation was being applied which could take angle outside
limits (-180 to 180 degrees) which caused it to lock at -180
also reduce recentre sample period