Add frequency input to Mahony AHRS algortihm

[gustavolaureano]

Hi, the actual implementation of the Mahony filter is working with a fixed frequency of 25Hz (sampleFreq, defined in src/utility/MahonyAHRS.cpp) and it returns innacurate values when the system is calling the filter in a different rate.

This patch adds an input for the acquisition frequency, giving more accurate sensor readings when the correct frequency is given.

here is a code using this feature: https://github.com/gustavolaureano/updated_balac_pid