Re-added AHRS Euler angle calculations since they were needed for the PID control algorithm