[H_CTL] Minor improvements on code.
Work in Progress on full Yaw Rate-based PID