修复bug： draw_atti_map的float('nan')判断问题
Add pos PID; Make it more pythonic