Blame |
Last modification |
View Log
| RSS feed
//------------------------------------------------------------------------------
// _ _
// | | | |
// ___ _ __ ___ ___ _ _ ___| |_ ___ ___| |__
// / _ \ '_ ` _ \/ __| | | / __| __/ _ \/ __| '_ \.
// | __/ | | | | \__ \ |_| \__ \ || __/ (__| | | |
// \___|_| |_| |_|___/\__, |___/\__\___|\___|_| |_|
// __/ |
// |___/ Engineering
//
// Filename: gps_ubx.c
// Description:
//
// Author: Martin Steppuhn
// History: 08.06.2007 Initial version
//
//------------------------------------------------------------------------------
/**** Includes ****************************************************************/
#include "std_c.h"
#include "gps_ubx.h"
/**** Preprocessing directives (#define) **************************************/
/**** Type definitions (typedef) **********************************************/
/**** Global constants ********************************************************/
/**** Global variables ********************************************************/
struct ubx_struct ubx
;
struct nav_sol_struct nav_sol
;
/**** Local constants ********************************************************/
/**** Local variables *********************************************************/
uint8 ubx_buf
[256];
uint8 ubx_buf_len
;
/**** Local function prototypes ***********************************************/
//------------------------------------------------------------------------------
// Name: ubx_push
// Function:
//
// Parameter:
// Return:
//------------------------------------------------------------------------------
void ubx_push
(uint8 c
)
{
uint8 length
;
uint8 check_a
,check_b
;
uint16 i
;
ubx_buf
[ubx_buf_len
] = c
;
ubx_buf_len
++;
if(ubx_buf_len
== 1) { if(ubx_buf
[0] != 0xB5) ubx_buf_len
= 0; } // Sync
else if(ubx_buf_len
== 2) { if(ubx_buf
[1] != 0x62) ubx_buf_len
= 0; } // Sync
else if(ubx_buf_len
>= 6)
{
length
= (((uint16)ubx_buf
[5]) << 8)+ ubx_buf
[4]; // length
if(ubx_buf_len
>= (length
+8)) // frame complete
{
//=== Calculate checksum ===
check_a
= 0;
check_b
= 0;
for(i
=2;i
<(ubx_buf_len
-2);i
++)
{
check_a
= check_a
+ ubx_buf
[i
];
check_b
= check_b
+ check_a
;
}
//=== Test checksum ===
if((check_a
== ubx_buf
[ubx_buf_len
-2]) && (check_b
== ubx_buf
[ubx_buf_len
-1]))
{
ubx.
msg_class = ubx_buf
[2];
ubx.
msg_id = ubx_buf
[3];
ubx.
length = length
;
for(i
=0;i
<ubx.
length;i
++) ubx.
data[i
] = ubx_buf
[i
+6]; // copy data
ubx.
update = true;
}
ubx_buf_len
= 0;
}
}
}
//------------------------------------------------------------------------------
// Name: ubx_decode
// Function:
//
// Parameter:
// Return:
//------------------------------------------------------------------------------
void ubx_decode
(struct ubx_struct
*ubx
)
{
if((ubx
->msg_class
== 0x01) && (ubx
->msg_id
== 0x06)) // NAV-SOL
{
nav_sol.
itow = *(uint32*)(&ubx
->data
[0]); // ms
nav_sol.
gpsfix = ubx
->data
[10];
nav_sol.
ecef_x = *(int32*)(&ubx
->data
[12]); // cm
nav_sol.
ecef_y = *(int32*)(&ubx
->data
[16]); // cm
nav_sol.
ecef_z = *(int32*)(&ubx
->data
[20]); // cm
nav_sol.
pacc = *(int32*)(&ubx
->data
[24]); // cm
nav_sol.
ecefvx = *(uint32*)(&ubx
->data
[28]); // cm/s
nav_sol.
ecefvy = *(uint32*)(&ubx
->data
[32]); // cm/s
nav_sol.
ecefvz = *(uint32*)(&ubx
->data
[36]); // cm/s
nav_sol.
sacc = *(uint32*)(&ubx
->data
[40]); // cm/s
nav_sol.
pdop = (uint16)ubx
->data
[44];
nav_sol.
numsv = ubx
->data
[47];
nav_sol.
update = true;
}
}
//------------------------------------------------------------------------------
// Name: show_ubx
// Function:
//
// Parameter:
// Return:
//------------------------------------------------------------------------------
void show_ubx
(struct ubx_struct
*ubx
)
{
uint8 i
;
printf("MsgClass=%u ",ubx
->msg_class
);
printf("MsgId=%u ",ubx
->msg_id
);
printf("Length=%u ",ubx
->length
);
printf("Data= ");
for(i
=0;i
<ubx
->length
;i
++) printf("%02X ",ubx
->data
[i
]);
printf("\r\n");
}