Navdata problem. All 0s
Added a video thread, did not test yet
Added in test x, y, and yaw P controllers
Fixed the tag reader
Stuff
Fixed navdata