BeRTOS
ramp.c
Go to the documentation of this file.
00001 
00104 #include "ramp.h"
00105 #include <cfg/debug.h>
00106 
00107 #include <string.h> // memcpy()
00108 
00109 void ramp_compute(struct Ramp *ramp, uint32_t clocksRamp, uint16_t clocksMinWL, uint16_t clocksMaxWL)
00110 {
00111     ASSERT(clocksMaxWL >= clocksMinWL);
00112 
00113     // Save values in ramp struct
00114     ramp->clocksRamp = clocksRamp;
00115     ramp->clocksMinWL = clocksMinWL;
00116     ramp->clocksMaxWL = clocksMaxWL;
00117 
00118 #if RAMP_USE_FLOATING_POINT
00119     ramp->precalc.gamma = ramp->clocksMaxWL - ramp->clocksMinWL;
00120     ramp->precalc.beta = (float)ramp->clocksMinWL * (float)ramp->clocksRamp;
00121     ramp->precalc.alpha = ramp->precalc.beta * (float)ramp->clocksMaxWL;
00122 
00123 #else
00124     ramp->precalc.max_div_min = ((uint32_t)clocksMinWL << 16) / (uint32_t)clocksMaxWL;
00125 
00126     /* Calcola 1/total_time in fixed point .32. Assumiamo che la rampa possa al
00127      * massimo avere 25 bit (cioƩ valore in tick fino a 2^25, che con il
00128      * prescaler=3 sono circa 7 secondi). Inoltre, togliamo qualche bit di precisione
00129      * da destra (secondo quanto specificato in RAMP_CLOCK_SHIFT_PRECISION).
00130      */
00131     ASSERT(ramp->clocksRamp < (1UL << (24 + RAMP_CLOCK_SHIFT_PRECISION)));
00132     ramp->precalc.inv_total_time = 0xFFFFFFFFUL / (ramp->clocksRamp >> RAMP_CLOCK_SHIFT_PRECISION);
00133     ASSERT(ramp->precalc.inv_total_time < 0x1000000UL);
00134 
00135 #endif
00136 }
00137 
00138 
00139 void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq)
00140 {
00141     uint32_t minWL, maxWL;
00142 
00143     minWL = TIME2CLOCKS(FREQ2MICROS(maxFreq));
00144     maxWL = TIME2CLOCKS(FREQ2MICROS(minFreq));
00145 
00146     ASSERT2(minWL < 65536UL, "Maximum frequency too high");
00147     ASSERT2(maxWL < 65536UL, "Minimum frequency too high");
00148     ASSERT(maxFreq > minFreq);
00149 
00150     ramp_compute(
00151         ramp,
00152         TIME2CLOCKS(length),
00153         TIME2CLOCKS(FREQ2MICROS(maxFreq)),
00154         TIME2CLOCKS(FREQ2MICROS(minFreq))
00155     );
00156 }
00157 
00158 void ramp_default(struct Ramp *ramp)
00159 {
00160     ramp_setup(ramp, RAMP_DEF_TIME, RAMP_DEF_MINFREQ, RAMP_DEF_MAXFREQ);
00161 }
00162 
00163 #if RAMP_USE_FLOATING_POINT
00164 
00165 float ramp_evaluate(const struct Ramp* ramp, float curClock)
00166 {
00167     return ramp->precalc.alpha / (curClock * ramp->precalc.gamma + ramp->precalc.beta);
00168 }
00169 
00170 #else
00171 
00172 INLINE uint32_t fix_mult32(uint32_t m1, uint32_t m2)
00173 {
00174     uint32_t accum = 0;
00175     accum += m1 * ((m2 >> 0) & 0xFF);
00176     accum >>= 8;
00177     accum += m1 * ((m2 >> 8) & 0xFF);
00178     accum >>= 8;
00179     accum += m1 * ((m2 >> 16) & 0xFF);
00180     return accum;
00181 }
00182 
00183 //   a*b >> 16
00184 INLINE uint16_t fix_mult16(uint16_t a, uint32_t b)
00185 {
00186     return (b*(uint32_t)a) >> 16;
00187 }
00188 
00189 uint16_t FAST_FUNC ramp_evaluate(const struct Ramp* ramp, uint32_t curClock)
00190 {
00191     uint16_t t = FIX_MULT32(curClock >> RAMP_CLOCK_SHIFT_PRECISION, ramp->precalc.inv_total_time);
00192     uint16_t denom =  fix_mult16((uint16_t)~t + 1, ramp->precalc.max_div_min) + t;
00193     uint16_t cur_delta = ((uint32_t)ramp->clocksMinWL << 16) / denom;
00194 
00195     return cur_delta;
00196 }
00197 
00198 #endif
00199 
00200