Fixes bug that causes coupling from g to roll and pitch when using AHRS_TRIM parameters

The current code uses the AHRS_TRIM_X and AHRS_TRIM_Y parameters to correct for angular misalignments between gyros and accelerometers. This can cause undesirable coupling from manoeuvre G to errors in roll and pitch angles if these trim values are used to correct for alignment differences between the control board and the airframe as indicated by the documentation.

This patch moves the angular correction such that it is applied to the output roll, pitch and yaw angles.