New mid waypoint with arduino and 0x10/16 command

I have and arduino mega doing a couple calculations and outputs new coordinates.
I’m wondering if the 0x10 / 16 MAV_CMD_NAV_WAYPOINT command is what i should be looking for and how to implement it.
So far, although i get the logic of MavLink, i still cannot get it working. Now, with the above command, i’m even more confused.

Since i include convenient functions, wouldn’t that be easier with just mavlink_msg_waypoint_set_current_send();
(if i fix the error “was not declared in this scope”)?

I tried the project’s “Follow me” libraries, but even there the functions/variables have different names.

The waypoint protocol is set

You can only change the current waypoint seq with mavlink_msg_waypoint_set_current_send

The current waypoint if changed has no impact on the autopilot.

You need to use Guided mode in APM for that. To use Guided mode you need to set the current waypoint

Below is the code snippet from APM Planner 2.0 (see … n.cpp#L680 for the receiving code in the APM)

void UASWaypointManager::goToWaypoint(Waypoint *wp)
if (!uas) return;

//Don't try to send a guided mode message to an AP that does not support it.
if (uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA)
    QLOG_DEBUG() << "APM: goToWaypont: " + wp->debugString();
    mavlink_mission_item_t mission;
    memset(&mission, 0, sizeof(mavlink_mission_item_t));   //initialize with zeros
    //const Waypoint *cur_s =;

    mission.autocontinue = 0;
    mission.current = 2; //2 for guided mode
    mission.param1 = wp->getParam1();
    mission.param2 = wp->getParam2();
    mission.param3 = wp->getParam3();
    mission.param4 = wp->getParam4();
    mission.frame = wp->getFrame();
    mission.command = wp->getAction();
    mission.seq = 0;     // don't read out the sequence number of the waypoint class
    mission.x = wp->getX();
    mission.y = wp->getY();
    mission.z = wp->getZ();
    mavlink_message_t message;
    mission.target_system = uasid;
    mission.target_component = MAV_COMP_ID_MISSIONPLANNER;
    mavlink_msg_mission_item_encode(uas->getSystemId(), uas->getComponentId(), &message, &mission);


So, i only have to make convenient functions work and send mavlink_msg_waypoint_set_current_send() once per waypoint? And after that, the plane is going to loiter?
Would it be a viable solution to use guided the whole time and just push new mission waypoints from arduino? (even the ones that i actually need).
Or is there a way to change from auto to guided, push the waypoint and then return to auto (after reaching it)?

Since i still haven’t found a solid solution, i’ll bypass the problem using a receiver to receive the new waypoint and telemetry (+tablet) to guide the uav manually.