Use INAV as default to not break existing setups
Fix warnings, use more efficient atan2f where it can be safely used