Added support for inverted flight
Checked in L1 position and TECS altitude control. Not test-flown in HIL or outdoors yet