gpstool.c

在ST单片机上实现GPS交通警示器功能.

/****************************************************************
**                                                              *
**  FILE         :  GpsTool.C                                   *
**  COPYRIGHT    :  (c) 2001 .Xiamen Yaxon NetWork CO.LTD       *
**                                                              *
**                                                              *
**  By : CCH 2002.1.15                                          *
****************************************************************/
#include "system.h"
#include "tools.h"


static void Add_hundred(INT8U *dptr, INT8U *ptr1, INT8U *ptr2)
{
    int   i;
    INT8U temp, carried;
    
    carried = 0;
    for (i = 3; i >= 0; i--) {
        temp    = ptr1[i] + ptr2[i] + carried;
        dptr[i] = temp % 100;
        carried = temp / 100;
    }
}

static BOOLEAN Dec_hundred(INT8U *dptr, INT8U *ptr1, INT8U *ptr2)
{
    int     i;
    INT8U   carried, *ptr;
    BOOLEAN ret;
    
    if (CmpString(ptr1, ptr2, 4) == STR_LESS) {
        ptr  = ptr2;
        ptr2 = ptr1;
        ptr1 = ptr;
        ret  = FALSE;
    } else {
        ret  = TRUE;
    }
    
    carried = 0;
    for (i = 3; i >= 0; i--) {
        if (ptr1[i] < (ptr2[i] + carried)) {
            dptr[i] = (ptr1[i] + 100) - (ptr2[i] + carried);
            carried = 1;
        } else {
            dptr[i] = ptr1[i] - (ptr2[i] + carried);
            carried = 0;
        }
    }
    return ret;
}

static void Mul_hundred_b(INT8U *dptr, INT8U *ptr, INT8U mul)
{
    int    i;
    INT8U  carried;
    INT16U temp;
    
    carried = 0;
    for (i = 3; i >= 0; i--) {
        temp    = ptr[i] * mul + carried;
        dptr[i] = temp % 100;
        carried = temp / 100;
    }
}

static void Mul_hundred(INT8U *dptr, INT8U *ptr, INT16U mul)
{
    INT8U tbuf[5];
    
    tbuf[0] = 0;
    Mul_hundred_b(&tbuf[1], ptr, mul);
    Mul_hundred_b(dptr,     ptr, mul >> 8);
    Add_hundred(dptr, dptr, tbuf);
}

/* ptr*100 / ptr2 */
static INT8U Div_hundred(INT8U *ptr1, INT8U *ptr2)
{
    INT8U tbuf[4], res;
    
    if (CmpString(ptr1, ptr2, 4) != STR_LESS) return 100;
    tbuf[0] = ptr1[1];
    tbuf[1] = ptr1[2];
    tbuf[2] = ptr1[3];
    tbuf[3] = 0;
    res = 0;
    for (;;) {
        if (Dec_hundred(tbuf, tbuf, ptr2)) res++;
        else return res;
        if (res >= 100) return 100;
    }
}

static void DegreeToHundred(INT8U *dptr, INT8U *sptr)
{
    INT16U temp;
    
    temp    = sptr[0] * 60 + sptr[1];
    dptr[1] = temp % 100;
    dptr[0] = temp / 100;
    dptr[2] = sptr[2];
    dptr[3] = sptr[3];
}


static code INT16U tbl_cos[91] = 
        { 0x6363, 0x6362, 0x635d, 0x6356, 0x634b, 0x633d, 0x632d, 0x6319, 0x6302, 0x624c,
          0x6230, 0x6210, 0x6151, 0x612b, 0x6102, 0x603b, 0x600c, 0x5f3f, 0x5f0a, 0x5e37,
          0x5d60, 0x5d23, 0x5c47, 0x5c05, 0x5b23, 0x5a3f, 0x5957, 0x590a, 0x581d, 0x572e,
          0x563c, 0x5547, 0x5450, 0x5356, 0x525a, 0x515b, 0x505a, 0x4f56, 0x4e50, 0x4d47,
          0x4c3c, 0x4b2f, 0x4a1f, 0x490d, 0x475d, 0x4647, 0x452e, 0x4413, 0x425b, 0x413c,
          0x401b, 0x3e5d, 0x3d38, 0x3c12, 0x3a4d, 0x3923, 0x375b, 0x362e, 0x3463, 0x3332,
          0x3200, 0x3030, 0x2e5e, 0x2d27, 0x2b53, 0x2a1a, 0x2843, 0x2707, 0x252e, 0x2353,
          0x2214, 0x2037, 0x1e5a, 0x1d17, 0x1b38, 0x1958, 0x1813, 0x1631, 0x144f, 0x1308,
          0x1124, 0x0f40, 0x0d5b, 0x0c12, 0x0a2d, 0x0847, 0x0661, 0x0517, 0x0330, 0x014a, 
          0x0000 };
          
          
static code INT16U tbl_squareroot[101] = 
        { 0x073c, 0x0729, 0x0717, 0x0705, 0x06f3, 0x06e1, 0x06d0, 0x06bf, 0x06ae, 0x069d,
          0x068d, 0x067c, 0x066c, 0x065d, 0x064d, 0x063e, 0x062f, 0x0621, 0x0612, 0x0604,
          0x05f7, 0x05e9, 0x05dc, 0x05d0, 0x05c4, 0x05b8, 0x05ac, 0x05a1, 0x0596, 0x058c,
          0x0582, 0x0578, 0x056f, 0x0567, 0x055e, 0x0557, 0x054f, 0x0549, 0x0542, 0x053c,
          0x0537, 0x0532, 0x052e, 0x052a, 0x0526, 0x0524, 0x0521, 0x051f, 0x051e, 0x051d,
          0x051d, 0x051d, 0x051e, 0x051f, 0x0521, 0x0524, 0x0526, 0x052a, 0x052e, 0x0532,
          0x0537, 0x053c, 0x0542, 0x0549, 0x054f, 0x0557, 0x055e, 0x0567, 0x056f, 0x0578,
          0x0582, 0x058c, 0x0596, 0x05a1, 0x05ac, 0x05b8, 0x05c4, 0x05d0, 0x05dc, 0x05e9,
          0x05f7, 0x0604, 0x0612, 0x0621, 0x062f, 0x063e, 0x064d, 0x065d, 0x066c, 0x067c,
          0x068d, 0x069d, 0x06ae, 0x06bf, 0x06d0, 0x06e1, 0x06f3, 0x0705, 0x0717, 0x0729,
          0x073c };


INT32U CalculateGpsSpace(INT8U *latitude1, INT8U *longitude1, INT8U *latitude2, INT8U *longitude2)
{
    INT8U  dx[4], dy[4], tbuf[5], angle;
    INT16U temp1, temp2;

    angle = latitude1[0];
    DegreeToHundred(dx,   latitude1);
    DegreeToHundred(tbuf, latitude2);
    Dec_hundred(dx, dx, tbuf);
    
    DegreeToHundred(dy,   longitude1);
    DegreeToHundred(tbuf, longitude2);
    Dec_hundred(dy, dy, tbuf);
    
    if (dx[0] > 0 || dy[0] > 0) return 0xffffffff;

    tbuf[0] = 0;
    Mul_hundred(&tbuf[1], dy, tbl_cos[angle]);
    Add_hundred(tbuf, dx, tbuf);
    temp1 = tbl_squareroot[Div_hundred(dx, tbuf)];
    if (tbuf[0] == 0 && tbuf[1] == 0) {
        temp2 = tbuf[2] * 100 + tbuf[3];
        return (INT32U)temp1 * temp2 / 10000L;
    } else if (tbuf[0] == 0) {
        temp2 = tbuf[1] * 100 + tbuf[2];
        return (INT32U)temp1 * temp2 / 100L;
    } else {
        temp2 = tbuf[0] * 100 + tbuf[1];
        return (INT32U)temp1 * temp2;
    }
}