Drone doesn't reach waypoint in GUIDED mode

Hi community,

I’m developing an application in which a companion board controls externally the waypoint trajectory of the drone in GUIDED mode via mavros. Every time the companion board determines that the drone has reached the desired GPS location, it computes the next setpoint in local coordinates and sends it to Ardupilot.

The issue comes when, I guess due to GPS errors, IMU errors and disturbances, the drone doesn’t reach the desired GPS coordinate within a certain tolerance. For some reason, this fact locks the drone indefinitely.

My question is, is there a way to read from Ardupilot when it thinks it has reached the goal position? I could use this as a flag to tell the board to move to the next waypoint, regardless of whether the drone has reach the current one or not.

EDIT: I was initially exploring the WPNAV_RADIUS parameter, as suggested in other similar posts. However, this parameter seems to be disabled in GUIDED mode.

Thanks beforehand,

Can you try AeduCopter 4.1.2 in SITL? That one sends a text message whenever a guided waypoint is reached,