Fixed Roll pitch error and added zeroing of integrals