Added flight code

Signed-off-by: Thomas Wang <twang@nps.edu>
Fixed initialization bug

Signed-off-by: Thomas Wang <twang@nps.edu>