Added control loops for sub control. Currently control surface power is not based on speed.