Rev 426 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 426 | Rev 436 | ||
---|---|---|---|
1 | #include "gps.h" |
1 | #include "gps.h" |
2 | #include "uart0.h" |
2 | #include "uart0.h" |
3 | #include "main.h" |
3 | #include "main.h" |
4 | #include "timer0.h" |
4 | #include "timer0.h" |
5 | 5 | ||
6 | #define GPS_TIMEOUT 1000 // of ne new gps data arrivw within that time an error ist set |
6 | #define GPS_TIMEOUT 1000 // if no new gps data arrive within that time an error is set |
7 | #define GPS_MINSATS 4 |
7 | #define GPS_MINSATS 4 |
8 | 8 | ||
9 | //------------------------------------------------------------ |
9 | //------------------------------------------------------------ |
10 | // copy GPS position from source position to target position |
10 | // copy GPS position from source position to target position |
11 | uint8_t GPS_CopyPosition(GPS_Pos_t * pGPSPosSrc, GPS_Pos_t* pGPSPosTgt) |
11 | uint8_t GPS_CopyPosition(GPS_Pos_t * pGPSPosSrc, GPS_Pos_t* pGPSPosTgt) |
12 | { |
12 | { |
13 | uint8_t retval = 0; |
13 | uint8_t retval = 0; |
14 | if((pGPSPosSrc == 0) || (pGPSPosTgt == 0)) return(retval); // bad pointer |
14 | if((pGPSPosSrc == 0) || (pGPSPosTgt == 0)) return(retval); // bad pointer |
15 | // copy only valid positions |
15 | // copy only valid positions |
16 | if(pGPSPosSrc->Status != INVALID) |
16 | if(pGPSPosSrc->Status != INVALID) |
17 | { |
17 | { |
18 | // if the source GPS position is not invalid |
18 | // if the source GPS position is not invalid |
19 | pGPSPosTgt->Longitude = pGPSPosSrc->Longitude; |
19 | pGPSPosTgt->Longitude = pGPSPosSrc->Longitude; |
20 | pGPSPosTgt->Latitude = pGPSPosSrc->Latitude; |
20 | pGPSPosTgt->Latitude = pGPSPosSrc->Latitude; |
21 | pGPSPosTgt->Altitude = pGPSPosSrc->Altitude; |
21 | pGPSPosTgt->Altitude = pGPSPosSrc->Altitude; |
22 | pGPSPosTgt->Status = NEWDATA; // mark data in target position as new |
22 | pGPSPosTgt->Status = NEWDATA; // mark data in target position as new |
23 | retval = 1; |
23 | retval = 1; |
24 | } |
24 | } |
25 | return(retval); |
25 | return(retval); |
26 | } |
26 | } |
27 | 27 | ||
28 | //------------------------------------------------------------ |
28 | //------------------------------------------------------------ |
29 | // clear position data |
29 | // clear position data |
30 | uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos) |
30 | uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos) |
31 | { |
31 | { |
32 | uint8_t retval = 0; |
32 | uint8_t retval = 0; |
33 | if(pGPSPos == 0) return(retval); // bad pointer |
33 | if(pGPSPos == 0) return(retval); // bad pointer |
34 | else |
34 | else |
35 | { |
35 | { |
36 | pGPSPos->Longitude = 0; |
36 | pGPSPos->Longitude = 0; |
37 | pGPSPos->Latitude = 0; |
37 | pGPSPos->Latitude = 0; |
38 | pGPSPos->Altitude = 0; |
38 | pGPSPos->Altitude = 0; |
39 | pGPSPos->Status = INVALID; |
39 | pGPSPos->Status = INVALID; |
40 | retval = 1; |
40 | retval = 1; |
41 | } |
41 | } |
42 | return (retval); |
42 | return (retval); |
43 | } |
43 | } |
44 | 44 | ||
45 | 45 | ||
46 | //------------------------------------------------------------ |
46 | //------------------------------------------------------------ |
47 | // check for new GPS data |
47 | // check for new GPS data |
48 | void GPS_Update(void) |
48 | void GPS_Update(void) |
49 | { |
49 | { |
50 | static uint16_t GPS_Timeout = 0; |
50 | static uint16_t GPS_Timeout = 0; |
51 | static uint16_t beep_rythm = 0; |
51 | static uint16_t beep_rythm = 0; |
52 | 52 | ||
53 | switch(GPSData.Status) |
53 | switch(GPSData.Status) |
54 | { |
54 | { |
55 | case INVALID: |
55 | case INVALID: |
56 | Error |= ERROR_GPS_RX_TIMEOUT; |
56 | Error |= ERROR_GPS_RX_TIMEOUT; |
57 | GPS_ClearPosition(&(FollowMe.Position)); // clear followme position |
57 | GPS_ClearPosition(&(FollowMe.Position)); // clear followme position |
58 | break; |
58 | break; |
59 | 59 | ||
60 | case PROCESSED: |
60 | case PROCESSED: |
61 | // wait for timeout |
61 | // wait for timeout |
62 | if(CheckDelay(GPS_Timeout)) |
62 | if(CheckDelay(GPS_Timeout)) |
63 | { |
63 | { |
64 | GPSData.Status = INVALID; |
64 | GPSData.Status = INVALID; |
65 | } |
65 | } |
66 | break; |
66 | break; |
67 | 67 | ||
68 | case NEWDATA: |
68 | case NEWDATA: |
69 | GPS_Timeout = SetDelay(GPS_TIMEOUT); // reset gps timeout |
69 | GPS_Timeout = SetDelay(GPS_TIMEOUT); // reset gps timeout |
70 | Error &= ~ERROR_GPS_RX_TIMEOUT; // clear possible error |
70 | Error &= ~ERROR_GPS_RX_TIMEOUT; // clear possible error |
71 | beep_rythm++; |
71 | beep_rythm++; |
72 | 72 | ||
73 | // update data in the follow me message |
73 | // update data in the follow me message |
74 | if((GPSData.SatFix & SATFIX_3D) && (GPSData.NumOfSats >= GPS_MINSATS)) |
74 | if((GPSData.SatFix & SATFIX_3D) && (GPSData.NumOfSats >= GPS_MINSATS)) |
75 | { |
75 | { |
76 | GPS_CopyPosition(&(GPSData.Position),&(FollowMe.Position)); |
76 | GPS_CopyPosition(&(GPSData.Position),&(FollowMe.Position)); |
77 | } |
77 | } |
78 | else |
78 | else |
79 | { |
79 | { |
80 | GPS_ClearPosition(&(FollowMe.Position)); // clear followme position |
80 | GPS_ClearPosition(&(FollowMe.Position)); // clear followme position |
81 | } |
81 | } |
82 | 82 | ||
83 | // NC like sound on bad gps signals |
83 | // NC like sound on bad gps signals |
84 | if(SysState == STATE_SEND_FOLLOWME) |
84 | if(SysState == STATE_SEND_FOLLOWME) |
85 | { |
85 | { |
86 | if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
86 | if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
87 | else if ((GPSData.NumOfSats < GPS_MINSATS) && !(beep_rythm % 5)) BeepTime = 10; |
87 | else if ((GPSData.NumOfSats < GPS_MINSATS) && !(beep_rythm % 5)) BeepTime = 10; |
88 | } |
88 | } |
89 | 89 | ||
90 | GPSData.Status = PROCESSED; // set to processed unlocks the buffer for new data |
90 | GPSData.Status = PROCESSED; // set to processed unlocks the buffer for new data |
91 | break; |
91 | break; |
92 | 92 | ||
93 | default: |
93 | default: |
94 | GPSData.Status = INVALID; |
94 | GPSData.Status = INVALID; |
95 | break; |
95 | break; |
96 | } |
96 | } |
97 | } |
97 | } |
98 | 98 | ||
99 | 99 |