Emulating the Geofence Upload State Machine

Ground Control Software Mission Planner
1

I am writing code in my custom ground station to upload waypoints, set the current waypoint, and download the geofence from an APM2. I found docs for uploading waypoints (qgroundcontrol.org/mavlink/waypoint_protocol), and I found out how to set the current waypoint (using WAYPOINT_SET_CURRENT or MISSION_SET_CURRENT), and was therefore able to get everything working except for downloading the geofence. Since I couldn’t find any documentation on how to do so, I looked at the code in Mission Planner. This is my interpretation of the protocol, implemented in FlightPlanner.cs in the MissionPlanner src repo

  1. Set FENCE_TOTAL parameter
    -------------- requires that you request the FENCE_TOTAL parameter and get the param_value in order to fill out a PARAM_SET message with the proper param_value
  2. Send Fence Point and receive Fence Point, make sure they match, repeat until finished

I have not yet implemented parameter setting functionality so I have yet to try the above, but I have some speculation on the operation. What I think happens when you set the FENCE_TOTAL parameter is that the APM begins its protocol for receiving the geofence. I haven’t taken a look in the APM code so I’m not sure, but there must be a trigger since when I just do (2.) without having done (1.), when I receive fence point 0 it is not the same as the fence point 0 that I just sent over.

Tl;dr How do you implement the geofence upload from GCS->APM?

2

I fixed it. The geofence upload protocol requires that you set the FENCE_TOTAL param, confirm its value by receiving a param value message in response, then send the fence points one-by-one and confirming receipt of each by sending a fence fetch point before moving on to the next one.

The FENCE_TOTAL param value must mach the “count” field in each FENCE_POINT message or else the AP throws them away and responds with a STATUSTEXT message declaring “bad fence point.” This can be observed in the AP source code in ArduPlane/GCS_Mavlink.pde in the “MAVLINK_MSG_ID_FENCE_POINT” case statement where it has

…

} else if (packet.count != g.fence_total) { send_text_P(SEVERITY_LOW,PSTR("bad fence point")); }
…

so it is critical that you set the FENCE_TOTAL param to the appropriate value prior to fence point transmission.