added most of things from debugging the dvl
added most of things from debugging the dvl

robotics-at-maryland/qubo,robotics-at-maryland/qubo,robotics-at-maryland/qubo,robotics-at-maryland/qubo,robotics-at-maryland/qubo