Modified for control mode 3