Problem with custom command in AutoMode

ArduCopter Copter 3.6
1

Hello!

I’m trying to create a new command for the AutoMode in ArduCopter.
So far I successfully created a new MAVLink command.
Also I added two new functions do_mycommand(…) and verify_command(…)

I created a new mission file with my custom command as a last entry.

The problem I now experience is:
In do_mycommand(…) I want the drone to fly to a specific point, therefore I call copter.wp_nav->set_wp_destination(…);
do_mycommand(…) is executed but verify_mycommand(…) is never called and also the drone is not flying to the specified position. It just stands stills. If I add another command after my custom command in the missions file, verify_mycommand is executed, but my request from set_wp_destination is ignored, the drone strictly executes the command with the specified parameters from the mission file.

Am I missing something? Is it not possible to add a new waypoint the drone should fly to during an active mission? If not, what are my options?

2

After reading the code extensively, I figured out, that I have to “mark” my command as a nav command, because do commands are executed in parallel with the next nav command?
I achieved this by adding a new condition check in the function

bool AP_Mission::is_nav_cmd(…)

in AP_MIssion.cpp

In my opinion this is not quite convenient. Are there better ways?