Position trajectory to MC position control

@tuloski You need a seperate controller in order to follow a trajectory. I am doing it by using offboard control setpoints using mavros.

It is still in a work-in-progress, but hope it helps.