If you are trying to write your own software, then nobody can help you without the relevant code fragments.
So, it's going to be up to you.
Fortunately, you have a tool available to you which will help you find out what is going on: the debugger. How you use it depends on your compiler system, but a quick Google for the name of your IDE and "debugger" should give you the info you need.
Put a breakpoint on the first line in the function, and run your code through the debugger. Then look at your code, and at your data and work out what should happen manually. Then single step each line checking that what you expected to happen is exactly what did. When it isn't, that's when you have a problem, and you can back-track (or run it again and look more closely) to find out why.
Sorry, but we can't do that for you - time for you to learn a new (and very, very useful) skill: debugging!
If you are trying to use software provided by the manufacturer, then we can't help at all. You should talk to the people who created it -
NYSC[
^] - they should provide technical support and will know more about their product than we will.