moved things around to support vn100 and vn200.
Fixed RPY - > Quaternion conversion.
trivial cleanup