Subversion Repositories FlightCtrl

Rev

Rev 702 | Go to most recent revision | Show entire file | Ignore 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