comb5015  0.22 dspic part

Fix a important error in the code and test on dspic30f4012 use old code
runing much better.