Restructured ahrs.c; starting control system implementation