ORCA2.cpp contains ORCA with stacked quads and a LQR controller. Need tilt values in nav_data and better estimate of mg