I trying delete and creation a new mission during the auto flight mode.
I use this metods:
And then I create a new command with the location specified as follow:
cmd.id = MAV_CMD_NAV_WAYPOINT;
cmd.p1 = 0;
cmd.content.location = inertial_nav.get_position();
cmd.index = 0;
cmd.create = true;
//Second wp next_wp.id = MAV_CMD_NAV_LAND; next_wp.p1 = 0; next_wp.content.location = location; next_wp.index = 4; copter.wp_nav->set_wp_destination(location); mission.add_cmd(next_wp);
But the problem is that it skips the first created wp and goes straight to the second. How do I make him follow both wp? I try change the index equal 0 in both.