Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
46 osiair 1
//------------------------------------------------------------------------------
2
//                                  _            _     
3
//                                 | |          | |    
4
//      ___ _ __ ___  ___ _   _ ___| |_ ___  ___| |__  
5
//     / _ \ '_ ` _ \/ __| | | / __| __/ _ \/ __| '_ \. 
6
//    |  __/ | | | | \__ \ |_| \__ \ ||  __/ (__| | | |
7
//     \___|_| |_| |_|___/\__, |___/\__\___|\___|_| |_|
8
//                         __/ |                       
9
//                        |___/    Engineering
10
//
11
// Filename:    GPS.c
12
// Description: 
13
//              
14
// Author:      Martin Steppuhn
15
// History:     15.06.2007 Initial version
16
//              
17
//------------------------------------------------------------------------------
18
 
19
 
20
/**** Includes ****************************************************************/
21
 
22
#include "std_c.h"
23
#include "main.h"
24
#include "fc.h"
25
#include "gps_ubx.h"
26
 
27
/**** Preprocessing directives (#define) **************************************/
28
 
29
/**** Type definitions (typedef) **********************************************/
30
 
31
/**** Global constants ********************************************************/
32
 
33
/**** Global variables ********************************************************/
34
 
35
signed int GPS_Nick = 0;
36
signed int GPS_Roll = 0;
37
 
38
/**** Local constants  ********************************************************/
39
 
40
/**** Local variables *********************************************************/
41
 
42
signed int test_nick = 0;
43
signed int test_roll = 0;
44
long GpsAktuell_X = 0;
45
long GpsAktuell_Y = 0;
46
long GpsZiel_X = 0;
47
long GpsZiel_Y = 0;
48
long gps_dx;
49
long gps_dy;
50
long gps_vx;
51
long gps_vy;
52
bool  gps_hold;
53
uint8 print_pos;
54
 
55
/**** Local function prototypes ***********************************************/
56
 
57
void print_uint16(uint16 value,uint8 width);
58
void print_int16(int16 value,uint8 width);
59
void print_string(char *s);
60
 
61
//------------------------------------------------------------------------------
62
// Name:      GPS_Neutral
63
// Function:  
64
//            
65
// Parameter: 
66
// Return:    
67
//------------------------------------------------------------------------------
68
void GPS_Neutral(void)
69
{
70
  GpsZiel_X = GpsAktuell_X;
71
  GpsZiel_Y = GpsAktuell_Y;
72
 
73
  GPS_Nick  = 0;
74
  GPS_Roll  = 0;
75
 
76
 // LED_GPS_DATA_OFF;
77
  //LED_GPS_FIX_OFF;
78
  //LED_GPS_DATA_ON;
79
 
80
}
81
 
82
//------------------------------------------------------------------------------
83
// Name:      gps_trace
84
// Function:  
85
//            
86
// Parameter: 
87
// Return:    
88
//------------------------------------------------------------------------------
89
void gps_trace(void)
90
{
91
  print_pos = 0;
92
 
93
  print_uint16(nav_sol.gpsfix,1);      
94
  print_string(" ");
95
  print_int16(gps_dy,7);
96
  print_int16(gps_dx,7);
97
 
98
  print_string(" | ");
99
 
100
  print_int16(gps_vy,7);
101
        print_int16(gps_vx,7);
102
 
103
  print_string(" | ");
104
 
105
  print_int16(GPS_Roll,7);
106
  print_int16(GPS_Nick,7);
107
 
108
 
109
  print_string(" # ");
110
 
111
  print_int16(StickRoll,7);
112
  print_int16(StickNick,7);
113
 
114
 
115
  print_string("\r\n");
116
  UebertragungAbgeschlossen = 0;
117
  UDR = SendeBuffer[0];             // Start
118
}
119
 
120
//------------------------------------------------------------------------------
121
// Name:      gps_main
122
// Function:  
123
//            
124
// Parameter: 
125
// Return:    
126
//------------------------------------------------------------------------------
127
void GPS_Main(void)
128
{
129
  if((Poti3 > 125) && (nav_sol.gpsfix) && (gps_hold == false))    // Enable Hold
130
  {
131
    GpsZiel_X = GpsAktuell_X;
132
    GpsZiel_Y = GpsAktuell_Y;
133
        gps_hold = true;    
134
  }
135
 
136
  if(Poti3 < 125)                   // Disable Hold
137
  {
138
    GpsZiel_X = 0;
139
    GpsZiel_Y = 0;
140
    GPS_Nick  = 0;
141
    GPS_Roll  = 0;
142
 
143
        gps_hold = false;
144
  }
145
 
146
 
147
 
148
 
149
  if(ubx.update)
150
  {
151
 
152
    ubx.update = false;
153
    ubx_decode(&ubx);
154
    if(nav_sol.update)
155
    {
156
      //trace_nav_sol();
157
      nav_sol.update = false;
158
 
159
      if(nav_sol.gpsfix)        
160
      {
161
        LED_GPS_FIX_TOGGLE;
162
        LED_GPS_DATA_OFF;
163
      }
164
      else
165
      {
166
 
167
        LED_GPS_DATA_TOGGLE;
168
        LED_GPS_FIX_OFF;
169
      }
170
 
171
      //=== new Position ====================
172
 
173
      GpsAktuell_X = nav_sol.ecef_x;
174
      GpsAktuell_Y = nav_sol.ecef_y;
175
 
176
      gps_dx = GpsZiel_X - GpsAktuell_X;    // Süden +
177
      gps_dy = GpsZiel_Y - GpsAktuell_Y;        // Osten +      
178
 
179
 
180
      gps_vx = -nav_sol.ecefvx;    // cm/s ECEF X velocity  
181
      gps_vy = -nav_sol.ecefvy;    // cm/s ECEF Y velocity  
182
 
183
 
184
      if(gps_hold)
185
                  {    
186
        GPS_Nick = ((gps_dx * Poti1) / 512) + ((gps_vx * Poti2) / 128);
187
        GPS_Roll = ((gps_dy * Poti1) / 512) + ((gps_vy * Poti2) / 128);
188
            }
189
            else
190
            {
191
                    GPS_Nick = 0;
192
                GPS_Roll = 0;
193
        }
194
 
195
       //=====================================
196
      }
197
          gps_trace();
198
    }
199
}
200
 
201
 
202
 
203
//==============================================================================
204
//==============================================================================
205
//==============================================================================
206
//
207
//==============================================================================
208
//==============================================================================
209
//==============================================================================
210
 
211
 
212
//------------------------------------------------------------------------------
213
// Name:      
214
// Function:  
215
//            
216
// Parameter: 
217
// Return:    
218
//------------------------------------------------------------------------------
219
void print_uint16(uint16 value,uint8 width)
220
{
221
        uint8 i,s[10];
222
 
223
        for (i = 0; i < width; i++)
224
        {
225
                s[width - i - 1] = '0' + (value % 10);
226
                value /= 10;
227
        }
228
        for (i=0; i<(width - 1); i++)           // Overwrite trailing Zeros
229
        {
230
                if (s[i] == '0')        s[i] = ' ';
231
                        else break;
232
        }
233
 
234
        for (i=0; i<width; i++) SendeBuffer[print_pos++] = s[i];                // Output String
235
//      for (i=0; i<width; i++) putchar(s[i]);          // Output String
236
 
237
}
238
 
239
//------------------------------------------------------------------------------
240
// Name:      
241
// Function:  
242
//            
243
// Parameter: 
244
// Return:    
245
//------------------------------------------------------------------------------
246
void print_int16(int16 value,uint8 width)
247
{
248
        uint8 i,s[10];
249
        bool  neg;
250
 
251
  neg = false;
252
        if(value < 0) { value = -value; neg = true;     }
253
 
254
        for (i = 0; i < width; i++)
255
        {
256
                s[width - i - 1] = '0' + (value % 10);
257
                value /= 10;
258
        }
259
 
260
        for (i=0; i<(width - 1); i++)           // Overwrite trailing Zeros
261
        {
262
                if (s[i] == '0')        s[i] = ' ';
263
                else                   
264
                {
265
          if(neg)  s[i-1] = '-';
266
                        break;
267
    }                                          
268
        }
269
        for (i=0; i<width; i++) SendeBuffer[print_pos++] = s[i];                // Output String
270
//      for (i=0; i<width; i++) putchar(s[i]);          // Output String
271
}
272
 
273
//------------------------------------------------------------------------------
274
// Name:      
275
// Function:  
276
//            
277
// Parameter: 
278
// Return:    
279
//------------------------------------------------------------------------------
280
void print_string(char *s)
281
{
282
  while(*s)
283
  {
284
    SendeBuffer[print_pos++] = *s;
285
    s++;
286
  }    
287
}