Altitude and attitude NDI control with flight mode transition, tested up until motor commands, motor commands don't arrive at motors yet