Thanks everyone,
I also managed to find some relevent equations on the web. Like ATCast said, the Rayleigh equations are needed when there is a supersonic shock across the pitot tube.
For the subsonic case, the equations here would be appropriate.
Isentropic Flow Equations
Take equation 6, substitute the specific heat ratio = 1.4 and manipulate it to make M the subject. You then have the equation to program your air data computer to read Mach number given total and static pressure inputs. (OK I don't have an air data computer, but I can do a Excel spreadsheet simulation.)
Now to get TAS your computer also needs a TAT input (kelvins). Use equation 7 to derive SAT from TAT with the Mach number you already have. Then:
TAS= 661.5*M*sqrt(SAT/288.15)