Added sendWaypointSetError(), sendWaypointAchieved() and sendWaypointTargeted(). sendWaypointSetError is not complete
Added processing for waypoints achieved, waypoint targetted and set waypoint error
Added extra debugging msg