Hi there! We are using ArduCopter 4.0.5 with a CubeOrange, and are trying to implement the MavLINK command DO_CHANGE_ALTITUDE for our drones.
Our goal is to use the Lidar data we have to stop the aircraft in case an object in nearby and change its altitude and after that continuing the mission. So far I’ve managed to do so in guided mode, however when its height changes the yaw usually turns too, and I would like to change it on auto mode too.
I tried calling “AC_WPNav::set_wp_destination(const Location& destination)”, by copying the current vehicle position and changing the altitude as the goal wp, however in doing so the wp it was going to is skipped, so I’m now not sure its a good idea to call that class directly.
For the guided mode I called “copter.mode_guided.set_destination(…)”, and it works fine except for that unexpected turn in yaw, and that does interfere with the lidar’s measurement.
Does anyone have any advice on how we could safely implement the command on ArduCopter? I would like if possible to get a stable result and make a pull request so others may use it too!
Thanks for all!