Added delay before calibration, finished roll and pitch stabilization