Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
45 osiair 1
//------------------------------------------------------------------------------
2
//                                  _            _     
3
//                                 | |          | |    
4
//      ___ _ __ ___  ___ _   _ ___| |_ ___  ___| |__  
5
//     / _ \ '_ ` _ \/ __| | | / __| __/ _ \/ __| '_ \. 
6
//    |  __/ | | | | \__ \ |_| \__ \ ||  __/ (__| | | |
7
//     \___|_| |_| |_|___/\__, |___/\__\___|\___|_| |_|
8
//                         __/ |                       
9
//                        |___/    Engineering
10
//
11
// Filename:    gps_ubx.c
12
// Description: 
13
//              
14
// Author:      Martin Steppuhn
15
// History:     08.06.2007 Initial version
16
//              
17
//------------------------------------------------------------------------------
18
 
19
/**** Includes ****************************************************************/
20
 
21
#include "std_c.h"
22
#include "gps_ubx.h"
23
 
24
/**** Preprocessing directives (#define) **************************************/
25
 
26
/**** Type definitions (typedef) **********************************************/
27
 
28
/**** Global constants ********************************************************/
29
 
30
/**** Global variables ********************************************************/
31
 
32
struct ubx_struct         ubx;
33
struct nav_sol_struct     nav_sol;
34
 
35
/**** Local constants  ********************************************************/
36
 
37
/**** Local variables *********************************************************/
38
 
39
uint8   ubx_buf[256];
40
uint8   ubx_buf_len;
41
 
42
/**** Local function prototypes ***********************************************/
43
 
44
 
45
 
46
//------------------------------------------------------------------------------
47
// Name:      ubx_push
48
// Function:  
49
//            
50
// Parameter: 
51
// Return:    
52
//------------------------------------------------------------------------------
53
void ubx_push(uint8 c)
54
{
55
  uint8  length;
56
  uint8  check_a,check_b;
57
  uint16 i;
58
 
59
  ubx_buf[ubx_buf_len] = c;
60
  ubx_buf_len++;
61
 
62
  if(ubx_buf_len == 1)      { if(ubx_buf[0] != 0xB5) ubx_buf_len = 0; }   // Sync
63
  else if(ubx_buf_len == 2) { if(ubx_buf[1] != 0x62) ubx_buf_len = 0; }   // Sync
64
  else if(ubx_buf_len >= 6)
65
  {
66
    length = (((uint16)ubx_buf[5]) << 8)+ ubx_buf[4];   // length
67
    if(ubx_buf_len >= (length+8))                                               // frame complete
68
    {
69
      //=== Calculate checksum ===
70
      check_a = 0;
71
      check_b = 0;
72
      for(i=2;i<(ubx_buf_len-2);i++)
73
      {
74
        check_a = check_a + ubx_buf[i];
75
        check_b = check_b + check_a;
76
      }
77
 
78
      //=== Test checksum ===
79
      if((check_a == ubx_buf[ubx_buf_len-2]) && (check_b == ubx_buf[ubx_buf_len-1]))
80
      {
81
        ubx.msg_class = ubx_buf[2];
82
        ubx.msg_id    = ubx_buf[3];
83
        ubx.length    = length;
84
        for(i=0;i<ubx.length;i++) ubx.data[i] = ubx_buf[i+6];   // copy data
85
        ubx.update = true;
86
      }
87
          ubx_buf_len = 0;
88
    }
89
  }
90
}
91
 
92
//------------------------------------------------------------------------------
93
// Name:      ubx_decode
94
// Function:  
95
//            
96
// Parameter: 
97
// Return:    
98
//------------------------------------------------------------------------------
99
void ubx_decode(struct ubx_struct *ubx)
100
{
101
  if((ubx->msg_class == 0x01) && (ubx->msg_id == 0x06))           // NAV-SOL
102
  {
103
    nav_sol.itow    = *(uint32*)(&ubx->data[0]);  // ms
104
    nav_sol.gpsfix  = ubx->data[10];
105
    nav_sol.ecef_x  = *(int32*)(&ubx->data[12]);  // cm
106
    nav_sol.ecef_y  = *(int32*)(&ubx->data[16]);  // cm
107
    nav_sol.ecef_z  = *(int32*)(&ubx->data[20]);  // cm
108
    nav_sol.pacc    = *(int32*)(&ubx->data[24]);  // cm
109
    nav_sol.ecefvx  = *(uint32*)(&ubx->data[28]); // cm/s
110
    nav_sol.ecefvy  = *(uint32*)(&ubx->data[32]); // cm/s
111
    nav_sol.ecefvz  = *(uint32*)(&ubx->data[36]); // cm/s
112
    nav_sol.sacc    = *(uint32*)(&ubx->data[40]); // cm/s
113
    nav_sol.pdop    = (uint16)ubx->data[44];      
114
    nav_sol.numsv   = ubx->data[47];
115
 
116
    nav_sol.update = true;
117
  }
118
}
119
 
120
//------------------------------------------------------------------------------
121
// Name:      show_ubx
122
// Function:  
123
//            
124
// Parameter: 
125
// Return:    
126
//------------------------------------------------------------------------------
127
void show_ubx(struct ubx_struct *ubx)
128
{
129
  uint8 i;
130
 
131
  printf("MsgClass=%u ",ubx->msg_class);
132
  printf("MsgId=%u ",ubx->msg_id);
133
  printf("Length=%u ",ubx->length);
134
  printf("Data= ");
135
  for(i=0;i<ubx->length;i++) printf("%02X ",ubx->data[i]);
136
  printf("\r\n");
137
}