GPS functions, carmode implementation, shakiness work