Added source code with everything working. GPS takes a while to connect but oh well