actually dmp works; code refactor; enabled yaw pitch roll, debugging