Changed filter coefficients to correct ones and got FIR working completely