Change scale of z axis
Fix ping handling

TODO check if needed
Use switch to handle incoming message
Add screen to see and debug orientation of quad