tuned drive functions and logged nav data. good stable drive test. FINAL COMMIT FOR VERSION2