gpsrecer.c

在ARM7和UC/OSII的平台上实现了GPS自动报站的功能,涉及GPS模块LEA_4S的驱动,位置速寻算法,语音芯片ISD4004的录放音驱动,LED页面管理等等.从启动代码到操作系统的移植以及到业

/****************************************************************
**                                                              *
**  FILE         :  GpsRecer.C                                  *
**  COPYRIGHT    :  (c) 2001 .Xiamen Yaxon NetWork CO.LTD       *
**                                                              *
**                                                              *
**  By : CCH 2002.1.15                                          *
****************************************************************/
#include "includes.h"
#include "bsp.h"
#include "tools.h"
#include "ramimage.h"
#include "public.h"
#include "gpstool.h"
#include "gpsdrv.h"
#include "gpsrecer.h"

/*
********************************************************************************
* define config parameters
********************************************************************************
*/
#define MAX_INVALID                             60
#define MAX_WAIT                                6
#define MIN_VECTOR                              5           /* 保存方向的最小速度 */

/*
********************************************************************************
*                  DEFINE GPRMC MESSAGE FIELDS INFORMATION
********************************************************************************
*/
#define LEN_TIME                                6
#define LEN_LAT                                 9
#define LEN_LONG                                10
#define LEN_VECTOR                              5
#define LEN_DIRECTION                           5
#define LEN_DATE                                6
#define LEN_TOW                                 8

#define COMMA_TIME                              0
#define COMMA_VALID                             1
#define COMMA_LATITUDE                          2
#define COMMA_LONGITUDE                         4
#define COMMA_VECTOR                            6
#define COMMA_DIRECTION                         7
#define COMMA_DATE                              8

#define COMMA_HEIGHT                            10

#define COMMA_TOW                               3
#define COMMA_GPSWEEK                           4

#define CHAR_VALID                              'A'
#define CHAR_INVALID                            'V'


/*
********************************************************************************
*                  DEFINE MODULE VARIANT
********************************************************************************
*/
static INT8U  vector[LEN_VECTOR], direction[LEN_DIRECTION], time[LEN_TIME], date[LEN_DATE];
static INT8U  latitude[LEN_LAT], longitude[LEN_LONG], validflag;
static INT16U c_vector, c_direction,p_direction;
static INT8U  c_latitude[4], c_longitude[4];
static TIME_STRUCT c_time;
static DATE_STRUCT c_date;

static DATUM_STRUCT curdatum;
static BOOLEAN prevalid, datevalid;

static INT16S height;
static INT16U gpsweek;
static INT8U  utc_tow[LEN_TOW];

static INT8U  ct_invalid, ct_wait;

#if EN_GSV > 0
static INT8U gsv_cnt;
static INT8U gsv_status;
#endif

#if EN_GSV>0
static struct {
    INT8U   viewnum;                            /* 可见卫星数 */
    INT8U   SNR[50];                            /* 信噪比 */
    INT8U   elevation[50];                      /* 仰角 */
    INT16U  azimuth[50];                        /* 方位角 */
} SatInf, BKSatInf;
#endif

static struct {
    INT8U   mode;                               /* 定位模式，'1'：未定位，'2'：2D定位，'3'：3D定位 */
    INT8U   used;                               /* 所用卫星个数 */
    INT8U   PDOP;                               /* PDOP值 */
} FixInf;





static void ConvertLatitude(INT8U *dptr, INT8U *sptr)
{
    dptr[0]  = AsciiToDec(&sptr[0], 2);
    dptr[1]  = AsciiToDec(&sptr[2], 2);
    dptr[2]  = AsciiToDec(&sptr[5], 2);
    dptr[3]  = AsciiToDec(&sptr[7], 2);
}

static void ConvertLongitude(INT8U *dptr, INT8U *sptr)
{
    dptr[0] = AsciiToDec(&sptr[0], 3);
    dptr[1] = AsciiToDec(&sptr[3], 2);
    dptr[2] = AsciiToDec(&sptr[6], 2);
    dptr[3] = AsciiToDec(&sptr[8], 2);
}

static void ResolveValid(void)
{
    INT8U pos;

    pos = FindCharPos(MSG_GPRMC, ',', COMMA_VALID, sizeof(MSG_GPRMC)) + 1;
    validflag = MSG_GPRMC[pos];
}

static void ResolveVector(void)
{
    INT8U pos, len;

    MovStr(vector, "000.0");
    if (GpsDataValid()) {
        pos = FindCharPos(MSG_GPRMC, ',', COMMA_VECTOR, sizeof(MSG_GPRMC)) + 1;
        len = FindCharPos(&MSG_GPRMC[pos], '.', 0, sizeof(vector)) + 2;
        if (len <= sizeof(vector)) {
            memcpy(&vector[sizeof(vector) - len], &MSG_GPRMC[pos], len);
        }
        vector[4] = '0';
        c_vector = AsciiToDec(vector, 3);
        if (c_vector > 100) validflag = CHAR_INVALID;
    }
}

static void ResolveDirection(void)
{
    INT8U pos, len;

    MovStr(direction, "000.0");
    if (GpsDataValid()) {
        pos = FindCharPos(MSG_GPRMC, ',', COMMA_DIRECTION, sizeof(MSG_GPRMC)) + 1;
        len = FindCharPos(&MSG_GPRMC[pos], '.', 0, sizeof(direction)) + 2;
        if (len <= sizeof(direction)) {
            memcpy(&direction[sizeof(direction) - len], &MSG_GPRMC[pos], len);
        }
        c_direction = AsciiToDec(direction, 3);
        if (c_direction > 360) validflag = CHAR_INVALID;
    }
}

static void ResolveTime(void)
{
    INT8U pos;

    pos = FindCharPos(MSG_GPRMC, ',', COMMA_TIME, sizeof(MSG_GPRMC)) + 1;
    memcpy(time, &MSG_GPRMC[pos], sizeof(time));
    c_time.hour   = AsciiToDec(&time[0], 2);
    c_time.minute = AsciiToDec(&time[2], 2);
    c_time.second = AsciiToDec(&time[4], 2);
    if (!TimeValid(&c_time)) validflag = CHAR_INVALID;
}

static void ResolveDate(void)
{
    INT8U pos;

    pos = FindCharPos(MSG_GPRMC, ',', COMMA_DATE, sizeof(MSG_GPRMC)) + 1;
    memcpy(date, &MSG_GPRMC[pos], sizeof(date));
    c_date.year  = AsciiToDec(&date[4], 2);
    c_date.month = AsciiToDec(&date[2], 2);
    c_date.day   = AsciiToDec(&date[0], 2);
    if (!DateValid(&c_date)) validflag = CHAR_INVALID;
    if (MSG_GPRMC[pos + sizeof(date)] == ',') {
        datevalid = TRUE;
    } else {
        datevalid = FALSE;
    }
}

static void ResolveLatitude(void)
{
    INT8U pos;

    if (GpsDataValid()) {
        pos = FindCharPos(MSG_GPRMC, ',', COMMA_LATITUDE, sizeof(MSG_GPRMC)) + 1;
        memcpy(latitude, &MSG_GPRMC[pos], sizeof(latitude));
        ConvertLatitude(c_latitude, latitude);
        if (!LatitudeValid(c_latitude)) validflag = CHAR_INVALID;
    }
}

static void ResolveLongitude(void)
{
    INT8U pos, len;

    MovStr(longitude,  "00000.0000");
    if (GpsDataValid()) {
        pos = FindCharPos(MSG_GPRMC, ',', COMMA_LONGITUDE, sizeof(MSG_GPRMC)) + 1;
        len = FindCharPos(MSG_GPRMC + pos, '.', 0, sizeof(longitude));
        if (len > 5) len = 0;
        else len = 5 - len;
        memcpy(&longitude[len], &MSG_GPRMC[pos], sizeof(longitude) - len);
        ConvertLongitude(c_longitude, longitude);
        if (!LongitudeValid(c_longitude)) validflag = CHAR_INVALID;
    }
}

static void ResumeOldPos()
{
    validflag   = CHAR_INVALID;
    c_vector    = 0;
    c_direction = 0;
    memcpy(direction, "000.0", sizeof("000.0") - 1);
    memcpy(latitude,  OldGpsPos.latitude,  sizeof(latitude));
    memcpy(longitude, OldGpsPos.longitude, sizeof(longitude));
    ConvertLatitude(c_latitude, latitude);
    ConvertLongitude(c_longitude, longitude);
}

void ResolveGPRMC(void)
{

   
    ResolveValid();
    ResolveTime();
    ResolveDate();
    ResolveVector();
    ResolveLatitude();
    ResolveLongitude();
    ResolveDirection();
	
   if (!GpsDataValid()) {
        if (ct_invalid < MAX_INVALID) {
            ct_invalid++;
        } else {
            ct_wait = MAX_WAIT;                             /* 如持续60秒GPS无效，则定位后过滤前面的5个GPS点 */
        }
        ResumeOldPos();
    } else {
        ct_invalid = 0;
        if (ct_wait > 0) {
            ct_wait--;
        }
        if (ct_wait > 0) {
            validflag = CHAR_INVALID;                       /* 将GPS数据置成无效标志 */
            ResumeOldPos();
        } else {
            if (c_vector >= MIN_VECTOR) {
                p_direction = c_direction;
            }
            StoreOldPosition();
        }
    }
}
#if EN_GGA > 0
void ResolveGPGGA(INT8U *msgbuf, INT8U len)
{
    INT8U   pos;
    INT16U  tmp;
    BOOLEAN negative;
    
    if (GpsDataValid()) {                                               /* 只有在GPS数据有效情况下, 才解析GPS高程 */
        pos = FindCharPos(msgbuf, ',', COMMA_HEIGHT, len) + 1;          /* 解析GPS高程 */
        if (msgbuf[pos] == ',') return;                                 /* 高程数据为空 */
        if (msgbuf[pos] == '-') {                                       /* 表示高程为负数 */
            pos++;
            negative = true;
        } else {
            negative = false;
        }
        tmp = SearchDigitalString(&msgbuf[pos], 10, '.', 1);//找小数点
        if (tmp != 0xffff) {
            if (negative) {
                height = 0 - tmp;
            } else {
                height = tmp;
            }
        }
        StoreOldPosition();
    }
}
#endif
void ResolveGPGSA(INT8U *msgbuf, INT8U len)
{
    INT8U pos, i;
    INT8U  *ptr;
    
    FixInf.mode = '1';
    FixInf.used = 0;
    FixInf.PDOP = 99;

    pos  = FindCharPos(msgbuf, ',', 1, len) + 1;
    FixInf.mode = msgbuf[pos];                                          /* 获取定位模式 */
    pos += 2;
    if (len > pos) {
        len -= pos;
        ptr  = &msgbuf[pos];
    } else {
        return;
    }
    
    for (i = 1; i <= 12; i++) {                                         /* 计算所用卫星数 */
        if (*ptr != ',') {
            FixInf.used++;
            pos  = FindCharPos(ptr, ',', 0, len) + 1;
            ptr += pos;
        } else {
            pos = 1;
            ptr++;
        }
        if (len > pos) {
            len -= pos;
        } else {
            return;
        }
    }
    FixInf.PDOP = SearchDigitalString(ptr, len, '.', 1);                /* 获取PDOP值 */
}

#if EN_GSV>0
void ResolveGPGSV(INT8U *msgbuf, INT8U len)
{
    INT8U  tmp, index, pos, ct, t_msg, c_msg, num;
    INT16U tmpw;
    INT8U   *ptr;
    
    if (len < 12) return;
    msgbuf[len - 5] = ',';                                              /* 将校验码之前的'*'转换成',' */
    msgbuf[len - 4] = ',';
    
    pos   = FindCharPos(msgbuf, ',', 0, len);
    t_msg = msgbuf[pos + 1];                                            /* 获取GPGSV消息总数 */

    ptr = msgbuf;
    pos = FindCharPos(ptr, ',', 1, len);                                /* 搜索当前消息序号 */
    if (pos < len) {
        pos++;
        ptr  += pos;
        len  -= pos;
        c_msg = *ptr;                                                   /* 获取当前消息号 */
        if (c_msg == '1') {                                             /* 第一条GSV数据 */
            BKSatInf.viewnum = 0;
            memset(BKSatInf.SNR, 0, sizeof(BKSatInf.SNR));
            memset(BKSatInf.azimuth,   0xff, sizeof(BKSatInf.azimuth));
            memset(BKSatInf.elevation, 0xff, sizeof(BKSatInf.elevation));
        }
        pos = FindCharPos(ptr, ',', 0, len);                            /* 搜索可见卫星数 */
        if (pos < len) {
            pos++;
            ptr += pos;
            len -= pos;
            if (*ptr == ',') {
                return;
            } else {
                tmp = SearchDigitalString(ptr, len, ',', 1);
                BKSatInf.viewnum = tmp;