MAV_CMD_NAV_WAYPOINT in GUIDED rejected

ArduPlane Plane 4.0
1

I’m trying to send a MAV_CMD_NAV_WAYPOINT command from my C application using the following call:

    mavlink_msg_command_int_pack(m_sysId, m_compId, &msg, m_sysId_UAS,
    m_compId_UAS, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT, 2, 0, 0, 10, 0, 0,
    static_cast<int32_t>(lat_deg * 1e7),
    static_cast<int32_t>(lon_deg * 1e7), alt_m_msl);

However, the simulation rejects the command with MAV_RESULT_UNSUPPORTED (3). I’ve verified that the simulation is in guided mode and the aircraft is simply loitering near the home point. Does this have to be wrapped in a full mission setup? If so, can someone point me to an example of how that is done?

2

Bump. Surely I can’t be the only person who is trying to do this! Any help is appreciated!

3

Mission planner code is always a good reference. Check https://github.com/ArduPilot/MissionPlanner/blob/master/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs for setGuidedModeWP function

4

Thanks, I’ll take a look!

5

That got me what I needed! Thanks, Andras!

FYI, here is what I have now:

    mavlink_msg_mission_item_int_pack(m_sysId, m_compId, &msg, m_sysId_UAS,
    m_compId_UAS, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT, 2, 1, 0, 0, 0,
    0, static_cast<int32_t>(lat_deg * 1e7),
    static_cast<int32_t>(lon_deg * 1e7), static_cast<int32_t>(alt_m_msl),
    MAV_MISSION_TYPE_MISSION);

This will fly the plane to that loiter point. Keep in mind, this is a loiter point, not a fly through waypoint. I have not found a way to do a fly through in guided mode (yet). I have a feeling I will need to do some math to determine what the actual WP is to account for my bearing to the waypoint and the loiter radius. Or maybe the better plan would be to package a two waypoint mission and always keep myself one waypoints ahead…

6

indeed, if you want to fly exactly through a point you have to do some math. luckily the PointLatLng class in Mission Planner has all the necessary methods for doing that, you can borrow the code from there :slight_smile: