When using 'GUIDED' mode, is there a way to figure out if a particular way-point has been reached? I am working on a program which basically does the following:
1) Set a way-point using 'MAV_CMD_NAV_WAYPOINT', make sure that the APM ACKs it (i.e. sends a MAVLINK_MSG_ID_MISSION_ACK).
2) Wait till the way-point is reached (i.e. the program should know that waypoint has reached).
3) Perform some action (at the very minimum, log this).
Start from (1) with a new waypoint.
(1) works fine - the UAV moves to the desired waypoint. Facing an issue with (2) - I am expecting a 'MAV_MSG_ID_MISSION_ITEM_REACHED' message to be received when the waypoint is reached. However, the program does not seem to be receiving any such message.
Would like to know if it is even possible with GUIDED mode. If it is, any pointers on which message to wait for will be really helpful.
Found that 'NAV_CONTROLLER_OUTPUT' has 'wp_dist' (distance to waypoint) and this is one of the telemetry messages. However, this seems to consider only the horizontal distance - for waypoints with same lat,long but different altitude, wp_dist returns zero.