Use an XOR gate with the original 40 KHz, followed by a low pass filter to a PIC A/D input.
When the frequencies are exactly the same (no motion) the phase detector output will be steady (at some value, you don't care). When the phase detector output begins to change, you have motion (doppler effect but you already know that). The frequency of this phase detector output (after the low pass filter) will occur at the difference between transmitted and received frequencies.