Updated for newest firmware and rev two troubles