Yaw control motor direction fix. C# trim function

The sign for yaw terms were backward.  Also added functionality to trim
pitch roll and throttle.