MahonyAHRS.cpp sqrt and invSqrt wrong usage in normalization

[mgrouch]

Describe the bug

MahonyAHRS.cpp sqrt and invSqrt wrong usage in normalization

To reproduce

Doesn't work with magnetormeter

Expected behavior

no NaNs

Screenshots

No response

Environment

• OS:
• IDE &IDE Version:
• Repository Version:

Additional context

No response

Issue checklist

• I searched for previous reports in the issue tracker
• My report contains all necessary details