Added 6dof setpoints to mavlink app