Testing Roll_Pitch Control

Signed-off-by: MQ <MINHQUAN001@e.ntu.edu.sg>