Thanks @modaltb , that worked! I had to set it to run as a background function using the &. It's running perfectly now.
I probably want to rewrite it as a task, but that example code looked tough to follow. I wish the PX4 developer manual included more step by step tutorials, like the ROS tutorials.