First working Yaw D controller
Included new flight plan relevant to ARPVEC's needs