mirror of
https://github.com/esp8266/Arduino.git
synced 2025-07-30 16:24:09 +03:00
Initial Arduino IDE based on Processing.
This commit is contained in:
David A. Mellis
288
build/shared/lib/avrlib/rsl/mitelgps.c
Executable file
288
build/shared/lib/avrlib/rsl/mitelgps.c
Executable file
@ -0,0 +1,288 @@
|
||||
/*! \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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user