Structuralized PID initialization, checked magnetometer, altitude is working