Resolve segmentation fault erro on gps points remove.
Fix error on reconnection
add Navigation System speed setpoint control