Tested TAKEOFF AND LAND -> WORKS ON IRIS+
Tested all the functionality, takeoff, land and goto work.

TODO:
1. Refractor into modules
2. Rewrite Code with Unit Testing