Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 262 → Rev 263

/trunk/ubx.c
54,6 → 54,7
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <string.h>
#include "91x_lib.h"
#include "uart1.h"
#include "ubx.h"
164,10 → 165,10
// global variables
 
// local buffers for the incomming ubx messages
volatile ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
volatile ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
volatile ubxmsg_t UbxMsg;
ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
ubxmsg_t UbxMsg;
 
// shared buffer
gps_data_t GPSData = {200,{0,0,0,INVALID},0,0,0,0,0,0,0, INVALID};
276,34 → 277,23
/********************************************************/
/* Upate GPS data stcructure */
/********************************************************/
void Update_GPSData (void)
void Update_GPSData(void)
{
static u32 Msg_Count_Timeout = 0;
static u8 Msg_Count = 0;
static u32 LastTimeStamp = 0;
u32 TimeStamp;
static u32 last_itow = 0;
 
// the timeout is used to detect the delay between two message sets
// and is used for synchronisation so that always a set is collected
// that belongs together
// _______NAVSOL|POSLLH|VELNED|___________________NAVSOL|POSLLH|VELNED|_____________
// | 8ms | 8ms | 184 ms | | |
// msg_count: 0 1 2 0 1 2
 
if(CheckDelay(Msg_Count_Timeout)) Msg_Count = 0;
else Msg_Count++;
Msg_Count_Timeout = SetDelay(100); // reset ubx msg timeout
 
// if a new set of ubx messages was collected
if((Msg_Count >= 2))
{ // if set is complete
if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
{ // and the itow is equal (same time base)
if((UbxSol.itow == UbxPosLlh.itow) && (UbxPosLlh.itow == UbxVelNed.itow))
{
DebugOut.Analog[23] = (u16)(UbxSol.itow-last_itow);
last_itow = UbxSol.itow; // update last itow
UBX_Timeout = SetDelay(UBX_TIMEOUT);
DebugOut.Analog[9]++;
 
// update GPS data only if the status is INVALID or PROCESSED and the last ubx message was received within less than 100 ms
if(GPSData.Status != NEWDATA) // if last data were processed
// update GPS data only if the status is INVALID or PROCESSED
if(GPSData.Status != NEWDATA)
{ // wait for new data at all neccesary ubx messages
GPSData.Status = INVALID;
// update message cycle time
330,15 → 320,15
GPSData.Speed_Top = -UbxVelNed.VEL_D;
GPSData.Speed_Ground = UbxVelNed.GSpeed;
GPSData.Heading = UbxVelNed.Heading;
 
GPSData.Status = NEWDATA; // new data available
} // EOF if(GPSData.Status != NEWDATA)
} // EOF all ubx messages received
// set state to collect new data
UbxSol.Status = PROCESSED; // ready for new data
UbxPosLlh.Status = PROCESSED; // ready for new data
UbxVelNed.Status = PROCESSED; // ready for new data
}
// set state to collect new data
UbxSol.Status = PROCESSED; // ready for new data
UbxPosLlh.Status = PROCESSED; // ready for new data
UbxVelNed.Status = PROCESSED; // ready for new data
} // EOF all itow are equal
} // EOF all ubx messages received
}
 
 
348,11 → 338,10
void UBX_RxParser(u8 c)
{
static ubxState_t ubxState = UBXSTATE_IDLE;
static u8 ubxclass;
static u8 ubxid;
static u16 msglen;
static ubxmsghdr_t RxHdr;
static u8 RxData[UBX_MSG_DATA_SIZE];
static u16 RxBytes = 0;
static u8 cka, ckb;
static u8 *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
 
 
//state machine
369,19 → 358,19
break;
 
case UBXSTATE_SYNC2: // check msg class to be NAV
ubxclass = c;
RxHdr.Class = c;
ubxState = UBXSTATE_CLASS;
break;
 
case UBXSTATE_CLASS: // check message identifier
ubxid = c;
RxHdr.Id = c;
ubxState = UBXSTATE_LEN1;
cka = ubxclass + ubxid;
ckb = ubxclass + cka;
cka = RxHdr.Class + RxHdr.Id;
ckb = RxHdr.Class + cka;
break;
 
case UBXSTATE_LEN1: // 1st message length byte
msglen = (u16)c; // lowbyte first
RxHdr.Length = (u16)c; // lowbyte first
cka += c;
ckb += cka;
ubxState = UBXSTATE_LEN2;
388,82 → 377,33
break;
 
case UBXSTATE_LEN2: // 2nd message length byte
msglen += ((u16)c)<<8; // high byte last
cka += c;
ckb += cka;
 
switch(ubxclass)
RxHdr.Length += ((u16)c)<<8; // high byte last
if (RxHdr.Length >= UBX_MSG_DATA_SIZE)
{
case UBX_CLASS_NAV:
switch(ubxid)
{
case UBX_ID_POSLLH: // geodetic position
ubxP = (u8 *)&UbxPosLlh; // data start pointer
ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer
break;
case UBX_ID_SOL: // navigation solution
ubxP = (u8 *)&UbxSol; // data start pointer
ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
ubxSp = (u8 *)&UbxSol.Status; // status pointer
break;
case UBX_ID_VELNED: // velocity vector in tangent plane
ubxP = (u8 *)&UbxVelNed; // data start pointer
ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
ubxSp = (u8 *)&UbxVelNed.Status; // status pointer
break;
default: // unsupported identifier
ubxState = UBXSTATE_IDLE;
return;
}
break;
 
default: // other classes
if(UbxMsg.Status == NEWDATA) ubxState = UBXSTATE_IDLE;
else if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (ubxclass&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (ubxid&UbxMsg.IdMask)))
{ // buffer is free and message matches to filter criteria
UbxMsg.Status = INVALID;
UbxMsg.Hdr.Class = ubxclass;
UbxMsg.Hdr.Id = ubxid;
UbxMsg.Hdr.Length = msglen;
ubxP = (u8 *)&(UbxMsg.Data); // data start pointer
ubxEp = (u8 *)(&UbxMsg + 1); // data end pointer
ubxSp = (u8 *)&UbxMsg.Status; // status pointer
}
else ubxState = UBXSTATE_IDLE;
break;
ubxState = UBXSTATE_IDLE;
DebugOut.Analog[25]++;
}
if(ubxState != UBXSTATE_IDLE)
else
{
// if the old data are not processed so far then break parsing now
// to avoid writing new data in ISR during reading by another function
if ( *ubxSp == NEWDATA )
{
ubxState = UBXSTATE_IDLE;
if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively
}
else // data invalid or allready processed
{
*ubxSp = INVALID; // mark invalid during buffer filling
ubxState = UBXSTATE_DATA;
}
cka += c;
ckb += cka;
RxBytes = 0; // reset data byte counter
ubxState = UBXSTATE_DATA;
}
break;
 
case UBXSTATE_DATA: // collecting data
if (ubxP < ubxEp)
if (RxBytes < UBX_MSG_DATA_SIZE)
{
*ubxP++ = c; // copy curent data byte if any space is left
RxData[RxBytes++] = c; // copy curent data byte if any space is left
cka += c;
ckb += cka;
if (--msglen == 0) ubxState = UBXSTATE_CKA; // switch to next state if all data was read
if (RxBytes >= RxHdr.Length) ubxState = UBXSTATE_CKA; // switch to next state if all data have been received
}
else // rx buffer overrun
{
ubxState = UBXSTATE_IDLE;
DebugOut.Analog[25]++;
}
break;
 
471,21 → 411,63
if (c == cka) ubxState = UBXSTATE_CKB;
else
{
*ubxSp = INVALID;
ubxState = UBXSTATE_IDLE;
DebugOut.Analog[24]++;
}
break;
 
case UBXSTATE_CKB:
if (c == ckb)
{
*ubxSp = NEWDATA; // new data are valid
if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively
}
else
{ // if checksum not match then set data invalid
*ubxSp = INVALID;
}
{ // checksum is ok
 
switch(RxHdr.Class)
{
case UBX_CLASS_NAV:
switch(RxHdr.Id)
{
case UBX_ID_POSLLH: // geodetic position
memcpy((u8*)&UbxPosLlh, RxData, RxHdr.Length);
UbxPosLlh.Status = NEWDATA;
break;
 
case UBX_ID_VELNED: // velocity vector in tangent plane
memcpy((u8*)&UbxVelNed, RxData, RxHdr.Length);
UbxVelNed.Status = NEWDATA;
break;
 
case UBX_ID_SOL: // navigation solution
memcpy((u8*)&UbxSol, RxData, RxHdr.Length);
UbxSol.Status = NEWDATA;
break;
 
default:
break;
} // EOF switch(Id)
Update_GPSData();
break;
default:
break;
} // EOF switch(class)
 
// check generic msg filter
if(UbxMsg.Status != NEWDATA)
{ // msg buffer is free
if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (RxHdr.Class&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (RxHdr.Id&UbxMsg.IdMask)))
{ // msg matches to the filter criteria
UbxMsg.Status = INVALID;
UbxMsg.Hdr.Class = RxHdr.Class;
UbxMsg.Hdr.Id = RxHdr.Id;
UbxMsg.Hdr.Length = RxHdr.Length;
if(UbxMsg.Hdr.Length <= UBX_MSG_DATA_SIZE)
{ // copy data block
memcpy(UbxMsg.Data, RxData, RxHdr.Length);
UbxMsg.Status = NEWDATA;
}
} // EOF filter matches
} // EOF != INVALID
}// EOF crc ok
else DebugOut.Analog[24]++;
ubxState = UBXSTATE_IDLE; // ready to parse new data
break;
 
526,3 → 508,50
pBuff->Position = 0; // reset buffer position for transmision
return(1);
}
/*
switch(ubxclass)
{
case UBX_CLASS_NAV:
switch(ubxid)
{
case UBX_ID_POSLLH: // geodetic position
ubxSp = (u8 *)&UbxPosLlh; // data start pointer
ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
ubxStP = (u8 *)&UbxPosLlh.Status; // status pointer
break;
case UBX_ID_SOL: // navigation solution
ubxSp = (u8 *)&UbxSol; // data start pointer
ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
ubxStP = (u8 *)&UbxSol.Status; // status pointer
break;
case UBX_ID_VELNED: // velocity vector in tangent plane
ubxSp = (u8 *)&UbxVelNed; // data start pointer
ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
ubxStP = (u8 *)&UbxVelNed.Status; // status pointer
break;
default: // unsupported identifier
ubxState = UBXSTATE_IDLE;
return;
}
break;
 
default: // other classes
if(UbxMsg.Status == NEWDATA) ubxState = UBXSTATE_IDLE;
else if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (ubxclass&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (ubxid&UbxMsg.IdMask)))
{ // buffer is free and message matches to filter criteria
UbxMsg.Status = INVALID;
UbxMsg.Hdr.Class = ubxclass;
UbxMsg.Hdr.Id = ubxid;
UbxMsg.Hdr.Length = msglen;
ubxSp = (u8 *)&(UbxMsg.Data); // data start pointer
ubxEp = (u8 *)(&UbxMsg + 1); // data end pointer
ubxStP = (u8 *)&UbxMsg.Status; // status pointer
}
else ubxState = UBXSTATE_IDLE;
break;
}
*/
 
/trunk/ubx.h
60,17 → 60,18
u16 Length;
} __attribute__((packed)) ubxmsghdr_t;
 
#define UBX_MSG_DATA_SIZE 200
typedef struct
{
u8 ClassMask;
u8 IdMask;
ubxmsghdr_t Hdr;
u8 Data[100];
u8 Data[UBX_MSG_DATA_SIZE];
u8 Status;
} __attribute__((packed)) ubxmsg_t;
// msg obj to reveive
// set Class and Id and correspoinding masks of a message that should be received
extern volatile ubxmsg_t UbxMsg;
extern ubxmsg_t UbxMsg;
 
 
extern u32 UBX_Timeout;