Add trim function

If button 1 on joy sitck is pressed, a command will be sent to the quad
which it execute trim function.  This sets the current command, yaw,
pitch, and roll to trim offsets