ramp.c

Go to the documentation of this file.

#include "ramp.h"
#include <cfg/debug.h>

#include <string.h> // memcpy()

#define FIX_MULT32(a,b)  (((uint64_t)(a)*(uint32_t)(b)) >> 16)

void ramp_compute(struct Ramp *ramp, uint32_t clocksRamp, uint16_t clocksMinWL, uint16_t clocksMaxWL)
{
    ASSERT(clocksMaxWL >= clocksMinWL);

    // Save values in ramp struct
    ramp->clocksRamp = clocksRamp;
    ramp->clocksMinWL = clocksMinWL;
    ramp->clocksMaxWL = clocksMaxWL;

#if RAMP_USE_FLOATING_POINT
    ramp->precalc.gamma = ramp->clocksMaxWL - ramp->clocksMinWL;
    ramp->precalc.beta = (float)ramp->clocksMinWL * (float)ramp->clocksRamp;
    ramp->precalc.alpha = ramp->precalc.beta * (float)ramp->clocksMaxWL;

#else
    ramp->precalc.max_div_min = ((uint32_t)clocksMinWL << 16) / (uint32_t)clocksMaxWL;

    /* Calcola 1/total_time in fixed point .32. Assumiamo che la rampa possa al
     * massimo avere 25 bit (cioé valore in tick fino a 2^25, che con il
     * prescaler=3 sono circa 7 secondi). Inoltre, togliamo qualche bit di precisione
     * da destra (secondo quanto specificato in RAMP_CLOCK_SHIFT_PRECISION).
     */
    ASSERT(ramp->clocksRamp < (1UL << (24 + RAMP_CLOCK_SHIFT_PRECISION)));
    ramp->precalc.inv_total_time = 0xFFFFFFFFUL / (ramp->clocksRamp >> RAMP_CLOCK_SHIFT_PRECISION);
    ASSERT(ramp->precalc.inv_total_time < 0x1000000UL);

#endif
}


void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq)
{
    uint32_t minWL, maxWL;

    minWL = TIME2CLOCKS(FREQ2MICROS(maxFreq));
    maxWL = TIME2CLOCKS(FREQ2MICROS(minFreq));

    ASSERT2(minWL < 65536UL, "Maximum frequency too high");
    ASSERT2(maxWL < 65536UL, "Minimum frequency too high");
    ASSERT(maxFreq > minFreq);

    ramp_compute(
        ramp,
        TIME2CLOCKS(length),
        TIME2CLOCKS(FREQ2MICROS(maxFreq)),
        TIME2CLOCKS(FREQ2MICROS(minFreq))
    );
}

void ramp_default(struct Ramp *ramp)
{
    ramp_setup(ramp, RAMP_DEF_TIME, RAMP_DEF_MINFREQ, RAMP_DEF_MAXFREQ);
}

#if RAMP_USE_FLOATING_POINT

float ramp_evaluate(const struct Ramp* ramp, float curClock)
{
    return ramp->precalc.alpha / (curClock * ramp->precalc.gamma + ramp->precalc.beta);
}

#else

INLINE uint32_t fix_mult32(uint32_t m1, uint32_t m2)
{
    uint32_t accum = 0;
    accum += m1 * ((m2 >> 0) & 0xFF);
    accum >>= 8;
    accum += m1 * ((m2 >> 8) & 0xFF);
    accum >>= 8;
    accum += m1 * ((m2 >> 16) & 0xFF);
    return accum;
}

//   a*b >> 16
INLINE uint16_t fix_mult16(uint16_t a, uint32_t b)
{
    return (b*(uint32_t)a) >> 16;
}

uint16_t FAST_FUNC ramp_evaluate(const struct Ramp* ramp, uint32_t curClock)
{
    uint16_t t = FIX_MULT32(curClock >> RAMP_CLOCK_SHIFT_PRECISION, ramp->precalc.inv_total_time);
    uint16_t denom =  fix_mult16((uint16_t)~t + 1, ramp->precalc.max_div_min) + t;
    uint16_t cur_delta = ((uint32_t)ramp->clocksMinWL << 16) / denom;

    return cur_delta;
}

#endif