Create rc debug print function
Fix printf argument passing issue
Merge https://github.com/Khan-drone/ahrs-core