| 0,0 → 1,137 |
|---|
| //------------------------------------------------------------------------------ |
| // _ _ |
| // | | | | |
| // ___ _ __ ___ ___ _ _ ___| |_ ___ ___| |__ |
| // / _ \ '_ ` _ \/ __| | | / __| __/ _ \/ __| '_ \. |
| // | __/ | | | | \__ \ |_| \__ \ || __/ (__| | | | |
| // \___|_| |_| |_|___/\__, |___/\__\___|\___|_| |_| |
| // __/ | |
| // |___/ 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"); |
| } |