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 roll_sensor,pitch_sensor and yaw_sensor output calculation and also to the get_dcm_matrix method that are used by control and navigation functions.