1
0
mirror of https://github.com/arduino/Arduino.git synced 2024-12-12 23:08:52 +01:00
Arduino/build/shared/lib/avrlib/rsl/mitelgps.c
2005-08-25 21:06:28 +00:00

289 lines
7.8 KiB
C
Executable File

/*! \file .c \brief Mitel GPS STX/ETX driver function library. */
//*****************************************************************************
//
// File Name : 'mitelgps.c'
// Title : Mitel GPS STX/ETX driver function library
// Author : Pascal Stang - Copyright (C) 2002
// Created : 2003.04.11
// Revised : 2003.08.26
// Version : 0.1
// Target MCU : Atmel AVR Series
// Editor Tabs : 4
//
// NOTE: This code is currently below version 1.0, and therefore is considered
// to be lacking in some functionality or documentation, or may not be fully
// tested. Nonetheless, you can expect most functions to work.
//
// This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************
#ifndef WIN32
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <stdlib.h>
#endif
#include "global.h"
#include "buffer.h"
#include "rprintf.h"
#include "uart2.h"
#include "gps.h"
#include "mitelgps.h"
// Program ROM constants
// Global variables
// external GPS information structure/repository (in gps.h/gps.c)
extern GpsInfoType GpsInfo;
// packet processing buffer
u08 MitelGpsPacket[MITELGPS_BUFFERSIZE];
// debug flag
u08 debug;
#define MITELGPS_DEBUG_PKTPARSE 0x01
#define MITELGPS_DEBUG_EXTRACT 0x02
// function pointer to single byte output routine
static void (*TxByteFunc)(unsigned char c);
void mitelgpsInit(void (*txbytefunc)(unsigned char c))
{
// set transmit function
// (this function will be used for all SendPacket commands)
TxByteFunc = txbytefunc;
// set debug status
debug = 0;
}
void mitelgpsSendPacket(u08* data, u08 dataLength)
{
u08 i;
u08 dataIdx = 0;
u08 checksum = 0;
// start of packet
MitelGpsPacket[dataIdx++] = STX;
// add packet type and packet data
for(i=0; i<dataLength; i++)
{
checksum ^= MitelGpsPacket[dataIdx];
MitelGpsPacket[dataIdx++] = *data++;
}
// checksum
convertIntToAsciiHex(checksum, &MitelGpsPacket[dataIdx], 2);
dataIdx += 2;
// end of packet
MitelGpsPacket[dataIdx++] = ETX;
// send it
for(i=0; i<dataIdx; i++)
TxByteFunc(MitelGpsPacket[i]);
}
u08 mitelgpsProcess(cBuffer* rxBuffer)
{
u08 foundpacket = FALSE;
u08 startFlag = FALSE;
u08 checksum = 0;
u08 packetType;
u16 i,j;
// process the receive buffer
// go through buffer looking for packets
while(rxBuffer->datalength > 1)
{
// look for a start of Mitel GPS STX/ETX packet
if(bufferGetAtIndex(rxBuffer,0) == STX)
{
// found start
startFlag = TRUE;
// done looking for start
break;
}
else
// not STX, dump character from buffer
bufferGetFromFront(rxBuffer);
}
// if we detected a start, look for end of packet
if(startFlag)
{
for(i=1; i<(rxBuffer->datalength); i++)
{
// check for end of Mitel GPS STX/ETX packet
if(bufferGetAtIndex(rxBuffer,i) == ETX)
{
// have a packet end
// dump initial STX
bufferGetFromFront(rxBuffer);
// copy data to MitelGpsPacket
for(j=0; j<(i-1); j++)
{
MitelGpsPacket[j] = bufferGetFromFront(rxBuffer);
checksum ^= MitelGpsPacket[j];
}
// null-terminate copied string
MitelGpsPacket[j] = 0;
// dump ending ETX
bufferGetFromFront(rxBuffer);
// verify checksum
// undo checksum summing of the checksum itself
checksum ^= MitelGpsPacket[j-2];
checksum ^= MitelGpsPacket[j-1];
if( checksum == convertAsciiHexToInt(&MitelGpsPacket[j-2], 2) )
{
// found a good packet
if(debug & MITELGPS_DEBUG_PKTPARSE)
{
rprintf("Rx Mitel GPS packet type: %c%c%c len: %d\r\n",
MitelGpsPacket[0], MitelGpsPacket[1], MitelGpsPacket[2], j);
rprintfStr(MitelGpsPacket);
rprintfCRLF();
}
// done with this processing session
foundpacket = TRUE;
break;
}
else
{
if(debug & MITELGPS_DEBUG_PKTPARSE)
{
rprintf("Rx Mitel GPS packet type: %c%c%c len: %d Bad Checksum Rcvd: 0x%c%c Calc: 0x%x\r\n",
MitelGpsPacket[0], MitelGpsPacket[1], MitelGpsPacket[2], j, MitelGpsPacket[j-2], MitelGpsPacket[j-1], checksum);
}
}
}
}
}
// handle and direct the received packet
if(foundpacket)
{
// switch on the packet type
packetType = convertAsciiHexToInt(&MitelGpsPacket[1], 2);
switch( packetType )
{
case MITELTYPE_NAVDATAGND: mitelgpsProcessNAVDATAGND(MitelGpsPacket); break;
case MITELTYPE_CHNLSTATGND: mitelgpsProcessCHNLSTATGND(MitelGpsPacket); break;
case MITELTYPE_NAVDATA: mitelgpsProcessNAVDATA(MitelGpsPacket); break;
case MITELTYPE_RAWDATA: mitelgpsProcessRAWDATA(MitelGpsPacket); break;
case MITELTYPE_CHNLSTAT: mitelgpsProcessCHNLSTAT(MitelGpsPacket); break;
case MITELTYPE_RELNAVECEF: break;
case MITELTYPE_RELNAVRTN: break;
default:
if(debug & MITELGPS_DEBUG_PKTPARSE)
rprintf("Unhandled Mitel GPS packet type: 0x%x\r\n", packetType);
break;
}
}
return foundpacket;
}
void mitelgpsProcessNAVDATAGND(u08* packet)
{
// process "F00" report packets - Navigation Data (Ground)
char* endptr;
if(debug & MITELGPS_DEBUG_EXTRACT)
{
rprintf("MITELGPS: ");
rprintfStr(packet);
rprintfCRLF();
}
// start parsing just after "F00"
// get latitude [sdd.dddddd]
GpsInfo.PosLLA.lat.f = strtod(&packet[3], &endptr);
// get longitude [sddd.dddddd]
GpsInfo.PosLLA.lon.f = strtod(&packet[3+10], &endptr);
// get altitude [sxxxxxx.x]
GpsInfo.PosLLA.alt.f = strtod(&packet[3+10+11], &endptr);
// get speed [sxxx.xx]
GpsInfo.VelHS.speed.f = strtod(&packet[3+10+11+9], &endptr);
// get heading [ddd]
GpsInfo.VelHS.heading.f = strtod(&packet[3+10+11+9+7], &endptr);
// get # of SVs tracked [xx]
GpsInfo.numSVs = atoi(&packet[3+10+11+9+7+5+7+5+5+5]);
}
void mitelgpsProcessCHNLSTATGND(u08* packet)
{
// process "F03" report packets - Channel Status (Ground)
}
void mitelgpsProcessNAVDATA(u08* packet)
{
// process "F40" report packets - Navigation Data
char* endptr;
// start parsing just after "F40"
// get gps week number [xxxx]=4
GpsInfo.WeekNum = atoi(&packet[3]);
// get gps time of week [xxxxxx.xxxxx]=12
GpsInfo.TimeOfWeek.f = strtod(&packet[3+4], &endptr);
// gps-utc time difference? [xx]=2
// get ECEF X [sxxxxxxxx.xx]=12
GpsInfo.PosECEF.x.f = strtod(&packet[3+4+12+2], &endptr);
// get ECEF Y [sxxxxxxxx.xx]=12
GpsInfo.PosECEF.y.f = strtod(&packet[3+4+12+2+12], &endptr);
// get ECEF Z [sxxxxxxxx.xx]=12
GpsInfo.PosECEF.z.f = strtod(&packet[3+4+12+2+12+12], &endptr);
// get ECEF vX [sxxxxxxxx.xx]=12
GpsInfo.VelECEF.x.f = strtod(&packet[3+4+12+2+12+12+12], &endptr);
// get ECEF vY [sxxxxxxxx.xx]=12
GpsInfo.VelECEF.y.f = strtod(&packet[3+4+12+2+12+12+12+12], &endptr);
// get ECEF vZ [sxxxxxxxx.xx]=12
GpsInfo.VelECEF.z.f = strtod(&packet[3+4+12+2+12+12+12+12+12], &endptr);
}
void mitelgpsProcessRAWDATA(u08* packet)
{
// process "F42" report packets - Pseudorange, carrier phase, doppler
}
void mitelgpsProcessCHNLSTAT(u08* packet)
{
// process "F43" report packets - Channel Status
}
// data conversions
u32 convertAsciiHexToInt(u08* string, u08 numdigits)
{
u08 i;
u32 num = 0;
for(i=0; i<numdigits; i++)
{
// shift number up
num = num<<4;
// decode hex digit
if(string[i] >= 'a')
num |= string[i]-'a'+10;
else if(string[i] >= 'A')
num |= string[i]-'A'+10;
else
num |= string[i]-'0';
}
return num;
}
void convertIntToAsciiHex(u32 num, u08* string, u08 numdigits)
{
u08 i;
for(i=0; i<numdigits; i++)
{
if((num & 0x0000000F) < 10)
string[numdigits-1-i] = (num & 0x0000000F)+'0';
else
string[numdigits-1-i] = (num & 0x0000000F)+'A'-10;
// next digit
num = num>>4;
}
}