Injection functions period inconsistencies

[Bharat-Ramadas]

The injection command currently work by wrapping the time based on the command period and the control task time period (Ts). An example of how this wrapping takes place is shown below with the square injection example:

ctx->curr_time += Ts;
        if (ctx->curr_time >= ctx->square.period) {
            ctx->curr_time = 0.0;
        }

The above implementation leads to instances where curr_time is equal to both period and 0, leading to incorrect injections being generated. This is observed to happen primarily when the injection period is not an integer multiple of Ts.

Solution After discussing with @npetersen2, the proper implementaion of the above time wrapping was determined to be

ctx->curr_time += Ts;
        if (ctx->curr_time >= ctx->square.period) {
            ctx->curr_time -= ctx->square.period;
        }

By subtracting the period from curr_time instead of setting it to 0, we can fix this bug as the curr_time will never equal both period and 0 in over 2 consecutive cycles.