BeRTOS
|
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