Simplified error reporting

Only adjust existing waypoints. Does not add waypoints.

For much better performance

Some digital joysticks have already this correction
Add habilit to add or removecircle correction
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>

These are defined in PlanManager. Having them here as well causes lost
signaling.