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; |
} |
*/ |
|