// first parameter should be a (struct pid_filter *)
int32_t pid_do_filter( void * data, int32_t in )
{
int32_t derivate ;
int32_t command ;
struct pid_filter * p = data;
uint8_t prev_index;
/*
* Integral value : the integral become bigger with time .. (think
* to area of graph, we add one area to the previous) so,
* integral = previous integral + current value
*/
/* derivate value
* f(t+h) - f(t) with f(t+h) = current value
* derivate = ------------- f(t) = previous value
* h
* so derivate = current error - previous error
*
* We can apply a filter to reduce noise on the derivate term,
* by using a bigger period.
*/
prev_index = p->index + 1;
if( prev_index >= p->derivate_nb_samples )
{
prev_index = 0;
}
// saturate input... it influences integral an derivate
if( p->max_in )
{
S_MAX(in, p->max_in) ;
}
derivate = in - p->prev_samples[prev_index];
p->integral += in ;
if( p->max_I )
{
S_MAX(p->integral, p->max_I) ;
}
// so, command = P.coef_P + I.coef_I + D.coef_D
command = in * p->gain_P +
p->integral * p->gain_I +
( derivate * p->gain_D ) / p->derivate_nb_samples ;
if( command < 0 )
{
command = -( -command >> p->out_shift );
}
static size_t get_cmp_size(const ss_t *s1, const ss_t *s2)
{
if (!s1 || !s2)
return 0;
size_t s1_size = sd_get_size(s1), s2_size = sd_get_size(s2);
return S_MAX(s1_size, s2_size);
}
/* global SET FUNCTION : we use a (void *) to be compliant with
* control_system functions. For instance pwm_set((void *)(PWM1B_NUM), x)
* is equivalent to pwm_set_1B(x) */
void pwm_set(void * data, int32_t value)
{
uint8_t num = (uint8_t)(int)data;
S_MAX(value, 0x7FFF);
switch(num)
{
#if (defined PWM0_NUM) && (defined PWM0_ENABLED)
case PWM0_NUM:
pwm_set_0(value);
break;
#endif
#if (defined PWM1A_NUM) && (defined PWM1A_ENABLED)
case PWM1A_NUM:
pwm_set_1A(value);
break;
#endif
#if (defined PWM1B_NUM) && (defined PWM1B_ENABLED)
case PWM1B_NUM:
pwm_set_1B(value);
break;
#endif
#if (defined PWM1C_NUM) && (defined PWM1C_ENABLED)
case PWM1C_NUM:
pwm_set_1C(value);
break;
#endif
#if (defined PWM2_NUM) && (defined PWM2_ENABLED)
case PWM2_NUM:
pwm_set_2(value);
break;
#endif
#if (defined PWM3A_NUM) && (defined PWM3A_ENABLED)
case PWM3A_NUM:
pwm_set_3A(value);
break;
#endif
#if (defined PWM3B_NUM) && (defined PWM3B_ENABLED)
case PWM3B_NUM:
pwm_set_3B(value);
break;
#endif
#if (defined PWM3C_NUM) && (defined PWM3C_ENABLED)
case PWM3C_NUM:
pwm_set_3C(value);
break;
#endif
default:
break;
}
}
S_API double s_rel_diff(double a, double b)
{
double c = S_ABS(a);
double d = S_ABS(b);
d = S_MAX(c, d);
return d == 0.0 ? 0.0 : (S_ABS(a - b) / d);
}
byte IrRecv::available()
{
byte result ;
word t = time ;
word i = (base + 1) % TABLE_SIZE ;
word now = micros() ;
if( (cnt != base) && ((now - t) > GAP) ) {
// cntまで解析して、buffに書く
if( S_MAX( SONY_START_H, table[i] ) ) sonyFormat() ;
else if( S_MAX( KASE_START_H, table[i] ) ) kaseikyoFormat() ;
else if( S_MAX( NEC_START_H, table[i] ) ) necFormat() ;
base = cnt ;
result = (wp + BUFF_SIZE - rp) % BUFF_SIZE ;
}
else if( rp != wp ) {
result = (wp + BUFF_SIZE - rp) % BUFF_SIZE ;
}
else {
result = 0 ;
}
return result ;
}
void set_pwm_motor3(void* dummy, int32_t pwm)
{
S_MAX(pwm,4095);
if( (pwm == 4095)||(pwm == -4095))
DEBUG(MOTORCS_ERROR,"PWM#3 saturated pwm=%d",pwm);
if(pwm>0)
{
pwm_set_1A(4095-pwm);
//sbi(MOTOR_CS_PWM3_PORT,MOTOR_CS_PWM3_PIN);
motor3_sign = 1;
}
else
{
pwm_set_1A(-pwm);
//cbi(MOTOR_CS_PWM3_PORT,MOTOR_CS_PWM3_PIN);
motor3_sign = 0;
}
return;
}
void IrRecv::necFormat( void )
{
byte data = 0 ;
byte fg = 1 ;
word wps = (wp + 1) % BUFF_SIZE ;
word i = (base + 2) % TABLE_SIZE ;
if( S_MAX( NEC_REPEAT_L, table[i] ) ) {
buff[wp] = NEC_REPEAT ;
wp = (wp + 1) % BUFF_SIZE ;
}
else {
i = (i + 1) % TABLE_SIZE ;
while( i != cnt ) {
if( (i & 1) == 1 ) {
// 奇数の場合は、点灯時間
if( !V_MAX( NEC_DATA_H, table[i] ) ) {
goto NEC_ERROR ;
}
}
else {
// 偶数の場合は、消灯時間
if( V_MAX( NEC_ONE_L, table[i] ) ) {
data |= fg ;
}
else if( V_MAX( NEC_ZERO_L, table[i] ) ) {
data &= ~fg ;
}
else {
break ;
}
fg <<= 1 ;
if( fg == 0 ) {
buff[wps] = data ;
data = 0 ;
fg = 1 ;
wps = (wps + 1) % BUFF_SIZE ;
}
}
i = (i+1) % TABLE_SIZE ;
}
word j = (i + TABLE_SIZE - base) % TABLE_SIZE ;
if( j == 68 ) buff[wp] = NEC ;
else goto NEC_ERROR ;
base = i ;
wp = wps ;
}
return ;
NEC_ERROR:
buff[wp] = ERROR ;
wp = (wp+1) % BUFF_SIZE ;
// baseをcntまたはGAPまで進める
while( i != cnt ) {
if( table[i] > GAP ) {
break ;
}
i = (i + 1) % TABLE_SIZE ;
}
base = i ;
return ;
}
static qboolean GL_LightPoint( vec3_t origin, vec3_t color ) {
bsp_t *bsp = gl_static.world.cache;
mface_t *surf;
int s, t, i;
byte *lightmap;
byte *b1, *b2, *b3, *b4;
int fracu, fracv;
int w1, w2, w3, w4;
byte temp[3];
int smax, tmax, size;
lightstyle_t *style;
vec3_t point;
if( !bsp || !bsp->lightmap ) {
return qfalse;
}
point[0] = origin[0];
point[1] = origin[1];
point[2] = origin[2] - 8192;
surf = BSP_LightPoint( bsp->nodes, origin, point, &s, &t );
if( !surf ) {
return qfalse;
}
fracu = s & 15;
fracv = t & 15;
w1 = ( 16 - fracu ) * ( 16 - fracv );
w2 = fracu * ( 16 - fracv );
w3 = fracu * fracv;
w4 = ( 16 - fracu ) * fracv;
s >>= 4;
t >>= 4;
smax = S_MAX( surf );
tmax = T_MAX( surf );
size = smax * tmax * 3;
VectorClear( color );
lightmap = surf->lightmap;
for( i = 0; i < surf->numstyles; i++ ) {
b1 = &lightmap[3 * ( ( t + 0 ) * smax + ( s + 0 ) )];
b2 = &lightmap[3 * ( ( t + 0 ) * smax + ( s + 1 ) )];
b3 = &lightmap[3 * ( ( t + 1 ) * smax + ( s + 1 ) )];
b4 = &lightmap[3 * ( ( t + 1 ) * smax + ( s + 0 ) )];
temp[0] = ( w1 * b1[0] + w2 * b2[0] + w3 * b3[0] + w4 * b4[0] ) >> 8;
temp[1] = ( w1 * b1[1] + w2 * b2[1] + w3 * b3[1] + w4 * b4[1] ) >> 8;
temp[2] = ( w1 * b1[2] + w2 * b2[2] + w3 * b3[2] + w4 * b4[2] ) >> 8;
style = LIGHT_STYLE( surf, i );
color[0] += temp[0] * style->rgb[0];
color[1] += temp[1] * style->rgb[1];
color[2] += temp[2] * style->rgb[2];
lightmap += size;
}
GL_AdjustColor( color );
return qtrue;
}
