Increase Kp gain for Roll and Pitch 1.2 -> 1.5
Modified hover point: Thrust = 165
Added MOTOR_ENABLE and increase Kp and Kp