used new pitch solution with a signed answer.
Changed to direct calculation of pitch

  - An important change in this revision, I changed from a
    vector solution to pitch using dot products to a direct
    calculation of pitch using the technique described in
    Freescale App Note 3461.
Removed unused variables