I am using custom made motion capture device and pixhawk 3 pro and I want to create a script that takes off and goes to the device which is 0,0,0. I pass the position of the drone related to the device in the mocap/pose topic but how can I have the initial takeoff position in my code. In other words, how to save in variables in c++ the x, y and z of the takeoff position. I appreciate any help.
Just create a new Vector 3 variable where you store pose.position from the local_poition/pose topic.