FM demodulation with arctan
The $\arctan2()$ function has a jump at $\pm\pi$ which will cause problems when trying to compute a derivative.

Since you're using the first difference discrete derivative approximation to compute $d\phi/dt$, there's a simple way to avoid the jumps.

First note that your signal samples can be written polar form:

$$s[n] = i[n]+jq[n] = r[n]\cdot e^{\phi[n]}$$

so you can use multiplication by a complex conjugate to perfom angle subtraction before taking the $\arctan2()$:

$$s[n]\cdot s^*[n-1] = (i[n] +jq[n])(i[n-1]-jq[n-1]) = r[n]e^{\phi[n]}r[n-1]e^{-\phi[n-1]}=r[n] r[n-1] e^{\phi[n]-\phi[n-1]}$$

Thus, to get the discrete derivative you want, just take:

$$\mathrm{Arg}[(i[n]+jq[n])(i[n-1]-jq[n-1])]$$