Code to simulate quadcopter control for PX4 integration