mylib.c

Keil平台

        str[1] = (yy/100)%10 + '0';
        str[2] = (yy/10)%10 + '0';
        str[3] = yy%10 + '0';
        str[4] = str[7] = '-';

        str[5] = (mm/10)%10 + '0';
        str[6] = mm%10 + '0';
        str[8] = (dd/10)%10 + '0';
        str[9] = dd%10 + '0';
        str[10] = '\0';
        return str;
    }
    else
    {
        str[0] = (yy/10)%10 + '0';
        str[1] = yy%10 + '0';
        str[2] = '-';
        str[3] = (mm/10)%10 + '0';
        str[4] = mm%10 + '0';
        str[5] = '-';
        str[6] = (dd/10)%10 + '0';
        str[7] = dd%10 + '0';
        str[8] = '\0';
        return str;
    }
}

int GetItemCount(const char  *str)
{
    int count = 0;
    while ( *str++)
    {
        if (*str == '\n')
            count++;
    }
    return count;
}
/*value = 1  tag="自动\n手动", then str=手动, rt=len */
int GetItem(int value,const char  *str, char *word)
{
    uchar len = 0,index = 0;
    len = GetItemCount(str);
    if (value > len)
    {
        value = len;
    }
    len = 0;
    while ( (*str!= '\0') && (index < value))
    {
        if (*str++ == '\n')
            index++;
    }
    while ((*str!= '\0') && (*str!= '\n'))
    {
        *word++ = *str++;
        len++;
    }
    *word = '\0';
    return len;
}
int left_nrm(char *str, int len)
{/* normalize */
    if (*str == '-')
        str++;
    while (*str)
    {
        if ( (*str > '0') && (*str <= '9') )
            break;
        if ( (*(str+1) < '0') || (*(str+1) > '9') )
            break;
        if (*str == '0') 
            *str = ' ';
        str++;
    }
//    *str = '\0';
    return len;
}

int  TextFormat(char *buf, char *fmt, int data)
{
    uchar ch;   /* character in format string     */
    uchar i;
    int len = 0, vlen = 0;
    uchar format = 0, flag = 0;   /* b7 - zero fill float       */
    int min, max, mult;
    format = 0;
    for( ;; )
    {
        min = max = 0;
        ch = *fmt++;
        if( !ch )
            break;
        else if( ch == '%' )
        {
            while( 1 )
            {
                ch = *fmt++;
                if( ch == '-' )
                    flag |= 0x40;	/* left alignment */
                else if( ch == '+' )
                    flag = (format & 0xF3) | 0x04;
                else if( ch == ' ' )
                {
                    /* do not overrule '+'	*/
                    if( !(format & 0x0C) )
                        flag |= 0x08;	/* sign or space */
                }
                else if( ch == '#' )
                    flag |= 0x10;	/* alternate print */
                else
                    break;
            }
            while( ch >= '0' && ch <= '9' )
            {   /* get minimum field width */
                min = min * 10 + ch - '0';
                ch = *fmt++;
            }
            if( ch == '.' || ch == '~' )
            {   /* next argument is maximum field width specifier */
                ch = *fmt++;
                while( ch >= '0' && ch <= '9' )
                {   /* get maximum field width */
                    max = max * 10 + ch - '0';
                    ch = *fmt++;
                }
            }
            switch (ch)
            {
            case 'd':
                if (min == 0 && max== 0)
                    min = 5;
                format = (max&0x0F) | (min<<4);
                len = fix2s(data, format&0x7F, &buf[len]) + len;
                vlen = len;
                break;
            case 'x':
            case 'X':
                i2hex(data, &buf[len]);
                for (i = len; i < 4-min+len; i++)
                   buf[i] = buf[i+4-min];
                buf[min] = '\0';
                len = min;
                vlen = len;
                break;
            case 's':
                //ss
                break;
            case 'b':
                break;
            case 'm':
                mult = min;
                break;
            default:
                buf[len++] = ch;
                break;
            }
        }
        else
        {
            buf[len++] = ch;
        }
    }
    buf[len] = '\0';
    return vlen;
}

/****************************************************************************/
/****************************************************************************/
int ad2abs(int value)
{
    if ((value & 0x2000) != 0)
    {
        value = ((~value) + 1) & 0x1FFF;
    }
    else
    {
        value = value & 0x1FFF;
    }
    return value;
}
/****************************************************************************/
int ad2i(uint value)
{
    if ((value & 0x2000) != 0)
    {
        value = ((~value) + 1) & 0x1FFF;
        value = -value;
    }
    else
    {
        value = value & 0x1FFF;
    }
    return value;
}
/****************************************************************************/
int CDT_Head(uchar *buf)
{
    if ((buf[0] == 0xeb)
        &&(buf[1] == 0x90)
        &&(buf[2] == 0xeb)
        &&(buf[3] == 0x90)
        &&(buf[4] == 0xeb)
        &&(buf[5] == 0x90) )
        return 1;
    else
        return 0;
}
/****************************************************************************/
ulong CalcSqrt(ulong Data)
{
    unsigned long R20,R24,Rd;
    unsigned long R26;

    Rd=Data;
    for(R26=0;R26<32;R26++)
    {
        if(Data>=0x80000000)
            break;
        Data<<=1;
    }
    if(R26==32)
        return 0;
    else
    {
        if(R26<16)
            R24=0xff;
        else
        {
            R26>>=1;
            R24=0xffff;
            R24>>=R26;
        }
        while(1)
        {
            R20=R24;
            R24=(Rd/R20+R20)>>1;
            if(R24-R20<=1)
                break;
        }
    }
    return ((unsigned long)R24);
}
/****************************************************************************/
/****************************************************************************/
uint Check_Len(uint value)
{
    value &= 0x0FFF;
    value |= ( (  (value>>8)&0x0F+(value>>4)&0x0F+(value)&0x0F  )<<12 )&0xF000;
    return value;
}
/****************************************************************************/
void Sort_Buf(uint *buf, int bufsize)
{
    int m, n, itemp;
    for (m = 0; m < bufsize; m++)
    {
        for ( n = m - 1; n >= 0; n--)
        {
            if (buf[n+1] > buf[n])
            {
                itemp = buf[n];
                buf[n] = buf[n+1];
                buf[n+1] = itemp;
            }
        }
    }
}
/****************************************************************************/
int Filter_Avg(int *buf, int bufsize)
{
    long sum;
    int i;
    sum = 0;
    for (i = 0; i < bufsize; i++)
    {
        sum += buf[i];
    }
    sum /= bufsize;
    return (int)sum;
}
/****************************************************************************/
void Filter_Midst(int *buf, int bufsize, int width)
{
    int i;
    for (i = 0; i < bufsize-width; i++)
    {
        buf[i] = Midst(&buf[i], width);
    }
    for (i = bufsize-width; i < bufsize; i++)
    {
        buf[i] = buf[bufsize-width-1];
    }
}
/****************************************************************************/
/* 求中间值 返回中间值*/
int Midst(int *pbuf, int width)
{
    int m, n, itemp;
    int buf[10];
    for (m = 0; m < width; m++)
    {
        buf[m] = pbuf[m];
    }
    for (m = 0; m < width; m++)
    {
        for ( n = m - 1; n >= 0; n--)
        {
            if (buf[n+1] > buf[n])
            {
                itemp = buf[n];
                buf[n] = buf[n+1];
                buf[n+1] = itemp;
            }
        }
    }
    return buf[width/2];
}
/****************************************************************************/
void Left_Roll(int *buf, int bufsize)
{
    int i;
    for (i = 0; i < bufsize-1; i++)
    {
        buf[i] = buf[i+1];
    }
}
/****************************************************************************/
/* 
  0   1   2   3   4   5   6   7
+---+---+---+---+---+---+---+----+
|dek|ek | y | r | p | i | d |flag|
+---+---+---+---+---+---+---+----+
*/
int InitPid(int *buf, int p, int i, int d, int r)
{
    buf[0] = 0;
    buf[1] = 0;
    buf[2] = r;
    buf[3] = r;
    buf[4] = p;
    buf[5] = i;
    buf[6] = d;
}
int PidSet(int *buf, int p, int i, int d)
{
    buf[4] = p;
    buf[5] = i;
    buf[6] = d;
}
int PidReset(int *buf, int y, int r)
{
    buf[0] = r - y;
    buf[1] = r - y;
    buf[2] = buf[3];
	return PidAdjust(buf, y, r);
}
/****************************************************************************/
/*
  0   1   2   3   4   5   6   7
+---+---+---+---+---+---+---+----+
|dek|ek | y | r | p | i | d |flag|
+---+---+---+---+---+---+---+----+
*/
int PidAdjust(int *buf, int y, int r)
{
	int du;
	int flag;
	int ek = 0, dek = 0, dek1 = 0;
	long int P = 0, I = 0, D = 0, E = 0;
	P = buf[4];
	I = buf[5];
	D = buf[6];
	buf[2] = y;
	buf[3] = r;

	ek   = r - y;//ek
	dek  = ek - buf[1];
	dek1 = buf[0];

	buf[1] = ek;
	buf[0] = dek;

	/* du = P(dek + Iek + Dek1)*/
	I *= (long int)ek;
//	I /= 100;
	D *= (long int)dek1;
//	D /= 100;
	dek = ek;
	E = (long int)dek - I + D;
	P *= E;
    P /= 100;
    du = (int)P;
    
    return du;
}
/*****************************************************************************/
uchar GetBufBit(uchar *buf, uint index)
{
    return buf[index>>3] & MaskCode[index&0x07]?0:1;
}
/****************************************************************************/
void SetBufBit(uchar *buf, uint index, uchar value)
{
    if (value)
        buf[index>>3] |= MaskCode[index&0x07];
    else
        buf[index>>3] &= ~MaskCode[index&0x07];
}
/****************************************************************************/