Cleaned up controllers a bit. Ready to test landing controller in the air