Things are working! I still need to add aero drag forces as well as guidance for payload / quadrotors.