Subversion Repositories FlightCtrl

Rev

Rev 702 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 702 Rev 722
Line 1... Line 1...
1
#include <inttypes.h>
1
#include <inttypes.h>
2
#include "ubx.h"
2
#include "ubx.h"
3
#include "main.h"
3
#include "main.h"
4
#include <avr/io.h>
4
#include <avr/io.h>
Line -... Line 5...
-
 
5
 
-
 
6
#include "uart.h"
-
 
7
#include "timer0.h"
5
 
8
 
6
// ubx protocol parser state machine
9
// ubx protocol parser state machine
7
#define UBXSTATE_IDLE   0
10
#define UBXSTATE_IDLE   0
8
#define UBXSTATE_SYNC1  1
11
#define UBXSTATE_SYNC1  1
9
#define UBXSTATE_SYNC2  2
12
#define UBXSTATE_SYNC2  2
Line 103... Line 106...
103
                GPSInfo.veleast = GpsVelNed.VEL_E;
106
                GPSInfo.veleast = GpsVelNed.VEL_E;
104
                GPSInfo.velnorth = GpsVelNed.VEL_N;
107
                GPSInfo.velnorth = GpsVelNed.VEL_N;
105
                GPSInfo.veltop = -GpsVelNed.VEL_D;
108
                GPSInfo.veltop = -GpsVelNed.VEL_D;
106
                GpsPosUtm.Status = PROCESSED;                   // never update old data
109
                GpsPosUtm.Status = PROCESSED;                   // never update old data
107
        }
110
        }
108
        if (GpsStatus.Status | GpsVelNed.Status | GpsPosLlh.Status | GpsPosUtm.Status)
111
        if (GpsStatus.Status != INVALID)
109
        {
112
        {
110
                GPSInfo.status = VALID;
113
                GPSInfo.status = VALID;               // set valid if data are updated
111
        }
114
        }
112
}
115
}
Line 113... Line 116...
113
 
116
 
114
// this function should be called within the UART RX ISR
117
// this function should be called within the UART RX ISR
Line 141... Line 144...
141
                        {
144
                        {
142
                                case UBX_ID_POSUTM: // utm position
145
                                case UBX_ID_POSUTM: // utm position
143
                                        ubxP =  (int8_t *)&GpsPosUtm; // data start pointer
146
                                        ubxP =  (int8_t *)&GpsPosUtm; // data start pointer
144
                                        ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
147
                                        ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
145
                                        ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer
148
                                        ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer
146
 
-
 
147
                                        break;
149
                                        break;
Line 148... Line 150...
148
 
150
 
149
                                case UBX_ID_POSLLH: // geodetic position
151
                                case UBX_ID_POSLLH: // geodetic position
150
                                        ubxP =  (int8_t *)&GpsStatus; // data start pointer
152
                                        ubxP =  (int8_t *)&GpsPosLlh; // data start pointer
151
                                        ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
153
                                        ubxEp = (int8_t *)(&GpsPosLlh + sizeof(GPS_POSLLH_t)); // data end pointer
152
                                        ubxSp = (int8_t *)&GpsStatus.Status; // status pointer
154
                                        ubxSp = (int8_t *)&GpsPosLlh.Status; // status pointer
Line 153... Line 155...
153
                                        break;
155
                                        break;
154
 
156
 
155
                                case UBX_ID_STATUS: // receiver status
157
                                case UBX_ID_STATUS: // receiver status
Line 204... Line 206...
204
                        if (--msglen == 0)      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
206
                        if (--msglen == 0)      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
205
                        break;
207
                        break;
Line 206... Line 208...
206
 
208
 
207
                case UBXSTATE_CKA:
209
                case UBXSTATE_CKA:
-
 
210
                        if (c == cka) ubxstate = UBXSTATE_CKB;
-
 
211
                        else
-
 
212
                        {
208
                        if (c == cka) ubxstate = UBXSTATE_CKB;
213
                                *ubxSp = INVALID;
-
 
214
                                ubxstate = UBXSTATE_IDLE;
209
                        else ubxstate = UBXSTATE_IDLE;
215
                        }
Line 210... Line 216...
210
                        break;
216
                        break;
211
 
217
 
212
                case UBXSTATE_CKB:
218
                case UBXSTATE_CKB:
213
                        if (c == ckb)
219
                        if (c == ckb)
214
                        {
-
 
215
                                *ubxSp = VALID; // new data are availabe
220
                        {
-
 
221
                                *ubxSp = VALID; // new data are valid
216
                                UpdateGPSInfo(); //update GPS info respectively
222
                                ROT_FLASH;
-
 
223
                                UpdateGPSInfo(); //update GPS info respectively
-
 
224
                        }
-
 
225
                        else
217
                                ROT_FLASH;
226
                        {       // if checksum not fit then set data invalid
218
 
227
                                *ubxSp = INVALID;
219
                        }
228
                        }
Line 220... Line 229...
220
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data
229
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data